1 /*******************************************************************************
2 * Copyright (c) 2012, 2013 Association for Decentralized Information Management in
4 * All rights reserved. This program and the accompanying materials
5 * are made available under the terms of the Eclipse Public License v1.0
6 * which accompanies this distribution, and is available at
7 * http://www.eclipse.org/legal/epl-v10.html
10 * VTT Technical Research Centre of Finland - initial API and implementation
11 *******************************************************************************/
12 package org.simantics.g3d.math;
14 import javax.vecmath.AxisAngle4d;
15 import javax.vecmath.Matrix3d;
16 import javax.vecmath.Matrix4d;
17 import javax.vecmath.Quat4d;
18 import javax.vecmath.Tuple2d;
19 import javax.vecmath.Tuple3d;
20 import javax.vecmath.Tuple4d;
21 import javax.vecmath.Vector2d;
22 import javax.vecmath.Vector2f;
23 import javax.vecmath.Vector3d;
25 import org.simantics.g3d.math.EulerTools.Order;
29 * Some useful geometry related math functions. Beware, methods may modify their input parameters!
31 * @author Marko Luukkainen
34 public class MathTools {
36 public static double NEAR_ZERO = 0.000000001;
37 public static double NEAR_HALF = 0.499999999;
39 public static final Vector3d Z_AXIS = new Vector3d(0.0,0.0,1.0);
40 public static final Vector3d Y_AXIS = new Vector3d(0.0,1.0,0.0);
41 public static final Vector3d X_AXIS = new Vector3d(1.0,0.0,0.0);
42 public static final Vector3d ORIGIN = new Vector3d(0.0,0.0,0.0);
44 final static double EPS = 1.0e-12;
47 public static boolean equals(double d1, double d2) {
48 return Math.abs(d1-d2) < EPS;
51 public static boolean equals(Tuple3d p1, Tuple3d p2) {
52 return distanceSquared(p1, p2) < NEAR_ZERO;
55 public static boolean equals(Tuple4d p1, Tuple4d p2) {
56 return distanceSquared(p1, p2) < NEAR_ZERO;
59 public static double distance(Tuple3d p1, Tuple3d p2) {
65 return Math.sqrt(dx*dx+dy*dy+dz*dz);
68 public static double distance(Tuple4d p1, Tuple4d p2) {
69 double dx, dy, dz, dw;
75 return Math.sqrt(dx*dx+dy*dy+dz*dz+dw*dw);
78 public static double distanceSquared(Tuple3d p1, Tuple3d p2) {
84 return dx*dx+dy*dy+dz*dz;
87 public static double distanceSquared(Tuple4d p1, Tuple4d p2) {
88 double dx, dy, dz, dw;
94 return dx*dx+dy*dy+dz*dz+dw*dw;
97 public static boolean isValid(Tuple3d t) {
98 return !(Double.isInfinite(t.x) || Double.isNaN(t.x) ||
99 Double.isInfinite(t.y) || Double.isNaN(t.y) ||
100 Double.isInfinite(t.z) || Double.isNaN(t.z));
103 public static Vector3d closestPointOnEdge(Vector3d point, Vector3d edgePoint1, Vector3d edgePoint2) {
104 point.sub(edgePoint1);
105 Vector3d v = new Vector3d(edgePoint2);
107 double t = v.dot(point);
108 t /= v.lengthSquared();
118 public static Vector3d closestPointOnStraight(Tuple3d point, Tuple3d straightPoint, Vector3d straightDir) {
119 Vector3d v = new Vector3d(point);
120 v.sub(straightPoint);
121 double t = straightDir.dot(v);
122 t /= straightDir.lengthSquared();
125 v.add(straightPoint);
129 public static Vector3d closestPointOnStraight(Tuple3d point, Tuple3d straightPoint, Vector3d straightDir, double u[]) {
130 Vector3d v = new Vector3d(point);
131 v.sub(straightPoint);
132 u[0] = straightDir.dot(v);
133 u[0] /= straightDir.lengthSquared();
136 v.add(straightPoint);
140 public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, Tuple3d planePoint) {
141 point.sub(planePoint);
143 return planeNormal.dot(point);
146 public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, float d) {
147 return (planeNormal.dot(point) + d);
150 public static Vector3d projectToPlane(Vector3d v, Vector3d planeNormal) {
152 //planeNormal.normalize();
153 Vector3d t = new Vector3d();
154 if (planeNormal == X_AXIS) {
157 else if (planeNormal == Y_AXIS) {
160 else if (planeNormal == Z_AXIS) {
164 t.cross(v,planeNormal);
165 t.cross(planeNormal, t);
171 public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Tuple3d intersectPoint) {
172 intersectPoint.set(planePoint);
173 intersectPoint.sub(linePoint);
174 double u = planeNormal.dot(new Vector3d(intersectPoint));
175 double v = planeNormal.dot(lineDir);
176 if (Math.abs(v) < NEAR_ZERO)
179 intersectPoint.set(lineDir);
180 intersectPoint.scale(u);
181 intersectPoint.add(linePoint);
185 public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Vector3d intersectPoint, double[] u) {
186 intersectPoint.set(planePoint);
187 intersectPoint.sub(linePoint);
188 u[0] = planeNormal.dot(intersectPoint);
189 double v = planeNormal.dot(lineDir);
190 if (Math.abs(v) < NEAR_ZERO)
193 intersectPoint.set(lineDir);
194 intersectPoint.scale(u[0]);
195 intersectPoint.add(linePoint);
199 public static boolean intersectLineLine(Tuple3d l1_start,Tuple3d l1_end,Tuple3d l2_start,Tuple3d l2_end,Tuple3d l1_pos, Tuple3d l2_pos) {
200 Vector3d p13 = new Vector3d();
201 Vector3d p43 = new Vector3d();
202 Vector3d p21 = new Vector3d();
203 double d1343,d4321,d1321,d4343,d2121;
205 p13.sub(l1_start, l2_start);
206 p43.sub(l2_end,l2_start);
207 if (Math.abs(p43.x) < NEAR_ZERO && Math.abs(p43.y) < NEAR_ZERO && Math.abs(p43.z) < NEAR_ZERO)
209 p21.sub(l1_end,l1_start);
210 if (Math.abs(p21.x) < NEAR_ZERO && Math.abs(p21.y) < NEAR_ZERO && Math.abs(p21.z) < NEAR_ZERO)
213 d1343 = p13.dot(p43);
214 d4321 = p43.dot(p21);
215 d1321 = p13.dot(p21);
216 d4343 = p43.lengthSquared();
217 d2121 = p21.lengthSquared();
219 denom = d2121 * d4343 - d4321 * d4321;
220 if (Math.abs(denom) < NEAR_ZERO)
222 numer = d1343 * d4321 - d1321 * d4343;
224 double mua = numer / denom;
225 double mub = (d1343 + d4321 * mua) / d4343;
227 l1_pos.x = l1_start.x + mua * p21.x;
228 l1_pos.y = l1_start.y + mua * p21.y;
229 l1_pos.z = l1_start.z + mua * p21.z;
230 l2_pos.x = l2_start.x + mub * p43.x;
231 l2_pos.y = l2_start.y + mub * p43.y;
232 l2_pos.z = l2_start.z + mub * p43.z;
237 public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb) {
238 Vector3d p13 = new Vector3d();
240 double d1343,d4321,d1321,d4343,d2121;
244 if (Math.abs(p43.x) < NEAR_ZERO && Math.abs(p43.y) < NEAR_ZERO && Math.abs(p43.z) < NEAR_ZERO)
246 if (Math.abs(p21.x) < NEAR_ZERO && Math.abs(p21.y) < NEAR_ZERO && Math.abs(p21.z) < NEAR_ZERO)
249 d1343 = p13.dot(p43);
250 d4321 = p43.dot(p21);
251 d1321 = p13.dot(p21);
252 d4343 = p43.lengthSquared();
253 d2121 = p21.lengthSquared();
255 denom = d2121 * d4343 - d4321 * d4321;
256 if (Math.abs(denom) < NEAR_ZERO)
258 numer = d1343 * d4321 - d1321 * d4343;
260 double mua = numer / denom;
261 double mub = (d1343 + d4321 * mua) / d4343;
263 pa.x = p1.x + mua * p21.x;
264 pa.y = p1.y + mua * p21.y;
265 pa.z = p1.z + mua * p21.z;
266 pb.x = p3.x + mub * p43.x;
267 pb.y = p3.y + mub * p43.y;
268 pb.z = p3.z + mub * p43.z;
274 * Calculate the line segment PaPb that is the shortest route between
275 * two lines P1P2 and P3P4. Calculate also the values of mua and mub where
276 * Pa = P1 + mua (P2 - P1)
277 * Pb = P3 + mub (P4 - P3)
287 public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb, double mu[]) {
288 Vector3d p13 = new Vector3d();
290 double d1343,d4321,d1321,d4343,d2121;
294 if (Math.abs(p43.x) < EPS && Math.abs(p43.y) < EPS && Math.abs(p43.z) < EPS)
296 if (Math.abs(p21.x) < EPS && Math.abs(p21.y) < EPS && Math.abs(p21.z) < EPS)
299 d1343 = p13.dot(p43);
300 d4321 = p43.dot(p21);
301 d1321 = p13.dot(p21);
302 d4343 = p43.lengthSquared();
303 d2121 = p21.lengthSquared();
305 denom = d2121 * d4343 - d4321 * d4321;
306 if (Math.abs(denom) < EPS)
308 numer = d1343 * d4321 - d1321 * d4343;
310 mu[0] = numer / denom;
311 mu[1] = (d1343 + d4321 * mu[0]) / d4343;
313 pa.x = p1.x + mu[0] * p21.x;
314 pa.y = p1.y + mu[0] * p21.y;
315 pa.z = p1.z + mu[0] * p21.z;
316 pb.x = p3.x + mu[1] * p43.x;
317 pb.y = p3.y + mu[1] * p43.y;
318 pb.z = p3.z + mu[1] * p43.z;
325 public static void rotate(Quat4d q, Tuple3d in, Tuple3d out) {
327 double tw = - q.x*in.x - q.y*in.y - q.z*in.z;
328 double tx = q.w*in.x + q.y*in.z - q.z*in.y;
329 double ty = q.w*in.y - q.x*in.z + q.z*in.x;
330 double tz = q.w*in.z + q.x*in.y - q.y*in.x ;
332 //temp * q' -> x = -x, y = -y z = -z
333 //out.w = tw*q.w + tx*q.x + ty*q.y + tz*q.z;
334 out.x = -tw*q.x + tx*q.w - ty*q.z + tz*q.y;
335 out.y = -tw*q.y + tx*q.z + ty*q.w - tz*q.x;
336 out.z = -tw*q.z - tx*q.y + ty*q.x + tz*q.w;
339 public static void getMatrix(Quat4d quat, Matrix3d m) {
340 m.m00 = 1.0f - 2.0 * (quat.y * quat.y + quat.z * quat.z);
341 m.m01 = 2.0 * (quat.x * quat.y + quat.w * quat.z);
342 m.m02 = 2.0 * (quat.x * quat.z - quat.w * quat.y);
343 m.m10 = 2.0 * (quat.x * quat.y - quat.w * quat.z);
344 m.m11 = 1.0 - 2.0f * (quat.x * quat.x + quat.z * quat.z);
345 m.m12 = 2.0 * (quat.y * quat.z + quat.w * quat.x);
346 m.m20 = 2.0 * (quat.x * quat.z + quat.w * quat.y);
347 m.m21 = 2.0 * (quat.y * quat.z - quat.w * quat.x);
348 m.m22 = 1.0 - 2.0f * (quat.x * quat.x + quat.y * quat.y);
352 public static void getMatrix(Quat4d quat, Matrix4d m) {
354 m.m00 = 1.0f - 2.0 * (quat.y * quat.y + quat.z * quat.z);
355 m.m01 = 2.0 * (quat.x * quat.y + quat.w * quat.z);
356 m.m02 = 2.0 * (quat.x * quat.z - quat.w * quat.y);
357 m.m10 = 2.0 * (quat.x * quat.y - quat.w * quat.z);
358 m.m11 = 1.0 - 2.0f * (quat.x * quat.x + quat.z * quat.z);
359 m.m12 = 2.0 * (quat.y * quat.z + quat.w * quat.x);
360 m.m20 = 2.0 * (quat.x * quat.z + quat.w * quat.y);
361 m.m21 = 2.0 * (quat.y * quat.z - quat.w * quat.x);
362 m.m22 = 1.0 - 2.0f * (quat.x * quat.x + quat.y * quat.y);
366 private static double q[] = new double[3];
367 private static int nxt[] = { 1, 2, 0 };
369 * Converts Matrix to Quaternion
371 * Note: non-thread safe.
376 public static void getQuat(Matrix3d mat, Quat4d quat) {
377 double tr = mat.m00 + mat.m11 + mat.m22;
379 double s = Math.sqrt(tr + 1.0);
382 quat.x = (mat.m21 - mat.m12) * s;
383 quat.y = (mat.m02 - mat.m20) * s;
384 quat.z = (mat.m10 - mat.m01) * s;
387 if (mat.m11 > mat.m00)
389 if (mat.m22 > mat.getElement(i, i))
396 double s = Math.sqrt((mat.getElement(i, i) - (mat.getElement(j, j) + mat.getElement(k, k))) + 1.0);
400 if (Math.abs(s) > 0.001)
403 quat.w = (mat.getElement(k, j) - mat.getElement(j, k)) * s;
404 q[j] = (mat.getElement(j, i) + mat.getElement(i, j)) * s;
405 q[k] = (mat.getElement(k, i) + mat.getElement(i, k)) * s;
413 public static Quat4d getQuat(Matrix3d mat) {
414 Quat4d q = new Quat4d();
419 public static AxisAngle4d getFromPseudoEuler(Vector3d euler) {
420 AxisAngle4d aa = new AxisAngle4d();
421 aa.angle = euler.length();
422 Vector3d normal = new Vector3d(euler);
423 if (aa.angle > NEAR_ZERO) {
437 public static Vector3d getPseudoEuler(AxisAngle4d aa) {
438 Vector3d euler = new Vector3d(aa.x,aa.y,aa.z);
439 euler.scale(aa.angle);
444 public static void getQuat(Vector3d euler, Quat4d quat) {
445 Quat4d q = EulerTools.getQuatFromEuler(Order.YXZ, euler.y,euler.x,euler.z);
447 // http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Conversion_formulae_between_formalisms
448 // Using the x-convention, the 3-1-3 Euler angles phi, theta and psi (around the Z, X and again the Z-axis)
449 // quat.x = -Math.cos((euler.x - euler.z)*0.5)*Math.sin(euler.y*0.5);
450 // quat.y = -Math.sin((euler.x - euler.z)*0.5)*Math.sin(euler.y*0.5);
451 // quat.z = -Math.sin((euler.x + euler.z)*0.5)*Math.cos(euler.y*0.5);
452 // quat.w = Math.sin((euler.x + euler.z)*0.5)*Math.cos(euler.y*0.5);
454 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
456 // double c1 = Math.cos(euler.y*0.5);
457 // double s1 = Math.sin(euler.y*0.5);
458 // double c2 = Math.cos(euler.z*0.5);
459 // double s2 = Math.sin(euler.z*0.5);
460 // double c3 = Math.cos(euler.x*0.5);
461 // double s3 = Math.sin(euler.x*0.5);
462 // double c1c2 = c1*c2;
463 // double s1s2 = s1*s2;
464 // quat.w =c1c2*c3 - s1s2*s3;
465 // quat.x =c1c2*s3 + s1s2*c3;
466 // quat.y =s1*c2*c3 + c1*s2*s3;
467 // quat.z =c1*s2*c3 - s1*c2*s3;
469 // Quat4d q2 = EulerTools.getQuatFromEuler(Order.YZX, euler.y,euler.z,euler.x);
470 // System.out.println("Q " + quat + " Q2 " + q2);
471 // double c1 = Math.cos(euler.y);
472 // double s1 = Math.sin(euler.y);
473 // double c2 = Math.cos(euler.z);
474 // double s2 = Math.sin(euler.z);
475 // double c3 = Math.cos(euler.x);
476 // double s3 = Math.sin(euler.x);
477 // quat.w = Math.sqrt(1.0 + c1 * c2 + c1*c3 - s1 * s2 * s3 + c2*c3) / 2.0;
478 // double w4 = (4.0 * quat.w);
479 // quat.x = (c2 * s3 + c1 * s3 + s1 * s2 * c3) / w4 ;
480 // quat.y = (s1 * c2 + s1 * c3 + c1 * s2 * s3) / w4 ;
481 // quat.z = (-s1 * s3 + c1 * s2 * c3 +s2) / w4 ;
487 public static void getEuler(Quat4d quat,Vector3d euler) {
488 Vector3d e = EulerTools.getEulerFromQuat(Order.YXZ, quat);
493 // http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Conversion_formulae_between_formalisms
494 // euler.x = Math.atan2(quat.x * quat.z + quat.y* quat.w, quat.y*quat.z - quat.x * quat.w);
495 // euler.y = Math.acos(-square(quat.x) - square(quat.y) + square(quat.z) + square(quat.w));
496 // euler.z = -Math.atan2(quat.x * quat.z - quat.y* quat.w, quat.y*quat.z + quat.x * quat.w);
498 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
500 // double test = quat.x * quat.y + quat.z * quat.w;
501 // if (test > NEAR_HALF) {
502 // euler.y = 2.0 * Math.atan2(quat.x,quat.w);
503 // euler.z = Math.PI * 0.5;
505 // } else if (test < -NEAR_HALF) {
506 // euler.y = -2.0 * Math.atan2(quat.x,quat.w);
507 // euler.z = -Math.PI * 0.5;
510 // double sqx = square(quat.x);
511 // double sqy = square(quat.y);
512 // double sqz = square(quat.z);
513 // euler.y = Math.atan2(2.0*(quat.y*quat.w-quat.x*quat.z), 1.0 - 2.0*(sqy-sqz));
514 // euler.z = Math.asin(2.0*test);
515 // euler.x = Math.atan2(2.0*(quat.x*quat.w-quat.y*quat.z), 1.0 - 2.0*(sqx-sqz));
516 // System.out.println(euler + " " + EulerTools.getEulerFromQuat(Order.YXZ, quat) + " " + quat);
518 // double sqw = quat.w*quat.w;
519 // double sqx = quat.x*quat.x;
520 // double sqy = quat.y*quat.y;
521 // double sqz = quat.z*quat.z;
522 // double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
523 // double test = quat.x*quat.y + quat.z*quat.w;
524 // if (test > 0.499*unit) { // singularity at north pole
525 // euler.y = 2 * Math.atan2(quat.x,quat.w);
526 // euler.z = Math.PI/2;
530 // if (test < -0.499*unit) { // singularity at south pole
531 // euler.y = -2 * Math.atan2(quat.x,quat.w);
532 // euler.z = -Math.PI/2;
536 // euler.y = Math.atan2(2*quat.y*quat.w-2*quat.x*quat.z , sqx - sqy - sqz + sqw);
537 // euler.z = Math.asin(2*test/unit);
538 // euler.x = Math.atan2(2*quat.x*quat.w-2*quat.y*quat.z , -sqx + sqy - sqz + sqw);
541 public static Quat4d getQuat(Vector3d euler) {
542 Quat4d q = new Quat4d();
548 public static Vector3d getEuler(Quat4d quat) {
549 Vector3d v = new Vector3d();
554 public static Quat4d getQuat(AxisAngle4d aa) {
555 Quat4d q = new Quat4d();
560 public static AxisAngle4d getAxisAngle(Quat4d q) {
561 AxisAngle4d aa = new AxisAngle4d();
562 double mag = q.x * q.x + q.y * q.y + q.z * q.z;
565 mag = Math.sqrt(mag);
566 aa.angle = 2.0 * Math.atan2(mag, q.w);
582 public static Quat4d getIdentityQuat() {
583 return new Quat4d(0, 0, 0, 1);
586 public static void getQuat(AxisAngle4d aa, Quat4d q) {
588 // Quat = cos(theta/2) + sin(theta/2)(roation_axis)
590 amag = Math.sqrt( aa.x*aa.x + aa.y*aa.y + aa.z*aa.z);
591 if( amag < NEAR_ZERO ) {
598 double a2 = aa.angle * 0.5;
612 private static final int IN = 0;
613 private static final int LEFT = 1;
614 private static final int RIGHT = 2;
615 private static final int BOTTOM = 4;
616 private static final int TOP = 8;
619 private static int bitcode(Vector2f p1, Vector2f min, Vector2f max) {
623 else if (p1.x > max.x)
627 else if (p1.y > max.y)
632 public static boolean clipLineRectangle(Vector2f p1,Vector2f p2, Vector2f min, Vector2f max, Vector2f r1, Vector2f r2) {
634 int o1 = bitcode(p1, min, max);
635 int o2 = bitcode(p2, min, max);
654 if ((o1 & TOP) != IN) {
655 float t = (max.y - p1.y) / (p2.y - p1.y);
656 p1.x += t * (p2.x - p1.x);
658 } else if ((o1 & BOTTOM) != IN) {
659 float t = (min.y - p1.y) / (p2.y - p1.y);
660 p1.x += t * (p2.x - p1.x);
662 } else if ((o1 & LEFT) != IN) {
663 float t = (min.x - p1.x) / (p2.x - p1.x);
664 p1.y += t * (p2.y - p1.y);
666 } else if ((o1 & RIGHT) != IN) {
667 float t = (max.x - p1.x) / (p2.x - p1.x);
668 p1.y += t * (p2.y - p1.y);
671 throw new RuntimeException("Error in clipping code");
677 public static double square(double d) {
682 public static void multiplyOrientation(AxisAngle4d aa, AxisAngle4d rot) {
683 Quat4d q1 = new Quat4d();
685 Quat4d q2 = new Quat4d();
691 public static double radToDeg(double rad) {
692 return (rad / Math.PI) * 180.0;
695 public static double degToRad(double deg) {
696 return (deg / 180.0) * Math.PI;
699 public static double clamp(double min, double max,double v) {
707 public static AxisAngle4d createRotation(Vector3d original, Vector3d rotated) {
708 AxisAngle4d result = new AxisAngle4d();
709 if (createRotation(original, rotated, result))
715 public static void setIdentity(Quat4d q) {
722 public static void setIdentity(AxisAngle4d aa) {
729 public static void set(Matrix3d mat, double m00, double m01, double m02,
730 double m10, double m11, double m12, double m20, double m21,
745 public static void set(Matrix4d mat, double[] v) {
768 public static boolean createRotation(Vector3d original, Vector3d rotated, AxisAngle4d result) {
770 if (rotated.lengthSquared() > 0.01)
774 double d = original.dot(rotated);
776 // original and rotated are parallel, pointing at the same direction
781 } else if (d < -0.9999) {
782 // original and rotated are parallel, pointing at the opposite direction
783 Vector3d a = projectToPlane(getMinAxis(original), original);
785 result.set(a, Math.PI);
787 double angle = original.angle(rotated);
788 Vector3d axis = new Vector3d();
789 axis.cross(original, rotated);
790 result.set(axis,angle);
795 public static boolean createRotation(Vector3d original, Vector3d rotated, Quat4d result) {
797 if (rotated.lengthSquared() > 0.01)
801 double d = original.dot(rotated);
803 // original and rotated are parallel, pointing at the same direction
808 } else if (d < -0.9999) {
809 // original and rotated are parallel, pointing at the opposite direction
810 Vector3d a = projectToPlane(getMinAxis(original), original);
812 result.set(a.getX(), a.getY(), a.getZ(), 0.0);
814 // Cosine and sine of half angle
815 double cosHalf = Math.sqrt((1 + d)/2);
816 double sinHalf = Math.sqrt((1 - d)/2);
817 Vector3d axis = new Vector3d();
818 axis.cross(original, rotated);
820 result.set(sinHalf * axis.getX(), sinHalf * axis.getY(), sinHalf * axis.getZ(), cosHalf);
825 public static Vector3d getMinAxis(Vector3d original) {
826 double absX = Math.abs(original.getX());
827 double absY = Math.abs(original.getY());
828 double absZ = Math.abs(original.getZ());
830 if (absX <= absY && absX <= absZ)
832 else if (absY <= absZ)
838 public static boolean createRotation(Vector3d original, Vector3d rotated, Vector3d axis, AxisAngle4d result) {
840 if (rotated.lengthSquared() > 0.01)
844 if (original.lengthSquared() > 0.01)
845 original.normalize();
848 if (axis.lengthSquared() > 0.01)
852 double d = original.dot(rotated);
854 // original and rotated are parallel, pointing at the same direction
859 } else if (d < -0.9999) {
860 // original and rotated are parallel, pointing at the opposite direction
861 result.angle = Math.PI;
866 // Project vectors to Axis plane
867 Vector3d p1 = projectToPlane(original, axis);
868 Vector3d p2 = projectToPlane(rotated, axis);
869 // Create vectors where z-axis is plane normal
870 Quat4d q = getQuat(createRotation(axis, Z_AXIS));
871 Vector3d t1 = new Vector3d();
872 Vector3d t2 = new Vector3d();
875 // Calculate angles on z-axis plane.
876 double a1 = Math.atan2(t1.y, t1.x);
877 double a2 = Math.atan2(t2.y, t2.x);
878 result.set(axis,a2-a1);
884 public static void getQuat(Vector3d axis, double angle, Quat4d q)
887 // Quat = cos(theta/2) + sin(theta/2)(roation_axis)
889 amag = Math.sqrt( axis.x*axis.x + axis.y*axis.y + axis.z*axis.z);
897 double a2 = angle*0.5;
900 q.x = axis.x*amag*mag;
901 q.y = axis.y*amag*mag;
902 q.z = axis.z*amag*mag;
908 * Linear interpolation of quaternions. Result IS set to q1.
913 public static void lip(Quat4d q1, Quat4d q2, double alpha) {
914 double s1 = 1.0 - alpha;
921 public static double dot(Quat4d q1, Quat4d q2) {
922 return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
925 public static void mad(Tuple3d q1, Tuple3d q2, double s2) {
931 public static void mad(Quat4d q1, Quat4d q2, double s2) {
941 * Sets results to q1. Modifies q2.
947 public static void sip(Quat4d q1, Quat4d q2, double alpha) {
948 double cosom = dot(q1,q2);
954 if (cosom > 0.9999) {
961 double theta_0 = Math.acos(cosom);
962 double theta = theta_0 * alpha;
963 Quat4d t = new Quat4d(q1);
967 t.scale(Math.sin(theta));
968 q1.scale(Math.cos(theta));
973 public static void rotate(double angle, Tuple2d v1, Tuple2d v2) {
974 // TODO : verify implementation
975 double sin = Math.sin(angle);
979 } else if (sin == -1.0) {
983 double cos = Math.cos(angle);
987 } else if (cos != 1.0) {
988 v2.x= v1.x * cos + v1.y * -sin;
989 v2.y= v1.x* sin + v1.y *cos;
994 public static Tuple3d getPosRot(double m3x2[]) {
995 Vector3d t = new Vector3d();
1000 Vector2d v2 = new Vector2d(1,0);
1001 Vector2d v = new Vector2d();
1002 // use rotation of (1,0) to calculate the rotation component
1005 double a1 = v2.angle(v);
1009 t.z = Math.PI*2.0 - a1;
1014 public static Matrix4d glFrustum(double l, double r, double b, double t, double n, double f) {
1015 Matrix4d mat = new Matrix4d();
1016 mat.m00 = 2.0 * n / (r - l);
1017 mat.m11 = 2.0 * n / (t - b);
1018 mat.m02 = (r+l) / (r-l);
1019 mat.m12 = (t+b) / (t-b);
1020 mat.m22 = -(f+n) / (f-n);
1021 mat.m23 = -(2.0 *f * n) / (f-n);
1026 public static Matrix4d glOrtho(double l, double r, double b, double t, double n, double f) {
1027 Matrix4d mat = new Matrix4d();
1028 mat.m00 = 2.0 / (r - l);
1029 mat.m11 = 2.0 / (t - b);
1030 mat.m22 = -2.0 / (f-n);
1032 mat.m03 = -(r+l)/(r-l);
1033 mat.m13 = -(t+b)/(t-b);
1034 mat.m23 = -(f+n)/(f-n);
1039 * Round the number to a given number of decimals, if the rounded result contains at least three
1040 * zero trailing decimal places within the rounded result.
1042 public static double round(double value, int nsig) {
1043 if (Math.abs(value) < NEAR_ZERO)
1046 int decimals = (int) Math.round(Math.log10(value));
1047 int shift = nsig - decimals;
1048 double multiplier = Math.pow(10.0, shift);
1049 long roundedValue = Math.round(value * multiplier);
1050 if (roundedValue % 1000 == 0) {
1051 // Rounding results in at least three zeroes at the end
1052 return roundedValue / multiplier;
1054 // Number was not close to a shorter decimal representation, return it as is