|
/*
* $RCSfile: Math3D.java,v $
*
* Copyright (c) 2007 Sun Microsystems, Inc. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* - Redistribution of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* - Redistribution in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
*
* Neither the name of Sun Microsystems, Inc. or the names of
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* This software is provided "AS IS," without a warranty of any
* kind. ALL EXPRESS OR IMPLIED CONDITIONS, REPRESENTATIONS AND
* WARRANTIES, INCLUDING ANY IMPLIED WARRANTY OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE OR NON-INFRINGEMENT, ARE HEREBY
* EXCLUDED. SUN MICROSYSTEMS, INC. ("SUN") AND ITS LICENSORS SHALL
* NOT BE LIABLE FOR ANY DAMAGES SUFFERED BY LICENSEE AS A RESULT OF
* USING, MODIFYING OR DISTRIBUTING THIS SOFTWARE OR ITS
* DERIVATIVES. IN NO EVENT WILL SUN OR ITS LICENSORS BE LIABLE FOR
* ANY LOST REVENUE, PROFIT OR DATA, OR FOR DIRECT, INDIRECT, SPECIAL,
* CONSEQUENTIAL, INCIDENTAL OR PUNITIVE DAMAGES, HOWEVER CAUSED AND
* REGARDLESS OF THE THEORY OF LIABILITY, ARISING OUT OF THE USE OF OR
* INABILITY TO USE THIS SOFTWARE, EVEN IF SUN HAS BEEN ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGES.
*
* You acknowledge that this software is not designed, licensed or
* intended for use in the design, construction, operation or
* maintenance of any nuclear facility.
*
* $Revision: 1.6 $
* $Date: 2007/08/28 16:42:24 $
* $State: Exp $
*/
import javax.vecmath.Point3f;
import javax.vecmath.Vector3f;
import javax.vecmath.Vector3d;
import javax.vecmath.Tuple3f;
import javax.vecmath.Matrix3d;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;
import javax.media.j3d.BoundingBox;
import javax.media.j3d.BoundingSphere;
import javax.media.j3d.Bounds;
/**
* 3D Math utilities
*
* @author Paul
*/;
public class Math3D {
/**
* Calculates the distance of a point from a line.
* <p><code>
* x1----------------------------x2 <br>
* | <br>
* | distance <br>
* | <br>
* point <br>
* </code>
* <p>
* The formula is <br>
* <code>
* d = |(x2-x1) x (x1-p)| <br>
* ------------------ <br>
* |x2-x1| <br>
* </code>
*
* Where p=point, lineStart=x1, lineEnd=x2
*
*/
public static float pointLineDistance( final Point3f lineStart,
final Point3f lineEnd,
final Point3f point ) {
Vector3f a = new Vector3f(lineEnd);
a.sub(lineStart);
Vector3f b = new Vector3f(lineStart);
b.sub(point);
Vector3f cross = new Vector3f();
cross.cross(a,b);
return cross.length()/a.length();
}
/**
* Converts the Matrix into Euler angles (roll, pitch, yaw )
*/
public static void toEuler( Matrix3d matrix, Vector3d euler ) {
Vector3d v3d = new Vector3d();
Vector3d zAxis = new Vector3d( 0, 0, -1 );
Vector3d yAxis = new Vector3d( 0, 1, 0 );
Vector3d xAxis = new Vector3d( 1, 0, 0 );
v3d.set( xAxis );
matrix.transform( v3d );
v3d.x = Math.abs( v3d.x );
v3d.z = 0;
v3d.normalize();
euler.x = xAxis.angle( v3d );
v3d.set( yAxis );
matrix.transform( v3d );
v3d.z = Math.abs( v3d.z );
v3d.x = 0;
v3d.normalize();
euler.y = yAxis.angle( v3d );
v3d.set( zAxis );
matrix.transform( v3d );
v3d.y = 0;
v3d.normalize();
euler.z = zAxis.angle( v3d );
if (v3d.x<0)
euler.z = 2*Math.PI-euler.z;
}
public static boolean epsilonEquals(float f1, float f2, float epsilon) {
float diff;
diff = f1 - f2;
if ((diff < 0 ? -diff : diff) > epsilon) {
return false;
}
return true;
}
public static boolean epsilonEquals(Tuple3f t1, Tuple3f t2, float epsilon) {
if (epsilonEquals(t1.x, t2.x, epsilon) &&
epsilonEquals(t1.y, t2.y, epsilon) &&
epsilonEquals(t1.z, t2.z, epsilon))
return true;
return false;
}
public static boolean encloses(Bounds parent, Bounds child) {
if (parent instanceof BoundingBox)
if (child instanceof BoundingBox)
return encloses((BoundingBox)parent, (BoundingBox)child);
else if (child instanceof BoundingSphere)
return encloses((BoundingBox)parent, (BoundingSphere)child);
else if (parent instanceof BoundingSphere)
if (child instanceof BoundingBox)
return encloses((BoundingSphere)parent, (BoundingBox)child);
else if (child instanceof BoundingSphere)
return encloses((BoundingSphere)parent, (BoundingSphere)child);
throw new UnsupportedOperationException("Unsupported bounds combination");
}
/**
* Returns true if the parent bounds fully encloses the child
*/
public static boolean encloses(BoundingBox parent, BoundingSphere child) {
Point3d upper = new Point3d();
Point3d lower = new Point3d();
Point3d center = new Point3d();
double radius;
parent.getUpper(upper);
parent.getLower(lower);
child.getCenter(center);
radius = child.getRadius();
if (center.x+radius > upper.x ||
center.y+radius > upper.y ||
center.z+radius > upper.z)
return false;
if (center.x-radius < lower.x ||
center.y-radius < lower.y ||
center.z-radius < lower.z)
return false;
return true;
}
/**
* Returns true if the parent bounds fully encloses the child
*/
public static boolean encloses(BoundingBox parent, BoundingBox child) {
Point3d pUpper = new Point3d();
Point3d pLower = new Point3d();
Point3d cUpper = new Point3d();
Point3d cLower = new Point3d();
parent.getUpper(pUpper);
parent.getLower(pLower);
child.getUpper(cUpper);
child.getLower(cLower);
if (cUpper.x > pUpper.x ||
cUpper.y > pUpper.y ||
cUpper.z > pUpper.z)
return false;
if (cLower.x < pLower.x ||
cLower.y < pLower.y ||
cLower.z < pLower.z)
return false;
return true;
}
/**
* Returns true if the parent bounds fully encloses the child
*/
public static boolean encloses(BoundingSphere parent, BoundingBox child) {
// if the distance from the center of the sphere to any corner of
// the box is greater than the sphere radius return false
Point3d lower = new Point3d();
Point3d upper = new Point3d();
Point3d parentCenter = new Point3d();
child.getLower(lower);
child.getUpper(upper);
parent.getCenter(parentCenter);
double xDim = upper.x - lower.x;
double yDim = upper.y - lower.y;
double radiusSquared = Math.pow(parent.getRadius(), 2);
Vector3d tmp = new Vector3d();
tmp.set(lower);
tmp.sub(parentCenter);
if (tmp.lengthSquared()>radiusSquared)
return false;
tmp.set(lower.x+xDim, lower.y, lower.z);
tmp.sub(parentCenter);
if (tmp.lengthSquared()>radiusSquared)
return false;
tmp.set(lower.x, lower.y+yDim, lower.z);
tmp.sub(parentCenter);
if (tmp.lengthSquared()>radiusSquared)
return false;
tmp.set(lower.x+xDim, lower.y+yDim, lower.z);
tmp.sub(parentCenter);
if (tmp.lengthSquared()>radiusSquared)
return false;
tmp.set(upper);
tmp.sub(parentCenter);
if (tmp.lengthSquared()>radiusSquared)
return false;
tmp.set(upper.x-xDim, upper.y, upper.z);
tmp.sub(parentCenter);
if (tmp.lengthSquared()>radiusSquared)
return false;
tmp.set(upper.x, upper.y-yDim, upper.z);
tmp.sub(parentCenter);
if (tmp.lengthSquared()>radiusSquared)
return false;
tmp.set(upper.x-xDim, upper.y-yDim, upper.z);
tmp.sub(parentCenter);
if (tmp.lengthSquared()>radiusSquared)
return false;
return true;
}
/**
* Returns true if the parent bounds fully encloses the child
*/
public static boolean encloses(BoundingSphere parent, BoundingSphere child) {
Point3d childCenter = new Point3d();
Point3d parentCenter = new Point3d();
child.getCenter(childCenter);
parent.getCenter(parentCenter);
double childR = child.getRadius();
double parentR = parent.getRadius();
if (childCenter.x+childR > parentCenter.x+parentR ||
childCenter.y+childR > parentCenter.y+parentR ||
childCenter.z+childR > parentCenter.z+parentR)
return false;
if (childCenter.x-childR < parentCenter.x-parentR ||
childCenter.y-childR < parentCenter.y-parentR ||
childCenter.z-childR < parentCenter.z-parentR)
return false;
return true;
}
}
|