1 /*******************************************************************************
\r
2 * Copyright (c) 2012, 2013 Association for Decentralized Information Management in
\r
4 * All rights reserved. This program and the accompanying materials
\r
5 * are made available under the terms of the Eclipse Public License v1.0
\r
6 * which accompanies this distribution, and is available at
\r
7 * http://www.eclipse.org/legal/epl-v10.html
\r
10 * VTT Technical Research Centre of Finland - initial API and implementation
\r
11 *******************************************************************************/
\r
12 package org.simantics.g3d.math;
\r
14 import javax.vecmath.AxisAngle4d;
\r
15 import javax.vecmath.Matrix3d;
\r
16 import javax.vecmath.Matrix4d;
\r
17 import javax.vecmath.Quat4d;
\r
18 import javax.vecmath.Tuple3d;
\r
19 import javax.vecmath.Tuple4d;
\r
20 import javax.vecmath.Vector2f;
\r
21 import javax.vecmath.Vector3d;
\r
23 import org.simantics.g3d.math.EulerTools.Order;
\r
27 * Some useful geometry related math functions. Beware, methods may modify their input parameters!
\r
29 * @author Marko Luukkainen
\r
32 public class MathTools {
\r
34 public static double NEAR_ZERO = 0.0000001;
\r
35 public static double NEAR_HALF = 0.4999999;
\r
37 public static final Vector3d Z_AXIS = new Vector3d(0.0,0.0,1.0);
\r
38 public static final Vector3d Y_AXIS = new Vector3d(0.0,1.0,0.0);
\r
39 public static final Vector3d X_AXIS = new Vector3d(1.0,0.0,0.0);
\r
40 public static final Vector3d ORIGIN = new Vector3d(0.0,0.0,0.0);
\r
42 final static double EPS = 1.0e-12;
\r
45 public static boolean equals(double d1, double d2) {
\r
46 return Math.abs(d1-d2) < EPS;
\r
49 public static boolean equals(Tuple3d p1, Tuple3d p2) {
\r
50 return distanceSquared(p1, p2) < NEAR_ZERO;
\r
53 public static boolean equals(Tuple4d p1, Tuple4d p2) {
\r
54 return distanceSquared(p1, p2) < NEAR_ZERO;
\r
57 public static double distance(Tuple3d p1, Tuple3d p2) {
\r
63 return Math.sqrt(dx*dx+dy*dy+dz*dz);
\r
66 public static double distance(Tuple4d p1, Tuple4d p2) {
\r
67 double dx, dy, dz, dw;
\r
73 return Math.sqrt(dx*dx+dy*dy+dz*dz+dw*dw);
\r
76 public static double distanceSquared(Tuple3d p1, Tuple3d p2) {
\r
82 return dx*dx+dy*dy+dz*dz;
\r
85 public static double distanceSquared(Tuple4d p1, Tuple4d p2) {
\r
86 double dx, dy, dz, dw;
\r
92 return dx*dx+dy*dy+dz*dz+dw*dw;
\r
95 public static boolean isValid(Tuple3d t) {
\r
96 return !(Double.isInfinite(t.x) || Double.isNaN(t.x) ||
\r
97 Double.isInfinite(t.y) || Double.isNaN(t.y) ||
\r
98 Double.isInfinite(t.z) || Double.isNaN(t.z));
\r
101 public static Vector3d closestPointOnEdge(Vector3d point, Vector3d edgePoint1, Vector3d edgePoint2) {
\r
102 point.sub(edgePoint1);
\r
103 Vector3d v = new Vector3d(edgePoint2);
\r
105 double t = v.dot(point);
\r
106 t /= v.lengthSquared();
\r
116 public static Vector3d closestPointOnStraight(Tuple3d point, Tuple3d straightPoint, Vector3d straightDir) {
\r
117 Vector3d v = new Vector3d(point);
\r
118 v.sub(straightPoint);
\r
119 double t = straightDir.dot(v);
\r
120 t /= straightDir.lengthSquared();
\r
121 v.set(straightDir);
\r
123 v.add(straightPoint);
\r
127 public static Vector3d closestPointOnStraight(Tuple3d point, Tuple3d straightPoint, Vector3d straightDir, double u[]) {
\r
128 Vector3d v = new Vector3d(point);
\r
129 v.sub(straightPoint);
\r
130 u[0] = straightDir.dot(v);
\r
131 u[0] /= straightDir.lengthSquared();
\r
132 v.set(straightDir);
\r
134 v.add(straightPoint);
\r
138 public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, Tuple3d planePoint) {
\r
139 point.sub(planePoint);
\r
141 return planeNormal.dot(point);
\r
144 public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, float d) {
\r
145 return (planeNormal.dot(point) + d);
\r
148 public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Tuple3d intersectPoint) {
\r
149 intersectPoint.set(planePoint);
\r
150 intersectPoint.sub(linePoint);
\r
151 double u = planeNormal.dot(new Vector3d(intersectPoint));
\r
152 double v = planeNormal.dot(lineDir);
\r
153 if (Math.abs(v) < NEAR_ZERO)
\r
156 intersectPoint.set(lineDir);
\r
157 intersectPoint.scale(u);
\r
158 intersectPoint.add(linePoint);
\r
162 public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Vector3d intersectPoint, double[] u) {
\r
163 intersectPoint.set(planePoint);
\r
164 intersectPoint.sub(linePoint);
\r
165 u[0] = planeNormal.dot(intersectPoint);
\r
166 double v = planeNormal.dot(lineDir);
\r
167 if (Math.abs(v) < NEAR_ZERO)
\r
170 intersectPoint.set(lineDir);
\r
171 intersectPoint.scale(u[0]);
\r
172 intersectPoint.add(linePoint);
\r
176 public static boolean intersectLineLine(Tuple3d l1_start,Tuple3d l1_end,Tuple3d l2_start,Tuple3d l2_end,Tuple3d l1_pos, Tuple3d l2_pos) {
\r
177 Vector3d p13 = new Vector3d();
\r
178 Vector3d p43 = new Vector3d();
\r
179 Vector3d p21 = new Vector3d();
\r
180 double d1343,d4321,d1321,d4343,d2121;
\r
181 double numer,denom;
\r
182 p13.sub(l1_start, l2_start);
\r
183 p43.sub(l2_end,l2_start);
\r
184 if (Math.abs(p43.x) < NEAR_ZERO && Math.abs(p43.y) < NEAR_ZERO && Math.abs(p43.z) < NEAR_ZERO)
\r
186 p21.sub(l1_end,l1_start);
\r
187 if (Math.abs(p21.x) < NEAR_ZERO && Math.abs(p21.y) < NEAR_ZERO && Math.abs(p21.z) < NEAR_ZERO)
\r
190 d1343 = p13.dot(p43);
\r
191 d4321 = p43.dot(p21);
\r
192 d1321 = p13.dot(p21);
\r
193 d4343 = p43.lengthSquared();
\r
194 d2121 = p21.lengthSquared();
\r
196 denom = d2121 * d4343 - d4321 * d4321;
\r
197 if (Math.abs(denom) < NEAR_ZERO)
\r
199 numer = d1343 * d4321 - d1321 * d4343;
\r
201 double mua = numer / denom;
\r
202 double mub = (d1343 + d4321 * mua) / d4343;
\r
204 l1_pos.x = l1_start.x + mua * p21.x;
\r
205 l1_pos.y = l1_start.y + mua * p21.y;
\r
206 l1_pos.z = l1_start.z + mua * p21.z;
\r
207 l2_pos.x = l2_start.x + mub * p43.x;
\r
208 l2_pos.y = l2_start.y + mub * p43.y;
\r
209 l2_pos.z = l2_start.z + mub * p43.z;
\r
214 public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb) {
\r
215 Vector3d p13 = new Vector3d();
\r
217 double d1343,d4321,d1321,d4343,d2121;
\r
218 double numer,denom;
\r
221 if (Math.abs(p43.x) < NEAR_ZERO && Math.abs(p43.y) < NEAR_ZERO && Math.abs(p43.z) < NEAR_ZERO)
\r
223 if (Math.abs(p21.x) < NEAR_ZERO && Math.abs(p21.y) < NEAR_ZERO && Math.abs(p21.z) < NEAR_ZERO)
\r
226 d1343 = p13.dot(p43);
\r
227 d4321 = p43.dot(p21);
\r
228 d1321 = p13.dot(p21);
\r
229 d4343 = p43.lengthSquared();
\r
230 d2121 = p21.lengthSquared();
\r
232 denom = d2121 * d4343 - d4321 * d4321;
\r
233 if (Math.abs(denom) < NEAR_ZERO)
\r
235 numer = d1343 * d4321 - d1321 * d4343;
\r
237 double mua = numer / denom;
\r
238 double mub = (d1343 + d4321 * mua) / d4343;
\r
240 pa.x = p1.x + mua * p21.x;
\r
241 pa.y = p1.y + mua * p21.y;
\r
242 pa.z = p1.z + mua * p21.z;
\r
243 pb.x = p3.x + mub * p43.x;
\r
244 pb.y = p3.y + mub * p43.y;
\r
245 pb.z = p3.z + mub * p43.z;
\r
251 * Calculate the line segment PaPb that is the shortest route between
\r
252 * two lines P1P2 and P3P4. Calculate also the values of mua and mub where
\r
253 * Pa = P1 + mua (P2 - P1)
\r
254 * Pb = P3 + mub (P4 - P3)
\r
264 public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb, double mu[]) {
\r
265 Vector3d p13 = new Vector3d();
\r
267 double d1343,d4321,d1321,d4343,d2121;
\r
268 double numer,denom;
\r
269 double EPS = 0.001;
\r
271 if (Math.abs(p43.x) < EPS && Math.abs(p43.y) < EPS && Math.abs(p43.z) < EPS)
\r
273 if (Math.abs(p21.x) < EPS && Math.abs(p21.y) < EPS && Math.abs(p21.z) < EPS)
\r
276 d1343 = p13.dot(p43);
\r
277 d4321 = p43.dot(p21);
\r
278 d1321 = p13.dot(p21);
\r
279 d4343 = p43.lengthSquared();
\r
280 d2121 = p21.lengthSquared();
\r
282 denom = d2121 * d4343 - d4321 * d4321;
\r
283 if (Math.abs(denom) < EPS)
\r
285 numer = d1343 * d4321 - d1321 * d4343;
\r
287 mu[0] = numer / denom;
\r
288 mu[1] = (d1343 + d4321 * mu[0]) / d4343;
\r
290 pa.x = p1.x + mu[0] * p21.x;
\r
291 pa.y = p1.y + mu[0] * p21.y;
\r
292 pa.z = p1.z + mu[0] * p21.z;
\r
293 pb.x = p3.x + mu[1] * p43.x;
\r
294 pb.y = p3.y + mu[1] * p43.y;
\r
295 pb.z = p3.z + mu[1] * p43.z;
\r
302 public static void rotate(Quat4d q, Tuple3d in, Tuple3d out) {
\r
304 double tw = - q.x*in.x - q.y*in.y - q.z*in.z;
\r
305 double tx = q.w*in.x + q.y*in.z - q.z*in.y;
\r
306 double ty = q.w*in.y - q.x*in.z + q.z*in.x;
\r
307 double tz = q.w*in.z + q.x*in.y - q.y*in.x ;
\r
309 //temp * q' -> x = -x, y = -y z = -z
\r
310 //out.w = tw*q.w + tx*q.x + ty*q.y + tz*q.z;
\r
311 out.x = -tw*q.x + tx*q.w - ty*q.z + tz*q.y;
\r
312 out.y = -tw*q.y + tx*q.z + ty*q.w - tz*q.x;
\r
313 out.z = -tw*q.z - tx*q.y + ty*q.x + tz*q.w;
\r
316 public static void getMatrix(Quat4d quat, Matrix3d m) {
\r
317 m.m00 = 1.0f - 2.0 * (quat.y * quat.y + quat.z * quat.z);
\r
318 m.m01 = 2.0 * (quat.x * quat.y + quat.w * quat.z);
\r
319 m.m02 = 2.0 * (quat.x * quat.z - quat.w * quat.y);
\r
320 m.m10 = 2.0 * (quat.x * quat.y - quat.w * quat.z);
\r
321 m.m11 = 1.0 - 2.0f * (quat.x * quat.x + quat.z * quat.z);
\r
322 m.m12 = 2.0 * (quat.y * quat.z + quat.w * quat.x);
\r
323 m.m20 = 2.0 * (quat.x * quat.z + quat.w * quat.y);
\r
324 m.m21 = 2.0 * (quat.y * quat.z - quat.w * quat.x);
\r
325 m.m22 = 1.0 - 2.0f * (quat.x * quat.x + quat.y * quat.y);
\r
330 private static double q[] = new double[3];
\r
331 private static int nxt[] = { 1, 2, 0 };
\r
333 * Converts Matrix to Quaternion
\r
335 * Note: non-thread safe.
\r
340 public static void getQuat(Matrix3d mat, Quat4d quat) {
\r
341 double tr = mat.m00 + mat.m11 + mat.m22;
\r
343 double s = Math.sqrt(tr + 1.0);
\r
346 quat.x = (mat.m21 - mat.m12) * s;
\r
347 quat.y = (mat.m02 - mat.m20) * s;
\r
348 quat.z = (mat.m10 - mat.m01) * s;
\r
351 if (mat.m11 > mat.m00)
\r
353 if (mat.m22 > mat.getElement(i, i))
\r
360 double s = Math.sqrt((mat.getElement(i, i) - (mat.getElement(j, j) + mat.getElement(k, k))) + 1.0);
\r
364 if (Math.abs(s) > 0.001)
\r
367 quat.w = (mat.getElement(k, j) - mat.getElement(j, k)) * s;
\r
368 q[j] = (mat.getElement(j, i) + mat.getElement(i, j)) * s;
\r
369 q[k] = (mat.getElement(k, i) + mat.getElement(i, k)) * s;
\r
377 public static Quat4d getQuat(Matrix3d mat) {
\r
378 Quat4d q = new Quat4d();
\r
383 public static AxisAngle4d getFromPseudoEuler(Vector3d euler) {
\r
384 AxisAngle4d aa = new AxisAngle4d();
\r
385 aa.angle = euler.length();
\r
386 Vector3d normal = new Vector3d(euler);
\r
387 if (aa.angle > NEAR_ZERO) {
\r
388 normal.normalize();
\r
401 public static Vector3d getPseudoEuler(AxisAngle4d aa) {
\r
402 Vector3d euler = new Vector3d(aa.x,aa.y,aa.z);
\r
403 euler.scale(aa.angle);
\r
408 public static void getQuat(Vector3d euler, Quat4d quat) {
\r
409 Quat4d q = EulerTools.getQuatFromEuler(Order.YXZ, euler.y,euler.x,euler.z);
\r
411 // http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Conversion_formulae_between_formalisms
\r
412 // Using the x-convention, the 3-1-3 Euler angles phi, theta and psi (around the Z, X and again the Z-axis)
\r
413 // quat.x = -Math.cos((euler.x - euler.z)*0.5)*Math.sin(euler.y*0.5);
\r
414 // quat.y = -Math.sin((euler.x - euler.z)*0.5)*Math.sin(euler.y*0.5);
\r
415 // quat.z = -Math.sin((euler.x + euler.z)*0.5)*Math.cos(euler.y*0.5);
\r
416 // quat.w = Math.sin((euler.x + euler.z)*0.5)*Math.cos(euler.y*0.5);
\r
418 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
\r
420 // double c1 = Math.cos(euler.y*0.5);
\r
421 // double s1 = Math.sin(euler.y*0.5);
\r
422 // double c2 = Math.cos(euler.z*0.5);
\r
423 // double s2 = Math.sin(euler.z*0.5);
\r
424 // double c3 = Math.cos(euler.x*0.5);
\r
425 // double s3 = Math.sin(euler.x*0.5);
\r
426 // double c1c2 = c1*c2;
\r
427 // double s1s2 = s1*s2;
\r
428 // quat.w =c1c2*c3 - s1s2*s3;
\r
429 // quat.x =c1c2*s3 + s1s2*c3;
\r
430 // quat.y =s1*c2*c3 + c1*s2*s3;
\r
431 // quat.z =c1*s2*c3 - s1*c2*s3;
\r
433 // Quat4d q2 = EulerTools.getQuatFromEuler(Order.YZX, euler.y,euler.z,euler.x);
\r
434 // System.out.println("Q " + quat + " Q2 " + q2);
\r
435 // double c1 = Math.cos(euler.y);
\r
436 // double s1 = Math.sin(euler.y);
\r
437 // double c2 = Math.cos(euler.z);
\r
438 // double s2 = Math.sin(euler.z);
\r
439 // double c3 = Math.cos(euler.x);
\r
440 // double s3 = Math.sin(euler.x);
\r
441 // quat.w = Math.sqrt(1.0 + c1 * c2 + c1*c3 - s1 * s2 * s3 + c2*c3) / 2.0;
\r
442 // double w4 = (4.0 * quat.w);
\r
443 // quat.x = (c2 * s3 + c1 * s3 + s1 * s2 * c3) / w4 ;
\r
444 // quat.y = (s1 * c2 + s1 * c3 + c1 * s2 * s3) / w4 ;
\r
445 // quat.z = (-s1 * s3 + c1 * s2 * c3 +s2) / w4 ;
\r
451 public static void getEuler(Quat4d quat,Vector3d euler) {
\r
452 Vector3d e = EulerTools.getEulerFromQuat(Order.YXZ, quat);
\r
457 // http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Conversion_formulae_between_formalisms
\r
458 // euler.x = Math.atan2(quat.x * quat.z + quat.y* quat.w, quat.y*quat.z - quat.x * quat.w);
\r
459 // euler.y = Math.acos(-square(quat.x) - square(quat.y) + square(quat.z) + square(quat.w));
\r
460 // euler.z = -Math.atan2(quat.x * quat.z - quat.y* quat.w, quat.y*quat.z + quat.x * quat.w);
\r
462 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
\r
464 // double test = quat.x * quat.y + quat.z * quat.w;
\r
465 // if (test > NEAR_HALF) {
\r
466 // euler.y = 2.0 * Math.atan2(quat.x,quat.w);
\r
467 // euler.z = Math.PI * 0.5;
\r
469 // } else if (test < -NEAR_HALF) {
\r
470 // euler.y = -2.0 * Math.atan2(quat.x,quat.w);
\r
471 // euler.z = -Math.PI * 0.5;
\r
474 // double sqx = square(quat.x);
\r
475 // double sqy = square(quat.y);
\r
476 // double sqz = square(quat.z);
\r
477 // euler.y = Math.atan2(2.0*(quat.y*quat.w-quat.x*quat.z), 1.0 - 2.0*(sqy-sqz));
\r
478 // euler.z = Math.asin(2.0*test);
\r
479 // euler.x = Math.atan2(2.0*(quat.x*quat.w-quat.y*quat.z), 1.0 - 2.0*(sqx-sqz));
\r
480 // System.out.println(euler + " " + EulerTools.getEulerFromQuat(Order.YXZ, quat) + " " + quat);
\r
482 // double sqw = quat.w*quat.w;
\r
483 // double sqx = quat.x*quat.x;
\r
484 // double sqy = quat.y*quat.y;
\r
485 // double sqz = quat.z*quat.z;
\r
486 // double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
\r
487 // double test = quat.x*quat.y + quat.z*quat.w;
\r
488 // if (test > 0.499*unit) { // singularity at north pole
\r
489 // euler.y = 2 * Math.atan2(quat.x,quat.w);
\r
490 // euler.z = Math.PI/2;
\r
494 // if (test < -0.499*unit) { // singularity at south pole
\r
495 // euler.y = -2 * Math.atan2(quat.x,quat.w);
\r
496 // euler.z = -Math.PI/2;
\r
500 // euler.y = Math.atan2(2*quat.y*quat.w-2*quat.x*quat.z , sqx - sqy - sqz + sqw);
\r
501 // euler.z = Math.asin(2*test/unit);
\r
502 // euler.x = Math.atan2(2*quat.x*quat.w-2*quat.y*quat.z , -sqx + sqy - sqz + sqw);
\r
505 public static Quat4d getQuat(Vector3d euler) {
\r
506 Quat4d q = new Quat4d();
\r
512 public static Vector3d getEuler(Quat4d quat) {
\r
513 Vector3d v = new Vector3d();
\r
518 public static Quat4d getQuat(AxisAngle4d aa) {
\r
519 Quat4d q = new Quat4d();
\r
524 public static AxisAngle4d getAxisAngle(Quat4d q) {
\r
525 AxisAngle4d aa = new AxisAngle4d();
\r
530 public static Quat4d getIdentityQuat() {
\r
531 return new Quat4d(0, 0, 0, 1);
\r
534 public static void getQuat(AxisAngle4d aa, Quat4d q) {
\r
536 // Quat = cos(theta/2) + sin(theta/2)(roation_axis)
\r
538 amag = Math.sqrt( aa.x*aa.x + aa.y*aa.y + aa.z*aa.z);
\r
539 if( amag < NEAR_ZERO ) {
\r
546 double a2 = aa.angle * 0.5;
\r
547 mag = Math.sin(a2);
\r
548 q.w = Math.cos(a2);
\r
549 q.x = aa.x*amag*mag;
\r
550 q.y = aa.y*amag*mag;
\r
551 q.z = aa.z*amag*mag;
\r
560 private static final int IN = 0;
\r
561 private static final int LEFT = 1;
\r
562 private static final int RIGHT = 2;
\r
563 private static final int BOTTOM = 4;
\r
564 private static final int TOP = 8;
\r
567 private static int bitcode(Vector2f p1, Vector2f min, Vector2f max) {
\r
571 else if (p1.x > max.x)
\r
575 else if (p1.y > max.y)
\r
580 public static boolean clipLineRectangle(Vector2f p1,Vector2f p2, Vector2f min, Vector2f max, Vector2f r1, Vector2f r2) {
\r
582 int o1 = bitcode(p1, min, max);
\r
583 int o2 = bitcode(p2, min, max);
\r
602 if ((o1 & TOP) != IN) {
\r
603 float t = (max.y - p1.y) / (p2.y - p1.y);
\r
604 p1.x += t * (p2.x - p1.x);
\r
606 } else if ((o1 & BOTTOM) != IN) {
\r
607 float t = (min.y - p1.y) / (p2.y - p1.y);
\r
608 p1.x += t * (p2.x - p1.x);
\r
610 } else if ((o1 & LEFT) != IN) {
\r
611 float t = (min.x - p1.x) / (p2.x - p1.x);
\r
612 p1.y += t * (p2.y - p1.y);
\r
614 } else if ((o1 & RIGHT) != IN) {
\r
615 float t = (max.x - p1.x) / (p2.x - p1.x);
\r
616 p1.y += t * (p2.y - p1.y);
\r
619 throw new RuntimeException("Error in clipping code");
\r
625 public static double square(double d) {
\r
630 public static void multiplyOrientation(AxisAngle4d aa, AxisAngle4d rot) {
\r
631 Quat4d q1 = new Quat4d();
\r
633 Quat4d q2 = new Quat4d();
\r
639 public static double radToDeg(double rad) {
\r
640 return (rad / Math.PI) * 180.0;
\r
643 public static double degToRad(double deg) {
\r
644 return (deg / 180.0) * Math.PI;
\r
647 public static double clamp(double min, double max,double v) {
\r
655 public static AxisAngle4d createRotation(Vector3d original, Vector3d rotated) {
\r
656 AxisAngle4d result = new AxisAngle4d();
\r
657 if (createRotation(original, rotated, result))
\r
663 public static void setIdentity(Quat4d q) {
\r
670 public static void setIdentity(AxisAngle4d aa) {
\r
677 public static void set(Matrix3d mat, double m00, double m01, double m02,
\r
678 double m10, double m11, double m12, double m20, double m21,
\r
693 public static void set(Matrix4d mat, double[] v) {
\r
716 public static boolean createRotation(Vector3d original, Vector3d rotated, AxisAngle4d result) {
\r
718 if (rotated.lengthSquared() > 0.01)
\r
719 rotated.normalize();
\r
722 double d = original.dot(rotated);
\r
724 // original and rotated are parallel, pointing at the same direction
\r
725 result.angle = 0.0;
\r
729 } else if (d < -0.9999) {
\r
730 // original and rotated are parallel, pointing at the opposite direction
\r
731 Vector3d a = Z_AXIS;
\r
732 if (Math.abs(a.dot(original)) > 0.8 )
\r
734 result.set(a, Math.PI);
\r
736 double angle = original.angle(rotated);
\r
737 Vector3d axis = new Vector3d();
\r
738 axis.cross(original, rotated);
\r
739 result.set(axis,angle);
\r
744 public static boolean createRotation(Vector3d original, Vector3d rotated, Quat4d result) {
\r
746 if (rotated.lengthSquared() > 0.01)
\r
747 rotated.normalize();
\r
750 double d = original.dot(rotated);
\r
752 // original and rotated are parallel, pointing at the same direction
\r
757 } else if (d < -0.9999) {
\r
758 // original and rotated are parallel, pointing at the opposite direction
\r
759 Vector3d a = Z_AXIS;
\r
760 if (Math.abs(a.dot(original)) > 0.8 )
\r
762 getQuat(a, Math.PI, result);
\r
765 double angle = original.angle(rotated);
\r
766 Vector3d axis = new Vector3d();
\r
767 axis.cross(original, rotated);
\r
768 getQuat(axis, angle, result);
\r
773 public static void getQuat(Vector3d axis, double angle, Quat4d q)
\r
776 // Quat = cos(theta/2) + sin(theta/2)(roation_axis)
\r
778 amag = Math.sqrt( axis.x*axis.x + axis.y*axis.y + axis.z*axis.z);
\r
786 double a2 = angle*0.5;
\r
787 mag = Math.sin(a2);
\r
788 q.w = Math.cos(a2);
\r
789 q.x = axis.x*amag*mag;
\r
790 q.y = axis.y*amag*mag;
\r
791 q.z = axis.z*amag*mag;
\r
797 * Linear interpolation of quaternions. Result IS set to q1.
\r
802 public static void lip(Quat4d q1, Quat4d q2, double alpha) {
\r
803 double s1 = 1.0 - alpha;
\r
810 public static double dot(Quat4d q1, Quat4d q2) {
\r
811 return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
\r
814 public static void mad(Tuple3d q1, Tuple3d q2, double s2) {
\r
820 public static void mad(Quat4d q1, Quat4d q2, double s2) {
\r
830 * Sets results to q1. Modifies q2.
\r
836 public static void sip(Quat4d q1, Quat4d q2, double alpha) {
\r
837 double cosom = dot(q1,q2);
\r
843 if (cosom > 0.9999) {
\r
850 double theta_0 = Math.acos(cosom);
\r
851 double theta = theta_0 * alpha;
\r
852 Quat4d t = new Quat4d(q1);
\r
856 t.scale(Math.sin(theta));
\r
857 q1.scale(Math.cos(theta));
\r