1 /*******************************************************************************
\r
2 * Copyright (c) 2007 VTT Technical Research Centre of Finland and others.
\r
3 * All rights reserved. This program and the accompanying materials
\r
4 * are made available under the terms of the Eclipse Public License v1.0
\r
5 * which accompanies this distribution, and is available at
\r
6 * http://www.eclipse.org/legal/epl-v10.html
\r
9 * VTT Technical Research Centre of Finland - initial API and implementation
\r
10 *******************************************************************************/
\r
11 package org.simantics.proconf.g3d.base;
\r
13 import javax.vecmath.AxisAngle4d;
\r
14 import javax.vecmath.Matrix3d;
\r
15 import javax.vecmath.Point3d;
\r
16 import javax.vecmath.Quat4d;
\r
17 import javax.vecmath.Tuple3d;
\r
18 import javax.vecmath.Vector3d;
\r
20 import com.jme.math.Vector2f;
\r
23 * Some useful geometry related math functions. Beware, methods may modify their input parameters!
\r
25 * @author Marko Luukkainen
\r
28 public class MathTools {
\r
30 static double EPS = 0.001;
\r
32 public static Vector3d closestPointOnEdge(Vector3d point, Vector3d edgePoint1, Vector3d edgePoint2) {
\r
33 point.sub(edgePoint1);
\r
34 Vector3d v = new Vector3d(edgePoint2);
\r
36 double t = v.dot(point);
\r
37 t /= v.lengthSquared();
\r
47 public static Vector3d closestPointOnStraight(Point3d point, Point3d straightPoint, Vector3d straightDir) {
\r
48 Vector3d v = new Vector3d(point);
\r
49 v.sub(straightPoint);
\r
50 double t = straightDir.dot(v);
\r
51 t /= straightDir.lengthSquared();
\r
54 v.add(straightPoint);
\r
58 public static Vector3d closestPointOnStraight(Point3d point, Point3d straightPoint, Vector3d straightDir, double u[]) {
\r
59 Vector3d v = new Vector3d(point);
\r
60 v.sub(straightPoint);
\r
61 u[0] = straightDir.dot(v);
\r
62 u[0] /= straightDir.lengthSquared();
\r
65 v.add(straightPoint);
\r
69 public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, Tuple3d planePoint) {
\r
70 point.sub(planePoint);
\r
72 return planeNormal.dot(point);
\r
75 public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, float d) {
\r
76 return (planeNormal.dot(point) + d);
\r
79 public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Tuple3d intersectPoint) {
\r
80 intersectPoint.set(planePoint);
\r
81 intersectPoint.sub(linePoint);
\r
82 double u = planeNormal.dot(new Vector3d(intersectPoint));
\r
83 double v = planeNormal.dot(lineDir);
\r
84 if (Math.abs(v) < EPS)
\r
87 intersectPoint.set(lineDir);
\r
88 intersectPoint.scale(u);
\r
89 intersectPoint.add(linePoint);
\r
93 public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Vector3d intersectPoint, double[] u) {
\r
94 intersectPoint.set(planePoint);
\r
95 intersectPoint.sub(linePoint);
\r
96 u[0] = planeNormal.dot(intersectPoint);
\r
97 double v = planeNormal.dot(lineDir);
\r
98 if (Math.abs(v) < EPS)
\r
101 intersectPoint.set(lineDir);
\r
102 intersectPoint.scale(u[0]);
\r
103 intersectPoint.add(linePoint);
\r
107 public static boolean intersectLineLine(Vector3d p1,Vector3d p2,Vector3d p3,Vector3d p4,Vector3d pa,Vector3d pb) {
\r
108 Vector3d p13 = new Vector3d();
\r
109 Vector3d p43 = new Vector3d();
\r
110 Vector3d p21 = new Vector3d();
\r
111 double d1343,d4321,d1321,d4343,d2121;
\r
112 double numer,denom;
\r
115 if (Math.abs(p43.x) < EPS && Math.abs(p43.y) < EPS && Math.abs(p43.z) < EPS)
\r
118 if (Math.abs(p21.x) < EPS && Math.abs(p21.y) < EPS && Math.abs(p21.z) < EPS)
\r
121 d1343 = p13.dot(p43);
\r
122 d4321 = p43.dot(p21);
\r
123 d1321 = p13.dot(p21);
\r
124 d4343 = p43.lengthSquared();
\r
125 d2121 = p21.lengthSquared();
\r
127 denom = d2121 * d4343 - d4321 * d4321;
\r
128 if (Math.abs(denom) < EPS)
\r
130 numer = d1343 * d4321 - d1321 * d4343;
\r
132 double mua = numer / denom;
\r
133 double mub = (d1343 + d4321 * mua) / d4343;
\r
135 pa.x = p1.x + mua * p21.x;
\r
136 pa.y = p1.y + mua * p21.y;
\r
137 pa.z = p1.z + mua * p21.z;
\r
138 pb.x = p3.x + mub * p43.x;
\r
139 pb.y = p3.y + mub * p43.y;
\r
140 pb.z = p3.z + mub * p43.z;
\r
145 public static boolean intersectStraightStraight(Vector3d p1,Vector3d p21,Vector3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb) {
\r
146 Vector3d p13 = new Vector3d();
\r
148 double d1343,d4321,d1321,d4343,d2121;
\r
149 double numer,denom;
\r
152 if (Math.abs(p43.x) < EPS && Math.abs(p43.y) < EPS && Math.abs(p43.z) < EPS)
\r
154 if (Math.abs(p21.x) < EPS && Math.abs(p21.y) < EPS && Math.abs(p21.z) < EPS)
\r
157 d1343 = p13.dot(p43);
\r
158 d4321 = p43.dot(p21);
\r
159 d1321 = p13.dot(p21);
\r
160 d4343 = p43.lengthSquared();
\r
161 d2121 = p21.lengthSquared();
\r
163 denom = d2121 * d4343 - d4321 * d4321;
\r
164 if (Math.abs(denom) < EPS)
\r
166 numer = d1343 * d4321 - d1321 * d4343;
\r
168 double mua = numer / denom;
\r
169 double mub = (d1343 + d4321 * mua) / d4343;
\r
171 pa.x = p1.x + mua * p21.x;
\r
172 pa.y = p1.y + mua * p21.y;
\r
173 pa.z = p1.z + mua * p21.z;
\r
174 pb.x = p3.x + mub * p43.x;
\r
175 pb.y = p3.y + mub * p43.y;
\r
176 pb.z = p3.z + mub * p43.z;
\r
182 * Calculate the line segment PaPb that is the shortest route between
\r
183 * two lines P1P2 and P3P4. Calculate also the values of mua and mub where
\r
184 * Pa = P1 + mua (P2 - P1)
\r
185 * Pb = P3 + mub (P4 - P3)
\r
195 public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb, double mu[]) {
\r
196 Vector3d p13 = new Vector3d();
\r
198 double d1343,d4321,d1321,d4343,d2121;
\r
199 double numer,denom;
\r
200 double EPS = 0.001;
\r
202 if (Math.abs(p43.x) < EPS && Math.abs(p43.y) < EPS && Math.abs(p43.z) < EPS)
\r
204 if (Math.abs(p21.x) < EPS && Math.abs(p21.y) < EPS && Math.abs(p21.z) < EPS)
\r
207 d1343 = p13.dot(p43);
\r
208 d4321 = p43.dot(p21);
\r
209 d1321 = p13.dot(p21);
\r
210 d4343 = p43.lengthSquared();
\r
211 d2121 = p21.lengthSquared();
\r
213 denom = d2121 * d4343 - d4321 * d4321;
\r
214 if (Math.abs(denom) < EPS)
\r
216 numer = d1343 * d4321 - d1321 * d4343;
\r
218 mu[0] = numer / denom;
\r
219 mu[1] = (d1343 + d4321 * mu[0]) / d4343;
\r
221 pa.x = p1.x + mu[0] * p21.x;
\r
222 pa.y = p1.y + mu[0] * p21.y;
\r
223 pa.z = p1.z + mu[0] * p21.z;
\r
224 pb.x = p3.x + mu[1] * p43.x;
\r
225 pb.y = p3.y + mu[1] * p43.y;
\r
226 pb.z = p3.z + mu[1] * p43.z;
\r
231 public static AxisAngle4d getFromEuler2(Vector3d euler) {
\r
232 AxisAngle4d aa = new AxisAngle4d();
\r
233 aa.angle = euler.length();
\r
234 Vector3d normal = new Vector3d(euler);
\r
235 if (aa.angle > EPS) {
\r
236 normal.normalize();
\r
249 public static Vector3d getEuler(AxisAngle4d aa) {
\r
250 Vector3d euler = new Vector3d(aa.x,aa.y,aa.z);
\r
251 euler.scale(aa.angle);
\r
255 public static void rotate(Quat4d q, Tuple3d in, Tuple3d out) {
\r
257 double tw = - q.x*in.x - q.y*in.y - q.z*in.z;
\r
258 double tx = q.w*in.x + q.y*in.z - q.z*in.y;
\r
259 double ty = q.w*in.y - q.x*in.z + q.z*in.x;
\r
260 double tz = q.w*in.z + q.x*in.y - q.y*in.x ;
\r
262 //temp * q' -> x = -x, y = -y z = -z
\r
263 //out.w = tw*q.w + tx*q.x + ty*q.y + tz*q.z;
\r
264 out.x = -tw*q.x + tx*q.w - ty*q.z + tz*q.y;
\r
265 out.y = -tw*q.y + tx*q.z + ty*q.w - tz*q.x;
\r
266 out.z = -tw*q.z - tx*q.y + ty*q.x + tz*q.w;
\r
269 public static void getMatrix(Quat4d quat, Matrix3d m) {
\r
270 m.m00 = 1.0f - 2.0 * (quat.y * quat.y + quat.z * quat.z);
\r
271 m.m01 = 2.0 * (quat.x * quat.y + quat.w * quat.z);
\r
272 m.m02 = 2.0 * (quat.x * quat.z - quat.w * quat.y);
\r
273 m.m10 = 2.0 * (quat.x * quat.y - quat.w * quat.z);
\r
274 m.m11 = 1.0 - 2.0f * (quat.x * quat.x + quat.z * quat.z);
\r
275 m.m12 = 2.0 * (quat.y * quat.z + quat.w * quat.x);
\r
276 m.m20 = 2.0 * (quat.x * quat.z + quat.w * quat.y);
\r
277 m.m21 = 2.0 * (quat.y * quat.z - quat.w * quat.x);
\r
278 m.m22 = 1.0 - 2.0f * (quat.x * quat.x + quat.y * quat.y);
\r
282 public static void getQuat(Matrix3d mat, Quat4d quat) {
\r
283 double tr = mat.m00 + mat.m11 + mat.m22;
\r
285 double s = Math.sqrt(tr + 1.0);
\r
288 quat.x = (mat.m21 - mat.m12) * s;
\r
289 quat.y = (mat.m02 - mat.m20) * s;
\r
290 quat.z = (mat.m10 - mat.m01) * s;
\r
293 if (mat.m11 > mat.m00)
\r
295 if (mat.m22 > mat.getElement(i, i))
\r
297 int nxt[] = { 1, 2, 0 };
\r
303 .sqrt((mat.getElement(i, i) - (mat.getElement(j, j) + mat
\r
304 .getElement(k, k))) + 1.0);
\r
306 double q[] = new double[3];
\r
309 if (Math.abs(s) > 0.001)
\r
312 quat.w = (mat.getElement(k, j) - mat.getElement(j, k)) * s;
\r
313 q[j] = (mat.getElement(j, i) + mat.getElement(i, j)) * s;
\r
314 q[k] = (mat.getElement(k, i) + mat.getElement(i, k)) * s;
\r
327 private static final int IN = 0;
\r
328 private static final int LEFT = 1;
\r
329 private static final int RIGHT = 2;
\r
330 private static final int BOTTOM = 4;
\r
331 private static final int TOP = 8;
\r
334 private static int bitcode(Vector2f p1, Vector2f min, Vector2f max) {
\r
338 else if (p1.x > max.x)
\r
342 else if (p1.y > max.y)
\r
347 public static boolean clipLineRectangle(Vector2f p1,Vector2f p2, Vector2f min, Vector2f max, Vector2f r1, Vector2f r2) {
\r
349 int o1 = bitcode(p1, min, max);
\r
350 int o2 = bitcode(p2, min, max);
\r
369 if ((o1 & TOP) != IN) {
\r
370 float t = (max.y - p1.y) / (p2.y - p1.y);
\r
371 p1.x += t * (p2.x - p1.x);
\r
373 } else if ((o1 & BOTTOM) != IN) {
\r
374 float t = (min.y - p1.y) / (p2.y - p1.y);
\r
375 p1.x += t * (p2.x - p1.x);
\r
377 } else if ((o1 & LEFT) != IN) {
\r
378 float t = (min.x - p1.x) / (p2.x - p1.x);
\r
379 p1.y += t * (p2.y - p1.y);
\r
381 } else if ((o1 & RIGHT) != IN) {
\r
382 float t = (max.x - p1.x) / (p2.x - p1.x);
\r
383 p1.y += t * (p2.y - p1.y);
\r
386 throw new RuntimeException("Error in clipping code");
\r
392 public static double square(double d) {
\r