1 package org.simantics.g3d.math;
\r
3 import javax.vecmath.AxisAngle4d;
\r
4 import javax.vecmath.Matrix3d;
\r
5 import javax.vecmath.Matrix4d;
\r
6 import javax.vecmath.Quat4d;
\r
7 import javax.vecmath.Tuple3d;
\r
8 import javax.vecmath.Tuple4d;
\r
9 import javax.vecmath.Vector2f;
\r
10 import javax.vecmath.Vector3d;
\r
12 import org.simantics.g3d.math.EulerTools.Order;
\r
16 * Some useful geometry related math functions. Beware, methods may modify their input parameters!
\r
18 * @author Marko Luukkainen
\r
21 public class MathTools {
\r
23 public static double NEAR_ZERO = 0.0000001;
\r
24 public static double NEAR_HALF = 0.4999999;
\r
26 public static final Vector3d Z_AXIS = new Vector3d(0.0,0.0,1.0);
\r
27 public static final Vector3d Y_AXIS = new Vector3d(0.0,1.0,0.0);
\r
28 public static final Vector3d X_AXIS = new Vector3d(1.0,0.0,0.0);
\r
29 public static final Vector3d ORIGIN = new Vector3d(0.0,0.0,0.0);
\r
31 final static double EPS = 1.0e-12;
\r
34 public static boolean equals(double d1, double d2) {
\r
35 return Math.abs(d1-d2) < EPS;
\r
38 public static boolean equals(Tuple3d p1, Tuple3d p2) {
\r
39 return distanceSquared(p1, p2) < NEAR_ZERO;
\r
42 public static boolean equals(Tuple4d p1, Tuple4d p2) {
\r
43 return distanceSquared(p1, p2) < NEAR_ZERO;
\r
46 public static double distance(Tuple3d p1, Tuple3d p2) {
\r
52 return Math.sqrt(dx*dx+dy*dy+dz*dz);
\r
55 public static double distance(Tuple4d p1, Tuple4d p2) {
\r
56 double dx, dy, dz, dw;
\r
62 return Math.sqrt(dx*dx+dy*dy+dz*dz+dw*dw);
\r
65 public static double distanceSquared(Tuple3d p1, Tuple3d p2) {
\r
71 return dx*dx+dy*dy+dz*dz;
\r
74 public static double distanceSquared(Tuple4d p1, Tuple4d p2) {
\r
75 double dx, dy, dz, dw;
\r
81 return dx*dx+dy*dy+dz*dz+dw*dw;
\r
84 public static boolean isValid(Tuple3d t) {
\r
85 return !(Double.isInfinite(t.x) || Double.isNaN(t.x) ||
\r
86 Double.isInfinite(t.y) || Double.isNaN(t.y) ||
\r
87 Double.isInfinite(t.z) || Double.isNaN(t.z));
\r
90 public static Vector3d closestPointOnEdge(Vector3d point, Vector3d edgePoint1, Vector3d edgePoint2) {
\r
91 point.sub(edgePoint1);
\r
92 Vector3d v = new Vector3d(edgePoint2);
\r
94 double t = v.dot(point);
\r
95 t /= v.lengthSquared();
\r
105 public static Vector3d closestPointOnStraight(Tuple3d point, Tuple3d straightPoint, Vector3d straightDir) {
\r
106 Vector3d v = new Vector3d(point);
\r
107 v.sub(straightPoint);
\r
108 double t = straightDir.dot(v);
\r
109 t /= straightDir.lengthSquared();
\r
110 v.set(straightDir);
\r
112 v.add(straightPoint);
\r
116 public static Vector3d closestPointOnStraight(Tuple3d point, Tuple3d straightPoint, Vector3d straightDir, double u[]) {
\r
117 Vector3d v = new Vector3d(point);
\r
118 v.sub(straightPoint);
\r
119 u[0] = straightDir.dot(v);
\r
120 u[0] /= straightDir.lengthSquared();
\r
121 v.set(straightDir);
\r
123 v.add(straightPoint);
\r
127 public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, Tuple3d planePoint) {
\r
128 point.sub(planePoint);
\r
130 return planeNormal.dot(point);
\r
133 public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, float d) {
\r
134 return (planeNormal.dot(point) + d);
\r
137 public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Tuple3d intersectPoint) {
\r
138 intersectPoint.set(planePoint);
\r
139 intersectPoint.sub(linePoint);
\r
140 double u = planeNormal.dot(new Vector3d(intersectPoint));
\r
141 double v = planeNormal.dot(lineDir);
\r
142 if (Math.abs(v) < NEAR_ZERO)
\r
145 intersectPoint.set(lineDir);
\r
146 intersectPoint.scale(u);
\r
147 intersectPoint.add(linePoint);
\r
151 public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Vector3d intersectPoint, double[] u) {
\r
152 intersectPoint.set(planePoint);
\r
153 intersectPoint.sub(linePoint);
\r
154 u[0] = planeNormal.dot(intersectPoint);
\r
155 double v = planeNormal.dot(lineDir);
\r
156 if (Math.abs(v) < NEAR_ZERO)
\r
159 intersectPoint.set(lineDir);
\r
160 intersectPoint.scale(u[0]);
\r
161 intersectPoint.add(linePoint);
\r
165 public static boolean intersectLineLine(Tuple3d l1_start,Tuple3d l1_end,Tuple3d l2_start,Tuple3d l2_end,Tuple3d l1_pos, Tuple3d l2_pos) {
\r
166 Vector3d p13 = new Vector3d();
\r
167 Vector3d p43 = new Vector3d();
\r
168 Vector3d p21 = new Vector3d();
\r
169 double d1343,d4321,d1321,d4343,d2121;
\r
170 double numer,denom;
\r
171 p13.sub(l1_start, l2_start);
\r
172 p43.sub(l2_end,l2_start);
\r
173 if (Math.abs(p43.x) < NEAR_ZERO && Math.abs(p43.y) < NEAR_ZERO && Math.abs(p43.z) < NEAR_ZERO)
\r
175 p21.sub(l1_end,l1_start);
\r
176 if (Math.abs(p21.x) < NEAR_ZERO && Math.abs(p21.y) < NEAR_ZERO && Math.abs(p21.z) < NEAR_ZERO)
\r
179 d1343 = p13.dot(p43);
\r
180 d4321 = p43.dot(p21);
\r
181 d1321 = p13.dot(p21);
\r
182 d4343 = p43.lengthSquared();
\r
183 d2121 = p21.lengthSquared();
\r
185 denom = d2121 * d4343 - d4321 * d4321;
\r
186 if (Math.abs(denom) < NEAR_ZERO)
\r
188 numer = d1343 * d4321 - d1321 * d4343;
\r
190 double mua = numer / denom;
\r
191 double mub = (d1343 + d4321 * mua) / d4343;
\r
193 l1_pos.x = l1_start.x + mua * p21.x;
\r
194 l1_pos.y = l1_start.y + mua * p21.y;
\r
195 l1_pos.z = l1_start.z + mua * p21.z;
\r
196 l2_pos.x = l2_start.x + mub * p43.x;
\r
197 l2_pos.y = l2_start.y + mub * p43.y;
\r
198 l2_pos.z = l2_start.z + mub * p43.z;
\r
203 public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb) {
\r
204 Vector3d p13 = new Vector3d();
\r
206 double d1343,d4321,d1321,d4343,d2121;
\r
207 double numer,denom;
\r
210 if (Math.abs(p43.x) < NEAR_ZERO && Math.abs(p43.y) < NEAR_ZERO && Math.abs(p43.z) < NEAR_ZERO)
\r
212 if (Math.abs(p21.x) < NEAR_ZERO && Math.abs(p21.y) < NEAR_ZERO && Math.abs(p21.z) < NEAR_ZERO)
\r
215 d1343 = p13.dot(p43);
\r
216 d4321 = p43.dot(p21);
\r
217 d1321 = p13.dot(p21);
\r
218 d4343 = p43.lengthSquared();
\r
219 d2121 = p21.lengthSquared();
\r
221 denom = d2121 * d4343 - d4321 * d4321;
\r
222 if (Math.abs(denom) < NEAR_ZERO)
\r
224 numer = d1343 * d4321 - d1321 * d4343;
\r
226 double mua = numer / denom;
\r
227 double mub = (d1343 + d4321 * mua) / d4343;
\r
229 pa.x = p1.x + mua * p21.x;
\r
230 pa.y = p1.y + mua * p21.y;
\r
231 pa.z = p1.z + mua * p21.z;
\r
232 pb.x = p3.x + mub * p43.x;
\r
233 pb.y = p3.y + mub * p43.y;
\r
234 pb.z = p3.z + mub * p43.z;
\r
240 * Calculate the line segment PaPb that is the shortest route between
\r
241 * two lines P1P2 and P3P4. Calculate also the values of mua and mub where
\r
242 * Pa = P1 + mua (P2 - P1)
\r
243 * Pb = P3 + mub (P4 - P3)
\r
253 public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb, double mu[]) {
\r
254 Vector3d p13 = new Vector3d();
\r
256 double d1343,d4321,d1321,d4343,d2121;
\r
257 double numer,denom;
\r
258 double EPS = 0.001;
\r
260 if (Math.abs(p43.x) < EPS && Math.abs(p43.y) < EPS && Math.abs(p43.z) < EPS)
\r
262 if (Math.abs(p21.x) < EPS && Math.abs(p21.y) < EPS && Math.abs(p21.z) < EPS)
\r
265 d1343 = p13.dot(p43);
\r
266 d4321 = p43.dot(p21);
\r
267 d1321 = p13.dot(p21);
\r
268 d4343 = p43.lengthSquared();
\r
269 d2121 = p21.lengthSquared();
\r
271 denom = d2121 * d4343 - d4321 * d4321;
\r
272 if (Math.abs(denom) < EPS)
\r
274 numer = d1343 * d4321 - d1321 * d4343;
\r
276 mu[0] = numer / denom;
\r
277 mu[1] = (d1343 + d4321 * mu[0]) / d4343;
\r
279 pa.x = p1.x + mu[0] * p21.x;
\r
280 pa.y = p1.y + mu[0] * p21.y;
\r
281 pa.z = p1.z + mu[0] * p21.z;
\r
282 pb.x = p3.x + mu[1] * p43.x;
\r
283 pb.y = p3.y + mu[1] * p43.y;
\r
284 pb.z = p3.z + mu[1] * p43.z;
\r
291 public static void rotate(Quat4d q, Tuple3d in, Tuple3d out) {
\r
293 double tw = - q.x*in.x - q.y*in.y - q.z*in.z;
\r
294 double tx = q.w*in.x + q.y*in.z - q.z*in.y;
\r
295 double ty = q.w*in.y - q.x*in.z + q.z*in.x;
\r
296 double tz = q.w*in.z + q.x*in.y - q.y*in.x ;
\r
298 //temp * q' -> x = -x, y = -y z = -z
\r
299 //out.w = tw*q.w + tx*q.x + ty*q.y + tz*q.z;
\r
300 out.x = -tw*q.x + tx*q.w - ty*q.z + tz*q.y;
\r
301 out.y = -tw*q.y + tx*q.z + ty*q.w - tz*q.x;
\r
302 out.z = -tw*q.z - tx*q.y + ty*q.x + tz*q.w;
\r
305 public static void getMatrix(Quat4d quat, Matrix3d m) {
\r
306 m.m00 = 1.0f - 2.0 * (quat.y * quat.y + quat.z * quat.z);
\r
307 m.m01 = 2.0 * (quat.x * quat.y + quat.w * quat.z);
\r
308 m.m02 = 2.0 * (quat.x * quat.z - quat.w * quat.y);
\r
309 m.m10 = 2.0 * (quat.x * quat.y - quat.w * quat.z);
\r
310 m.m11 = 1.0 - 2.0f * (quat.x * quat.x + quat.z * quat.z);
\r
311 m.m12 = 2.0 * (quat.y * quat.z + quat.w * quat.x);
\r
312 m.m20 = 2.0 * (quat.x * quat.z + quat.w * quat.y);
\r
313 m.m21 = 2.0 * (quat.y * quat.z - quat.w * quat.x);
\r
314 m.m22 = 1.0 - 2.0f * (quat.x * quat.x + quat.y * quat.y);
\r
319 private static double q[] = new double[3];
\r
320 private static int nxt[] = { 1, 2, 0 };
\r
322 * Converts Matrix to Quaternion
\r
324 * Note: non-thread safe.
\r
329 public static void getQuat(Matrix3d mat, Quat4d quat) {
\r
330 double tr = mat.m00 + mat.m11 + mat.m22;
\r
332 double s = Math.sqrt(tr + 1.0);
\r
335 quat.x = (mat.m21 - mat.m12) * s;
\r
336 quat.y = (mat.m02 - mat.m20) * s;
\r
337 quat.z = (mat.m10 - mat.m01) * s;
\r
340 if (mat.m11 > mat.m00)
\r
342 if (mat.m22 > mat.getElement(i, i))
\r
349 double s = Math.sqrt((mat.getElement(i, i) - (mat.getElement(j, j) + mat.getElement(k, k))) + 1.0);
\r
353 if (Math.abs(s) > 0.001)
\r
356 quat.w = (mat.getElement(k, j) - mat.getElement(j, k)) * s;
\r
357 q[j] = (mat.getElement(j, i) + mat.getElement(i, j)) * s;
\r
358 q[k] = (mat.getElement(k, i) + mat.getElement(i, k)) * s;
\r
366 public static Quat4d getQuat(Matrix3d mat) {
\r
367 Quat4d q = new Quat4d();
\r
372 public static AxisAngle4d getFromPseudoEuler(Vector3d euler) {
\r
373 AxisAngle4d aa = new AxisAngle4d();
\r
374 aa.angle = euler.length();
\r
375 Vector3d normal = new Vector3d(euler);
\r
376 if (aa.angle > NEAR_ZERO) {
\r
377 normal.normalize();
\r
390 public static Vector3d getPseudoEuler(AxisAngle4d aa) {
\r
391 Vector3d euler = new Vector3d(aa.x,aa.y,aa.z);
\r
392 euler.scale(aa.angle);
\r
397 public static void getQuat(Vector3d euler, Quat4d quat) {
\r
398 Quat4d q = EulerTools.getQuatFromEuler(Order.YXZ, euler.y,euler.x,euler.z);
\r
400 // http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Conversion_formulae_between_formalisms
\r
401 // Using the x-convention, the 3-1-3 Euler angles phi, theta and psi (around the Z, X and again the Z-axis)
\r
402 // quat.x = -Math.cos((euler.x - euler.z)*0.5)*Math.sin(euler.y*0.5);
\r
403 // quat.y = -Math.sin((euler.x - euler.z)*0.5)*Math.sin(euler.y*0.5);
\r
404 // quat.z = -Math.sin((euler.x + euler.z)*0.5)*Math.cos(euler.y*0.5);
\r
405 // quat.w = Math.sin((euler.x + euler.z)*0.5)*Math.cos(euler.y*0.5);
\r
407 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
\r
409 // double c1 = Math.cos(euler.y*0.5);
\r
410 // double s1 = Math.sin(euler.y*0.5);
\r
411 // double c2 = Math.cos(euler.z*0.5);
\r
412 // double s2 = Math.sin(euler.z*0.5);
\r
413 // double c3 = Math.cos(euler.x*0.5);
\r
414 // double s3 = Math.sin(euler.x*0.5);
\r
415 // double c1c2 = c1*c2;
\r
416 // double s1s2 = s1*s2;
\r
417 // quat.w =c1c2*c3 - s1s2*s3;
\r
418 // quat.x =c1c2*s3 + s1s2*c3;
\r
419 // quat.y =s1*c2*c3 + c1*s2*s3;
\r
420 // quat.z =c1*s2*c3 - s1*c2*s3;
\r
422 // Quat4d q2 = EulerTools.getQuatFromEuler(Order.YZX, euler.y,euler.z,euler.x);
\r
423 // System.out.println("Q " + quat + " Q2 " + q2);
\r
424 // double c1 = Math.cos(euler.y);
\r
425 // double s1 = Math.sin(euler.y);
\r
426 // double c2 = Math.cos(euler.z);
\r
427 // double s2 = Math.sin(euler.z);
\r
428 // double c3 = Math.cos(euler.x);
\r
429 // double s3 = Math.sin(euler.x);
\r
430 // quat.w = Math.sqrt(1.0 + c1 * c2 + c1*c3 - s1 * s2 * s3 + c2*c3) / 2.0;
\r
431 // double w4 = (4.0 * quat.w);
\r
432 // quat.x = (c2 * s3 + c1 * s3 + s1 * s2 * c3) / w4 ;
\r
433 // quat.y = (s1 * c2 + s1 * c3 + c1 * s2 * s3) / w4 ;
\r
434 // quat.z = (-s1 * s3 + c1 * s2 * c3 +s2) / w4 ;
\r
440 public static void getEuler(Quat4d quat,Vector3d euler) {
\r
441 Vector3d e = EulerTools.getEulerFromQuat(Order.YXZ, quat);
\r
446 // http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Conversion_formulae_between_formalisms
\r
447 // euler.x = Math.atan2(quat.x * quat.z + quat.y* quat.w, quat.y*quat.z - quat.x * quat.w);
\r
448 // euler.y = Math.acos(-square(quat.x) - square(quat.y) + square(quat.z) + square(quat.w));
\r
449 // euler.z = -Math.atan2(quat.x * quat.z - quat.y* quat.w, quat.y*quat.z + quat.x * quat.w);
\r
451 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
\r
453 // double test = quat.x * quat.y + quat.z * quat.w;
\r
454 // if (test > NEAR_HALF) {
\r
455 // euler.y = 2.0 * Math.atan2(quat.x,quat.w);
\r
456 // euler.z = Math.PI * 0.5;
\r
458 // } else if (test < -NEAR_HALF) {
\r
459 // euler.y = -2.0 * Math.atan2(quat.x,quat.w);
\r
460 // euler.z = -Math.PI * 0.5;
\r
463 // double sqx = square(quat.x);
\r
464 // double sqy = square(quat.y);
\r
465 // double sqz = square(quat.z);
\r
466 // euler.y = Math.atan2(2.0*(quat.y*quat.w-quat.x*quat.z), 1.0 - 2.0*(sqy-sqz));
\r
467 // euler.z = Math.asin(2.0*test);
\r
468 // euler.x = Math.atan2(2.0*(quat.x*quat.w-quat.y*quat.z), 1.0 - 2.0*(sqx-sqz));
\r
469 // System.out.println(euler + " " + EulerTools.getEulerFromQuat(Order.YXZ, quat) + " " + quat);
\r
471 // double sqw = quat.w*quat.w;
\r
472 // double sqx = quat.x*quat.x;
\r
473 // double sqy = quat.y*quat.y;
\r
474 // double sqz = quat.z*quat.z;
\r
475 // double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
\r
476 // double test = quat.x*quat.y + quat.z*quat.w;
\r
477 // if (test > 0.499*unit) { // singularity at north pole
\r
478 // euler.y = 2 * Math.atan2(quat.x,quat.w);
\r
479 // euler.z = Math.PI/2;
\r
483 // if (test < -0.499*unit) { // singularity at south pole
\r
484 // euler.y = -2 * Math.atan2(quat.x,quat.w);
\r
485 // euler.z = -Math.PI/2;
\r
489 // euler.y = Math.atan2(2*quat.y*quat.w-2*quat.x*quat.z , sqx - sqy - sqz + sqw);
\r
490 // euler.z = Math.asin(2*test/unit);
\r
491 // euler.x = Math.atan2(2*quat.x*quat.w-2*quat.y*quat.z , -sqx + sqy - sqz + sqw);
\r
494 public static Quat4d getQuat(Vector3d euler) {
\r
495 Quat4d q = new Quat4d();
\r
501 public static Vector3d getEuler(Quat4d quat) {
\r
502 Vector3d v = new Vector3d();
\r
507 public static Quat4d getQuat(AxisAngle4d aa) {
\r
508 Quat4d q = new Quat4d();
\r
513 public static AxisAngle4d getAxisAngle(Quat4d q) {
\r
514 AxisAngle4d aa = new AxisAngle4d();
\r
519 public static Quat4d getIdentityQuat() {
\r
520 return new Quat4d(0, 0, 0, 1);
\r
523 public static void getQuat(AxisAngle4d aa, Quat4d q) {
\r
525 // Quat = cos(theta/2) + sin(theta/2)(roation_axis)
\r
527 amag = Math.sqrt( aa.x*aa.x + aa.y*aa.y + aa.z*aa.z);
\r
528 if( amag < NEAR_ZERO ) {
\r
535 double a2 = aa.angle * 0.5;
\r
536 mag = Math.sin(a2);
\r
537 q.w = Math.cos(a2);
\r
538 q.x = aa.x*amag*mag;
\r
539 q.y = aa.y*amag*mag;
\r
540 q.z = aa.z*amag*mag;
\r
549 private static final int IN = 0;
\r
550 private static final int LEFT = 1;
\r
551 private static final int RIGHT = 2;
\r
552 private static final int BOTTOM = 4;
\r
553 private static final int TOP = 8;
\r
556 private static int bitcode(Vector2f p1, Vector2f min, Vector2f max) {
\r
560 else if (p1.x > max.x)
\r
564 else if (p1.y > max.y)
\r
569 public static boolean clipLineRectangle(Vector2f p1,Vector2f p2, Vector2f min, Vector2f max, Vector2f r1, Vector2f r2) {
\r
571 int o1 = bitcode(p1, min, max);
\r
572 int o2 = bitcode(p2, min, max);
\r
591 if ((o1 & TOP) != IN) {
\r
592 float t = (max.y - p1.y) / (p2.y - p1.y);
\r
593 p1.x += t * (p2.x - p1.x);
\r
595 } else if ((o1 & BOTTOM) != IN) {
\r
596 float t = (min.y - p1.y) / (p2.y - p1.y);
\r
597 p1.x += t * (p2.x - p1.x);
\r
599 } else if ((o1 & LEFT) != IN) {
\r
600 float t = (min.x - p1.x) / (p2.x - p1.x);
\r
601 p1.y += t * (p2.y - p1.y);
\r
603 } else if ((o1 & RIGHT) != IN) {
\r
604 float t = (max.x - p1.x) / (p2.x - p1.x);
\r
605 p1.y += t * (p2.y - p1.y);
\r
608 throw new RuntimeException("Error in clipping code");
\r
614 public static double square(double d) {
\r
619 public static void multiplyOrientation(AxisAngle4d aa, AxisAngle4d rot) {
\r
620 Quat4d q1 = new Quat4d();
\r
622 Quat4d q2 = new Quat4d();
\r
628 public static double radToDeg(double rad) {
\r
629 return (rad / Math.PI) * 180.0;
\r
632 public static double degToRad(double deg) {
\r
633 return (deg / 180.0) * Math.PI;
\r
636 public static double clamp(double min, double max,double v) {
\r
644 public static AxisAngle4d createRotation(Vector3d original, Vector3d rotated) {
\r
645 AxisAngle4d result = new AxisAngle4d();
\r
646 if (createRotation(original, rotated, result))
\r
652 public static void setIdentity(Quat4d q) {
\r
659 public static void setIdentity(AxisAngle4d aa) {
\r
666 public static void set(Matrix3d mat, double m00, double m01, double m02,
\r
667 double m10, double m11, double m12, double m20, double m21,
\r
682 public static void set(Matrix4d mat, double[] v) {
\r
705 public static boolean createRotation(Vector3d original, Vector3d rotated, AxisAngle4d result) {
\r
707 if (rotated.lengthSquared() > 0.01)
\r
708 rotated.normalize();
\r
711 double d = original.dot(rotated);
\r
713 // original and rotated are parallel, pointing at the same direction
\r
714 result.angle = 0.0;
\r
718 } else if (d < -0.9999) {
\r
719 // original and rotated are parallel, pointing at the opposite direction
\r
720 Vector3d a = Z_AXIS;
\r
721 if (Math.abs(a.dot(original)) > 0.8 )
\r
723 result.set(a, Math.PI);
\r
725 double angle = original.angle(rotated);
\r
726 Vector3d axis = new Vector3d();
\r
727 axis.cross(original, rotated);
\r
728 result.set(axis,angle);
\r
733 public static boolean createRotation(Vector3d original, Vector3d rotated, Quat4d result) {
\r
735 if (rotated.lengthSquared() > 0.01)
\r
736 rotated.normalize();
\r
739 double d = original.dot(rotated);
\r
741 // original and rotated are parallel, pointing at the same direction
\r
746 } else if (d < -0.9999) {
\r
747 // original and rotated are parallel, pointing at the opposite direction
\r
748 Vector3d a = Z_AXIS;
\r
749 if (Math.abs(a.dot(original)) > 0.8 )
\r
751 getQuat(a, Math.PI, result);
\r
754 double angle = original.angle(rotated);
\r
755 Vector3d axis = new Vector3d();
\r
756 axis.cross(original, rotated);
\r
757 getQuat(axis, angle, result);
\r
762 public static void getQuat(Vector3d axis, double angle, Quat4d q)
\r
765 // Quat = cos(theta/2) + sin(theta/2)(roation_axis)
\r
767 amag = Math.sqrt( axis.x*axis.x + axis.y*axis.y + axis.z*axis.z);
\r
775 double a2 = angle*0.5;
\r
776 mag = Math.sin(a2);
\r
777 q.w = Math.cos(a2);
\r
778 q.x = axis.x*amag*mag;
\r
779 q.y = axis.y*amag*mag;
\r
780 q.z = axis.z*amag*mag;
\r
786 * Linear interpolation of quaternions. Result IS set to q1.
\r
791 public static void lip(Quat4d q1, Quat4d q2, double alpha) {
\r
792 double s1 = 1.0 - alpha;
\r
799 public static double dot(Quat4d q1, Quat4d q2) {
\r
800 return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
\r
803 public static void mad(Tuple3d q1, Tuple3d q2, double s2) {
\r
809 public static void mad(Quat4d q1, Quat4d q2, double s2) {
\r
819 * Sets results to q1. Modifies q2.
\r
825 public static void sip(Quat4d q1, Quat4d q2, double alpha) {
\r
826 double cosom = dot(q1,q2);
\r
832 if (cosom > 0.9999) {
\r
839 double theta_0 = Math.acos(cosom);
\r
840 double theta = theta_0 * alpha;
\r
841 Quat4d t = new Quat4d(q1);
\r
845 t.scale(Math.sin(theta));
\r
846 q1.scale(Math.cos(theta));
\r