X-Git-Url: https://gerrit.simantics.org/r/gitweb?a=blobdiff_plain;f=org.simantics.g3d%2Fsrc%2Forg%2Fsimantics%2Fproconf%2Fg3d%2Fbase%2FMathTools.java;fp=org.simantics.g3d%2Fsrc%2Forg%2Fsimantics%2Fproconf%2Fg3d%2Fbase%2FMathTools.java;h=0000000000000000000000000000000000000000;hb=6b6fcff5d6c326feef07ccf8401f97911778fffe;hp=5a287b55fa56bfb0076a72002a71918931f2caad;hpb=504c111db40d78f4913badddd126b283b5504dbb;p=simantics%2F3d.git diff --git a/org.simantics.g3d/src/org/simantics/proconf/g3d/base/MathTools.java b/org.simantics.g3d/src/org/simantics/proconf/g3d/base/MathTools.java deleted file mode 100644 index 5a287b55..00000000 --- a/org.simantics.g3d/src/org/simantics/proconf/g3d/base/MathTools.java +++ /dev/null @@ -1,395 +0,0 @@ -/******************************************************************************* - * Copyright (c) 2007- VTT Technical Research Centre of Finland. - * 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.proconf.g3d.base; - -import javax.vecmath.AxisAngle4d; -import javax.vecmath.Matrix3d; -import javax.vecmath.Point3d; -import javax.vecmath.Quat4d; -import javax.vecmath.Tuple3d; -import javax.vecmath.Vector3d; - -import com.jme.math.Vector2f; - -/** - * Some useful geometry related math functions. Beware, methods may modify their input parameters! - * - * @author Marko Luukkainen - * - */ -public class MathTools { - - static double EPS = 0.001; - - 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(Point3d point, Point3d 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(Point3d point, Point3d 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) < EPS) - 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) < EPS) - return false; - u[0] /= v; - intersectPoint.set(lineDir); - intersectPoint.scale(u[0]); - intersectPoint.add(linePoint); - return true; - } - - public static boolean intersectLineLine(Vector3d p1,Vector3d p2,Vector3d p3,Vector3d p4,Vector3d pa,Vector3d pb) { - Vector3d p13 = new Vector3d(); - Vector3d p43 = new Vector3d(); - Vector3d p21 = new Vector3d(); - double d1343,d4321,d1321,d4343,d2121; - double numer,denom; - p13.sub(p1, p3); - p43.sub(p4,p3); - if (Math.abs(p43.x) < EPS && Math.abs(p43.y) < EPS && Math.abs(p43.z) < EPS) - return false; - p21.sub(p2,p1); - 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; - - 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; - } - - public static boolean intersectStraightStraight(Vector3d p1,Vector3d p21,Vector3d 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) < 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; - - 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 AxisAngle4d getFromEuler2(Vector3d euler) { - AxisAngle4d aa = new AxisAngle4d(); - aa.angle = euler.length(); - Vector3d normal = new Vector3d(euler); - if (aa.angle > EPS) { - 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 getEuler(AxisAngle4d aa) { - Vector3d euler = new Vector3d(aa.x,aa.y,aa.z); - euler.scale(aa.angle); - return euler; - } - - 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 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; - int nxt[] = { 1, 2, 0 }; - - j = nxt[i]; - k = nxt[j]; - - double s = Math - .sqrt((mat.getElement(i, i) - (mat.getElement(j, j) + mat - .getElement(k, k))) + 1.0); - - double q[] = new double[3]; - 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]; - } - } - - - /* - * 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; - } -}