Dogcows Code - chaz/yoink/blob - src/Moof/cml/mathlib/matrix_transform.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_transform_h
  14 #define matrix_transform_h
  15
  16 #include <cml/mathlib/matrix_basis.h>
  17 #include <cml/mathlib/matrix_rotation.h>
  18 #include <cml/mathlib/matrix_translation.h>
  19
  20 /* Functions for building matrix transforms other than rotations
  21  * (matrix_rotation.h) and viewing projections (matrix_projection.h).
  22  */
  23
  24 namespace cml {
  25
  26 //////////////////////////////////////////////////////////////////////////////
  27 // 3D translation
  28 //////////////////////////////////////////////////////////////////////////////
  29
  30 /** Build a matrix representing a 3D translation */
  31 template < typename E, class A, class B, class L > void
  32 matrix_translation(matrix<E,A,B,L>& m, E x, E y, E z)
  33 {
  34     identity_transform(m);
  35     matrix_set_translation(m,x,y,z);
  36 }
  37
  38 /** Build a matrix representing a 3D translation with z set to 0 */
  39 template < typename E, class A, class B, class L > void
  40 matrix_translation(matrix<E,A,B,L>& m, E x, E y)
  41 {
  42     identity_transform(m);
  43     matrix_set_translation(m,x,y);
  44 }
  45
  46 /** Build a matrix representing a 3D translation */
  47 template < typename E, class A, class B, class L, class VecT > void
  48 matrix_translation(matrix<E,A,B,L>& m, const VecT& translation)
  49 {
  50     identity_transform(m);
  51     matrix_set_translation(m,translation);
  52 }
  53
  54 //////////////////////////////////////////////////////////////////////////////
  55 // 2D translation
  56 //////////////////////////////////////////////////////////////////////////////
  57
  58 /** Build a matrix representing a 2D translation */
  59 template < typename E, class A, class B, class L > void
  60 matrix_translation_2D(matrix<E,A,B,L>& m, E x, E y)
  61 {
  62     identity_transform(m);
  63     matrix_set_translation_2D(m,x,y);
  64 }
  65
  66 /** Build a matrix representing a 2D translation */
  67 template < typename E, class A, class B, class L, class VecT > void
  68 matrix_translation_2D(matrix<E,A,B,L>& m, const VecT& translation)
  69 {
  70     identity_transform(m);
  71     matrix_set_translation_2D(m, translation);
  72 }
  73
  74 //////////////////////////////////////////////////////////////////////////////
  75 // 3D scale
  76 //////////////////////////////////////////////////////////////////////////////
  77
  78 /** Build a matrix representing a uniform 3D scale */
  79 template < typename E, class A, class B, class L > void
  80 matrix_uniform_scale(matrix<E,A,B,L>& m, E scale) {
  81     matrix_scale(m,scale,scale,scale);
  82 }
  83
  84 /** Build a matrix representing a non-uniform 3D scale */
  85 template < typename E, class A, class B, class L > void
  86 matrix_scale(matrix<E,A,B,L>& m, E scale_x, E scale_y, E scale_z)
  87 {
  88     /* Checking */
  89     detail::CheckMatLinear3D(m);
  90
  91     identity_transform(m);
  92
  93     m.set_basis_element(0,0,scale_x);
  94     m.set_basis_element(1,1,scale_y);
  95     m.set_basis_element(2,2,scale_z);
  96 }
  97
  98 /** Build a matrix representing a non-uniform 3D scale */
  99 template < typename E, class A, class B, class L, class VecT > void
 100 matrix_scale(matrix<E,A,B,L>& m, const VecT& scale)
 101 {
 102     /* Checking */
 103     detail::CheckVec3(scale);
 104
 105     matrix_scale(m, scale[0], scale[1], scale[2]);
 106 }
 107
 108 //////////////////////////////////////////////////////////////////////////////
 109 // 2D scale
 110 //////////////////////////////////////////////////////////////////////////////
 111
 112 /** Build a matrix representing a uniform 2D scale */
 113 template < typename E, class A, class B, class L > void
 114 matrix_uniform_scale_2D(matrix<E,A,B,L>& m, E scale) {
 115     matrix_scale_2D(m,scale,scale);
 116 }
 117
 118 /** Build a matrix representing a non-uniform 2D scale */
 119 template < typename E, class A, class B, class L > void
 120 matrix_scale_2D(matrix<E,A,B,L>& m, E scale_x, E scale_y)
 121 {
 122     /* Checking */
 123     detail::CheckMatLinear2D(m);
 124
 125     identity_transform(m);
 126
 127     m.set_basis_element(0,0,scale_x);
 128     m.set_basis_element(1,1,scale_y);
 129 }
 130
 131 /** Build a matrix representing a non-uniform 2D scale */
 132 template < typename E, class A, class B, class L, class VecT > void
 133 matrix_scale_2D(matrix<E,A,B,L>& m, const VecT& scale)
 134 {
 135     /* Checking */
 136     detail::CheckVec2(scale);
 137
 138     matrix_scale_2D(m, scale[0], scale[1]);
 139 }
 140
 141 //////////////////////////////////////////////////////////////////////////////
 142 // 3D scale along axis
 143 //////////////////////////////////////////////////////////////////////////////
 144
 145 /** Build a matrix representing a 3D scale along an arbitrary axis */
 146 template < typename E, class A, class B, class L, class VecT > void
 147 matrix_scale_along_axis(matrix<E,A,B,L>&m, const VecT& axis, E scale)
 148 {
 149     typedef matrix<E,A,B,L> matrix_type;
 150     typedef typename matrix_type::value_type value_type;
 151
 152     /* Checking */
 153     detail::CheckVec3(axis);
 154
 155     matrix<E,fixed<3,3>,B,L> outer_p = outer(axis,axis)*(scale-value_type(1));
 156     outer_p(0,0) += value_type(1);
 157     outer_p(1,1) += value_type(1);
 158     outer_p(2,2) += value_type(1);
 159
 160     matrix_linear_transform(m, outer_p);
 161 }
 162
 163 //////////////////////////////////////////////////////////////////////////////
 164 // 2D scale along axis
 165 //////////////////////////////////////////////////////////////////////////////
 166
 167 /** Build a matrix representing a 2D scale along an arbitrary axis */
 168 template < typename E, class A, class B, class L, class VecT >
 169 void matrix_scale_along_axis_2D(matrix<E,A,B,L>&  m, const VecT& axis,
 170     E scale)
 171 {
 172     typedef matrix<E,A,B,L> matrix_type;
 173     typedef typename matrix_type::value_type value_type;
 174
 175     /* Checking */
 176     detail::CheckVec2(axis);
 177
 178     matrix<E,fixed<2,2>,B,L> outer_p = outer(axis,axis)*(scale-value_type(1));
 179     outer_p(0,0) += value_type(1);
 180     outer_p(1,1) += value_type(1);
 181
 182     matrix_linear_transform_2D(m, outer_p);
 183 }
 184
 185 //////////////////////////////////////////////////////////////////////////////
 186 // 3D shear
 187 //////////////////////////////////////////////////////////////////////////////
 188
 189 /** Build a matrix representing a 3D shear along the specified world axis */
 190 template < typename E, class A, class B, class L > void
 191 matrix_shear(matrix<E,A,B,L>& m, size_t axis, E shear_s, E shear_t)
 192 {
 193     /* Checking */
 194     detail::CheckMatLinear3D(m);
 195     detail::CheckIndex3(axis);
 196
 197     identity_transform(m);
 198
 199     size_t i, j, k;
 200     cyclic_permutation(axis, i, j, k);
 201
 202     m.set_basis_element(i,j,shear_s);
 203     m.set_basis_element(i,k,shear_t);
 204 }
 205
 206 /** Build a matrix representing a 3D shear along the world x axis */
 207 template < typename E, class A, class B, class L > void
 208 matrix_shear_x(matrix<E,A,B,L>& m, E shear_s, E shear_t) {
 209     matrix_shear(m,0,shear_s,shear_t);
 210 }
 211
 212 /** Build a matrix representing a 3D shear along the world y axis */
 213 template < typename E, class A, class B, class L > void
 214 matrix_shear_y(matrix<E,A,B,L>& m, E shear_s, E shear_t) {
 215     matrix_shear(m,1,shear_s,shear_t);
 216 }
 217
 218 /** Build a matrix representing a 3D shear along the world z axis */
 219 template < typename E, class A, class B, class L > void
 220 matrix_shear_z(matrix<E,A,B,L>& m, E shear_s, E shear_t) {
 221     matrix_shear(m,2,shear_s,shear_t);
 222 }
 223
 224 //////////////////////////////////////////////////////////////////////////////
 225 // 2D shear
 226 //////////////////////////////////////////////////////////////////////////////
 227
 228 /** Build a matrix representing a 2D shear along the specified world axis */
 229 template < typename E, class A, class B, class L > void
 230 matrix_shear_2D(matrix<E,A,B,L>& m, size_t axis, E shear)
 231 {
 232     /* Checking */
 233     detail::CheckMatLinear2D(m);
 234     detail::CheckIndex2(axis);
 235
 236     identity_transform(m);
 237
 238     size_t i, j;
 239     cyclic_permutation(axis, i, j);
 240
 241     m.set_basis_element(i,j,shear);
 242 }
 243
 244 /** Build a matrix representing a 2D shear along the world x axis */
 245 template < typename E, class A, class B, class L > void
 246 matrix_shear_x_2D(matrix<E,A,B,L>& m, E shear) {
 247     matrix_shear_2D(m,0,shear);
 248 }
 249
 250 /** Build a matrix representing a 2D shear along the world y axis */
 251 template < typename E, class A, class B, class L > void
 252 matrix_shear_y_2D(matrix<E,A,B,L>& m, E shear) {
 253     matrix_shear_2D(m,1,shear);
 254 }
 255
 256 //////////////////////////////////////////////////////////////////////////////
 257 // 3D reflection
 258 //////////////////////////////////////////////////////////////////////////////
 259
 260 /** Build a matrix representing a 3D reflection along the given world axis */
 261 template < typename E, class A, class B, class L > void
 262 matrix_reflect(matrix<E,A,B,L>& m, size_t axis)
 263 {
 264     typedef matrix<E,A,B,L> matrix_type;
 265     typedef typename matrix_type::value_type value_type;
 266
 267     /* Checking */
 268     detail::CheckMatLinear3D(m);
 269     detail::CheckIndex3(axis);
 270
 271     identity_transform(m);
 272
 273     m(axis,axis) = value_type(-1);
 274 }
 275
 276 /** Build a matrix representing a 3D reflection along the world x axis */
 277 template < typename E, class A, class B, class L > void
 278 matrix_reflect_x(matrix<E,A,B,L>& m) {
 279     matrix_reflect(m,0);
 280 }
 281
 282 /** Build a matrix representing a 3D reflection along the world y axis */
 283 template < typename E, class A, class B, class L > void
 284 matrix_reflect_y(matrix<E,A,B,L>& m) {
 285     matrix_reflect(m,1);
 286 }
 287
 288 /** Build a matrix representing a 3D reflection along the world z axis */
 289 template < typename E, class A, class B, class L > void
 290 matrix_reflect_z(matrix<E,A,B,L>& m) {
 291     matrix_reflect(m,2);
 292 }
 293
 294 //////////////////////////////////////////////////////////////////////////////
 295 // 2D reflection
 296 //////////////////////////////////////////////////////////////////////////////
 297
 298 /** Build a matrix representing a 2D reflection along the given world axis */
 299 template < typename E, class A, class B, class L > void
 300 matrix_reflect_2D(matrix<E,A,B,L>& m, size_t axis)
 301 {
 302     typedef matrix<E,A,B,L> matrix_type;
 303     typedef typename matrix_type::value_type value_type;
 304
 305     /* Checking */
 306     detail::CheckMatLinear2D(m);
 307     detail::CheckIndex2(axis);
 308
 309     identity_transform(m);
 310
 311     m(axis,axis) = value_type(-1);
 312 }
 313
 314 /** Build a matrix representing a 2D reflection along the world x axis */
 315 template < typename E, class A, class B, class L > void
 316 matrix_reflect_x_2D(matrix<E,A,B,L>& m) {
 317     matrix_reflect_2D(m,0);
 318 }
 319
 320 /** Build a matrix representing a 2D reflection along the world y axis */
 321 template < typename E, class A, class B, class L > void
 322 matrix_reflect_y_2D(matrix<E,A,B,L>& m) {
 323     matrix_reflect_2D(m,1);
 324 }
 325
 326 //////////////////////////////////////////////////////////////////////////////
 327 // 3D reflection about hyperplane
 328 //////////////////////////////////////////////////////////////////////////////
 329
 330 /** Build a matrix representing a 3D reflection about the given hyperplane */
 331 template < typename E, class A, class B, class L, class VecT > void
 332 matrix_reflect_about_hplane(matrix<E,A,B,L>& m, const VecT& normal)
 333 {
 334     typedef matrix<E,A,B,L> matrix_type;
 335     typedef typename matrix_type::value_type value_type;
 336
 337     matrix_scale_along_axis(m, normal, value_type(-1));
 338 }
 339
 340 //////////////////////////////////////////////////////////////////////////////
 341 // 2D reflection about hyperplane
 342 //////////////////////////////////////////////////////////////////////////////
 343
 344 /** Build a matrix representing a 2D reflection about the given hyperplane */
 345 template < typename E, class A, class B, class L, class VecT > void
 346 matrix_reflect_about_hplane_2D(matrix<E,A,B,L>&m, const VecT& normal)
 347 {
 348     typedef matrix<E,A,B,L> matrix_type;
 349     typedef typename matrix_type::value_type value_type;
 350
 351     matrix_scale_along_axis_2D(m, normal, value_type(-1));
 352 }
 353
 354 //////////////////////////////////////////////////////////////////////////////
 355 // 3D orthographic projection to cardinal hyperplane
 356 //////////////////////////////////////////////////////////////////////////////
 357
 358 /** Build a matrix representing an orthographic projection onto a plane */
 359 template < typename E, class A, class B, class L > void
 360 matrix_ortho_project(matrix<E,A,B,L>& m, size_t axis)
 361 {
 362     typedef matrix<E,A,B,L> matrix_type;
 363     typedef typename matrix_type::value_type value_type;
 364
 365     /* Checking */
 366     detail::CheckMatLinear3D(m);
 367     detail::CheckIndex3(axis);
 368
 369     identity_transform(m);
 370
 371     m(axis,axis) = value_type(0);
 372 }
 373
 374 /** Build a matrix representing an orthographic projection onto the yz plane*/
 375 template < typename E, class A, class B, class L > void
 376 matrix_ortho_project_yz(matrix<E,A,B,L>& m) {
 377     matrix_ortho_project(m,0);
 378 }
 379
 380 /** Build a matrix representing an orthographic projection onto the zx plane*/
 381 template < typename E, class A, class B, class L > void
 382 matrix_ortho_project_zx(matrix<E,A,B,L>& m) {
 383     matrix_ortho_project(m,1);
 384 }
 385
 386 /** Build a matrix representing an orthographic projection onto the zy plane*/
 387 template < typename E, class A, class B, class L > void
 388 matrix_ortho_project_xy(matrix<E,A,B,L>& m) {
 389     matrix_ortho_project(m,2);
 390 }
 391
 392 //////////////////////////////////////////////////////////////////////////////
 393 // 2D orthographic projection to cardinal hyperplane
 394 //////////////////////////////////////////////////////////////////////////////
 395
 396 /** Build a matrix representing a 2D orthographic projection */
 397 template < typename E, class A, class B, class L > void
 398 matrix_ortho_project_2D(matrix<E,A,B,L>& m, size_t axis)
 399 {
 400     typedef matrix<E,A,B,L> matrix_type;
 401     typedef typename matrix_type::value_type value_type;
 402
 403     /* Checking */
 404     detail::CheckMatLinear2D(m);
 405     detail::CheckIndex2(axis);
 406
 407     identity_transform(m);
 408
 409     m(axis,axis) = value_type(0);
 410 }
 411
 412 /** Build a matrix representing an orthographic projection onto the y axis */
 413 template < typename E, class A, class B, class L > void
 414 matrix_ortho_project_y_2D(matrix<E,A,B,L>& m) {
 415     matrix_ortho_project_2D(m,0);
 416 }
 417
 418 /** Build a matrix representing an orthographic projection onto the x axis */
 419 template < typename E, class A, class B, class L > void
 420 matrix_ortho_project_x_2D(matrix<E,A,B,L>& m) {
 421     matrix_ortho_project_2D(m,1);
 422 }
 423
 424 //////////////////////////////////////////////////////////////////////////////
 425 // 3D orthographic projection to hyperplane
 426 //////////////////////////////////////////////////////////////////////////////
 427
 428 /** Build a matrix representing a 3D orthographic projection about the given
 429  * hyperplane passing through the origin.
 430  */
 431 template < typename E, class A, class B, class L, class VecT > void
 432 matrix_ortho_project_to_hplane(matrix<E,A,B,L>& m, const VecT& normal)
 433 {
 434     typedef matrix<E,A,B,L> matrix_type;
 435     typedef typename matrix_type::value_type value_type;
 436
 437     matrix_scale_along_axis(m, normal, value_type(0));
 438 }
 439
 440 //////////////////////////////////////////////////////////////////////////////
 441 // 2D orthographic projection to hyperplane
 442 //////////////////////////////////////////////////////////////////////////////
 443
 444 /** Build a matrix representing a 2D orthographic projection about the given
 445  * hyperplane passing through the origin.
 446  */
 447 template < typename E, class A, class B, class L, class VecT > void
 448 matrix_ortho_project_to_hplane_2D(matrix<E,A,B,L>& m, const VecT& normal)
 449 {
 450     typedef matrix<E,A,B,L> matrix_type;
 451     typedef typename matrix_type::value_type value_type;
 452
 453     matrix_scale_along_axis_2D(m, normal, value_type(0));
 454 }
 455
 456 //////////////////////////////////////////////////////////////////////////////
 457 // 3D 'aim at'
 458 //////////////////////////////////////////////////////////////////////////////
 459
 460 /** See vector_ortho.h for details */
 461 template < typename E, class A, class B, class L,
 462     class VecT_1, class VecT_2, class VecT_3 > void
 463 matrix_aim_at(matrix<E,A,B,L>& m, const VecT_1& pos, const VecT_2& target,
 464     const VecT_3& reference,
 465     AxisOrder order = axis_order_zyx)
 466 {
 467     matrix_rotation_aim_at(m, pos, target, reference, order);
 468     matrix_set_translation(m, pos);
 469 }
 470
 471 /** See vector_ortho.h for details */
 472 template < typename E, class A, class B, class L,
 473     class VecT_1, class VecT_2 > void
 474 matrix_aim_at(matrix<E,A,B,L>& m, const VecT_1& pos, const VecT_2& target,
 475     AxisOrder order = axis_order_zyx)
 476 {
 477     matrix_rotation_aim_at(m, pos, target, order);
 478     matrix_set_translation(m, pos);
 479 }
 480
 481 /** See vector_ortho.h for details */
 482 template < typename E, class A, class B, class L,
 483     class VecT_1, class VecT_2, class VecT_3 > void
 484 matrix_aim_at_axial(
 485     matrix<E,A,B,L>& m,
 486     const VecT_1& pos,
 487     const VecT_2& target,
 488     const VecT_3& axis,
 489     AxisOrder order = axis_order_zyx)
 490 {
 491     matrix_rotation_aim_at_axial(m, pos, target, axis, order);
 492     matrix_set_translation(m, pos);
 493 }
 494
 495 /** See vector_ortho.h for details */
 496 template < typename E,class A,class B,class L,class VecT,class MatT > void
 497 matrix_aim_at_viewplane(
 498     matrix<E,A,B,L>& m,
 499     const VecT& pos,
 500     const MatT& view_matrix,
 501     Handedness handedness,
 502     AxisOrder order = axis_order_zyx)
 503 {
 504     matrix_rotation_align_viewplane(m, view_matrix, handedness, order);
 505     matrix_set_translation(m, pos);
 506 }
 507
 508 //////////////////////////////////////////////////////////////////////////////
 509 // 2D 'aim at'
 510 //////////////////////////////////////////////////////////////////////////////
 511
 512 /** See vector_ortho.h for details */
 513 template < typename E,class A,class B,class L,class VecT_1,class VecT_2 > void
 514 matrix_aim_at_2D(
 515     matrix<E,A,B,L>& m,
 516     const VecT_1& pos,
 517     const VecT_2& target,
 518     AxisOrder2D order = axis_order_xy)
 519 {
 520     matrix_rotation_align_2D(m, target - pos, true, order);
 521     matrix_set_translation_2D(m, pos);
 522 }
 523
 524 //////////////////////////////////////////////////////////////////////////////
 525 // 3D 'look at' view matrix
 526 //////////////////////////////////////////////////////////////////////////////
 527
 528 /** Build a matrix representing a 'look at' view transform */
 529 template < typename E, class A, class B, class L,
 530     class VecT_1, class VecT_2, class VecT_3 > void
 531 matrix_look_at(
 532     matrix<E,A,B,L>& m,
 533     const VecT_1& eye,
 534     const VecT_2& target,
 535     const VecT_3& up,
 536     Handedness handedness)
 537 {
 538     typedef matrix<E,A,B,L> matrix_type;
 539     typedef vector< E,fixed<3> > vector_type;
 540     typedef typename matrix_type::value_type value_type;
 541
 542     /* Checking */
 543     detail::CheckMatAffine3D(m);
 544
 545     identity_transform(m);
 546
 547     value_type s = handedness == left_handed ? 1 : -1;
 548     vector_type z = s * normalize(target - eye);
 549     vector_type x = unit_cross(up,z);
 550     vector_type y = cross(z,x);
 551
 552     matrix_set_transposed_basis_vectors(m,x,y,z);
 553     matrix_set_translation(m,-dot(eye,x),-dot(eye,y),-dot(eye,z));
 554 }
 555
 556 /** Build a matrix representing a left-handedness 'look at' view transform */
 557 template < typename E, class A, class B, class L,
 558     class VecT_1, class VecT_2, class VecT_3 > void
 559 matrix_look_at_LH(matrix<E,A,B,L>& m, const VecT_1& eye,
 560     const VecT_2& target, const VecT_3& up)
 561 {
 562     matrix_look_at(m, eye, target, up, left_handed);
 563 }
 564
 565 /** Build a matrix representing a right-handedness 'look at' view transform */
 566 template < typename E, class A, class B, class L,
 567     class VecT_1, class VecT_2, class VecT_3 > void
 568 matrix_look_at_RH(matrix<E,A,B,L>& m, const VecT_1& eye,
 569     const VecT_2& target, const VecT_3& up)
 570 {
 571     matrix_look_at(m, eye, target, up, right_handed);
 572 }
 573
 574 /** Build a matrix representing a 'look at' view transform */
 575 template < typename E, class A, class B, class L > void
 576 matrix_look_at(matrix<E,A,B,L>& m, E eye_x, E eye_y, E eye_z, E target_x,
 577     E target_y, E target_z, E up_x, E up_y, E up_z,
 578     Handedness handedness)
 579 {
 580     typedef vector< E, fixed<3> > vector_type;
 581
 582     matrix_look_at(m,
 583         vector_type(eye_x,eye_y,eye_z),
 584         vector_type(target_x,target_y,target_z),
 585         vector_type(up_x,up_y,up_z),
 586         handedness
 587     );
 588 }
 589
 590 /** Build a matrix representing a left-handed'look at' view transform */
 591 template < typename E, class A, class B, class L > void
 592 matrix_look_at_LH(matrix<E,A,B,L>& m, E eye_x, E eye_y, E eye_z,
 593     E target_x, E target_y, E target_z, E up_x, E up_y, E up_z)
 594 {
 595     matrix_look_at(m,eye_x,eye_y,eye_z,target_x,target_y,target_z,up_x,up_y,
 596         up_z,left_handed);
 597 }
 598
 599 /** Build a matrix representing a right-handed'look at' view transform */
 600 template < typename E, class A, class B, class L > void
 601 matrix_look_at_RH(matrix<E,A,B,L>& m, E eye_x, E eye_y, E eye_z,
 602     E target_x, E target_y, E target_z, E up_x, E up_y, E up_z)
 603 {
 604     matrix_look_at(m,eye_x,eye_y,eye_z,target_x,target_y,target_z,up_x,up_y,
 605         up_z,right_handed);
 606 }
 607
 608 //////////////////////////////////////////////////////////////////////////////
 609 // 3D linear transform
 610 //////////////////////////////////////////////////////////////////////////////
 611
 612 /** Build a matrix from the 3x3 linear transform part of another matrix */
 613 template < typename E, class A, class B, class L, class MatT > void
 614 matrix_linear_transform(matrix<E,A,B,L>& m, const MatT& linear)
 615 {
 616     /* Checking */
 617     detail::CheckMatLinear3D(m);
 618     detail::CheckMatLinear3D(linear);
 619
 620     identity_transform(m);
 621
 622     for(size_t i = 0; i < 3; ++i) {
 623         for(size_t j = 0; j < 3; ++j) {
 624             m.set_basis_element(i,j,linear.basis_element(i,j));
 625         }
 626     }
 627 }
 628
 629 //////////////////////////////////////////////////////////////////////////////
 630 // 2D linear transform
 631 //////////////////////////////////////////////////////////////////////////////
 632
 633 /** Build a matrix from the 2x2 linear transform part of another matrix */
 634 template < typename E, class A, class B, class L, class MatT > void
 635 matrix_linear_transform_2D(matrix<E,A,B,L>& m, const MatT& linear)
 636 {
 637     /* Checking */
 638     detail::CheckMatLinear2D(m);
 639     detail::CheckMatLinear2D(linear);
 640
 641     identity_transform(m);
 642
 643     for(size_t i = 0; i < 2; ++i) {
 644         for(size_t j = 0; j < 2; ++j) {
 645             m.set_basis_element(i,j,linear.basis_element(i,j));
 646         }
 647     }
 648 }
 649
 650 //////////////////////////////////////////////////////////////////////////////
 651 // 3D affine transform
 652 //////////////////////////////////////////////////////////////////////////////
 653
 654 /** 3D affine transform from three basis vectors and a translation */
 655 template <typename E, class A, class B, class L,
 656     class VecT_1, class VecT_2, class VecT_3, class VecT_4 > void
 657 matrix_affine_transform(matrix<E,A,B,L>& m, const VecT_1& x, const VecT_2& y,
 658     const VecT_3& z, const VecT_4& translation)
 659 {
 660     identity_transform(m);
 661     matrix_set_basis_vectors(m,x,y,z);
 662     matrix_set_translation(m,translation);
 663 }
 664
 665 /** 3D affine transform from a quaternion and a translation */
 666 template <
 667     typename E, class A, class B, class L,
 668     typename QE, class QA, class O, class C, class VecT > void
 669 matrix_affine_transform(
 670     matrix<E,A,B,L>& m, const quaternion<QE,QA,O,C>& q,
 671     const VecT& translation)
 672 {
 673     matrix_rotation_quaternion(m,q);
 674     matrix_set_translation(m,translation);
 675 }
 676
 677 /** 3D affine transform from a quaternion expression and a translation */
 678 template < typename E,class A,class B,class L,class XprT,class VecT > void
 679 matrix_affine_transform(
 680     matrix<E,A,B,L>& m, const et::QuaternionXpr<XprT>& q,
 681     const VecT& translation)
 682 {
 683     matrix_rotation_quaternion(m,q);
 684     matrix_set_translation(m,translation);
 685 }
 686
 687 /** 3D affine transform from an axis-angle pair and a translation */
 688 template <
 689     typename E, class A, class B, class L, class VecT_1, class VecT_2 > void
 690 matrix_affine_transform(
 691     matrix<E,A,B,L>& m,const VecT_1& axis,E angle,const VecT_2& translation)
 692 {
 693     matrix_rotation_axis_angle(m,axis,angle);
 694     matrix_set_translation(m,translation);
 695 }
 696
 697 /** 3D affine transform from an Euler-angle triple and a translation */
 698 template < typename E, class A, class B, class L, class VecT > void
 699 matrix_affine_transform(matrix<E,A,B,L>& m, E angle_0, E angle_1,
 700     E angle_2, EulerOrder order, const VecT& translation)
 701 {
 702     matrix_rotation_euler(m,angle_0,angle_1,angle_2,order);
 703     matrix_set_translation(m,translation);
 704 }
 705
 706 /** 3D affine transform from a matrix and a translation */
 707 template <
 708     typename E, class A, class B, class L,
 709     typename ME, class MA, class MB, class ML, class VecT > void
 710 matrix_affine_transform(matrix<E,A,B,L>& m,
 711     const matrix<ME,MA,MB,ML>& linear, const VecT& translation)
 712 {
 713     matrix_linear_transform(m,linear);
 714     matrix_set_translation(m,translation);
 715 }
 716
 717 /** 3D affine transform from a matrix expression and a translation */
 718 template < typename E,class A,class B,class L,class XprT,class VecT > void
 719 matrix_affine_transform(
 720     matrix<E,A,B,L>& m, const et::MatrixXpr<XprT>& linear,
 721     const VecT& translation)
 722 {
 723     matrix_linear_transform(m,linear);
 724     matrix_set_translation(m,translation);
 725 }
 726
 727 //////////////////////////////////////////////////////////////////////////////
 728 // 2D affine transform
 729 //////////////////////////////////////////////////////////////////////////////
 730
 731 /** 2D affine transform from two basis vectors and a translation */
 732 template <typename E, class A, class B, class L,
 733     class VecT_1, class VecT_2, class VecT_3 > void
 734 matrix_affine_transform_2D(matrix<E,A,B,L>& m, const VecT_1& x,
 735     const VecT_2& y, const VecT_3& translation)
 736 {
 737     identity_transform(m);
 738     matrix_set_basis_vectors_2D(m,x,y);
 739     matrix_set_translation_2D(m,translation);
 740 }
 741
 742 /** 2D affine transform from a rotation angle and a translation */
 743 template <typename E, class A, class B, class L, class VecT >
 744 void matrix_affine_transform_2D(matrix<E,A,B,L>& m, E angle,
 745     const VecT& translation)
 746 {
 747     matrix_rotation_2D(m,angle);
 748     matrix_set_translation_2D(m,translation);
 749 }
 750
 751 /** 2D affine transform from a matrix and a translation */
 752 template < typename E,class A,class B,class L,class MatT,class VecT > void
 753 matrix_affine_transform_2D(
 754     matrix<E,A,B,L>& m, const MatT& linear, const VecT& translation)
 755 {
 756     matrix_linear_transform_2D(m, linear);
 757     matrix_set_translation_2D(m,translation);
 758 }
 759
 760 //////////////////////////////////////////////////////////////////////////////
 761 // 3D affine from 2D affine
 762 //////////////////////////////////////////////////////////////////////////////
 763
 764 /** Construct a 3D affine transform from a 2D affine transform */
 765 template < typename E, class A, class B, class L, class MatT > void
 766 matrix_3D_affine_from_2D_affine(matrix<E,A,B,L>& m, const MatT& affine_2D)
 767 {
 768     typedef vector< E, fixed<2> > vector_type;
 769
 770     vector_type x = matrix_get_x_basis_vector_2D(affine_2D);
 771     vector_type y = matrix_get_y_basis_vector_2D(affine_2D);
 772     vector_type p = matrix_get_translation_2D(affine_2D);
 773
 774     identity_transform(m);
 775
 776     matrix_set_basis_vectors_2D(m,x,y);
 777     matrix_set_translation(m,p);
 778 }
 779
 780 //////////////////////////////////////////////////////////////////////////////
 781 // 3D affine from 3D affine
 782 //////////////////////////////////////////////////////////////////////////////
 783
 784 /** Construct a 3D affine transform from another 3D affine transform */
 785 template < typename E, class A, class B, class L, class MatT > void
 786 matrix_3D_affine_from_3D_affine(matrix<E,A,B,L>& m, const MatT& affine_3D)
 787 {
 788     typedef vector< E, fixed<3> > vector_type;
 789
 790     vector_type x = matrix_get_x_basis_vector(affine_3D);
 791     vector_type y = matrix_get_y_basis_vector(affine_3D);
 792     vector_type z = matrix_get_z_basis_vector(affine_3D);
 793     vector_type p = matrix_get_translation(affine_3D);
 794
 795     identity_transform(m);
 796
 797     matrix_set_basis_vectors(m,x,y,z);
 798     matrix_set_translation(m,p);
 799 }
 800
 801 //////////////////////////////////////////////////////////////////////////////
 802 // Matrix decomposition (scale->rotate->translate)
 803 //////////////////////////////////////////////////////////////////////////////
 804
 805 /* 3x3 matrix version */
 806 template <
 807     class MatT,
 808     typename Real,
 809     typename ME,
 810     class MA,
 811     class B,
 812     class L,
 813     typename VE,
 814     class VA
 815 >
 816 void matrix_decompose_SRT(
 817     const MatT& m,
 818     Real& scale_x,
 819     Real& scale_y,
 820     Real& scale_z,
 821     matrix<ME,MA,B,L>& rotation,
 822     vector<VE,VA>& translation)
 823 {
 824     typedef MatT matrix_type;
 825     typedef typename matrix_type::value_type value_type;
 826     typedef vector<value_type, fixed<3> > vector_type;
 827
 828     /* Checking */
 829     detail::CheckMatAffine3D(m);
 830     detail::CheckMatLinear3D(rotation);
 831
 832     vector_type x, y, z;
 833     matrix_get_basis_vectors(m, x, y, z);
 834
 835     scale_x = x.length();
 836     scale_y = y.length();
 837     scale_z = z.length();
 838
 839     x /= scale_x;
 840     y /= scale_y;
 841     z /= scale_z;
 842
 843     matrix_set_basis_vectors(rotation, x, y, z);
 844     translation = matrix_get_translation(m);
 845 }
 846
 847 /* Quaternion version */
 848 template <
 849     class MatT,
 850     typename Real,
 851     typename QE,
 852     class QA,
 853     class O,
 854     class C,
 855     typename VE,
 856     class VA
 857 >
 858 void matrix_decompose_SRT(
 859     const MatT& m,
 860     Real& scale_x,
 861     Real& scale_y,
 862     Real& scale_z,
 863     quaternion<QE,QA,O,C>& rotation,
 864     vector<VE,VA>& translation)
 865 {
 866     typedef MatT matrix_type;
 867     typedef typename matrix_type::value_type value_type;
 868     typedef matrix< value_type, fixed<3,3> > rotation_type;
 869
 870     rotation_type rotation_matrix;
 871     matrix_decompose_SRT(
 872         m, scale_x, scale_y, scale_z, rotation_matrix, translation);
 873     quaternion_rotation_matrix(rotation, rotation_matrix);
 874 }
 875
 876 /* Euler angle version */
 877 template < class MatT, typename Real, typename E, class A >
 878 void matrix_decompose_SRT(
 879     const MatT& m,
 880     Real& scale_x,
 881     Real& scale_y,
 882     Real& scale_z,
 883     Real& angle_0,
 884     Real& angle_1,
 885     Real& angle_2,
 886     EulerOrder order,
 887     vector<E,A>& translation,
 888     Real tolerance = epsilon<Real>::placeholder())
 889 {
 890     typedef MatT matrix_type;
 891     typedef typename matrix_type::value_type value_type;
 892     typedef matrix< value_type, fixed<3,3> > rotation_type;
 893
 894     rotation_type rotation_matrix;
 895     matrix_decompose_SRT(
 896         m, scale_x, scale_y, scale_z, rotation_matrix, translation);
 897     matrix_to_euler(
 898         rotation_matrix, angle_0, angle_1, angle_2, order, tolerance);
 899 }
 900
 901 /* Axis-angle version */
 902 template < class MatT, typename Real, typename E, class A >
 903 void matrix_decompose_SRT(
 904     const MatT& m,
 905     Real& scale_x,
 906     Real& scale_y,
 907     Real& scale_z,
 908     vector<E,A>& axis,
 909     Real& angle,
 910     vector<E,A>& translation,
 911     Real tolerance = epsilon<Real>::placeholder())
 912 {
 913     typedef MatT matrix_type;
 914     typedef typename matrix_type::value_type value_type;
 915     typedef matrix< value_type, fixed<3,3> > rotation_type;
 916
 917     rotation_type rotation_matrix;
 918     matrix_decompose_SRT(
 919         m, scale_x, scale_y, scale_z, rotation_matrix, translation);
 920     matrix_to_axis_angle(rotation_matrix, axis, angle, tolerance);
 921 }
 922
 923 /* 2x2 matrix version, 2-d */
 924 template <
 925     class MatT,
 926     typename Real,
 927     typename ME,
 928     class MA,
 929     class B,
 930     class L,
 931     typename VE,
 932     class VA
 933 >
 934 void matrix_decompose_SRT_2D(
 935     const MatT& m,
 936     Real& scale_x,
 937     Real& scale_y,
 938     matrix<ME,MA,B,L>& rotation,
 939     vector<VE,VA>& translation)
 940 {
 941     typedef MatT matrix_type;
 942     typedef typename matrix_type::value_type value_type;
 943     typedef vector<value_type, fixed<2> > vector_type;
 944
 945     /* Checking */
 946     detail::CheckMatAffine2D(m);
 947     detail::CheckMatLinear2D(rotation);
 948
 949     vector_type x, y;
 950     matrix_get_basis_vectors_2D(m, x, y);
 951
 952     scale_x = x.length();
 953     scale_y = y.length();
 954
 955     x /= scale_x;
 956     y /= scale_y;
 957
 958     matrix_set_basis_vectors_2D(rotation, x, y);
 959     translation = matrix_get_translation_2D(m);
 960 }
 961
 962 /* Angle version, 2-d */
 963 template < class MatT, typename Real, typename E, class A >
 964 void matrix_decompose_SRT_2D(
 965     const MatT& m,
 966     Real& scale_x,
 967     Real& scale_y,
 968     Real& angle,
 969     vector<E,A>& translation)
 970 {
 971     typedef MatT matrix_type;
 972     typedef typename matrix_type::value_type value_type;
 973     typedef matrix< value_type, fixed<2,2> > rotation_type;
 974
 975     rotation_type rotation_matrix;
 976     matrix_decompose_SRT_2D(
 977         m, scale_x, scale_y, rotation_matrix, translation);
 978     angle = matrix_to_rotation_2D(rotation_matrix);
 979 }
 980
 981 } // namespace cml
 982
 983 #endif