now using cml for vectors and math stuff

[chaz/yoink] / src / cml / mathlib / matrix_ortho.h

diff --git a/src/cml/mathlib/matrix_ortho.h b/src/cml/mathlib/matrix_ortho.h
new file mode 100644 (file)
index 0000000..f0087f2
--- /dev/null
+++ b/src/cml/mathlib/matrix_ortho.h
@@ -0,0 +1,60 @@
+/* -*- 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