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

   1 package org.simantics.g3d.math;\r
   2 \r
   3 import javax.vecmath.AxisAngle4d;\r
   4 import javax.vecmath.Matrix3d;\r
   5 import javax.vecmath.Matrix4d;\r
   6 import javax.vecmath.Quat4d;\r
   7 import javax.vecmath.Tuple3d;\r
   8 import javax.vecmath.Tuple4d;\r
   9 import javax.vecmath.Vector2f;\r
  10 import javax.vecmath.Vector3d;\r
  11 \r
  12 import org.simantics.g3d.math.EulerTools.Order;\r
  13 \r
  14 \r
  15 /**\r
  16  * Some useful geometry related math functions. Beware, methods may modify their input parameters!\r
  17  * \r
  18  * @author Marko Luukkainen\r
  19  *\r
  20  */\r
  21 public class MathTools {\r
  22     \r
  23     public static double NEAR_ZERO = 0.0000001;\r
  24     public static double NEAR_HALF = 0.4999999;\r
  25     \r
  26     public static final Vector3d Z_AXIS = new Vector3d(0.0,0.0,1.0);\r
  27         public static final Vector3d Y_AXIS = new Vector3d(0.0,1.0,0.0);\r
  28         public static final Vector3d X_AXIS = new Vector3d(1.0,0.0,0.0);\r
  29         public static final Vector3d ORIGIN = new Vector3d(0.0,0.0,0.0);\r
  30         \r
  31         final static double EPS = 1.0e-12;\r
  32         \r
  33         \r
  34         public static boolean equals(double d1, double d2) {\r
  35                 return Math.abs(d1-d2) < EPS;\r
  36         }\r
  37         \r
  38         public static boolean equals(Tuple3d p1, Tuple3d p2) {\r
  39                 return distanceSquared(p1, p2) < NEAR_ZERO;\r
  40         }\r
  41         \r
  42         public static boolean equals(Tuple4d p1, Tuple4d p2) {\r
  43                 return distanceSquared(p1, p2) < NEAR_ZERO;\r
  44         }\r
  45         \r
  46         public static double distance(Tuple3d p1, Tuple3d p2) {\r
  47                 double dx, dy, dz;\r
  48 \r
  49                 dx = p2.x - p1.x;\r
  50                 dy = p2.y - p1.y;\r
  51                 dz = p2.z - p1.z;\r
  52                 return Math.sqrt(dx*dx+dy*dy+dz*dz);\r
  53         }\r
  54         \r
  55         public static double distance(Tuple4d p1, Tuple4d p2) {\r
  56                 double dx, dy, dz, dw;\r
  57 \r
  58                 dx = p2.x - p1.x;\r
  59                 dy = p2.y - p1.y;\r
  60                 dz = p2.z - p1.z;\r
  61                 dw = p2.w - p1.w;\r
  62                 return Math.sqrt(dx*dx+dy*dy+dz*dz+dw*dw);\r
  63         }\r
  64         \r
  65         public static double distanceSquared(Tuple3d p1, Tuple3d p2) {\r
  66                 double dx, dy, dz;\r
  67 \r
  68                 dx = p2.x - p1.x;\r
  69                 dy = p2.y - p1.y;\r
  70                 dz = p2.z - p1.z;\r
  71                 return dx*dx+dy*dy+dz*dz;\r
  72         }\r
  73         \r
  74         public static double distanceSquared(Tuple4d p1, Tuple4d p2) {\r
  75                 double dx, dy, dz, dw;\r
  76 \r
  77                 dx = p2.x - p1.x;\r
  78                 dy = p2.y - p1.y;\r
  79                 dz = p2.z - p1.z;\r
  80                 dw = p2.w - p1.w;\r
  81                 return dx*dx+dy*dy+dz*dz+dw*dw;\r
  82         }\r
  83         \r
  84         public static boolean isValid(Tuple3d t) {\r
  85                 return !(Double.isInfinite(t.x) || Double.isNaN(t.x) ||\r
  86                                  Double.isInfinite(t.y) || Double.isNaN(t.y) ||\r
  87                                  Double.isInfinite(t.z) || Double.isNaN(t.z));\r
  88         }\r
  89     \r
  90     public static Vector3d closestPointOnEdge(Vector3d point, Vector3d edgePoint1, Vector3d edgePoint2) {\r
  91         point.sub(edgePoint1);\r
  92         Vector3d v = new Vector3d(edgePoint2);\r
  93         v.sub(edgePoint1);\r
  94         double t = v.dot(point);\r
  95         t /= v.lengthSquared();\r
  96         if (t <= 0.0f)\r
  97           return edgePoint1;\r
  98         if (t >= 1.0f)\r
  99           return edgePoint2;\r
 100         v.scale(t);\r
 101         v.add(edgePoint1);\r
 102         return v;   \r
 103     }\r
 104     \r
 105     public static Vector3d closestPointOnStraight(Tuple3d point, Tuple3d straightPoint, Vector3d straightDir) {\r
 106         Vector3d v = new Vector3d(point);\r
 107         v.sub(straightPoint);\r
 108         double t = straightDir.dot(v);\r
 109         t /= straightDir.lengthSquared();\r
 110         v.set(straightDir);\r
 111         v.scale(t);\r
 112         v.add(straightPoint);\r
 113         return v;   \r
 114     }\r
 115     \r
 116     public static Vector3d closestPointOnStraight(Tuple3d point, Tuple3d straightPoint, Vector3d straightDir, double u[]) {\r
 117         Vector3d v = new Vector3d(point);\r
 118         v.sub(straightPoint);\r
 119         u[0] = straightDir.dot(v);\r
 120         u[0] /= straightDir.lengthSquared();\r
 121         v.set(straightDir);\r
 122         v.scale(u[0]);\r
 123         v.add(straightPoint);\r
 124         return v;   \r
 125     }\r
 126     \r
 127     public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, Tuple3d planePoint) {\r
 128         point.sub(planePoint);\r
 129         \r
 130         return planeNormal.dot(point);\r
 131     }\r
 132       \r
 133     public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, float d) {\r
 134         return (planeNormal.dot(point) + d);\r
 135     }\r
 136     \r
 137     public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Tuple3d intersectPoint) {\r
 138         intersectPoint.set(planePoint);\r
 139         intersectPoint.sub(linePoint);\r
 140         double u = planeNormal.dot(new Vector3d(intersectPoint));\r
 141         double v = planeNormal.dot(lineDir);\r
 142         if (Math.abs(v) < NEAR_ZERO)\r
 143             return false;\r
 144         u /= v;\r
 145         intersectPoint.set(lineDir);\r
 146         intersectPoint.scale(u);\r
 147         intersectPoint.add(linePoint);\r
 148         return true;\r
 149     }\r
 150     \r
 151     public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Vector3d intersectPoint, double[] u) {\r
 152         intersectPoint.set(planePoint);\r
 153         intersectPoint.sub(linePoint);\r
 154         u[0] = planeNormal.dot(intersectPoint);\r
 155         double v = planeNormal.dot(lineDir);\r
 156         if (Math.abs(v) < NEAR_ZERO)\r
 157             return false;\r
 158         u[0] /= v;\r
 159         intersectPoint.set(lineDir);\r
 160         intersectPoint.scale(u[0]);\r
 161         intersectPoint.add(linePoint);\r
 162         return true;\r
 163     }\r
 164     \r
 165     public static boolean intersectLineLine(Tuple3d l1_start,Tuple3d l1_end,Tuple3d l2_start,Tuple3d l2_end,Tuple3d l1_pos, Tuple3d l2_pos) {\r
 166             Vector3d p13 = new Vector3d();\r
 167             Vector3d p43 = new Vector3d();\r
 168             Vector3d p21 = new Vector3d();\r
 169             double d1343,d4321,d1321,d4343,d2121;\r
 170             double numer,denom;\r
 171             p13.sub(l1_start, l2_start);\r
 172             p43.sub(l2_end,l2_start);\r
 173             if (Math.abs(p43.x)  < NEAR_ZERO && Math.abs(p43.y)  < NEAR_ZERO && Math.abs(p43.z)  < NEAR_ZERO)\r
 174                return false;\r
 175             p21.sub(l1_end,l1_start);\r
 176             if (Math.abs(p21.x)  < NEAR_ZERO && Math.abs(p21.y)  < NEAR_ZERO && Math.abs(p21.z)  < NEAR_ZERO)\r
 177                return false;\r
 178 \r
 179             d1343 = p13.dot(p43);\r
 180             d4321 = p43.dot(p21);\r
 181             d1321 = p13.dot(p21);\r
 182             d4343 = p43.lengthSquared();\r
 183             d2121 = p21.lengthSquared();\r
 184 \r
 185             denom = d2121 * d4343 - d4321 * d4321;\r
 186             if (Math.abs(denom) < NEAR_ZERO)\r
 187                return false;\r
 188             numer = d1343 * d4321 - d1321 * d4343;\r
 189 \r
 190             double mua = numer / denom;\r
 191             double mub = (d1343 + d4321 * mua) / d4343;\r
 192  \r
 193             l1_pos.x = l1_start.x + mua * p21.x;\r
 194             l1_pos.y = l1_start.y + mua * p21.y;\r
 195             l1_pos.z = l1_start.z + mua * p21.z;\r
 196             l2_pos.x = l2_start.x + mub * p43.x;\r
 197             l2_pos.y = l2_start.y + mub * p43.y;\r
 198             l2_pos.z = l2_start.z + mub * p43.z;\r
 199 \r
 200             return true;\r
 201     }\r
 202     \r
 203     public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb) {\r
 204         Vector3d p13 = new Vector3d();\r
 205 \r
 206         double d1343,d4321,d1321,d4343,d2121;\r
 207         double numer,denom;\r
 208         \r
 209         p13.sub(p1, p3);\r
 210         if (Math.abs(p43.x)  < NEAR_ZERO && Math.abs(p43.y)  < NEAR_ZERO && Math.abs(p43.z)  < NEAR_ZERO)\r
 211            return false;\r
 212         if (Math.abs(p21.x)  < NEAR_ZERO && Math.abs(p21.y)  < NEAR_ZERO && Math.abs(p21.z)  < NEAR_ZERO)\r
 213            return false;\r
 214 \r
 215         d1343 = p13.dot(p43);\r
 216         d4321 = p43.dot(p21);\r
 217         d1321 = p13.dot(p21);\r
 218         d4343 = p43.lengthSquared();\r
 219         d2121 = p21.lengthSquared();\r
 220 \r
 221         denom = d2121 * d4343 - d4321 * d4321;\r
 222         if (Math.abs(denom) < NEAR_ZERO)\r
 223            return false;\r
 224         numer = d1343 * d4321 - d1321 * d4343;\r
 225 \r
 226         double mua = numer / denom;\r
 227         double mub = (d1343 + d4321 * mua) / d4343;\r
 228 \r
 229         pa.x = p1.x + mua * p21.x;\r
 230         pa.y = p1.y + mua * p21.y;\r
 231         pa.z = p1.z + mua * p21.z;\r
 232         pb.x = p3.x + mub * p43.x;\r
 233         pb.y = p3.y + mub * p43.y;\r
 234         pb.z = p3.z + mub * p43.z;\r
 235 \r
 236         return true;\r
 237    }\r
 238     \r
 239     /**\r
 240      * Calculate the line segment PaPb that is the shortest route between\r
 241      *  two lines P1P2 and P3P4. Calculate also the values of mua and mub where\r
 242      *  Pa = P1 + mua (P2 - P1)\r
 243      *  Pb = P3 + mub (P4 - P3)\r
 244      * @param p1\r
 245      * @param p21\r
 246      * @param p3\r
 247      * @param p43\r
 248      * @param pa\r
 249      * @param pb\r
 250      * @param mu\r
 251      * @return\r
 252      */\r
 253     public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb, double mu[]) {\r
 254         Vector3d p13 = new Vector3d();\r
 255 \r
 256         double d1343,d4321,d1321,d4343,d2121;\r
 257         double numer,denom;\r
 258         double EPS = 0.001;\r
 259         p13.sub(p1, p3);\r
 260         if (Math.abs(p43.x)  < EPS && Math.abs(p43.y)  < EPS && Math.abs(p43.z)  < EPS)\r
 261            return false;\r
 262         if (Math.abs(p21.x)  < EPS && Math.abs(p21.y)  < EPS && Math.abs(p21.z)  < EPS)\r
 263            return false;\r
 264 \r
 265         d1343 = p13.dot(p43);\r
 266         d4321 = p43.dot(p21);\r
 267         d1321 = p13.dot(p21);\r
 268         d4343 = p43.lengthSquared();\r
 269         d2121 = p21.lengthSquared();\r
 270 \r
 271         denom = d2121 * d4343 - d4321 * d4321;\r
 272         if (Math.abs(denom) < EPS)\r
 273            return false;\r
 274         numer = d1343 * d4321 - d1321 * d4343;\r
 275 \r
 276         mu[0] = numer / denom;\r
 277         mu[1] = (d1343 + d4321 * mu[0]) / d4343;\r
 278 \r
 279         pa.x = p1.x + mu[0] * p21.x;\r
 280         pa.y = p1.y + mu[0] * p21.y;\r
 281         pa.z = p1.z + mu[0] * p21.z;\r
 282         pb.x = p3.x + mu[1] * p43.x;\r
 283         pb.y = p3.y + mu[1] * p43.y;\r
 284         pb.z = p3.z + mu[1] * p43.z;\r
 285 \r
 286         return true;\r
 287    }\r
 288    \r
 289   \r
 290    \r
 291    public static void rotate(Quat4d q, Tuple3d in, Tuple3d out) {\r
 292        // p' = q * p * q'\r
 293        double tw =           - q.x*in.x - q.y*in.y - q.z*in.z;\r
 294        double tx =  q.w*in.x            + q.y*in.z - q.z*in.y;\r
 295        double ty =  q.w*in.y - q.x*in.z            + q.z*in.x;\r
 296        double tz =  q.w*in.z + q.x*in.y - q.y*in.x           ;\r
 297        \r
 298        //temp * q' -> x = -x, y = -y z = -z\r
 299        //out.w = tw*q.w + tx*q.x + ty*q.y + tz*q.z;\r
 300        out.x =  -tw*q.x + tx*q.w - ty*q.z + tz*q.y;\r
 301        out.y =  -tw*q.y + tx*q.z + ty*q.w - tz*q.x;\r
 302        out.z =  -tw*q.z - tx*q.y + ty*q.x + tz*q.w;  \r
 303    }\r
 304    \r
 305    public static void getMatrix(Quat4d quat, Matrix3d m) {\r
 306                    m.m00 = 1.0f - 2.0 * (quat.y * quat.y + quat.z * quat.z);\r
 307                    m.m01 = 2.0 * (quat.x * quat.y + quat.w * quat.z);\r
 308                    m.m02 = 2.0 * (quat.x * quat.z - quat.w * quat.y);\r
 309                    m.m10 = 2.0 * (quat.x * quat.y - quat.w * quat.z);\r
 310                    m.m11 = 1.0 - 2.0f * (quat.x * quat.x + quat.z * quat.z);\r
 311                    m.m12 = 2.0 * (quat.y * quat.z + quat.w * quat.x);\r
 312                    m.m20 = 2.0 * (quat.x * quat.z + quat.w * quat.y);\r
 313                    m.m21 = 2.0 * (quat.y * quat.z - quat.w * quat.x);\r
 314                    m.m22 = 1.0 - 2.0f * (quat.x * quat.x + quat.y * quat.y);\r
 315 \r
 316    }\r
 317    \r
 318    \r
 319    private static double q[] = new double[3];\r
 320    private static int nxt[] = { 1, 2, 0 };\r
 321    /**\r
 322     * Converts Matrix to Quaternion\r
 323     * \r
 324     * Note: non-thread safe.\r
 325     * \r
 326     * @param mat\r
 327     * @param quat\r
 328     */\r
 329    public static void getQuat(Matrix3d mat, Quat4d quat) {\r
 330            double tr = mat.m00 + mat.m11 + mat.m22;\r
 331                 if (tr > 0.0) {\r
 332                         double s = Math.sqrt(tr + 1.0);\r
 333                         quat.w = 0.5 * s;\r
 334                         s = 0.5 / s;\r
 335                         quat.x = (mat.m21 - mat.m12) * s;\r
 336                         quat.y = (mat.m02 - mat.m20) * s;\r
 337                         quat.z = (mat.m10 - mat.m01) * s;\r
 338                 } else {\r
 339                         int i = 0, j, k;\r
 340                         if (mat.m11 > mat.m00)\r
 341                                 i = 1;\r
 342                         if (mat.m22 > mat.getElement(i, i))\r
 343                                 i = 2;\r
 344                         \r
 345 \r
 346                         j = nxt[i];\r
 347                         k = nxt[j];\r
 348 \r
 349                         double s = Math.sqrt((mat.getElement(i, i) - (mat.getElement(j, j) + mat.getElement(k, k))) + 1.0);\r
 350 \r
 351                         q[i] = s * 0.5;\r
 352 \r
 353                         if (Math.abs(s) > 0.001)\r
 354                                 s = 0.5 / s;\r
 355 \r
 356                         quat.w = (mat.getElement(k, j) - mat.getElement(j, k)) * s;\r
 357                         q[j] = (mat.getElement(j, i) + mat.getElement(i, j)) * s;\r
 358                         q[k] = (mat.getElement(k, i) + mat.getElement(i, k)) * s;\r
 359 \r
 360                         quat.x = q[0];\r
 361                         quat.y = q[1];\r
 362                         quat.z = q[2];\r
 363                 }\r
 364         }\r
 365    \r
 366    public static Quat4d getQuat(Matrix3d mat) {\r
 367            Quat4d q = new Quat4d();\r
 368            getQuat(mat, q);\r
 369            return q;\r
 370    }\r
 371    \r
 372    public static AxisAngle4d getFromPseudoEuler(Vector3d euler) {\r
 373        AxisAngle4d aa = new AxisAngle4d();\r
 374        aa.angle = euler.length();\r
 375        Vector3d normal = new Vector3d(euler);\r
 376        if (aa.angle > NEAR_ZERO) {\r
 377            normal.normalize();\r
 378            aa.x = normal.x;\r
 379            aa.y = normal.y;\r
 380            aa.z = normal.z;\r
 381        } else {\r
 382            aa.x = 1.0;\r
 383            aa.y = 0.0;\r
 384            aa.z = 0.0;\r
 385        }\r
 386        \r
 387        return aa;\r
 388    }\r
 389    \r
 390    public static Vector3d getPseudoEuler(AxisAngle4d aa) {\r
 391        Vector3d euler = new Vector3d(aa.x,aa.y,aa.z);\r
 392        euler.scale(aa.angle);\r
 393        return euler;\r
 394    }\r
 395    \r
 396    \r
 397    public static void getQuat(Vector3d euler, Quat4d quat)  {\r
 398            Quat4d q = EulerTools.getQuatFromEuler(Order.YXZ, euler.y,euler.x,euler.z);\r
 399            quat.set(q);\r
 400         // http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Conversion_formulae_between_formalisms\r
 401         // Using the x-convention, the 3-1-3 Euler angles phi, theta and psi (around the Z, X and again the Z-axis)   \r
 402 //         quat.x = -Math.cos((euler.x - euler.z)*0.5)*Math.sin(euler.y*0.5);\r
 403 //         quat.y = -Math.sin((euler.x - euler.z)*0.5)*Math.sin(euler.y*0.5);\r
 404 //         quat.z = -Math.sin((euler.x + euler.z)*0.5)*Math.cos(euler.y*0.5);\r
 405 //         quat.w = Math.sin((euler.x + euler.z)*0.5)*Math.cos(euler.y*0.5);\r
 406            \r
 407            // http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm\r
 408            // Y, Z, X order\r
 409 //          double c1 = Math.cos(euler.y*0.5);\r
 410 //          double s1 = Math.sin(euler.y*0.5);\r
 411 //          double c2 = Math.cos(euler.z*0.5);\r
 412 //          double s2 = Math.sin(euler.z*0.5);\r
 413 //          double c3 = Math.cos(euler.x*0.5);\r
 414 //          double s3 = Math.sin(euler.x*0.5);\r
 415 //          double c1c2 = c1*c2;\r
 416 //          double s1s2 = s1*s2;\r
 417 //          quat.w =c1c2*c3 - s1s2*s3;\r
 418 //          quat.x =c1c2*s3 + s1s2*c3;\r
 419 //          quat.y =s1*c2*c3 + c1*s2*s3;\r
 420 //          quat.z =c1*s2*c3 - s1*c2*s3;\r
 421 \r
 422 //          Quat4d q2 = EulerTools.getQuatFromEuler(Order.YZX, euler.y,euler.z,euler.x);\r
 423 //          System.out.println("Q " + quat + " Q2 " + q2);\r
 424 //         double c1 = Math.cos(euler.y);\r
 425 //          double s1 = Math.sin(euler.y);\r
 426 //          double c2 = Math.cos(euler.z);\r
 427 //          double s2 = Math.sin(euler.z);\r
 428 //          double c3 = Math.cos(euler.x);\r
 429 //          double s3 = Math.sin(euler.x);\r
 430 //          quat.w = Math.sqrt(1.0 + c1 * c2 + c1*c3 - s1 * s2 * s3 + c2*c3) / 2.0;\r
 431 //          double w4 = (4.0 * quat.w);\r
 432 //          quat.x = (c2 * s3 + c1 * s3 + s1 * s2 * c3) / w4 ;\r
 433 //          quat.y = (s1 * c2 + s1 * c3 + c1 * s2 * s3) / w4 ;\r
 434 //          quat.z = (-s1 * s3 + c1 * s2 * c3 +s2) / w4 ;\r
 435    }\r
 436    \r
 437    \r
 438   \r
 439    \r
 440    public static void getEuler(Quat4d quat,Vector3d euler)  {\r
 441            Vector3d e = EulerTools.getEulerFromQuat(Order.YXZ, quat);\r
 442            euler.x = e.y;\r
 443            euler.y = e.x;\r
 444            euler.z = e.z;\r
 445            \r
 446            // http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Conversion_formulae_between_formalisms\r
 447 //         euler.x = Math.atan2(quat.x * quat.z + quat.y* quat.w, quat.y*quat.z - quat.x * quat.w);\r
 448 //         euler.y = Math.acos(-square(quat.x) - square(quat.y) + square(quat.z) + square(quat.w));\r
 449 //         euler.z = -Math.atan2(quat.x * quat.z - quat.y* quat.w, quat.y*quat.z + quat.x * quat.w);\r
 450            \r
 451            // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm\r
 452           // Y, Z, X order\r
 453 //         double test = quat.x * quat.y + quat.z * quat.w;\r
 454 //         if (test > NEAR_HALF) {\r
 455 //                 euler.y = 2.0 * Math.atan2(quat.x,quat.w);\r
 456 //                 euler.z = Math.PI * 0.5;\r
 457 //                 euler.x = 0.0;\r
 458 //         } else if (test < -NEAR_HALF) {\r
 459 //                 euler.y = -2.0 * Math.atan2(quat.x,quat.w);\r
 460 //                 euler.z = -Math.PI * 0.5;\r
 461 //                 euler.x = 0.0;\r
 462 //         } else {\r
 463 //                 double sqx = square(quat.x);\r
 464 //                 double sqy = square(quat.y);\r
 465 //                 double sqz = square(quat.z);\r
 466 //                 euler.y = Math.atan2(2.0*(quat.y*quat.w-quat.x*quat.z), 1.0 - 2.0*(sqy-sqz));\r
 467 //                 euler.z = Math.asin(2.0*test);\r
 468 //                 euler.x = Math.atan2(2.0*(quat.x*quat.w-quat.y*quat.z), 1.0 - 2.0*(sqx-sqz));\r
 469 //                 System.out.println(euler + " " + EulerTools.getEulerFromQuat(Order.YXZ, quat) +  " " + quat);\r
 470 //         }\r
 471 //         double sqw = quat.w*quat.w;\r
 472 //          double sqx = quat.x*quat.x;\r
 473 //          double sqy = quat.y*quat.y;\r
 474 //          double sqz = quat.z*quat.z;\r
 475 //              double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor\r
 476 //              double test = quat.x*quat.y + quat.z*quat.w;\r
 477 //              if (test > 0.499*unit) { // singularity at north pole\r
 478 //                      euler.y = 2 * Math.atan2(quat.x,quat.w);\r
 479 //                      euler.z = Math.PI/2;\r
 480 //                      euler.x = 0;\r
 481 //                      return;\r
 482 //              }\r
 483 //              if (test < -0.499*unit) { // singularity at south pole\r
 484 //                      euler.y = -2 * Math.atan2(quat.x,quat.w);\r
 485 //                      euler.z = -Math.PI/2;\r
 486 //                      euler.x = 0;\r
 487 //                      return;\r
 488 //              }\r
 489 //              euler.y = Math.atan2(2*quat.y*quat.w-2*quat.x*quat.z , sqx - sqy - sqz + sqw);\r
 490 //              euler.z = Math.asin(2*test/unit);\r
 491 //              euler.x = Math.atan2(2*quat.x*quat.w-2*quat.y*quat.z , -sqx + sqy - sqz + sqw);\r
 492    }\r
 493    \r
 494    public static Quat4d getQuat(Vector3d euler) {\r
 495            Quat4d q = new Quat4d();\r
 496            getQuat(euler,q);\r
 497            return q;\r
 498    }\r
 499    \r
 500    \r
 501    public static Vector3d getEuler(Quat4d quat) {\r
 502            Vector3d v = new Vector3d();\r
 503            getEuler(quat, v);\r
 504            return v;\r
 505    }\r
 506    \r
 507    public static Quat4d getQuat(AxisAngle4d aa) {\r
 508            Quat4d q = new Quat4d();\r
 509            getQuat(aa, q);\r
 510            return q;\r
 511    }\r
 512    \r
 513    public static AxisAngle4d getAxisAngle(Quat4d q) {\r
 514            AxisAngle4d aa = new AxisAngle4d();\r
 515            aa.set(q);\r
 516            return aa;\r
 517    }\r
 518    \r
 519    public static Quat4d getIdentityQuat() {\r
 520            return new Quat4d(0, 0, 0, 1);\r
 521    }\r
 522    \r
 523    public static void getQuat(AxisAngle4d aa, Quat4d q) {\r
 524            double mag,amag;\r
 525                 // Quat = cos(theta/2) + sin(theta/2)(roation_axis) \r
 526                 \r
 527                 amag = Math.sqrt( aa.x*aa.x + aa.y*aa.y + aa.z*aa.z);\r
 528                 if( amag < NEAR_ZERO ) {\r
 529                     q.w = 1.0;\r
 530                     q.x = 0.0;\r
 531                     q.y = 0.0;\r
 532                     q.z = 0.0;\r
 533                 } else {  \r
 534                    amag = 1.0/amag; \r
 535                    double a2 = aa.angle * 0.5;\r
 536                    mag = Math.sin(a2);\r
 537                    q.w = Math.cos(a2);\r
 538                q.x = aa.x*amag*mag;\r
 539                q.y = aa.y*amag*mag;\r
 540                q.z = aa.z*amag*mag;\r
 541                 }\r
 542    }\r
 543    \r
 544    \r
 545         /*\r
 546          * Cohen-Sutherland\r
 547          */\r
 548         \r
 549         private static final int IN = 0;\r
 550         private static final int LEFT = 1;\r
 551         private static final int RIGHT = 2;\r
 552         private static final int BOTTOM = 4;\r
 553         private static final int TOP = 8;\r
 554         \r
 555         \r
 556         private static int bitcode(Vector2f p1, Vector2f min, Vector2f max) {\r
 557                 int code = IN;\r
 558                 if (p1.x < min.x)\r
 559                         code |= LEFT;\r
 560                 else if (p1.x > max.x)\r
 561                         code |= RIGHT;\r
 562                 if (p1.y < min.y)\r
 563                         code |= BOTTOM;\r
 564                 else if (p1.y > max.y)\r
 565                         code |= TOP;\r
 566                 return code;\r
 567         }\r
 568         \r
 569         public static boolean clipLineRectangle(Vector2f p1,Vector2f p2, Vector2f min, Vector2f max, Vector2f r1, Vector2f r2) {\r
 570                 while (true) {\r
 571                         int o1 = bitcode(p1, min, max);\r
 572                         int o2 = bitcode(p2, min, max);\r
 573                         int and = o1 & o2;\r
 574                         int or = o1 | o2;\r
 575                         if (and != IN) {\r
 576                                 return false;\r
 577                         }\r
 578                         if (or == IN) {\r
 579                                 r1.set(p1);\r
 580                                 r2.set(p2);\r
 581                                 return true;\r
 582                         }\r
 583                         if (o1 == IN) {\r
 584                                 Vector2f t = p1;\r
 585                                 p1 = p2;\r
 586                                 p2 = t;\r
 587                                 int t2 = o1;\r
 588                                 o1 = o2;\r
 589                                 o2 = t2;\r
 590                         }\r
 591                         if ((o1 & TOP) != IN) {\r
 592                                 float t = (max.y - p1.y) / (p2.y - p1.y);\r
 593                                 p1.x += t * (p2.x - p1.x);\r
 594                                 p1.y = max.y;\r
 595                         } else if ((o1 & BOTTOM) != IN) {\r
 596                                 float t = (min.y - p1.y) / (p2.y - p1.y);\r
 597                                 p1.x += t * (p2.x - p1.x);\r
 598                                 p1.y = min.y;\r
 599                         } else if ((o1 & LEFT) != IN) {\r
 600                                 float t = (min.x - p1.x) / (p2.x - p1.x);\r
 601                                 p1.y += t * (p2.y - p1.y);\r
 602                                 p1.x = min.x;\r
 603                         } else if ((o1 & RIGHT) != IN) {\r
 604                                 float t = (max.x - p1.x) / (p2.x - p1.x);\r
 605                                 p1.y += t * (p2.y - p1.y);\r
 606                                 p1.x = max.x;\r
 607                         } else {\r
 608                                 throw new RuntimeException("Error in clipping code");\r
 609                         }\r
 610                 }\r
 611                 \r
 612         }\r
 613         \r
 614         public static double square(double d) {\r
 615                 return d * d;\r
 616         }\r
 617         \r
 618         \r
 619          public static void multiplyOrientation(AxisAngle4d aa, AxisAngle4d rot)  {\r
 620                 Quat4d q1 = new Quat4d();\r
 621                 getQuat(aa, q1);\r
 622                 Quat4d q2 = new Quat4d();\r
 623                 getQuat(rot, q2);\r
 624                 q2.mul(q1);\r
 625                 rot.set(q2);\r
 626          }\r
 627          \r
 628          public static double radToDeg(double rad) {\r
 629                  return (rad / Math.PI) * 180.0;\r
 630          }\r
 631          \r
 632          public static double degToRad(double deg) {\r
 633                  return (deg / 180.0) * Math.PI;\r
 634          }\r
 635          \r
 636          public static double clamp(double min, double max,double v) {\r
 637                  if (v < min)\r
 638                          return min;\r
 639                  if (v > max)\r
 640                          return max;\r
 641                  return v;\r
 642          }\r
 643          \r
 644          public static AxisAngle4d createRotation(Vector3d original, Vector3d rotated) { \r
 645                 AxisAngle4d result = new AxisAngle4d();\r
 646                 if (createRotation(original, rotated, result))\r
 647                         return result;\r
 648                 return null;\r
 649          }\r
 650          \r
 651          \r
 652          public static void setIdentity(Quat4d q) {\r
 653                 q.w = 1.0;\r
 654                 q.x = 0.0;\r
 655                 q.y = 0.0;\r
 656                 q.z = 0.0;\r
 657          }\r
 658          \r
 659         public static void setIdentity(AxisAngle4d aa) {\r
 660                 aa.angle = 0.0;\r
 661                 aa.x = 0.0;\r
 662                 aa.y = 1.0;\r
 663                 aa.z = 0.0;\r
 664         }\r
 665         \r
 666         public static void set(Matrix3d mat, double m00, double m01, double m02,\r
 667                         double m10, double m11, double m12, double m20, double m21,\r
 668                         double m22) {\r
 669                 mat.m00 = m00;\r
 670                 mat.m01 = m01;\r
 671                 mat.m02 = m02;\r
 672 \r
 673                 mat.m10 = m10;\r
 674                 mat.m11 = m11;\r
 675                 mat.m12 = m12;\r
 676 \r
 677                 mat.m20 = m20;\r
 678                 mat.m21 = m21;\r
 679                 mat.m22 = m22;\r
 680         }\r
 681         \r
 682         public static void set(Matrix4d mat, double[] v) {\r
 683                 mat.m00 = v[0];\r
 684                 mat.m01 = v[1];\r
 685                 mat.m02 = v[2];\r
 686                 mat.m03 = v[3];\r
 687 \r
 688                 mat.m10 = v[4];\r
 689                 mat.m11 = v[5];\r
 690                 mat.m12 = v[6];\r
 691                 mat.m13 = v[7];\r
 692 \r
 693                 mat.m20 = v[8];\r
 694                 mat.m21 = v[9];\r
 695                 mat.m22 = v[10];\r
 696                 mat.m23 = v[11];\r
 697 \r
 698                 mat.m30 = v[12];\r
 699                 mat.m31 = v[13];\r
 700                 mat.m32 = v[14];\r
 701                 mat.m33 = v[15];\r
 702 \r
 703         }\r
 704          \r
 705          public static boolean createRotation(Vector3d original, Vector3d rotated, AxisAngle4d result) {\r
 706                  \r
 707                         if (rotated.lengthSquared() > 0.01)\r
 708                                 rotated.normalize();\r
 709                         else\r
 710                                 return false;\r
 711                         double d = original.dot(rotated);\r
 712                         if (d > 0.9999) {\r
 713                                 // original and rotated are parallel, pointing at the same direction\r
 714                                 result.angle = 0.0;\r
 715                                 result.x = 0.0;\r
 716                                 result.y = 1.0;\r
 717                                 result.z = 0.0;\r
 718                         } else if (d < -0.9999) {\r
 719                                 // original and rotated are parallel, pointing at the opposite direction \r
 720                                 Vector3d a = Z_AXIS;\r
 721                                 if (Math.abs(a.dot(original)) > 0.8 )\r
 722                                         a = Y_AXIS;\r
 723                                 result.set(a, Math.PI);\r
 724                         } else {\r
 725                                 double angle = original.angle(rotated);\r
 726                                 Vector3d axis = new Vector3d();\r
 727                                 axis.cross(original, rotated);\r
 728                                 result.set(axis,angle);\r
 729                         }\r
 730                         return true;\r
 731                  }\r
 732          \r
 733          public static boolean createRotation(Vector3d original, Vector3d rotated, Quat4d result) {\r
 734                  \r
 735                         if (rotated.lengthSquared() > 0.01)\r
 736                                 rotated.normalize();\r
 737                         else\r
 738                                 return false;\r
 739                         double d = original.dot(rotated);\r
 740                         if (d > 0.9999) {\r
 741                                 // original and rotated are parallel, pointing at the same direction\r
 742                                 result.w = 1.0;\r
 743                                 result.x = 0.0;\r
 744                                 result.y = 0.0;\r
 745                                 result.z = 0.0;\r
 746                         } else if (d < -0.9999) {\r
 747                                 // original and rotated are parallel, pointing at the opposite direction \r
 748                                 Vector3d a = Z_AXIS;\r
 749                                 if (Math.abs(a.dot(original)) > 0.8 )\r
 750                                         a = Y_AXIS;\r
 751                                 getQuat(a, Math.PI, result);\r
 752                                 \r
 753                         } else {\r
 754                                 double angle = original.angle(rotated);\r
 755                                 Vector3d axis = new Vector3d();\r
 756                                 axis.cross(original, rotated);\r
 757                                 getQuat(axis, angle, result);\r
 758                         }\r
 759                         return true;\r
 760                  }\r
 761          \r
 762          public static void getQuat(Vector3d axis, double angle, Quat4d q)\r
 763     {\r
 764                 double mag,amag;\r
 765                 // Quat = cos(theta/2) + sin(theta/2)(roation_axis) \r
 766                 \r
 767                 amag = Math.sqrt( axis.x*axis.x + axis.y*axis.y + axis.z*axis.z);\r
 768                 if( amag < EPS ) {\r
 769                     q.w = 1.0;\r
 770                     q.x = 0.0;\r
 771                     q.y = 0.0;\r
 772                     q.z = 0.0;\r
 773                 } else {  \r
 774                     amag = 1.0/amag; \r
 775                     double a2 = angle*0.5;\r
 776                     mag = Math.sin(a2);\r
 777                     q.w = Math.cos(a2);\r
 778                     q.x = axis.x*amag*mag;\r
 779                     q.y = axis.y*amag*mag;\r
 780                     q.z = axis.z*amag*mag;\r
 781                 }\r
 782         \r
 783     }\r
 784          \r
 785          /**\r
 786           * Linear interpolation of quaternions. Result IS set to q1.\r
 787           * @param q1\r
 788           * @param q2\r
 789           * @param alpha\r
 790           */\r
 791          public static void lip(Quat4d q1, Quat4d q2, double alpha) {\r
 792                  double s1 = 1.0 - alpha;\r
 793                  double s2 = alpha;\r
 794                  q1.scale(s1);\r
 795                  mad(q1,q2,s2);\r
 796                  q1.normalize();\r
 797          }\r
 798          \r
 799          public static double dot(Quat4d q1, Quat4d q2) {\r
 800                  return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;\r
 801          }\r
 802          \r
 803          public static void mad(Tuple3d q1, Tuple3d q2, double s2) {\r
 804                  q1.x += q2.x * s2;\r
 805                  q1.y += q2.y * s2;\r
 806                  q1.z += q2.z * s2;\r
 807          }\r
 808          \r
 809          public static void mad(Quat4d q1, Quat4d q2, double s2) {\r
 810                  q1.x += q2.x * s2;\r
 811                  q1.y += q2.y * s2;\r
 812                  q1.z += q2.z * s2;\r
 813                  q1.w += q2.w * s2;\r
 814          }\r
 815          \r
 816          /**\r
 817           * Slerp\r
 818           * \r
 819           * Sets results to q1. Modifies q2.\r
 820           * \r
 821           * @param q1\r
 822           * @param q2\r
 823           * @param alpha\r
 824           */\r
 825          public static void sip(Quat4d q1, Quat4d q2, double alpha) {\r
 826                  double cosom = dot(q1,q2);\r
 827                  if (cosom < 0.0) {\r
 828                          cosom = -cosom;\r
 829                          q2.negate();\r
 830                  }\r
 831                  \r
 832                  if (cosom > 0.9999) {\r
 833                          q2.sub(q1);\r
 834                          q2.scale(alpha);\r
 835                          q1.add(q2);\r
 836                          q1.normalize();\r
 837                          return;\r
 838                  }\r
 839                  double theta_0 = Math.acos(cosom);\r
 840                  double theta = theta_0 * alpha;\r
 841                  Quat4d t = new Quat4d(q1);\r
 842                  t.scale(-cosom);\r
 843                  t.add(q2);\r
 844                  t.normalize();\r
 845                  t.scale(Math.sin(theta));\r
 846                  q1.scale(Math.cos(theta));\r
 847                  q1.add(t);\r
 848          }\r
 849 }\r