1 /*
2 *
3 * Mathematics Subpackage (VrMath)
4 *
5 *
6 * Author: Samuel R. Buss, sbuss@ucsd.edu.
7 * Web page: http://math.ucsd.edu/~sbuss/MathCG
8 *
9 *
10 This software is provided 'as-is', without any express or implied warranty.
11 In no event will the authors be held liable for any damages arising from the use of this software.
12 Permission is granted to anyone to use this software for any purpose,
13 including commercial applications, and to alter it and redistribute it freely,
14 subject to the following restrictions:
15
16 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
18 3. This notice may not be removed or altered from any source distribution.
19 *
20 *
21 */
22
23 #include "MathMisc.h"
24 #include "LinearR3.h"
25 #include "Spherical.h"
26
27 // ******************************************************
28 // * VectorR3 class - math library functions *
29 // * * * * * * * * * * * * * * * * * * * * * * * * * * **
30
31 const VectorR3 UnitVecIR3(1.0, 0.0, 0.0);
32 const VectorR3 UnitVecJR3(0.0, 1.0, 0.0);
33 const VectorR3 UnitVecKR3(0.0, 0.0, 1.0);
34
35 const VectorR3 VectorR3::Zero(0.0, 0.0, 0.0);
36 const VectorR3 VectorR3::UnitX(1.0, 0.0, 0.0);
37 const VectorR3 VectorR3::UnitY(0.0, 1.0, 0.0);
38 const VectorR3 VectorR3::UnitZ(0.0, 0.0, 1.0);
39 const VectorR3 VectorR3::NegUnitX(-1.0, 0.0, 0.0);
40 const VectorR3 VectorR3::NegUnitY(0.0, -1.0, 0.0);
41 const VectorR3 VectorR3::NegUnitZ(0.0, 0.0, -1.0);
42
43 const Matrix3x3 Matrix3x3::Identity(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
44 const Matrix3x4 Matrix3x4::Identity(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0);
45
MaxAbs() const46 double VectorR3::MaxAbs() const
47 {
48 double m;
49 m = (x > 0.0) ? x : -x;
50 if (y > m)
51 m = y;
52 else if (-y > m)
53 m = -y;
54 if (z > m)
55 m = z;
56 else if (-z > m)
57 m = -z;
58 return m;
59 }
60
Set(const Quaternion & q)61 VectorR3& VectorR3::Set(const Quaternion& q)
62 {
63 double sinhalf = sqrt(Square(q.x) + Square(q.y) + Square(q.z));
64 if (sinhalf > 0.0)
65 {
66 double theta = atan2(sinhalf, q.w);
67 theta += theta;
68 this->Set(q.x, q.y, q.z);
69 (*this) *= (theta / sinhalf);
70 }
71 else
72 {
73 this->SetZero();
74 }
75 return *this;
76 }
77
78 // *********************************************************************
79 // Rotation routines *
80 // *********************************************************************
81
82 // s.Rotate(theta, u) rotates s and returns s
83 // rotated theta degrees around unit vector w.
Rotate(double theta,const VectorR3 & w)84 VectorR3& VectorR3::Rotate(double theta, const VectorR3& w)
85 {
86 double c = cos(theta);
87 double s = sin(theta);
88 double dotw = (x * w.x + y * w.y + z * w.z);
89 double v0x = dotw * w.x;
90 double v0y = dotw * w.y; // v0 = provjection onto w
91 double v0z = dotw * w.z;
92 double v1x = x - v0x;
93 double v1y = y - v0y; // v1 = projection onto plane normal to w
94 double v1z = z - v0z;
95 double v2x = w.y * v1z - w.z * v1y;
96 double v2y = w.z * v1x - w.x * v1z; // v2 = w * v1 (cross product)
97 double v2z = w.x * v1y - w.y * v1x;
98
99 x = v0x + c * v1x + s * v2x;
100 y = v0y + c * v1y + s * v2y;
101 z = v0z + c * v1z + s * v2z;
102
103 return (*this);
104 }
105
106 // Rotate unit vector x in the direction of "dir": length of dir is rotation angle.
107 // x must be a unit vector. dir must be perpindicular to x.
RotateUnitInDirection(const VectorR3 & dir)108 VectorR3& VectorR3::RotateUnitInDirection(const VectorR3& dir)
109 {
110 double theta = dir.NormSq();
111 if (theta == 0.0)
112 {
113 return *this;
114 }
115 else
116 {
117 theta = sqrt(theta);
118 double costheta = cos(theta);
119 double sintheta = sin(theta);
120 VectorR3 dirUnit = dir / theta;
121 *this = costheta * (*this) + sintheta * dirUnit;
122 return (*this);
123 }
124 }
125
126 // ******************************************************
127 // * Matrix3x3 class - math library functions *
128 // * * * * * * * * * * * * * * * * * * * * * * * * * * **
129
ReNormalize()130 Matrix3x3& Matrix3x3::ReNormalize() // Re-normalizes nearly orthonormal matrix
131 {
132 double alpha = m11 * m11 + m21 * m21 + m31 * m31; // First column's norm squared
133 double beta = m12 * m12 + m22 * m22 + m32 * m32; // Second column's norm squared
134 double gamma = m13 * m13 + m23 * m23 + m33 * m33; // Third column's norm squared
135 alpha = 1.0 - 0.5 * (alpha - 1.0); // Get mult. factor
136 beta = 1.0 - 0.5 * (beta - 1.0);
137 gamma = 1.0 - 0.5 * (gamma - 1.0);
138 m11 *= alpha; // Renormalize first column
139 m21 *= alpha;
140 m31 *= alpha;
141 m12 *= beta; // Renormalize second column
142 m22 *= beta;
143 m32 *= beta;
144 m13 *= gamma;
145 m23 *= gamma;
146 m33 *= gamma;
147 alpha = m11 * m12 + m21 * m22 + m31 * m32; // First and second column dot product
148 beta = m11 * m13 + m21 * m23 + m31 * m33; // First and third column dot product
149 gamma = m12 * m13 + m22 * m23 + m32 * m33; // Second and third column dot product
150 alpha *= 0.5;
151 beta *= 0.5;
152 gamma *= 0.5;
153 double temp1, temp2;
154 temp1 = m11 - alpha * m12 - beta * m13; // Update row1
155 temp2 = m12 - alpha * m11 - gamma * m13;
156 m13 -= beta * m11 + gamma * m12;
157 m11 = temp1;
158 m12 = temp2;
159 temp1 = m21 - alpha * m22 - beta * m23; // Update row2
160 temp2 = m22 - alpha * m21 - gamma * m23;
161 m23 -= beta * m21 + gamma * m22;
162 m21 = temp1;
163 m22 = temp2;
164 temp1 = m31 - alpha * m32 - beta * m33; // Update row3
165 temp2 = m32 - alpha * m31 - gamma * m33;
166 m33 -= beta * m31 + gamma * m32;
167 m31 = temp1;
168 m32 = temp2;
169 return *this;
170 }
171
OperatorTimesEquals(const Matrix3x3 & B)172 void Matrix3x3::OperatorTimesEquals(const Matrix3x3& B) // Matrix product
173 {
174 double t1, t2; // temporary values
175 t1 = m11 * B.m11 + m12 * B.m21 + m13 * B.m31;
176 t2 = m11 * B.m12 + m12 * B.m22 + m13 * B.m32;
177 m13 = m11 * B.m13 + m12 * B.m23 + m13 * B.m33;
178 m11 = t1;
179 m12 = t2;
180
181 t1 = m21 * B.m11 + m22 * B.m21 + m23 * B.m31;
182 t2 = m21 * B.m12 + m22 * B.m22 + m23 * B.m32;
183 m23 = m21 * B.m13 + m22 * B.m23 + m23 * B.m33;
184 m21 = t1;
185 m22 = t2;
186
187 t1 = m31 * B.m11 + m32 * B.m21 + m33 * B.m31;
188 t2 = m31 * B.m12 + m32 * B.m22 + m33 * B.m32;
189 m33 = m31 * B.m13 + m32 * B.m23 + m33 * B.m33;
190 m31 = t1;
191 m32 = t2;
192 return;
193 }
194
Solve(const VectorR3 & u) const195 VectorR3 Matrix3x3::Solve(const VectorR3& u) const // Returns solution
196 { // based on Cramer's rule
197 double sd11 = m22 * m33 - m23 * m32;
198 double sd21 = m32 * m13 - m12 * m33;
199 double sd31 = m12 * m23 - m22 * m13;
200 double sd12 = m31 * m23 - m21 * m33;
201 double sd22 = m11 * m33 - m31 * m13;
202 double sd32 = m21 * m13 - m11 * m23;
203 double sd13 = m21 * m32 - m31 * m22;
204 double sd23 = m31 * m12 - m11 * m32;
205 double sd33 = m11 * m22 - m21 * m12;
206
207 double detInv = 1.0 / (m11 * sd11 + m12 * sd12 + m13 * sd13);
208
209 double rx = (u.x * sd11 + u.y * sd21 + u.z * sd31) * detInv;
210 double ry = (u.x * sd12 + u.y * sd22 + u.z * sd32) * detInv;
211 double rz = (u.x * sd13 + u.y * sd23 + u.z * sd33) * detInv;
212
213 return (VectorR3(rx, ry, rz));
214 }
215
216 // ******************************************************
217 // * Matrix3x4 class - math library functions *
218 // * * * * * * * * * * * * * * * * * * * * * * * * * * **
219
ReNormalize()220 Matrix3x4& Matrix3x4::ReNormalize() // Re-normalizes nearly orthonormal matrix
221 {
222 double alpha = m11 * m11 + m21 * m21 + m31 * m31; // First column's norm squared
223 double beta = m12 * m12 + m22 * m22 + m32 * m32; // Second column's norm squared
224 double gamma = m13 * m13 + m23 * m23 + m33 * m33; // Third column's norm squared
225 alpha = 1.0 - 0.5 * (alpha - 1.0); // Get mult. factor
226 beta = 1.0 - 0.5 * (beta - 1.0);
227 gamma = 1.0 - 0.5 * (gamma - 1.0);
228 m11 *= alpha; // Renormalize first column
229 m21 *= alpha;
230 m31 *= alpha;
231 m12 *= beta; // Renormalize second column
232 m22 *= beta;
233 m32 *= beta;
234 m13 *= gamma;
235 m23 *= gamma;
236 m33 *= gamma;
237 alpha = m11 * m12 + m21 * m22 + m31 * m32; // First and second column dot product
238 beta = m11 * m13 + m21 * m23 + m31 * m33; // First and third column dot product
239 gamma = m12 * m13 + m22 * m23 + m32 * m33; // Second and third column dot product
240 alpha *= 0.5;
241 beta *= 0.5;
242 gamma *= 0.5;
243 double temp1, temp2;
244 temp1 = m11 - alpha * m12 - beta * m13; // Update row1
245 temp2 = m12 - alpha * m11 - gamma * m13;
246 m13 -= beta * m11 + gamma * m12;
247 m11 = temp1;
248 m12 = temp2;
249 temp1 = m21 - alpha * m22 - beta * m23; // Update row2
250 temp2 = m22 - alpha * m21 - gamma * m23;
251 m23 -= beta * m21 + gamma * m22;
252 m21 = temp1;
253 m22 = temp2;
254 temp1 = m31 - alpha * m32 - beta * m33; // Update row3
255 temp2 = m32 - alpha * m31 - gamma * m33;
256 m33 -= beta * m31 + gamma * m32;
257 m31 = temp1;
258 m32 = temp2;
259 return *this;
260 }
261
OperatorTimesEquals(const Matrix3x4 & B)262 void Matrix3x4::OperatorTimesEquals(const Matrix3x4& B) // Composition
263 {
264 m14 += m11 * B.m14 + m12 * B.m24 + m13 * B.m34;
265 m24 += m21 * B.m14 + m22 * B.m24 + m23 * B.m34;
266 m34 += m31 * B.m14 + m32 * B.m24 + m33 * B.m34;
267
268 double t1, t2; // temporary values
269 t1 = m11 * B.m11 + m12 * B.m21 + m13 * B.m31;
270 t2 = m11 * B.m12 + m12 * B.m22 + m13 * B.m32;
271 m13 = m11 * B.m13 + m12 * B.m23 + m13 * B.m33;
272 m11 = t1;
273 m12 = t2;
274
275 t1 = m21 * B.m11 + m22 * B.m21 + m23 * B.m31;
276 t2 = m21 * B.m12 + m22 * B.m22 + m23 * B.m32;
277 m23 = m21 * B.m13 + m22 * B.m23 + m23 * B.m33;
278 m21 = t1;
279 m22 = t2;
280
281 t1 = m31 * B.m11 + m32 * B.m21 + m33 * B.m31;
282 t2 = m31 * B.m12 + m32 * B.m22 + m33 * B.m32;
283 m33 = m31 * B.m13 + m32 * B.m23 + m33 * B.m33;
284 m31 = t1;
285 m32 = t2;
286 }
287
OperatorTimesEquals(const Matrix3x3 & B)288 void Matrix3x4::OperatorTimesEquals(const Matrix3x3& B) // Composition
289 {
290 double t1, t2; // temporary values
291 t1 = m11 * B.m11 + m12 * B.m21 + m13 * B.m31;
292 t2 = m11 * B.m12 + m12 * B.m22 + m13 * B.m32;
293 m13 = m11 * B.m13 + m12 * B.m23 + m13 * B.m33;
294 m11 = t1;
295 m12 = t2;
296
297 t1 = m21 * B.m11 + m22 * B.m21 + m23 * B.m31;
298 t2 = m21 * B.m12 + m22 * B.m22 + m23 * B.m32;
299 m23 = m21 * B.m13 + m22 * B.m23 + m23 * B.m33;
300 m21 = t1;
301 m22 = t2;
302
303 t1 = m31 * B.m11 + m32 * B.m21 + m33 * B.m31;
304 t2 = m31 * B.m12 + m32 * B.m22 + m33 * B.m32;
305 m33 = m31 * B.m13 + m32 * B.m23 + m33 * B.m33;
306 m31 = t1;
307 m32 = t2;
308 }
309
310 // ******************************************************
311 // * LinearMapR3 class - math library functions *
312 // * * * * * * * * * * * * * * * * * * * * * * * * * * **
313
operator *(const LinearMapR3 & A,const LinearMapR3 & B)314 LinearMapR3 operator*(const LinearMapR3& A, const LinearMapR3& B)
315 {
316 return (LinearMapR3(A.m11 * B.m11 + A.m12 * B.m21 + A.m13 * B.m31,
317 A.m21 * B.m11 + A.m22 * B.m21 + A.m23 * B.m31,
318 A.m31 * B.m11 + A.m32 * B.m21 + A.m33 * B.m31,
319 A.m11 * B.m12 + A.m12 * B.m22 + A.m13 * B.m32,
320 A.m21 * B.m12 + A.m22 * B.m22 + A.m23 * B.m32,
321 A.m31 * B.m12 + A.m32 * B.m22 + A.m33 * B.m32,
322 A.m11 * B.m13 + A.m12 * B.m23 + A.m13 * B.m33,
323 A.m21 * B.m13 + A.m22 * B.m23 + A.m23 * B.m33,
324 A.m31 * B.m13 + A.m32 * B.m23 + A.m33 * B.m33));
325 }
326
Determinant() const327 double LinearMapR3::Determinant() const // Returns the determinant
328 {
329 return (m11 * (m22 * m33 - m23 * m32) - m12 * (m21 * m33 - m31 * m23) + m13 * (m21 * m23 - m31 * m22));
330 }
331
Inverse() const332 LinearMapR3 LinearMapR3::Inverse() const // Returns inverse
333 {
334 double sd11 = m22 * m33 - m23 * m32;
335 double sd21 = m32 * m13 - m12 * m33;
336 double sd31 = m12 * m23 - m22 * m13;
337 double sd12 = m31 * m23 - m21 * m33;
338 double sd22 = m11 * m33 - m31 * m13;
339 double sd32 = m21 * m13 - m11 * m23;
340 double sd13 = m21 * m32 - m31 * m22;
341 double sd23 = m31 * m12 - m11 * m32;
342 double sd33 = m11 * m22 - m21 * m12;
343
344 double detInv = 1.0 / (m11 * sd11 + m12 * sd12 + m13 * sd13);
345
346 return (LinearMapR3(sd11 * detInv, sd12 * detInv, sd13 * detInv,
347 sd21 * detInv, sd22 * detInv, sd23 * detInv,
348 sd31 * detInv, sd32 * detInv, sd33 * detInv));
349 }
350
Invert()351 LinearMapR3& LinearMapR3::Invert() // Converts into inverse.
352 {
353 double sd11 = m22 * m33 - m23 * m32;
354 double sd21 = m32 * m13 - m12 * m33;
355 double sd31 = m12 * m23 - m22 * m13;
356 double sd12 = m31 * m23 - m21 * m33;
357 double sd22 = m11 * m33 - m31 * m13;
358 double sd32 = m21 * m13 - m11 * m23;
359 double sd13 = m21 * m32 - m31 * m22;
360 double sd23 = m31 * m12 - m11 * m32;
361 double sd33 = m11 * m22 - m21 * m12;
362
363 double detInv = 1.0 / (m11 * sd11 + m12 * sd12 + m13 * sd13);
364
365 m11 = sd11 * detInv;
366 m12 = sd21 * detInv;
367 m13 = sd31 * detInv;
368 m21 = sd12 * detInv;
369 m22 = sd22 * detInv;
370 m23 = sd32 * detInv;
371 m31 = sd13 * detInv;
372 m32 = sd23 * detInv;
373 m33 = sd33 * detInv;
374
375 return (*this);
376 }
377
378 // ******************************************************
379 // * AffineMapR3 class - math library functions *
380 // * * * * * * * * * * * * * * * * * * * * * * * * * * **
381
operator *(const AffineMapR3 & A,const AffineMapR3 & B)382 AffineMapR3 operator*(const AffineMapR3& A, const AffineMapR3& B)
383 {
384 return (AffineMapR3(A.m11 * B.m11 + A.m12 * B.m21 + A.m13 * B.m31,
385 A.m21 * B.m11 + A.m22 * B.m21 + A.m23 * B.m31,
386 A.m31 * B.m11 + A.m32 * B.m21 + A.m33 * B.m31,
387 A.m11 * B.m12 + A.m12 * B.m22 + A.m13 * B.m32,
388 A.m21 * B.m12 + A.m22 * B.m22 + A.m23 * B.m32,
389 A.m31 * B.m12 + A.m32 * B.m22 + A.m33 * B.m32,
390 A.m11 * B.m13 + A.m12 * B.m23 + A.m13 * B.m33,
391 A.m21 * B.m13 + A.m22 * B.m23 + A.m23 * B.m33,
392 A.m31 * B.m13 + A.m32 * B.m23 + A.m33 * B.m33,
393 A.m11 * B.m14 + A.m12 * B.m24 + A.m13 * B.m34 + A.m14,
394 A.m21 * B.m14 + A.m22 * B.m24 + A.m23 * B.m34 + A.m24,
395 A.m31 * B.m14 + A.m32 * B.m24 + A.m33 * B.m34 + A.m34));
396 }
397
operator *(const LinearMapR3 & A,const AffineMapR3 & B)398 AffineMapR3 operator*(const LinearMapR3& A, const AffineMapR3& B)
399 {
400 return (AffineMapR3(A.m11 * B.m11 + A.m12 * B.m21 + A.m13 * B.m31,
401 A.m21 * B.m11 + A.m22 * B.m21 + A.m23 * B.m31,
402 A.m31 * B.m11 + A.m32 * B.m21 + A.m33 * B.m31,
403 A.m11 * B.m12 + A.m12 * B.m22 + A.m13 * B.m32,
404 A.m21 * B.m12 + A.m22 * B.m22 + A.m23 * B.m32,
405 A.m31 * B.m12 + A.m32 * B.m22 + A.m33 * B.m32,
406 A.m11 * B.m13 + A.m12 * B.m23 + A.m13 * B.m33,
407 A.m21 * B.m13 + A.m22 * B.m23 + A.m23 * B.m33,
408 A.m31 * B.m13 + A.m32 * B.m23 + A.m33 * B.m33,
409 A.m11 * B.m14 + A.m12 * B.m24 + A.m13 * B.m34,
410 A.m21 * B.m14 + A.m22 * B.m24 + A.m23 * B.m34,
411 A.m31 * B.m14 + A.m32 * B.m24 + A.m33 * B.m34));
412 }
413
operator *(const AffineMapR3 & A,const LinearMapR3 & B)414 AffineMapR3 operator*(const AffineMapR3& A, const LinearMapR3& B)
415 {
416 return (AffineMapR3(A.m11 * B.m11 + A.m12 * B.m21 + A.m13 * B.m31,
417 A.m21 * B.m11 + A.m22 * B.m21 + A.m23 * B.m31,
418 A.m31 * B.m11 + A.m32 * B.m21 + A.m33 * B.m31,
419 A.m11 * B.m12 + A.m12 * B.m22 + A.m13 * B.m32,
420 A.m21 * B.m12 + A.m22 * B.m22 + A.m23 * B.m32,
421 A.m31 * B.m12 + A.m32 * B.m22 + A.m33 * B.m32,
422 A.m11 * B.m13 + A.m12 * B.m23 + A.m13 * B.m33,
423 A.m21 * B.m13 + A.m22 * B.m23 + A.m23 * B.m33,
424 A.m31 * B.m13 + A.m32 * B.m23 + A.m33 * B.m33,
425 A.m14,
426 A.m24,
427 A.m34));
428 }
429
Inverse() const430 AffineMapR3 AffineMapR3::Inverse() const // Returns inverse
431 {
432 double sd11 = m22 * m33 - m23 * m32;
433 double sd21 = m32 * m13 - m12 * m33;
434 double sd31 = m12 * m23 - m22 * m13;
435 double sd12 = m31 * m23 - m21 * m33;
436 double sd22 = m11 * m33 - m31 * m13;
437 double sd32 = m21 * m13 - m11 * m23;
438 double sd13 = m21 * m32 - m31 * m22;
439 double sd23 = m31 * m12 - m11 * m32;
440 double sd33 = m11 * m22 - m21 * m12;
441
442 double detInv = 1.0 / (m11 * sd11 + m12 * sd12 + m13 * sd13);
443
444 // Make sd's hold the (transpose of) the inverse of the 3x3 part
445 sd11 *= detInv;
446 sd12 *= detInv;
447 sd13 *= detInv;
448 sd21 *= detInv;
449 sd22 *= detInv;
450 sd23 *= detInv;
451 sd31 *= detInv;
452 sd32 *= detInv;
453 sd33 *= detInv;
454 double sd41 = -(m14 * sd11 + m24 * sd21 + m34 * sd31);
455 double sd42 = -(m14 * sd12 + m24 * sd22 + m34 * sd32);
456 double sd43 = -(m14 * sd12 + m24 * sd23 + m34 * sd33);
457
458 return (AffineMapR3(sd11, sd12, sd13,
459 sd21, sd22, sd23,
460 sd31, sd32, sd33,
461 sd41, sd42, sd43));
462 }
463
Invert()464 AffineMapR3& AffineMapR3::Invert() // Converts into inverse.
465 {
466 double sd11 = m22 * m33 - m23 * m32;
467 double sd21 = m32 * m13 - m12 * m33;
468 double sd31 = m12 * m23 - m22 * m13;
469 double sd12 = m31 * m23 - m21 * m33;
470 double sd22 = m11 * m33 - m31 * m13;
471 double sd32 = m21 * m13 - m11 * m23;
472 double sd13 = m21 * m32 - m31 * m22;
473 double sd23 = m31 * m12 - m11 * m32;
474 double sd33 = m11 * m22 - m21 * m12;
475
476 double detInv = 1.0 / (m11 * sd11 + m12 * sd12 + m13 * sd13);
477
478 m11 = sd11 * detInv;
479 m12 = sd21 * detInv;
480 m13 = sd31 * detInv;
481 m21 = sd12 * detInv;
482 m22 = sd22 * detInv;
483 m23 = sd32 * detInv;
484 m31 = sd13 * detInv;
485 m32 = sd23 * detInv;
486 m33 = sd33 * detInv;
487 double sd41 = -(m14 * m11 + m24 * m12 + m34 * m13); // Compare to ::Inverse.
488 double sd42 = -(m14 * m21 + m24 * m22 + m34 * m23);
489 double sd43 = -(m14 * m31 + m24 * m32 + m34 * m33);
490 m14 = sd41;
491 m24 = sd42;
492 m34 = sd43;
493
494 return (*this);
495 }
496
497 // **************************************************************
498 // * RotationMapR3 class - math library functions *
499 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * **
500
operator *(const RotationMapR3 & A,const RotationMapR3 & B)501 RotationMapR3 operator*(const RotationMapR3& A, const RotationMapR3& B)
502 // Matrix product (composition)
503 {
504 return (RotationMapR3(A.m11 * B.m11 + A.m12 * B.m21 + A.m13 * B.m31,
505 A.m21 * B.m11 + A.m22 * B.m21 + A.m23 * B.m31,
506 A.m31 * B.m11 + A.m32 * B.m21 + A.m33 * B.m31,
507 A.m11 * B.m12 + A.m12 * B.m22 + A.m13 * B.m32,
508 A.m21 * B.m12 + A.m22 * B.m22 + A.m23 * B.m32,
509 A.m31 * B.m12 + A.m32 * B.m22 + A.m33 * B.m32,
510 A.m11 * B.m13 + A.m12 * B.m23 + A.m13 * B.m33,
511 A.m21 * B.m13 + A.m22 * B.m23 + A.m23 * B.m33,
512 A.m31 * B.m13 + A.m32 * B.m23 + A.m33 * B.m33));
513 }
514
Set(const Quaternion & quat)515 RotationMapR3& RotationMapR3::Set(const Quaternion& quat)
516 {
517 double wSq = quat.w * quat.w;
518 double xSq = quat.x * quat.x;
519 double ySq = quat.y * quat.y;
520 double zSq = quat.z * quat.z;
521 double Dqwx = 2.0 * quat.w * quat.x;
522 double Dqwy = 2.0 * quat.w * quat.y;
523 double Dqwz = 2.0 * quat.w * quat.z;
524 double Dqxy = 2.0 * quat.x * quat.y;
525 double Dqyz = 2.0 * quat.y * quat.z;
526 double Dqxz = 2.0 * quat.x * quat.z;
527 m11 = wSq + xSq - ySq - zSq;
528 m22 = wSq - xSq + ySq - zSq;
529 m33 = wSq - xSq - ySq + zSq;
530 m12 = Dqxy - Dqwz;
531 m21 = Dqxy + Dqwz;
532 m13 = Dqxz + Dqwy;
533 m31 = Dqxz - Dqwy;
534 m23 = Dqyz - Dqwx;
535 m32 = Dqyz + Dqwx;
536 return *this;
537 }
538
Set(const VectorR3 & u,double theta)539 RotationMapR3& RotationMapR3::Set(const VectorR3& u, double theta)
540 {
541 assert(fabs(u.NormSq() - 1.0) < 2.0e-6);
542 double c = cos(theta);
543 double s = sin(theta);
544 double mc = 1.0 - c;
545 double xmc = u.x * mc;
546 double xymc = xmc * u.y;
547 double xzmc = xmc * u.z;
548 double yzmc = u.y * u.z * mc;
549 double xs = u.x * s;
550 double ys = u.y * s;
551 double zs = u.z * s;
552 Matrix3x3::Set(u.x * u.x * mc + c, xymc + zs, xzmc - ys,
553 xymc - zs, u.y * u.y * mc + c, yzmc + xs,
554 xzmc + ys, yzmc - xs, u.z * u.z * mc + c);
555 return *this;
556 }
557
558 // The rotation axis vector u MUST be a UNIT vector!!!
Set(const VectorR3 & u,double s,double c)559 RotationMapR3& RotationMapR3::Set(const VectorR3& u, double s, double c)
560 {
561 assert(fabs(u.NormSq() - 1.0) < 2.0e-6);
562 double mc = 1.0 - c;
563 double xmc = u.x * mc;
564 double xymc = xmc * u.y;
565 double xzmc = xmc * u.z;
566 double yzmc = u.y * u.z * mc;
567 double xs = u.x * s;
568 double ys = u.y * s;
569 double zs = u.z * s;
570 Matrix3x3::Set(u.x * u.x * mc + c, xymc + zs, xzmc - ys,
571 xymc - zs, u.y * u.y * mc + c, yzmc + xs,
572 xzmc + ys, yzmc - xs, u.z * u.z * mc + c);
573 return *this;
574 }
575
576 // ToAxisAndAngle - find a unit vector in the direction of the rotation axis,
577 // and the angle theta of rotation. Returns true if the rotation angle is non-zero,
578 // and false if it is zero.
ToAxisAndAngle(VectorR3 * u,double * theta) const579 bool RotationMapR3::ToAxisAndAngle(VectorR3* u, double* theta) const
580 {
581 double alpha = m11 + m22 + m33 - 1.0;
582 double beta = sqrt(Square(m32 - m23) + Square(m13 - m31) + Square(m21 - m12));
583 if (beta == 0.0)
584 {
585 *u = VectorR3::UnitY;
586 *theta = 0.0;
587 return false;
588 }
589 else
590 {
591 u->Set(m32 - m23, m13 - m31, m21 - m12);
592 *u /= beta;
593 *theta = atan2(beta, alpha);
594 return true;
595 }
596 }
597
598 // VrRotate is similar to glRotate. Returns a matrix (LinearMapR3)
599 // that will perform the rotation when multiplied on the left.
600 // u is supposed to be a *unit* vector. Otherwise, the LinearMapR3
601 // returned will be garbage!
VrRotate(double theta,const VectorR3 & u)602 RotationMapR3 VrRotate(double theta, const VectorR3& u)
603 {
604 RotationMapR3 ret;
605 ret.Set(u, theta);
606 return ret;
607 }
608
609 // This version of rotate takes the cosine and sine of theta
610 // instead of theta. u must still be a unit vector.
611
VrRotate(double c,double s,const VectorR3 & u)612 RotationMapR3 VrRotate(double c, double s, const VectorR3& u)
613 {
614 RotationMapR3 ret;
615 ret.Set(u, s, c);
616 return ret;
617 }
618
619 // Returns a RotationMapR3 which rotates the fromVec to be colinear
620 // with toVec.
621
VrRotateAlign(const VectorR3 & fromVec,const VectorR3 & toVec)622 RotationMapR3 VrRotateAlign(const VectorR3& fromVec, const VectorR3& toVec)
623 {
624 VectorR3 crossVec = fromVec;
625 crossVec *= toVec;
626 double sinetheta = crossVec.Norm(); // Not yet normalized!
627 if (sinetheta < 1.0e-40)
628 {
629 return (RotationMapR3(
630 1.0, 0.0, 0.0,
631 0.0, 1.0, 0.0,
632 0.0, 0.0, 1.0));
633 }
634 crossVec /= sinetheta; // Normalize the vector
635 double scale = 1.0 / sqrt(fromVec.NormSq() * toVec.NormSq());
636 sinetheta *= scale;
637 double cosinetheta = (fromVec ^ toVec) * scale;
638 return (VrRotate(cosinetheta, sinetheta, crossVec));
639 }
640
641 // RotateToMap returns a rotation map which rotates fromVec to have the
642 // same direction as toVec.
643 // fromVec and toVec should be unit vectors
RotateToMap(const VectorR3 & fromVec,const VectorR3 & toVec)644 RotationMapR3 RotateToMap(const VectorR3& fromVec, const VectorR3& toVec)
645 {
646 VectorR3 crossVec = fromVec;
647 crossVec *= toVec;
648 double sintheta = crossVec.Norm();
649 double costheta = fromVec ^ toVec;
650 if (sintheta <= 1.0e-40)
651 {
652 if (costheta > 0.0)
653 {
654 return (RotationMapR3(
655 1.0, 0.0, 0.0,
656 0.0, 1.0, 0.0,
657 0.0, 0.0, 1.0));
658 }
659 else
660 {
661 GetOrtho(toVec, crossVec); // Get arbitrary orthonormal vector
662 return (VrRotate(costheta, sintheta, crossVec));
663 }
664 }
665 else
666 {
667 crossVec /= sintheta; // Normalize the vector
668 return (VrRotate(costheta, sintheta, crossVec));
669 }
670 }
671
672 // **************************************************************
673 // * RigidMapR3 class - math library functions *
674 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * **
675
676 // The rotation axis vector u MUST be a UNIT vector!!!
SetRotationPart(const VectorR3 & u,double theta)677 RigidMapR3& RigidMapR3::SetRotationPart(const VectorR3& u, double theta)
678 {
679 assert(fabs(u.NormSq() - 1.0) < 2.0e-6);
680 double c = cos(theta);
681 double s = sin(theta);
682 double mc = 1.0 - c;
683 double xmc = u.x * mc;
684 double xymc = xmc * u.y;
685 double xzmc = xmc * u.z;
686 double yzmc = u.y * u.z * mc;
687 double xs = u.x * s;
688 double ys = u.y * s;
689 double zs = u.z * s;
690 Matrix3x4::Set3x3(u.x * u.x * mc + c, xymc + zs, xzmc - ys,
691 xymc - zs, u.y * u.y * mc + c, yzmc + xs,
692 xzmc + ys, yzmc - xs, u.z * u.z * mc + c);
693 return *this;
694 }
695
696 // The rotation axis vector u MUST be a UNIT vector!!!
SetRotationPart(const VectorR3 & u,double s,double c)697 RigidMapR3& RigidMapR3::SetRotationPart(const VectorR3& u, double s, double c)
698 {
699 assert(fabs(u.NormSq() - 1.0) < 2.0e-6);
700 double mc = 1.0 - c;
701 double xmc = u.x * mc;
702 double xymc = xmc * u.y;
703 double xzmc = xmc * u.z;
704 double yzmc = u.y * u.z * mc;
705 double xs = u.x * s;
706 double ys = u.y * s;
707 double zs = u.z * s;
708 Matrix3x4::Set3x3(u.x * u.x * mc + c, xymc + zs, xzmc - ys,
709 xymc - zs, u.y * u.y * mc + c, yzmc + xs,
710 xzmc + ys, yzmc - xs, u.z * u.z * mc + c);
711 return *this;
712 }
713
714 // CalcGlideRotation - converts a rigid map into an equivalent
715 // glide rotation (screw motion). It returns the rotation axis
716 // as base point u, and a rotation axis v. The vectors u and v are
717 // always orthonormal. v will be a unit vector.
718 // It also returns the glide distance, which is the translation parallel
719 // to v. Further, it returns the signed rotation angle theta (right hand rule
720 // specifies the direction.
721 // The glide rotation means a rotation around the point u with axis direction v.
722 // Return code "true" if the rotation amount is non-zero. "false" if pure translation.
CalcGlideRotation(VectorR3 * u,VectorR3 * v,double * glideDist,double * rotation) const723 bool RigidMapR3::CalcGlideRotation(VectorR3* u, VectorR3* v,
724 double* glideDist, double* rotation) const
725 {
726 // Compare to the code for ToAxisAndAngle.
727 double alpha = m11 + m22 + m33 - 1.0;
728 double beta = sqrt(Square(m32 - m23) + Square(m13 - m31) + Square(m21 - m12));
729 if (beta == 0.0)
730 {
731 double vN = m14 * m14 + m24 * m24 + m34 * m34;
732 if (vN > 0.0)
733 {
734 vN = sqrt(vN);
735 v->Set(m14, m24, m34);
736 *v /= vN;
737 *glideDist = vN;
738 }
739 else
740 {
741 *v = VectorR3::UnitX;
742 *glideDist = 0.0;
743 }
744 u->SetZero();
745 *rotation = 0;
746 return false;
747 }
748 else
749 {
750 v->Set(m32 - m23, m13 - m31, m21 - m12);
751 *v /= beta; // v - unit vector, rotation axis
752 *rotation = atan2(beta, alpha);
753 u->Set(m14, m24, m34);
754 *glideDist = ((*u) ^ (*v));
755 VectorR3 temp = *v;
756 temp *= *glideDist;
757 *u -= temp; // Subtract component in direction of rot. axis v
758 temp = *v;
759 temp *= *u;
760 temp /= tan((*rotation) / 2); // temp = (v \times u) / tan(rotation/2)
761 *u += temp;
762 *u *= 0.5;
763 return true;
764 }
765 }
766
767 // ***************************************************************
768 // Linear Algebra Utilities *
769 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
770
771 // Returns a righthanded orthonormal basis to complement vector u
GetOrtho(const VectorR3 & u,VectorR3 & v,VectorR3 & w)772 void GetOrtho(const VectorR3& u, VectorR3& v, VectorR3& w)
773 {
774 if (u.x > 0.5 || u.x < -0.5 || u.y > 0.5 || u.y < -0.5)
775 {
776 v.Set(u.y, -u.x, 0.0);
777 }
778 else
779 {
780 v.Set(0.0, u.z, -u.y);
781 }
782 v.Normalize();
783 w = u;
784 w *= v;
785 w.Normalize();
786 // w.NormalizeFast();
787 return;
788 }
789
790 // Returns a vector v orthonormal to unit vector u
GetOrtho(const VectorR3 & u,VectorR3 & v)791 void GetOrtho(const VectorR3& u, VectorR3& v)
792 {
793 if (u.x > 0.5 || u.x < -0.5 || u.y > 0.5 || u.y < -0.5)
794 {
795 v.Set(u.y, -u.x, 0.0);
796 }
797 else
798 {
799 v.Set(0.0, u.z, -u.y);
800 }
801 v.Normalize();
802 return;
803 }
804
805 // ***************************************************************
806 // Stream Output Routines *
807 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
808
operator <<(ostream & os,const VectorR3 & u)809 ostream& operator<<(ostream& os, const VectorR3& u)
810 {
811 return (os << "<" << u.x << "," << u.y << "," << u.z << ">");
812 }
813
operator <<(ostream & os,const Matrix3x3 & A)814 ostream& operator<<(ostream& os, const Matrix3x3& A)
815 {
816 os << " <" << A.m11 << ", " << A.m12 << ", " << A.m13 << ">\n"
817 << " <" << A.m21 << ", " << A.m22 << ", " << A.m23 << ">\n"
818 << " <" << A.m31 << ", " << A.m32 << ", " << A.m33 << ">\n";
819 return (os);
820 }
821
operator <<(ostream & os,const Matrix3x4 & A)822 ostream& operator<<(ostream& os, const Matrix3x4& A)
823 {
824 os << " <" << A.m11 << ", " << A.m12 << ", " << A.m13
825 << "; " << A.m14 << ">\n"
826 << " <" << A.m21 << ", " << A.m22 << ", " << A.m23
827 << "; " << A.m24 << ">\n"
828 << " <" << A.m31 << ", " << A.m32 << ", " << A.m33
829 << "; " << A.m34 << ">\n";
830 return (os);
831 }
832