Initial commit

Lib/Include/CML/mathlib/matrix_misc.h

@@ -0,0 +1,135 @@
+/* -*- 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_misc_h
+#define matrix_misc_h
+
+#include <cml/mathlib/checking.h>
+
+/* Miscellaneous matrix functions. */
+
+namespace cml {
+
+/** Set a (possibly non-square) matrix to represent an identity transform */
+template < typename E, class A, class B, class L > void
+identity_transform(matrix<E,A,B,L>& m)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+
+    for (size_t i = 0; i < m.rows(); ++i) {
+        for (size_t j = 0; j < m.cols(); ++j) {
+            m(i,j) = value_type((i == j) ? 1 : 0);
+        }
+    }
+}
+
+/** Trace of a square matrix */
+template < class MatT > typename MatT::value_type
+trace(const MatT& m)
+{
+    typedef typename MatT::value_type value_type;
+    
+    /* Checking */
+    detail::CheckMatSquare(m);
+
+    value_type t = value_type(0);
+    for (size_t i = 0; i < m.rows(); ++i) {
+        t += m(i,i);
+    }
+    return t;
+}
+
+/** Trace of the upper-left 3x3 part of a matrix */
+template < class MatT > typename MatT::value_type
+trace_3x3(const MatT& m)
+{
+    /* Checking */
+    detail::CheckMatMin3x3(m);
+    
+    return m(0,0) + m(1,1) + m(2,2);
+}
+
+/** Trace of the upper-left 2x2 part of a matrix */
+template < class MatT > typename MatT::value_type
+trace_2x2(const MatT& m)
+{
+    /* Checking */
+    detail::CheckMatMin2x2(m);
+    
+    return m(0,0) + m(1,1);
+}
+
+/** 3D skew-symmetric matrix */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_skew_symmetric(matrix<E,A,B,L>& m, const VecT& v)
+{
+    /* Checking */
+    detail::CheckMatMin3x3(m);
+    detail::CheckVec3(v);
+
+    m.zero();
+
+    m.set_basis_element(1,2, v[0]);
+    m.set_basis_element(2,1,-v[0]);
+    m.set_basis_element(2,0, v[1]);
+    m.set_basis_element(0,2,-v[1]);
+    m.set_basis_element(0,1, v[2]);
+    m.set_basis_element(1,0,-v[2]);
+}
+
+/** 2D skew-symmetric matrix */
+template < typename E, class A, class B, class L > void
+matrix_skew_symmetric_2D(matrix<E,A,B,L>& m, E s)
+{
+    /* Checking */
+    detail::CheckMatMin2x2(m);
+
+    m.zero();
+
+    m.set_basis_element(0,1, s);
+    m.set_basis_element(1,0,-s);
+}
+
+/* @todo: Clean this up, and implement SRT as well */
+
+/** Invert a matrix consisting of a 3D rotation and translation */
+template < typename E, class A, class B, class L > void
+matrix_invert_RT_only(matrix<E,A,B,L>& m)
+{
+    typedef vector< E, fixed<3> > vector_type;
+
+    vector_type x, y, z;
+    matrix_get_basis_vectors(m,x,y,z);
+    matrix_set_transposed_basis_vectors(m,x,y,z);
+    
+    vector_type p = matrix_get_translation(m);
+    matrix_set_translation(m,-dot(p,x),-dot(p,y),-dot(p,z));
+}
+
+/** Invert a matrix consisting of a 2D rotation and ranslation */
+template < typename E, class A, class B, class L > void
+matrix_invert_RT_only_2D(matrix<E,A,B,L>& m)
+{
+    typedef vector< E, fixed<2> > vector_type;
+
+    vector_type x, y;
+    matrix_get_basis_vectors_2D(m,x,y);
+    matrix_set_transposed_basis_vectors_2D(m,x,y);
+    
+    vector_type p = matrix_get_translation_2D(m);
+    matrix_set_translation_2D(m,-dot(p,x),-dot(p,y));
+}
+
+} // namespace cml
+
+#endif