]> Dogcows Code - chaz/yoink/blob - src/Moof/cml/mathlib/matrix_rotation.h
projects / chaz / yoink / blob
? search:
summary | shortlog | log | commit | commitdiff | tree
history | raw | HEAD
beginnings of scene rendering
[chaz/yoink] / src / Moof / cml / mathlib / matrix_rotation.h
1 /* -*- C++ -*- ------------------------------------------------------------
2
3 Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
4
5 The Configurable Math Library (CML) is distributed under the terms of the
6 Boost Software License, v1.0 (see cml/LICENSE for details).
7
8 *-----------------------------------------------------------------------*/
9 /** @file
10 * @brief
11 */
12
13 #ifndef matrix_rotation_h
14 #define matrix_rotation_h
15
16 #include <cml/mathlib/matrix_misc.h>
17 #include <cml/mathlib/vector_ortho.h>
18
19 /* Functions related to matrix rotations in 3D and 2D. */
20
21 namespace cml {
22
23 //////////////////////////////////////////////////////////////////////////////
24 // 3D rotation about world axes
25 //////////////////////////////////////////////////////////////////////////////
26
27 /** Build a matrix representing a 3D rotation about the given world axis */
28 template < typename E, class A, class B, class L > void
29 matrix_rotation_world_axis( matrix<E,A,B,L>& m, size_t axis, E angle)
30 {
31 typedef matrix<E,A,B,L> matrix_type;
32 typedef typename matrix_type::value_type value_type;
33
34 /* Checking */
35 detail::CheckMatLinear3D(m);
36 detail::CheckIndex3(axis);
37
38 size_t i, j, k;
39 cyclic_permutation(axis, i, j, k);
40
41 value_type s = value_type(std::sin(angle));
42 value_type c = value_type(std::cos(angle));
43
44 identity_transform(m);
45
46 m.set_basis_element(j,j, c);
47 m.set_basis_element(j,k, s);
48 m.set_basis_element(k,j,-s);
49 m.set_basis_element(k,k, c);
50 }
51
52 /** Build a matrix representing a 3D rotation about the world x axis */
53 template < typename E, class A, class B, class L > void
54 matrix_rotation_world_x(matrix<E,A,B,L>& m, E angle) {
55 matrix_rotation_world_axis(m,0,angle);
56 }
57
58 /** Build a matrix representing a 3D rotation about the world y axis */
59 template < typename E, class A, class B, class L > void
60 matrix_rotation_world_y(matrix<E,A,B,L>& m, E angle) {
61 matrix_rotation_world_axis(m,1,angle);
62 }
63
64 /** Build a matrix representing a 3D rotation about the world z axis */
65 template < typename E, class A, class B, class L > void
66 matrix_rotation_world_z(matrix<E,A,B,L>& m, E angle) {
67 matrix_rotation_world_axis(m,2,angle);
68 }
69
70 //////////////////////////////////////////////////////////////////////////////
71 // 3D rotation from an axis-angle pair
72 //////////////////////////////////////////////////////////////////////////////
73
74 /** Build a rotation matrix from an axis-angle pair */
75 template < typename E, class A, class B, class L, class VecT > void
76 matrix_rotation_axis_angle(matrix<E,A,B,L>& m, const VecT& axis, E angle)
77 {
78 typedef matrix<E,A,B,L> matrix_type;
79 typedef typename matrix_type::value_type value_type;
80
81 /* Checking */
82 detail::CheckMatLinear3D(m);
83 detail::CheckVec3(axis);
84
85 identity_transform(m);
86
87 value_type s = std::sin(angle);
88 value_type c = std::cos(angle);
89 value_type omc = value_type(1) - c;
90
91 value_type xomc = axis[0] * omc;
92 value_type yomc = axis[1] * omc;
93 value_type zomc = axis[2] * omc;
94
95 value_type xxomc = axis[0] * xomc;
96 value_type yyomc = axis[1] * yomc;
97 value_type zzomc = axis[2] * zomc;
98 value_type xyomc = axis[0] * yomc;
99 value_type yzomc = axis[1] * zomc;
100 value_type zxomc = axis[2] * xomc;
101
102 value_type xs = axis[0] * s;
103 value_type ys = axis[1] * s;
104 value_type zs = axis[2] * s;
105
106 m.set_basis_element(0,0, xxomc + c );
107 m.set_basis_element(0,1, xyomc + zs);
108 m.set_basis_element(0,2, zxomc - ys);
109 m.set_basis_element(1,0, xyomc - zs);
110 m.set_basis_element(1,1, yyomc + c );
111 m.set_basis_element(1,2, yzomc + xs);
112 m.set_basis_element(2,0, zxomc + ys);
113 m.set_basis_element(2,1, yzomc - xs);
114 m.set_basis_element(2,2, zzomc + c );
115 }
116
117 //////////////////////////////////////////////////////////////////////////////
118 // 3D rotation from a quaternion
119 //////////////////////////////////////////////////////////////////////////////
120
121 /** Build a rotation matrix from a quaternion */
122 template < typename E, class A, class B, class L, class QuatT > void
123 matrix_rotation_quaternion(matrix<E,A,B,L>& m, const QuatT& q)
124 {
125 typedef matrix<E,A,B,L> matrix_type;
126 typedef QuatT quaternion_type;
127 typedef typename quaternion_type::order_type order_type;
128 typedef typename matrix_type::value_type value_type;
129
130 enum {
131 W = order_type::W,
132 X = order_type::X,
133 Y = order_type::Y,
134 Z = order_type::Z
135 };
136
137 /* Checking */
138 detail::CheckMatLinear3D(m);
139 detail::CheckQuat(q);
140
141 identity_transform(m);
142
143 value_type x2 = q[X] + q[X];
144 value_type y2 = q[Y] + q[Y];
145 value_type z2 = q[Z] + q[Z];
146
147 value_type xx2 = q[X] * x2;
148 value_type yy2 = q[Y] * y2;
149 value_type zz2 = q[Z] * z2;
150 value_type xy2 = q[X] * y2;
151 value_type yz2 = q[Y] * z2;
152 value_type zx2 = q[Z] * x2;
153 value_type xw2 = q[W] * x2;
154 value_type yw2 = q[W] * y2;
155 value_type zw2 = q[W] * z2;
156
157 m.set_basis_element(0,0, value_type(1) - yy2 - zz2);
158 m.set_basis_element(0,1, xy2 + zw2);
159 m.set_basis_element(0,2, zx2 - yw2);
160 m.set_basis_element(1,0, xy2 - zw2);
161 m.set_basis_element(1,1, value_type(1) - zz2 - xx2);
162 m.set_basis_element(1,2, yz2 + xw2);
163 m.set_basis_element(2,0, zx2 + yw2);
164 m.set_basis_element(2,1, yz2 - xw2);
165 m.set_basis_element(2,2, value_type(1) - xx2 - yy2);
166 }
167
168 //////////////////////////////////////////////////////////////////////////////
169 // 3D rotation from Euler angles
170 //////////////////////////////////////////////////////////////////////////////
171
172 /** Build a rotation matrix from an Euler-angle triple
173 *
174 * The rotations are applied about the cardinal axes in the order specified by
175 * the 'order' argument, where 'order' is one of the following enumerants:
176 *
177 * euler_order_xyz
178 * euler_order_xzy
179 * euler_order_xyx
180 * euler_order_xzx
181 * euler_order_yzx
182 * euler_order_yxz
183 * euler_order_yzy
184 * euler_order_yxy
185 * euler_order_zxy
186 * euler_order_zyx
187 * euler_order_zxz
188 * euler_order_zyz
189 */
190
191 template < typename E, class A, class B, class L > void
192 matrix_rotation_euler(matrix<E,A,B,L>& m, E angle_0, E angle_1, E angle_2,
193 EulerOrder order)
194 {
195 typedef matrix<E,A,B,L> matrix_type;
196 typedef typename matrix_type::value_type value_type;
197
198 /* Checking */
199 detail::CheckMatLinear3D(m);
200
201 identity_transform(m);
202
203 size_t i, j, k;
204 bool odd, repeat;
205 detail::unpack_euler_order(order, i, j, k, odd, repeat);
206
207 if (odd) {
208 angle_0 = -angle_0;
209 angle_1 = -angle_1;
210 angle_2 = -angle_2;
211 }
212
213 value_type s0 = std::sin(angle_0);
214 value_type c0 = std::cos(angle_0);
215 value_type s1 = std::sin(angle_1);
216 value_type c1 = std::cos(angle_1);
217 value_type s2 = std::sin(angle_2);
218 value_type c2 = std::cos(angle_2);
219
220 value_type s0s2 = s0 * s2;
221 value_type s0c2 = s0 * c2;
222 value_type c0s2 = c0 * s2;
223 value_type c0c2 = c0 * c2;
224
225 if (repeat) {
226 m.set_basis_element(i,i, c1 );
227 m.set_basis_element(i,j, s1 * s2 );
228 m.set_basis_element(i,k,-s1 * c2 );
229 m.set_basis_element(j,i, s0 * s1 );
230 m.set_basis_element(j,j,-c1 * s0s2 + c0c2);
231 m.set_basis_element(j,k, c1 * s0c2 + c0s2);
232 m.set_basis_element(k,i, c0 * s1 );
233 m.set_basis_element(k,j,-c1 * c0s2 - s0c2);
234 m.set_basis_element(k,k, c1 * c0c2 - s0s2);
235 } else {
236 m.set_basis_element(i,i, c1 * c2 );
237 m.set_basis_element(i,j, c1 * s2 );
238 m.set_basis_element(i,k,-s1 );
239 m.set_basis_element(j,i, s1 * s0c2 - c0s2);
240 m.set_basis_element(j,j, s1 * s0s2 + c0c2);
241 m.set_basis_element(j,k, s0 * c1 );
242 m.set_basis_element(k,i, s1 * c0c2 + s0s2);
243 m.set_basis_element(k,j, s1 * c0s2 - s0c2);
244 m.set_basis_element(k,k, c0 * c1 );
245 }
246 }
247
248 //////////////////////////////////////////////////////////////////////////////
249 // 3D rotation to align with a vector, multiple vectors, or the view plane
250 //////////////////////////////////////////////////////////////////////////////
251
252 /** See vector_ortho.h for details */
253 template < typename E,class A,class B,class L,class VecT_1,class VecT_2 > void
254 matrix_rotation_align(
255 matrix<E,A,B,L>& m,
256 const VecT_1& align,
257 const VecT_2& reference,
258 bool normalize = true,
259 AxisOrder order = axis_order_zyx)
260 {
261 typedef vector< E,fixed<3> > vector_type;
262
263 identity_transform(m);
264
265 vector_type x, y, z;
266
267 orthonormal_basis(align, reference, x, y, z, normalize, order);
268 matrix_set_basis_vectors(m, x, y, z);
269 }
270
271 /** See vector_ortho.h for details */
272 template < typename E, class A, class B, class L, class VecT > void
273 matrix_rotation_align(matrix<E,A,B,L>& m, const VecT& align,
274 bool normalize = true, AxisOrder order = axis_order_zyx)
275 {
276 typedef vector< E,fixed<3> > vector_type;
277
278 identity_transform(m);
279
280 vector_type x, y, z;
281
282 orthonormal_basis(align, x, y, z, normalize, order);
283 matrix_set_basis_vectors(m, x, y, z);
284 }
285
286 /** See vector_ortho.h for details */
287 template < typename E,class A,class B,class L,class VecT_1,class VecT_2 > void
288 matrix_rotation_align_axial(matrix<E,A,B,L>& m, const VecT_1& align,
289 const VecT_2& axis, bool normalize = true,
290 AxisOrder order = axis_order_zyx)
291 {
292 typedef vector< E,fixed<3> > vector_type;
293
294 identity_transform(m);
295
296 vector_type x, y, z;
297
298 orthonormal_basis_axial(align, axis, x, y, z, normalize, order);
299 matrix_set_basis_vectors(m, x, y, z);
300 }
301
302 /** See vector_ortho.h for details */
303 template < typename E, class A, class B, class L, class MatT > void
304 matrix_rotation_align_viewplane(
305 matrix<E,A,B,L>& m,
306 const MatT& view_matrix,
307 Handedness handedness,
308 AxisOrder order = axis_order_zyx)
309 {
310 typedef vector< E, fixed<3> > vector_type;
311
312 identity_transform(m);
313
314 vector_type x, y, z;
315
316 orthonormal_basis_viewplane(view_matrix, x, y, z, handedness, order);
317 matrix_set_basis_vectors(m, x, y, z);
318 }
319
320 /** See vector_ortho.h for details */
321 template < typename E, class A, class B, class L, class MatT > void
322 matrix_rotation_align_viewplane_LH(
323 matrix<E,A,B,L>& m,
324 const MatT& view_matrix,
325 AxisOrder order = axis_order_zyx)
326 {
327 matrix_rotation_align_viewplane(
328 m,view_matrix,left_handed,order);
329 }
330
331 /** See vector_ortho.h for details */
332 template < typename E, class A, class B, class L, class MatT > void
333 matrix_rotation_align_viewplane_RH(
334 matrix<E,A,B,L>& m,
335 const MatT& view_matrix,
336 AxisOrder order = axis_order_zyx)
337 {
338 matrix_rotation_align_viewplane(
339 m,view_matrix,right_handed,order);
340 }
341
342 //////////////////////////////////////////////////////////////////////////////
343 // 3D rotation to aim at a target
344 //////////////////////////////////////////////////////////////////////////////
345
346 /** See vector_ortho.h for details */
347 template < typename E, class A, class B, class L,
348 class VecT_1, class VecT_2, class VecT_3 > void
349 matrix_rotation_aim_at(
350 matrix<E,A,B,L>& m,
351 const VecT_1& pos,
352 const VecT_2& target,
353 const VecT_3& reference,
354 AxisOrder order = axis_order_zyx)
355 {
356 matrix_rotation_align(m, target - pos, reference, true, order);
357 }
358
359 /** See vector_ortho.h for details */
360 template < typename E, class A, class B, class L,
361 class VecT_1, class VecT_2 > void
362 matrix_rotation_aim_at(
363 matrix<E,A,B,L>& m,
364 const VecT_1& pos,
365 const VecT_2& target,
366 AxisOrder order = axis_order_zyx)
367 {
368 matrix_rotation_align(m, target - pos, true, order);
369 }
370
371 /** See vector_ortho.h for details */
372 template < typename E, class A, class B, class L,
373 class VecT_1, class VecT_2, class VecT_3 > void
374 matrix_rotation_aim_at_axial(
375 matrix<E,A,B,L>& m,
376 const VecT_1& pos,
377 const VecT_2& target,
378 const VecT_3& axis,
379 AxisOrder order = axis_order_zyx)
380 {
381 matrix_rotation_align_axial(m, target - pos, axis, true, order);
382 }
383
384 //////////////////////////////////////////////////////////////////////////////
385 // 2D rotation
386 //////////////////////////////////////////////////////////////////////////////
387
388 /** Build a matrix representing a 2D rotation */
389 template < typename E, class A, class B, class L > void
390 matrix_rotation_2D( matrix<E,A,B,L>& m, E angle)
391 {
392 typedef matrix<E,A,B,L> matrix_type;
393 typedef typename matrix_type::value_type value_type;
394
395 /* Checking */
396 detail::CheckMatLinear2D(m);
397
398 value_type s = value_type(std::sin(angle));
399 value_type c = value_type(std::cos(angle));
400
401 identity_transform(m);
402
403 m.set_basis_element(0,0, c);
404 m.set_basis_element(0,1, s);
405 m.set_basis_element(1,0,-s);
406 m.set_basis_element(1,1, c);
407 }
408
409 //////////////////////////////////////////////////////////////////////////////
410 // 2D rotation to align with a vector
411 //////////////////////////////////////////////////////////////////////////////
412
413 /** See vector_ortho.h for details */
414 template < typename E, class A, class B, class L, class VecT > void
415 matrix_rotation_align_2D(matrix<E,A,B,L>& m, const VecT& align,
416 bool normalize = true, AxisOrder2D order = axis_order_xy)
417 {
418 typedef vector< E, fixed<2> > vector_type;
419
420 identity_transform(m);
421
422 vector_type x, y;
423
424 orthonormal_basis_2D(align, x, y, normalize, order);
425 matrix_set_basis_vectors_2D(m, x, y);
426 }
427
428 //////////////////////////////////////////////////////////////////////////////
429 // 3D relative rotation about world axes
430 //////////////////////////////////////////////////////////////////////////////
431
432 /** Rotate a rotation matrix about the given world axis */
433 template < typename E, class A, class B, class L > void
434 matrix_rotate_about_world_axis(matrix<E,A,B,L>& m, size_t axis, E angle)
435 {
436 typedef matrix<E,A,B,L> matrix_type;
437 typedef typename matrix_type::value_type value_type;
438
439 /* Checking */
440 detail::CheckMatLinear3D(m);
441 detail::CheckIndex3(axis);
442
443 size_t i, j, k;
444 cyclic_permutation(axis, i, j, k);
445
446 value_type s = value_type(std::sin(angle));
447 value_type c = value_type(std::cos(angle));
448
449 value_type ij = c * m.basis_element(i,j) - s * m.basis_element(i,k);
450 value_type jj = c * m.basis_element(j,j) - s * m.basis_element(j,k);
451 value_type kj = c * m.basis_element(k,j) - s * m.basis_element(k,k);
452
453 m.set_basis_element(i,k, s*m.basis_element(i,j) + c*m.basis_element(i,k));
454 m.set_basis_element(j,k, s*m.basis_element(j,j) + c*m.basis_element(j,k));
455 m.set_basis_element(k,k, s*m.basis_element(k,j) + c*m.basis_element(k,k));
456
457 m.set_basis_element(i,j,ij);
458 m.set_basis_element(j,j,jj);
459 m.set_basis_element(k,j,kj);
460 }
461
462 /** Rotate a rotation matrix about the world x axis */
463 template < typename E, class A, class B, class L > void
464 matrix_rotate_about_world_x(matrix<E,A,B,L>& m, E angle) {
465 matrix_rotate_about_world_axis(m,0,angle);
466 }
467
468 /** Rotate a rotation matrix about the world y axis */
469 template < typename E, class A, class B, class L > void
470 matrix_rotate_about_world_y(matrix<E,A,B,L>& m, E angle) {
471 matrix_rotate_about_world_axis(m,1,angle);
472 }
473
474 /** Rotate a rotation matrix about the world z axis */
475 template < typename E, class A, class B, class L > void
476 matrix_rotate_about_world_z(matrix<E,A,B,L>& m, E angle) {
477 matrix_rotate_about_world_axis(m,2,angle);
478 }
479
480 //////////////////////////////////////////////////////////////////////////////
481 // 3D relative rotation about local axes
482 //////////////////////////////////////////////////////////////////////////////
483
484 /** Rotate a rotation matrix about the given local axis */
485 template < typename E, class A, class B, class L > void
486 matrix_rotate_about_local_axis(matrix<E,A,B,L>& m, size_t axis, E angle)
487 {
488 typedef matrix<E,A,B,L> matrix_type;
489 typedef typename matrix_type::value_type value_type;
490
491 /* Checking */
492 detail::CheckMatLinear3D(m);
493 detail::CheckIndex3(axis);
494
495 size_t i, j, k;
496 cyclic_permutation(axis, i, j, k);
497
498 value_type s = value_type(std::sin(angle));
499 value_type c = value_type(std::cos(angle));
500
501 value_type j0 = c * m.basis_element(j,0) + s * m.basis_element(k,0);
502 value_type j1 = c * m.basis_element(j,1) + s * m.basis_element(k,1);
503 value_type j2 = c * m.basis_element(j,2) + s * m.basis_element(k,2);
504
505 m.set_basis_element(k,0, c*m.basis_element(k,0) - s*m.basis_element(j,0));
506 m.set_basis_element(k,1, c*m.basis_element(k,1) - s*m.basis_element(j,1));
507 m.set_basis_element(k,2, c*m.basis_element(k,2) - s*m.basis_element(j,2));
508
509 m.set_basis_element(j,0,j0);
510 m.set_basis_element(j,1,j1);
511 m.set_basis_element(j,2,j2);
512 }
513
514 /** Rotate a rotation matrix about its local x axis */
515 template < typename E, class A, class B, class L > void
516 matrix_rotate_about_local_x(matrix<E,A,B,L>& m, E angle) {
517 matrix_rotate_about_local_axis(m,0,angle);
518 }
519
520 /** Rotate a rotation matrix about its local y axis */
521 template < typename E, class A, class B, class L > void
522 matrix_rotate_about_local_y(matrix<E,A,B,L>& m, E angle) {
523 matrix_rotate_about_local_axis(m,1,angle);
524 }
525
526 /** Rotate a rotation matrix about its local z axis */
527 template < typename E, class A, class B, class L > void
528 matrix_rotate_about_local_z(matrix<E,A,B,L>& m, E angle) {
529 matrix_rotate_about_local_axis(m,2,angle);
530 }
531
532 //////////////////////////////////////////////////////////////////////////////
533 // 2D relative rotation
534 //////////////////////////////////////////////////////////////////////////////
535
536 template < typename E, class A, class B, class L > void
537 matrix_rotate_2D(matrix<E,A,B,L>& m, E angle)
538 {
539 typedef matrix<E,A,B,L> matrix_type;
540 typedef typename matrix_type::value_type value_type;
541
542 /* Checking */
543 detail::CheckMatLinear2D(m);
544
545 value_type s = value_type(std::sin(angle));
546 value_type c = value_type(std::cos(angle));
547
548 value_type m00 = c * m.basis_element(0,0) - s * m.basis_element(0,1);
549 value_type m10 = c * m.basis_element(1,0) - s * m.basis_element(1,1);
550
551 m.set_basis_element(0,1, s*m.basis_element(0,0) + c*m.basis_element(0,1));
552 m.set_basis_element(1,1, s*m.basis_element(1,0) + c*m.basis_element(1,1));
553
554 m.set_basis_element(0,0,m00);
555 m.set_basis_element(1,0,m10);
556 }
557
558 //////////////////////////////////////////////////////////////////////////////
559 // Rotation from vector to vector
560 //////////////////////////////////////////////////////////////////////////////
561
562 /** Build a rotation matrix to rotate from one vector to another
563 *
564 * Note: The quaternion algorithm is more stable than the matrix algorithm, so
565 * we simply pass off to the quaternion function here.
566 */
567 template < class E,class A,class B,class L,class VecT_1,class VecT_2 > void
568 matrix_rotation_vec_to_vec(
569 matrix<E,A,B,L>& m,
570 const VecT_1& v1,
571 const VecT_2& v2,
572 bool unit_length_vectors = false)
573 {
574 typedef quaternion< E,fixed<>,vector_first,positive_cross >
575 quaternion_type;
576
577 quaternion_type q;
578 quaternion_rotation_vec_to_vec(q,v1,v2,unit_length_vectors);
579 matrix_rotation_quaternion(m,q);
580 }
581
582 //////////////////////////////////////////////////////////////////////////////
583 // Scale the angle of a rotation matrix
584 //////////////////////////////////////////////////////////////////////////////
585
586 /** Scale the angle of a 3D rotation matrix */
587 template < typename E, class A, class B, class L > void
588 matrix_scale_rotation_angle(matrix<E,A,B,L>& m, E t,
589 E tolerance = epsilon<E>::placeholder())
590 {
591 typedef vector< E,fixed<3> > vector_type;
592 typedef typename vector_type::value_type value_type;
593
594 vector_type axis;
595 value_type angle;
596 matrix_to_axis_angle(m, axis, angle, tolerance);
597 matrix_rotation_axis_angle(m, axis, angle * t);
598 }
599
600 /** Scale the angle of a 2D rotation matrix */
601 template < typename E, class A, class B, class L > void
602 matrix_scale_rotation_angle_2D(
603 matrix<E,A,B,L>& m, E t, E tolerance = epsilon<E>::placeholder())
604 {
605 typedef vector< E,fixed<2> > vector_type;
606 typedef typename vector_type::value_type value_type;
607
608 value_type angle = matrix_to_rotation_2D(m);
609 matrix_rotation_2D(m, angle * t);
610 }
611
612 //////////////////////////////////////////////////////////////////////////////
613 // Support functions for uniform handling of row- and column-basis matrices
614 //////////////////////////////////////////////////////////////////////////////
615
616 /* Note: The matrix rotation slerp, difference and concatenation functions do
617 * not use et::MatrixPromote<M1,M2>::temporary_type as the return type, even
618 * though that is the return type of the underlying matrix multiplication.
619 * This is because the sizes of these matrices are known at compile time (3x3
620 * and 2x2), and using fixed<> obviates the need for resizing of intermediate
621 * temporaries.
622 *
623 * Also, no size- or type-checking is done on the arguments to these
624 * functions, as any such errors will be caught by the matrix multiplication
625 * and assignment to the 3x3 temporary.
626 */
627
628 /** A fixed-size temporary 3x3 matrix */
629 #define MAT_TEMP_3X3 matrix< \
630 typename et::ScalarPromote< \
631 typename MatT_1::value_type, \
632 typename MatT_2::value_type \
633 >::type, \
634 fixed<3,3>, \
635 typename MatT_1::basis_orient, \
636 row_major \
637 >
638
639 /** A fixed-size temporary 2x2 matrix */
640 #define MAT_TEMP_2X2 matrix< \
641 typename et::ScalarPromote< \
642 typename MatT_1::value_type, \
643 typename MatT_2::value_type \
644 >::type, \
645 fixed<2,2>, \
646 typename MatT_1::basis_orient, \
647 row_major \
648 >
649
650 namespace detail {
651
652 /** Concatenate two 3D row-basis rotation matrices in the order m1->m2 */
653 template < class MatT_1, class MatT_2 > MAT_TEMP_3X3
654 matrix_concat_rotations(const MatT_1& m1, const MatT_2& m2, row_basis) {
655 return m1*m2;
656 }
657
658 /** Concatenate two 3D col-basis rotation matrices in the order m1->m2 */
659 template < class MatT_1, class MatT_2 > MAT_TEMP_3X3
660 matrix_concat_rotations(const MatT_1& m1, const MatT_2& m2, col_basis) {
661 return m2*m1;
662 }
663
664 /** Concatenate two 3D rotation matrices in the order m1->m2 */
665 template < class MatT_1, class MatT_2 > MAT_TEMP_3X3
666 matrix_concat_rotations(const MatT_1& m1, const MatT_2& m2) {
667 return matrix_concat_rotations(m1,m2,typename MatT_1::basis_orient());
668 }
669
670 /** Concatenate two 2D row-basis rotation matrices in the order m1->m2 */
671 template < class MatT_1, class MatT_2 > MAT_TEMP_2X2
672 matrix_concat_rotations_2D(const MatT_1& m1, const MatT_2& m2, row_basis) {
673 return m1*m2;
674 }
675
676 /** Concatenate two 2D col-basis rotation matrices in the order m1->m2 */
677 template < class MatT_1, class MatT_2 > MAT_TEMP_2X2
678 matrix_concat_rotations_2D(const MatT_1& m1, const MatT_2& m2, col_basis) {
679 return m2*m1;
680 }
681
682 /** Concatenate two 2D rotation matrices in the order m1->m2 */
683 template < class MatT_1, class MatT_2 > MAT_TEMP_2X2
684 matrix_concat_rotations_2D(const MatT_1& m1, const MatT_2& m2) {
685 return matrix_concat_rotations_2D(m1,m2,typename MatT_1::basis_orient());
686 }
687
688 } // namespace detail
689
690 //////////////////////////////////////////////////////////////////////////////
691 // Matrix rotation difference
692 //////////////////////////////////////////////////////////////////////////////
693
694 /** Return the rotational 'difference' between two 3D rotation matrices */
695 template < class MatT_1, class MatT_2 > MAT_TEMP_3X3
696 matrix_rotation_difference(const MatT_1& m1, const MatT_2& m2) {
697 return detail::matrix_concat_rotations(transpose(m1),m2);
698 }
699
700 /** Return the rotational 'difference' between two 2D rotation matrices */
701 template < class MatT_1, class MatT_2 > MAT_TEMP_2X2
702 matrix_rotation_difference_2D(const MatT_1& m1, const MatT_2& m2) {
703 return detail::matrix_concat_rotations_2D(transpose(m1),m2);
704 }
705
706 //////////////////////////////////////////////////////////////////////////////
707 // Spherical linear interpolation of rotation matrices
708 //////////////////////////////////////////////////////////////////////////////
709
710 /* @todo: It might be as fast or faster to simply convert the matrices to
711 * quaternions, interpolate, and convert back.
712 *
713 * @todo: The behavior of matrix slerp is currently a little different than
714 * for quaternions: in the matrix function, when the two matrices are close
715 * to identical the first is returned, while in the quaternion function the
716 * quaternions are nlerp()'d in this case.
717 *
718 * I still need to do the equivalent of nlerp() for matrices, in which case
719 * these functions could be revised to pass off to nlerp() when the matrices
720 * are nearly aligned.
721 */
722
723 /** Spherical linear interpolation of two 3D rotation matrices */
724 template < class MatT_1, class MatT_2, typename E > MAT_TEMP_3X3
725 matrix_slerp(const MatT_1& m1, const MatT_2& m2, E t,
726 E tolerance = epsilon<E>::placeholder())
727 {
728 typedef MAT_TEMP_3X3 temporary_type;
729
730 temporary_type m = matrix_rotation_difference(m1,m2);
731 matrix_scale_rotation_angle(m,t,tolerance);
732 return detail::matrix_concat_rotations(m1,m);
733 }
734
735 /** Spherical linear interpolation of two 2D rotation matrices */
736 template < class MatT_1, class MatT_2, typename E > MAT_TEMP_2X2
737 matrix_slerp_2D(const MatT_1& m1, const MatT_2& m2, E t,
738 E tolerance = epsilon<E>::placeholder())
739 {
740 typedef MAT_TEMP_2X2 temporary_type;
741
742 temporary_type m = matrix_rotation_difference_2D(m1,m2);
743 matrix_scale_rotation_angle_2D(m,t,tolerance);
744 return detail::matrix_concat_rotations_2D(m1,m);
745 }
746
747 #undef MAT_TEMP_3X3
748 #undef MAT_TEMP_2X2
749
750 //////////////////////////////////////////////////////////////////////////////
751 // Conversions
752 //////////////////////////////////////////////////////////////////////////////
753
754 /** Convert a 3D rotation matrix to an axis-angle pair */
755 template < class MatT, typename E, class A > void
756 matrix_to_axis_angle(
757 const MatT& m,
758 vector<E,A >& axis,
759 E& angle,
760 E tolerance = epsilon<E>::placeholder())
761 {
762 typedef MatT matrix_type;
763 typedef typename matrix_type::value_type value_type;
764
765 /* Checking */
766 detail::CheckMatLinear3D(m);
767
768 axis.set(
769 m.basis_element(1,2) - m.basis_element(2,1),
770 m.basis_element(2,0) - m.basis_element(0,2),
771 m.basis_element(0,1) - m.basis_element(1,0)
772 );
773 value_type l = length(axis);
774 value_type tmo = trace_3x3(m) - value_type(1);
775
776 if (l > tolerance) {
777 axis /= l;
778 angle = std::atan2(l, tmo); // l=2sin(theta),tmo=2cos(theta)
779 } else if (tmo > value_type(0)) {
780 axis.zero();
781 angle = value_type(0);
782 } else {
783 size_t largest_diagonal_element =
784 index_of_max(
785 m.basis_element(0,0),
786 m.basis_element(1,1),
787 m.basis_element(2,2)
788 );
789 size_t i, j, k;
790 cyclic_permutation(largest_diagonal_element, i, j, k);
791 axis[i] =
792 std::sqrt(
793 m.basis_element(i,i) -
794 m.basis_element(j,j) -
795 m.basis_element(k,k) +
796 value_type(1)
797 ) * value_type(.5);
798 value_type s = value_type(.5) / axis[i];
799 axis[j] = m.basis_element(i,j) * s;
800 axis[k] = m.basis_element(i,k) * s;
801 angle = constants<value_type>::pi();
802 }
803 }
804
805 /** Convert a 3D rotation matrix to an Euler-angle triple */
806 template < class MatT, typename Real >
807 void matrix_to_euler(
808 const MatT& m,
809 Real& angle_0,
810 Real& angle_1,
811 Real& angle_2,
812 EulerOrder order,
813 Real tolerance = epsilon<Real>::placeholder())
814 {
815 typedef MatT matrix_type;
816 typedef typename matrix_type::value_type value_type;
817
818 /* Checking */
819 detail::CheckMatLinear3D(m);
820
821 size_t i, j, k;
822 bool odd, repeat;
823 detail::unpack_euler_order(order, i, j, k, odd, repeat);
824
825 if (repeat) {
826 value_type s1 = length(m.basis_element(j,i),m.basis_element(k,i));
827 value_type c1 = m.basis_element(i,i);
828
829 angle_1 = std::atan2(s1, c1);
830 if (s1 > tolerance) {
831 angle_0 = std::atan2(m.basis_element(j,i),m.basis_element(k,i));
832 angle_2 = std::atan2(m.basis_element(i,j),-m.basis_element(i,k));
833 } else {
834 angle_0 = value_type(0);
835 angle_2 = sign(c1) *
836 std::atan2(-m.basis_element(k,j),m.basis_element(j,j));
837 }
838 } else {
839 value_type s1 = -m.basis_element(i,k);
840 value_type c1 = length(m.basis_element(i,i),m.basis_element(i,j));
841
842 angle_1 = std::atan2(s1, c1);
843 if (c1 > tolerance) {
844 angle_0 = std::atan2(m.basis_element(j,k),m.basis_element(k,k));
845 angle_2 = std::atan2(m.basis_element(i,j),m.basis_element(i,i));
846 } else {
847 angle_0 = value_type(0);
848 angle_2 = -sign(s1) *
849 std::atan2(-m.basis_element(k,j),m.basis_element(j,j));
850 }
851 }
852
853 if (odd) {
854 angle_0 = -angle_0;
855 angle_1 = -angle_1;
856 angle_2 = -angle_2;
857 }
858 }
859
860 /** Convert a 2D rotation matrix to a rotation angle */
861 template < class MatT > typename MatT::value_type
862 matrix_to_rotation_2D(const MatT& m)
863 {
864 /* Checking */
865 detail::CheckMatLinear2D(m);
866
867 return std::atan2(m.basis_element(0,1),m.basis_element(0,0));
868 }
869
870 } // namespace cml
871
872 #endif
port of a 3d side-scroller action game
This page took 0.079464 seconds and 4 git commands to generate.