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.0000001;
  37     public static double NEAR_HALF = 0.4999999;
  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 boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Tuple3d intersectPoint) {
 151         intersectPoint.set(planePoint);
 152         intersectPoint.sub(linePoint);
 153         double u = planeNormal.dot(new Vector3d(intersectPoint));
 154         double v = planeNormal.dot(lineDir);
 155         if (Math.abs(v) < NEAR_ZERO)
 156             return false;
 157         u /= v;
 158         intersectPoint.set(lineDir);
 159         intersectPoint.scale(u);
 160         intersectPoint.add(linePoint);
 161         return true;
 162     }
 163
 164     public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Vector3d intersectPoint, double[] u) {
 165         intersectPoint.set(planePoint);
 166         intersectPoint.sub(linePoint);
 167         u[0] = planeNormal.dot(intersectPoint);
 168         double v = planeNormal.dot(lineDir);
 169         if (Math.abs(v) < NEAR_ZERO)
 170             return false;
 171         u[0] /= v;
 172         intersectPoint.set(lineDir);
 173         intersectPoint.scale(u[0]);
 174         intersectPoint.add(linePoint);
 175         return true;
 176     }
 177
 178     public static boolean intersectLineLine(Tuple3d l1_start,Tuple3d l1_end,Tuple3d l2_start,Tuple3d l2_end,Tuple3d l1_pos, Tuple3d l2_pos) {
 179             Vector3d p13 = new Vector3d();
 180             Vector3d p43 = new Vector3d();
 181             Vector3d p21 = new Vector3d();
 182             double d1343,d4321,d1321,d4343,d2121;
 183             double numer,denom;
 184             p13.sub(l1_start, l2_start);
 185             p43.sub(l2_end,l2_start);
 186             if (Math.abs(p43.x)  < NEAR_ZERO && Math.abs(p43.y)  < NEAR_ZERO && Math.abs(p43.z)  < NEAR_ZERO)
 187                return false;
 188             p21.sub(l1_end,l1_start);
 189             if (Math.abs(p21.x)  < NEAR_ZERO && Math.abs(p21.y)  < NEAR_ZERO && Math.abs(p21.z)  < NEAR_ZERO)
 190                return false;
 191
 192             d1343 = p13.dot(p43);
 193             d4321 = p43.dot(p21);
 194             d1321 = p13.dot(p21);
 195             d4343 = p43.lengthSquared();
 196             d2121 = p21.lengthSquared();
 197
 198             denom = d2121 * d4343 - d4321 * d4321;
 199             if (Math.abs(denom) < NEAR_ZERO)
 200                return false;
 201             numer = d1343 * d4321 - d1321 * d4343;
 202
 203             double mua = numer / denom;
 204             double mub = (d1343 + d4321 * mua) / d4343;
 205
 206             l1_pos.x = l1_start.x + mua * p21.x;
 207             l1_pos.y = l1_start.y + mua * p21.y;
 208             l1_pos.z = l1_start.z + mua * p21.z;
 209             l2_pos.x = l2_start.x + mub * p43.x;
 210             l2_pos.y = l2_start.y + mub * p43.y;
 211             l2_pos.z = l2_start.z + mub * p43.z;
 212
 213             return true;
 214     }
 215
 216     public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb) {
 217         Vector3d p13 = new Vector3d();
 218
 219         double d1343,d4321,d1321,d4343,d2121;
 220         double numer,denom;
 221
 222         p13.sub(p1, p3);
 223         if (Math.abs(p43.x)  < NEAR_ZERO && Math.abs(p43.y)  < NEAR_ZERO && Math.abs(p43.z)  < NEAR_ZERO)
 224            return false;
 225         if (Math.abs(p21.x)  < NEAR_ZERO && Math.abs(p21.y)  < NEAR_ZERO && Math.abs(p21.z)  < NEAR_ZERO)
 226            return false;
 227
 228         d1343 = p13.dot(p43);
 229         d4321 = p43.dot(p21);
 230         d1321 = p13.dot(p21);
 231         d4343 = p43.lengthSquared();
 232         d2121 = p21.lengthSquared();
 233
 234         denom = d2121 * d4343 - d4321 * d4321;
 235         if (Math.abs(denom) < NEAR_ZERO)
 236            return false;
 237         numer = d1343 * d4321 - d1321 * d4343;
 238
 239         double mua = numer / denom;
 240         double mub = (d1343 + d4321 * mua) / d4343;
 241
 242         pa.x = p1.x + mua * p21.x;
 243         pa.y = p1.y + mua * p21.y;
 244         pa.z = p1.z + mua * p21.z;
 245         pb.x = p3.x + mub * p43.x;
 246         pb.y = p3.y + mub * p43.y;
 247         pb.z = p3.z + mub * p43.z;
 248
 249         return true;
 250    }
 251
 252     /**
 253      * Calculate the line segment PaPb that is the shortest route between
 254      *  two lines P1P2 and P3P4. Calculate also the values of mua and mub where
 255      *  Pa = P1 + mua (P2 - P1)
 256      *  Pb = P3 + mub (P4 - P3)
 257      * @param p1
 258      * @param p21
 259      * @param p3
 260      * @param p43
 261      * @param pa
 262      * @param pb
 263      * @param mu
 264      * @return
 265      */
 266     public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb, double mu[]) {
 267         Vector3d p13 = new Vector3d();
 268
 269         double d1343,d4321,d1321,d4343,d2121;
 270         double numer,denom;
 271         double EPS = 0.001;
 272         p13.sub(p1, p3);
 273         if (Math.abs(p43.x)  < EPS && Math.abs(p43.y)  < EPS && Math.abs(p43.z)  < EPS)
 274            return false;
 275         if (Math.abs(p21.x)  < EPS && Math.abs(p21.y)  < EPS && Math.abs(p21.z)  < EPS)
 276            return false;
 277
 278         d1343 = p13.dot(p43);
 279         d4321 = p43.dot(p21);
 280         d1321 = p13.dot(p21);
 281         d4343 = p43.lengthSquared();
 282         d2121 = p21.lengthSquared();
 283
 284         denom = d2121 * d4343 - d4321 * d4321;
 285         if (Math.abs(denom) < EPS)
 286            return false;
 287         numer = d1343 * d4321 - d1321 * d4343;
 288
 289         mu[0] = numer / denom;
 290         mu[1] = (d1343 + d4321 * mu[0]) / d4343;
 291
 292         pa.x = p1.x + mu[0] * p21.x;
 293         pa.y = p1.y + mu[0] * p21.y;
 294         pa.z = p1.z + mu[0] * p21.z;
 295         pb.x = p3.x + mu[1] * p43.x;
 296         pb.y = p3.y + mu[1] * p43.y;
 297         pb.z = p3.z + mu[1] * p43.z;
 298
 299         return true;
 300    }
 301
 302
 303
 304    public static void rotate(Quat4d q, Tuple3d in, Tuple3d out) {
 305        // p' = q * p * q'
 306        double tw =           - q.x*in.x - q.y*in.y - q.z*in.z;
 307        double tx =  q.w*in.x            + q.y*in.z - q.z*in.y;
 308        double ty =  q.w*in.y - q.x*in.z            + q.z*in.x;
 309        double tz =  q.w*in.z + q.x*in.y - q.y*in.x           ;
 310
 311        //temp * q' -> x = -x, y = -y z = -z
 312        //out.w = tw*q.w + tx*q.x + ty*q.y + tz*q.z;
 313        out.x =  -tw*q.x + tx*q.w - ty*q.z + tz*q.y;
 314        out.y =  -tw*q.y + tx*q.z + ty*q.w - tz*q.x;
 315        out.z =  -tw*q.z - tx*q.y + ty*q.x + tz*q.w;
 316    }
 317
 318    public static void getMatrix(Quat4d quat, Matrix3d m) {
 319                    m.m00 = 1.0f - 2.0 * (quat.y * quat.y + quat.z * quat.z);
 320                    m.m01 = 2.0 * (quat.x * quat.y + quat.w * quat.z);
 321                    m.m02 = 2.0 * (quat.x * quat.z - quat.w * quat.y);
 322                    m.m10 = 2.0 * (quat.x * quat.y - quat.w * quat.z);
 323                    m.m11 = 1.0 - 2.0f * (quat.x * quat.x + quat.z * quat.z);
 324                    m.m12 = 2.0 * (quat.y * quat.z + quat.w * quat.x);
 325                    m.m20 = 2.0 * (quat.x * quat.z + quat.w * quat.y);
 326                    m.m21 = 2.0 * (quat.y * quat.z - quat.w * quat.x);
 327                    m.m22 = 1.0 - 2.0f * (quat.x * quat.x + quat.y * quat.y);
 328
 329    }
 330
 331
 332    private static double q[] = new double[3];
 333    private static int nxt[] = { 1, 2, 0 };
 334    /**
 335     * Converts Matrix to Quaternion
 336     *
 337     * Note: non-thread safe.
 338     *
 339     * @param mat
 340     * @param quat
 341     */
 342    public static void getQuat(Matrix3d mat, Quat4d quat) {
 343            double tr = mat.m00 + mat.m11 + mat.m22;
 344                 if (tr > 0.0) {
 345                         double s = Math.sqrt(tr + 1.0);
 346                         quat.w = 0.5 * s;
 347                         s = 0.5 / s;
 348                         quat.x = (mat.m21 - mat.m12) * s;
 349                         quat.y = (mat.m02 - mat.m20) * s;
 350                         quat.z = (mat.m10 - mat.m01) * s;
 351                 } else {
 352                         int i = 0, j, k;
 353                         if (mat.m11 > mat.m00)
 354                                 i = 1;
 355                         if (mat.m22 > mat.getElement(i, i))
 356                                 i = 2;
 357
 358
 359                         j = nxt[i];
 360                         k = nxt[j];
 361
 362                         double s = Math.sqrt((mat.getElement(i, i) - (mat.getElement(j, j) + mat.getElement(k, k))) + 1.0);
 363
 364                         q[i] = s * 0.5;
 365
 366                         if (Math.abs(s) > 0.001)
 367                                 s = 0.5 / s;
 368
 369                         quat.w = (mat.getElement(k, j) - mat.getElement(j, k)) * s;
 370                         q[j] = (mat.getElement(j, i) + mat.getElement(i, j)) * s;
 371                         q[k] = (mat.getElement(k, i) + mat.getElement(i, k)) * s;
 372
 373                         quat.x = q[0];
 374                         quat.y = q[1];
 375                         quat.z = q[2];
 376                 }
 377         }
 378
 379    public static Quat4d getQuat(Matrix3d mat) {
 380            Quat4d q = new Quat4d();
 381            getQuat(mat, q);
 382            return q;
 383    }
 384
 385    public static AxisAngle4d getFromPseudoEuler(Vector3d euler) {
 386        AxisAngle4d aa = new AxisAngle4d();
 387        aa.angle = euler.length();
 388        Vector3d normal = new Vector3d(euler);
 389        if (aa.angle > NEAR_ZERO) {
 390            normal.normalize();
 391            aa.x = normal.x;
 392            aa.y = normal.y;
 393            aa.z = normal.z;
 394        } else {
 395            aa.x = 1.0;
 396            aa.y = 0.0;
 397            aa.z = 0.0;
 398        }
 399
 400        return aa;
 401    }
 402
 403    public static Vector3d getPseudoEuler(AxisAngle4d aa) {
 404        Vector3d euler = new Vector3d(aa.x,aa.y,aa.z);
 405        euler.scale(aa.angle);
 406        return euler;
 407    }
 408
 409
 410    public static void getQuat(Vector3d euler, Quat4d quat)  {
 411            Quat4d q = EulerTools.getQuatFromEuler(Order.YXZ, euler.y,euler.x,euler.z);
 412            quat.set(q);
 413         // http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Conversion_formulae_between_formalisms
 414         // Using the x-convention, the 3-1-3 Euler angles phi, theta and psi (around the Z, X and again the Z-axis)
 415 //         quat.x = -Math.cos((euler.x - euler.z)*0.5)*Math.sin(euler.y*0.5);
 416 //         quat.y = -Math.sin((euler.x - euler.z)*0.5)*Math.sin(euler.y*0.5);
 417 //         quat.z = -Math.sin((euler.x + euler.z)*0.5)*Math.cos(euler.y*0.5);
 418 //         quat.w = Math.sin((euler.x + euler.z)*0.5)*Math.cos(euler.y*0.5);
 419
 420            // http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
 421            // Y, Z, X order
 422 //          double c1 = Math.cos(euler.y*0.5);
 423 //          double s1 = Math.sin(euler.y*0.5);
 424 //          double c2 = Math.cos(euler.z*0.5);
 425 //          double s2 = Math.sin(euler.z*0.5);
 426 //          double c3 = Math.cos(euler.x*0.5);
 427 //          double s3 = Math.sin(euler.x*0.5);
 428 //          double c1c2 = c1*c2;
 429 //          double s1s2 = s1*s2;
 430 //          quat.w =c1c2*c3 - s1s2*s3;
 431 //          quat.x =c1c2*s3 + s1s2*c3;
 432 //          quat.y =s1*c2*c3 + c1*s2*s3;
 433 //          quat.z =c1*s2*c3 - s1*c2*s3;
 434
 435 //          Quat4d q2 = EulerTools.getQuatFromEuler(Order.YZX, euler.y,euler.z,euler.x);
 436 //          System.out.println("Q " + quat + " Q2 " + q2);
 437 //         double c1 = Math.cos(euler.y);
 438 //          double s1 = Math.sin(euler.y);
 439 //          double c2 = Math.cos(euler.z);
 440 //          double s2 = Math.sin(euler.z);
 441 //          double c3 = Math.cos(euler.x);
 442 //          double s3 = Math.sin(euler.x);
 443 //          quat.w = Math.sqrt(1.0 + c1 * c2 + c1*c3 - s1 * s2 * s3 + c2*c3) / 2.0;
 444 //          double w4 = (4.0 * quat.w);
 445 //          quat.x = (c2 * s3 + c1 * s3 + s1 * s2 * c3) / w4 ;
 446 //          quat.y = (s1 * c2 + s1 * c3 + c1 * s2 * s3) / w4 ;
 447 //          quat.z = (-s1 * s3 + c1 * s2 * c3 +s2) / w4 ;
 448    }
 449
 450
 451
 452
 453    public static void getEuler(Quat4d quat,Vector3d euler)  {
 454            Vector3d e = EulerTools.getEulerFromQuat(Order.YXZ, quat);
 455            euler.x = e.y;
 456            euler.y = e.x;
 457            euler.z = e.z;
 458
 459            // http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Conversion_formulae_between_formalisms
 460 //         euler.x = Math.atan2(quat.x * quat.z + quat.y* quat.w, quat.y*quat.z - quat.x * quat.w);
 461 //         euler.y = Math.acos(-square(quat.x) - square(quat.y) + square(quat.z) + square(quat.w));
 462 //         euler.z = -Math.atan2(quat.x * quat.z - quat.y* quat.w, quat.y*quat.z + quat.x * quat.w);
 463
 464            // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
 465           // Y, Z, X order
 466 //         double test = quat.x * quat.y + quat.z * quat.w;
 467 //         if (test > NEAR_HALF) {
 468 //                 euler.y = 2.0 * Math.atan2(quat.x,quat.w);
 469 //                 euler.z = Math.PI * 0.5;
 470 //                 euler.x = 0.0;
 471 //         } else if (test < -NEAR_HALF) {
 472 //                 euler.y = -2.0 * Math.atan2(quat.x,quat.w);
 473 //                 euler.z = -Math.PI * 0.5;
 474 //                 euler.x = 0.0;
 475 //         } else {
 476 //                 double sqx = square(quat.x);
 477 //                 double sqy = square(quat.y);
 478 //                 double sqz = square(quat.z);
 479 //                 euler.y = Math.atan2(2.0*(quat.y*quat.w-quat.x*quat.z), 1.0 - 2.0*(sqy-sqz));
 480 //                 euler.z = Math.asin(2.0*test);
 481 //                 euler.x = Math.atan2(2.0*(quat.x*quat.w-quat.y*quat.z), 1.0 - 2.0*(sqx-sqz));
 482 //                 System.out.println(euler + " " + EulerTools.getEulerFromQuat(Order.YXZ, quat) +  " " + quat);
 483 //         }
 484 //         double sqw = quat.w*quat.w;
 485 //          double sqx = quat.x*quat.x;
 486 //          double sqy = quat.y*quat.y;
 487 //          double sqz = quat.z*quat.z;
 488 //              double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
 489 //              double test = quat.x*quat.y + quat.z*quat.w;
 490 //              if (test > 0.499*unit) { // singularity at north pole
 491 //                      euler.y = 2 * Math.atan2(quat.x,quat.w);
 492 //                      euler.z = Math.PI/2;
 493 //                      euler.x = 0;
 494 //                      return;
 495 //              }
 496 //              if (test < -0.499*unit) { // singularity at south pole
 497 //                      euler.y = -2 * Math.atan2(quat.x,quat.w);
 498 //                      euler.z = -Math.PI/2;
 499 //                      euler.x = 0;
 500 //                      return;
 501 //              }
 502 //              euler.y = Math.atan2(2*quat.y*quat.w-2*quat.x*quat.z , sqx - sqy - sqz + sqw);
 503 //              euler.z = Math.asin(2*test/unit);
 504 //              euler.x = Math.atan2(2*quat.x*quat.w-2*quat.y*quat.z , -sqx + sqy - sqz + sqw);
 505    }
 506
 507    public static Quat4d getQuat(Vector3d euler) {
 508            Quat4d q = new Quat4d();
 509            getQuat(euler,q);
 510            return q;
 511    }
 512
 513
 514    public static Vector3d getEuler(Quat4d quat) {
 515            Vector3d v = new Vector3d();
 516            getEuler(quat, v);
 517            return v;
 518    }
 519
 520    public static Quat4d getQuat(AxisAngle4d aa) {
 521            Quat4d q = new Quat4d();
 522            getQuat(aa, q);
 523            return q;
 524    }
 525
 526         public static AxisAngle4d getAxisAngle(Quat4d q) {
 527                 AxisAngle4d aa = new AxisAngle4d();
 528                 double mag = q.x * q.x + q.y * q.y + q.z * q.z;
 529
 530                 if (mag > EPS) {
 531                         mag = Math.sqrt(mag);
 532                         aa.angle = 2.0 * Math.atan2(mag, q.w);
 533                         mag = 1.0 / mag;
 534                         aa.x = q.x * mag;
 535                         aa.y = q.y * mag;
 536                         aa.z = q.z * mag;
 537
 538                 } else {
 539                         aa.x = 0.0;
 540                         aa.y = 1.0;
 541                         aa.z = 0.0;
 542                         aa.angle = 0.0;
 543                 }
 544                 // aa.set(q);
 545                 return aa;
 546         }
 547
 548    public static Quat4d getIdentityQuat() {
 549            return new Quat4d(0, 0, 0, 1);
 550    }
 551
 552    public static void getQuat(AxisAngle4d aa, Quat4d q) {
 553            double mag,amag;
 554                 // Quat = cos(theta/2) + sin(theta/2)(roation_axis)
 555
 556                 amag = Math.sqrt( aa.x*aa.x + aa.y*aa.y + aa.z*aa.z);
 557                 if( amag < NEAR_ZERO ) {
 558                     q.w = 1.0;
 559                     q.x = 0.0;
 560                     q.y = 0.0;
 561                     q.z = 0.0;
 562                 } else {
 563                    amag = 1.0/amag;
 564                    double a2 = aa.angle * 0.5;
 565                    mag = Math.sin(a2);
 566                    q.w = Math.cos(a2);
 567                q.x = aa.x*amag*mag;
 568                q.y = aa.y*amag*mag;
 569                q.z = aa.z*amag*mag;
 570                 }
 571    }
 572
 573
 574         /*
 575          * Cohen-Sutherland
 576          */
 577
 578         private static final int IN = 0;
 579         private static final int LEFT = 1;
 580         private static final int RIGHT = 2;
 581         private static final int BOTTOM = 4;
 582         private static final int TOP = 8;
 583
 584
 585         private static int bitcode(Vector2f p1, Vector2f min, Vector2f max) {
 586                 int code = IN;
 587                 if (p1.x < min.x)
 588                         code |= LEFT;
 589                 else if (p1.x > max.x)
 590                         code |= RIGHT;
 591                 if (p1.y < min.y)
 592                         code |= BOTTOM;
 593                 else if (p1.y > max.y)
 594                         code |= TOP;
 595                 return code;
 596         }
 597
 598         public static boolean clipLineRectangle(Vector2f p1,Vector2f p2, Vector2f min, Vector2f max, Vector2f r1, Vector2f r2) {
 599                 while (true) {
 600                         int o1 = bitcode(p1, min, max);
 601                         int o2 = bitcode(p2, min, max);
 602                         int and = o1 & o2;
 603                         int or = o1 | o2;
 604                         if (and != IN) {
 605                                 return false;
 606                         }
 607                         if (or == IN) {
 608                                 r1.set(p1);
 609                                 r2.set(p2);
 610                                 return true;
 611                         }
 612                         if (o1 == IN) {
 613                                 Vector2f t = p1;
 614                                 p1 = p2;
 615                                 p2 = t;
 616                                 int t2 = o1;
 617                                 o1 = o2;
 618                                 o2 = t2;
 619                         }
 620                         if ((o1 & TOP) != IN) {
 621                                 float t = (max.y - p1.y) / (p2.y - p1.y);
 622                                 p1.x += t * (p2.x - p1.x);
 623                                 p1.y = max.y;
 624                         } else if ((o1 & BOTTOM) != IN) {
 625                                 float t = (min.y - p1.y) / (p2.y - p1.y);
 626                                 p1.x += t * (p2.x - p1.x);
 627                                 p1.y = min.y;
 628                         } else if ((o1 & LEFT) != IN) {
 629                                 float t = (min.x - p1.x) / (p2.x - p1.x);
 630                                 p1.y += t * (p2.y - p1.y);
 631                                 p1.x = min.x;
 632                         } else if ((o1 & RIGHT) != IN) {
 633                                 float t = (max.x - p1.x) / (p2.x - p1.x);
 634                                 p1.y += t * (p2.y - p1.y);
 635                                 p1.x = max.x;
 636                         } else {
 637                                 throw new RuntimeException("Error in clipping code");
 638                         }
 639                 }
 640
 641         }
 642
 643         public static double square(double d) {
 644                 return d * d;
 645         }
 646
 647
 648          public static void multiplyOrientation(AxisAngle4d aa, AxisAngle4d rot)  {
 649                 Quat4d q1 = new Quat4d();
 650                 getQuat(aa, q1);
 651                 Quat4d q2 = new Quat4d();
 652                 getQuat(rot, q2);
 653                 q2.mul(q1);
 654                 rot.set(q2);
 655          }
 656
 657          public static double radToDeg(double rad) {
 658                  return (rad / Math.PI) * 180.0;
 659          }
 660
 661          public static double degToRad(double deg) {
 662                  return (deg / 180.0) * Math.PI;
 663          }
 664
 665          public static double clamp(double min, double max,double v) {
 666                  if (v < min)
 667                          return min;
 668                  if (v > max)
 669                          return max;
 670                  return v;
 671          }
 672
 673          public static AxisAngle4d createRotation(Vector3d original, Vector3d rotated) {
 674                 AxisAngle4d result = new AxisAngle4d();
 675                 if (createRotation(original, rotated, result))
 676                         return result;
 677                 return null;
 678          }
 679
 680
 681          public static void setIdentity(Quat4d q) {
 682                 q.w = 1.0;
 683                 q.x = 0.0;
 684                 q.y = 0.0;
 685                 q.z = 0.0;
 686          }
 687
 688         public static void setIdentity(AxisAngle4d aa) {
 689                 aa.angle = 0.0;
 690                 aa.x = 0.0;
 691                 aa.y = 1.0;
 692                 aa.z = 0.0;
 693         }
 694
 695         public static void set(Matrix3d mat, double m00, double m01, double m02,
 696                         double m10, double m11, double m12, double m20, double m21,
 697                         double m22) {
 698                 mat.m00 = m00;
 699                 mat.m01 = m01;
 700                 mat.m02 = m02;
 701
 702                 mat.m10 = m10;
 703                 mat.m11 = m11;
 704                 mat.m12 = m12;
 705
 706                 mat.m20 = m20;
 707                 mat.m21 = m21;
 708                 mat.m22 = m22;
 709         }
 710
 711         public static void set(Matrix4d mat, double[] v) {
 712                 mat.m00 = v[0];
 713                 mat.m01 = v[1];
 714                 mat.m02 = v[2];
 715                 mat.m03 = v[3];
 716
 717                 mat.m10 = v[4];
 718                 mat.m11 = v[5];
 719                 mat.m12 = v[6];
 720                 mat.m13 = v[7];
 721
 722                 mat.m20 = v[8];
 723                 mat.m21 = v[9];
 724                 mat.m22 = v[10];
 725                 mat.m23 = v[11];
 726
 727                 mat.m30 = v[12];
 728                 mat.m31 = v[13];
 729                 mat.m32 = v[14];
 730                 mat.m33 = v[15];
 731
 732         }
 733
 734          public static boolean createRotation(Vector3d original, Vector3d rotated, AxisAngle4d result) {
 735
 736                         if (rotated.lengthSquared() > 0.01)
 737                                 rotated.normalize();
 738                         else
 739                                 return false;
 740                         double d = original.dot(rotated);
 741                         if (d > 0.9999) {
 742                                 // original and rotated are parallel, pointing at the same direction
 743                                 result.angle = 0.0;
 744                                 result.x = 0.0;
 745                                 result.y = 1.0;
 746                                 result.z = 0.0;
 747                         } else if (d < -0.9999) {
 748                                 // original and rotated are parallel, pointing at the opposite direction
 749                                 Vector3d a = Z_AXIS;
 750                                 if (Math.abs(a.dot(original)) > 0.8 )
 751                                         a = Y_AXIS;
 752                                 result.set(a, Math.PI);
 753                         } else {
 754                                 double angle = original.angle(rotated);
 755                                 Vector3d axis = new Vector3d();
 756                                 axis.cross(original, rotated);
 757                                 result.set(axis,angle);
 758                         }
 759                         return true;
 760                  }
 761
 762          public static boolean createRotation(Vector3d original, Vector3d rotated, Quat4d result) {
 763
 764                         if (rotated.lengthSquared() > 0.01)
 765                                 rotated.normalize();
 766                         else
 767                                 return false;
 768                         double d = original.dot(rotated);
 769                         if (d > 0.9999) {
 770                                 // original and rotated are parallel, pointing at the same direction
 771                                 result.w = 1.0;
 772                                 result.x = 0.0;
 773                                 result.y = 0.0;
 774                                 result.z = 0.0;
 775                         } else if (d < -0.9999) {
 776                                 // original and rotated are parallel, pointing at the opposite direction
 777                                 Vector3d a = Z_AXIS;
 778                                 if (Math.abs(a.dot(original)) > 0.8 )
 779                                         a = Y_AXIS;
 780                                 getQuat(a, Math.PI, result);
 781
 782                         } else {
 783                                 double angle = original.angle(rotated);
 784                                 Vector3d axis = new Vector3d();
 785                                 axis.cross(original, rotated);
 786                                 getQuat(axis, angle, result);
 787                         }
 788                         return true;
 789                  }
 790
 791          public static void getQuat(Vector3d axis, double angle, Quat4d q)
 792     {
 793                 double mag,amag;
 794                 // Quat = cos(theta/2) + sin(theta/2)(roation_axis)
 795
 796                 amag = Math.sqrt( axis.x*axis.x + axis.y*axis.y + axis.z*axis.z);
 797                 if( amag < EPS ) {
 798                     q.w = 1.0;
 799                     q.x = 0.0;
 800                     q.y = 0.0;
 801                     q.z = 0.0;
 802                 } else {
 803                     amag = 1.0/amag;
 804                     double a2 = angle*0.5;
 805                     mag = Math.sin(a2);
 806                     q.w = Math.cos(a2);
 807                     q.x = axis.x*amag*mag;
 808                     q.y = axis.y*amag*mag;
 809                     q.z = axis.z*amag*mag;
 810                 }
 811
 812     }
 813
 814          /**
 815           * Linear interpolation of quaternions. Result IS set to q1.
 816           * @param q1
 817           * @param q2
 818           * @param alpha
 819           */
 820          public static void lip(Quat4d q1, Quat4d q2, double alpha) {
 821                  double s1 = 1.0 - alpha;
 822                  double s2 = alpha;
 823                  q1.scale(s1);
 824                  mad(q1,q2,s2);
 825                  q1.normalize();
 826          }
 827
 828          public static double dot(Quat4d q1, Quat4d q2) {
 829                  return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
 830          }
 831
 832          public static void mad(Tuple3d q1, Tuple3d q2, double s2) {
 833                  q1.x += q2.x * s2;
 834                  q1.y += q2.y * s2;
 835                  q1.z += q2.z * s2;
 836          }
 837
 838          public static void mad(Quat4d q1, Quat4d q2, double s2) {
 839                  q1.x += q2.x * s2;
 840                  q1.y += q2.y * s2;
 841                  q1.z += q2.z * s2;
 842                  q1.w += q2.w * s2;
 843          }
 844
 845          /**
 846           * Slerp
 847           *
 848           * Sets results to q1. Modifies q2.
 849           *
 850           * @param q1
 851           * @param q2
 852           * @param alpha
 853           */
 854          public static void sip(Quat4d q1, Quat4d q2, double alpha) {
 855                  double cosom = dot(q1,q2);
 856                  if (cosom < 0.0) {
 857                          cosom = -cosom;
 858                          q2.negate();
 859                  }
 860
 861                  if (cosom > 0.9999) {
 862                          q2.sub(q1);
 863                          q2.scale(alpha);
 864                          q1.add(q2);
 865                          q1.normalize();
 866                          return;
 867                  }
 868                  double theta_0 = Math.acos(cosom);
 869                  double theta = theta_0 * alpha;
 870                  Quat4d t = new Quat4d(q1);
 871                  t.scale(-cosom);
 872                  t.add(q2);
 873                  t.normalize();
 874                  t.scale(Math.sin(theta));
 875                  q1.scale(Math.cos(theta));
 876                  q1.add(t);
 877          }
 878
 879
 880          public static void rotate(double angle, Tuple2d v1, Tuple2d v2) {
 881                  // TODO : verify implementation
 882         double sin = Math.sin(angle);
 883         if (sin == 1.0) {
 884             v2.x = v1.y;
 885             v2.y = -v1.x;
 886         } else if (sin == -1.0) {
 887                 v2.x = -v1.y;
 888             v2.y = v1.x;
 889         } else {
 890             double cos = Math.cos(angle);
 891             if (cos == -1.0) {
 892                 v2.x = -v1.x;
 893                 v2.y = -v1.y;
 894             } else if (cos != 1.0) {
 895                 v2.x= v1.x * cos + v1.y * -sin;
 896                 v2.y= v1.x* sin + v1.y *cos;
 897             }
 898         }
 899          }
 900
 901          public static Tuple3d getPosRot(double m3x2[]) {
 902                  Vector3d t = new Vector3d();
 903                  t.x = m3x2[4];
 904                  t.y = m3x2[5];
 905
 906
 907                  Vector2d v2 = new Vector2d(1,0);
 908                  Vector2d v = new Vector2d();
 909                  // use rotation of (1,0) to calculate the rotation component
 910          v.x  =  m3x2[0];
 911          v.y  =  m3x2[2];
 912          double a1 = v2.angle(v);
 913          if (v.y < 0) {
 914                  t.z = a1;
 915          } else {
 916                  t.z = Math.PI*2.0 - a1;
 917          }
 918          return t;
 919          }
 920 }