1 /***
2 * Virtual Mockup for Machine Vision Copyright (C) 2001-2003 Fabio R. de
3 * Miranda, João E. Kogler Jr., Carlos S. Santos. Virtual Mockup for Machine
4 * Vision Project funded by SENAC-SP Permission is granted to redistribute
5 * and/or modify this software under the terms of the GNU Lesser General Public
6 * License as published by the Free Software Foundation; either version 2.1 of
7 * the License, or (at your option) any later version. This software is
8 * distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
9 * without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
10 * PARTICULAR PURPOSE. See the GNU Lesser General Public License
11 * (http://www.gnu.org/copyleft/lesser.html) for more details.
12 */
13
14 package camera3d;
15
16 import javax.vecmath.*;
17
18
19 /***
20 * Utility for performing some common vector math operations.
21 *
22 *@author Fábio Roberto de Miranda, Carlos da Silva dos Santos
23 *@created October 22, 2003
24 *@version 1.0
25 */
26 public final class MathUtility {
27
28 private static Point3d tempP3d = new Point3d();
29 private static Point3d tempP3d2 = new Point3d();
30 private static Vector3d tempVec3d = new Vector3d();
31 private static Vector3d tempVec3d2 = new Vector3d();
32
33
34 // to forbid instatiation
35 /***
36 * Constructor for the MathUtility object
37 */
38 private MathUtility() { }
39
40
41 /***
42 * Creates the description of a plane that contains the three points given
43 * as input. The input points (p1, p2 and p3) are supposed to arrive in
44 * world coordinates, and must be non colinear. The plane description is
45 * placed in the Vector4d passed in as input. A point (a,b,c) belongs to
46 * the plane if it obeys the following equation:</b> a*plane.x + b*plane.y
47 * + c*plane.z + plane.w = 0 In case it's necessary to obtain the normal to
48 * the plane, all that is required is assembling a vector with the (x,y,z)
49 * componentes of the plane Vector4d.
50 *
51 *@param plane receives the plane equation parameters.
52 *@param p1 first point belonging to the plane.
53 *@param p2 second point belonging to the plane.
54 *@param p3 third point belonging to the plane.
55 */
56 public static void buildPlaneFromPoints(Vector4d plane, Point3d p1, Point3d p2, Point3d p3) {
57 // Now we must find the plane defined by the points p1, p2 e p3
58 Vector3d u = new Vector3d(p2);
59 Vector3d v = new Vector3d(p3);
60 u.sub(p1);
61 v.sub(p2);
62 if(u.dot(v) == 0) {
63 throw new IllegalArgumentException("Cannot build plane: p1, p2 and p3 are colinear.");
64 }
65 /*
66 * The calculation goes as follows:
67 * Consider the vectors u = (p2 - p1) and v = (p3 - p1). Let P be a generic point,
68 * with the vector k = (P - p1). P will belong to the desired plane if k.(u^v) = 0 (1),
69 * where . denotes internal product and ^ denotes cross product. Developing the equation
70 * (1) will give us the following expressions:
71 */
72 plane.x = p1.y * (p2.z - p3.z) + p2.y * (p3.z - p1.z) + p3.y * (p1.z - p2.z);
73 plane.y = p1.z * (p2.x - p3.x) + p2.z * (p3.x - p1.x) + p3.z * (p1.x - p2.x);
74 plane.z = p1.x * (p2.y - p3.y) + p2.x * (p3.y - p1.y) + p3.x * (p1.y - p2.y);
75 plane.w = -(p1.x * (p2.y * p3.z - p3.y * p2.z) + p2.x * (p3.y * p1.z - p1.y * p3.z) + p3.x * (p1.y * p2.z - p2.y * p1.z));
76 }
77
78
79 /***
80 * Creates the description of a plane that contains the point given as
81 * input and whose normal is determined by the input Vector3d argument. The
82 * input points is supposed to arrive in world coordinates. The plane
83 * description is placed in the Vector4d passed in as input. A point
84 * (a,b,c) belongs to the plane if it obeys the following equation:</b>
85 * a*plane.x + b*plane.y + c*plane.z + plane.w = 0
86 *
87 *@param plane receives the plane equation parameters.
88 *@param planePoint point belonging to the plane.
89 *@param planeNormal vector normal to desired plane.
90 */
91 public static void buildPlaneFromPointAndNormal(Vector4d plane, Point3d planePoint, Vector3d planeNormal) {
92 plane.x = planeNormal.x;
93 plane.y = planeNormal.y;
94 plane.z = planeNormal.z;
95 double n = plane.x * planePoint.x + plane.y * planePoint.y + plane.z * planePoint.z;
96 plane.w = -n;
97 }
98
99
100 /***
101 * Finds the intersection point between a plane and a line. The plane is
102 * defined by a Vector4d holding the parameters of the plane equation. The
103 * line is defined by one of its points and its direction vector.
104 *
105 *@param result receives the resultant intersection point.
106 *@param linePoint point belonging to line
107 *@param lineVec line direction
108 *@param plane Vector4d equation of the plane in the form Ax + By + Cz
109 * + D = 0
110 *@return the scalar to multiply lineVector so that: scalar *
111 * lineVector + linePoint = intersection
112 */
113 public static double computeIntersection(Tuple3d result, Point3d linePoint, Vector3d lineVec, Vector4d plane) {
114 // copies linePoint parameters into auxiliary variables for
115 // the sake of brevity
116 double x1 = linePoint.x;
117 double y1 = linePoint.y;
118 double z1 = linePoint.z;
119
120 // copies plane parameters into auxiliary variables for
121 // the sake of brevity
122 double a = plane.x;
123 double b = plane.y;
124 double c = plane.z;
125 double d = plane.w;
126
127 // the intersection point we are looking for can be written as:
128 // result = linePoint + s*lineVec
129 // where s is some scalar
130 // defining linePoint = {x1,y1,z1} and lineVec = {x2,y2,z2}
131 // all we have to do is substitute result in the plane equation and resolve it:
132 // a*(x1 + s*x2) + b*(y1 + s*y2) + c*(z1 + s*z2) + d =0
133 // now we will solve the plane equation:
134 double num = a * x1 + b * y1 + c * z1 + d;
135 double den = -(a * lineVec.x + b * lineVec.y + c * lineVec.z);
136 num /= den;
137 // num is the factor to multiply the lineVector to arrive to
138 // the point we want
139
140 result.scaleAdd(num, lineVec, linePoint);
141 //result = linePoint + num * lineVec
142
143 return num;
144 }
145
146
147 /***
148 * Converts a plane in the form Ax + By + Cz + D = 0 to a plane represented
149 * by a normal and a point
150 *
151 *@param plane is the Ax + By + Cz + D representation of the plan
152 *@param planeNormal is the resulting plane normal (Bet you could've
153 * guessed that :) )
154 *@param planePoint is a point belonging to the plane
155 */
156 public static void planeToPointAndNormal(Vector4d plane, Vector3d planeNormal, Point3d planePoint) {
157 planeNormal.x = plane.x;
158 planeNormal.y = plane.y;
159 planeNormal.z = plane.z;
160
161 double x = -(plane.w / plane.x);
162 double y = -(plane.w / plane.y);
163 double z = -(plane.w / plane.w);
164
165 if(!Double.isNaN(y)) {
166 if(!Double.isInfinite(y)) {
167 planePoint.x = 0.0;
168 planePoint.z = 0.0;
169 planePoint.y = y;
170 }
171 } else {
172 if(!Double.isNaN(x)) {
173 if(!Double.isInfinite(x)) {
174 planePoint.y = 0.0;
175 planePoint.z = 0.0;
176 planePoint.x = x;
177 }
178 } else {
179 if(!Double.isNaN(z)) {
180 if(!Double.isInfinite(z)) {
181 planePoint.x = 0.0;
182 planePoint.y = 0.0;
183 planePoint.z = z;
184 }
185 }
186 }
187 }
188
189 }
190
191
192 /***
193 *@param result is the point to contain the result
194 *@param linePoint and
195 *@param lineVector are line parameters,
196 *@param planePoint and
197 *@param planeNormal are the parameters of the plane
198 *@return the scalar to multiply lineVector so that: scalar *
199 * lineVector + linePoint = intersection *
200 */
201 public static double computeIntersection(Tuple3d result, Point3d linePoint, Vector3d lineVector, Point3d planePoint, Vector3d planeNormal) {
202 // Based on lineVector and on linePoint, we compute other point that belongs to the line
203 tempP3d.set(linePoint);
204 tempP3d.add(lineVector);
205 // linePoint + factor*(tempP3d - linePoint) will be the point od intersection between the line
206 // and the plane
207 double factor;
208 tempVec3d.sub(planePoint, linePoint);
209 tempVec3d2.sub(tempP3d, linePoint);
210 double denom = planeNormal.dot(tempVec3d);
211 double div = planeNormal.dot(tempVec3d2);
212 factor = denom / div;
213
214 result.x = result.y = result.z = 0.0;
215 result.scaleAdd(factor, lineVector, linePoint);
216 //result = factor*(tempP3d-linePoint) + linePoint
217 return factor;
218 }
219
220
221 /***
222 * Description of the Method
223 *
224 *@param axis Description of the Parameter
225 *@param deltaAngle Description of the Parameter
226 *@param rotQuat Description of the Parameter
227 */
228 public static void generateRotQuat(TransformScope axis, double deltaAngle, Quat4d rotQuat) {
229 double angle = deltaAngle / 2;
230 double sin = Math.sin(angle);
231 double cos = Math.cos(angle);
232 if(axis == TransformScope.X) {
233 rotQuat.set(sin, 0, 0, cos);
234 } else if(axis == TransformScope.Y) {
235 rotQuat.set(0, sin, 0, cos);
236 } else if(axis == TransformScope.Z) {
237 rotQuat.set(0, 0, sin, cos);
238 } else {
239 // will default to X rotation
240 rotQuat.set(sin, 0, 0, cos);
241 }
242 }
243
244
245 /***
246 * Transforms euler angles into the corresponding rotation quaternion. The
247 * order in which rotations are applied is: roll(X axii)-->pitch(Y axii)-->
248 * yaw(Z axii) Extracted from Nick Bobick's Gamasutra Code.
249 *
250 *@param roll rotation around x axis (radians).
251 *@param pitch rotation around y axis (radians).
252 *@param yaw rotation around z axis (radians).
253 *@param quat Description of the Parameter
254 */
255 public static void eulerToQuat(double roll, double pitch, double yaw, Quat4d quat) {
256 double cr;
257 double cp;
258 double cy;
259 double sr;
260 double sp;
261 double sy;
262 double cpcy;
263 double spsy;
264 /*
265 * The order in which rotations are applied is:
266 * roll(X axii)-->pitch(Y axii)-->yaw(Z axii)
267 * each rotation can be represented by a quaternion, say qr,qp and qy
268 * qr = cr + sr(1,0,0)
269 * qp = cp + sp(0,1,0)
270 * qy = cy + sy(0,0,1)
271 * Notes:
272 * - the first element of each quaternion is a scalar;
273 * - we use (X,Y,Z) triple to denote a vector;
274 * - the quaternion representing the combined rotations is:
275 * q = qy*qp*qr
276 * if we represent a quaternion as n + v, where n is the scalar part and
277 * v is the vector part, the product between quaternions q1 = n1 + v1 and
278 * q2 = n2 + v2 is given by:
279 * q1*q2 = n1*n2 - v1.v2 + n1*v2 + n2*v1 + v1xv2
280 * where:
281 * -> product by an scalar
282 * . -> internal product
283 * x -> cross product
284 * thus:
285 * qy*qp = cp*cy + (-sp*sy,cy*sp,cp*sy)
286 * and:
287 * (qy*qp)*qr = cr*cp*cy - sr*sp*sy +
288 * + (sr*cp*cy - cr*sp*sy,
289 * cr*sp*cy + sr*cp*sy,
290 * cr*cp*sy - sr*sp*cy)
291 */
292 //calculate trig identities
293 cr = Math.cos(roll / 2);
294 cp = Math.cos(pitch / 2);
295 cy = Math.cos(yaw / 2);
296
297 sr = Math.sin(roll / 2);
298 sp = Math.sin(pitch / 2);
299 sy = Math.sin(yaw / 2);
300
301 cpcy = cp * cy;
302 spsy = sp * sy;
303
304 quat.w = cr * cpcy + sr * spsy;
305 quat.x = sr * cpcy - cr * spsy;
306 quat.y = cr * sp * cy + sr * cp * sy;
307 quat.z = cr * cp * sy - sr * sp * cy;
308
309 // code commented out below would perform the transformation, but now
310 // supposing the inverse order of rotation, i.e.
311 // yaw (Z axii)--> pitch (Y axii)--> roll (Z axii)
312 /*
313 * double crcp = cr * cp;
314 * double srsp = sr * sp;
315 * quat.w = crcp*cy - srsp*sy;
316 * quat.x = sr*cp*cy + cr*sp*sy;
317 * quat.y = cr*sp*cy - sr*cp*sy;
318 * quat.z = sr*sp*cy + cr*cp*sy;
319 */
320 quat.normalize();
321 }
322
323
324 }
This page was automatically generated by Maven