org.simantics.proconf.g3d/src/org/simantics/proconf/g3d/base/MathTools.java

   1 /*******************************************************************************\r
   2  * Copyright (c) 2007 VTT Technical Research Centre of Finland and others.\r
   3  * All rights reserved. This program and the accompanying materials\r
   4  * are made available under the terms of the Eclipse Public License v1.0\r
   5  * which accompanies this distribution, and is available at\r
   6  * http://www.eclipse.org/legal/epl-v10.html\r
   7  *\r
   8  * Contributors:\r
   9  *     VTT Technical Research Centre of Finland - initial API and implementation\r
  10  *******************************************************************************/\r
  11 package org.simantics.proconf.g3d.base;\r
  12 \r
  13 import javax.vecmath.AxisAngle4d;\r
  14 import javax.vecmath.Matrix3d;\r
  15 import javax.vecmath.Point3d;\r
  16 import javax.vecmath.Quat4d;\r
  17 import javax.vecmath.Tuple3d;\r
  18 import javax.vecmath.Vector3d;\r
  19 \r
  20 import com.jme.math.Vector2f;\r
  21 \r
  22 /**\r
  23  * Some useful geometry related math functions. Beware, methods may modify their input parameters!\r
  24  * \r
  25  * @author Marko Luukkainen\r
  26  *\r
  27  */\r
  28 public class MathTools {\r
  29     \r
  30     static double EPS = 0.001;\r
  31     \r
  32     public static Vector3d closestPointOnEdge(Vector3d point, Vector3d edgePoint1, Vector3d edgePoint2) {\r
  33         point.sub(edgePoint1);\r
  34         Vector3d v = new Vector3d(edgePoint2);\r
  35         v.sub(edgePoint1);\r
  36         double t = v.dot(point);\r
  37         t /= v.lengthSquared();\r
  38         if (t <= 0.0f)\r
  39           return edgePoint1;\r
  40         if (t >= 1.0f)\r
  41           return edgePoint2;\r
  42         v.scale(t);\r
  43         v.add(edgePoint1);\r
  44         return v;   \r
  45     }\r
  46     \r
  47     public static Vector3d closestPointOnStraight(Point3d point, Point3d straightPoint, Vector3d straightDir) {\r
  48         Vector3d v = new Vector3d(point);\r
  49         v.sub(straightPoint);\r
  50         double t = straightDir.dot(v);\r
  51         t /= straightDir.lengthSquared();\r
  52         v.set(straightDir);\r
  53         v.scale(t);\r
  54         v.add(straightPoint);\r
  55         return v;   \r
  56     }\r
  57     \r
  58     public static Vector3d closestPointOnStraight(Point3d point, Point3d straightPoint, Vector3d straightDir, double u[]) {\r
  59         Vector3d v = new Vector3d(point);\r
  60         v.sub(straightPoint);\r
  61         u[0] = straightDir.dot(v);\r
  62         u[0] /= straightDir.lengthSquared();\r
  63         v.set(straightDir);\r
  64         v.scale(u[0]);\r
  65         v.add(straightPoint);\r
  66         return v;   \r
  67     }\r
  68     \r
  69     public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, Tuple3d planePoint) {\r
  70         point.sub(planePoint);\r
  71         \r
  72         return planeNormal.dot(point);\r
  73     }\r
  74       \r
  75     public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, float d) {\r
  76         return (planeNormal.dot(point) + d);\r
  77     }\r
  78     \r
  79     public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Tuple3d intersectPoint) {\r
  80         intersectPoint.set(planePoint);\r
  81         intersectPoint.sub(linePoint);\r
  82         double u = planeNormal.dot(new Vector3d(intersectPoint));\r
  83         double v = planeNormal.dot(lineDir);\r
  84         if (Math.abs(v) < EPS)\r
  85             return false;\r
  86         u /= v;\r
  87         intersectPoint.set(lineDir);\r
  88         intersectPoint.scale(u);\r
  89         intersectPoint.add(linePoint);\r
  90         return true;\r
  91     }\r
  92     \r
  93     public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Vector3d intersectPoint, double[] u) {\r
  94         intersectPoint.set(planePoint);\r
  95         intersectPoint.sub(linePoint);\r
  96         u[0] = planeNormal.dot(intersectPoint);\r
  97         double v = planeNormal.dot(lineDir);\r
  98         if (Math.abs(v) < EPS)\r
  99             return false;\r
 100         u[0] /= v;\r
 101         intersectPoint.set(lineDir);\r
 102         intersectPoint.scale(u[0]);\r
 103         intersectPoint.add(linePoint);\r
 104         return true;\r
 105     }\r
 106     \r
 107     public static boolean intersectLineLine(Vector3d p1,Vector3d p2,Vector3d p3,Vector3d p4,Vector3d pa,Vector3d pb) {\r
 108             Vector3d p13 = new Vector3d();\r
 109             Vector3d p43 = new Vector3d();\r
 110             Vector3d p21 = new Vector3d();\r
 111             double d1343,d4321,d1321,d4343,d2121;\r
 112             double numer,denom;\r
 113             p13.sub(p1, p3);\r
 114             p43.sub(p4,p3);\r
 115             if (Math.abs(p43.x)  < EPS && Math.abs(p43.y)  < EPS && Math.abs(p43.z)  < EPS)\r
 116                return false;\r
 117             p21.sub(p2,p1);\r
 118             if (Math.abs(p21.x)  < EPS && Math.abs(p21.y)  < EPS && Math.abs(p21.z)  < EPS)\r
 119                return false;\r
 120 \r
 121             d1343 = p13.dot(p43);\r
 122             d4321 = p43.dot(p21);\r
 123             d1321 = p13.dot(p21);\r
 124             d4343 = p43.lengthSquared();\r
 125             d2121 = p21.lengthSquared();\r
 126 \r
 127             denom = d2121 * d4343 - d4321 * d4321;\r
 128             if (Math.abs(denom) < EPS)\r
 129                return false;\r
 130             numer = d1343 * d4321 - d1321 * d4343;\r
 131 \r
 132             double mua = numer / denom;\r
 133             double mub = (d1343 + d4321 * mua) / d4343;\r
 134  \r
 135             pa.x = p1.x + mua * p21.x;\r
 136             pa.y = p1.y + mua * p21.y;\r
 137             pa.z = p1.z + mua * p21.z;\r
 138             pb.x = p3.x + mub * p43.x;\r
 139             pb.y = p3.y + mub * p43.y;\r
 140             pb.z = p3.z + mub * p43.z;\r
 141 \r
 142             return true;\r
 143     }\r
 144     \r
 145     public static boolean intersectStraightStraight(Vector3d p1,Vector3d p21,Vector3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb) {\r
 146         Vector3d p13 = new Vector3d();\r
 147 \r
 148         double d1343,d4321,d1321,d4343,d2121;\r
 149         double numer,denom;\r
 150         \r
 151         p13.sub(p1, p3);\r
 152         if (Math.abs(p43.x)  < EPS && Math.abs(p43.y)  < EPS && Math.abs(p43.z)  < EPS)\r
 153            return false;\r
 154         if (Math.abs(p21.x)  < EPS && Math.abs(p21.y)  < EPS && Math.abs(p21.z)  < EPS)\r
 155            return false;\r
 156 \r
 157         d1343 = p13.dot(p43);\r
 158         d4321 = p43.dot(p21);\r
 159         d1321 = p13.dot(p21);\r
 160         d4343 = p43.lengthSquared();\r
 161         d2121 = p21.lengthSquared();\r
 162 \r
 163         denom = d2121 * d4343 - d4321 * d4321;\r
 164         if (Math.abs(denom) < EPS)\r
 165            return false;\r
 166         numer = d1343 * d4321 - d1321 * d4343;\r
 167 \r
 168         double mua = numer / denom;\r
 169         double mub = (d1343 + d4321 * mua) / d4343;\r
 170 \r
 171         pa.x = p1.x + mua * p21.x;\r
 172         pa.y = p1.y + mua * p21.y;\r
 173         pa.z = p1.z + mua * p21.z;\r
 174         pb.x = p3.x + mub * p43.x;\r
 175         pb.y = p3.y + mub * p43.y;\r
 176         pb.z = p3.z + mub * p43.z;\r
 177 \r
 178         return true;\r
 179    }\r
 180     \r
 181     /**\r
 182      * Calculate the line segment PaPb that is the shortest route between\r
 183      *  two lines P1P2 and P3P4. Calculate also the values of mua and mub where\r
 184      *  Pa = P1 + mua (P2 - P1)\r
 185      *  Pb = P3 + mub (P4 - P3)\r
 186      * @param p1\r
 187      * @param p21\r
 188      * @param p3\r
 189      * @param p43\r
 190      * @param pa\r
 191      * @param pb\r
 192      * @param mu\r
 193      * @return\r
 194      */\r
 195     public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb, double mu[]) {\r
 196         Vector3d p13 = new Vector3d();\r
 197 \r
 198         double d1343,d4321,d1321,d4343,d2121;\r
 199         double numer,denom;\r
 200         double EPS = 0.001;\r
 201         p13.sub(p1, p3);\r
 202         if (Math.abs(p43.x)  < EPS && Math.abs(p43.y)  < EPS && Math.abs(p43.z)  < EPS)\r
 203            return false;\r
 204         if (Math.abs(p21.x)  < EPS && Math.abs(p21.y)  < EPS && Math.abs(p21.z)  < EPS)\r
 205            return false;\r
 206 \r
 207         d1343 = p13.dot(p43);\r
 208         d4321 = p43.dot(p21);\r
 209         d1321 = p13.dot(p21);\r
 210         d4343 = p43.lengthSquared();\r
 211         d2121 = p21.lengthSquared();\r
 212 \r
 213         denom = d2121 * d4343 - d4321 * d4321;\r
 214         if (Math.abs(denom) < EPS)\r
 215            return false;\r
 216         numer = d1343 * d4321 - d1321 * d4343;\r
 217 \r
 218         mu[0] = numer / denom;\r
 219         mu[1] = (d1343 + d4321 * mu[0]) / d4343;\r
 220 \r
 221         pa.x = p1.x + mu[0] * p21.x;\r
 222         pa.y = p1.y + mu[0] * p21.y;\r
 223         pa.z = p1.z + mu[0] * p21.z;\r
 224         pb.x = p3.x + mu[1] * p43.x;\r
 225         pb.y = p3.y + mu[1] * p43.y;\r
 226         pb.z = p3.z + mu[1] * p43.z;\r
 227 \r
 228         return true;\r
 229    }\r
 230    \r
 231    public static AxisAngle4d getFromEuler2(Vector3d euler) {\r
 232        AxisAngle4d aa = new AxisAngle4d();\r
 233        aa.angle = euler.length();\r
 234        Vector3d normal = new Vector3d(euler);\r
 235        if (aa.angle > EPS) {\r
 236            normal.normalize();\r
 237            aa.x = normal.x;\r
 238            aa.y = normal.y;\r
 239            aa.z = normal.z;\r
 240        } else {\r
 241            aa.x = 1.0;\r
 242            aa.y = 0.0;\r
 243            aa.z = 0.0;\r
 244        }\r
 245        \r
 246        return aa;\r
 247    }\r
 248    \r
 249    public static Vector3d getEuler(AxisAngle4d aa) {\r
 250        Vector3d euler = new Vector3d(aa.x,aa.y,aa.z);\r
 251        euler.scale(aa.angle);\r
 252        return euler;\r
 253    }\r
 254    \r
 255    public static void rotate(Quat4d q, Tuple3d in, Tuple3d out) {\r
 256        // p' = q * p * q'\r
 257        double tw =           - q.x*in.x - q.y*in.y - q.z*in.z;\r
 258        double tx =  q.w*in.x            + q.y*in.z - q.z*in.y;\r
 259        double ty =  q.w*in.y - q.x*in.z            + q.z*in.x;\r
 260        double tz =  q.w*in.z + q.x*in.y - q.y*in.x           ;\r
 261        \r
 262        //temp * q' -> x = -x, y = -y z = -z\r
 263        //out.w = tw*q.w + tx*q.x + ty*q.y + tz*q.z;\r
 264        out.x =  -tw*q.x + tx*q.w - ty*q.z + tz*q.y;\r
 265        out.y =  -tw*q.y + tx*q.z + ty*q.w - tz*q.x;\r
 266        out.z =  -tw*q.z - tx*q.y + ty*q.x + tz*q.w;  \r
 267    }\r
 268    \r
 269    public static void getMatrix(Quat4d quat, Matrix3d m) {\r
 270                    m.m00 = 1.0f - 2.0 * (quat.y * quat.y + quat.z * quat.z);\r
 271                    m.m01 = 2.0 * (quat.x * quat.y + quat.w * quat.z);\r
 272                    m.m02 = 2.0 * (quat.x * quat.z - quat.w * quat.y);\r
 273                    m.m10 = 2.0 * (quat.x * quat.y - quat.w * quat.z);\r
 274                    m.m11 = 1.0 - 2.0f * (quat.x * quat.x + quat.z * quat.z);\r
 275                    m.m12 = 2.0 * (quat.y * quat.z + quat.w * quat.x);\r
 276                    m.m20 = 2.0 * (quat.x * quat.z + quat.w * quat.y);\r
 277                    m.m21 = 2.0 * (quat.y * quat.z - quat.w * quat.x);\r
 278                    m.m22 = 1.0 - 2.0f * (quat.x * quat.x + quat.y * quat.y);\r
 279 \r
 280    }\r
 281    \r
 282    public static void getQuat(Matrix3d mat, Quat4d quat) {\r
 283            double tr = mat.m00 + mat.m11 + mat.m22;\r
 284                 if (tr > 0.0) {\r
 285                         double s = Math.sqrt(tr + 1.0);\r
 286                         quat.w = 0.5 * s;\r
 287                         s = 0.5 / s;\r
 288                         quat.x = (mat.m21 - mat.m12) * s;\r
 289                         quat.y = (mat.m02 - mat.m20) * s;\r
 290                         quat.z = (mat.m10 - mat.m01) * s;\r
 291                 } else {\r
 292                         int i = 0, j, k;\r
 293                         if (mat.m11 > mat.m00)\r
 294                                 i = 1;\r
 295                         if (mat.m22 > mat.getElement(i, i))\r
 296                                 i = 2;\r
 297                         int nxt[] = { 1, 2, 0 };\r
 298 \r
 299                         j = nxt[i];\r
 300                         k = nxt[j];\r
 301 \r
 302                         double s = Math\r
 303                                         .sqrt((mat.getElement(i, i) - (mat.getElement(j, j) + mat\r
 304                                                         .getElement(k, k))) + 1.0);\r
 305 \r
 306                         double q[] = new double[3];\r
 307                         q[i] = s * 0.5;\r
 308 \r
 309                         if (Math.abs(s) > 0.001)\r
 310                                 s = 0.5 / s;\r
 311 \r
 312                         quat.w = (mat.getElement(k, j) - mat.getElement(j, k)) * s;\r
 313                         q[j] = (mat.getElement(j, i) + mat.getElement(i, j)) * s;\r
 314                         q[k] = (mat.getElement(k, i) + mat.getElement(i, k)) * s;\r
 315 \r
 316                         quat.x = q[0];\r
 317                         quat.y = q[1];\r
 318                         quat.z = q[2];\r
 319                 }\r
 320         }\r
 321    \r
 322    \r
 323         /*\r
 324          * Cohen-Sutherland\r
 325          */\r
 326         \r
 327         private static final int IN = 0;\r
 328         private static final int LEFT = 1;\r
 329         private static final int RIGHT = 2;\r
 330         private static final int BOTTOM = 4;\r
 331         private static final int TOP = 8;\r
 332         \r
 333         \r
 334         private static int bitcode(Vector2f p1, Vector2f min, Vector2f max) {\r
 335                 int code = IN;\r
 336                 if (p1.x < min.x)\r
 337                         code |= LEFT;\r
 338                 else if (p1.x > max.x)\r
 339                         code |= RIGHT;\r
 340                 if (p1.y < min.y)\r
 341                         code |= BOTTOM;\r
 342                 else if (p1.y > max.y)\r
 343                         code |= TOP;\r
 344                 return code;\r
 345         }\r
 346         \r
 347         public static boolean clipLineRectangle(Vector2f p1,Vector2f p2, Vector2f min, Vector2f max, Vector2f r1, Vector2f r2) {\r
 348                 while (true) {\r
 349                         int o1 = bitcode(p1, min, max);\r
 350                         int o2 = bitcode(p2, min, max);\r
 351                         int and = o1 & o2;\r
 352                         int or = o1 | o2;\r
 353                         if (and != IN) {\r
 354                                 return false;\r
 355                         }\r
 356                         if (or == IN) {\r
 357                                 r1.set(p1);\r
 358                                 r2.set(p2);\r
 359                                 return true;\r
 360                         }\r
 361                         if (o1 == IN) {\r
 362                                 Vector2f t = p1;\r
 363                                 p1 = p2;\r
 364                                 p2 = t;\r
 365                                 int t2 = o1;\r
 366                                 o1 = o2;\r
 367                                 o2 = t2;\r
 368                         }\r
 369                         if ((o1 & TOP) != IN) {\r
 370                                 float t = (max.y - p1.y) / (p2.y - p1.y);\r
 371                                 p1.x += t * (p2.x - p1.x);\r
 372                                 p1.y = max.y;\r
 373                         } else if ((o1 & BOTTOM) != IN) {\r
 374                                 float t = (min.y - p1.y) / (p2.y - p1.y);\r
 375                                 p1.x += t * (p2.x - p1.x);\r
 376                                 p1.y = min.y;\r
 377                         } else if ((o1 & LEFT) != IN) {\r
 378                                 float t = (min.x - p1.x) / (p2.x - p1.x);\r
 379                                 p1.y += t * (p2.y - p1.y);\r
 380                                 p1.x = min.x;\r
 381                         } else if ((o1 & RIGHT) != IN) {\r
 382                                 float t = (max.x - p1.x) / (p2.x - p1.x);\r
 383                                 p1.y += t * (p2.y - p1.y);\r
 384                                 p1.x = max.x;\r
 385                         } else {\r
 386                                 throw new RuntimeException("Error in clipping code");\r
 387                         }\r
 388                 }\r
 389                 \r
 390         }\r
 391         \r
 392         public static double square(double d) {\r
 393                 return d * d;\r
 394         }\r
 395 }\r