Dogcows Code - chaz/yoink/blob - src/moof/cml/mathlib/matrix_misc.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_misc_h
  14 #define matrix_misc_h
  15
  16 #include <cml/mathlib/checking.h>
  17
  18 /* Miscellaneous matrix functions. */
  19
  20 namespace cml {
  21
  22 /** Set a (possibly non-square) matrix to represent an identity transform */
  23 template < typename E, class A, class B, class L > void
  24 identity_transform(matrix<E,A,B,L>& m)
  25 {
  26     typedef matrix<E,A,B,L> matrix_type;
  27     typedef typename matrix_type::value_type value_type;
  28
  29     for (size_t i = 0; i < m.rows(); ++i) {
  30         for (size_t j = 0; j < m.cols(); ++j) {
  31             m(i,j) = value_type((i == j) ? 1 : 0);
  32         }
  33     }
  34 }
  35
  36 /** Trace of a square matrix */
  37 template < class MatT > typename MatT::value_type
  38 trace(const MatT& m)
  39 {
  40     typedef typename MatT::value_type value_type;
  41
  42     /* Checking */
  43     detail::CheckMatSquare(m);
  44
  45     value_type t = value_type(0);
  46     for (size_t i = 0; i < m.rows(); ++i) {
  47         t += m(i,i);
  48     }
  49     return t;
  50 }
  51
  52 /** Trace of the upper-left 3x3 part of a matrix */
  53 template < class MatT > typename MatT::value_type
  54 trace_3x3(const MatT& m)
  55 {
  56     /* Checking */
  57     detail::CheckMatMin3x3(m);
  58
  59     return m(0,0) + m(1,1) + m(2,2);
  60 }
  61
  62 /** Trace of the upper-left 2x2 part of a matrix */
  63 template < class MatT > typename MatT::value_type
  64 trace_2x2(const MatT& m)
  65 {
  66     /* Checking */
  67     detail::CheckMatMin2x2(m);
  68
  69     return m(0,0) + m(1,1);
  70 }
  71
  72 /** 3D skew-symmetric matrix */
  73 template < typename E, class A, class B, class L, class VecT > void
  74 matrix_skew_symmetric(matrix<E,A,B,L>& m, const VecT& v)
  75 {
  76     /* Checking */
  77     detail::CheckMatMin3x3(m);
  78     detail::CheckVec3(v);
  79
  80     m.zero();
  81
  82     m.set_basis_element(1,2, v[0]);
  83     m.set_basis_element(2,1,-v[0]);
  84     m.set_basis_element(2,0, v[1]);
  85     m.set_basis_element(0,2,-v[1]);
  86     m.set_basis_element(0,1, v[2]);
  87     m.set_basis_element(1,0,-v[2]);
  88 }
  89
  90 /** 2D skew-symmetric matrix */
  91 template < typename E, class A, class B, class L > void
  92 matrix_skew_symmetric_2D(matrix<E,A,B,L>& m, E s)
  93 {
  94     /* Checking */
  95     detail::CheckMatMin2x2(m);
  96
  97     m.zero();
  98
  99     m.set_basis_element(0,1, s);
 100     m.set_basis_element(1,0,-s);
 101 }
 102
 103 /* @todo: Clean this up, and implement SRT as well */
 104
 105 /** Invert a matrix consisting of a 3D rotation and translation */
 106 template < typename E, class A, class B, class L > void
 107 matrix_invert_RT_only(matrix<E,A,B,L>& m)
 108 {
 109     typedef vector< E, fixed<3> > vector_type;
 110
 111     vector_type x, y, z;
 112     matrix_get_basis_vectors(m,x,y,z);
 113     matrix_set_transposed_basis_vectors(m,x,y,z);
 114
 115     vector_type p = matrix_get_translation(m);
 116     matrix_set_translation(m,-dot(p,x),-dot(p,y),-dot(p,z));
 117 }
 118
 119 /** Invert a matrix consisting of a 2D rotation and ranslation */
 120 template < typename E, class A, class B, class L > void
 121 matrix_invert_RT_only_2D(matrix<E,A,B,L>& m)
 122 {
 123     typedef vector< E, fixed<2> > vector_type;
 124
 125     vector_type x, y;
 126     matrix_get_basis_vectors_2D(m,x,y);
 127     matrix_set_transposed_basis_vectors_2D(m,x,y);
 128
 129     vector_type p = matrix_get_translation_2D(m);
 130     matrix_set_translation_2D(m,-dot(p,x),-dot(p,y));
 131 }
 132
 133 } // namespace cml
 134
 135 #endif