Dogcows Code - chaz/yoink/blob - src/Moof/cml/mathlib/matrix_ortho.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_ortho_h
  14 #define matrix_ortho_h
  15
  16 #include <cml/mathlib/vector_ortho.h>
  17
  18 /* Functions for orthogonalizing a matrix.
  19  *
  20  * matrix_orthogonalize_3x3() and _2x2() operate on the upper-left-hand part
  21  * of any matrix of suitable size; this is to allow orthonormalization of the
  22  * rotation part of an affine transform matrix.
  23  *
  24  * Note: These functions pass off to the orthonormalization functions in
  25  * vector_ortho.h, so see that file for details on the optional parameters.
  26  *
  27  * @todo: General NxN matrix orthogonalization.
  28  */
  29
  30 namespace cml {
  31
  32 /** Orthogonalize the upper-left 3x3 portion of a matrix */
  33 template < typename E, class A, class B, class L > void
  34 matrix_orthogonalize_3x3(matrix<E,A,B,L>& m, size_t stable_axis = 2,
  35     size_t num_iter = 0, E s = E(1))
  36 {
  37     typedef vector< E, fixed<3> > vector_type;
  38
  39     vector_type x, y, z;
  40     matrix_get_basis_vectors(m,x,y,z);
  41     orthonormalize(x,y,z,stable_axis,num_iter,s);
  42     matrix_set_basis_vectors(m,x,y,z);
  43 }
  44
  45 /** Orthogonalize the upper-left 2x2 portion of a matrix */
  46 template < typename E, class A, class B, class L > void
  47 matrix_orthogonalize_2x2(matrix<E,A,B,L>& m, size_t stable_axis = 0,
  48     size_t num_iter = 0, E s = E(1))
  49 {
  50     typedef vector< E, fixed<2> > vector_type;
  51
  52     vector_type x, y;
  53     matrix_get_basis_vectors_2D(m,x,y);
  54     orthonormalize(x,y,stable_axis,num_iter,s);
  55     matrix_set_basis_vectors_2D(m,x,y);
  56 }
  57
  58 } // namespace cml
  59
  60 #endif