+++ /dev/null
-/*******************************************************************************\r
- * Copyright (c) 2007- VTT Technical Research Centre of Finland.\r
- * All rights reserved. This program and the accompanying materials\r
- * are made available under the terms of the Eclipse Public License v1.0\r
- * which accompanies this distribution, and is available at\r
- * http://www.eclipse.org/legal/epl-v10.html\r
- *\r
- * Contributors:\r
- * VTT Technical Research Centre of Finland - initial API and implementation\r
- *******************************************************************************/\r
-package org.simantics.proconf.g3d.base;\r
-\r
-import javax.vecmath.AxisAngle4d;\r
-import javax.vecmath.Matrix3d;\r
-import javax.vecmath.Point3d;\r
-import javax.vecmath.Quat4d;\r
-import javax.vecmath.Tuple3d;\r
-import javax.vecmath.Vector3d;\r
-\r
-import com.jme.math.Vector2f;\r
-\r
-/**\r
- * Some useful geometry related math functions. Beware, methods may modify their input parameters!\r
- * \r
- * @author Marko Luukkainen\r
- *\r
- */\r
-public class MathTools {\r
- \r
- static double EPS = 0.001;\r
- \r
- public static Vector3d closestPointOnEdge(Vector3d point, Vector3d edgePoint1, Vector3d edgePoint2) {\r
- point.sub(edgePoint1);\r
- Vector3d v = new Vector3d(edgePoint2);\r
- v.sub(edgePoint1);\r
- double t = v.dot(point);\r
- t /= v.lengthSquared();\r
- if (t <= 0.0f)\r
- return edgePoint1;\r
- if (t >= 1.0f)\r
- return edgePoint2;\r
- v.scale(t);\r
- v.add(edgePoint1);\r
- return v; \r
- }\r
- \r
- public static Vector3d closestPointOnStraight(Point3d point, Point3d straightPoint, Vector3d straightDir) {\r
- Vector3d v = new Vector3d(point);\r
- v.sub(straightPoint);\r
- double t = straightDir.dot(v);\r
- t /= straightDir.lengthSquared();\r
- v.set(straightDir);\r
- v.scale(t);\r
- v.add(straightPoint);\r
- return v; \r
- }\r
- \r
- public static Vector3d closestPointOnStraight(Point3d point, Point3d straightPoint, Vector3d straightDir, double u[]) {\r
- Vector3d v = new Vector3d(point);\r
- v.sub(straightPoint);\r
- u[0] = straightDir.dot(v);\r
- u[0] /= straightDir.lengthSquared();\r
- v.set(straightDir);\r
- v.scale(u[0]);\r
- v.add(straightPoint);\r
- return v; \r
- }\r
- \r
- public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, Tuple3d planePoint) {\r
- point.sub(planePoint);\r
- \r
- return planeNormal.dot(point);\r
- }\r
- \r
- public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, float d) {\r
- return (planeNormal.dot(point) + d);\r
- }\r
- \r
- public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Tuple3d intersectPoint) {\r
- intersectPoint.set(planePoint);\r
- intersectPoint.sub(linePoint);\r
- double u = planeNormal.dot(new Vector3d(intersectPoint));\r
- double v = planeNormal.dot(lineDir);\r
- if (Math.abs(v) < EPS)\r
- return false;\r
- u /= v;\r
- intersectPoint.set(lineDir);\r
- intersectPoint.scale(u);\r
- intersectPoint.add(linePoint);\r
- return true;\r
- }\r
- \r
- public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Vector3d intersectPoint, double[] u) {\r
- intersectPoint.set(planePoint);\r
- intersectPoint.sub(linePoint);\r
- u[0] = planeNormal.dot(intersectPoint);\r
- double v = planeNormal.dot(lineDir);\r
- if (Math.abs(v) < EPS)\r
- return false;\r
- u[0] /= v;\r
- intersectPoint.set(lineDir);\r
- intersectPoint.scale(u[0]);\r
- intersectPoint.add(linePoint);\r
- return true;\r
- }\r
- \r
- public static boolean intersectLineLine(Vector3d p1,Vector3d p2,Vector3d p3,Vector3d p4,Vector3d pa,Vector3d pb) {\r
- Vector3d p13 = new Vector3d();\r
- Vector3d p43 = new Vector3d();\r
- Vector3d p21 = new Vector3d();\r
- double d1343,d4321,d1321,d4343,d2121;\r
- double numer,denom;\r
- p13.sub(p1, p3);\r
- p43.sub(p4,p3);\r
- if (Math.abs(p43.x) < EPS && Math.abs(p43.y) < EPS && Math.abs(p43.z) < EPS)\r
- return false;\r
- p21.sub(p2,p1);\r
- if (Math.abs(p21.x) < EPS && Math.abs(p21.y) < EPS && Math.abs(p21.z) < EPS)\r
- return false;\r
-\r
- d1343 = p13.dot(p43);\r
- d4321 = p43.dot(p21);\r
- d1321 = p13.dot(p21);\r
- d4343 = p43.lengthSquared();\r
- d2121 = p21.lengthSquared();\r
-\r
- denom = d2121 * d4343 - d4321 * d4321;\r
- if (Math.abs(denom) < EPS)\r
- return false;\r
- numer = d1343 * d4321 - d1321 * d4343;\r
-\r
- double mua = numer / denom;\r
- double mub = (d1343 + d4321 * mua) / d4343;\r
- \r
- pa.x = p1.x + mua * p21.x;\r
- pa.y = p1.y + mua * p21.y;\r
- pa.z = p1.z + mua * p21.z;\r
- pb.x = p3.x + mub * p43.x;\r
- pb.y = p3.y + mub * p43.y;\r
- pb.z = p3.z + mub * p43.z;\r
-\r
- return true;\r
- }\r
- \r
- public static boolean intersectStraightStraight(Vector3d p1,Vector3d p21,Vector3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb) {\r
- Vector3d p13 = new Vector3d();\r
-\r
- double d1343,d4321,d1321,d4343,d2121;\r
- double numer,denom;\r
- \r
- p13.sub(p1, p3);\r
- if (Math.abs(p43.x) < EPS && Math.abs(p43.y) < EPS && Math.abs(p43.z) < EPS)\r
- return false;\r
- if (Math.abs(p21.x) < EPS && Math.abs(p21.y) < EPS && Math.abs(p21.z) < EPS)\r
- return false;\r
-\r
- d1343 = p13.dot(p43);\r
- d4321 = p43.dot(p21);\r
- d1321 = p13.dot(p21);\r
- d4343 = p43.lengthSquared();\r
- d2121 = p21.lengthSquared();\r
-\r
- denom = d2121 * d4343 - d4321 * d4321;\r
- if (Math.abs(denom) < EPS)\r
- return false;\r
- numer = d1343 * d4321 - d1321 * d4343;\r
-\r
- double mua = numer / denom;\r
- double mub = (d1343 + d4321 * mua) / d4343;\r
-\r
- pa.x = p1.x + mua * p21.x;\r
- pa.y = p1.y + mua * p21.y;\r
- pa.z = p1.z + mua * p21.z;\r
- pb.x = p3.x + mub * p43.x;\r
- pb.y = p3.y + mub * p43.y;\r
- pb.z = p3.z + mub * p43.z;\r
-\r
- return true;\r
- }\r
- \r
- /**\r
- * Calculate the line segment PaPb that is the shortest route between\r
- * two lines P1P2 and P3P4. Calculate also the values of mua and mub where\r
- * Pa = P1 + mua (P2 - P1)\r
- * Pb = P3 + mub (P4 - P3)\r
- * @param p1\r
- * @param p21\r
- * @param p3\r
- * @param p43\r
- * @param pa\r
- * @param pb\r
- * @param mu\r
- * @return\r
- */\r
- public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb, double mu[]) {\r
- Vector3d p13 = new Vector3d();\r
-\r
- double d1343,d4321,d1321,d4343,d2121;\r
- double numer,denom;\r
- double EPS = 0.001;\r
- p13.sub(p1, p3);\r
- if (Math.abs(p43.x) < EPS && Math.abs(p43.y) < EPS && Math.abs(p43.z) < EPS)\r
- return false;\r
- if (Math.abs(p21.x) < EPS && Math.abs(p21.y) < EPS && Math.abs(p21.z) < EPS)\r
- return false;\r
-\r
- d1343 = p13.dot(p43);\r
- d4321 = p43.dot(p21);\r
- d1321 = p13.dot(p21);\r
- d4343 = p43.lengthSquared();\r
- d2121 = p21.lengthSquared();\r
-\r
- denom = d2121 * d4343 - d4321 * d4321;\r
- if (Math.abs(denom) < EPS)\r
- return false;\r
- numer = d1343 * d4321 - d1321 * d4343;\r
-\r
- mu[0] = numer / denom;\r
- mu[1] = (d1343 + d4321 * mu[0]) / d4343;\r
-\r
- pa.x = p1.x + mu[0] * p21.x;\r
- pa.y = p1.y + mu[0] * p21.y;\r
- pa.z = p1.z + mu[0] * p21.z;\r
- pb.x = p3.x + mu[1] * p43.x;\r
- pb.y = p3.y + mu[1] * p43.y;\r
- pb.z = p3.z + mu[1] * p43.z;\r
-\r
- return true;\r
- }\r
- \r
- public static AxisAngle4d getFromEuler2(Vector3d euler) {\r
- AxisAngle4d aa = new AxisAngle4d();\r
- aa.angle = euler.length();\r
- Vector3d normal = new Vector3d(euler);\r
- if (aa.angle > EPS) {\r
- normal.normalize();\r
- aa.x = normal.x;\r
- aa.y = normal.y;\r
- aa.z = normal.z;\r
- } else {\r
- aa.x = 1.0;\r
- aa.y = 0.0;\r
- aa.z = 0.0;\r
- }\r
- \r
- return aa;\r
- }\r
- \r
- public static Vector3d getEuler(AxisAngle4d aa) {\r
- Vector3d euler = new Vector3d(aa.x,aa.y,aa.z);\r
- euler.scale(aa.angle);\r
- return euler;\r
- }\r
- \r
- public static void rotate(Quat4d q, Tuple3d in, Tuple3d out) {\r
- // p' = q * p * q'\r
- double tw = - q.x*in.x - q.y*in.y - q.z*in.z;\r
- double tx = q.w*in.x + q.y*in.z - q.z*in.y;\r
- double ty = q.w*in.y - q.x*in.z + q.z*in.x;\r
- double tz = q.w*in.z + q.x*in.y - q.y*in.x ;\r
- \r
- //temp * q' -> x = -x, y = -y z = -z\r
- //out.w = tw*q.w + tx*q.x + ty*q.y + tz*q.z;\r
- out.x = -tw*q.x + tx*q.w - ty*q.z + tz*q.y;\r
- out.y = -tw*q.y + tx*q.z + ty*q.w - tz*q.x;\r
- out.z = -tw*q.z - tx*q.y + ty*q.x + tz*q.w; \r
- }\r
- \r
- public static void getMatrix(Quat4d quat, Matrix3d m) {\r
- m.m00 = 1.0f - 2.0 * (quat.y * quat.y + quat.z * quat.z);\r
- m.m01 = 2.0 * (quat.x * quat.y + quat.w * quat.z);\r
- m.m02 = 2.0 * (quat.x * quat.z - quat.w * quat.y);\r
- m.m10 = 2.0 * (quat.x * quat.y - quat.w * quat.z);\r
- m.m11 = 1.0 - 2.0f * (quat.x * quat.x + quat.z * quat.z);\r
- m.m12 = 2.0 * (quat.y * quat.z + quat.w * quat.x);\r
- m.m20 = 2.0 * (quat.x * quat.z + quat.w * quat.y);\r
- m.m21 = 2.0 * (quat.y * quat.z - quat.w * quat.x);\r
- m.m22 = 1.0 - 2.0f * (quat.x * quat.x + quat.y * quat.y);\r
-\r
- }\r
- \r
- public static void getQuat(Matrix3d mat, Quat4d quat) {\r
- double tr = mat.m00 + mat.m11 + mat.m22;\r
- if (tr > 0.0) {\r
- double s = Math.sqrt(tr + 1.0);\r
- quat.w = 0.5 * s;\r
- s = 0.5 / s;\r
- quat.x = (mat.m21 - mat.m12) * s;\r
- quat.y = (mat.m02 - mat.m20) * s;\r
- quat.z = (mat.m10 - mat.m01) * s;\r
- } else {\r
- int i = 0, j, k;\r
- if (mat.m11 > mat.m00)\r
- i = 1;\r
- if (mat.m22 > mat.getElement(i, i))\r
- i = 2;\r
- int nxt[] = { 1, 2, 0 };\r
-\r
- j = nxt[i];\r
- k = nxt[j];\r
-\r
- double s = Math\r
- .sqrt((mat.getElement(i, i) - (mat.getElement(j, j) + mat\r
- .getElement(k, k))) + 1.0);\r
-\r
- double q[] = new double[3];\r
- q[i] = s * 0.5;\r
-\r
- if (Math.abs(s) > 0.001)\r
- s = 0.5 / s;\r
-\r
- quat.w = (mat.getElement(k, j) - mat.getElement(j, k)) * s;\r
- q[j] = (mat.getElement(j, i) + mat.getElement(i, j)) * s;\r
- q[k] = (mat.getElement(k, i) + mat.getElement(i, k)) * s;\r
-\r
- quat.x = q[0];\r
- quat.y = q[1];\r
- quat.z = q[2];\r
- }\r
- }\r
- \r
- \r
- /*\r
- * Cohen-Sutherland\r
- */\r
- \r
- private static final int IN = 0;\r
- private static final int LEFT = 1;\r
- private static final int RIGHT = 2;\r
- private static final int BOTTOM = 4;\r
- private static final int TOP = 8;\r
- \r
- \r
- private static int bitcode(Vector2f p1, Vector2f min, Vector2f max) {\r
- int code = IN;\r
- if (p1.x < min.x)\r
- code |= LEFT;\r
- else if (p1.x > max.x)\r
- code |= RIGHT;\r
- if (p1.y < min.y)\r
- code |= BOTTOM;\r
- else if (p1.y > max.y)\r
- code |= TOP;\r
- return code;\r
- }\r
- \r
- public static boolean clipLineRectangle(Vector2f p1,Vector2f p2, Vector2f min, Vector2f max, Vector2f r1, Vector2f r2) {\r
- while (true) {\r
- int o1 = bitcode(p1, min, max);\r
- int o2 = bitcode(p2, min, max);\r
- int and = o1 & o2;\r
- int or = o1 | o2;\r
- if (and != IN) {\r
- return false;\r
- }\r
- if (or == IN) {\r
- r1.set(p1);\r
- r2.set(p2);\r
- return true;\r
- }\r
- if (o1 == IN) {\r
- Vector2f t = p1;\r
- p1 = p2;\r
- p2 = t;\r
- int t2 = o1;\r
- o1 = o2;\r
- o2 = t2;\r
- }\r
- if ((o1 & TOP) != IN) {\r
- float t = (max.y - p1.y) / (p2.y - p1.y);\r
- p1.x += t * (p2.x - p1.x);\r
- p1.y = max.y;\r
- } else if ((o1 & BOTTOM) != IN) {\r
- float t = (min.y - p1.y) / (p2.y - p1.y);\r
- p1.x += t * (p2.x - p1.x);\r
- p1.y = min.y;\r
- } else if ((o1 & LEFT) != IN) {\r
- float t = (min.x - p1.x) / (p2.x - p1.x);\r
- p1.y += t * (p2.y - p1.y);\r
- p1.x = min.x;\r
- } else if ((o1 & RIGHT) != IN) {\r
- float t = (max.x - p1.x) / (p2.x - p1.x);\r
- p1.y += t * (p2.y - p1.y);\r
- p1.x = max.x;\r
- } else {\r
- throw new RuntimeException("Error in clipping code");\r
- }\r
- }\r
- \r
- }\r
- \r
- public static double square(double d) {\r
- return d * d;\r
- }\r
-}\r
Code Review / simantics / 3d.git / blobdiff