001: /*
002: * $RCSfile: Math3D.java,v $
003: *
004: * Copyright (c) 2007 Sun Microsystems, Inc. All rights reserved.
005: *
006: * Redistribution and use in source and binary forms, with or without
007: * modification, are permitted provided that the following conditions
008: * are met:
009: *
010: * - Redistribution of source code must retain the above copyright
011: * notice, this list of conditions and the following disclaimer.
012: *
013: * - Redistribution in binary form must reproduce the above copyright
014: * notice, this list of conditions and the following disclaimer in
015: * the documentation and/or other materials provided with the
016: * distribution.
017: *
018: * Neither the name of Sun Microsystems, Inc. or the names of
019: * contributors may be used to endorse or promote products derived
020: * from this software without specific prior written permission.
021: *
022: * This software is provided "AS IS," without a warranty of any
023: * kind. ALL EXPRESS OR IMPLIED CONDITIONS, REPRESENTATIONS AND
024: * WARRANTIES, INCLUDING ANY IMPLIED WARRANTY OF MERCHANTABILITY,
025: * FITNESS FOR A PARTICULAR PURPOSE OR NON-INFRINGEMENT, ARE HEREBY
026: * EXCLUDED. SUN MICROSYSTEMS, INC. ("SUN") AND ITS LICENSORS SHALL
027: * NOT BE LIABLE FOR ANY DAMAGES SUFFERED BY LICENSEE AS A RESULT OF
028: * USING, MODIFYING OR DISTRIBUTING THIS SOFTWARE OR ITS
029: * DERIVATIVES. IN NO EVENT WILL SUN OR ITS LICENSORS BE LIABLE FOR
030: * ANY LOST REVENUE, PROFIT OR DATA, OR FOR DIRECT, INDIRECT, SPECIAL,
031: * CONSEQUENTIAL, INCIDENTAL OR PUNITIVE DAMAGES, HOWEVER CAUSED AND
032: * REGARDLESS OF THE THEORY OF LIABILITY, ARISING OUT OF THE USE OF OR
033: * INABILITY TO USE THIS SOFTWARE, EVEN IF SUN HAS BEEN ADVISED OF THE
034: * POSSIBILITY OF SUCH DAMAGES.
035: *
036: * You acknowledge that this software is not designed, licensed or
037: * intended for use in the design, construction, operation or
038: * maintenance of any nuclear facility.
039: *
040: * $Revision: 1.6 $
041: * $Date: 2007/08/28 16:42:24 $
042: * $State: Exp $
043: */
044:
045: package org.jdesktop.j3d.utils.math;
046:
047: import javax.vecmath.Point3f;
048: import javax.vecmath.Vector3f;
049: import javax.vecmath.Vector3d;
050: import javax.vecmath.Tuple3f;
051: import javax.vecmath.Matrix3d;
052: import javax.vecmath.Point3d;
053: import javax.vecmath.Vector3d;
054: import javax.media.j3d.BoundingBox;
055: import javax.media.j3d.BoundingSphere;
056: import javax.media.j3d.Bounds;
057:
058: /**
059: * 3D Math utilities
060: *
061: * @author Paul
062: */
063: ;
064: public class Math3D {
065:
066: /**
067: * Calculates the distance of a point from a line.
068: * <p><code>
069: * x1----------------------------x2 <br>
070: * | <br>
071: * | distance <br>
072: * | <br>
073: * point <br>
074: * </code>
075: * <p>
076: * The formula is <br>
077: * <code>
078: * d = |(x2-x1) x (x1-p)| <br>
079: * ------------------ <br>
080: * |x2-x1| <br>
081: * </code>
082: *
083: * Where p=point, lineStart=x1, lineEnd=x2
084: *
085: */
086: public static float pointLineDistance(final Point3f lineStart,
087: final Point3f lineEnd, final Point3f point) {
088: Vector3f a = new Vector3f(lineEnd);
089: a.sub(lineStart);
090:
091: Vector3f b = new Vector3f(lineStart);
092: b.sub(point);
093:
094: Vector3f cross = new Vector3f();
095: cross.cross(a, b);
096:
097: return cross.length() / a.length();
098: }
099:
100: /**
101: * Converts the Matrix into Euler angles (roll, pitch, yaw )
102: */
103: public static void toEuler(Matrix3d matrix, Vector3d euler) {
104: Vector3d v3d = new Vector3d();
105:
106: Vector3d zAxis = new Vector3d(0, 0, -1);
107: Vector3d yAxis = new Vector3d(0, 1, 0);
108: Vector3d xAxis = new Vector3d(1, 0, 0);
109:
110: v3d.set(xAxis);
111: matrix.transform(v3d);
112: v3d.x = Math.abs(v3d.x);
113: v3d.z = 0;
114: v3d.normalize();
115:
116: euler.x = xAxis.angle(v3d);
117:
118: v3d.set(yAxis);
119: matrix.transform(v3d);
120: v3d.z = Math.abs(v3d.z);
121: v3d.x = 0;
122: v3d.normalize();
123:
124: euler.y = yAxis.angle(v3d);
125:
126: v3d.set(zAxis);
127: matrix.transform(v3d);
128: v3d.y = 0;
129: v3d.normalize();
130:
131: euler.z = zAxis.angle(v3d);
132: if (v3d.x < 0)
133: euler.z = 2 * Math.PI - euler.z;
134: }
135:
136: public static boolean epsilonEquals(float f1, float f2,
137: float epsilon) {
138: float diff;
139:
140: diff = f1 - f2;
141: if ((diff < 0 ? -diff : diff) > epsilon) {
142: return false;
143: }
144:
145: return true;
146: }
147:
148: public static boolean epsilonEquals(Tuple3f t1, Tuple3f t2,
149: float epsilon) {
150: if (epsilonEquals(t1.x, t2.x, epsilon)
151: && epsilonEquals(t1.y, t2.y, epsilon)
152: && epsilonEquals(t1.z, t2.z, epsilon))
153: return true;
154:
155: return false;
156: }
157:
158: public static boolean encloses(Bounds parent, Bounds child) {
159: if (parent instanceof BoundingBox)
160: if (child instanceof BoundingBox)
161: return encloses((BoundingBox) parent,
162: (BoundingBox) child);
163: else if (child instanceof BoundingSphere)
164: return encloses((BoundingBox) parent,
165: (BoundingSphere) child);
166: else if (parent instanceof BoundingSphere)
167: if (child instanceof BoundingBox)
168: return encloses((BoundingSphere) parent,
169: (BoundingBox) child);
170: else if (child instanceof BoundingSphere)
171: return encloses((BoundingSphere) parent,
172: (BoundingSphere) child);
173:
174: throw new UnsupportedOperationException(
175: "Unsupported bounds combination");
176: }
177:
178: /**
179: * Returns true if the parent bounds fully encloses the child
180: */
181: public static boolean encloses(BoundingBox parent,
182: BoundingSphere child) {
183: Point3d upper = new Point3d();
184: Point3d lower = new Point3d();
185: Point3d center = new Point3d();
186: double radius;
187:
188: parent.getUpper(upper);
189: parent.getLower(lower);
190: child.getCenter(center);
191: radius = child.getRadius();
192:
193: if (center.x + radius > upper.x || center.y + radius > upper.y
194: || center.z + radius > upper.z)
195: return false;
196:
197: if (center.x - radius < lower.x || center.y - radius < lower.y
198: || center.z - radius < lower.z)
199: return false;
200:
201: return true;
202: }
203:
204: /**
205: * Returns true if the parent bounds fully encloses the child
206: */
207: public static boolean encloses(BoundingBox parent, BoundingBox child) {
208: Point3d pUpper = new Point3d();
209: Point3d pLower = new Point3d();
210: Point3d cUpper = new Point3d();
211: Point3d cLower = new Point3d();
212:
213: parent.getUpper(pUpper);
214: parent.getLower(pLower);
215: child.getUpper(cUpper);
216: child.getLower(cLower);
217:
218: if (cUpper.x > pUpper.x || cUpper.y > pUpper.y
219: || cUpper.z > pUpper.z)
220: return false;
221:
222: if (cLower.x < pLower.x || cLower.y < pLower.y
223: || cLower.z < pLower.z)
224: return false;
225:
226: return true;
227: }
228:
229: /**
230: * Returns true if the parent bounds fully encloses the child
231: */
232: public static boolean encloses(BoundingSphere parent,
233: BoundingBox child) {
234: // if the distance from the center of the sphere to any corner of
235: // the box is greater than the sphere radius return false
236:
237: Point3d lower = new Point3d();
238: Point3d upper = new Point3d();
239:
240: Point3d parentCenter = new Point3d();
241:
242: child.getLower(lower);
243: child.getUpper(upper);
244:
245: parent.getCenter(parentCenter);
246:
247: double xDim = upper.x - lower.x;
248: double yDim = upper.y - lower.y;
249:
250: double radiusSquared = Math.pow(parent.getRadius(), 2);
251:
252: Vector3d tmp = new Vector3d();
253:
254: tmp.set(lower);
255: tmp.sub(parentCenter);
256: if (tmp.lengthSquared() > radiusSquared)
257: return false;
258: tmp.set(lower.x + xDim, lower.y, lower.z);
259: tmp.sub(parentCenter);
260: if (tmp.lengthSquared() > radiusSquared)
261: return false;
262: tmp.set(lower.x, lower.y + yDim, lower.z);
263: tmp.sub(parentCenter);
264: if (tmp.lengthSquared() > radiusSquared)
265: return false;
266: tmp.set(lower.x + xDim, lower.y + yDim, lower.z);
267: tmp.sub(parentCenter);
268: if (tmp.lengthSquared() > radiusSquared)
269: return false;
270:
271: tmp.set(upper);
272: tmp.sub(parentCenter);
273: if (tmp.lengthSquared() > radiusSquared)
274: return false;
275: tmp.set(upper.x - xDim, upper.y, upper.z);
276: tmp.sub(parentCenter);
277: if (tmp.lengthSquared() > radiusSquared)
278: return false;
279: tmp.set(upper.x, upper.y - yDim, upper.z);
280: tmp.sub(parentCenter);
281: if (tmp.lengthSquared() > radiusSquared)
282: return false;
283: tmp.set(upper.x - xDim, upper.y - yDim, upper.z);
284: tmp.sub(parentCenter);
285: if (tmp.lengthSquared() > radiusSquared)
286: return false;
287:
288: return true;
289: }
290:
291: /**
292: * Returns true if the parent bounds fully encloses the child
293: */
294: public static boolean encloses(BoundingSphere parent,
295: BoundingSphere child) {
296: Point3d childCenter = new Point3d();
297: Point3d parentCenter = new Point3d();
298: child.getCenter(childCenter);
299: parent.getCenter(parentCenter);
300: double childR = child.getRadius();
301: double parentR = parent.getRadius();
302:
303: if (childCenter.x + childR > parentCenter.x + parentR
304: || childCenter.y + childR > parentCenter.y + parentR
305: || childCenter.z + childR > parentCenter.z + parentR)
306: return false;
307:
308: if (childCenter.x - childR < parentCenter.x - parentR
309: || childCenter.y - childR < parentCenter.y - parentR
310: || childCenter.z - childR < parentCenter.z - parentR)
311: return false;
312:
313: return true;
314: }
315:
316: }
|