X-Git-Url: https://gerrit.simantics.org/r/gitweb?a=blobdiff_plain;f=org.simantics.g3d%2Fsrc%2Forg%2Fsimantics%2Fg3d%2Fmath%2FMathTools.java;h=d761b0ec8350450e8f45829fc8446ffb823bff01;hb=eb67906ff85a83b2f71b823110e5c3d12da8bfc2;hp=92d954f4b0375ce5ff855baafb13ff54bcbb5384;hpb=289aaab900078ef56efc8779e4b15830e472149e;p=simantics%2F3d.git diff --git a/org.simantics.g3d/src/org/simantics/g3d/math/MathTools.java b/org.simantics.g3d/src/org/simantics/g3d/math/MathTools.java index 92d954f4..d761b0ec 100644 --- a/org.simantics.g3d/src/org/simantics/g3d/math/MathTools.java +++ b/org.simantics.g3d/src/org/simantics/g3d/math/MathTools.java @@ -1,860 +1,1004 @@ -/******************************************************************************* - * 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.Tuple3d; -import javax.vecmath.Tuple4d; -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.0000001; - public static double NEAR_HALF = 0.4999999; - - 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 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); - - } - - - 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(); - 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 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); - } -} +/******************************************************************************* + * 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 { + Vector3d p1 = projectToPlane(original, axis); + Vector3d p2 = projectToPlane(rotated, axis); + double angle = p1.angle(p2); + result.set(axis,angle); + + } + 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; + } +}