- Double.isInfinite(t.y) || Double.isNaN(t.y) ||
- Double.isInfinite(t.z) || Double.isNaN(t.z));
- }
-
- public static Vector3d closestPointOnEdge(Vector3d point, Vector3d edgePoint1, Vector3d edgePoint2) {
- point.sub(edgePoint1);
- Vector3d v = new Vector3d(edgePoint2);
- v.sub(edgePoint1);
- double t = v.dot(point);
- t /= v.lengthSquared();
- if (t <= 0.0f)
- return edgePoint1;
- if (t >= 1.0f)
- return edgePoint2;
- v.scale(t);
- v.add(edgePoint1);
- return v;
- }
-
- public static Vector3d closestPointOnStraight(Tuple3d point, Tuple3d straightPoint, Vector3d straightDir) {
- Vector3d v = new Vector3d(point);
- v.sub(straightPoint);
- double t = straightDir.dot(v);
- t /= straightDir.lengthSquared();
- v.set(straightDir);
- v.scale(t);
- v.add(straightPoint);
- return v;
- }
-
- public static Vector3d closestPointOnStraight(Tuple3d point, Tuple3d straightPoint, Vector3d straightDir, double u[]) {
- Vector3d v = new Vector3d(point);
- v.sub(straightPoint);
- u[0] = straightDir.dot(v);
- u[0] /= straightDir.lengthSquared();
- v.set(straightDir);
- v.scale(u[0]);
- v.add(straightPoint);
- return v;
- }
-
- public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, Tuple3d planePoint) {
- point.sub(planePoint);
-
- return planeNormal.dot(point);
- }
-
- public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, float d) {
- return (planeNormal.dot(point) + d);
- }
-
- public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Tuple3d intersectPoint) {
- intersectPoint.set(planePoint);
- intersectPoint.sub(linePoint);
- double u = planeNormal.dot(new Vector3d(intersectPoint));
- double v = planeNormal.dot(lineDir);
- if (Math.abs(v) < NEAR_ZERO)
- return false;
- u /= v;
- intersectPoint.set(lineDir);
- intersectPoint.scale(u);
- intersectPoint.add(linePoint);
- return true;
- }
-
- public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Vector3d intersectPoint, double[] u) {
- intersectPoint.set(planePoint);
- intersectPoint.sub(linePoint);
- u[0] = planeNormal.dot(intersectPoint);
- double v = planeNormal.dot(lineDir);
- if (Math.abs(v) < NEAR_ZERO)
- return false;
- u[0] /= v;
- intersectPoint.set(lineDir);
- intersectPoint.scale(u[0]);
- intersectPoint.add(linePoint);
- return true;
- }
-
- public static boolean intersectLineLine(Tuple3d l1_start,Tuple3d l1_end,Tuple3d l2_start,Tuple3d l2_end,Tuple3d l1_pos, Tuple3d l2_pos) {
- Vector3d p13 = new Vector3d();
- Vector3d p43 = new Vector3d();
- Vector3d p21 = new Vector3d();
- double d1343,d4321,d1321,d4343,d2121;
- double numer,denom;
- p13.sub(l1_start, l2_start);
- p43.sub(l2_end,l2_start);
- if (Math.abs(p43.x) < NEAR_ZERO && Math.abs(p43.y) < NEAR_ZERO && Math.abs(p43.z) < NEAR_ZERO)
- return false;
- p21.sub(l1_end,l1_start);
- if (Math.abs(p21.x) < NEAR_ZERO && Math.abs(p21.y) < NEAR_ZERO && Math.abs(p21.z) < NEAR_ZERO)
- return false;
-
- d1343 = p13.dot(p43);
- d4321 = p43.dot(p21);
- d1321 = p13.dot(p21);
- d4343 = p43.lengthSquared();
- d2121 = p21.lengthSquared();
-
- denom = d2121 * d4343 - d4321 * d4321;
- if (Math.abs(denom) < NEAR_ZERO)
- return false;
- numer = d1343 * d4321 - d1321 * d4343;
-
- double mua = numer / denom;
- double mub = (d1343 + d4321 * mua) / d4343;
-
- l1_pos.x = l1_start.x + mua * p21.x;
- l1_pos.y = l1_start.y + mua * p21.y;
- l1_pos.z = l1_start.z + mua * p21.z;
- l2_pos.x = l2_start.x + mub * p43.x;
- l2_pos.y = l2_start.y + mub * p43.y;
- l2_pos.z = l2_start.z + mub * p43.z;
-
- return true;
- }
-
- public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb) {
- Vector3d p13 = new Vector3d();
-
- double d1343,d4321,d1321,d4343,d2121;
- double numer,denom;
-
- p13.sub(p1, p3);
- if (Math.abs(p43.x) < NEAR_ZERO && Math.abs(p43.y) < NEAR_ZERO && Math.abs(p43.z) < NEAR_ZERO)
- return false;
- if (Math.abs(p21.x) < NEAR_ZERO && Math.abs(p21.y) < NEAR_ZERO && Math.abs(p21.z) < NEAR_ZERO)
- return false;
-
- d1343 = p13.dot(p43);
- d4321 = p43.dot(p21);
- d1321 = p13.dot(p21);
- d4343 = p43.lengthSquared();
- d2121 = p21.lengthSquared();
-
- denom = d2121 * d4343 - d4321 * d4321;
- if (Math.abs(denom) < NEAR_ZERO)
- return false;
- numer = d1343 * d4321 - d1321 * d4343;
-
- double mua = numer / denom;
- double mub = (d1343 + d4321 * mua) / d4343;
-
- pa.x = p1.x + mua * p21.x;
- pa.y = p1.y + mua * p21.y;
- pa.z = p1.z + mua * p21.z;
- pb.x = p3.x + mub * p43.x;
- pb.y = p3.y + mub * p43.y;
- pb.z = p3.z + mub * p43.z;
-
- return true;
- }
-
- /**
- * Calculate the line segment PaPb that is the shortest route between
- * two lines P1P2 and P3P4. Calculate also the values of mua and mub where
- * Pa = P1 + mua (P2 - P1)
- * Pb = P3 + mub (P4 - P3)
- * @param p1
- * @param p21
- * @param p3
- * @param p43
- * @param pa
- * @param pb
- * @param mu
- * @return
- */
- public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb, double mu[]) {
- Vector3d p13 = new Vector3d();
-
- double d1343,d4321,d1321,d4343,d2121;
- double numer,denom;
- double EPS = 0.001;
- p13.sub(p1, p3);
- if (Math.abs(p43.x) < EPS && Math.abs(p43.y) < EPS && Math.abs(p43.z) < EPS)
- return false;
- if (Math.abs(p21.x) < EPS && Math.abs(p21.y) < EPS && Math.abs(p21.z) < EPS)
- return false;
-
- d1343 = p13.dot(p43);
- d4321 = p43.dot(p21);
- d1321 = p13.dot(p21);
- d4343 = p43.lengthSquared();
- d2121 = p21.lengthSquared();
-
- denom = d2121 * d4343 - d4321 * d4321;
- if (Math.abs(denom) < EPS)
- return false;
- numer = d1343 * d4321 - d1321 * d4343;
-
- mu[0] = numer / denom;
- mu[1] = (d1343 + d4321 * mu[0]) / d4343;
-
- pa.x = p1.x + mu[0] * p21.x;
- pa.y = p1.y + mu[0] * p21.y;
- pa.z = p1.z + mu[0] * p21.z;
- pb.x = p3.x + mu[1] * p43.x;
- pb.y = p3.y + mu[1] * p43.y;
- pb.z = p3.z + mu[1] * p43.z;
-
- return true;
- }
-
-
-
- public static void rotate(Quat4d q, Tuple3d in, Tuple3d out) {
- // p' = q * p * q'
- double tw = - q.x*in.x - q.y*in.y - q.z*in.z;
- double tx = q.w*in.x + q.y*in.z - q.z*in.y;
- double ty = q.w*in.y - q.x*in.z + q.z*in.x;
- double tz = q.w*in.z + q.x*in.y - q.y*in.x ;
-
- //temp * q' -> x = -x, y = -y z = -z
- //out.w = tw*q.w + tx*q.x + ty*q.y + tz*q.z;
- out.x = -tw*q.x + tx*q.w - ty*q.z + tz*q.y;
- out.y = -tw*q.y + tx*q.z + ty*q.w - tz*q.x;
- out.z = -tw*q.z - tx*q.y + ty*q.x + tz*q.w;
- }
-
- public static void getMatrix(Quat4d quat, Matrix3d m) {
- m.m00 = 1.0f - 2.0 * (quat.y * quat.y + quat.z * quat.z);
- m.m01 = 2.0 * (quat.x * quat.y + quat.w * quat.z);
- m.m02 = 2.0 * (quat.x * quat.z - quat.w * quat.y);
- m.m10 = 2.0 * (quat.x * quat.y - quat.w * quat.z);
- m.m11 = 1.0 - 2.0f * (quat.x * quat.x + quat.z * quat.z);
- m.m12 = 2.0 * (quat.y * quat.z + quat.w * quat.x);
- m.m20 = 2.0 * (quat.x * quat.z + quat.w * quat.y);
- m.m21 = 2.0 * (quat.y * quat.z - quat.w * quat.x);
- m.m22 = 1.0 - 2.0f * (quat.x * quat.x + quat.y * quat.y);
-
- }
-
-
- private static double q[] = new double[3];
- private static int nxt[] = { 1, 2, 0 };
- /**
- * Converts Matrix to Quaternion
- *
- * Note: non-thread safe.
- *
- * @param mat
- * @param quat
- */
- public static void getQuat(Matrix3d mat, Quat4d quat) {
- double tr = mat.m00 + mat.m11 + mat.m22;
+ Double.isInfinite(t.y) || Double.isNaN(t.y) ||
+ Double.isInfinite(t.z) || Double.isNaN(t.z));
+ }
+
+ public static Vector3d closestPointOnEdge(Vector3d point, Vector3d edgePoint1, Vector3d edgePoint2) {
+ point.sub(edgePoint1);
+ Vector3d v = new Vector3d(edgePoint2);
+ v.sub(edgePoint1);
+ double t = v.dot(point);
+ t /= v.lengthSquared();
+ if (t <= 0.0f)
+ return edgePoint1;
+ if (t >= 1.0f)
+ return edgePoint2;
+ v.scale(t);
+ v.add(edgePoint1);
+ return v;
+ }
+
+ public static Vector3d closestPointOnStraight(Tuple3d point, Tuple3d straightPoint, Vector3d straightDir) {
+ Vector3d v = new Vector3d(point);
+ v.sub(straightPoint);
+ double t = straightDir.dot(v);
+ t /= straightDir.lengthSquared();
+ v.set(straightDir);
+ v.scale(t);
+ v.add(straightPoint);
+ return v;
+ }
+
+ public static Vector3d closestPointOnStraight(Tuple3d point, Tuple3d straightPoint, Vector3d straightDir, double u[]) {
+ Vector3d v = new Vector3d(point);
+ v.sub(straightPoint);
+ u[0] = straightDir.dot(v);
+ u[0] /= straightDir.lengthSquared();
+ v.set(straightDir);
+ v.scale(u[0]);
+ v.add(straightPoint);
+ return v;
+ }
+
+ public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, Tuple3d planePoint) {
+ point.sub(planePoint);
+
+ return planeNormal.dot(point);
+ }
+
+ public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, float d) {
+ return (planeNormal.dot(point) + d);
+ }
+
+ public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Tuple3d intersectPoint) {
+ intersectPoint.set(planePoint);
+ intersectPoint.sub(linePoint);
+ double u = planeNormal.dot(new Vector3d(intersectPoint));
+ double v = planeNormal.dot(lineDir);
+ if (Math.abs(v) < NEAR_ZERO)
+ return false;
+ u /= v;
+ intersectPoint.set(lineDir);
+ intersectPoint.scale(u);
+ intersectPoint.add(linePoint);
+ return true;
+ }
+
+ public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Vector3d intersectPoint, double[] u) {
+ intersectPoint.set(planePoint);
+ intersectPoint.sub(linePoint);
+ u[0] = planeNormal.dot(intersectPoint);
+ double v = planeNormal.dot(lineDir);
+ if (Math.abs(v) < NEAR_ZERO)
+ return false;
+ u[0] /= v;
+ intersectPoint.set(lineDir);
+ intersectPoint.scale(u[0]);
+ intersectPoint.add(linePoint);
+ return true;
+ }
+
+ public static boolean intersectLineLine(Tuple3d l1_start,Tuple3d l1_end,Tuple3d l2_start,Tuple3d l2_end,Tuple3d l1_pos, Tuple3d l2_pos) {
+ Vector3d p13 = new Vector3d();
+ Vector3d p43 = new Vector3d();
+ Vector3d p21 = new Vector3d();
+ double d1343,d4321,d1321,d4343,d2121;
+ double numer,denom;
+ p13.sub(l1_start, l2_start);
+ p43.sub(l2_end,l2_start);
+ if (Math.abs(p43.x) < NEAR_ZERO && Math.abs(p43.y) < NEAR_ZERO && Math.abs(p43.z) < NEAR_ZERO)
+ return false;
+ p21.sub(l1_end,l1_start);
+ if (Math.abs(p21.x) < NEAR_ZERO && Math.abs(p21.y) < NEAR_ZERO && Math.abs(p21.z) < NEAR_ZERO)
+ return false;
+
+ d1343 = p13.dot(p43);
+ d4321 = p43.dot(p21);
+ d1321 = p13.dot(p21);
+ d4343 = p43.lengthSquared();
+ d2121 = p21.lengthSquared();
+
+ denom = d2121 * d4343 - d4321 * d4321;
+ if (Math.abs(denom) < NEAR_ZERO)
+ return false;
+ numer = d1343 * d4321 - d1321 * d4343;
+
+ double mua = numer / denom;
+ double mub = (d1343 + d4321 * mua) / d4343;
+
+ l1_pos.x = l1_start.x + mua * p21.x;
+ l1_pos.y = l1_start.y + mua * p21.y;
+ l1_pos.z = l1_start.z + mua * p21.z;
+ l2_pos.x = l2_start.x + mub * p43.x;
+ l2_pos.y = l2_start.y + mub * p43.y;
+ l2_pos.z = l2_start.z + mub * p43.z;
+
+ return true;
+ }
+
+ public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb) {
+ Vector3d p13 = new Vector3d();
+
+ double d1343,d4321,d1321,d4343,d2121;
+ double numer,denom;
+
+ p13.sub(p1, p3);
+ if (Math.abs(p43.x) < NEAR_ZERO && Math.abs(p43.y) < NEAR_ZERO && Math.abs(p43.z) < NEAR_ZERO)
+ return false;
+ if (Math.abs(p21.x) < NEAR_ZERO && Math.abs(p21.y) < NEAR_ZERO && Math.abs(p21.z) < NEAR_ZERO)
+ return false;
+
+ d1343 = p13.dot(p43);
+ d4321 = p43.dot(p21);
+ d1321 = p13.dot(p21);
+ d4343 = p43.lengthSquared();
+ d2121 = p21.lengthSquared();
+
+ denom = d2121 * d4343 - d4321 * d4321;
+ if (Math.abs(denom) < NEAR_ZERO)
+ return false;
+ numer = d1343 * d4321 - d1321 * d4343;
+
+ double mua = numer / denom;
+ double mub = (d1343 + d4321 * mua) / d4343;
+
+ pa.x = p1.x + mua * p21.x;
+ pa.y = p1.y + mua * p21.y;
+ pa.z = p1.z + mua * p21.z;
+ pb.x = p3.x + mub * p43.x;
+ pb.y = p3.y + mub * p43.y;
+ pb.z = p3.z + mub * p43.z;
+
+ return true;
+ }
+
+ /**
+ * Calculate the line segment PaPb that is the shortest route between
+ * two lines P1P2 and P3P4. Calculate also the values of mua and mub where
+ * Pa = P1 + mua (P2 - P1)
+ * Pb = P3 + mub (P4 - P3)
+ * @param p1
+ * @param p21
+ * @param p3
+ * @param p43
+ * @param pa
+ * @param pb
+ * @param mu
+ * @return
+ */
+ public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb, double mu[]) {
+ Vector3d p13 = new Vector3d();
+
+ double d1343,d4321,d1321,d4343,d2121;
+ double numer,denom;
+ double EPS = 0.001;
+ p13.sub(p1, p3);
+ if (Math.abs(p43.x) < EPS && Math.abs(p43.y) < EPS && Math.abs(p43.z) < EPS)
+ return false;
+ if (Math.abs(p21.x) < EPS && Math.abs(p21.y) < EPS && Math.abs(p21.z) < EPS)
+ return false;
+
+ d1343 = p13.dot(p43);
+ d4321 = p43.dot(p21);
+ d1321 = p13.dot(p21);
+ d4343 = p43.lengthSquared();
+ d2121 = p21.lengthSquared();
+
+ denom = d2121 * d4343 - d4321 * d4321;
+ if (Math.abs(denom) < EPS)
+ return false;
+ numer = d1343 * d4321 - d1321 * d4343;
+
+ mu[0] = numer / denom;
+ mu[1] = (d1343 + d4321 * mu[0]) / d4343;
+
+ pa.x = p1.x + mu[0] * p21.x;
+ pa.y = p1.y + mu[0] * p21.y;
+ pa.z = p1.z + mu[0] * p21.z;
+ pb.x = p3.x + mu[1] * p43.x;
+ pb.y = p3.y + mu[1] * p43.y;
+ pb.z = p3.z + mu[1] * p43.z;
+
+ return true;
+ }
+
+
+
+ public static void rotate(Quat4d q, Tuple3d in, Tuple3d out) {
+ // p' = q * p * q'
+ double tw = - q.x*in.x - q.y*in.y - q.z*in.z;
+ double tx = q.w*in.x + q.y*in.z - q.z*in.y;
+ double ty = q.w*in.y - q.x*in.z + q.z*in.x;
+ double tz = q.w*in.z + q.x*in.y - q.y*in.x ;
+
+ //temp * q' -> x = -x, y = -y z = -z
+ //out.w = tw*q.w + tx*q.x + ty*q.y + tz*q.z;
+ out.x = -tw*q.x + tx*q.w - ty*q.z + tz*q.y;
+ out.y = -tw*q.y + tx*q.z + ty*q.w - tz*q.x;
+ out.z = -tw*q.z - tx*q.y + ty*q.x + tz*q.w;
+ }
+
+ public static void getMatrix(Quat4d quat, Matrix3d m) {
+ m.m00 = 1.0f - 2.0 * (quat.y * quat.y + quat.z * quat.z);
+ m.m01 = 2.0 * (quat.x * quat.y + quat.w * quat.z);
+ m.m02 = 2.0 * (quat.x * quat.z - quat.w * quat.y);
+ m.m10 = 2.0 * (quat.x * quat.y - quat.w * quat.z);
+ m.m11 = 1.0 - 2.0f * (quat.x * quat.x + quat.z * quat.z);
+ m.m12 = 2.0 * (quat.y * quat.z + quat.w * quat.x);
+ m.m20 = 2.0 * (quat.x * quat.z + quat.w * quat.y);
+ m.m21 = 2.0 * (quat.y * quat.z - quat.w * quat.x);
+ m.m22 = 1.0 - 2.0f * (quat.x * quat.x + quat.y * quat.y);
+
+ }
+
+ public static void getMatrix(Quat4d quat, Matrix4d m) {
+ m.setZero();
+ m.m00 = 1.0f - 2.0 * (quat.y * quat.y + quat.z * quat.z);
+ m.m01 = 2.0 * (quat.x * quat.y + quat.w * quat.z);
+ m.m02 = 2.0 * (quat.x * quat.z - quat.w * quat.y);
+ m.m10 = 2.0 * (quat.x * quat.y - quat.w * quat.z);
+ m.m11 = 1.0 - 2.0f * (quat.x * quat.x + quat.z * quat.z);
+ m.m12 = 2.0 * (quat.y * quat.z + quat.w * quat.x);
+ m.m20 = 2.0 * (quat.x * quat.z + quat.w * quat.y);
+ m.m21 = 2.0 * (quat.y * quat.z - quat.w * quat.x);
+ m.m22 = 1.0 - 2.0f * (quat.x * quat.x + quat.y * quat.y);
+ m.m33 = 1.0;
+ }
+
+ private static double q[] = new double[3];
+ private static int nxt[] = { 1, 2, 0 };
+ /**
+ * Converts Matrix to Quaternion
+ *
+ * Note: non-thread safe.
+ *
+ * @param mat
+ * @param quat
+ */
+ public static void getQuat(Matrix3d mat, Quat4d quat) {
+ double tr = mat.m00 + mat.m11 + mat.m22;