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

   1 /*******************************************************************************\r
   2  * Copyright (c) 2012, 2013 Association for Decentralized Information Management in\r
   3  * Industry THTH ry.\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
   8  *\r
   9  * Contributors:\r
  10  *     VTT Technical Research Centre of Finland - initial API and implementation\r
  11  *******************************************************************************/\r
  12 package org.simantics.g3d.math;\r
  13 \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
  22 \r
  23 import org.simantics.g3d.math.EulerTools.Order;\r
  24 \r
  25 \r
  26 /**\r
  27  * Some useful geometry related math functions. Beware, methods may modify their input parameters!\r
  28  * \r
  29  * @author Marko Luukkainen\r
  30  *\r
  31  */\r
  32 public class MathTools {\r
  33     \r
  34     public static double NEAR_ZERO = 0.0000001;\r
  35     public static double NEAR_HALF = 0.4999999;\r
  36     \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
  41         \r
  42         final static double EPS = 1.0e-12;\r
  43         \r
  44         \r
  45         public static boolean equals(double d1, double d2) {\r
  46                 return Math.abs(d1-d2) < EPS;\r
  47         }\r
  48         \r
  49         public static boolean equals(Tuple3d p1, Tuple3d p2) {\r
  50                 return distanceSquared(p1, p2) < NEAR_ZERO;\r
  51         }\r
  52         \r
  53         public static boolean equals(Tuple4d p1, Tuple4d p2) {\r
  54                 return distanceSquared(p1, p2) < NEAR_ZERO;\r
  55         }\r
  56         \r
  57         public static double distance(Tuple3d p1, Tuple3d p2) {\r
  58                 double dx, dy, dz;\r
  59 \r
  60                 dx = p2.x - p1.x;\r
  61                 dy = p2.y - p1.y;\r
  62                 dz = p2.z - p1.z;\r
  63                 return Math.sqrt(dx*dx+dy*dy+dz*dz);\r
  64         }\r
  65         \r
  66         public static double distance(Tuple4d p1, Tuple4d p2) {\r
  67                 double dx, dy, dz, dw;\r
  68 \r
  69                 dx = p2.x - p1.x;\r
  70                 dy = p2.y - p1.y;\r
  71                 dz = p2.z - p1.z;\r
  72                 dw = p2.w - p1.w;\r
  73                 return Math.sqrt(dx*dx+dy*dy+dz*dz+dw*dw);\r
  74         }\r
  75         \r
  76         public static double distanceSquared(Tuple3d p1, Tuple3d p2) {\r
  77                 double dx, dy, dz;\r
  78 \r
  79                 dx = p2.x - p1.x;\r
  80                 dy = p2.y - p1.y;\r
  81                 dz = p2.z - p1.z;\r
  82                 return dx*dx+dy*dy+dz*dz;\r
  83         }\r
  84         \r
  85         public static double distanceSquared(Tuple4d p1, Tuple4d p2) {\r
  86                 double dx, dy, dz, dw;\r
  87 \r
  88                 dx = p2.x - p1.x;\r
  89                 dy = p2.y - p1.y;\r
  90                 dz = p2.z - p1.z;\r
  91                 dw = p2.w - p1.w;\r
  92                 return dx*dx+dy*dy+dz*dz+dw*dw;\r
  93         }\r
  94         \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
  99         }\r
 100     \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
 104         v.sub(edgePoint1);\r
 105         double t = v.dot(point);\r
 106         t /= v.lengthSquared();\r
 107         if (t <= 0.0f)\r
 108           return edgePoint1;\r
 109         if (t >= 1.0f)\r
 110           return edgePoint2;\r
 111         v.scale(t);\r
 112         v.add(edgePoint1);\r
 113         return v;   \r
 114     }\r
 115     \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
 122         v.scale(t);\r
 123         v.add(straightPoint);\r
 124         return v;   \r
 125     }\r
 126     \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
 133         v.scale(u[0]);\r
 134         v.add(straightPoint);\r
 135         return v;   \r
 136     }\r
 137     \r
 138     public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, Tuple3d planePoint) {\r
 139         point.sub(planePoint);\r
 140         \r
 141         return planeNormal.dot(point);\r
 142     }\r
 143       \r
 144     public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, float d) {\r
 145         return (planeNormal.dot(point) + d);\r
 146     }\r
 147     \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
 154             return false;\r
 155         u /= v;\r
 156         intersectPoint.set(lineDir);\r
 157         intersectPoint.scale(u);\r
 158         intersectPoint.add(linePoint);\r
 159         return true;\r
 160     }\r
 161     \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
 168             return false;\r
 169         u[0] /= v;\r
 170         intersectPoint.set(lineDir);\r
 171         intersectPoint.scale(u[0]);\r
 172         intersectPoint.add(linePoint);\r
 173         return true;\r
 174     }\r
 175     \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
 185                return false;\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
 188                return false;\r
 189 \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
 195 \r
 196             denom = d2121 * d4343 - d4321 * d4321;\r
 197             if (Math.abs(denom) < NEAR_ZERO)\r
 198                return false;\r
 199             numer = d1343 * d4321 - d1321 * d4343;\r
 200 \r
 201             double mua = numer / denom;\r
 202             double mub = (d1343 + d4321 * mua) / d4343;\r
 203  \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
 210 \r
 211             return true;\r
 212     }\r
 213     \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
 216 \r
 217         double d1343,d4321,d1321,d4343,d2121;\r
 218         double numer,denom;\r
 219         \r
 220         p13.sub(p1, p3);\r
 221         if (Math.abs(p43.x)  < NEAR_ZERO && Math.abs(p43.y)  < NEAR_ZERO && Math.abs(p43.z)  < NEAR_ZERO)\r
 222            return false;\r
 223         if (Math.abs(p21.x)  < NEAR_ZERO && Math.abs(p21.y)  < NEAR_ZERO && Math.abs(p21.z)  < NEAR_ZERO)\r
 224            return false;\r
 225 \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
 231 \r
 232         denom = d2121 * d4343 - d4321 * d4321;\r
 233         if (Math.abs(denom) < NEAR_ZERO)\r
 234            return false;\r
 235         numer = d1343 * d4321 - d1321 * d4343;\r
 236 \r
 237         double mua = numer / denom;\r
 238         double mub = (d1343 + d4321 * mua) / d4343;\r
 239 \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
 246 \r
 247         return true;\r
 248    }\r
 249     \r
 250     /**\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
 255      * @param p1\r
 256      * @param p21\r
 257      * @param p3\r
 258      * @param p43\r
 259      * @param pa\r
 260      * @param pb\r
 261      * @param mu\r
 262      * @return\r
 263      */\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
 266 \r
 267         double d1343,d4321,d1321,d4343,d2121;\r
 268         double numer,denom;\r
 269         double EPS = 0.001;\r
 270         p13.sub(p1, p3);\r
 271         if (Math.abs(p43.x)  < EPS && Math.abs(p43.y)  < EPS && Math.abs(p43.z)  < EPS)\r
 272            return false;\r
 273         if (Math.abs(p21.x)  < EPS && Math.abs(p21.y)  < EPS && Math.abs(p21.z)  < EPS)\r
 274            return false;\r
 275 \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
 281 \r
 282         denom = d2121 * d4343 - d4321 * d4321;\r
 283         if (Math.abs(denom) < EPS)\r
 284            return false;\r
 285         numer = d1343 * d4321 - d1321 * d4343;\r
 286 \r
 287         mu[0] = numer / denom;\r
 288         mu[1] = (d1343 + d4321 * mu[0]) / d4343;\r
 289 \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
 296 \r
 297         return true;\r
 298    }\r
 299    \r
 300   \r
 301    \r
 302    public static void rotate(Quat4d q, Tuple3d in, Tuple3d out) {\r
 303        // p' = q * p * q'\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
 308        \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
 314    }\r
 315    \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
 326 \r
 327    }\r
 328    \r
 329    \r
 330    private static double q[] = new double[3];\r
 331    private static int nxt[] = { 1, 2, 0 };\r
 332    /**\r
 333     * Converts Matrix to Quaternion\r
 334     * \r
 335     * Note: non-thread safe.\r
 336     * \r
 337     * @param mat\r
 338     * @param quat\r
 339     */\r
 340    public static void getQuat(Matrix3d mat, Quat4d quat) {\r
 341            double tr = mat.m00 + mat.m11 + mat.m22;\r
 342                 if (tr > 0.0) {\r
 343                         double s = Math.sqrt(tr + 1.0);\r
 344                         quat.w = 0.5 * s;\r
 345                         s = 0.5 / s;\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
 349                 } else {\r
 350                         int i = 0, j, k;\r
 351                         if (mat.m11 > mat.m00)\r
 352                                 i = 1;\r
 353                         if (mat.m22 > mat.getElement(i, i))\r
 354                                 i = 2;\r
 355                         \r
 356 \r
 357                         j = nxt[i];\r
 358                         k = nxt[j];\r
 359 \r
 360                         double s = Math.sqrt((mat.getElement(i, i) - (mat.getElement(j, j) + mat.getElement(k, k))) + 1.0);\r
 361 \r
 362                         q[i] = s * 0.5;\r
 363 \r
 364                         if (Math.abs(s) > 0.001)\r
 365                                 s = 0.5 / s;\r
 366 \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
 370 \r
 371                         quat.x = q[0];\r
 372                         quat.y = q[1];\r
 373                         quat.z = q[2];\r
 374                 }\r
 375         }\r
 376    \r
 377    public static Quat4d getQuat(Matrix3d mat) {\r
 378            Quat4d q = new Quat4d();\r
 379            getQuat(mat, q);\r
 380            return q;\r
 381    }\r
 382    \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
 389            aa.x = normal.x;\r
 390            aa.y = normal.y;\r
 391            aa.z = normal.z;\r
 392        } else {\r
 393            aa.x = 1.0;\r
 394            aa.y = 0.0;\r
 395            aa.z = 0.0;\r
 396        }\r
 397        \r
 398        return aa;\r
 399    }\r
 400    \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
 404        return euler;\r
 405    }\r
 406    \r
 407    \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
 410            quat.set(q);\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
 417            \r
 418            // http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm\r
 419            // Y, Z, X order\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
 432 \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
 446    }\r
 447    \r
 448    \r
 449   \r
 450    \r
 451    public static void getEuler(Quat4d quat,Vector3d euler)  {\r
 452            Vector3d e = EulerTools.getEulerFromQuat(Order.YXZ, quat);\r
 453            euler.x = e.y;\r
 454            euler.y = e.x;\r
 455            euler.z = e.z;\r
 456            \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
 461            \r
 462            // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm\r
 463           // Y, Z, X order\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
 468 //                 euler.x = 0.0;\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
 472 //                 euler.x = 0.0;\r
 473 //         } else {\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
 481 //         }\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
 491 //                      euler.x = 0;\r
 492 //                      return;\r
 493 //              }\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
 497 //                      euler.x = 0;\r
 498 //                      return;\r
 499 //              }\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
 503    }\r
 504    \r
 505    public static Quat4d getQuat(Vector3d euler) {\r
 506            Quat4d q = new Quat4d();\r
 507            getQuat(euler,q);\r
 508            return q;\r
 509    }\r
 510    \r
 511    \r
 512    public static Vector3d getEuler(Quat4d quat) {\r
 513            Vector3d v = new Vector3d();\r
 514            getEuler(quat, v);\r
 515            return v;\r
 516    }\r
 517    \r
 518    public static Quat4d getQuat(AxisAngle4d aa) {\r
 519            Quat4d q = new Quat4d();\r
 520            getQuat(aa, q);\r
 521            return q;\r
 522    }\r
 523    \r
 524    public static AxisAngle4d getAxisAngle(Quat4d q) {\r
 525            AxisAngle4d aa = new AxisAngle4d();\r
 526            aa.set(q);\r
 527            return aa;\r
 528    }\r
 529    \r
 530    public static Quat4d getIdentityQuat() {\r
 531            return new Quat4d(0, 0, 0, 1);\r
 532    }\r
 533    \r
 534    public static void getQuat(AxisAngle4d aa, Quat4d q) {\r
 535            double mag,amag;\r
 536                 // Quat = cos(theta/2) + sin(theta/2)(roation_axis) \r
 537                 \r
 538                 amag = Math.sqrt( aa.x*aa.x + aa.y*aa.y + aa.z*aa.z);\r
 539                 if( amag < NEAR_ZERO ) {\r
 540                     q.w = 1.0;\r
 541                     q.x = 0.0;\r
 542                     q.y = 0.0;\r
 543                     q.z = 0.0;\r
 544                 } else {  \r
 545                    amag = 1.0/amag; \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
 552                 }\r
 553    }\r
 554    \r
 555    \r
 556         /*\r
 557          * Cohen-Sutherland\r
 558          */\r
 559         \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
 565         \r
 566         \r
 567         private static int bitcode(Vector2f p1, Vector2f min, Vector2f max) {\r
 568                 int code = IN;\r
 569                 if (p1.x < min.x)\r
 570                         code |= LEFT;\r
 571                 else if (p1.x > max.x)\r
 572                         code |= RIGHT;\r
 573                 if (p1.y < min.y)\r
 574                         code |= BOTTOM;\r
 575                 else if (p1.y > max.y)\r
 576                         code |= TOP;\r
 577                 return code;\r
 578         }\r
 579         \r
 580         public static boolean clipLineRectangle(Vector2f p1,Vector2f p2, Vector2f min, Vector2f max, Vector2f r1, Vector2f r2) {\r
 581                 while (true) {\r
 582                         int o1 = bitcode(p1, min, max);\r
 583                         int o2 = bitcode(p2, min, max);\r
 584                         int and = o1 & o2;\r
 585                         int or = o1 | o2;\r
 586                         if (and != IN) {\r
 587                                 return false;\r
 588                         }\r
 589                         if (or == IN) {\r
 590                                 r1.set(p1);\r
 591                                 r2.set(p2);\r
 592                                 return true;\r
 593                         }\r
 594                         if (o1 == IN) {\r
 595                                 Vector2f t = p1;\r
 596                                 p1 = p2;\r
 597                                 p2 = t;\r
 598                                 int t2 = o1;\r
 599                                 o1 = o2;\r
 600                                 o2 = t2;\r
 601                         }\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
 605                                 p1.y = max.y;\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
 609                                 p1.y = min.y;\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
 613                                 p1.x = min.x;\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
 617                                 p1.x = max.x;\r
 618                         } else {\r
 619                                 throw new RuntimeException("Error in clipping code");\r
 620                         }\r
 621                 }\r
 622                 \r
 623         }\r
 624         \r
 625         public static double square(double d) {\r
 626                 return d * d;\r
 627         }\r
 628         \r
 629         \r
 630          public static void multiplyOrientation(AxisAngle4d aa, AxisAngle4d rot)  {\r
 631                 Quat4d q1 = new Quat4d();\r
 632                 getQuat(aa, q1);\r
 633                 Quat4d q2 = new Quat4d();\r
 634                 getQuat(rot, q2);\r
 635                 q2.mul(q1);\r
 636                 rot.set(q2);\r
 637          }\r
 638          \r
 639          public static double radToDeg(double rad) {\r
 640                  return (rad / Math.PI) * 180.0;\r
 641          }\r
 642          \r
 643          public static double degToRad(double deg) {\r
 644                  return (deg / 180.0) * Math.PI;\r
 645          }\r
 646          \r
 647          public static double clamp(double min, double max,double v) {\r
 648                  if (v < min)\r
 649                          return min;\r
 650                  if (v > max)\r
 651                          return max;\r
 652                  return v;\r
 653          }\r
 654          \r
 655          public static AxisAngle4d createRotation(Vector3d original, Vector3d rotated) { \r
 656                 AxisAngle4d result = new AxisAngle4d();\r
 657                 if (createRotation(original, rotated, result))\r
 658                         return result;\r
 659                 return null;\r
 660          }\r
 661          \r
 662          \r
 663          public static void setIdentity(Quat4d q) {\r
 664                 q.w = 1.0;\r
 665                 q.x = 0.0;\r
 666                 q.y = 0.0;\r
 667                 q.z = 0.0;\r
 668          }\r
 669          \r
 670         public static void setIdentity(AxisAngle4d aa) {\r
 671                 aa.angle = 0.0;\r
 672                 aa.x = 0.0;\r
 673                 aa.y = 1.0;\r
 674                 aa.z = 0.0;\r
 675         }\r
 676         \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
 679                         double m22) {\r
 680                 mat.m00 = m00;\r
 681                 mat.m01 = m01;\r
 682                 mat.m02 = m02;\r
 683 \r
 684                 mat.m10 = m10;\r
 685                 mat.m11 = m11;\r
 686                 mat.m12 = m12;\r
 687 \r
 688                 mat.m20 = m20;\r
 689                 mat.m21 = m21;\r
 690                 mat.m22 = m22;\r
 691         }\r
 692         \r
 693         public static void set(Matrix4d mat, double[] v) {\r
 694                 mat.m00 = v[0];\r
 695                 mat.m01 = v[1];\r
 696                 mat.m02 = v[2];\r
 697                 mat.m03 = v[3];\r
 698 \r
 699                 mat.m10 = v[4];\r
 700                 mat.m11 = v[5];\r
 701                 mat.m12 = v[6];\r
 702                 mat.m13 = v[7];\r
 703 \r
 704                 mat.m20 = v[8];\r
 705                 mat.m21 = v[9];\r
 706                 mat.m22 = v[10];\r
 707                 mat.m23 = v[11];\r
 708 \r
 709                 mat.m30 = v[12];\r
 710                 mat.m31 = v[13];\r
 711                 mat.m32 = v[14];\r
 712                 mat.m33 = v[15];\r
 713 \r
 714         }\r
 715          \r
 716          public static boolean createRotation(Vector3d original, Vector3d rotated, AxisAngle4d result) {\r
 717                  \r
 718                         if (rotated.lengthSquared() > 0.01)\r
 719                                 rotated.normalize();\r
 720                         else\r
 721                                 return false;\r
 722                         double d = original.dot(rotated);\r
 723                         if (d > 0.9999) {\r
 724                                 // original and rotated are parallel, pointing at the same direction\r
 725                                 result.angle = 0.0;\r
 726                                 result.x = 0.0;\r
 727                                 result.y = 1.0;\r
 728                                 result.z = 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
 733                                         a = Y_AXIS;\r
 734                                 result.set(a, Math.PI);\r
 735                         } else {\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
 740                         }\r
 741                         return true;\r
 742                  }\r
 743          \r
 744          public static boolean createRotation(Vector3d original, Vector3d rotated, Quat4d result) {\r
 745                  \r
 746                         if (rotated.lengthSquared() > 0.01)\r
 747                                 rotated.normalize();\r
 748                         else\r
 749                                 return false;\r
 750                         double d = original.dot(rotated);\r
 751                         if (d > 0.9999) {\r
 752                                 // original and rotated are parallel, pointing at the same direction\r
 753                                 result.w = 1.0;\r
 754                                 result.x = 0.0;\r
 755                                 result.y = 0.0;\r
 756                                 result.z = 0.0;\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
 761                                         a = Y_AXIS;\r
 762                                 getQuat(a, Math.PI, result);\r
 763                                 \r
 764                         } else {\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
 769                         }\r
 770                         return true;\r
 771                  }\r
 772          \r
 773          public static void getQuat(Vector3d axis, double angle, Quat4d q)\r
 774     {\r
 775                 double mag,amag;\r
 776                 // Quat = cos(theta/2) + sin(theta/2)(roation_axis) \r
 777                 \r
 778                 amag = Math.sqrt( axis.x*axis.x + axis.y*axis.y + axis.z*axis.z);\r
 779                 if( amag < EPS ) {\r
 780                     q.w = 1.0;\r
 781                     q.x = 0.0;\r
 782                     q.y = 0.0;\r
 783                     q.z = 0.0;\r
 784                 } else {  \r
 785                     amag = 1.0/amag; \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
 792                 }\r
 793         \r
 794     }\r
 795          \r
 796          /**\r
 797           * Linear interpolation of quaternions. Result IS set to q1.\r
 798           * @param q1\r
 799           * @param q2\r
 800           * @param alpha\r
 801           */\r
 802          public static void lip(Quat4d q1, Quat4d q2, double alpha) {\r
 803                  double s1 = 1.0 - alpha;\r
 804                  double s2 = alpha;\r
 805                  q1.scale(s1);\r
 806                  mad(q1,q2,s2);\r
 807                  q1.normalize();\r
 808          }\r
 809          \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
 812          }\r
 813          \r
 814          public static void mad(Tuple3d q1, Tuple3d q2, double s2) {\r
 815                  q1.x += q2.x * s2;\r
 816                  q1.y += q2.y * s2;\r
 817                  q1.z += q2.z * s2;\r
 818          }\r
 819          \r
 820          public static void mad(Quat4d q1, Quat4d q2, double s2) {\r
 821                  q1.x += q2.x * s2;\r
 822                  q1.y += q2.y * s2;\r
 823                  q1.z += q2.z * s2;\r
 824                  q1.w += q2.w * s2;\r
 825          }\r
 826          \r
 827          /**\r
 828           * Slerp\r
 829           * \r
 830           * Sets results to q1. Modifies q2.\r
 831           * \r
 832           * @param q1\r
 833           * @param q2\r
 834           * @param alpha\r
 835           */\r
 836          public static void sip(Quat4d q1, Quat4d q2, double alpha) {\r
 837                  double cosom = dot(q1,q2);\r
 838                  if (cosom < 0.0) {\r
 839                          cosom = -cosom;\r
 840                          q2.negate();\r
 841                  }\r
 842                  \r
 843                  if (cosom > 0.9999) {\r
 844                          q2.sub(q1);\r
 845                          q2.scale(alpha);\r
 846                          q1.add(q2);\r
 847                          q1.normalize();\r
 848                          return;\r
 849                  }\r
 850                  double theta_0 = Math.acos(cosom);\r
 851                  double theta = theta_0 * alpha;\r
 852                  Quat4d t = new Quat4d(q1);\r
 853                  t.scale(-cosom);\r
 854                  t.add(q2);\r
 855                  t.normalize();\r
 856                  t.scale(Math.sin(theta));\r
 857                  q1.scale(Math.cos(theta));\r
 858                  q1.add(t);\r
 859          }\r
 860 }\r