61 lines
1.8 KiB
C++
61 lines
1.8 KiB
C++
/* -*- C++ -*- ------------------------------------------------------------
|
|
|
|
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
|
|
|
|
The Configurable Math Library (CML) is distributed under the terms of the
|
|
Boost Software License, v1.0 (see cml/LICENSE for details).
|
|
|
|
*-----------------------------------------------------------------------*/
|
|
/** @file
|
|
* @brief
|
|
*/
|
|
|
|
#ifndef matrix_ortho_h
|
|
#define matrix_ortho_h
|
|
|
|
#include <cml/mathlib/vector_ortho.h>
|
|
|
|
/* Functions for orthogonalizing a matrix.
|
|
*
|
|
* matrix_orthogonalize_3x3() and _2x2() operate on the upper-left-hand part
|
|
* of any matrix of suitable size; this is to allow orthonormalization of the
|
|
* rotation part of an affine transform matrix.
|
|
*
|
|
* Note: These functions pass off to the orthonormalization functions in
|
|
* vector_ortho.h, so see that file for details on the optional parameters.
|
|
*
|
|
* @todo: General NxN matrix orthogonalization.
|
|
*/
|
|
|
|
namespace cml {
|
|
|
|
/** Orthogonalize the upper-left 3x3 portion of a matrix */
|
|
template < typename E, class A, class B, class L > void
|
|
matrix_orthogonalize_3x3(matrix<E,A,B,L>& m, size_t stable_axis = 2,
|
|
size_t num_iter = 0, E s = E(1))
|
|
{
|
|
typedef vector< E, fixed<3> > vector_type;
|
|
|
|
vector_type x, y, z;
|
|
matrix_get_basis_vectors(m,x,y,z);
|
|
orthonormalize(x,y,z,stable_axis,num_iter,s);
|
|
matrix_set_basis_vectors(m,x,y,z);
|
|
}
|
|
|
|
/** Orthogonalize the upper-left 2x2 portion of a matrix */
|
|
template < typename E, class A, class B, class L > void
|
|
matrix_orthogonalize_2x2(matrix<E,A,B,L>& m, size_t stable_axis = 0,
|
|
size_t num_iter = 0, E s = E(1))
|
|
{
|
|
typedef vector< E, fixed<2> > vector_type;
|
|
|
|
vector_type x, y;
|
|
matrix_get_basis_vectors_2D(m,x,y);
|
|
orthonormalize(x,y,stable_axis,num_iter,s);
|
|
matrix_set_basis_vectors_2D(m,x,y);
|
|
}
|
|
|
|
} // namespace cml
|
|
|
|
#endif
|