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