org.simantics.g3d/src/org/simantics/g3d/math/MathTools.java

   1 /*******************************************************************************
   2  * Copyright (c) 2012, 2013 Association for Decentralized Information Management in
   3  * Industry THTH ry.
   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
   8  *
   9  * Contributors:
  10  *     VTT Technical Research Centre of Finland - initial API and implementation
  11  *******************************************************************************/
  12 package org.simantics.g3d.math;
  13
  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;
  24
  25 import org.simantics.g3d.math.EulerTools.Order;
  26
  27
  28 /**
  29  * Some useful geometry related math functions. Beware, methods may modify their input parameters!
  30  *
  31  * @author Marko Luukkainen
  32  *
  33  */
  34 public class MathTools {
  35
  36         public static double NEAR_ZERO = 0.000000001;
  37         public static double NEAR_HALF = 0.499999999;
  38
  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);
  43
  44         final static double EPS = 1.0e-12;
  45
  46
  47         public static boolean equals(double d1, double d2) {
  48                 return Math.abs(d1-d2) < EPS;
  49         }
  50
  51         public static boolean equals(Tuple3d p1, Tuple3d p2) {
  52                 return distanceSquared(p1, p2) < NEAR_ZERO;
  53         }
  54
  55         public static boolean equals(Tuple4d p1, Tuple4d p2) {
  56                 return distanceSquared(p1, p2) < NEAR_ZERO;
  57         }
  58
  59         public static double distance(Tuple3d p1, Tuple3d p2) {
  60                 double dx, dy, dz;
  61
  62                 dx = p2.x - p1.x;
  63                 dy = p2.y - p1.y;
  64                 dz = p2.z - p1.z;
  65                 return Math.sqrt(dx*dx+dy*dy+dz*dz);
  66         }
  67
  68         public static double distance(Tuple4d p1, Tuple4d p2) {
  69                 double dx, dy, dz, dw;
  70
  71                 dx = p2.x - p1.x;
  72                 dy = p2.y - p1.y;
  73                 dz = p2.z - p1.z;
  74                 dw = p2.w - p1.w;
  75                 return Math.sqrt(dx*dx+dy*dy+dz*dz+dw*dw);
  76         }
  77
  78         public static double distanceSquared(Tuple3d p1, Tuple3d p2) {
  79                 double dx, dy, dz;
  80
  81                 dx = p2.x - p1.x;
  82                 dy = p2.y - p1.y;
  83                 dz = p2.z - p1.z;
  84                 return dx*dx+dy*dy+dz*dz;
  85         }
  86
  87         public static double distanceSquared(Tuple4d p1, Tuple4d p2) {
  88                 double dx, dy, dz, dw;
  89
  90                 dx = p2.x - p1.x;
  91                 dy = p2.y - p1.y;
  92                 dz = p2.z - p1.z;
  93                 dw = p2.w - p1.w;
  94                 return dx*dx+dy*dy+dz*dz+dw*dw;
  95         }
  96
  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));
 101         }
 102
 103         public static Vector3d closestPointOnEdge(Vector3d point, Vector3d edgePoint1, Vector3d edgePoint2) {
 104                 point.sub(edgePoint1);
 105                 Vector3d v = new Vector3d(edgePoint2);
 106                 v.sub(edgePoint1);
 107                 double t = v.dot(point);
 108                 t /= v.lengthSquared();
 109                 if (t <= 0.0f)
 110                         return edgePoint1;
 111                 if (t >= 1.0f)
 112                         return edgePoint2;
 113                 v.scale(t);
 114                 v.add(edgePoint1);
 115                 return v;
 116         }
 117
 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();
 123                 v.set(straightDir);
 124                 v.scale(t);
 125                 v.add(straightPoint);
 126                 return v;
 127         }
 128
 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();
 134                 v.set(straightDir);
 135                 v.scale(u[0]);
 136                 v.add(straightPoint);
 137                 return v;
 138         }
 139
 140         public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, Tuple3d planePoint) {
 141                 point.sub(planePoint);
 142
 143                 return planeNormal.dot(point);
 144         }
 145
 146         public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, float d) {
 147                 return (planeNormal.dot(point) + d);
 148         }
 149
 150         public static Vector3d projectToPlane(Vector3d v, Vector3d planeNormal) {
 151                 //v.normalize();
 152                 //planeNormal.normalize();
 153                 Vector3d t = new Vector3d();
 154                 if (planeNormal == X_AXIS) {
 155                         t.set(0, v.y, v.z);
 156                 }
 157                 else if (planeNormal == Y_AXIS) {
 158                         t.set(v.x, 0, v.z);
 159                 }
 160                 else if (planeNormal == Z_AXIS) {
 161                         t.set(v.x, v.y, 0);
 162                 }
 163                 else {
 164                         t.cross(v,planeNormal);
 165                         t.cross(planeNormal, t);
 166                 }
 167
 168                 return t;
 169         }
 170
 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)
 177                         return false;
 178                 u /= v;
 179                 intersectPoint.set(lineDir);
 180                 intersectPoint.scale(u);
 181                 intersectPoint.add(linePoint);
 182                 return true;
 183         }
 184
 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)
 191                         return false;
 192                 u[0] /= v;
 193                 intersectPoint.set(lineDir);
 194                 intersectPoint.scale(u[0]);
 195                 intersectPoint.add(linePoint);
 196                 return true;
 197         }
 198
 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;
 204                         double numer,denom;
 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)
 208                                 return false;
 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)
 211                                 return false;
 212
 213                         d1343 = p13.dot(p43);
 214                         d4321 = p43.dot(p21);
 215                         d1321 = p13.dot(p21);
 216                         d4343 = p43.lengthSquared();
 217                         d2121 = p21.lengthSquared();
 218
 219                         denom = d2121 * d4343 - d4321 * d4321;
 220                         if (Math.abs(denom) < NEAR_ZERO)
 221                                 return false;
 222                         numer = d1343 * d4321 - d1321 * d4343;
 223
 224                         double mua = numer / denom;
 225                         double mub = (d1343 + d4321 * mua) / d4343;
 226
 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;
 233
 234                         return true;
 235         }
 236
 237         public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb) {
 238                 Vector3d p13 = new Vector3d();
 239
 240                 double d1343,d4321,d1321,d4343,d2121;
 241                 double numer,denom;
 242
 243                 p13.sub(p1, p3);
 244                 if (Math.abs(p43.x)  < NEAR_ZERO && Math.abs(p43.y)  < NEAR_ZERO && Math.abs(p43.z)  < NEAR_ZERO)
 245                         return false;
 246                 if (Math.abs(p21.x)  < NEAR_ZERO && Math.abs(p21.y)  < NEAR_ZERO && Math.abs(p21.z)  < NEAR_ZERO)
 247                         return false;
 248
 249                 d1343 = p13.dot(p43);
 250                 d4321 = p43.dot(p21);
 251                 d1321 = p13.dot(p21);
 252                 d4343 = p43.lengthSquared();
 253                 d2121 = p21.lengthSquared();
 254
 255                 denom = d2121 * d4343 - d4321 * d4321;
 256                 if (Math.abs(denom) < NEAR_ZERO)
 257                         return false;
 258                 numer = d1343 * d4321 - d1321 * d4343;
 259
 260                 double mua = numer / denom;
 261                 double mub = (d1343 + d4321 * mua) / d4343;
 262
 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;
 269
 270                 return true;
 271         }
 272
 273         /**
 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)
 278          * @param p1
 279          * @param p21
 280          * @param p3
 281          * @param p43
 282          * @param pa
 283          * @param pb
 284          * @param mu
 285          * @return
 286          */
 287         public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb, double mu[]) {
 288                 Vector3d p13 = new Vector3d();
 289
 290                 double d1343,d4321,d1321,d4343,d2121;
 291                 double numer,denom;
 292                 double EPS = 0.001;
 293                 p13.sub(p1, p3);
 294                 if (Math.abs(p43.x)  < EPS && Math.abs(p43.y)  < EPS && Math.abs(p43.z)  < EPS)
 295                         return false;
 296                 if (Math.abs(p21.x)  < EPS && Math.abs(p21.y)  < EPS && Math.abs(p21.z)  < EPS)
 297                         return false;
 298
 299                 d1343 = p13.dot(p43);
 300                 d4321 = p43.dot(p21);
 301                 d1321 = p13.dot(p21);
 302                 d4343 = p43.lengthSquared();
 303                 d2121 = p21.lengthSquared();
 304
 305                 denom = d2121 * d4343 - d4321 * d4321;
 306                 if (Math.abs(denom) < EPS)
 307                         return false;
 308                 numer = d1343 * d4321 - d1321 * d4343;
 309
 310                 mu[0] = numer / denom;
 311                 mu[1] = (d1343 + d4321 * mu[0]) / d4343;
 312
 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;
 319
 320                 return true;
 321         }
 322
 323
 324
 325         public static void rotate(Quat4d q, Tuple3d in, Tuple3d out) {
 326                 // p' = q * p * q'
 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           ;
 331
 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;
 337         }
 338
 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);
 349
 350         }
 351
 352         public static void getMatrix(Quat4d quat, Matrix4d m) {
 353                 m.setZero();
 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);
 363                 m.m33 = 1.0;
 364         }
 365
 366         private static double q[] = new double[3];
 367         private static int nxt[] = { 1, 2, 0 };
 368         /**
 369          * Converts Matrix to Quaternion
 370          *
 371          * Note: non-thread safe.
 372          *
 373          * @param mat
 374          * @param quat
 375          */
 376         public static void getQuat(Matrix3d mat, Quat4d quat) {
 377                 double tr = mat.m00 + mat.m11 + mat.m22;
 378                 if (tr > 0.0) {
 379                         double s = Math.sqrt(tr + 1.0);
 380                         quat.w = 0.5 * s;
 381                         s = 0.5 / s;
 382                         quat.x = (mat.m21 - mat.m12) * s;
 383                         quat.y = (mat.m02 - mat.m20) * s;
 384                         quat.z = (mat.m10 - mat.m01) * s;
 385                 } else {
 386                         int i = 0, j, k;
 387                         if (mat.m11 > mat.m00)
 388                                 i = 1;
 389                         if (mat.m22 > mat.getElement(i, i))
 390                                 i = 2;
 391
 392
 393                         j = nxt[i];
 394                         k = nxt[j];
 395
 396                         double s = Math.sqrt((mat.getElement(i, i) - (mat.getElement(j, j) + mat.getElement(k, k))) + 1.0);
 397
 398                         q[i] = s * 0.5;
 399
 400                         if (Math.abs(s) > 0.001)
 401                                 s = 0.5 / s;
 402
 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;
 406
 407                         quat.x = q[0];
 408                         quat.y = q[1];
 409                         quat.z = q[2];
 410                 }
 411         }
 412
 413         public static Quat4d getQuat(Matrix3d mat) {
 414                 Quat4d q = new Quat4d();
 415                 getQuat(mat, q);
 416                 return q;
 417         }
 418
 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) {
 424                         normal.normalize();
 425                         aa.x = normal.x;
 426                         aa.y = normal.y;
 427                         aa.z = normal.z;
 428                 } else {
 429                         aa.x = 1.0;
 430                         aa.y = 0.0;
 431                         aa.z = 0.0;
 432                 }
 433
 434                 return aa;
 435         }
 436
 437         public static Vector3d getPseudoEuler(AxisAngle4d aa) {
 438                 Vector3d euler = new Vector3d(aa.x,aa.y,aa.z);
 439                 euler.scale(aa.angle);
 440                 return euler;
 441         }
 442
 443
 444         public static void getQuat(Vector3d euler, Quat4d quat)  {
 445                 Quat4d q = EulerTools.getQuatFromEuler(Order.YXZ, euler.y,euler.x,euler.z);
 446                 quat.set(q);
 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);
 453
 454                 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
 455                 // Y, Z, X order
 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;
 468
 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 ;
 482         }
 483
 484
 485
 486
 487         public static void getEuler(Quat4d quat,Vector3d euler)  {
 488                 Vector3d e = EulerTools.getEulerFromQuat(Order.YXZ, quat);
 489                 euler.x = e.y;
 490                 euler.y = e.x;
 491                 euler.z = e.z;
 492
 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);
 497
 498                 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
 499         // Y, Z, X order
 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;
 504 //                 euler.x = 0.0;
 505 //         } else if (test < -NEAR_HALF) {
 506 //                 euler.y = -2.0 * Math.atan2(quat.x,quat.w);
 507 //                 euler.z = -Math.PI * 0.5;
 508 //                 euler.x = 0.0;
 509 //         } else {
 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);
 517 //         }
 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;
 527 //                      euler.x = 0;
 528 //                      return;
 529 //              }
 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;
 533 //                      euler.x = 0;
 534 //                      return;
 535 //              }
 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);
 539         }
 540
 541         public static Quat4d getQuat(Vector3d euler) {
 542                 Quat4d q = new Quat4d();
 543                 getQuat(euler,q);
 544                 return q;
 545         }
 546
 547
 548         public static Vector3d getEuler(Quat4d quat) {
 549                 Vector3d v = new Vector3d();
 550                 getEuler(quat, v);
 551                 return v;
 552         }
 553
 554         public static Quat4d getQuat(AxisAngle4d aa) {
 555                 Quat4d q = new Quat4d();
 556                 getQuat(aa, q);
 557                 return q;
 558         }
 559
 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;
 563
 564                 if (mag > EPS) {
 565                         mag = Math.sqrt(mag);
 566                         aa.angle = 2.0 * Math.atan2(mag, q.w);
 567                         mag = 1.0 / mag;
 568                         aa.x = q.x * mag;
 569                         aa.y = q.y * mag;
 570                         aa.z = q.z * mag;
 571
 572                 } else {
 573                         aa.x = 0.0;
 574                         aa.y = 1.0;
 575                         aa.z = 0.0;
 576                         aa.angle = 0.0;
 577                 }
 578                 // aa.set(q);
 579                 return aa;
 580         }
 581
 582         public static Quat4d getIdentityQuat() {
 583                 return new Quat4d(0, 0, 0, 1);
 584         }
 585
 586         public static void getQuat(AxisAngle4d aa, Quat4d q) {
 587                 double mag,amag;
 588                 // Quat = cos(theta/2) + sin(theta/2)(roation_axis)
 589
 590                 amag = Math.sqrt( aa.x*aa.x + aa.y*aa.y + aa.z*aa.z);
 591                 if( amag < NEAR_ZERO ) {
 592                         q.w = 1.0;
 593                         q.x = 0.0;
 594                         q.y = 0.0;
 595                         q.z = 0.0;
 596                 } else {
 597                         amag = 1.0/amag;
 598                         double a2 = aa.angle * 0.5;
 599                         mag = Math.sin(a2);
 600                         q.w = Math.cos(a2);
 601                         q.x = aa.x*amag*mag;
 602                         q.y = aa.y*amag*mag;
 603                         q.z = aa.z*amag*mag;
 604                 }
 605         }
 606
 607
 608         /*
 609          * Cohen-Sutherland
 610          */
 611
 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;
 617
 618
 619         private static int bitcode(Vector2f p1, Vector2f min, Vector2f max) {
 620                 int code = IN;
 621                 if (p1.x < min.x)
 622                         code |= LEFT;
 623                 else if (p1.x > max.x)
 624                         code |= RIGHT;
 625                 if (p1.y < min.y)
 626                         code |= BOTTOM;
 627                 else if (p1.y > max.y)
 628                         code |= TOP;
 629                 return code;
 630         }
 631
 632         public static boolean clipLineRectangle(Vector2f p1,Vector2f p2, Vector2f min, Vector2f max, Vector2f r1, Vector2f r2) {
 633                 while (true) {
 634                         int o1 = bitcode(p1, min, max);
 635                         int o2 = bitcode(p2, min, max);
 636                         int and = o1 & o2;
 637                         int or = o1 | o2;
 638                         if (and != IN) {
 639                                 return false;
 640                         }
 641                         if (or == IN) {
 642                                 r1.set(p1);
 643                                 r2.set(p2);
 644                                 return true;
 645                         }
 646                         if (o1 == IN) {
 647                                 Vector2f t = p1;
 648                                 p1 = p2;
 649                                 p2 = t;
 650                                 int t2 = o1;
 651                                 o1 = o2;
 652                                 o2 = t2;
 653                         }
 654                         if ((o1 & TOP) != IN) {
 655                                 float t = (max.y - p1.y) / (p2.y - p1.y);
 656                                 p1.x += t * (p2.x - p1.x);
 657                                 p1.y = max.y;
 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);
 661                                 p1.y = min.y;
 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);
 665                                 p1.x = min.x;
 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);
 669                                 p1.x = max.x;
 670                         } else {
 671                                 throw new RuntimeException("Error in clipping code");
 672                         }
 673                 }
 674
 675         }
 676
 677         public static double square(double d) {
 678                 return d * d;
 679         }
 680
 681
 682         public static void multiplyOrientation(AxisAngle4d aa, AxisAngle4d rot)  {
 683                 Quat4d q1 = new Quat4d();
 684                 getQuat(aa, q1);
 685                 Quat4d q2 = new Quat4d();
 686                 getQuat(rot, q2);
 687                 q2.mul(q1);
 688                 rot.set(q2);
 689         }
 690
 691         public static double radToDeg(double rad) {
 692                 return (rad / Math.PI) * 180.0;
 693         }
 694
 695         public static double degToRad(double deg) {
 696                 return (deg / 180.0) * Math.PI;
 697         }
 698
 699         public static double clamp(double min, double max,double v) {
 700                 if (v < min)
 701                         return min;
 702                 if (v > max)
 703                         return max;
 704                 return v;
 705         }
 706
 707         public static AxisAngle4d createRotation(Vector3d original, Vector3d rotated) {
 708                 AxisAngle4d result = new AxisAngle4d();
 709                 if (createRotation(original, rotated, result))
 710                         return result;
 711                 return null;
 712         }
 713
 714
 715         public static void setIdentity(Quat4d q) {
 716                 q.w = 1.0;
 717                 q.x = 0.0;
 718                 q.y = 0.0;
 719                 q.z = 0.0;
 720         }
 721
 722         public static void setIdentity(AxisAngle4d aa) {
 723                 aa.angle = 0.0;
 724                 aa.x = 0.0;
 725                 aa.y = 1.0;
 726                 aa.z = 0.0;
 727         }
 728
 729         public static void set(Matrix3d mat, double m00, double m01, double m02,
 730                         double m10, double m11, double m12, double m20, double m21,
 731                         double m22) {
 732                 mat.m00 = m00;
 733                 mat.m01 = m01;
 734                 mat.m02 = m02;
 735
 736                 mat.m10 = m10;
 737                 mat.m11 = m11;
 738                 mat.m12 = m12;
 739
 740                 mat.m20 = m20;
 741                 mat.m21 = m21;
 742                 mat.m22 = m22;
 743         }
 744
 745         public static void set(Matrix4d mat, double[] v) {
 746                 mat.m00 = v[0];
 747                 mat.m01 = v[1];
 748                 mat.m02 = v[2];
 749                 mat.m03 = v[3];
 750
 751                 mat.m10 = v[4];
 752                 mat.m11 = v[5];
 753                 mat.m12 = v[6];
 754                 mat.m13 = v[7];
 755
 756                 mat.m20 = v[8];
 757                 mat.m21 = v[9];
 758                 mat.m22 = v[10];
 759                 mat.m23 = v[11];
 760
 761                 mat.m30 = v[12];
 762                 mat.m31 = v[13];
 763                 mat.m32 = v[14];
 764                 mat.m33 = v[15];
 765
 766         }
 767
 768         public static boolean createRotation(Vector3d original, Vector3d rotated, AxisAngle4d result) {
 769
 770                 if (rotated.lengthSquared() > 0.01)
 771                         rotated.normalize();
 772                 else
 773                         return false;
 774                 double d = original.dot(rotated);
 775                 if (d > 0.9999) {
 776                         // original and rotated are parallel, pointing at the same direction
 777                         result.angle = 0.0;
 778                         result.x = 0.0;
 779                         result.y = 1.0;
 780                         result.z = 0.0;
 781                 } else if (d < -0.9999) {
 782                         // original and rotated are parallel, pointing at the opposite direction
 783                         Vector3d a = projectToPlane(getMinAxis(original), original);
 784                         a.normalize();
 785                         result.set(a, Math.PI);
 786                 } else {
 787                         double angle = original.angle(rotated);
 788                         Vector3d axis = new Vector3d();
 789                         axis.cross(original, rotated);
 790                         result.set(axis,angle);
 791                 }
 792                 return true;
 793         }
 794
 795         public static boolean createRotation(Vector3d original, Vector3d rotated, Quat4d result) {
 796
 797                 if (rotated.lengthSquared() > 0.01)
 798                         rotated.normalize();
 799                 else
 800                         return false;
 801                 double d = original.dot(rotated);
 802                 if (d > 0.9999) {
 803                         // original and rotated are parallel, pointing at the same direction
 804                         result.w = 1.0;
 805                         result.x = 0.0;
 806                         result.y = 0.0;
 807                         result.z = 0.0;
 808                 } else if (d < -0.9999) {
 809                         // original and rotated are parallel, pointing at the opposite direction
 810                         Vector3d a = projectToPlane(getMinAxis(original), original);
 811                         a.normalize();
 812                         result.set(a.getX(), a.getY(), a.getZ(), 0.0);
 813                 } else {
 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);
 819                         axis.normalize();
 820                         result.set(sinHalf * axis.getX(), sinHalf * axis.getY(), sinHalf * axis.getZ(), cosHalf);
 821                 }
 822                 return true;
 823         }
 824
 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());
 829
 830                 if (absX <= absY && absX <= absZ)
 831                         return X_AXIS;
 832                 else if (absY <= absZ)
 833                         return Y_AXIS;
 834                 else
 835                         return Z_AXIS;
 836         }
 837
 838         public static boolean createRotation(Vector3d original, Vector3d rotated, Vector3d axis, AxisAngle4d result) {
 839
 840         if (rotated.lengthSquared() > 0.01)
 841             rotated.normalize();
 842         else
 843             return false;
 844         if (original.lengthSquared() > 0.01)
 845             original.normalize();
 846         else
 847             return false;
 848         if (axis.lengthSquared() > 0.01)
 849             axis.normalize();
 850         else
 851             return false;
 852         double d = original.dot(rotated);
 853         if (d > 0.9999) {
 854             // original and rotated are parallel, pointing at the same direction
 855             result.angle = 0.0;
 856             result.x = axis.x;
 857             result.y = axis.y;
 858             result.z = axis.z;
 859         } else if (d < -0.9999) {
 860             // original and rotated are parallel, pointing at the opposite direction
 861             result.angle = Math.PI;
 862             result.x = axis.x;
 863             result.y = axis.y;
 864             result.z = axis.z;
 865         } else {
 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();
 873             rotate(q, p1, t1);
 874             rotate(q, p2, t2);
 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);
 879
 880         }
 881         return true;
 882     }
 883
 884         public static void getQuat(Vector3d axis, double angle, Quat4d q)
 885         {
 886                 double mag,amag;
 887                 // Quat = cos(theta/2) + sin(theta/2)(roation_axis)
 888
 889                 amag = Math.sqrt( axis.x*axis.x + axis.y*axis.y + axis.z*axis.z);
 890                 if( amag < EPS ) {
 891                         q.w = 1.0;
 892                         q.x = 0.0;
 893                         q.y = 0.0;
 894                         q.z = 0.0;
 895                 } else {
 896                         amag = 1.0/amag;
 897                         double a2 = angle*0.5;
 898                         mag = Math.sin(a2);
 899                         q.w = Math.cos(a2);
 900                         q.x = axis.x*amag*mag;
 901                         q.y = axis.y*amag*mag;
 902                         q.z = axis.z*amag*mag;
 903                 }
 904
 905         }
 906
 907         /**
 908          * Linear interpolation of quaternions. Result IS set to q1.
 909          * @param q1
 910          * @param q2
 911          * @param alpha
 912          */
 913         public static void lip(Quat4d q1, Quat4d q2, double alpha) {
 914                 double s1 = 1.0 - alpha;
 915                 double s2 = alpha;
 916                 q1.scale(s1);
 917                 mad(q1,q2,s2);
 918                 q1.normalize();
 919         }
 920
 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;
 923         }
 924
 925         public static void mad(Tuple3d q1, Tuple3d q2, double s2) {
 926                 q1.x += q2.x * s2;
 927                 q1.y += q2.y * s2;
 928                 q1.z += q2.z * s2;
 929         }
 930
 931         public static void mad(Quat4d q1, Quat4d q2, double s2) {
 932                 q1.x += q2.x * s2;
 933                 q1.y += q2.y * s2;
 934                 q1.z += q2.z * s2;
 935                 q1.w += q2.w * s2;
 936         }
 937
 938         /**
 939          * Slerp
 940          *
 941          * Sets results to q1. Modifies q2.
 942          *
 943          * @param q1
 944          * @param q2
 945          * @param alpha
 946          */
 947         public static void sip(Quat4d q1, Quat4d q2, double alpha) {
 948                 double cosom = dot(q1,q2);
 949                 if (cosom < 0.0) {
 950                         cosom = -cosom;
 951                         q2.negate();
 952                 }
 953
 954                 if (cosom > 0.9999) {
 955                         q2.sub(q1);
 956                         q2.scale(alpha);
 957                         q1.add(q2);
 958                         q1.normalize();
 959                         return;
 960                 }
 961                 double theta_0 = Math.acos(cosom);
 962                 double theta = theta_0 * alpha;
 963                 Quat4d t = new Quat4d(q1);
 964                 t.scale(-cosom);
 965                 t.add(q2);
 966                 t.normalize();
 967                 t.scale(Math.sin(theta));
 968                 q1.scale(Math.cos(theta));
 969                 q1.add(t);
 970         }
 971
 972
 973         public static void rotate(double angle, Tuple2d v1, Tuple2d v2) {
 974                 // TODO : verify implementation
 975                 double sin = Math.sin(angle);
 976                 if (sin == 1.0) {
 977                         v2.x = v1.y;
 978                         v2.y = -v1.x;
 979                 } else if (sin == -1.0) {
 980                         v2.x = -v1.y;
 981                         v2.y = v1.x;
 982                 } else {
 983                         double cos = Math.cos(angle);
 984                         if (cos == -1.0) {
 985                                 v2.x = -v1.x;
 986                                 v2.y = -v1.y;
 987                         } else if (cos != 1.0) {
 988                                 v2.x= v1.x * cos + v1.y * -sin;
 989                                 v2.y= v1.x* sin + v1.y *cos;
 990                         }
 991                 }
 992         }
 993
 994         public static Tuple3d getPosRot(double m3x2[]) {
 995                 Vector3d t = new Vector3d();
 996                 t.x = m3x2[4];
 997                 t.y = m3x2[5];
 998
 999
1000                 Vector2d v2 = new Vector2d(1,0);
1001                 Vector2d v = new Vector2d();
1002                 // use rotation of (1,0) to calculate the rotation component
1003                 v.x  =  m3x2[0];
1004                 v.y  =  m3x2[2];
1005                 double a1 = v2.angle(v);
1006                 if (v.y < 0) {
1007                         t.z = a1;
1008                 } else {
1009                         t.z = Math.PI*2.0 - a1;
1010                 }
1011                 return t;
1012         }
1013
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);
1022                 mat.m32 = -1.0;
1023                 return mat;
1024         }
1025
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);
1031                 mat.m33 =  1.0;
1032                 mat.m03 = -(r+l)/(r-l);
1033                 mat.m13 = -(t+b)/(t-b);
1034                 mat.m23 = -(f+n)/(f-n);
1035                 return mat;
1036         }
1037
1038         /**
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.
1041          */
1042         public static double round(double value, int nsig) {
1043                 if (Math.abs(value) < NEAR_ZERO)
1044                         return 0.0;
1045
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;
1053                 } else {
1054                         // Number was not close to a shorter decimal representation, return it as is
1055                         return value;
1056                 }
1057         }
1058 }