-/*******************************************************************************\r
- * Copyright (c) 2012, 2013 Association for Decentralized Information Management in\r
- * Industry THTH ry.\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.g3d.math;\r
-\r
-import javax.vecmath.AxisAngle4d;\r
-import javax.vecmath.Matrix3d;\r
-import javax.vecmath.Matrix4d;\r
-import javax.vecmath.Quat4d;\r
-import javax.vecmath.Tuple3d;\r
-import javax.vecmath.Tuple4d;\r
-import javax.vecmath.Vector2f;\r
-import javax.vecmath.Vector3d;\r
-\r
-import org.simantics.g3d.math.EulerTools.Order;\r
-\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
- public static double NEAR_ZERO = 0.0000001;\r
- public static double NEAR_HALF = 0.4999999;\r
- \r
- public static final Vector3d Z_AXIS = new Vector3d(0.0,0.0,1.0);\r
- public static final Vector3d Y_AXIS = new Vector3d(0.0,1.0,0.0);\r
- public static final Vector3d X_AXIS = new Vector3d(1.0,0.0,0.0);\r
- public static final Vector3d ORIGIN = new Vector3d(0.0,0.0,0.0);\r
- \r
- final static double EPS = 1.0e-12;\r
- \r
- \r
- public static boolean equals(double d1, double d2) {\r
- return Math.abs(d1-d2) < EPS;\r
- }\r
- \r
- public static boolean equals(Tuple3d p1, Tuple3d p2) {\r
- return distanceSquared(p1, p2) < NEAR_ZERO;\r
- }\r
- \r
- public static boolean equals(Tuple4d p1, Tuple4d p2) {\r
- return distanceSquared(p1, p2) < NEAR_ZERO;\r
- }\r
- \r
- public static double distance(Tuple3d p1, Tuple3d p2) {\r
- double dx, dy, dz;\r
-\r
- dx = p2.x - p1.x;\r
- dy = p2.y - p1.y;\r
- dz = p2.z - p1.z;\r
- return Math.sqrt(dx*dx+dy*dy+dz*dz);\r
- }\r
- \r
- public static double distance(Tuple4d p1, Tuple4d p2) {\r
- double dx, dy, dz, dw;\r
-\r
- dx = p2.x - p1.x;\r
- dy = p2.y - p1.y;\r
- dz = p2.z - p1.z;\r
- dw = p2.w - p1.w;\r
- return Math.sqrt(dx*dx+dy*dy+dz*dz+dw*dw);\r
- }\r
- \r
- public static double distanceSquared(Tuple3d p1, Tuple3d p2) {\r
- double dx, dy, dz;\r
-\r
- dx = p2.x - p1.x;\r
- dy = p2.y - p1.y;\r
- dz = p2.z - p1.z;\r
- return dx*dx+dy*dy+dz*dz;\r
- }\r
- \r
- public static double distanceSquared(Tuple4d p1, Tuple4d p2) {\r
- double dx, dy, dz, dw;\r
-\r
- dx = p2.x - p1.x;\r
- dy = p2.y - p1.y;\r
- dz = p2.z - p1.z;\r
- dw = p2.w - p1.w;\r
- return dx*dx+dy*dy+dz*dz+dw*dw;\r
- }\r
- \r
- public static boolean isValid(Tuple3d t) {\r
- return !(Double.isInfinite(t.x) || Double.isNaN(t.x) ||\r
- Double.isInfinite(t.y) || Double.isNaN(t.y) ||\r
- Double.isInfinite(t.z) || Double.isNaN(t.z));\r
- }\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(Tuple3d point, Tuple3d 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(Tuple3d point, Tuple3d 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) < NEAR_ZERO)\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) < NEAR_ZERO)\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(Tuple3d l1_start,Tuple3d l1_end,Tuple3d l2_start,Tuple3d l2_end,Tuple3d l1_pos, Tuple3d l2_pos) {\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(l1_start, l2_start);\r
- p43.sub(l2_end,l2_start);\r
- if (Math.abs(p43.x) < NEAR_ZERO && Math.abs(p43.y) < NEAR_ZERO && Math.abs(p43.z) < NEAR_ZERO)\r
- return false;\r
- p21.sub(l1_end,l1_start);\r
- if (Math.abs(p21.x) < NEAR_ZERO && Math.abs(p21.y) < NEAR_ZERO && Math.abs(p21.z) < NEAR_ZERO)\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) < NEAR_ZERO)\r
- return false;\r
- numer = d1343 * d4321 - d1321 * d4343;\r
-\r
- double mua = numer / denom;\r
- double mub = (d1343 + d4321 * mua) / d4343;\r
- \r
- l1_pos.x = l1_start.x + mua * p21.x;\r
- l1_pos.y = l1_start.y + mua * p21.y;\r
- l1_pos.z = l1_start.z + mua * p21.z;\r
- l2_pos.x = l2_start.x + mub * p43.x;\r
- l2_pos.y = l2_start.y + mub * p43.y;\r
- l2_pos.z = l2_start.z + mub * p43.z;\r
-\r
- return true;\r
- }\r
- \r
- public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d 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) < NEAR_ZERO && Math.abs(p43.y) < NEAR_ZERO && Math.abs(p43.z) < NEAR_ZERO)\r
- return false;\r
- if (Math.abs(p21.x) < NEAR_ZERO && Math.abs(p21.y) < NEAR_ZERO && Math.abs(p21.z) < NEAR_ZERO)\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) < NEAR_ZERO)\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
- \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
- \r
- private static double q[] = new double[3];\r
- private static int nxt[] = { 1, 2, 0 };\r
- /**\r
- * Converts Matrix to Quaternion\r
- * \r
- * Note: non-thread safe.\r
- * \r
- * @param mat\r
- * @param quat\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
- \r
-\r
- j = nxt[i];\r
- k = nxt[j];\r
-\r
- double s = Math.sqrt((mat.getElement(i, i) - (mat.getElement(j, j) + mat.getElement(k, k))) + 1.0);\r
-\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
- public static Quat4d getQuat(Matrix3d mat) {\r
- Quat4d q = new Quat4d();\r
- getQuat(mat, q);\r
- return q;\r
- }\r
- \r
- public static AxisAngle4d getFromPseudoEuler(Vector3d euler) {\r
- AxisAngle4d aa = new AxisAngle4d();\r
- aa.angle = euler.length();\r
- Vector3d normal = new Vector3d(euler);\r
- if (aa.angle > NEAR_ZERO) {\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 getPseudoEuler(AxisAngle4d aa) {\r
- Vector3d euler = new Vector3d(aa.x,aa.y,aa.z);\r
- euler.scale(aa.angle);\r
- return euler;\r
- }\r
- \r
- \r
- public static void getQuat(Vector3d euler, Quat4d quat) {\r
- Quat4d q = EulerTools.getQuatFromEuler(Order.YXZ, euler.y,euler.x,euler.z);\r
- quat.set(q);\r
- // http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Conversion_formulae_between_formalisms\r
- // Using the x-convention, the 3-1-3 Euler angles phi, theta and psi (around the Z, X and again the Z-axis) \r
-// quat.x = -Math.cos((euler.x - euler.z)*0.5)*Math.sin(euler.y*0.5);\r
-// quat.y = -Math.sin((euler.x - euler.z)*0.5)*Math.sin(euler.y*0.5);\r
-// quat.z = -Math.sin((euler.x + euler.z)*0.5)*Math.cos(euler.y*0.5);\r
-// quat.w = Math.sin((euler.x + euler.z)*0.5)*Math.cos(euler.y*0.5);\r
- \r
- // http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm\r
- // Y, Z, X order\r
-// double c1 = Math.cos(euler.y*0.5);\r
-// double s1 = Math.sin(euler.y*0.5);\r
-// double c2 = Math.cos(euler.z*0.5);\r
-// double s2 = Math.sin(euler.z*0.5);\r
-// double c3 = Math.cos(euler.x*0.5);\r
-// double s3 = Math.sin(euler.x*0.5);\r
-// double c1c2 = c1*c2;\r
-// double s1s2 = s1*s2;\r
-// quat.w =c1c2*c3 - s1s2*s3;\r
-// quat.x =c1c2*s3 + s1s2*c3;\r
-// quat.y =s1*c2*c3 + c1*s2*s3;\r
-// quat.z =c1*s2*c3 - s1*c2*s3;\r
-\r
-// Quat4d q2 = EulerTools.getQuatFromEuler(Order.YZX, euler.y,euler.z,euler.x);\r
-// System.out.println("Q " + quat + " Q2 " + q2);\r
-// double c1 = Math.cos(euler.y);\r
-// double s1 = Math.sin(euler.y);\r
-// double c2 = Math.cos(euler.z);\r
-// double s2 = Math.sin(euler.z);\r
-// double c3 = Math.cos(euler.x);\r
-// double s3 = Math.sin(euler.x);\r
-// quat.w = Math.sqrt(1.0 + c1 * c2 + c1*c3 - s1 * s2 * s3 + c2*c3) / 2.0;\r
-// double w4 = (4.0 * quat.w);\r
-// quat.x = (c2 * s3 + c1 * s3 + s1 * s2 * c3) / w4 ;\r
-// quat.y = (s1 * c2 + s1 * c3 + c1 * s2 * s3) / w4 ;\r
-// quat.z = (-s1 * s3 + c1 * s2 * c3 +s2) / w4 ;\r
- }\r
- \r
- \r
- \r
- \r
- public static void getEuler(Quat4d quat,Vector3d euler) {\r
- Vector3d e = EulerTools.getEulerFromQuat(Order.YXZ, quat);\r
- euler.x = e.y;\r
- euler.y = e.x;\r
- euler.z = e.z;\r
- \r
- // http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Conversion_formulae_between_formalisms\r
-// euler.x = Math.atan2(quat.x * quat.z + quat.y* quat.w, quat.y*quat.z - quat.x * quat.w);\r
-// euler.y = Math.acos(-square(quat.x) - square(quat.y) + square(quat.z) + square(quat.w));\r
-// euler.z = -Math.atan2(quat.x * quat.z - quat.y* quat.w, quat.y*quat.z + quat.x * quat.w);\r
- \r
- // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm\r
- // Y, Z, X order\r
-// double test = quat.x * quat.y + quat.z * quat.w;\r
-// if (test > NEAR_HALF) {\r
-// euler.y = 2.0 * Math.atan2(quat.x,quat.w);\r
-// euler.z = Math.PI * 0.5;\r
-// euler.x = 0.0;\r
-// } else if (test < -NEAR_HALF) {\r
-// euler.y = -2.0 * Math.atan2(quat.x,quat.w);\r
-// euler.z = -Math.PI * 0.5;\r
-// euler.x = 0.0;\r
-// } else {\r
-// double sqx = square(quat.x);\r
-// double sqy = square(quat.y);\r
-// double sqz = square(quat.z);\r
-// euler.y = Math.atan2(2.0*(quat.y*quat.w-quat.x*quat.z), 1.0 - 2.0*(sqy-sqz));\r
-// euler.z = Math.asin(2.0*test);\r
-// euler.x = Math.atan2(2.0*(quat.x*quat.w-quat.y*quat.z), 1.0 - 2.0*(sqx-sqz));\r
-// System.out.println(euler + " " + EulerTools.getEulerFromQuat(Order.YXZ, quat) + " " + quat);\r
-// }\r
-// double sqw = quat.w*quat.w;\r
-// double sqx = quat.x*quat.x;\r
-// double sqy = quat.y*quat.y;\r
-// double sqz = quat.z*quat.z;\r
-// double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor\r
-// double test = quat.x*quat.y + quat.z*quat.w;\r
-// if (test > 0.499*unit) { // singularity at north pole\r
-// euler.y = 2 * Math.atan2(quat.x,quat.w);\r
-// euler.z = Math.PI/2;\r
-// euler.x = 0;\r
-// return;\r
-// }\r
-// if (test < -0.499*unit) { // singularity at south pole\r
-// euler.y = -2 * Math.atan2(quat.x,quat.w);\r
-// euler.z = -Math.PI/2;\r
-// euler.x = 0;\r
-// return;\r
-// }\r
-// euler.y = Math.atan2(2*quat.y*quat.w-2*quat.x*quat.z , sqx - sqy - sqz + sqw);\r
-// euler.z = Math.asin(2*test/unit);\r
-// euler.x = Math.atan2(2*quat.x*quat.w-2*quat.y*quat.z , -sqx + sqy - sqz + sqw);\r
- }\r
- \r
- public static Quat4d getQuat(Vector3d euler) {\r
- Quat4d q = new Quat4d();\r
- getQuat(euler,q);\r
- return q;\r
- }\r
- \r
- \r
- public static Vector3d getEuler(Quat4d quat) {\r
- Vector3d v = new Vector3d();\r
- getEuler(quat, v);\r
- return v;\r
- }\r
- \r
- public static Quat4d getQuat(AxisAngle4d aa) {\r
- Quat4d q = new Quat4d();\r
- getQuat(aa, q);\r
- return q;\r
- }\r
- \r
- public static AxisAngle4d getAxisAngle(Quat4d q) {\r
- AxisAngle4d aa = new AxisAngle4d();\r
- aa.set(q);\r
- return aa;\r
- }\r
- \r
- public static Quat4d getIdentityQuat() {\r
- return new Quat4d(0, 0, 0, 1);\r
- }\r
- \r
- public static void getQuat(AxisAngle4d aa, Quat4d q) {\r
- double mag,amag;\r
- // Quat = cos(theta/2) + sin(theta/2)(roation_axis) \r
- \r
- amag = Math.sqrt( aa.x*aa.x + aa.y*aa.y + aa.z*aa.z);\r
- if( amag < NEAR_ZERO ) {\r
- q.w = 1.0;\r
- q.x = 0.0;\r
- q.y = 0.0;\r
- q.z = 0.0;\r
- } else { \r
- amag = 1.0/amag; \r
- double a2 = aa.angle * 0.5;\r
- mag = Math.sin(a2);\r
- q.w = Math.cos(a2);\r
- q.x = aa.x*amag*mag;\r
- q.y = aa.y*amag*mag;\r
- q.z = aa.z*amag*mag;\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
- \r
- public static void multiplyOrientation(AxisAngle4d aa, AxisAngle4d rot) {\r
- Quat4d q1 = new Quat4d();\r
- getQuat(aa, q1);\r
- Quat4d q2 = new Quat4d();\r
- getQuat(rot, q2);\r
- q2.mul(q1);\r
- rot.set(q2);\r
- }\r
- \r
- public static double radToDeg(double rad) {\r
- return (rad / Math.PI) * 180.0;\r
- }\r
- \r
- public static double degToRad(double deg) {\r
- return (deg / 180.0) * Math.PI;\r
- }\r
- \r
- public static double clamp(double min, double max,double v) {\r
- if (v < min)\r
- return min;\r
- if (v > max)\r
- return max;\r
- return v;\r
- }\r
- \r
- public static AxisAngle4d createRotation(Vector3d original, Vector3d rotated) { \r
- AxisAngle4d result = new AxisAngle4d();\r
- if (createRotation(original, rotated, result))\r
- return result;\r
- return null;\r
- }\r
- \r
- \r
- public static void setIdentity(Quat4d q) {\r
- q.w = 1.0;\r
- q.x = 0.0;\r
- q.y = 0.0;\r
- q.z = 0.0;\r
- }\r
- \r
- public static void setIdentity(AxisAngle4d aa) {\r
- aa.angle = 0.0;\r
- aa.x = 0.0;\r
- aa.y = 1.0;\r
- aa.z = 0.0;\r
- }\r
- \r
- public static void set(Matrix3d mat, double m00, double m01, double m02,\r
- double m10, double m11, double m12, double m20, double m21,\r
- double m22) {\r
- mat.m00 = m00;\r
- mat.m01 = m01;\r
- mat.m02 = m02;\r
-\r
- mat.m10 = m10;\r
- mat.m11 = m11;\r
- mat.m12 = m12;\r
-\r
- mat.m20 = m20;\r
- mat.m21 = m21;\r
- mat.m22 = m22;\r
- }\r
- \r
- public static void set(Matrix4d mat, double[] v) {\r
- mat.m00 = v[0];\r
- mat.m01 = v[1];\r
- mat.m02 = v[2];\r
- mat.m03 = v[3];\r
-\r
- mat.m10 = v[4];\r
- mat.m11 = v[5];\r
- mat.m12 = v[6];\r
- mat.m13 = v[7];\r
-\r
- mat.m20 = v[8];\r
- mat.m21 = v[9];\r
- mat.m22 = v[10];\r
- mat.m23 = v[11];\r
-\r
- mat.m30 = v[12];\r
- mat.m31 = v[13];\r
- mat.m32 = v[14];\r
- mat.m33 = v[15];\r
-\r
- }\r
- \r
- public static boolean createRotation(Vector3d original, Vector3d rotated, AxisAngle4d result) {\r
- \r
- if (rotated.lengthSquared() > 0.01)\r
- rotated.normalize();\r
- else\r
- return false;\r
- double d = original.dot(rotated);\r
- if (d > 0.9999) {\r
- // original and rotated are parallel, pointing at the same direction\r
- result.angle = 0.0;\r
- result.x = 0.0;\r
- result.y = 1.0;\r
- result.z = 0.0;\r
- } else if (d < -0.9999) {\r
- // original and rotated are parallel, pointing at the opposite direction \r
- Vector3d a = Z_AXIS;\r
- if (Math.abs(a.dot(original)) > 0.8 )\r
- a = Y_AXIS;\r
- result.set(a, Math.PI);\r
- } else {\r
- double angle = original.angle(rotated);\r
- Vector3d axis = new Vector3d();\r
- axis.cross(original, rotated);\r
- result.set(axis,angle);\r
- }\r
- return true;\r
- }\r
- \r
- public static boolean createRotation(Vector3d original, Vector3d rotated, Quat4d result) {\r
- \r
- if (rotated.lengthSquared() > 0.01)\r
- rotated.normalize();\r
- else\r
- return false;\r
- double d = original.dot(rotated);\r
- if (d > 0.9999) {\r
- // original and rotated are parallel, pointing at the same direction\r
- result.w = 1.0;\r
- result.x = 0.0;\r
- result.y = 0.0;\r
- result.z = 0.0;\r
- } else if (d < -0.9999) {\r
- // original and rotated are parallel, pointing at the opposite direction \r
- Vector3d a = Z_AXIS;\r
- if (Math.abs(a.dot(original)) > 0.8 )\r
- a = Y_AXIS;\r
- getQuat(a, Math.PI, result);\r
- \r
- } else {\r
- double angle = original.angle(rotated);\r
- Vector3d axis = new Vector3d();\r
- axis.cross(original, rotated);\r
- getQuat(axis, angle, result);\r
- }\r
- return true;\r
- }\r
- \r
- public static void getQuat(Vector3d axis, double angle, Quat4d q)\r
- {\r
- double mag,amag;\r
- // Quat = cos(theta/2) + sin(theta/2)(roation_axis) \r
- \r
- amag = Math.sqrt( axis.x*axis.x + axis.y*axis.y + axis.z*axis.z);\r
- if( amag < EPS ) {\r
- q.w = 1.0;\r
- q.x = 0.0;\r
- q.y = 0.0;\r
- q.z = 0.0;\r
- } else { \r
- amag = 1.0/amag; \r
- double a2 = angle*0.5;\r
- mag = Math.sin(a2);\r
- q.w = Math.cos(a2);\r
- q.x = axis.x*amag*mag;\r
- q.y = axis.y*amag*mag;\r
- q.z = axis.z*amag*mag;\r
- }\r
- \r
- }\r
- \r
- /**\r
- * Linear interpolation of quaternions. Result IS set to q1.\r
- * @param q1\r
- * @param q2\r
- * @param alpha\r
- */\r
- public static void lip(Quat4d q1, Quat4d q2, double alpha) {\r
- double s1 = 1.0 - alpha;\r
- double s2 = alpha;\r
- q1.scale(s1);\r
- mad(q1,q2,s2);\r
- q1.normalize();\r
- }\r
- \r
- public static double dot(Quat4d q1, Quat4d q2) {\r
- return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;\r
- }\r
- \r
- public static void mad(Tuple3d q1, Tuple3d q2, double s2) {\r
- q1.x += q2.x * s2;\r
- q1.y += q2.y * s2;\r
- q1.z += q2.z * s2;\r
- }\r
- \r
- public static void mad(Quat4d q1, Quat4d q2, double s2) {\r
- q1.x += q2.x * s2;\r
- q1.y += q2.y * s2;\r
- q1.z += q2.z * s2;\r
- q1.w += q2.w * s2;\r
- }\r
- \r
- /**\r
- * Slerp\r
- * \r
- * Sets results to q1. Modifies q2.\r
- * \r
- * @param q1\r
- * @param q2\r
- * @param alpha\r
- */\r
- public static void sip(Quat4d q1, Quat4d q2, double alpha) {\r
- double cosom = dot(q1,q2);\r
- if (cosom < 0.0) {\r
- cosom = -cosom;\r
- q2.negate();\r
- }\r
- \r
- if (cosom > 0.9999) {\r
- q2.sub(q1);\r
- q2.scale(alpha);\r
- q1.add(q2);\r
- q1.normalize();\r
- return;\r
- }\r
- double theta_0 = Math.acos(cosom);\r
- double theta = theta_0 * alpha;\r
- Quat4d t = new Quat4d(q1);\r
- t.scale(-cosom);\r
- t.add(q2);\r
- t.normalize();\r
- t.scale(Math.sin(theta));\r
- q1.scale(Math.cos(theta));\r
- q1.add(t);\r
- }\r
-}\r
+/*******************************************************************************
+ * Copyright (c) 2012, 2013 Association for Decentralized Information Management in
+ * Industry THTH ry.
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * which accompanies this distribution, and is available at
+ * http://www.eclipse.org/legal/epl-v10.html
+ *
+ * Contributors:
+ * VTT Technical Research Centre of Finland - initial API and implementation
+ *******************************************************************************/
+package org.simantics.g3d.math;
+
+import javax.vecmath.AxisAngle4d;
+import javax.vecmath.Matrix3d;
+import javax.vecmath.Matrix4d;
+import javax.vecmath.Quat4d;
+import javax.vecmath.Tuple2d;
+import javax.vecmath.Tuple3d;
+import javax.vecmath.Tuple4d;
+import javax.vecmath.Vector2d;
+import javax.vecmath.Vector2f;
+import javax.vecmath.Vector3d;
+
+import org.simantics.g3d.math.EulerTools.Order;
+
+
+/**
+ * Some useful geometry related math functions. Beware, methods may modify their input parameters!
+ *
+ * @author Marko Luukkainen
+ *
+ */
+public class MathTools {
+
+ public static double NEAR_ZERO = 0.000000001;
+ public static double NEAR_HALF = 0.499999999;
+
+ public static final Vector3d Z_AXIS = new Vector3d(0.0,0.0,1.0);
+ public static final Vector3d Y_AXIS = new Vector3d(0.0,1.0,0.0);
+ public static final Vector3d X_AXIS = new Vector3d(1.0,0.0,0.0);
+ public static final Vector3d ORIGIN = new Vector3d(0.0,0.0,0.0);
+
+ final static double EPS = 1.0e-12;
+
+
+ public static boolean equals(double d1, double d2) {
+ return Math.abs(d1-d2) < EPS;
+ }
+
+ public static boolean equals(Tuple3d p1, Tuple3d p2) {
+ return distanceSquared(p1, p2) < NEAR_ZERO;
+ }
+
+ public static boolean equals(Tuple4d p1, Tuple4d p2) {
+ return distanceSquared(p1, p2) < NEAR_ZERO;
+ }
+
+ public static double distance(Tuple3d p1, Tuple3d p2) {
+ double dx, dy, dz;
+
+ dx = p2.x - p1.x;
+ dy = p2.y - p1.y;
+ dz = p2.z - p1.z;
+ return Math.sqrt(dx*dx+dy*dy+dz*dz);
+ }
+
+ public static double distance(Tuple4d p1, Tuple4d p2) {
+ double dx, dy, dz, dw;
+
+ dx = p2.x - p1.x;
+ dy = p2.y - p1.y;
+ dz = p2.z - p1.z;
+ dw = p2.w - p1.w;
+ return Math.sqrt(dx*dx+dy*dy+dz*dz+dw*dw);
+ }
+
+ public static double distanceSquared(Tuple3d p1, Tuple3d p2) {
+ double dx, dy, dz;
+
+ dx = p2.x - p1.x;
+ dy = p2.y - p1.y;
+ dz = p2.z - p1.z;
+ return dx*dx+dy*dy+dz*dz;
+ }
+
+ public static double distanceSquared(Tuple4d p1, Tuple4d p2) {
+ double dx, dy, dz, dw;
+
+ dx = p2.x - p1.x;
+ dy = p2.y - p1.y;
+ dz = p2.z - p1.z;
+ dw = p2.w - p1.w;
+ return dx*dx+dy*dy+dz*dz+dw*dw;
+ }
+
+ public static boolean isValid(Tuple3d t) {
+ return !(Double.isInfinite(t.x) || Double.isNaN(t.x) ||
+ Double.isInfinite(t.y) || Double.isNaN(t.y) ||
+ Double.isInfinite(t.z) || Double.isNaN(t.z));
+ }
+
+ public static Vector3d closestPointOnEdge(Vector3d point, Vector3d edgePoint1, Vector3d edgePoint2) {
+ point.sub(edgePoint1);
+ Vector3d v = new Vector3d(edgePoint2);
+ v.sub(edgePoint1);
+ double t = v.dot(point);
+ t /= v.lengthSquared();
+ if (t <= 0.0f)
+ return edgePoint1;
+ if (t >= 1.0f)
+ return edgePoint2;
+ v.scale(t);
+ v.add(edgePoint1);
+ return v;
+ }
+
+ public static Vector3d closestPointOnStraight(Tuple3d point, Tuple3d straightPoint, Vector3d straightDir) {
+ Vector3d v = new Vector3d(point);
+ v.sub(straightPoint);
+ double t = straightDir.dot(v);
+ t /= straightDir.lengthSquared();
+ v.set(straightDir);
+ v.scale(t);
+ v.add(straightPoint);
+ return v;
+ }
+
+ public static Vector3d closestPointOnStraight(Tuple3d point, Tuple3d straightPoint, Vector3d straightDir, double u[]) {
+ Vector3d v = new Vector3d(point);
+ v.sub(straightPoint);
+ u[0] = straightDir.dot(v);
+ u[0] /= straightDir.lengthSquared();
+ v.set(straightDir);
+ v.scale(u[0]);
+ v.add(straightPoint);
+ return v;
+ }
+
+ public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, Tuple3d planePoint) {
+ point.sub(planePoint);
+
+ return planeNormal.dot(point);
+ }
+
+ public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, float d) {
+ return (planeNormal.dot(point) + d);
+ }
+
+ public static Vector3d projectToPlane(Vector3d v, Vector3d planeNormal) {
+ //v.normalize();
+ //planeNormal.normalize();
+ Vector3d t = new Vector3d();
+ t.cross(v,planeNormal);
+ t.cross(planeNormal, t);
+ return t;
+
+ }
+
+ public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Tuple3d intersectPoint) {
+ intersectPoint.set(planePoint);
+ intersectPoint.sub(linePoint);
+ double u = planeNormal.dot(new Vector3d(intersectPoint));
+ double v = planeNormal.dot(lineDir);
+ if (Math.abs(v) < NEAR_ZERO)
+ return false;
+ u /= v;
+ intersectPoint.set(lineDir);
+ intersectPoint.scale(u);
+ intersectPoint.add(linePoint);
+ return true;
+ }
+
+ public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Vector3d intersectPoint, double[] u) {
+ intersectPoint.set(planePoint);
+ intersectPoint.sub(linePoint);
+ u[0] = planeNormal.dot(intersectPoint);
+ double v = planeNormal.dot(lineDir);
+ if (Math.abs(v) < NEAR_ZERO)
+ return false;
+ u[0] /= v;
+ intersectPoint.set(lineDir);
+ intersectPoint.scale(u[0]);
+ intersectPoint.add(linePoint);
+ return true;
+ }
+
+ public static boolean intersectLineLine(Tuple3d l1_start,Tuple3d l1_end,Tuple3d l2_start,Tuple3d l2_end,Tuple3d l1_pos, Tuple3d l2_pos) {
+ Vector3d p13 = new Vector3d();
+ Vector3d p43 = new Vector3d();
+ Vector3d p21 = new Vector3d();
+ double d1343,d4321,d1321,d4343,d2121;
+ double numer,denom;
+ p13.sub(l1_start, l2_start);
+ p43.sub(l2_end,l2_start);
+ if (Math.abs(p43.x) < NEAR_ZERO && Math.abs(p43.y) < NEAR_ZERO && Math.abs(p43.z) < NEAR_ZERO)
+ return false;
+ p21.sub(l1_end,l1_start);
+ if (Math.abs(p21.x) < NEAR_ZERO && Math.abs(p21.y) < NEAR_ZERO && Math.abs(p21.z) < NEAR_ZERO)
+ return false;
+
+ d1343 = p13.dot(p43);
+ d4321 = p43.dot(p21);
+ d1321 = p13.dot(p21);
+ d4343 = p43.lengthSquared();
+ d2121 = p21.lengthSquared();
+
+ denom = d2121 * d4343 - d4321 * d4321;
+ if (Math.abs(denom) < NEAR_ZERO)
+ return false;
+ numer = d1343 * d4321 - d1321 * d4343;
+
+ double mua = numer / denom;
+ double mub = (d1343 + d4321 * mua) / d4343;
+
+ l1_pos.x = l1_start.x + mua * p21.x;
+ l1_pos.y = l1_start.y + mua * p21.y;
+ l1_pos.z = l1_start.z + mua * p21.z;
+ l2_pos.x = l2_start.x + mub * p43.x;
+ l2_pos.y = l2_start.y + mub * p43.y;
+ l2_pos.z = l2_start.z + mub * p43.z;
+
+ return true;
+ }
+
+ public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb) {
+ Vector3d p13 = new Vector3d();
+
+ double d1343,d4321,d1321,d4343,d2121;
+ double numer,denom;
+
+ p13.sub(p1, p3);
+ if (Math.abs(p43.x) < NEAR_ZERO && Math.abs(p43.y) < NEAR_ZERO && Math.abs(p43.z) < NEAR_ZERO)
+ return false;
+ if (Math.abs(p21.x) < NEAR_ZERO && Math.abs(p21.y) < NEAR_ZERO && Math.abs(p21.z) < NEAR_ZERO)
+ return false;
+
+ d1343 = p13.dot(p43);
+ d4321 = p43.dot(p21);
+ d1321 = p13.dot(p21);
+ d4343 = p43.lengthSquared();
+ d2121 = p21.lengthSquared();
+
+ denom = d2121 * d4343 - d4321 * d4321;
+ if (Math.abs(denom) < NEAR_ZERO)
+ return false;
+ numer = d1343 * d4321 - d1321 * d4343;
+
+ double mua = numer / denom;
+ double mub = (d1343 + d4321 * mua) / d4343;
+
+ pa.x = p1.x + mua * p21.x;
+ pa.y = p1.y + mua * p21.y;
+ pa.z = p1.z + mua * p21.z;
+ pb.x = p3.x + mub * p43.x;
+ pb.y = p3.y + mub * p43.y;
+ pb.z = p3.z + mub * p43.z;
+
+ return true;
+ }
+
+ /**
+ * Calculate the line segment PaPb that is the shortest route between
+ * two lines P1P2 and P3P4. Calculate also the values of mua and mub where
+ * Pa = P1 + mua (P2 - P1)
+ * Pb = P3 + mub (P4 - P3)
+ * @param p1
+ * @param p21
+ * @param p3
+ * @param p43
+ * @param pa
+ * @param pb
+ * @param mu
+ * @return
+ */
+ public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb, double mu[]) {
+ Vector3d p13 = new Vector3d();
+
+ double d1343,d4321,d1321,d4343,d2121;
+ double numer,denom;
+ double EPS = 0.001;
+ p13.sub(p1, p3);
+ if (Math.abs(p43.x) < EPS && Math.abs(p43.y) < EPS && Math.abs(p43.z) < EPS)
+ return false;
+ if (Math.abs(p21.x) < EPS && Math.abs(p21.y) < EPS && Math.abs(p21.z) < EPS)
+ return false;
+
+ d1343 = p13.dot(p43);
+ d4321 = p43.dot(p21);
+ d1321 = p13.dot(p21);
+ d4343 = p43.lengthSquared();
+ d2121 = p21.lengthSquared();
+
+ denom = d2121 * d4343 - d4321 * d4321;
+ if (Math.abs(denom) < EPS)
+ return false;
+ numer = d1343 * d4321 - d1321 * d4343;
+
+ mu[0] = numer / denom;
+ mu[1] = (d1343 + d4321 * mu[0]) / d4343;
+
+ pa.x = p1.x + mu[0] * p21.x;
+ pa.y = p1.y + mu[0] * p21.y;
+ pa.z = p1.z + mu[0] * p21.z;
+ pb.x = p3.x + mu[1] * p43.x;
+ pb.y = p3.y + mu[1] * p43.y;
+ pb.z = p3.z + mu[1] * p43.z;
+
+ return true;
+ }
+
+
+
+ public static void rotate(Quat4d q, Tuple3d in, Tuple3d out) {
+ // p' = q * p * q'
+ double tw = - q.x*in.x - q.y*in.y - q.z*in.z;
+ double tx = q.w*in.x + q.y*in.z - q.z*in.y;
+ double ty = q.w*in.y - q.x*in.z + q.z*in.x;
+ double tz = q.w*in.z + q.x*in.y - q.y*in.x ;
+
+ //temp * q' -> x = -x, y = -y z = -z
+ //out.w = tw*q.w + tx*q.x + ty*q.y + tz*q.z;
+ out.x = -tw*q.x + tx*q.w - ty*q.z + tz*q.y;
+ out.y = -tw*q.y + tx*q.z + ty*q.w - tz*q.x;
+ out.z = -tw*q.z - tx*q.y + ty*q.x + tz*q.w;
+ }
+
+ public static void getMatrix(Quat4d quat, Matrix3d m) {
+ m.m00 = 1.0f - 2.0 * (quat.y * quat.y + quat.z * quat.z);
+ m.m01 = 2.0 * (quat.x * quat.y + quat.w * quat.z);
+ m.m02 = 2.0 * (quat.x * quat.z - quat.w * quat.y);
+ m.m10 = 2.0 * (quat.x * quat.y - quat.w * quat.z);
+ m.m11 = 1.0 - 2.0f * (quat.x * quat.x + quat.z * quat.z);
+ m.m12 = 2.0 * (quat.y * quat.z + quat.w * quat.x);
+ m.m20 = 2.0 * (quat.x * quat.z + quat.w * quat.y);
+ m.m21 = 2.0 * (quat.y * quat.z - quat.w * quat.x);
+ m.m22 = 1.0 - 2.0f * (quat.x * quat.x + quat.y * quat.y);
+
+ }
+
+ public static void getMatrix(Quat4d quat, Matrix4d m) {
+ m.setZero();
+ m.m00 = 1.0f - 2.0 * (quat.y * quat.y + quat.z * quat.z);
+ m.m01 = 2.0 * (quat.x * quat.y + quat.w * quat.z);
+ m.m02 = 2.0 * (quat.x * quat.z - quat.w * quat.y);
+ m.m10 = 2.0 * (quat.x * quat.y - quat.w * quat.z);
+ m.m11 = 1.0 - 2.0f * (quat.x * quat.x + quat.z * quat.z);
+ m.m12 = 2.0 * (quat.y * quat.z + quat.w * quat.x);
+ m.m20 = 2.0 * (quat.x * quat.z + quat.w * quat.y);
+ m.m21 = 2.0 * (quat.y * quat.z - quat.w * quat.x);
+ m.m22 = 1.0 - 2.0f * (quat.x * quat.x + quat.y * quat.y);
+ m.m33 = 1.0;
+ }
+
+ private static double q[] = new double[3];
+ private static int nxt[] = { 1, 2, 0 };
+ /**
+ * Converts Matrix to Quaternion
+ *
+ * Note: non-thread safe.
+ *
+ * @param mat
+ * @param quat
+ */
+ public static void getQuat(Matrix3d mat, Quat4d quat) {
+ double tr = mat.m00 + mat.m11 + mat.m22;
+ if (tr > 0.0) {
+ double s = Math.sqrt(tr + 1.0);
+ quat.w = 0.5 * s;
+ s = 0.5 / s;
+ quat.x = (mat.m21 - mat.m12) * s;
+ quat.y = (mat.m02 - mat.m20) * s;
+ quat.z = (mat.m10 - mat.m01) * s;
+ } else {
+ int i = 0, j, k;
+ if (mat.m11 > mat.m00)
+ i = 1;
+ if (mat.m22 > mat.getElement(i, i))
+ i = 2;
+
+
+ j = nxt[i];
+ k = nxt[j];
+
+ double s = Math.sqrt((mat.getElement(i, i) - (mat.getElement(j, j) + mat.getElement(k, k))) + 1.0);
+
+ q[i] = s * 0.5;
+
+ if (Math.abs(s) > 0.001)
+ s = 0.5 / s;
+
+ quat.w = (mat.getElement(k, j) - mat.getElement(j, k)) * s;
+ q[j] = (mat.getElement(j, i) + mat.getElement(i, j)) * s;
+ q[k] = (mat.getElement(k, i) + mat.getElement(i, k)) * s;
+
+ quat.x = q[0];
+ quat.y = q[1];
+ quat.z = q[2];
+ }
+ }
+
+ public static Quat4d getQuat(Matrix3d mat) {
+ Quat4d q = new Quat4d();
+ getQuat(mat, q);
+ return q;
+ }
+
+ public static AxisAngle4d getFromPseudoEuler(Vector3d euler) {
+ AxisAngle4d aa = new AxisAngle4d();
+ aa.angle = euler.length();
+ Vector3d normal = new Vector3d(euler);
+ if (aa.angle > NEAR_ZERO) {
+ normal.normalize();
+ aa.x = normal.x;
+ aa.y = normal.y;
+ aa.z = normal.z;
+ } else {
+ aa.x = 1.0;
+ aa.y = 0.0;
+ aa.z = 0.0;
+ }
+
+ return aa;
+ }
+
+ public static Vector3d getPseudoEuler(AxisAngle4d aa) {
+ Vector3d euler = new Vector3d(aa.x,aa.y,aa.z);
+ euler.scale(aa.angle);
+ return euler;
+ }
+
+
+ public static void getQuat(Vector3d euler, Quat4d quat) {
+ Quat4d q = EulerTools.getQuatFromEuler(Order.YXZ, euler.y,euler.x,euler.z);
+ quat.set(q);
+ // http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Conversion_formulae_between_formalisms
+ // Using the x-convention, the 3-1-3 Euler angles phi, theta and psi (around the Z, X and again the Z-axis)
+// quat.x = -Math.cos((euler.x - euler.z)*0.5)*Math.sin(euler.y*0.5);
+// quat.y = -Math.sin((euler.x - euler.z)*0.5)*Math.sin(euler.y*0.5);
+// quat.z = -Math.sin((euler.x + euler.z)*0.5)*Math.cos(euler.y*0.5);
+// quat.w = Math.sin((euler.x + euler.z)*0.5)*Math.cos(euler.y*0.5);
+
+ // http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
+ // Y, Z, X order
+// double c1 = Math.cos(euler.y*0.5);
+// double s1 = Math.sin(euler.y*0.5);
+// double c2 = Math.cos(euler.z*0.5);
+// double s2 = Math.sin(euler.z*0.5);
+// double c3 = Math.cos(euler.x*0.5);
+// double s3 = Math.sin(euler.x*0.5);
+// double c1c2 = c1*c2;
+// double s1s2 = s1*s2;
+// quat.w =c1c2*c3 - s1s2*s3;
+// quat.x =c1c2*s3 + s1s2*c3;
+// quat.y =s1*c2*c3 + c1*s2*s3;
+// quat.z =c1*s2*c3 - s1*c2*s3;
+
+// Quat4d q2 = EulerTools.getQuatFromEuler(Order.YZX, euler.y,euler.z,euler.x);
+// System.out.println("Q " + quat + " Q2 " + q2);
+// double c1 = Math.cos(euler.y);
+// double s1 = Math.sin(euler.y);
+// double c2 = Math.cos(euler.z);
+// double s2 = Math.sin(euler.z);
+// double c3 = Math.cos(euler.x);
+// double s3 = Math.sin(euler.x);
+// quat.w = Math.sqrt(1.0 + c1 * c2 + c1*c3 - s1 * s2 * s3 + c2*c3) / 2.0;
+// double w4 = (4.0 * quat.w);
+// quat.x = (c2 * s3 + c1 * s3 + s1 * s2 * c3) / w4 ;
+// quat.y = (s1 * c2 + s1 * c3 + c1 * s2 * s3) / w4 ;
+// quat.z = (-s1 * s3 + c1 * s2 * c3 +s2) / w4 ;
+ }
+
+
+
+
+ public static void getEuler(Quat4d quat,Vector3d euler) {
+ Vector3d e = EulerTools.getEulerFromQuat(Order.YXZ, quat);
+ euler.x = e.y;
+ euler.y = e.x;
+ euler.z = e.z;
+
+ // http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Conversion_formulae_between_formalisms
+// euler.x = Math.atan2(quat.x * quat.z + quat.y* quat.w, quat.y*quat.z - quat.x * quat.w);
+// euler.y = Math.acos(-square(quat.x) - square(quat.y) + square(quat.z) + square(quat.w));
+// euler.z = -Math.atan2(quat.x * quat.z - quat.y* quat.w, quat.y*quat.z + quat.x * quat.w);
+
+ // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
+ // Y, Z, X order
+// double test = quat.x * quat.y + quat.z * quat.w;
+// if (test > NEAR_HALF) {
+// euler.y = 2.0 * Math.atan2(quat.x,quat.w);
+// euler.z = Math.PI * 0.5;
+// euler.x = 0.0;
+// } else if (test < -NEAR_HALF) {
+// euler.y = -2.0 * Math.atan2(quat.x,quat.w);
+// euler.z = -Math.PI * 0.5;
+// euler.x = 0.0;
+// } else {
+// double sqx = square(quat.x);
+// double sqy = square(quat.y);
+// double sqz = square(quat.z);
+// euler.y = Math.atan2(2.0*(quat.y*quat.w-quat.x*quat.z), 1.0 - 2.0*(sqy-sqz));
+// euler.z = Math.asin(2.0*test);
+// euler.x = Math.atan2(2.0*(quat.x*quat.w-quat.y*quat.z), 1.0 - 2.0*(sqx-sqz));
+// System.out.println(euler + " " + EulerTools.getEulerFromQuat(Order.YXZ, quat) + " " + quat);
+// }
+// double sqw = quat.w*quat.w;
+// double sqx = quat.x*quat.x;
+// double sqy = quat.y*quat.y;
+// double sqz = quat.z*quat.z;
+// double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
+// double test = quat.x*quat.y + quat.z*quat.w;
+// if (test > 0.499*unit) { // singularity at north pole
+// euler.y = 2 * Math.atan2(quat.x,quat.w);
+// euler.z = Math.PI/2;
+// euler.x = 0;
+// return;
+// }
+// if (test < -0.499*unit) { // singularity at south pole
+// euler.y = -2 * Math.atan2(quat.x,quat.w);
+// euler.z = -Math.PI/2;
+// euler.x = 0;
+// return;
+// }
+// euler.y = Math.atan2(2*quat.y*quat.w-2*quat.x*quat.z , sqx - sqy - sqz + sqw);
+// euler.z = Math.asin(2*test/unit);
+// euler.x = Math.atan2(2*quat.x*quat.w-2*quat.y*quat.z , -sqx + sqy - sqz + sqw);
+ }
+
+ public static Quat4d getQuat(Vector3d euler) {
+ Quat4d q = new Quat4d();
+ getQuat(euler,q);
+ return q;
+ }
+
+
+ public static Vector3d getEuler(Quat4d quat) {
+ Vector3d v = new Vector3d();
+ getEuler(quat, v);
+ return v;
+ }
+
+ public static Quat4d getQuat(AxisAngle4d aa) {
+ Quat4d q = new Quat4d();
+ getQuat(aa, q);
+ return q;
+ }
+
+ public static AxisAngle4d getAxisAngle(Quat4d q) {
+ AxisAngle4d aa = new AxisAngle4d();
+ double mag = q.x * q.x + q.y * q.y + q.z * q.z;
+
+ if (mag > EPS) {
+ mag = Math.sqrt(mag);
+ aa.angle = 2.0 * Math.atan2(mag, q.w);
+ mag = 1.0 / mag;
+ aa.x = q.x * mag;
+ aa.y = q.y * mag;
+ aa.z = q.z * mag;
+
+ } else {
+ aa.x = 0.0;
+ aa.y = 1.0;
+ aa.z = 0.0;
+ aa.angle = 0.0;
+ }
+ // aa.set(q);
+ return aa;
+ }
+
+ public static Quat4d getIdentityQuat() {
+ return new Quat4d(0, 0, 0, 1);
+ }
+
+ public static void getQuat(AxisAngle4d aa, Quat4d q) {
+ double mag,amag;
+ // Quat = cos(theta/2) + sin(theta/2)(roation_axis)
+
+ amag = Math.sqrt( aa.x*aa.x + aa.y*aa.y + aa.z*aa.z);
+ if( amag < NEAR_ZERO ) {
+ q.w = 1.0;
+ q.x = 0.0;
+ q.y = 0.0;
+ q.z = 0.0;
+ } else {
+ amag = 1.0/amag;
+ double a2 = aa.angle * 0.5;
+ mag = Math.sin(a2);
+ q.w = Math.cos(a2);
+ q.x = aa.x*amag*mag;
+ q.y = aa.y*amag*mag;
+ q.z = aa.z*amag*mag;
+ }
+ }
+
+
+ /*
+ * Cohen-Sutherland
+ */
+
+ private static final int IN = 0;
+ private static final int LEFT = 1;
+ private static final int RIGHT = 2;
+ private static final int BOTTOM = 4;
+ private static final int TOP = 8;
+
+
+ private static int bitcode(Vector2f p1, Vector2f min, Vector2f max) {
+ int code = IN;
+ if (p1.x < min.x)
+ code |= LEFT;
+ else if (p1.x > max.x)
+ code |= RIGHT;
+ if (p1.y < min.y)
+ code |= BOTTOM;
+ else if (p1.y > max.y)
+ code |= TOP;
+ return code;
+ }
+
+ public static boolean clipLineRectangle(Vector2f p1,Vector2f p2, Vector2f min, Vector2f max, Vector2f r1, Vector2f r2) {
+ while (true) {
+ int o1 = bitcode(p1, min, max);
+ int o2 = bitcode(p2, min, max);
+ int and = o1 & o2;
+ int or = o1 | o2;
+ if (and != IN) {
+ return false;
+ }
+ if (or == IN) {
+ r1.set(p1);
+ r2.set(p2);
+ return true;
+ }
+ if (o1 == IN) {
+ Vector2f t = p1;
+ p1 = p2;
+ p2 = t;
+ int t2 = o1;
+ o1 = o2;
+ o2 = t2;
+ }
+ if ((o1 & TOP) != IN) {
+ float t = (max.y - p1.y) / (p2.y - p1.y);
+ p1.x += t * (p2.x - p1.x);
+ p1.y = max.y;
+ } else if ((o1 & BOTTOM) != IN) {
+ float t = (min.y - p1.y) / (p2.y - p1.y);
+ p1.x += t * (p2.x - p1.x);
+ p1.y = min.y;
+ } else if ((o1 & LEFT) != IN) {
+ float t = (min.x - p1.x) / (p2.x - p1.x);
+ p1.y += t * (p2.y - p1.y);
+ p1.x = min.x;
+ } else if ((o1 & RIGHT) != IN) {
+ float t = (max.x - p1.x) / (p2.x - p1.x);
+ p1.y += t * (p2.y - p1.y);
+ p1.x = max.x;
+ } else {
+ throw new RuntimeException("Error in clipping code");
+ }
+ }
+
+ }
+
+ public static double square(double d) {
+ return d * d;
+ }
+
+
+ public static void multiplyOrientation(AxisAngle4d aa, AxisAngle4d rot) {
+ Quat4d q1 = new Quat4d();
+ getQuat(aa, q1);
+ Quat4d q2 = new Quat4d();
+ getQuat(rot, q2);
+ q2.mul(q1);
+ rot.set(q2);
+ }
+
+ public static double radToDeg(double rad) {
+ return (rad / Math.PI) * 180.0;
+ }
+
+ public static double degToRad(double deg) {
+ return (deg / 180.0) * Math.PI;
+ }
+
+ public static double clamp(double min, double max,double v) {
+ if (v < min)
+ return min;
+ if (v > max)
+ return max;
+ return v;
+ }
+
+ public static AxisAngle4d createRotation(Vector3d original, Vector3d rotated) {
+ AxisAngle4d result = new AxisAngle4d();
+ if (createRotation(original, rotated, result))
+ return result;
+ return null;
+ }
+
+
+ public static void setIdentity(Quat4d q) {
+ q.w = 1.0;
+ q.x = 0.0;
+ q.y = 0.0;
+ q.z = 0.0;
+ }
+
+ public static void setIdentity(AxisAngle4d aa) {
+ aa.angle = 0.0;
+ aa.x = 0.0;
+ aa.y = 1.0;
+ aa.z = 0.0;
+ }
+
+ public static void set(Matrix3d mat, double m00, double m01, double m02,
+ double m10, double m11, double m12, double m20, double m21,
+ double m22) {
+ mat.m00 = m00;
+ mat.m01 = m01;
+ mat.m02 = m02;
+
+ mat.m10 = m10;
+ mat.m11 = m11;
+ mat.m12 = m12;
+
+ mat.m20 = m20;
+ mat.m21 = m21;
+ mat.m22 = m22;
+ }
+
+ public static void set(Matrix4d mat, double[] v) {
+ mat.m00 = v[0];
+ mat.m01 = v[1];
+ mat.m02 = v[2];
+ mat.m03 = v[3];
+
+ mat.m10 = v[4];
+ mat.m11 = v[5];
+ mat.m12 = v[6];
+ mat.m13 = v[7];
+
+ mat.m20 = v[8];
+ mat.m21 = v[9];
+ mat.m22 = v[10];
+ mat.m23 = v[11];
+
+ mat.m30 = v[12];
+ mat.m31 = v[13];
+ mat.m32 = v[14];
+ mat.m33 = v[15];
+
+ }
+
+ public static boolean createRotation(Vector3d original, Vector3d rotated, AxisAngle4d result) {
+
+ if (rotated.lengthSquared() > 0.01)
+ rotated.normalize();
+ else
+ return false;
+ double d = original.dot(rotated);
+ if (d > 0.9999) {
+ // original and rotated are parallel, pointing at the same direction
+ result.angle = 0.0;
+ result.x = 0.0;
+ result.y = 1.0;
+ result.z = 0.0;
+ } else if (d < -0.9999) {
+ // original and rotated are parallel, pointing at the opposite direction
+ Vector3d a = Z_AXIS;
+ if (Math.abs(a.dot(original)) > 0.8 )
+ a = Y_AXIS;
+ result.set(a, Math.PI);
+ } else {
+ double angle = original.angle(rotated);
+ Vector3d axis = new Vector3d();
+ axis.cross(original, rotated);
+ result.set(axis,angle);
+ }
+ return true;
+ }
+
+ public static boolean createRotation(Vector3d original, Vector3d rotated, Quat4d result) {
+
+ if (rotated.lengthSquared() > 0.01)
+ rotated.normalize();
+ else
+ return false;
+ double d = original.dot(rotated);
+ if (d > 0.9999) {
+ // original and rotated are parallel, pointing at the same direction
+ result.w = 1.0;
+ result.x = 0.0;
+ result.y = 0.0;
+ result.z = 0.0;
+ } else if (d < -0.9999) {
+ // original and rotated are parallel, pointing at the opposite direction
+ Vector3d a = Z_AXIS;
+ if (Math.abs(a.dot(original)) > 0.8 )
+ a = Y_AXIS;
+ getQuat(a, Math.PI, result);
+
+ } else {
+ double angle = original.angle(rotated);
+ Vector3d axis = new Vector3d();
+ axis.cross(original, rotated);
+ getQuat(axis, angle, result);
+ }
+ return true;
+ }
+
+ public static boolean createRotation(Vector3d original, Vector3d rotated, Vector3d axis, AxisAngle4d result) {
+
+ if (rotated.lengthSquared() > 0.01)
+ rotated.normalize();
+ else
+ return false;
+ if (original.lengthSquared() > 0.01)
+ original.normalize();
+ else
+ return false;
+ if (axis.lengthSquared() > 0.01)
+ axis.normalize();
+ else
+ return false;
+ double d = original.dot(rotated);
+ if (d > 0.9999) {
+ // original and rotated are parallel, pointing at the same direction
+ result.angle = 0.0;
+ result.x = axis.x;
+ result.y = axis.y;
+ result.z = axis.z;
+ } else if (d < -0.9999) {
+ // original and rotated are parallel, pointing at the opposite direction
+ result.angle = Math.PI;
+ result.x = axis.x;
+ result.y = axis.y;
+ result.z = axis.z;
+ } else {
+ // Project vectors to Axis plane
+ Vector3d p1 = projectToPlane(original, axis);
+ Vector3d p2 = projectToPlane(rotated, axis);
+ // Create vectors where z-axis is plane normal
+ Quat4d q = getQuat(createRotation(axis, Z_AXIS));
+ Vector3d t1 = new Vector3d();
+ Vector3d t2 = new Vector3d();
+ rotate(q, p1, t1);
+ rotate(q, p2, t2);
+ // Calculate angles on z-axis plane.
+ double a1 = Math.atan2(t1.y, t1.x);
+ double a2 = Math.atan2(t2.y, t2.x);
+ result.set(axis,a2-a1);
+
+ }
+ return true;
+ }
+
+ public static void getQuat(Vector3d axis, double angle, Quat4d q)
+ {
+ double mag,amag;
+ // Quat = cos(theta/2) + sin(theta/2)(roation_axis)
+
+ amag = Math.sqrt( axis.x*axis.x + axis.y*axis.y + axis.z*axis.z);
+ if( amag < EPS ) {
+ q.w = 1.0;
+ q.x = 0.0;
+ q.y = 0.0;
+ q.z = 0.0;
+ } else {
+ amag = 1.0/amag;
+ double a2 = angle*0.5;
+ mag = Math.sin(a2);
+ q.w = Math.cos(a2);
+ q.x = axis.x*amag*mag;
+ q.y = axis.y*amag*mag;
+ q.z = axis.z*amag*mag;
+ }
+
+ }
+
+ /**
+ * Linear interpolation of quaternions. Result IS set to q1.
+ * @param q1
+ * @param q2
+ * @param alpha
+ */
+ public static void lip(Quat4d q1, Quat4d q2, double alpha) {
+ double s1 = 1.0 - alpha;
+ double s2 = alpha;
+ q1.scale(s1);
+ mad(q1,q2,s2);
+ q1.normalize();
+ }
+
+ public static double dot(Quat4d q1, Quat4d q2) {
+ return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
+ }
+
+ public static void mad(Tuple3d q1, Tuple3d q2, double s2) {
+ q1.x += q2.x * s2;
+ q1.y += q2.y * s2;
+ q1.z += q2.z * s2;
+ }
+
+ public static void mad(Quat4d q1, Quat4d q2, double s2) {
+ q1.x += q2.x * s2;
+ q1.y += q2.y * s2;
+ q1.z += q2.z * s2;
+ q1.w += q2.w * s2;
+ }
+
+ /**
+ * Slerp
+ *
+ * Sets results to q1. Modifies q2.
+ *
+ * @param q1
+ * @param q2
+ * @param alpha
+ */
+ public static void sip(Quat4d q1, Quat4d q2, double alpha) {
+ double cosom = dot(q1,q2);
+ if (cosom < 0.0) {
+ cosom = -cosom;
+ q2.negate();
+ }
+
+ if (cosom > 0.9999) {
+ q2.sub(q1);
+ q2.scale(alpha);
+ q1.add(q2);
+ q1.normalize();
+ return;
+ }
+ double theta_0 = Math.acos(cosom);
+ double theta = theta_0 * alpha;
+ Quat4d t = new Quat4d(q1);
+ t.scale(-cosom);
+ t.add(q2);
+ t.normalize();
+ t.scale(Math.sin(theta));
+ q1.scale(Math.cos(theta));
+ q1.add(t);
+ }
+
+
+ public static void rotate(double angle, Tuple2d v1, Tuple2d v2) {
+ // TODO : verify implementation
+ double sin = Math.sin(angle);
+ if (sin == 1.0) {
+ v2.x = v1.y;
+ v2.y = -v1.x;
+ } else if (sin == -1.0) {
+ v2.x = -v1.y;
+ v2.y = v1.x;
+ } else {
+ double cos = Math.cos(angle);
+ if (cos == -1.0) {
+ v2.x = -v1.x;
+ v2.y = -v1.y;
+ } else if (cos != 1.0) {
+ v2.x= v1.x * cos + v1.y * -sin;
+ v2.y= v1.x* sin + v1.y *cos;
+ }
+ }
+ }
+
+ public static Tuple3d getPosRot(double m3x2[]) {
+ Vector3d t = new Vector3d();
+ t.x = m3x2[4];
+ t.y = m3x2[5];
+
+
+ Vector2d v2 = new Vector2d(1,0);
+ Vector2d v = new Vector2d();
+ // use rotation of (1,0) to calculate the rotation component
+ v.x = m3x2[0];
+ v.y = m3x2[2];
+ double a1 = v2.angle(v);
+ if (v.y < 0) {
+ t.z = a1;
+ } else {
+ t.z = Math.PI*2.0 - a1;
+ }
+ return t;
+ }
+
+ public static Matrix4d glFrustum(double l, double r, double b, double t, double n, double f) {
+ Matrix4d mat = new Matrix4d();
+ mat.m00 = 2.0 * n / (r - l);
+ mat.m11 = 2.0 * n / (t - b);
+ mat.m02 = (r+l) / (r-l);
+ mat.m12 = (t+b) / (t-b);
+ mat.m22 = -(f+n) / (f-n);
+ mat.m23 = -(2.0 *f * n) / (f-n);
+ mat.m32 = -1.0;
+ return mat;
+ }
+
+ public static Matrix4d glOrtho(double l, double r, double b, double t, double n, double f) {
+ Matrix4d mat = new Matrix4d();
+ mat.m00 = 2.0 / (r - l);
+ mat.m11 = 2.0 / (t - b);
+ mat.m22 = -2.0 / (f-n);
+ mat.m33 = 1.0;
+ mat.m03 = -(r+l)/(r-l);
+ mat.m13 = -(t+b)/(t-b);
+ mat.m23 = -(f+n)/(f-n);
+ return mat;
+ }
+}
Code Review / simantics / 3d.git / blobdiff