]> Dogcows Code - chaz/yoink/blob - src/cml/mathlib/matrix_rotation.h
projects / chaz / yoink / blob
? search:
summary | shortlog | log | commit | commitdiff | tree
history | raw | HEAD
testing new non-autotools build system
[chaz/yoink] / src / 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 * e.g. euler_order_xyz means compute the column-basis rotation matrix
191 * equivalent to R_x * R_y * R_z, where R_i is the rotation matrix above
192 * axis i (the row-basis matrix would be R_z * R_y * R_x).
193 */
194 template < typename E, class A, class B, class L > void
195 matrix_rotation_euler(matrix<E,A,B,L>& m, E angle_0, E angle_1, E angle_2,
196 EulerOrder order)
197 {
198 typedef matrix<E,A,B,L> matrix_type;
199 typedef typename matrix_type::value_type value_type;
200
201 /* Checking */
202 detail::CheckMatLinear3D(m);
203
204 identity_transform(m);
205
206 size_t i, j, k;
207 bool odd, repeat;
208 detail::unpack_euler_order(order, i, j, k, odd, repeat);
209
210 if (odd) {
211 angle_0 = -angle_0;
212 angle_1 = -angle_1;
213 angle_2 = -angle_2;
214 }
215
216 value_type s0 = std::sin(angle_0);
217 value_type c0 = std::cos(angle_0);
218 value_type s1 = std::sin(angle_1);
219 value_type c1 = std::cos(angle_1);
220 value_type s2 = std::sin(angle_2);
221 value_type c2 = std::cos(angle_2);
222
223 value_type s0s2 = s0 * s2;
224 value_type s0c2 = s0 * c2;
225 value_type c0s2 = c0 * s2;
226 value_type c0c2 = c0 * c2;
227
228 if (repeat) {
229 m.set_basis_element(i,i, c1 );
230 m.set_basis_element(i,j, s1 * s2 );
231 m.set_basis_element(i,k,-s1 * c2 );
232 m.set_basis_element(j,i, s0 * s1 );
233 m.set_basis_element(j,j,-c1 * s0s2 + c0c2);
234 m.set_basis_element(j,k, c1 * s0c2 + c0s2);
235 m.set_basis_element(k,i, c0 * s1 );
236 m.set_basis_element(k,j,-c1 * c0s2 - s0c2);
237 m.set_basis_element(k,k, c1 * c0c2 - s0s2);
238 } else {
239 m.set_basis_element(i,i, c1 * c2 );
240 m.set_basis_element(i,j, c1 * s2 );
241 m.set_basis_element(i,k,-s1 );
242 m.set_basis_element(j,i, s1 * s0c2 - c0s2);
243 m.set_basis_element(j,j, s1 * s0s2 + c0c2);
244 m.set_basis_element(j,k, s0 * c1 );
245 m.set_basis_element(k,i, s1 * c0c2 + s0s2);
246 m.set_basis_element(k,j, s1 * c0s2 - s0c2);
247 m.set_basis_element(k,k, c0 * c1 );
248 }
249 }
250
251 /** Build a matrix of derivatives of Euler angles about the specified axis.
252 *
253 * The rotation derivatives are applied about the cardinal axes in the
254 * order specified by the 'order' argument, where 'order' is one of the
255 * following enumerants:
256 *
257 * euler_order_xyz
258 * euler_order_xzy
259 * euler_order_yzx
260 * euler_order_yxz
261 * euler_order_zxy
262 * euler_order_zyx
263 *
264 * e.g. euler_order_xyz means compute the column-basis rotation matrix
265 * equivalent to R_x * R_y * R_z, where R_i is the rotation matrix above
266 * axis i (the row-basis matrix would be R_z * R_y * R_x).
267 *
268 * The derivative is taken with respect to the specified 'axis', which is
269 * the position of the axis in the triple; e.g. if order = euler_order_xyz,
270 * then axis = 0 would mean take the derivative with respect to x. Note
271 * that repeated axes are not currently supported.
272 */
273 template < typename E, class A, class B, class L > void
274 matrix_rotation_euler_derivatives(
275 matrix<E,A,B,L>& m, int axis, E angle_0, E angle_1, E angle_2,
276 EulerOrder order)
277 {
278 typedef matrix<E,A,B,L> matrix_type;
279 typedef typename matrix_type::value_type value_type;
280
281 /* Checking */
282 detail::CheckMatLinear3D(m);
283
284 size_t i, j, k;
285 bool odd, repeat;
286 detail::unpack_euler_order(order, i, j, k, odd, repeat);
287 if(repeat) throw std::invalid_argument(
288 "matrix_rotation_euler_derivatives does not support repeated axes");
289
290 if (odd) {
291 angle_0 = -angle_0;
292 angle_1 = -angle_1;
293 angle_2 = -angle_2;
294 }
295
296 value_type s0 = std::sin(angle_0);
297 value_type c0 = std::cos(angle_0);
298 value_type s1 = std::sin(angle_1);
299 value_type c1 = std::cos(angle_1);
300 value_type s2 = std::sin(angle_2);
301 value_type c2 = std::cos(angle_2);
302
303 value_type s0s2 = s0 * s2;
304 value_type s0c2 = s0 * c2;
305 value_type c0s2 = c0 * s2;
306 value_type c0c2 = c0 * c2;
307
308 if(axis == 0) {
309 m.set_basis_element(i,i, 0. );
310 m.set_basis_element(i,j, 0. );
311 m.set_basis_element(i,k, 0. );
312 m.set_basis_element(j,i, s1 * c0*c2 + s0*s2);
313 m.set_basis_element(j,j, s1 * c0*s2 - s0*c2);
314 m.set_basis_element(j,k, c0 * c1 );
315 m.set_basis_element(k,i,-s1 * s0*c2 + c0*s2);
316 m.set_basis_element(k,j,-s1 * s0*s2 - c0*c2);
317 m.set_basis_element(k,k,-s0 * c1 );
318 } else if(axis == 1) {
319 m.set_basis_element(i,i,-s1 * c2 );
320 m.set_basis_element(i,j,-s1 * s2 );
321 m.set_basis_element(i,k,-c1 );
322 m.set_basis_element(j,i, c1 * s0*c2 );
323 m.set_basis_element(j,j, c1 * s0*s2 );
324 m.set_basis_element(j,k,-s0 * s1 );
325 m.set_basis_element(k,i, c1 * c0*c2 );
326 m.set_basis_element(k,j, c1 * c0*s2 );
327 m.set_basis_element(k,k,-c0 * s1 );
328 } else if(axis == 2) {
329 m.set_basis_element(i,i,-c1 * s2 );
330 m.set_basis_element(i,j, c1 * c2 );
331 m.set_basis_element(i,k, 0. );
332 m.set_basis_element(j,i,-s1 * s0*s2 - c0*c2);
333 m.set_basis_element(j,j, s1 * s0*c2 - c0*s2);
334 m.set_basis_element(j,k, 0. );
335 m.set_basis_element(k,i,-s1 * c0*s2 + s0*c2);
336 m.set_basis_element(k,j, s1 * c0*c2 + s0*s2);
337 m.set_basis_element(k,k, 0. );
338 }
339 }
340
341 //////////////////////////////////////////////////////////////////////////////
342 // 3D rotation to align with a vector, multiple vectors, or the view plane
343 //////////////////////////////////////////////////////////////////////////////
344
345 /** See vector_ortho.h for details */
346 template < typename E,class A,class B,class L,class VecT_1,class VecT_2 > void
347 matrix_rotation_align(
348 matrix<E,A,B,L>& m,
349 const VecT_1& align,
350 const VecT_2& reference,
351 bool normalize = true,
352 AxisOrder order = axis_order_zyx)
353 {
354 typedef vector< E,fixed<3> > vector_type;
355
356 identity_transform(m);
357
358 vector_type x, y, z;
359
360 orthonormal_basis(align, reference, x, y, z, normalize, order);
361 matrix_set_basis_vectors(m, x, y, z);
362 }
363
364 /** See vector_ortho.h for details */
365 template < typename E, class A, class B, class L, class VecT > void
366 matrix_rotation_align(matrix<E,A,B,L>& m, const VecT& align,
367 bool normalize = true, AxisOrder order = axis_order_zyx)
368 {
369 typedef vector< E,fixed<3> > vector_type;
370
371 identity_transform(m);
372
373 vector_type x, y, z;
374
375 orthonormal_basis(align, x, y, z, normalize, order);
376 matrix_set_basis_vectors(m, x, y, z);
377 }
378
379 /** See vector_ortho.h for details */
380 template < typename E,class A,class B,class L,class VecT_1,class VecT_2 > void
381 matrix_rotation_align_axial(matrix<E,A,B,L>& m, const VecT_1& align,
382 const VecT_2& axis, bool normalize = true,
383 AxisOrder order = axis_order_zyx)
384 {
385 typedef vector< E,fixed<3> > vector_type;
386
387 identity_transform(m);
388
389 vector_type x, y, z;
390
391 orthonormal_basis_axial(align, axis, x, y, z, normalize, order);
392 matrix_set_basis_vectors(m, x, y, z);
393 }
394
395 /** See vector_ortho.h for details */
396 template < typename E, class A, class B, class L, class MatT > void
397 matrix_rotation_align_viewplane(
398 matrix<E,A,B,L>& m,
399 const MatT& view_matrix,
400 Handedness handedness,
401 AxisOrder order = axis_order_zyx)
402 {
403 typedef vector< E, fixed<3> > vector_type;
404
405 identity_transform(m);
406
407 vector_type x, y, z;
408
409 orthonormal_basis_viewplane(view_matrix, x, y, z, handedness, order);
410 matrix_set_basis_vectors(m, x, y, z);
411 }
412
413 /** See vector_ortho.h for details */
414 template < typename E, class A, class B, class L, class MatT > void
415 matrix_rotation_align_viewplane_LH(
416 matrix<E,A,B,L>& m,
417 const MatT& view_matrix,
418 AxisOrder order = axis_order_zyx)
419 {
420 matrix_rotation_align_viewplane(
421 m,view_matrix,left_handed,order);
422 }
423
424 /** See vector_ortho.h for details */
425 template < typename E, class A, class B, class L, class MatT > void
426 matrix_rotation_align_viewplane_RH(
427 matrix<E,A,B,L>& m,
428 const MatT& view_matrix,
429 AxisOrder order = axis_order_zyx)
430 {
431 matrix_rotation_align_viewplane(
432 m,view_matrix,right_handed,order);
433 }
434
435 //////////////////////////////////////////////////////////////////////////////
436 // 3D rotation to aim at a target
437 //////////////////////////////////////////////////////////////////////////////
438
439 /** See vector_ortho.h for details */
440 template < typename E, class A, class B, class L,
441 class VecT_1, class VecT_2, class VecT_3 > void
442 matrix_rotation_aim_at(
443 matrix<E,A,B,L>& m,
444 const VecT_1& pos,
445 const VecT_2& target,
446 const VecT_3& reference,
447 AxisOrder order = axis_order_zyx)
448 {
449 matrix_rotation_align(m, target - pos, reference, true, order);
450 }
451
452 /** See vector_ortho.h for details */
453 template < typename E, class A, class B, class L,
454 class VecT_1, class VecT_2 > void
455 matrix_rotation_aim_at(
456 matrix<E,A,B,L>& m,
457 const VecT_1& pos,
458 const VecT_2& target,
459 AxisOrder order = axis_order_zyx)
460 {
461 matrix_rotation_align(m, target - pos, true, order);
462 }
463
464 /** See vector_ortho.h for details */
465 template < typename E, class A, class B, class L,
466 class VecT_1, class VecT_2, class VecT_3 > void
467 matrix_rotation_aim_at_axial(
468 matrix<E,A,B,L>& m,
469 const VecT_1& pos,
470 const VecT_2& target,
471 const VecT_3& axis,
472 AxisOrder order = axis_order_zyx)
473 {
474 matrix_rotation_align_axial(m, target - pos, axis, true, order);
475 }
476
477 //////////////////////////////////////////////////////////////////////////////
478 // 2D rotation
479 //////////////////////////////////////////////////////////////////////////////
480
481 /** Build a matrix representing a 2D rotation */
482 template < typename E, class A, class B, class L > void
483 matrix_rotation_2D( matrix<E,A,B,L>& m, E angle)
484 {
485 typedef matrix<E,A,B,L> matrix_type;
486 typedef typename matrix_type::value_type value_type;
487
488 /* Checking */
489 detail::CheckMatLinear2D(m);
490
491 value_type s = value_type(std::sin(angle));
492 value_type c = value_type(std::cos(angle));
493
494 identity_transform(m);
495
496 m.set_basis_element(0,0, c);
497 m.set_basis_element(0,1, s);
498 m.set_basis_element(1,0,-s);
499 m.set_basis_element(1,1, c);
500 }
501
502 //////////////////////////////////////////////////////////////////////////////
503 // 2D rotation to align with a vector
504 //////////////////////////////////////////////////////////////////////////////
505
506 /** See vector_ortho.h for details */
507 template < typename E, class A, class B, class L, class VecT > void
508 matrix_rotation_align_2D(matrix<E,A,B,L>& m, const VecT& align,
509 bool normalize = true, AxisOrder2D order = axis_order_xy)
510 {
511 typedef vector< E, fixed<2> > vector_type;
512
513 identity_transform(m);
514
515 vector_type x, y;
516
517 orthonormal_basis_2D(align, x, y, normalize, order);
518 matrix_set_basis_vectors_2D(m, x, y);
519 }
520
521 //////////////////////////////////////////////////////////////////////////////
522 // 3D relative rotation about world axes
523 //////////////////////////////////////////////////////////////////////////////
524
525 /** Rotate a rotation matrix about the given world axis */
526 template < typename E, class A, class B, class L > void
527 matrix_rotate_about_world_axis(matrix<E,A,B,L>& m, size_t axis, E angle)
528 {
529 typedef matrix<E,A,B,L> matrix_type;
530 typedef typename matrix_type::value_type value_type;
531
532 /* Checking */
533 detail::CheckMatLinear3D(m);
534 detail::CheckIndex3(axis);
535
536 size_t i, j, k;
537 cyclic_permutation(axis, i, j, k);
538
539 value_type s = value_type(std::sin(angle));
540 value_type c = value_type(std::cos(angle));
541
542 value_type ij = c * m.basis_element(i,j) - s * m.basis_element(i,k);
543 value_type jj = c * m.basis_element(j,j) - s * m.basis_element(j,k);
544 value_type kj = c * m.basis_element(k,j) - s * m.basis_element(k,k);
545
546 m.set_basis_element(i,k, s*m.basis_element(i,j) + c*m.basis_element(i,k));
547 m.set_basis_element(j,k, s*m.basis_element(j,j) + c*m.basis_element(j,k));
548 m.set_basis_element(k,k, s*m.basis_element(k,j) + c*m.basis_element(k,k));
549
550 m.set_basis_element(i,j,ij);
551 m.set_basis_element(j,j,jj);
552 m.set_basis_element(k,j,kj);
553 }
554
555 /** Rotate a rotation matrix about the world x axis */
556 template < typename E, class A, class B, class L > void
557 matrix_rotate_about_world_x(matrix<E,A,B,L>& m, E angle) {
558 matrix_rotate_about_world_axis(m,0,angle);
559 }
560
561 /** Rotate a rotation matrix about the world y axis */
562 template < typename E, class A, class B, class L > void
563 matrix_rotate_about_world_y(matrix<E,A,B,L>& m, E angle) {
564 matrix_rotate_about_world_axis(m,1,angle);
565 }
566
567 /** Rotate a rotation matrix about the world z axis */
568 template < typename E, class A, class B, class L > void
569 matrix_rotate_about_world_z(matrix<E,A,B,L>& m, E angle) {
570 matrix_rotate_about_world_axis(m,2,angle);
571 }
572
573 //////////////////////////////////////////////////////////////////////////////
574 // 3D relative rotation about local axes
575 //////////////////////////////////////////////////////////////////////////////
576
577 /** Rotate a rotation matrix about the given local axis */
578 template < typename E, class A, class B, class L > void
579 matrix_rotate_about_local_axis(matrix<E,A,B,L>& m, size_t axis, E angle)
580 {
581 typedef matrix<E,A,B,L> matrix_type;
582 typedef typename matrix_type::value_type value_type;
583
584 /* Checking */
585 detail::CheckMatLinear3D(m);
586 detail::CheckIndex3(axis);
587
588 size_t i, j, k;
589 cyclic_permutation(axis, i, j, k);
590
591 value_type s = value_type(std::sin(angle));
592 value_type c = value_type(std::cos(angle));
593
594 value_type j0 = c * m.basis_element(j,0) + s * m.basis_element(k,0);
595 value_type j1 = c * m.basis_element(j,1) + s * m.basis_element(k,1);
596 value_type j2 = c * m.basis_element(j,2) + s * m.basis_element(k,2);
597
598 m.set_basis_element(k,0, c*m.basis_element(k,0) - s*m.basis_element(j,0));
599 m.set_basis_element(k,1, c*m.basis_element(k,1) - s*m.basis_element(j,1));
600 m.set_basis_element(k,2, c*m.basis_element(k,2) - s*m.basis_element(j,2));
601
602 m.set_basis_element(j,0,j0);
603 m.set_basis_element(j,1,j1);
604 m.set_basis_element(j,2,j2);
605 }
606
607 /** Rotate a rotation matrix about its local x axis */
608 template < typename E, class A, class B, class L > void
609 matrix_rotate_about_local_x(matrix<E,A,B,L>& m, E angle) {
610 matrix_rotate_about_local_axis(m,0,angle);
611 }
612
613 /** Rotate a rotation matrix about its local y axis */
614 template < typename E, class A, class B, class L > void
615 matrix_rotate_about_local_y(matrix<E,A,B,L>& m, E angle) {
616 matrix_rotate_about_local_axis(m,1,angle);
617 }
618
619 /** Rotate a rotation matrix about its local z axis */
620 template < typename E, class A, class B, class L > void
621 matrix_rotate_about_local_z(matrix<E,A,B,L>& m, E angle) {
622 matrix_rotate_about_local_axis(m,2,angle);
623 }
624
625 //////////////////////////////////////////////////////////////////////////////
626 // 2D relative rotation
627 //////////////////////////////////////////////////////////////////////////////
628
629 template < typename E, class A, class B, class L > void
630 matrix_rotate_2D(matrix<E,A,B,L>& m, E angle)
631 {
632 typedef matrix<E,A,B,L> matrix_type;
633 typedef typename matrix_type::value_type value_type;
634
635 /* Checking */
636 detail::CheckMatLinear2D(m);
637
638 value_type s = value_type(std::sin(angle));
639 value_type c = value_type(std::cos(angle));
640
641 value_type m00 = c * m.basis_element(0,0) - s * m.basis_element(0,1);
642 value_type m10 = c * m.basis_element(1,0) - s * m.basis_element(1,1);
643
644 m.set_basis_element(0,1, s*m.basis_element(0,0) + c*m.basis_element(0,1));
645 m.set_basis_element(1,1, s*m.basis_element(1,0) + c*m.basis_element(1,1));
646
647 m.set_basis_element(0,0,m00);
648 m.set_basis_element(1,0,m10);
649 }
650
651 //////////////////////////////////////////////////////////////////////////////
652 // Rotation from vector to vector
653 //////////////////////////////////////////////////////////////////////////////
654
655 /** Build a rotation matrix to rotate from one vector to another
656 *
657 * Note: The quaternion algorithm is more stable than the matrix algorithm, so
658 * we simply pass off to the quaternion function here.
659 */
660 template < class E,class A,class B,class L,class VecT_1,class VecT_2 > void
661 matrix_rotation_vec_to_vec(
662 matrix<E,A,B,L>& m,
663 const VecT_1& v1,
664 const VecT_2& v2,
665 bool unit_length_vectors = false)
666 {
667 typedef quaternion< E,fixed<>,vector_first,positive_cross >
668 quaternion_type;
669
670 quaternion_type q;
671 quaternion_rotation_vec_to_vec(q,v1,v2,unit_length_vectors);
672 matrix_rotation_quaternion(m,q);
673 }
674
675 //////////////////////////////////////////////////////////////////////////////
676 // Scale the angle of a rotation matrix
677 //////////////////////////////////////////////////////////////////////////////
678
679 /** Scale the angle of a 3D rotation matrix */
680 template < typename E, class A, class B, class L > void
681 matrix_scale_rotation_angle(matrix<E,A,B,L>& m, E t,
682 E tolerance = epsilon<E>::placeholder())
683 {
684 typedef vector< E,fixed<3> > vector_type;
685 typedef typename vector_type::value_type value_type;
686
687 vector_type axis;
688 value_type angle;
689 matrix_to_axis_angle(m, axis, angle, tolerance);
690 matrix_rotation_axis_angle(m, axis, angle * t);
691 }
692
693 /** Scale the angle of a 2D rotation matrix */
694 template < typename E, class A, class B, class L > void
695 matrix_scale_rotation_angle_2D(
696 matrix<E,A,B,L>& m, E t, E tolerance = epsilon<E>::placeholder())
697 {
698 typedef vector< E,fixed<2> > vector_type;
699 typedef typename vector_type::value_type value_type;
700
701 value_type angle = matrix_to_rotation_2D(m);
702 matrix_rotation_2D(m, angle * t);
703 }
704
705 //////////////////////////////////////////////////////////////////////////////
706 // Support functions for uniform handling of row- and column-basis matrices
707 //////////////////////////////////////////////////////////////////////////////
708
709 /* Note: The matrix rotation slerp, difference and concatenation functions do
710 * not use et::MatrixPromote<M1,M2>::temporary_type as the return type, even
711 * though that is the return type of the underlying matrix multiplication.
712 * This is because the sizes of these matrices are known at compile time (3x3
713 * and 2x2), and using fixed<> obviates the need for resizing of intermediate
714 * temporaries.
715 *
716 * Also, no size- or type-checking is done on the arguments to these
717 * functions, as any such errors will be caught by the matrix multiplication
718 * and assignment to the 3x3 temporary.
719 */
720
721 /** A fixed-size temporary 3x3 matrix */
722 #define MAT_TEMP_3X3 matrix< \
723 typename et::ScalarPromote< \
724 typename MatT_1::value_type, \
725 typename MatT_2::value_type \
726 >::type, \
727 fixed<3,3>, \
728 typename MatT_1::basis_orient, \
729 row_major \
730 >
731
732 /** A fixed-size temporary 2x2 matrix */
733 #define MAT_TEMP_2X2 matrix< \
734 typename et::ScalarPromote< \
735 typename MatT_1::value_type, \
736 typename MatT_2::value_type \
737 >::type, \
738 fixed<2,2>, \
739 typename MatT_1::basis_orient, \
740 row_major \
741 >
742
743 namespace detail {
744
745 /** Concatenate two 3D row-basis rotation matrices in the order m1->m2 */
746 template < class MatT_1, class MatT_2 > MAT_TEMP_3X3
747 matrix_concat_rotations(const MatT_1& m1, const MatT_2& m2, row_basis) {
748 return m1*m2;
749 }
750
751 /** Concatenate two 3D col-basis rotation matrices in the order m1->m2 */
752 template < class MatT_1, class MatT_2 > MAT_TEMP_3X3
753 matrix_concat_rotations(const MatT_1& m1, const MatT_2& m2, col_basis) {
754 return m2*m1;
755 }
756
757 /** Concatenate two 3D rotation matrices in the order m1->m2 */
758 template < class MatT_1, class MatT_2 > MAT_TEMP_3X3
759 matrix_concat_rotations(const MatT_1& m1, const MatT_2& m2) {
760 return matrix_concat_rotations(m1,m2,typename MatT_1::basis_orient());
761 }
762
763 /** Concatenate two 2D row-basis rotation matrices in the order m1->m2 */
764 template < class MatT_1, class MatT_2 > MAT_TEMP_2X2
765 matrix_concat_rotations_2D(const MatT_1& m1, const MatT_2& m2, row_basis) {
766 return m1*m2;
767 }
768
769 /** Concatenate two 2D col-basis rotation matrices in the order m1->m2 */
770 template < class MatT_1, class MatT_2 > MAT_TEMP_2X2
771 matrix_concat_rotations_2D(const MatT_1& m1, const MatT_2& m2, col_basis) {
772 return m2*m1;
773 }
774
775 /** Concatenate two 2D rotation matrices in the order m1->m2 */
776 template < class MatT_1, class MatT_2 > MAT_TEMP_2X2
777 matrix_concat_rotations_2D(const MatT_1& m1, const MatT_2& m2) {
778 return matrix_concat_rotations_2D(m1,m2,typename MatT_1::basis_orient());
779 }
780
781 } // namespace detail
782
783 //////////////////////////////////////////////////////////////////////////////
784 // Matrix rotation difference
785 //////////////////////////////////////////////////////////////////////////////
786
787 /** Return the rotational 'difference' between two 3D rotation matrices */
788 template < class MatT_1, class MatT_2 > MAT_TEMP_3X3
789 matrix_rotation_difference(const MatT_1& m1, const MatT_2& m2) {
790 return detail::matrix_concat_rotations(transpose(m1),m2);
791 }
792
793 /** Return the rotational 'difference' between two 2D rotation matrices */
794 template < class MatT_1, class MatT_2 > MAT_TEMP_2X2
795 matrix_rotation_difference_2D(const MatT_1& m1, const MatT_2& m2) {
796 return detail::matrix_concat_rotations_2D(transpose(m1),m2);
797 }
798
799 //////////////////////////////////////////////////////////////////////////////
800 // Spherical linear interpolation of rotation matrices
801 //////////////////////////////////////////////////////////////////////////////
802
803 /* @todo: It might be as fast or faster to simply convert the matrices to
804 * quaternions, interpolate, and convert back.
805 *
806 * @todo: The behavior of matrix slerp is currently a little different than
807 * for quaternions: in the matrix function, when the two matrices are close
808 * to identical the first is returned, while in the quaternion function the
809 * quaternions are nlerp()'d in this case.
810 *
811 * I still need to do the equivalent of nlerp() for matrices, in which case
812 * these functions could be revised to pass off to nlerp() when the matrices
813 * are nearly aligned.
814 */
815
816 /** Spherical linear interpolation of two 3D rotation matrices */
817 template < class MatT_1, class MatT_2, typename E > MAT_TEMP_3X3
818 matrix_slerp(const MatT_1& m1, const MatT_2& m2, E t,
819 E tolerance = epsilon<E>::placeholder())
820 {
821 typedef MAT_TEMP_3X3 temporary_type;
822
823 temporary_type m = matrix_rotation_difference(m1,m2);
824 matrix_scale_rotation_angle(m,t,tolerance);
825 return detail::matrix_concat_rotations(m1,m);
826 }
827
828 /** Spherical linear interpolation of two 2D rotation matrices */
829 template < class MatT_1, class MatT_2, typename E > MAT_TEMP_2X2
830 matrix_slerp_2D(const MatT_1& m1, const MatT_2& m2, E t,
831 E tolerance = epsilon<E>::placeholder())
832 {
833 typedef MAT_TEMP_2X2 temporary_type;
834
835 temporary_type m = matrix_rotation_difference_2D(m1,m2);
836 matrix_scale_rotation_angle_2D(m,t,tolerance);
837 return detail::matrix_concat_rotations_2D(m1,m);
838 }
839
840 #undef MAT_TEMP_3X3
841 #undef MAT_TEMP_2X2
842
843 //////////////////////////////////////////////////////////////////////////////
844 // Conversions
845 //////////////////////////////////////////////////////////////////////////////
846
847 /** Convert a 3D rotation matrix to an axis-angle pair */
848 template < class MatT, typename E, class A > void
849 matrix_to_axis_angle(
850 const MatT& m,
851 vector<E,A >& axis,
852 E& angle,
853 E tolerance = epsilon<E>::placeholder())
854 {
855 typedef MatT matrix_type;
856 typedef typename matrix_type::value_type value_type;
857
858 /* Checking */
859 detail::CheckMatLinear3D(m);
860
861 axis.set(
862 m.basis_element(1,2) - m.basis_element(2,1),
863 m.basis_element(2,0) - m.basis_element(0,2),
864 m.basis_element(0,1) - m.basis_element(1,0)
865 );
866 value_type l = length(axis);
867 value_type tmo = trace_3x3(m) - value_type(1);
868
869 if (l > tolerance) {
870 axis /= l;
871 angle = std::atan2(l, tmo); // l=2sin(theta),tmo=2cos(theta)
872 } else if (tmo > value_type(0)) {
873 axis.zero();
874 angle = value_type(0);
875 } else {
876 size_t largest_diagonal_element =
877 index_of_max(
878 m.basis_element(0,0),
879 m.basis_element(1,1),
880 m.basis_element(2,2)
881 );
882 size_t i, j, k;
883 cyclic_permutation(largest_diagonal_element, i, j, k);
884 axis[i] =
885 std::sqrt(
886 m.basis_element(i,i) -
887 m.basis_element(j,j) -
888 m.basis_element(k,k) +
889 value_type(1)
890 ) * value_type(.5);
891 value_type s = value_type(.5) / axis[i];
892 axis[j] = m.basis_element(i,j) * s;
893 axis[k] = m.basis_element(i,k) * s;
894 angle = constants<value_type>::pi();
895 }
896 }
897
898 /** Convert a 3D rotation matrix to an Euler-angle triple */
899 template < class MatT, typename Real >
900 void matrix_to_euler(
901 const MatT& m,
902 Real& angle_0,
903 Real& angle_1,
904 Real& angle_2,
905 EulerOrder order,
906 Real tolerance = epsilon<Real>::placeholder())
907 {
908 typedef MatT matrix_type;
909 typedef typename matrix_type::value_type value_type;
910
911 /* Checking */
912 detail::CheckMatLinear3D(m);
913
914 size_t i, j, k;
915 bool odd, repeat;
916 detail::unpack_euler_order(order, i, j, k, odd, repeat);
917
918 if (repeat) {
919 value_type s1 = length(m.basis_element(j,i),m.basis_element(k,i));
920 value_type c1 = m.basis_element(i,i);
921
922 angle_1 = std::atan2(s1, c1);
923 if (s1 > tolerance) {
924 angle_0 = std::atan2(m.basis_element(j,i),m.basis_element(k,i));
925 angle_2 = std::atan2(m.basis_element(i,j),-m.basis_element(i,k));
926 } else {
927 angle_0 = value_type(0);
928 angle_2 = sign(c1) *
929 std::atan2(-m.basis_element(k,j),m.basis_element(j,j));
930 }
931 } else {
932 value_type s1 = -m.basis_element(i,k);
933 value_type c1 = length(m.basis_element(i,i),m.basis_element(i,j));
934
935 angle_1 = std::atan2(s1, c1);
936 if (c1 > tolerance) {
937 angle_0 = std::atan2(m.basis_element(j,k),m.basis_element(k,k));
938 angle_2 = std::atan2(m.basis_element(i,j),m.basis_element(i,i));
939 } else {
940 angle_0 = value_type(0);
941 angle_2 = -sign(s1) *
942 std::atan2(-m.basis_element(k,j),m.basis_element(j,j));
943 }
944 }
945
946 if (odd) {
947 angle_0 = -angle_0;
948 angle_1 = -angle_1;
949 angle_2 = -angle_2;
950 }
951 }
952
953 /** Convenience function to return a 3D vector containing the Euler angles
954 * in the requested order.
955 */
956 template < class MatT, typename Real > vector< Real, fixed<3> >
957 matrix_to_euler(
958 const MatT& m,
959 EulerOrder order,
960 Real tolerance = epsilon<Real>::placeholder())
961 {
962 Real e0, e1, e2;
963 matrix_to_euler(m, e0, e1, e2, order, tolerance);
964 return vector< Real, fixed<3> >(e0, e1, e2);
965 }
966
967 /** Convert a 2D rotation matrix to a rotation angle */
968 template < class MatT > typename MatT::value_type
969 matrix_to_rotation_2D(const MatT& m)
970 {
971 /* Checking */
972 detail::CheckMatLinear2D(m);
973
974 return std::atan2(m.basis_element(0,1),m.basis_element(0,0));
975 }
976
977 } // namespace cml
978
979 #endif
port of a 3d side-scroller action game
This page took 0.079739 seconds and 4 git commands to generate.