]> Dogcows Code - chaz/yoink/commitdiff
now using cml for vectors and math stuff
authorCharles McGarvey <chazmcgarvey@brokenzipper.com>
Thu, 9 Jul 2009 08:00:37 +0000 (02:00 -0600)
committerCharles McGarvey <chazmcgarvey@brokenzipper.com>
Thu, 9 Jul 2009 08:00:37 +0000 (02:00 -0600)
107 files changed:
src/cml/cml.h [new file with mode: 0644]
src/cml/constants.h [new file with mode: 0644]
src/cml/core/cml_assert.h [new file with mode: 0644]
src/cml/core/cml_meta.h [new file with mode: 0644]
src/cml/core/common.h [new file with mode: 0644]
src/cml/core/dynamic_1D.h [new file with mode: 0644]
src/cml/core/dynamic_2D.h [new file with mode: 0644]
src/cml/core/external_1D.h [new file with mode: 0644]
src/cml/core/external_2D.h [new file with mode: 0644]
src/cml/core/fixed_1D.h [new file with mode: 0644]
src/cml/core/fixed_2D.h [new file with mode: 0644]
src/cml/core/fwd.h [new file with mode: 0644]
src/cml/core/meta/common.h [new file with mode: 0644]
src/cml/core/meta/if.h [new file with mode: 0644]
src/cml/core/meta/switch.h [new file with mode: 0644]
src/cml/defaults.h [new file with mode: 0644]
src/cml/dynamic.h [new file with mode: 0644]
src/cml/et/array_promotions.h [new file with mode: 0644]
src/cml/et/scalar_ops.h [new file with mode: 0644]
src/cml/et/scalar_promotions.h [new file with mode: 0644]
src/cml/et/size_checking.h [new file with mode: 0644]
src/cml/et/tags.h [new file with mode: 0644]
src/cml/et/traits.h [new file with mode: 0644]
src/cml/external.h [new file with mode: 0644]
src/cml/fixed.h [new file with mode: 0644]
src/cml/mathlib/checking.h [new file with mode: 0644]
src/cml/mathlib/coord_conversion.h [new file with mode: 0644]
src/cml/mathlib/epsilon.h [new file with mode: 0644]
src/cml/mathlib/frustum.h [new file with mode: 0644]
src/cml/mathlib/helper.h [new file with mode: 0644]
src/cml/mathlib/interpolation.h [new file with mode: 0644]
src/cml/mathlib/mathlib.h [new file with mode: 0644]
src/cml/mathlib/matrix_basis.h [new file with mode: 0644]
src/cml/mathlib/matrix_concat.h [new file with mode: 0644]
src/cml/mathlib/matrix_misc.h [new file with mode: 0644]
src/cml/mathlib/matrix_ortho.h [new file with mode: 0644]
src/cml/mathlib/matrix_projection.h [new file with mode: 0644]
src/cml/mathlib/matrix_rotation.h [new file with mode: 0644]
src/cml/mathlib/matrix_transform.h [new file with mode: 0644]
src/cml/mathlib/matrix_translation.h [new file with mode: 0644]
src/cml/mathlib/misc.h [new file with mode: 0644]
src/cml/mathlib/picking.h [new file with mode: 0644]
src/cml/mathlib/projection.h [new file with mode: 0644]
src/cml/mathlib/quaternion_basis.h [new file with mode: 0644]
src/cml/mathlib/quaternion_rotation.h [new file with mode: 0644]
src/cml/mathlib/typedef.h [new file with mode: 0644]
src/cml/mathlib/vector_angle.h [new file with mode: 0644]
src/cml/mathlib/vector_misc.h [new file with mode: 0644]
src/cml/mathlib/vector_ortho.h [new file with mode: 0644]
src/cml/mathlib/vector_transform.h [new file with mode: 0644]
src/cml/matrix.h [new file with mode: 0644]
src/cml/matrix/class_ops.h [new file with mode: 0644]
src/cml/matrix/determinant.h [new file with mode: 0644]
src/cml/matrix/dynamic.h [new file with mode: 0644]
src/cml/matrix/external.h [new file with mode: 0644]
src/cml/matrix/fixed.h [new file with mode: 0644]
src/cml/matrix/inverse.h [new file with mode: 0644]
src/cml/matrix/lu.h [new file with mode: 0644]
src/cml/matrix/matop_macros.h [new file with mode: 0644]
src/cml/matrix/matrix_comparison.h [new file with mode: 0644]
src/cml/matrix/matrix_expr.h [new file with mode: 0644]
src/cml/matrix/matrix_functions.h [new file with mode: 0644]
src/cml/matrix/matrix_mul.h [new file with mode: 0644]
src/cml/matrix/matrix_ops.h [new file with mode: 0644]
src/cml/matrix/matrix_print.h [new file with mode: 0644]
src/cml/matrix/matrix_promotions.h [new file with mode: 0644]
src/cml/matrix/matrix_rowcol.h [new file with mode: 0644]
src/cml/matrix/matrix_traits.h [new file with mode: 0644]
src/cml/matrix/matrix_transpose.h [new file with mode: 0644]
src/cml/matrix/matrix_unroller.h [new file with mode: 0644]
src/cml/matvec/matvec_mul.h [new file with mode: 0644]
src/cml/matvec/matvec_promotions.h [new file with mode: 0644]
src/cml/quaternion.h [new file with mode: 0644]
src/cml/quaternion/conjugate.h [new file with mode: 0644]
src/cml/quaternion/inverse.h [new file with mode: 0644]
src/cml/quaternion/quaternion.h [new file with mode: 0644]
src/cml/quaternion/quaternion_comparison.h [new file with mode: 0644]
src/cml/quaternion/quaternion_dot.h [new file with mode: 0644]
src/cml/quaternion/quaternion_expr.h [new file with mode: 0644]
src/cml/quaternion/quaternion_functions.h [new file with mode: 0644]
src/cml/quaternion/quaternion_mul.h [new file with mode: 0644]
src/cml/quaternion/quaternion_ops.h [new file with mode: 0644]
src/cml/quaternion/quaternion_print.h [new file with mode: 0644]
src/cml/quaternion/quaternion_promotions.h [new file with mode: 0644]
src/cml/quaternion/quaternion_traits.h [new file with mode: 0644]
src/cml/quaternion/quatop_macros.h [new file with mode: 0644]
src/cml/util.h [new file with mode: 0644]
src/cml/vector.h [new file with mode: 0644]
src/cml/vector/class_ops.h [new file with mode: 0644]
src/cml/vector/dynamic.h [new file with mode: 0644]
src/cml/vector/external.h [new file with mode: 0644]
src/cml/vector/fixed.h [new file with mode: 0644]
src/cml/vector/vecop_macros.h [new file with mode: 0644]
src/cml/vector/vector_comparison.h [new file with mode: 0644]
src/cml/vector/vector_expr.h [new file with mode: 0644]
src/cml/vector/vector_functions.h [new file with mode: 0644]
src/cml/vector/vector_ops.h [new file with mode: 0644]
src/cml/vector/vector_print.h [new file with mode: 0644]
src/cml/vector/vector_products.h [new file with mode: 0644]
src/cml/vector/vector_promotions.h [new file with mode: 0644]
src/cml/vector/vector_traits.h [new file with mode: 0644]
src/cml/vector/vector_unroller.h [new file with mode: 0644]
src/math.cc [deleted file]
src/math.hh
src/matrix.hh [deleted file]
src/quaternion.hh [deleted file]
src/vector.hh [deleted file]

diff --git a/src/cml/cml.h b/src/cml/cml.h
new file mode 100644 (file)
index 0000000..666bb07
--- /dev/null
@@ -0,0 +1,75 @@
+/* -*- 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 Main CML header to include all CML functionality.
+ *
+ * @todo load vectors, matrices, and quaternions from a stream.
+ *
+ * @todo Move common vector and matrix class ops to a base class (requires
+ * SCOOP-like programming, see below).
+ *
+ * @todo Implement matrix<>::orthogonalize().
+ *
+ * @todo Add is_square<>, is_rectangular<>, etc. to make it easier to
+ * detect specific matrix types.
+ *
+ * @todo Implement dedicated square matrix classes to get rid of duplicated
+ * code in the specialized matrix classes.
+ *
+ * @todo Implement automatic temporary generation, along with expression
+ * node return types for mat-vec and mat-mat operators.
+ *
+ * @todo switch to ssize_t instead of size_t to avoid having to explicitly
+ * deal with wrap-arounds to 2^32-1 when a size_t is subtracted from.
+ *
+ * @todo Finish tests for mat-vec multiply.
+ *
+ * @todo Differentiate between references used for function arguments, and
+ * those used for variable types.  In particular, GCC 3.4 requires const T &
+ * function arguments to ensure complete unrolling/inlining of expressions.
+ *
+ * @todo Specialize matrix multiplication based upon the size type (fixed or
+ * dynamic). This makes a difference for at least GCC 3.4.
+ *
+ * @todo need a build system for the tests/ and examples/ directories.
+ *
+ * @todo clean up the testing infrastructure, and make it easier to add new
+ * tests
+ *
+ * @todo figure out if scalars should be passed by value or reference, or
+ * if it should be determined by traits
+ *
+ * @todo change use of typename and class to be like Alexandrescu book
+ *
+ * @todo figure out if it makes sense to unroll assignment if either the
+ * source expression or the target vector/matrix has a fixed size (right
+ * now, unrolling happens only if the target has a fixed size)
+ *
+ * @todo Allow addition of new types, a la glommable ETs (but simpler).
+ * Can use ideas from "SCOOP" method: Nicolas Burrus, Alexandre Duret-Lutz,
+ * Thierry Géraud, David Lesage and Raphaël Poss. A Static C++
+ * Object-Oriented Programming (SCOOP) Paradigm Mixing Benefits of
+ * Traditional OOP and Generic Programming. In the Proceedings of the
+ * Workshop on Multiple Paradigm with OO Languages (MPOOL'03) Anaheim, CA,
+ * USA Oct. 2003
+ */
+
+#ifndef cml_h
+#define cml_h
+
+#include <cml/vector.h>
+#include <cml/matrix.h>
+#include <cml/quaternion.h>
+#include <cml/util.h>
+#include <cml/mathlib/mathlib.h>
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/constants.h b/src/cml/constants.h
new file mode 100644 (file)
index 0000000..060705c
--- /dev/null
@@ -0,0 +1,87 @@
+/* -*- 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 Useful constants.
+ */
+
+#ifndef cml_constants_h
+#define cml_constants_h
+
+#include <cmath>
+
+#if !defined(M_PI)
+#define M_PI 3.14159265358979323846264338327950288
+#endif
+
+#if !defined(M_SQRT2)
+#define M_SQRT2 1.41421356237309504880168872420969808
+#endif
+
+#if !defined(M_E)
+#define M_E 2.71828182845904523536028747135266250
+#endif
+
+namespace cml {
+
+#if 1
+
+/** Templated constants struct.
+ *
+ * Either float or double can be used.
+ */
+template<typename Float>
+struct constants {
+    static Float pi()           { return M_PI;                             }
+    static Float two_pi()       { return 2.*M_PI;                          }
+    static Float inv_pi()       { return 1./M_PI;                          }
+    static Float inv_two_pi()   { return 1./(2.*M_PI);                     }
+    static Float pi_over_2()    { return M_PI/2.;                          }
+    static Float pi_over_4()    { return M_PI/4.;                          }
+    static Float deg_per_rad()  { return 180./M_PI;                        }
+    static Float rad_per_deg()  { return M_PI/180.;                        }
+    static Float sqrt_2()       { return M_SQRT2;                          }
+    static Float sqrt_3()       { return 1.732050807568877293527446341505; }
+    static Float sqrt_5()       { return 2.236067977499789696409173668731; }
+    static Float sqrt_6()       { return 2.449489742783178098197284074705; }
+    static Float e()            { return M_E;                              }
+};
+
+#else
+
+/* XXX This version requires an explicit instantiation of *every* constant
+ * below, e.g.:
+ *
+ * template<typename F> const F cml::constants<F>::pi;
+ */
+/** Templated constants struct.
+ *
+ * Either float or double can be used.
+ */
+template<typename Float>
+struct constants {
+    static const Float pi = M_PI;
+    static const Float two_pi = 2.*M_PI;
+    static const Float inv_pi = 1./M_PI;                /* 1/pi */
+    static const Float inv_two_pi = 1./(2.*M_PI);       /* 1/(2*pi) */
+    static const Float pi_over_2 = M_PI/2.;             /* pi/2 */
+    static const Float pi_over_4 = M_PI/4.;             /* pi/4 */
+    static const Float deg_per_rad = 180./M_PI;
+    static const Float rad_per_deg = M_PI/180.;
+    static const Float sqrt_2 = M_SQRT2;
+    static const Float sqrt_3 = 1.73205080756887729352744634150587237;
+    static const Float sqrt_5 = 2.23606797749978969640917366873127624;
+    static const Float sqrt_6 = 2.44948974278317809819728407470589139;
+    static const Float e = M_E;
+};
+
+#endif
+
+} // namespace cml
+
+#endif
diff --git a/src/cml/core/cml_assert.h b/src/cml/core/cml_assert.h
new file mode 100644 (file)
index 0000000..9b29c6d
--- /dev/null
@@ -0,0 +1,97 @@
+/* -*- 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
+ *
+ * Macros and template metaprogramming to implement compile- and run-time
+ * assertions.
+ */
+
+#ifndef cml_assert_h
+#define cml_assert_h
+
+#include <cml/core/cml_meta.h>
+
+namespace cml {
+
+/* Join preprocessor macros into a new preprocessor macro: */
+#define CML_JOIN(X,Y) CML_DO_JOIN(X,Y)
+#define CML_DO_JOIN(X,Y) CML_DO_JOIN2(X,Y)
+#define CML_DO_JOIN2(X,Y) X##Y
+
+/* Change a macro value into a string: */
+#define TO_STRING(X) TO_STRING2(X)
+#define TO_STRING2(X) #X
+
+/** Default undefined compile-time assertion struct. */
+template<bool T> struct STATIC_ASSERTION_FAILURE;
+
+/** Struct instantiated when a true assertion is made at compile-time. */
+template<> struct STATIC_ASSERTION_FAILURE<true> {
+    typedef true_type result;
+    enum { value = true };
+};
+
+/** Create a compile-time assertion.
+ *
+ * @note Compile-time assertions must be expressions that can be evaluated at
+ * comile time.  This means that the expression must only rely on constants,
+ * enums, and/or template parameters, not variables having run-time storage
+ * requirements.
+ *
+ * @warning Enclose expressions that have commas with parens, otherwise the
+ * preprocessor will parse the commas as macro argument separators!
+ *
+ * @sa STATIC_ASSERTION_FAILURE
+ */
+#define CML_STATIC_REQUIRE(_E_) \
+    typedef typename STATIC_ASSERTION_FAILURE<(_E_)>::result \
+        CML_JOIN(__cml_assert_test_typedef_, __LINE__)
+
+
+/** A more meaningful compile-time assertion struct.
+ *
+ * The parameter M is a struct type which has been declared but not
+ * defined; e.g. struct this_is_an_error.
+ *
+ * When used with CML_STATIC_REQUIRE_M(<expr>,M), the compiler errors will
+ * contain the struct name at the point of the error.
+ */
+template<bool T, typename M> struct STATIC_ASSERTION_FAILURE_M {
+    typename M::bogus result;
+};
+
+/** Instantiated for true assertions. */
+template<typename M> struct STATIC_ASSERTION_FAILURE_M<true,M> {
+    typedef true_type result;
+    enum { value = true };
+};
+
+/** Create a compile-time assertion with a message.
+ *
+ * @note Compile-time assertions must be expressions that can be evaluated at
+ * comile time.  This means that the expression must only rely on constants,
+ * enums, and/or template parameters, not variables having run-time storage
+ * requirements.
+ *
+ * @warning Enclose expressions that have commas with parens, otherwise the
+ * preprocessor will parse the commas as macro argument separators!
+ *
+ * @sa STATIC_ASSERTION_FAILURE_M
+ */
+#define CML_STATIC_REQUIRE_M(_E_, _M_) \
+    typedef typename STATIC_ASSERTION_FAILURE_M<(_E_),_M_> \
+        ::result CML_JOIN(__bogus_assert_type_, __LINE__)
+
+} // namespace cml
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/core/cml_meta.h b/src/cml/core/cml_meta.h
new file mode 100644 (file)
index 0000000..679f761
--- /dev/null
@@ -0,0 +1,24 @@
+/* -*- 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 A few simple metaprogramming tools.
+ */
+
+#ifndef cml_meta_h
+#define cml_meta_h
+
+/* Include all of the template metaprogramming tools: */
+#include <cml/core/meta/common.h>
+#include <cml/core/meta/if.h>
+#include <cml/core/meta/switch.h>
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/core/common.h b/src/cml/core/common.h
new file mode 100644 (file)
index 0000000..01b3f7e
--- /dev/null
@@ -0,0 +1,87 @@
+/* -*- 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 core_common_h
+#define core_common_h
+
+// XXX This isn't really the right place for this.
+#if defined(_MSC_VER)
+#include <cstdlib>
+#ifndef _SSIZE_T_DEFINED
+#ifdef  _WIN64
+typedef __int64    ssize_t;
+#else
+typedef _W64 int   ssize_t;
+#endif
+#define _SSIZE_T_DEFINED
+#endif
+#endif
+
+#include <cstddef>              // for size_t
+#include <utility>              // for std::pair<>
+#include <cml/defaults.h>
+
+namespace cml {
+
+/** 1D tag (to select array shape). */
+struct oned_tag {};
+
+/** 2D tag (to select array shape). */
+struct twod_tag {};
+
+/** Statically-allocated memory tag. */
+struct fixed_memory_tag {};
+
+/** Dynamically-allocated memory tag. */
+struct dynamic_memory_tag {};
+
+/** Externally-allocated memory tag. */
+struct external_memory_tag {};
+
+/** Statically-sized tag. */
+struct fixed_size_tag {};
+
+/** Runtime-sized tag. */
+struct dynamic_size_tag {};
+
+/** Resizable tag. */
+struct resizable_tag {};
+
+/** Not resizable tag. */
+struct not_resizable_tag {};
+
+/** Unit-sized tag. */
+struct unit_size_tag {};
+
+/** Row-major storage tag. */
+struct row_major {};
+
+/** Col-major storage tag. */
+struct col_major {};
+
+/** Row-vector matrix basis tag. */
+struct row_basis {};
+
+/** Column-vector matrix basis tag. */
+struct col_basis {};
+
+/* This is the pair returned from the matrix size() method, as well as from
+ * the matrix expression size checking code:
+ */
+typedef std::pair<size_t,size_t> matrix_size;
+
+} // namespace cml
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/core/dynamic_1D.h b/src/cml/core/dynamic_1D.h
new file mode 100644 (file)
index 0000000..2aae2c6
--- /dev/null
@@ -0,0 +1,127 @@
+/* -*- 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 dynamic_1D_h
+#define dynamic_1D_h
+
+#include <vector>
+#include <cml/core/common.h>
+#include <cml/dynamic.h>
+
+namespace cml {
+
+/** Dynamically-sized and allocated 1D array.
+ *
+ * @note The allocator should be an STL-compatible allocator.
+ *
+ * @internal The internal array type <em>must</em> have the proper copy
+ * semantics, otherwise copy construction will fail.
+ */
+template<typename Element, class Alloc>
+class dynamic_1D
+{
+  public:
+
+    /* Record the allocator type: */
+    typedef typename Alloc::template rebind<Element>::other allocator_type;
+
+    /* Record the generator: */
+    typedef dynamic<Alloc> generator_type;
+
+    /* Array implementation: */
+    typedef std::vector<Element,allocator_type> array_impl;
+
+    /* Standard: */
+    typedef typename array_impl::value_type value_type;
+    typedef typename array_impl::pointer pointer; 
+    typedef typename array_impl::reference reference; 
+    typedef typename array_impl::const_reference const_reference; 
+    typedef typename array_impl::const_pointer const_pointer; 
+
+    /* For matching by memory type: */
+    typedef dynamic_memory_tag memory_tag;
+
+    /* For matching by size type: */
+    typedef dynamic_size_tag size_tag;
+
+    /* For matching by resizability: */
+    typedef resizable_tag resizing_tag;
+
+    /* For matching by dimensions: */
+    typedef oned_tag dimension_tag;
+
+
+  public:
+
+    /** Dynamic arrays have no fixed size. */
+    enum { array_size = -1 };
+
+
+  public:
+
+    /** Construct a dynamic array with no size. */
+    dynamic_1D() {}
+
+    /** Construct a dynamic array given the size. */
+    explicit dynamic_1D(size_t size) : m_data(size) {}
+
+
+  public:
+
+    /** Return the number of elements in the array. */
+    size_t size() const { return this->m_data.size(); }
+
+    /** Access to the data as a C array.
+     *
+     * @param i a size_t index into the array.
+     * @return a mutable reference to the array value at i.
+     *
+     * @note This function does not range-check the argument.
+     */
+    reference operator[](size_t i) { return this->m_data[i]; }
+
+    /** Const access to the data as a C array.
+     *
+     * @param i a size_t index into the array.
+     * @return a const reference to the array value at i.
+     *
+     * @note This function does not range-check the argument.
+     */
+    const_reference operator[](size_t i) const { return this->m_data[i]; }
+
+    /** Return access to the data as a raw pointer. */
+    pointer data() { return &m_data[0]; }
+
+    /** Return access to the data as a raw pointer. */
+    const_pointer data() const { return &m_data[0]; }
+
+
+  public:
+
+    /** Set the array size to the given value.
+     *
+     * @warning This is not guaranteed to preserve the original data.
+     */
+    void resize(size_t s) { this->m_data.resize(s); }
+
+
+  protected:
+
+    array_impl                  m_data;
+};
+
+} // namespace cml
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/core/dynamic_2D.h b/src/cml/core/dynamic_2D.h
new file mode 100644 (file)
index 0000000..c6ea386
--- /dev/null
@@ -0,0 +1,178 @@
+/* -*- 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 dynamic_2D_h
+#define dynamic_2D_h
+
+#include <vector>
+#include <cml/core/common.h>
+#include <cml/core/dynamic_1D.h>
+#include <cml/dynamic.h>
+
+namespace cml {
+
+/** Dynamically-sized and allocated 2D array.
+ *
+ * @note The allocator should be an STL-compatible allocator.
+ *
+ * @internal The internal array type <em>must</em> have the proper copy
+ * semantics, otherwise copy construction will fail.
+ *
+ * @internal This class does not need a destructor.
+ */
+template<typename Element, typename Layout, class Alloc>
+class dynamic_2D
+{
+  public:
+
+    /* Record the allocator type: */
+    typedef typename Alloc::template rebind<Element>::other allocator_type;
+
+    /* Record the generator: */
+    typedef dynamic<Alloc> generator_type;
+
+    /* Array implementation: */
+    typedef std::vector<Element,allocator_type> array_impl;
+
+    /* Standard: */
+    typedef typename array_impl::value_type value_type;
+    typedef typename array_impl::pointer pointer; 
+    typedef typename array_impl::reference reference; 
+    typedef typename array_impl::const_reference const_reference; 
+    typedef typename array_impl::const_pointer const_pointer; 
+
+    /* For matching by memory layout: */
+    typedef Layout layout;
+
+    /* For matching by memory type: */
+    typedef dynamic_memory_tag memory_tag;
+
+    /* For matching by size type: */
+    typedef dynamic_size_tag size_tag;
+
+    /* For matching by resizability: */
+    typedef resizable_tag resizing_tag;
+
+    /* For matching by dimensions: */
+    typedef twod_tag dimension_tag;
+
+    /* To simplify the matrix transpose operator: */
+    typedef dynamic_2D<Element,Layout,Alloc> transposed_type;
+
+    /* To simplify the matrix row and column operators: */
+    typedef dynamic_1D<Element,Alloc> row_array_type;
+    typedef dynamic_1D<Element,Alloc> col_array_type;
+
+
+  protected:
+
+    /** Construct a dynamic array with no size. */
+    dynamic_2D() {}
+
+    /** Construct a dynamic matrix given the dimensions.
+     *
+     * This constructor is guaranteed to throw only if the allocator throws.
+     * If the array implementation guarantees that the array data structure is
+     * not modified after an exception, then this constructor is
+     * exception-safe.
+     *
+     * @throws only if the allocator throws during an allocation.
+     */
+    explicit dynamic_2D(size_t rows, size_t cols) 
+        : m_rows(rows), m_cols(cols), m_data(rows*cols) {}
+
+
+  public:
+
+    enum { array_rows = -1, array_cols = -1 };
+
+
+  public:
+
+    /** Return the number of rows in the array. */
+    size_t rows() const { return m_rows; }
+
+    /** Return the number of cols in the array. */
+    size_t cols() const { return m_cols; }
+
+
+  public:
+
+    /** Access the given element of the matrix.
+     *
+     * @param row row of element.
+     * @param col column of element.
+     * @returns mutable reference.
+     */
+    reference operator()(size_t row, size_t col) {
+        return get_element(row, col, layout());
+    }
+
+    /** Access the given element of the matrix.
+     *
+     * @param row row of element.
+     * @param col column of element.
+     * @returns const reference.
+     */
+    const_reference operator()(size_t row, size_t col) const {
+        return get_element(row, col, layout());
+    }
+
+    /** Return access to the data as a raw pointer. */
+    pointer data() { return &m_data[0]; }
+
+    /** Return access to the data as a raw pointer. */
+    const_pointer data() const { return &m_data[0]; }
+
+
+  public:
+
+    /** Resize the array.
+     *
+     * @warning This is not guaranteed to preserve the original data.
+     */
+    void resize(size_t rows, size_t cols) {
+        m_data.resize(rows*cols); m_rows = rows; m_cols = cols;
+    }
+
+
+  protected:
+
+    reference get_element(size_t row, size_t col, row_major) {
+        return m_data[row*m_cols + col];
+    }
+
+    const_reference get_element(size_t row, size_t col, row_major) const {
+        return m_data[row*m_cols + col];
+    }
+
+    reference get_element(size_t row, size_t col, col_major) {
+        return m_data[col*m_rows + row];
+    }
+
+    const_reference get_element(size_t row, size_t col, col_major) const {
+        return m_data[col*m_rows + row];
+    }
+
+
+  protected:
+
+    size_t                      m_rows, m_cols;
+    array_impl                  m_data;
+};
+
+} // namespace cml
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/core/external_1D.h b/src/cml/core/external_1D.h
new file mode 100644 (file)
index 0000000..92939e1
--- /dev/null
@@ -0,0 +1,216 @@
+/* -*- 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
+ *
+ * Defines the fixed-size and runtime-sized external 1D arrays.
+ *
+ * @todo Need a better way to designate non-resizable, run-time sized
+ * arrays (e.g. by a resizeable tag).
+ */
+
+#ifndef external_1D_h
+#define external_1D_h
+
+#include <cml/core/common.h>
+#include <cml/core/cml_meta.h>
+#include <cml/core/cml_assert.h>
+#include <cml/external.h>
+
+namespace cml {
+
+/** Fixed-size external 1D array.
+ *
+ * Both the memory and the size are fixed at compile time, and cannot be
+ * changed.
+ */
+template<typename Element, int Size = -1>
+class external_1D
+{
+  public:
+
+    /* Require Size > 0: */
+    CML_STATIC_REQUIRE(Size > 0);
+
+    /* Record the generator: */
+    typedef external<Size,-1> generator_type;
+
+    /* Standard: */
+    typedef Element value_type;
+    typedef Element* pointer;
+    typedef Element& reference;
+    typedef const Element& const_reference;
+    typedef const Element* const_pointer;
+
+    /* Array implementation: */
+    typedef value_type array_impl[Size];
+
+    /* For matching by memory type: */
+    typedef external_memory_tag memory_tag;
+
+    /* For matching by size type: */
+    typedef fixed_size_tag size_tag;
+
+    /* For matching by resizability: */
+    typedef not_resizable_tag resizing_tag;
+
+    /* For matching by dimensions: */
+    typedef oned_tag dimension_tag;
+
+
+  public:
+
+    /** The length as an enumerated value. */
+    enum { array_size = Size };
+
+
+  public:
+
+    external_1D(pointer const ptr)
+        : m_data(ptr) {}
+
+
+  public:
+
+    /** Return the number of elements in the array. */
+    size_t size() const { return size_t(array_size); }
+
+    /** Access to the data as a C array.
+     *
+     * @param i a size_t index into the array.
+     * @return a mutable reference to the array value at i.
+     *
+     * @note This function does not range-check the argument.
+     */
+    reference operator[](size_t i) { return m_data[i]; }
+
+    /** Const access to the data as a C array.
+     *
+     * @param i a size_t index into the array.
+     * @return a const reference to the array value at i.
+     *
+     * @note This function does not range-check the argument.
+     */
+    const_reference operator[](size_t i) const { return m_data[i]; }
+
+    /** Return access to the data as a raw pointer. */
+    pointer data() { return m_data; }
+
+    /** Return access to the data as a raw pointer. */
+    const_pointer data() const { return m_data; }
+
+
+  protected:
+
+    pointer const               m_data;
+
+
+  private:
+
+    /* Initialization without an argument isn't allowed: */
+    external_1D();
+};
+
+/** Run-time sized external 1D array.
+ *
+ * Both the memory and the size are fixed at run-time, and cannot be
+ * changed.  This is a specialization for the case that Rows and Cols are
+ * not specified (i.e. given as the default of -1,-1).
+ */
+template<typename Element>
+class external_1D<Element,-1>
+{
+  public:
+
+    /* Record the generator.  Note: this is *not* unique, as it is the same
+     * generator used by external_2D.  However, external_2D is used only by
+     * matrix<> classes, so this is not a problem.
+     */
+    typedef external<> generator_type;
+
+    /* Standard: */
+    typedef Element value_type;
+    typedef Element* pointer;
+    typedef Element& reference;
+    typedef const Element& const_reference;
+    typedef const Element* const_pointer;
+
+    /* For matching by memory type: */
+    typedef external_memory_tag memory_tag;
+
+    /* For matching by size type: */
+    typedef dynamic_size_tag size_tag;
+
+    /* For matching by resizability: */
+    typedef not_resizable_tag resizing_tag;
+
+    /* For matching by dimensions: */
+    typedef oned_tag dimension_tag;
+
+
+  public:
+
+    /** The length as an enumerated value. */
+    enum { array_size = -1 };
+
+
+  public:
+
+    external_1D(pointer const ptr, size_t size)
+        : m_data(ptr), m_size(size) {}
+
+
+  public:
+
+    /** Return the number of elements in the array. */
+    size_t size() const { return m_size; }
+
+    /** Access to the data as a C array.
+     *
+     * @param i a size_t index into the array.
+     * @return a mutable reference to the array value at i.
+     *
+     * @note This function does not range-check the argument.
+     */
+    reference operator[](size_t i) { return m_data[i]; }
+
+    /** Const access to the data as a C array.
+     *
+     * @param i a size_t index into the array.
+     * @return a const reference to the array value at i.
+     *
+     * @note This function does not range-check the argument.
+     */
+    const_reference operator[](size_t i) const { return m_data[i]; }
+
+    /** Return access to the data as a raw pointer. */
+    pointer data() { return m_data; }
+
+    /** Return access to the data as a raw pointer. */
+    const_pointer data() const { return m_data; }
+
+
+  protected:
+
+    pointer const               m_data;
+    const size_t                m_size;
+
+
+  private:
+
+    /* Initialization without an argument isn't allowed: */
+    external_1D();
+};
+
+} // namespace cml
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/core/external_2D.h b/src/cml/core/external_2D.h
new file mode 100644 (file)
index 0000000..7461c51
--- /dev/null
@@ -0,0 +1,304 @@
+/* -*- 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
+ *
+ * Defines the fixed-size and runtime-sized external 2D arrays.
+ *
+ * @todo Would casting get better performance in the external_2D<> element
+ * access methods?
+ */
+
+#ifndef external_2D_h
+#define external_2D_h
+
+#include <cml/core/common.h>
+#include <cml/core/fixed_1D.h>
+#include <cml/core/fixed_2D.h>
+#include <cml/core/dynamic_1D.h>
+#include <cml/core/dynamic_2D.h>
+#include <cml/external.h>
+
+namespace cml {
+
+/** Fixed-size external 2D array.
+ *
+ * Both the memory and the size are fixed at compile time, and cannot be
+ * changed.
+ */
+template<typename Element, int Rows, int Cols, typename Layout>
+class external_2D
+{
+  public:
+
+    /* Require Rows > 0, Cols > 0: */
+    CML_STATIC_REQUIRE((Rows > 0) && (Cols > 0));
+
+    /* Record the generator: */
+    typedef external<Rows,Cols> generator_type;
+
+    /* Standard: */
+    typedef Element value_type;
+    typedef Element* pointer;
+    typedef Element& reference;
+    typedef const Element& const_reference;
+    typedef const Element* const_pointer;
+
+    /* For matching by memory layout: */
+    typedef Layout layout;
+
+    /* For matching by memory type: */
+    typedef external_memory_tag memory_tag;
+
+    /* For matching by size type: */
+    typedef fixed_size_tag size_tag;
+
+    /* For matching by resizability: */
+    typedef not_resizable_tag resizing_tag;
+
+    /* For matching by dimensions: */
+    typedef twod_tag dimension_tag;
+
+    /* To simplify the matrix transpose operator: */
+    typedef fixed_2D<Element,Cols,Rows,Layout> transposed_type;
+    /* Note: the transposed type must be fixed_2D, since an external array
+     * cannot be specified without a corresponding memory location.
+     */
+
+    /* To simplify the matrix row and column operators: */
+    typedef fixed_1D<Element,Rows> row_array_type;
+    typedef fixed_1D<Element,Cols> col_array_type;
+    /* Note: the row types must be fixed_1D, since external arrays cannot be
+     * specified without a memory location.
+     */
+
+
+  public:
+
+    enum { array_rows = Rows, array_cols = Cols };
+
+
+  public:
+
+    /** Construct an external array from a pointer. */
+    external_2D(value_type const ptr[Rows][Cols])
+        : m_data(const_cast<pointer>(&ptr[0][0])) {}
+
+    /** Construct an external array from a pointer. */
+    external_2D(value_type* const ptr) : m_data(ptr) {}
+
+
+  public:
+
+    /** Return the number of rows in the array. */
+    size_t rows() const { return size_t(array_rows); }
+
+    /** Return the number of cols in the array. */
+    size_t cols() const { return size_t(array_cols); }
+
+
+  public:
+
+    /** Access element (row,col) of the matrix.
+     *
+     * @param row row of element.
+     * @param col column of element.
+     * @returns mutable reference.
+     *
+     * @note This function does not range-check the arguments.
+     */
+    reference operator()(size_t row, size_t col) {
+        /* Dispatch to the right function based on layout: */
+        return get_element(row,col,layout());
+    }
+
+    /** Const access element (row,col) of the matrix.
+     *
+     * @param row row of element.
+     * @param col column of element.
+     * @returns const reference.
+     *
+     * @note This function does not range-check the arguments.
+     */
+    const_reference operator()(size_t row, size_t col) const {
+        /* Dispatch to the right function based on layout: */
+        return get_element(row,col,layout());
+    }
+
+    /** Return access to the data as a raw pointer. */
+    pointer data() { return m_data; }
+
+    /** Return access to the data as a raw pointer. */
+    const_pointer data() const { return m_data; }
+
+
+  protected:
+
+    /* XXX May be able to cast to get better performance? */
+    reference get_element(size_t row, size_t col, row_major) {
+        return m_data[row*Cols + col];
+    }
+
+    const_reference get_element(size_t row, size_t col, row_major) const {
+        return m_data[row*Cols + col];
+    }
+
+    reference get_element(size_t row, size_t col, col_major) {
+        return m_data[col*Rows + row];
+    }
+
+    const_reference get_element(size_t row, size_t col, col_major) const {
+        return m_data[col*Rows + row];
+    }
+
+
+  protected:
+
+    /* Declare the data array: */
+    pointer const               m_data;
+};
+
+/** Run-time sized external 2D array.
+ *
+ * Both the memory and the size are fixed at run-time, but cannot be changed.
+ * This is a specialization for the case that Rows and Cols are not specified
+ * (i.e. given as the default of -1,-1).
+ */
+template<typename Element, typename Layout>
+class external_2D<Element,-1,-1,Layout>
+{
+  public:
+
+    /* Record the generator.  Note: this is *not* unique, as it is the same
+     * generator used by external_1D.  However, external_1D is used only by
+     * vector<> classes, so this is not a problem.
+     */
+    typedef external<> generator_type;
+
+    /* Standard: */
+    typedef Element value_type;
+    typedef Element* pointer;
+    typedef Element& reference;
+    typedef const Element& const_reference;
+    typedef const Element* const_pointer;
+
+    /* For matching by memory layout: */
+    typedef Layout layout;
+
+    /* For matching by memory type: */
+    typedef external_memory_tag memory_tag;
+
+    /* For matching by size type: */
+    typedef dynamic_size_tag size_tag;
+
+    /* For matching by resizability: */
+    typedef not_resizable_tag resizing_tag;
+
+    /* For matching by dimensions: */
+    typedef twod_tag dimension_tag;
+
+    /* To simplify the matrix transpose operator: */
+    typedef dynamic_2D<Element,Layout, CML_DEFAULT_ARRAY_ALLOC>
+        transposed_type;
+
+    /* To simplify the matrix row and column operators: */
+    typedef dynamic_1D<Element, CML_DEFAULT_ARRAY_ALLOC> row_array_type;
+    typedef dynamic_1D<Element, CML_DEFAULT_ARRAY_ALLOC> col_array_type;
+
+
+  public:
+
+    enum { array_rows = -1, array_cols = -1 };
+
+
+  public:
+
+    /** Construct an external array with no size. */
+    external_2D(pointer const ptr, size_t rows, size_t cols)
+        : m_data(ptr), m_rows(rows), m_cols(cols) {}
+
+
+  public:
+
+    /** Return the number of rows in the array. */
+    size_t rows() const { return m_rows; }
+
+    /** Return the number of cols in the array. */
+    size_t cols() const { return m_cols; }
+
+
+  public:
+
+    /** Access element (row,col) of the matrix.
+     *
+     * @param row row of element.
+     * @param col column of element.
+     * @returns mutable reference.
+     *
+     * @note This function does not range-check the arguments.
+     */
+    reference operator()(size_t row, size_t col) {
+        /* Dispatch to the right function based on layout: */
+        return get_element(row,col,layout());
+    }
+
+    /** Const access element (row,col) of the matrix.
+     *
+     * @param row row of element.
+     * @param col column of element.
+     * @returns const reference.
+     *
+     * @note This function does not range-check the arguments.
+     */
+    const_reference operator()(size_t row, size_t col) const {
+        /* Dispatch to the right function based on layout: */
+        return get_element(row,col,layout());
+    }
+
+    /** Return access to the data as a raw pointer. */
+    pointer data() { return m_data; }
+
+    /** Return access to the data as a raw pointer. */
+    const_pointer data() const { return m_data; }
+
+
+  protected:
+
+    /* XXX May be able to cast to get better performance? */
+    reference get_element(size_t row, size_t col, row_major) {
+        return m_data[row*m_cols + col];
+    }
+
+    const_reference get_element(size_t row, size_t col, row_major) const {
+        return m_data[row*m_cols + col];
+    }
+
+    reference get_element(size_t row, size_t col, col_major) {
+        return m_data[col*m_rows + row];
+    }
+
+    const_reference get_element(size_t row, size_t col, col_major) const {
+        return m_data[col*m_rows + row];
+    }
+
+
+  protected:
+
+    /* Declare the data array: */
+    value_type* const           m_data;
+    const size_t                m_rows;
+    const size_t                m_cols;
+};
+
+} // namespace cml
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/core/fixed_1D.h b/src/cml/core/fixed_1D.h
new file mode 100644 (file)
index 0000000..eb691af
--- /dev/null
@@ -0,0 +1,130 @@
+/* -*- 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 fixed_1D_h
+#define fixed_1D_h
+
+#include <cml/core/common.h>
+#include <cml/core/cml_meta.h>
+#include <cml/core/cml_assert.h>
+#include <cml/fixed.h>
+
+namespace cml {
+
+/** Statically-allocated array.
+ *
+ * @note This class is designed to have the same size as a C array with the
+ * same length.  It's therefore possible (but not recommended!) to coerce
+ * a normal C array into a fixed_1D<> like this:
+ *
+ * typedef fixed_1D<double,10> array;
+ * double c_array[10];
+ * array& array_object = *((array*)&c_array);
+ * double e1 = array_object[1];
+ *
+ * It's also possible to do this with a pointer to an array of values (e.g. a
+ * double*), whether or not it was actually declared as a fixed C array.  This
+ * is HIGHLY DISCOURAGED, though. It's relatively straightforward to implement
+ * a separate class to take a C array (or pointer) and turn it into an array
+ * object.
+ *
+ * @sa cml::fixed
+ *
+ * @internal Do <em>not</em> add the empty constructor and destructor; at
+ * least one compiler (Intel C++ 9.0) fails to optimize them away, and they
+ * aren't needed anyway here.
+ */
+template<typename Element, int Size>
+class fixed_1D
+{
+  public:
+
+    /* Require Size > 0: */
+    CML_STATIC_REQUIRE(Size > 0);
+
+    /* Record the generator: */
+    typedef fixed<Size,-1> generator_type;
+
+    /* Standard: */
+    typedef Element value_type;
+    typedef Element* pointer;
+    typedef Element& reference;
+    typedef const Element& const_reference;
+    typedef const Element* const_pointer;
+
+    /* Array implementation: */
+    typedef value_type array_impl[Size];
+
+    /* For matching by memory type: */
+    typedef fixed_memory_tag memory_tag;
+
+    /* For matching by size type: */
+    typedef fixed_size_tag size_tag;
+
+    /* For matching by resizability: */
+    typedef not_resizable_tag resizing_tag;
+
+    /* For matching by dimensions: */
+    typedef oned_tag dimension_tag;
+
+
+  public:
+
+    /** The length as an enumerated value. */
+    enum { array_size = Size };
+
+
+  public:
+
+    /** Return the number of elements in the array. */
+    size_t size() const { return size_t(array_size); }
+
+    /** Access to the data as a C array.
+     *
+     * @param i a size_t index into the array.
+     * @return a mutable reference to the array value at i.
+     *
+     * @note This function does not range-check the argument.
+     */
+    reference operator[](size_t i) { return m_data[i]; }
+
+    /** Const access to the data as a C array.
+     *
+     * @param i a size_t index into the array.
+     * @return a const reference to the array value at i.
+     *
+     * @note This function does not range-check the argument.
+     */
+    const_reference operator[](size_t i) const { return m_data[i]; }
+
+    /** Return access to the data as a raw pointer. */
+    pointer data() { return &m_data[0]; }
+
+    /** Return access to the data as a raw pointer. */
+    const_pointer data() const { return &m_data[0]; }
+
+  protected:
+
+    fixed_1D() {}
+
+
+  protected:
+
+    array_impl                  m_data;
+};
+
+} // namespace cml
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/core/fixed_2D.h b/src/cml/core/fixed_2D.h
new file mode 100644 (file)
index 0000000..44af7e4
--- /dev/null
@@ -0,0 +1,204 @@
+/* -*- 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 fixed_2D_h
+#define fixed_2D_h
+
+#include <cml/core/common.h>
+#include <cml/core/fixed_1D.h>
+
+/* This is used below to create a more meaningful compile-time error when
+ * an unknown layout argument is given:
+ */
+struct invalid_layout_type_error;
+
+/* This is used below to create a more meaningful compile-time error when
+ * a negative size is given.
+ */
+struct negative_array_size_error;
+
+namespace cml {
+
+/** The internal statically-allocated 2D-array implementation class.
+ *
+ * This uses an internal class to setup the data matrix with the proper
+ * layout.  The alternative is to use a 1D array with size Rows*Cols and a
+ * multiplication to dereference an element, but it seems that compilers
+ * better optimize 2D array dereferences.  This is different from
+ * dynamic_2D<>, which must use the 1D array method.
+ *
+ * @sa cml::fixed
+ *
+ * @note This class is designed to have the same size as a C array with the
+ * same dimensions.  It's therefore possible (but not recommended!) to coerce
+ * a normal C array into a fixed_2D<> like this:
+ *
+ * typedef fixed_2D<double,10,10,row_major> array;
+ * double c_array[10][10];
+ * array& array_object = *((array*)&c_array);
+ * double e11 = array_object[1][1];
+ *
+ * It's also possible to do this with a pointer to an array of values (e.g. a
+ * double*), whether or not it was actually declared as a fixed C array.  This
+ * is HIGHLY DISCOURAGED, though, since it's relatively straightforward to
+ * implement a separate class to take a C array (or pointer) and turn it into
+ * an array object.
+ *
+ * @internal Do <em>not</em> add the empty constructor and destructor; at
+ * least one compiler (Intel C++ 9.0) fails to optimize them away, and they
+ * aren't needed anyway here.
+ */
+template<typename Element, int Rows, int Cols, typename Layout>
+class fixed_2D
+{
+  public:
+
+    /* Require Rows > 0, Cols > 0: */
+    CML_STATIC_REQUIRE_M(
+            (Rows > 0) && (Cols > 0),
+            negative_array_size_error);
+
+    /* Require Layout to be row_major or col_major: */
+    CML_STATIC_REQUIRE_M(
+            (same_type<Layout,row_major>::is_true
+             || same_type<Layout,col_major>::is_true),
+            invalid_layout_type_error);
+
+
+    /* Record the generator: */
+    typedef fixed<Rows,Cols> generator_type;
+
+    /* Standard: */
+    typedef Element value_type;
+    typedef Element* pointer;
+    typedef Element& reference;
+    typedef const Element& const_reference;
+    typedef const Element* const_pointer;
+
+    /* For matching by memory layout: */
+    typedef Layout layout;
+
+    /* For matching by memory type: */
+    typedef fixed_memory_tag memory_tag;
+
+    /* For matching by size type: */
+    typedef fixed_size_tag size_tag;
+
+    /* For matching by resizability: */
+    typedef not_resizable_tag resizing_tag;
+
+    /* For matching by dimensions: */
+    typedef twod_tag dimension_tag;
+
+    /* To simplify the matrix transpose operator: */
+    typedef fixed_2D<Element,Cols,Rows,Layout> transposed_type;
+
+    /* To simplify the matrix row and column operators: */
+    typedef fixed_1D<Element,Rows> row_array_type;
+    typedef fixed_1D<Element,Cols> col_array_type;
+
+
+  public:
+
+    enum { array_rows = Rows, array_cols = Cols };
+
+
+  public:
+
+    /** Return the number of rows in the array. */
+    size_t rows() const { return size_t(array_rows); }
+
+    /** Return the number of cols in the array. */
+    size_t cols() const { return size_t(array_cols); }
+
+
+  public:
+
+    /** Access element (row,col) of the matrix.
+     *
+     * @param row row of element.
+     * @param col column of element.
+     * @returns mutable reference.
+     *
+     * @note This function does not range-check the arguments.
+     */
+    reference operator()(size_t row, size_t col) {
+        /* Dispatch to the right function based on layout: */
+        return get_element(row,col,layout());
+    }
+
+    /** Const access element (row,col) of the matrix.
+     *
+     * @param row row of element.
+     * @param col column of element.
+     * @returns const reference.
+     *
+     * @note This function does not range-check the arguments.
+     */
+    const_reference operator()(size_t row, size_t col) const {
+        /* Dispatch to the right function based on layout: */
+        return get_element(row,col,layout());
+    }
+
+    /** Return access to the data as a raw pointer. */
+    pointer data() { return &m_data[0][0]; }
+
+    /** Return access to the data as a raw pointer. */
+    const_pointer data() const { return &m_data[0][0]; }
+
+
+  public:
+
+    fixed_2D() {}
+
+
+  protected:
+
+    reference get_element(size_t row, size_t col, row_major) {
+        return m_data[row][col];
+    }
+
+    const_reference get_element(size_t row, size_t col, row_major) const {
+        return m_data[row][col];
+    }
+
+    reference get_element(size_t row, size_t col, col_major) {
+        return m_data[col][row];
+    }
+
+    const_reference get_element(size_t row, size_t col, col_major) const {
+        return m_data[col][row];
+    }
+
+
+  protected:
+
+    /* Typedef the possible layouts: */
+    typedef Element row_major_array[Rows][Cols];
+    typedef Element col_major_array[Cols][Rows];
+
+    /* Now, select the right layout for the current matrix: */
+    typedef typename select_switch<
+        Layout, row_major, row_major_array,     /* Case 1 */
+                col_major, col_major_array      /* Case 2 */
+        >::result array_data;
+
+    /* Declare the data array: */
+    array_data                  m_data;
+};
+
+} // namespace cml
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/core/fwd.h b/src/cml/core/fwd.h
new file mode 100644 (file)
index 0000000..84fada0
--- /dev/null
@@ -0,0 +1,63 @@
+/* -*- 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
+ *
+ * Forward declarations, useful to avoid including lots of headers.
+ *
+ * @sa cml/et/array_promotions.h
+ */
+
+#ifndef core_fwd_h
+#define core_fwd_h
+
+namespace cml {
+
+/* cml/core/fixed_1D.h */
+template<typename E, int S> class fixed_1D;
+
+/* cml/core/fixed_2D.h */
+template<typename E, int R, int C, class L> class fixed_2D;
+
+/* cml/core/dynamic_1D.h */
+template<typename E, class A> class dynamic_1D;
+
+/* cml/core/dynamic_2D.h */
+template<typename E, class L, class A> class dynamic_2D;
+
+/* cml/core/external_1D.h */
+template<typename E, int S> class external_1D;
+
+/* cml/core/external_2D.h */
+template<typename E, int R, int C, class L> class external_2D;
+
+/* cml/fixed.h */
+template<int Dim1, int Dim2> struct fixed;
+
+/* cml/dynamic.h */
+template<class Alloc> struct dynamic;
+
+/* cml/external.h */
+template<int Dim1, int Dim2> struct external;
+
+/* cml/vector.h */
+template<typename E, class AT> class vector;
+
+/* cml/matrix.h */
+template<typename E, class AT, class BO, class L> class matrix;
+
+/* cml/quaternion.h */
+template<typename E, class AT, class OT, class CT> class quaternion;
+
+} // namespace cml
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/core/meta/common.h b/src/cml/core/meta/common.h
new file mode 100644 (file)
index 0000000..6d9b876
--- /dev/null
@@ -0,0 +1,88 @@
+/* -*- 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 core_meta_common_h
+#define core_meta_common_h
+
+namespace cml {
+
+/** Type of a true statement. */
+struct true_type {};
+
+/** Type of a false statement. */
+struct false_type {};
+
+template<bool B> struct is_true {
+    typedef false_type result;
+};
+
+template<> struct is_true<true> {
+    typedef true_type result;
+};
+
+/** A "type pair". */
+template<typename T1, typename T2> struct type_pair {
+    typedef T1 first;
+    typedef T2 second;
+};
+
+/** A "type quadruple". */
+template<typename T1, typename T2, typename T3, typename T4>
+struct type_quad {
+    typedef T1 first;
+    typedef T2 second;
+    typedef T3 third;
+    typedef T3 fourth;
+};
+
+/** Match any type (for use with same_type<> and select_switch<>). */
+struct any_type {};
+
+/** Determine if two types are the same.
+ *
+ * Defaults to false.
+ */
+template<typename T, typename U> struct same_type {
+    typedef false_type result;
+    enum { is_true = false, is_false = true };
+};
+
+/** Match the same type for both of same_type's template arguments. */
+template<typename T> struct same_type<T,T> {
+    typedef true_type result;
+    enum { is_true = true, is_false = false };
+};
+
+/** Match a type and any_type. */
+template<typename T> struct same_type<T,any_type> {
+    typedef true_type result;
+    enum { is_true = true, is_false = false };
+};
+
+/** Match a type and any_type. */
+template<typename T> struct same_type<any_type,T> {
+    typedef true_type result;
+    enum { is_true = true, is_false = false };
+};
+
+/** Disambiguate pair of any_type's. */
+template<> struct same_type<any_type,any_type> {
+    typedef true_type result;
+    enum { is_true = true, is_false = false };
+};
+
+} // namespace cml
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/core/meta/if.h b/src/cml/core/meta/if.h
new file mode 100644 (file)
index 0000000..1ea3235
--- /dev/null
@@ -0,0 +1,42 @@
+/* -*- 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 meta_if_h
+#define meta_if_h
+
+#include <cml/core/meta/common.h>
+
+namespace cml {
+
+/** Select argument type based upon truth value. */
+template<bool yn, typename TrueT, typename FalseT> struct select_if;
+
+/** Result is TrueT if true. */
+template<typename TrueT, typename FalseT>
+struct select_if<true,TrueT,FalseT> {
+    typedef TrueT result;
+    enum { is_true = true };
+};
+
+/** Result is FalseT if false. */
+template<typename TrueT, typename FalseT>
+struct select_if<false,TrueT,FalseT> {
+    typedef FalseT result;
+    enum { is_true = false };
+};
+
+} // namespace cml
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/core/meta/switch.h b/src/cml/core/meta/switch.h
new file mode 100644 (file)
index 0000000..6f4383a
--- /dev/null
@@ -0,0 +1,173 @@
+/* -*- 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 meta_switch_h
+#define meta_switch_h
+
+#include <cml/core/meta/common.h>
+#include <cml/core/meta/if.h>
+
+namespace cml {
+
+struct NilCase {};      /* For terminating the case list. */
+struct Default {};      /* For indicating the default result. */
+
+/* The working parts of the meta-switch go into namespace meta: */
+namespace meta {
+
+/* "Interior" case statements: */
+template<typename Case, typename Result, typename NextCase>
+struct select_case
+{
+    template<typename Find> struct match {
+        typedef typename select_if<
+            same_type<Find,Case>::is_true,
+            Result,
+            typename NextCase::template match<Find>::result
+        >::result result;
+    };
+};
+
+/* Default case, returned when no match is found in a previous case: */
+template<typename Result>
+struct select_case<Default,Result,NilCase>
+{
+    template<typename Find> struct match {
+        typedef Result result;
+    };
+};
+
+/* The last case statement (if no match until now, the result is 'void'): */
+template<typename Case, typename Result>
+struct select_case<Case,Result,NilCase>
+{
+    template<typename Find> struct match {
+        typedef typename select_if<
+            same_type<Find,Case>::is_true,
+            Result,
+            void
+        >::result result;
+    };
+};
+
+} // namespace meta
+
+/** Return the matched type (like a switch/case statement).
+ *
+ * This is a convenience wrapper to avoid having to explicitly type out
+ * select_case for each case in the list of types to match against.
+ */
+template<typename Find
+, typename T1,           typename R1
+, typename T2 = NilCase, typename R2 = void
+, typename T3 = NilCase, typename R3 = void
+, typename T4 = NilCase, typename R4 = void
+, typename T5 = NilCase, typename R5 = void
+, typename T6 = NilCase, typename R6 = void
+, typename T7 = NilCase, typename R7 = void
+, typename T8 = NilCase, typename R8 = void
+, typename T9 = NilCase, typename R9 = void
+, typename T10 = NilCase, typename R10 = void
+, typename T11 = NilCase, typename R11 = void
+, typename T12 = NilCase, typename R12 = void
+, typename T13 = NilCase, typename R13 = void
+, typename T14 = NilCase, typename R14 = void
+, typename T15 = NilCase, typename R15 = void
+, typename T16 = NilCase, typename R16 = void
+#if !defined(_MSC_VER)
+, typename T17 = NilCase, typename R17 = void
+, typename T18 = NilCase, typename R18 = void
+, typename T19 = NilCase, typename R19 = void
+, typename T20 = NilCase, typename R20 = void
+, typename T21 = NilCase, typename R21 = void
+, typename T22 = NilCase, typename R22 = void
+, typename T23 = NilCase, typename R23 = void
+, typename T24 = NilCase, typename R24 = void
+, typename T25 = NilCase, typename R25 = void
+, typename T26 = NilCase, typename R26 = void
+, typename T27 = NilCase, typename R27 = void
+, typename T28 = NilCase, typename R28 = void
+, typename T29 = NilCase, typename R29 = void
+, typename T30 = NilCase, typename R30 = void
+, typename T31 = NilCase, typename R31 = void
+, typename T32 = NilCase, typename R32 = void
+, typename T33 = NilCase, typename R33 = void
+, typename T34 = NilCase, typename R34 = void
+, typename T35 = NilCase, typename R35 = void
+, typename T36 = NilCase, typename R36 = void
+, typename T37 = NilCase, typename R37 = void
+, typename T38 = NilCase, typename R38 = void
+, typename T39 = NilCase, typename R39 = void
+, typename T40 = NilCase, typename R40 = void
+#endif
+> struct select_switch
+{
+    typedef typename
+          meta::select_case< T1,R1
+        , meta::select_case< T2,R2
+        , meta::select_case< T3,R3
+        , meta::select_case< T4,R4
+        , meta::select_case< T5,R5
+        , meta::select_case< T6,R6
+        , meta::select_case< T7,R7
+        , meta::select_case< T8,R8
+        , meta::select_case< T9,R9
+        , meta::select_case< T10,R10
+        , meta::select_case< T11,R11
+        , meta::select_case< T12,R12
+        , meta::select_case< T13,R13
+        , meta::select_case< T14,R14
+        , meta::select_case< T15,R15
+        , meta::select_case< T16,R16
+#if !defined(_MSC_VER)
+        , meta::select_case< T17,R17
+        , meta::select_case< T18,R18
+        , meta::select_case< T19,R19
+        , meta::select_case< T20,R20
+        , meta::select_case< T21,R21
+        , meta::select_case< T22,R22
+        , meta::select_case< T23,R23
+        , meta::select_case< T24,R24
+        , meta::select_case< T25,R25
+        , meta::select_case< T26,R26
+        , meta::select_case< T27,R27
+        , meta::select_case< T28,R28
+        , meta::select_case< T29,R29
+        , meta::select_case< T30,R30
+        , meta::select_case< T31,R31
+        , meta::select_case< T32,R32
+        , meta::select_case< T33,R33
+        , meta::select_case< T34,R34
+        , meta::select_case< T35,R35
+        , meta::select_case< T36,R36
+        , meta::select_case< T37,R37
+        , meta::select_case< T38,R38
+        , meta::select_case< T39,R39
+        , meta::select_case< T40,R40
+        , NilCase
+        > > > > > > > > > >     /* 10 */
+        > > > > > > > > > >     /* 10 */
+        > > > >    /* 4 */
+#else
+        , NilCase
+#endif
+        > > > > > >     /* 6 */
+        > > > > > > > > > >     /* 10 */
+        ::template match<Find>::result result;
+};
+
+} // namespace cml
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/defaults.h b/src/cml/defaults.h
new file mode 100644 (file)
index 0000000..4b7c4cf
--- /dev/null
@@ -0,0 +1,89 @@
+/* -*- 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 Default values for certain parameters.
+ */
+
+#ifndef defaults_h
+#define defaults_h
+
+#if defined(_MSC_VER)
+
+#if _MSC_VER >= 1400
+
+/* Ignore "C4003: not enough actual parameters for macro": */
+#pragma warning (disable: 4003)
+
+/* This one is odd, but apparently harmless (but should be fixed!):
+ * "C4348: redefinition of default parameter"
+ */
+#pragma warning (disable: 4348)
+
+#endif
+
+#endif
+
+/* The default vector unroll limit: */
+#if !defined(CML_VECTOR_UNROLL_LIMIT)
+#define CML_VECTOR_UNROLL_LIMIT 8
+#endif
+
+/* Don't unroll matrix operations by default: */
+#if !defined(CML_2D_UNROLLER) && !defined(CML_NO_2D_UNROLLER)
+#define CML_NO_2D_UNROLLER
+#endif
+
+/* The default vector dot() unroll limit: */
+#if !defined(CML_VECTOR_DOT_UNROLL_LIMIT)
+#define CML_VECTOR_DOT_UNROLL_LIMIT CML_VECTOR_UNROLL_LIMIT
+#endif
+
+/* The default array layout is the C/C++ row-major array layout: */
+#if !defined(CML_DEFAULT_ARRAY_LAYOUT)
+#define CML_DEFAULT_ARRAY_LAYOUT cml::row_major
+#endif
+
+/* The default basis orientation: */
+#if !defined(CML_DEFAULT_BASIS_ORIENTATION)
+#define CML_DEFAULT_BASIS_ORIENTATION cml::col_basis
+#endif
+
+/* Always use the default layout in promotions, by default: */
+#if !defined(CML_ALWAYS_PROMOTE_TO_DEFAULT_LAYOUT)
+#define CML_ALWAYS_PROMOTE_TO_DEFAULT_LAYOUT
+#endif
+
+/* The default memory allocator is std::allocator<void>: */
+#if !defined(CML_DEFAULT_ARRAY_ALLOC)
+#include <memory>               // for std::allocator
+#define CML_DEFAULT_ARRAY_ALLOC std::allocator<void>
+#endif
+
+/* By default, automatically resize dynamic vectors and matrices: */
+#if !defined(CML_AUTOMATIC_VECTOR_RESIZE_ON_ASSIGNMENT)
+#define CML_AUTOMATIC_VECTOR_RESIZE_ON_ASSIGNMENT
+#endif
+
+#if !defined(CML_AUTOMATIC_MATRIX_RESIZE_ON_ASSIGNMENT)
+#define CML_AUTOMATIC_MATRIX_RESIZE_ON_ASSIGNMENT
+#endif
+
+/* By default, check vector and matrix sizes: */
+#if !defined(CML_CHECK_VECTOR_EXPR_SIZES)
+#define CML_CHECK_VECTOR_EXPR_SIZES
+#endif
+
+#if !defined(CML_CHECK_MATRIX_EXPR_SIZES)
+#define CML_CHECK_MATRIX_EXPR_SIZES
+#endif
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/dynamic.h b/src/cml/dynamic.h
new file mode 100644 (file)
index 0000000..8c942b4
--- /dev/null
@@ -0,0 +1,35 @@
+/* -*- 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 dynamic_h
+#define dynamic_h
+
+#include <cml/defaults.h>
+
+namespace cml {
+
+/** This is a selector for dynamic 1D and 2D arrays.
+ *
+ * The dynamic<> struct has no implementation; it is used only to select a
+ * 1D or 2D array type as the base class of a vector or matrix.
+ *
+ * @sa fixed
+ * @sa external
+ */
+template<class Alloc = CML_DEFAULT_ARRAY_ALLOC> struct dynamic;
+
+} // namespace cml
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/et/array_promotions.h b/src/cml/et/array_promotions.h
new file mode 100644 (file)
index 0000000..418211d
--- /dev/null
@@ -0,0 +1,288 @@
+/* -*- 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
+ *
+ * Defines promotions between array types.
+ *
+ * @todo Can/should an expression with a fixed-size argument promote to a
+ * fixed array instead of a dynamic array?
+ */
+
+#ifndef array_promotions_h
+#define array_promotions_h
+
+#include <cml/core/cml_meta.h>
+#include <cml/et/scalar_promotions.h>
+
+namespace cml {
+namespace et {
+
+#define VAL_MAX(_a_,_b_)        ( ((_a_)>(_b_))?(_a_):(_b_) )
+
+namespace detail {
+
+/* This is specialized for 1D and 2D promotions: */
+template<class A1, class A2, typename DTag1, typename DTag2,
+    typename PromotedSizeTag> struct promote;
+
+/* Promote 1D fixed-size arrays to a 1D fixed-size array: */
+template<class A1, class A2>
+struct promote<A1,A2,oned_tag,oned_tag,fixed_size_tag>
+{
+    typedef typename A1::value_type left_scalar;
+    typedef typename A2::value_type right_scalar;
+
+    /* First, promote the scalar type: */
+    typedef typename ScalarPromote<
+        left_scalar,right_scalar>::type promoted_scalar;
+
+    /* Next, deduce the array size: */
+    enum { Size = VAL_MAX((size_t)A1::array_size, (size_t)A2::array_size) };
+
+    /* Finally, generate the promoted array type: */
+    typedef fixed_1D<promoted_scalar,Size> type;
+};
+
+/* Promote 1D dynamic arrays to a 1D dynamic array: */
+template<class A1, class A2>
+struct promote<A1,A2,oned_tag,oned_tag,dynamic_size_tag>
+{
+    typedef typename A1::value_type left_scalar;
+    typedef typename A2::value_type right_scalar;
+
+    /* First, promote the scalar type: */
+    typedef typename ScalarPromote<
+        left_scalar,right_scalar>::type promoted_scalar;
+
+    /* Next, rebind to get the proper allocator: */
+    typedef typename CML_DEFAULT_ARRAY_ALLOC
+        ::rebind<promoted_scalar>::other allocator;
+
+    /* Finally, generate the promoted array type: */
+    typedef dynamic_1D<promoted_scalar,allocator> type;
+};
+
+/* Promote fixed 2D+1D array expressions to a fixed 1D array: */
+template<class A1, class A2>
+struct promote<A1,A2,twod_tag,oned_tag,fixed_size_tag>
+{
+    typedef typename A1::value_type left_scalar;
+    typedef typename A2::value_type right_scalar;
+
+    /* First, promote the scalar type: */
+    typedef typename ScalarPromote<
+        left_scalar,right_scalar>::type promoted_scalar;
+
+    /* Next, deduce the array size: */
+    enum { Size = (size_t)A1::array_rows };
+
+    /* Finally, generate the promoted array type: */
+    typedef fixed_1D<promoted_scalar,Size> type;
+};
+
+/* Promote fixed 1D+2D array expressions to a fixed 1D array: */
+template<class A1, class A2>
+struct promote<A1,A2,oned_tag,twod_tag,fixed_size_tag>
+{
+    typedef typename A1::value_type left_scalar;
+    typedef typename A2::value_type right_scalar;
+
+    /* First, promote the scalar type: */
+    typedef typename ScalarPromote<
+        left_scalar,right_scalar>::type promoted_scalar;
+
+    /* Next, deduce the array size: */
+    enum { Size = (size_t)A2::array_cols };
+
+    /* Finally, generate the promoted array type: */
+    typedef fixed_1D<promoted_scalar,Size> type;
+};
+
+/* Promote dynamic 2D+1D array expression to a 1D dynamic array: */
+template<class A1, class A2>
+struct promote<A1,A2,twod_tag,oned_tag,dynamic_size_tag>
+{
+    typedef typename A1::value_type left_scalar;
+    typedef typename A2::value_type right_scalar;
+
+    /* First, promote the scalar type: */
+    typedef typename ScalarPromote<
+        left_scalar,right_scalar>::type promoted_scalar;
+
+    /* Next, rebind to get the proper allocator: */
+    typedef typename CML_DEFAULT_ARRAY_ALLOC
+        ::rebind<promoted_scalar>::other allocator;
+
+    /* Finally, generate the promoted array type: */
+    typedef dynamic_1D<promoted_scalar,allocator> type;
+};
+
+/* Promote dynamic 1D+2D array expression to a 1D dynamic array: */
+template<class A1, class A2>
+struct promote<A1,A2,oned_tag,twod_tag,dynamic_size_tag>
+{
+    typedef typename A1::value_type left_scalar;
+    typedef typename A2::value_type right_scalar;
+
+    /* First, promote the scalar type: */
+    typedef typename ScalarPromote<
+        left_scalar,right_scalar>::type promoted_scalar;
+
+    /* Next, rebind to get the proper allocator: */
+    typedef typename CML_DEFAULT_ARRAY_ALLOC
+        ::rebind<promoted_scalar>::other allocator;
+
+    /* Finally, generate the promoted array type: */
+    typedef dynamic_1D<promoted_scalar,allocator> type;
+};
+
+
+/* This is a helper to deduce the result of a promoted 2D array: */
+template<typename LeftL, typename RightL> struct deduce_layout {
+#if defined(CML_ALWAYS_PROMOTE_TO_DEFAULT_LAYOUT)
+    typedef CML_DEFAULT_ARRAY_LAYOUT promoted_layout;
+#else
+    typedef typename select_if<
+        same_type<LeftL,RightL>::is_true, LeftL,
+        CML_DEFAULT_ARRAY_LAYOUT>::result promoted_layout;
+#endif
+};
+
+/* Promote 2D fixed-size arrays to a 2D fixed-size array.  The resulting
+ * matrix has the same number of rows as A1, and the same number of
+ * columns as A2.
+ */
+template<class A1, class A2>
+struct promote<A1,A2,twod_tag,twod_tag,fixed_size_tag>
+{
+    typedef typename A1::value_type left_scalar;
+    typedef typename A2::value_type right_scalar;
+
+    /* First, promote the scalar type: */
+    typedef typename ScalarPromote<
+        left_scalar,right_scalar>::type promoted_scalar;
+
+    /* Next, deduce the array size: */
+    enum {
+        Rows = (size_t)A1::array_rows,
+        Cols = (size_t)A2::array_cols
+    };
+
+    /* Then deduce the array layout: */
+    typedef typename A1::layout left_layout;
+    typedef typename A2::layout right_layout;
+    typedef typename deduce_layout<left_layout,right_layout>
+        ::promoted_layout promoted_layout;
+
+    /* Finally, generate the promoted array type: */
+    typedef fixed_2D<promoted_scalar,Rows,Cols,promoted_layout> type;
+};
+
+/* Promote 2D dynamic arrays to a 2D dynamic array: */
+template<class A1, class A2>
+struct promote<A1,A2,twod_tag,twod_tag,dynamic_size_tag>
+{
+    typedef typename A1::value_type left_scalar;
+    typedef typename A2::value_type right_scalar;
+
+    /* First, promote the scalar type: */
+    typedef typename ScalarPromote<
+        left_scalar,right_scalar>::type promoted_scalar;
+
+    /* Next, rebind to get the proper allocator: */
+    typedef typename CML_DEFAULT_ARRAY_ALLOC
+        ::rebind<promoted_scalar>::other allocator;
+
+    /* Then deduce the array layout: */
+    typedef typename A1::layout left_layout;
+    typedef typename A2::layout right_layout;
+    typedef typename deduce_layout<left_layout,right_layout>
+        ::promoted_layout promoted_layout;
+
+    /* Finally, generate the promoted array type: */
+    typedef dynamic_2D<promoted_scalar,promoted_layout,allocator> type;
+};
+
+} // namespace detail
+
+/** Class to promote array types.
+ *
+ * Both arguments must be array types.
+ *
+ * @sa fixed_1D
+ * @sa fixed_2D
+ * @sa dynamic_1D
+ * @sa dynamic_2D
+ */
+template<class A1, class A2>
+struct ArrayPromote
+{
+    /* Shorthand: */
+    //typedef typename A1::value_type left_scalar;
+    //typedef typename A2::value_type right_scalar;
+    typedef typename A1::dimension_tag left_dtag;
+    typedef typename A2::dimension_tag right_dtag;
+
+    /* Deduce the proper type based upon the characteristics of AT1 and
+     * AT2.  This is the table of type conversions:
+     *
+     *         AT1               AT2              Result
+     *   memory   size     memory   size      memory   size
+     *
+     *   fixed    fixed    fixed    fixed     fixed    fixed
+     *   fixed    fixed    dynamic  dynamic   dynamic  dynamic
+     *   fixed    fixed    external fixed     fixed    fixed
+     *   fixed    fixed    external dynamic   dynamic  dynamic
+     *
+     *   dynamic  dynamic  fixed    fixed     dynamic  dynamic
+     *   dynamic  dynamic  dynamic  dynamic   dynamic  dynamic
+     *   dynamic  dynamic  external fixed     dynamic  dynamic
+     *   dynamic  dynamic  external dynamic   dynamic  dynamic
+     *
+     *   external fixed    external fixed     fixed    fixed
+     *   external fixed    fixed    fixed     fixed    fixed
+     *   external fixed    dynamic  dynamic   dynamic  dynamic
+     *   external fixed    external dynamic   dynamic  dynamic
+     *
+     *   external dynamic  external fixed     dynamic  dynamic
+     *   external dynamic  fixed    fixed     dynamic  dynamic
+     *   external dynamic  dynamic  dynamic   dynamic  dynamic
+     *   external dynamic  external dynamic   dynamic  dynamic
+     *
+     * Note that if one argument is a dynamically-sized array, the result
+     * must be a dynamically allocated and sized array.  Likewise, if both
+     * arguments have fixed size, the result can be a fixed-sized array.
+     */
+
+    /* Check if both arguments are fixed-size arrays.  If so, the promoted
+     * array will be a fixed array, and if not, it will be a dynamic array:
+     */
+    typedef typename select_if<
+        (same_type<typename A1::size_tag, fixed_size_tag>::is_true
+         && same_type<typename A2::size_tag, fixed_size_tag>::is_true),
+        fixed_size_tag,         /* True */
+        dynamic_size_tag        /* False */
+    >::result promoted_size_tag;
+
+    /* Deduce the promoted type: */
+    typedef typename detail::promote<
+        A1, A2, left_dtag, right_dtag, promoted_size_tag>::type type;
+};
+
+/* Cleanup internal macros: */
+#undef VAL_MAX
+
+} // namespace et
+} // namespace cml
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/et/scalar_ops.h b/src/cml/et/scalar_ops.h
new file mode 100644 (file)
index 0000000..6bd236d
--- /dev/null
@@ -0,0 +1,138 @@
+/* -*- 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 ops_h
+#define ops_h
+
+#include <cml/et/traits.h>
+#include <cml/et/scalar_promotions.h>
+
+/** Declare a unary scalar operator, like negation. */
+#define CML_UNARY_SCALAR_OP(_op_, _op_name_)                            \
+template<typename ArgT> struct _op_name_ {                              \
+    typedef ExprTraits<ArgT> arg_traits;                                \
+    typedef typename arg_traits::const_reference arg_reference;         \
+    typedef typename arg_traits::value_type value_type;                 \
+    typedef scalar_result_tag result_tag;                               \
+    value_type apply(arg_reference arg) const { return _op_ arg; }      \
+};
+
+/** Declare a binary scalar operator, like addition, s1+s2. */
+#define CML_BINARY_SCALAR_OP(_op_, _op_name_)                            \
+template<typename LeftT, typename RightT> struct _op_name_ {             \
+    typedef ExprTraits<LeftT> left_traits;                               \
+    typedef ExprTraits<RightT> right_traits;                             \
+    typedef typename left_traits::const_reference left_reference;        \
+    typedef typename right_traits::const_reference right_reference;      \
+    typedef typename left_traits::value_type left_value;                 \
+    typedef typename right_traits::value_type right_value;               \
+    typedef typename ScalarPromote<left_value,right_value>::type value_type; \
+    typedef scalar_result_tag result_tag;                               \
+    value_type apply(left_reference left, right_reference right) const { \
+        return left _op_ right; }                                        \
+};
+
+/** Declare an op-assignment operator.
+ *
+ * @note The ExprTraits for both argument types must be defined, LeftT must
+ * have an assignment operator, and ExprTraits<LeftT>::reference must specify
+ * a type that allows assignment.
+ */
+#define CML_BINARY_SCALAR_OP_ASSIGN(_op_, _op_name_)                     \
+template<typename LeftT, typename RightT> struct _op_name_ {             \
+    typedef ExprTraits<LeftT> left_traits;                               \
+    typedef ExprTraits<RightT> right_traits;                             \
+    typedef typename left_traits::reference left_reference;              \
+    typedef typename right_traits::const_reference right_reference;      \
+    typedef typename left_traits::value_type left_value;                 \
+    typedef typename right_traits::value_type right_value;               \
+    typedef typename ScalarPromote<left_value,right_value>::type value_type; \
+    typedef scalar_result_tag result_tag;                                \
+    value_type apply(left_reference left, right_reference right) const { \
+        return left _op_ (LeftT) right; }                                \
+};
+
+/** Declare a binary boolean operator, like less-than, s1 < s2.
+ *
+ * The operator should return the appropriate truth value for the operator.
+ *
+ * @note Both scalar types must have operator<() defined.
+ */
+#define CML_BOOLEAN_SCALAR_OP(_op_, _op_name_)                           \
+template<typename LeftT, typename RightT> struct _op_name_ {             \
+    typedef ExprTraits<LeftT> left_traits;                               \
+    typedef ExprTraits<RightT> right_traits;                             \
+    typedef typename left_traits::const_reference left_reference;        \
+    typedef typename right_traits::const_reference right_reference;      \
+    typedef scalar_result_tag result_tag;                                \
+    bool apply(left_reference left, right_reference right) const {       \
+        return left _op_ right; }                                        \
+};
+
+namespace cml {
+namespace et {
+
+/* Define the operators: */
+
+/* Unary scalar ops: */
+CML_UNARY_SCALAR_OP(-, OpNeg)
+CML_UNARY_SCALAR_OP(+, OpPos)
+
+/* Binary scalar ops: */
+CML_BINARY_SCALAR_OP(+, OpAdd)
+CML_BINARY_SCALAR_OP(-, OpSub)
+CML_BINARY_SCALAR_OP(*, OpMul)
+
+#if defined(CML_RECIPROCAL_OPTIMIZATION)
+/* XXX Yikes... this should really be written out in full. *= 1./ is the
+ * "_op_" parameter to the macro (see above):
+ */
+CML_BINARY_SCALAR_OP(* value_type(1)/, OpDiv)
+#else
+CML_BINARY_SCALAR_OP(/, OpDiv)
+#endif
+
+/* Binary scalar op-assigns: */
+CML_BINARY_SCALAR_OP_ASSIGN( =, OpAssign)
+CML_BINARY_SCALAR_OP_ASSIGN(+=, OpAddAssign)
+CML_BINARY_SCALAR_OP_ASSIGN(-=, OpSubAssign)
+CML_BINARY_SCALAR_OP_ASSIGN(*=, OpMulAssign)
+
+#if defined(CML_RECIPROCAL_OPTIMIZATION)
+/* XXX Yikes... this should really be written out in full. *= 1./ is the
+ * "_op_" parameter to the macro (see above):
+ */
+CML_BINARY_SCALAR_OP_ASSIGN(*= value_type(1)/, OpDivAssign)
+#else
+CML_BINARY_SCALAR_OP_ASSIGN(/=, OpDivAssign)
+#endif
+
+/* Boolean operators for scalars: */
+CML_BOOLEAN_SCALAR_OP(==, OpEqual)
+CML_BOOLEAN_SCALAR_OP(!=, OpNotEqual)
+CML_BOOLEAN_SCALAR_OP( <, OpLess)
+CML_BOOLEAN_SCALAR_OP( >, OpGreater)
+CML_BOOLEAN_SCALAR_OP(<=, OpLessEqual)
+CML_BOOLEAN_SCALAR_OP(>=, OpGreaterEqual)
+
+#undef CML_UNARY_SCALAR_OP
+#undef CML_BINARY_SCALAR_OP
+#undef CML_BINARY_SCALAR_OP_ASSIGN
+#undef CML_BOOLEAN_SCALAR_OP
+
+} // namespace et
+} // namespace cml
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/et/scalar_promotions.h b/src/cml/et/scalar_promotions.h
new file mode 100644 (file)
index 0000000..55d033c
--- /dev/null
@@ -0,0 +1,120 @@
+/* -*- 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 scalar_promotions_h
+#define scalar_promotions_h
+
+#include <cml/core/cml_meta.h>
+
+namespace cml {
+namespace et {
+
+namespace detail {
+
+/** @class IntPromote
+ *  @brief Helper template to int-promote a type.
+ */
+template<class T> struct IntPromote
+{
+    /* Signed -> signed int, unsigned -> unsigned int: */
+    typedef typename select_switch<T,
+            unsigned char,                       unsigned int,
+            unsigned short,                      unsigned int,
+            signed char,                         int,
+            char,                                int,
+            short,                               int,
+            T,                                   T
+    >::result   result;
+};
+
+} // namespace detail
+
+/** @class ScalarPromote
+ *  @brief Template for compile-time type promotion via C promotion rules.
+ */
+template<class E1_in, class E2_in> struct ScalarPromote
+{
+
+    /* Integral-promote the types (if possible). */
+    typedef typename detail::IntPromote<E1_in>::result  E1;
+    typedef typename detail::IntPromote<E2_in>::result  E2;
+
+    /* If sizeof(long) == sizeof(unsigned int), promote to unsigned long.
+     * Otherwise, sizeof(long) > sizeof(int), so promote to long.
+     */
+    typedef typename select_if<sizeof(long) == sizeof(unsigned int),
+            unsigned long,
+            long
+    >::result   uint_promotion;
+
+    /* Do the selection on the promoted types: */
+    typedef typename select_switch<
+        type_pair<E1,E2>,
+
+#if defined(CML_USE_LONG_DOUBLE)
+        type_pair<long double,long double>,       long double,
+        type_pair<long double,E2>,                long double,
+        type_pair<E1,long double>,                long double,
+#endif
+
+        type_pair<double,double>,                 double,
+        type_pair<double,E2>,                     double,
+        type_pair<E1,double>,                     double,
+
+        type_pair<float,float>,                   float,
+        type_pair<float,E2>,                      float,
+        type_pair<E1,float>,                      float,
+
+        type_pair<E1,E2>,                         void
+
+    >::result   float_filter;
+
+    /* The promoted integral types really matter here: */
+    typedef typename select_switch<
+        type_pair<E1,E2>,
+
+        type_pair<unsigned long,unsigned long>,   unsigned long,
+        type_pair<unsigned long,E2>,              unsigned long,
+        type_pair<E1,unsigned long>,              unsigned long,
+
+        type_pair<long,long>,                     long,
+        type_pair<long,unsigned int>,             uint_promotion,
+        type_pair<unsigned int,long>,             uint_promotion,
+
+        type_pair<long,E2>,                       long,
+        type_pair<E1,long>,                       long,
+
+        type_pair<unsigned int,unsigned int>,     unsigned int,
+        type_pair<unsigned int,E2>,               unsigned int,
+        type_pair<E1,unsigned int>,               unsigned int,
+
+        type_pair<int,int>,                       int,
+        type_pair<int,E2>,                        int,
+        type_pair<E1,int>,                        int,
+
+        type_pair<E1,E2>,                         void
+
+    >::result   int_filter;
+
+    /* Deduce the final type: */
+    typedef typename select_if<
+        same_type<float_filter,void>::is_true,
+        int_filter, float_filter>::result         type;
+};
+
+} // namespace et
+} // namespace cml
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/et/size_checking.h b/src/cml/et/size_checking.h
new file mode 100644 (file)
index 0000000..323afb5
--- /dev/null
@@ -0,0 +1,426 @@
+/* -*- 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
+ *
+ * Define matrix and vector linear expression size-checking classes.
+ */
+
+#ifndef size_checking_h
+#define size_checking_h
+
+#include <stdexcept>
+#include <cml/core/cml_meta.h>
+#include <cml/core/cml_assert.h>
+#include <cml/core/fwd.h>
+#include <cml/et/traits.h>
+
+#if defined(_MSC_VER) && _MSC_VER < 1400
+#pragma warning(push)
+#pragma warning(disable:4348)
+// XXX This is a terrible hack for VC7.1, and should really be fixed by
+// separating out the "impl" templates from GetCheckedSize.
+#endif
+
+/* This is used below to create a more meaningful compile-time error when
+ * fixed-size vector arguments don't match at compile time:
+ */
+struct incompatible_expression_size_error;
+
+/* This is used below to create a more meaningful compile-time error when a
+ * function is not provided with a square matrix or MatrixExpr argument:
+ */
+struct square_matrix_arg_expected_error;
+
+namespace cml {
+namespace et {
+namespace detail {
+
+} // namespace detail
+
+/* Forward declare for specialization below: */
+template<typename LeftT, typename RightT, typename SizeT>
+    struct GetCheckedSize;
+
+/* Checking for fixed-size expression: */
+template<typename LeftT, typename RightT>
+struct GetCheckedSize<LeftT,RightT,fixed_size_tag>
+{
+    /* Record argument traits: */
+    typedef ExprTraits<LeftT> left_traits;
+    typedef ExprTraits<RightT> right_traits;
+
+    /* Result types: */
+    typedef typename left_traits::result_tag left_result;
+    typedef typename right_traits::result_tag right_result;
+
+
+    /* For specialization below: */
+    template<typename LR, typename RR, class X = void> struct impl;
+
+    /* Check for two matrices (linear operators only): */
+    template<class X> struct impl<matrix_result_tag,matrix_result_tag,X> {
+        typedef matrix_size size_type;
+        CML_STATIC_REQUIRE_M(
+                (size_t)LeftT::array_rows == (size_t)RightT::array_rows
+                && (size_t)LeftT::array_cols == (size_t)RightT::array_cols,
+                incompatible_expression_size_error);
+
+        /* Record the array size as a constant: */
+        enum {
+            array_rows = LeftT::array_rows,
+            array_cols = LeftT::array_cols
+        };
+
+        /* Return the matrix size: */
+        size_type size() const { return size_type(array_rows,array_cols); }
+    };
+
+    /* Check for a matrix and a vector: */
+    template<class X> struct impl<matrix_result_tag,vector_result_tag,X> {
+        typedef size_t size_type;
+        CML_STATIC_REQUIRE_M(
+                (size_t)LeftT::array_cols == (size_t)RightT::array_size,
+                incompatible_expression_size_error);
+
+        /* Record the array size as a constant: */
+        enum { array_size = LeftT::array_rows };
+
+        /* Return the vector size: */
+        size_type size() const { return size_type(array_size); }
+    };
+
+    /* Check for a vector and a matrix: */
+    template<class X> struct impl<vector_result_tag,matrix_result_tag,X> {
+        typedef size_t size_type;
+        CML_STATIC_REQUIRE_M(
+                (size_t)LeftT::array_size == (size_t)RightT::array_rows,
+                incompatible_expression_size_error);
+
+        /* Record the array size as a constant: */
+        enum { array_size = RightT::array_cols };
+
+        /* Return the vector size: */
+        size_type size() const { return size_type(array_size); }
+    };
+
+    /* Check for a matrix and a scalar: */
+    template<class X> struct impl<matrix_result_tag,scalar_result_tag,X> {
+        typedef matrix_size size_type;
+
+        /* Record the array size as a constant: */
+        enum {
+            array_rows = LeftT::array_rows,
+            array_cols = LeftT::array_cols
+        };
+
+        /* Return the matrix size: */
+        size_type size() const { return size_type(array_rows,array_cols); }
+    };
+
+    /* Check for a scalar and a matrix: */
+    template<class X> struct impl<scalar_result_tag,matrix_result_tag,X> {
+        typedef matrix_size size_type;
+
+        /* Record the array size as a constant: */
+        enum {
+            array_rows = RightT::array_rows,
+            array_cols = RightT::array_cols
+        };
+
+        /* Return the matrix size: */
+        size_type size() const { return size_type(array_rows,array_cols); }
+    };
+
+
+    /* Check for two vectors: */
+    template<class X> struct impl<vector_result_tag,vector_result_tag,X> {
+        typedef size_t size_type;
+        CML_STATIC_REQUIRE_M(
+                (size_t)LeftT::array_size == (size_t)RightT::array_size,
+                incompatible_expression_size_error);
+
+        /* Record the array size as a constant: */
+        enum { array_size = LeftT::array_size };
+
+        /* Return the vector size: */
+        size_type size() const { return size_type(array_size); }
+    };
+
+    /* Check for a vector and a scalar: */
+    template<class X> struct impl<vector_result_tag,scalar_result_tag,X> {
+        typedef size_t size_type;
+
+        /* Record the array size as a constant: */
+        enum { array_size = LeftT::array_size };
+
+        /* Return the vector size: */
+        size_type size() const { return size_type(array_size); }
+    };
+
+    /* Check for a scalar and a vector: */
+    template<class X> struct impl<scalar_result_tag,vector_result_tag,X> {
+        typedef size_t size_type;
+
+        /* Record the array size as a constant: */
+        enum { array_size = RightT::array_size };
+
+        /* Return the vector size: */
+        size_type size() const { return size_type(array_size); }
+    };
+
+
+    /* Check for two quaternions: */
+    template<class X>
+    struct impl<quaternion_result_tag,quaternion_result_tag,X> {
+        typedef size_t size_type;
+
+        /* Record the quaternion size as a constant: */
+        enum { array_size = 4 };
+
+        /* Return the quaternion size: */
+        size_type size() const { return size_type(array_size); }
+    };
+
+    /* Check for a quaternion and a vector: */
+    template<class X> struct impl<quaternion_result_tag,vector_result_tag,X> {
+        typedef size_t size_type;
+        CML_STATIC_REQUIRE_M(
+                RightT::array_size == 4,
+                incompatible_expression_size_error);
+
+        /* Record the quaternion size as a constant: */
+        enum { array_size = 4 };
+
+        /* Return the quaternion size: */
+        size_type size() const { return size_type(array_size); }
+    };
+
+    /* Check for a vector and a quaternion: */
+    template<class X> struct impl<vector_result_tag,quaternion_result_tag,X> {
+        typedef size_t size_type;
+        CML_STATIC_REQUIRE_M(
+                LeftT::array_size == 4,
+                incompatible_expression_size_error);
+
+        /* Record the quaternion size as a constant: */
+        enum { array_size = 4 };
+
+        /* Return the quaternion size: */
+        size_type size() const { return size_type(array_size); }
+    };
+
+    /* Check for a quaternion and a scalar: */
+    template<class X> struct impl<quaternion_result_tag,scalar_result_tag,X> {
+        typedef size_t size_type;
+
+        /* Record the quaternion size as a constant: */
+        enum { array_size = 4 };
+
+        /* Return the quaternion size: */
+        size_type size() const { return size_type(array_size); }
+    };
+
+    /* Check for a scalar and a quaternion: */
+    template<class X> struct impl<scalar_result_tag,quaternion_result_tag,X> {
+        typedef size_t size_type;
+
+        /* Record the array size as a constant: */
+        enum { array_size = 4 };
+
+        /* Return the quaternion size: */
+        size_type size() const { return size_type(array_size); }
+    };
+
+    /* Record the type of the checker: */
+    typedef impl<left_result,right_result> check_type;
+    typedef typename check_type::size_type size_type;
+
+    /* The implementation: */
+    size_type operator()(const LeftT&, const RightT&) const {
+        return check_type().size();
+    }
+};
+
+/* Checking for resizeable expression: */
+template<typename LeftT, typename RightT>
+struct GetCheckedSize<LeftT,RightT,dynamic_size_tag>
+{
+    /* Type of the size checker (for calling equal_or_fail): */
+    typedef GetCheckedSize<LeftT,RightT,dynamic_size_tag> self;
+
+    /* Record argument traits: */
+    typedef ExprTraits<LeftT> left_traits;
+    typedef ExprTraits<RightT> right_traits;
+
+    /* Result types: */
+    typedef typename left_traits::result_tag left_result;
+    typedef typename right_traits::result_tag right_result;
+
+
+    /* For specialization below: */
+    template<typename LR, typename RR, class X = void> struct impl;
+
+    /* Return the size if the same, or fail if different: */
+    template<typename V> V equal_or_fail(V left, V right) const {
+        if(left != right)
+            throw std::invalid_argument(
+                    "expressions have incompatible sizes.");
+        return left;
+    }
+
+    /* Check for two matrices (linear operators only): */
+    template<class X> struct impl<matrix_result_tag,matrix_result_tag,X> {
+        typedef matrix_size size_type;
+
+        /* Return the matrix size, or fail if incompatible: */
+        size_type size(const LeftT& left, const RightT& right) const {
+#if defined(CML_CHECK_MATRIX_EXPR_SIZES)
+            return self().equal_or_fail(left.size(), right.size());
+#else
+            return left.size();
+#endif
+        }
+    };
+
+    /* Check for a matrix and a vector: */
+    template<class X> struct impl<matrix_result_tag,vector_result_tag,X> {
+        typedef size_t size_type;
+
+        /* Return the vector size: */
+        size_type size(const LeftT& left, const RightT& right) const {
+#if defined(CML_CHECK_MATVEC_EXPR_SIZES)
+            self().equal_or_fail(left.cols(), right.size());
+#endif
+            return left.rows();
+        }
+    };
+
+    /* Check for a vector and a matrix: */
+    template<class X> struct impl<vector_result_tag,matrix_result_tag,X> {
+        typedef size_t size_type;
+
+        /* Return the vector size: */
+        size_type size(const LeftT& left, const RightT& right) const {
+#if defined(CML_CHECK_MATVEC_EXPR_SIZES)
+            self().equal_or_fail(left.size(), right.rows());
+#endif
+            return right.cols(right);
+        }
+    };
+
+    /* Check for a matrix and a scalar: */
+    template<class X> struct impl<matrix_result_tag,scalar_result_tag,X> {
+        typedef matrix_size size_type;
+
+        /* Return the matrix size: */
+        size_type size(const LeftT& left, const RightT&) const {
+            return left.size();
+        }
+    };
+
+    /* Check for a scalar and a matrix: */
+    template<class X> struct impl<scalar_result_tag,matrix_result_tag,X> {
+        typedef matrix_size size_type;
+
+        /* Return the matrix size: */
+        size_type size(const LeftT&, const RightT& right) const {
+            return right.size();
+        }
+    };
+
+    /* Check for two vectors: */
+    template<class X> struct impl<vector_result_tag,vector_result_tag,X> {
+        typedef size_t size_type;
+
+        /* Return the vector size: */
+        size_type size(const LeftT& left, const RightT& right) const {
+#if defined(CML_CHECK_VECTOR_EXPR_SIZES)
+            return self().equal_or_fail(left.size(), right.size());
+#else
+            return left.size();
+#endif
+        }
+    };
+
+    /* Check for a vector and a scalar: */
+    template<class X> struct impl<vector_result_tag,scalar_result_tag,X> {
+        typedef size_t size_type;
+
+        /* Return the vector size: */
+        size_type size(const LeftT& left, const RightT&) const {
+            return left.size();
+        }
+    };
+
+    /* Check for a scalar and a vector: */
+    template<class X> struct impl<scalar_result_tag,vector_result_tag,X> {
+        typedef size_t size_type;
+
+        /* Return the vector size: */
+        size_type size(const LeftT&, const RightT& right) const {
+            return right.size();
+        }
+    };
+
+    /* Record the type of the checker: */
+    typedef impl<left_result,right_result> check_type;
+    typedef typename check_type::size_type size_type;
+
+    /* The implementation: */
+    size_type operator()(const LeftT& left, const RightT& right) const {
+        return check_type().size(left,right);
+    }
+};
+
+/** Generator for GetCheckedSize. */
+template<typename LeftT, typename RightT, typename SizeTag>
+inline typename et::GetCheckedSize<LeftT,RightT,SizeTag>::size_type
+CheckedSize(const LeftT& left, const RightT& right, SizeTag)
+{
+    return et::GetCheckedSize<LeftT,RightT,SizeTag>()(left,right);
+}
+
+/** Verify the sizes of the argument matrices for matrix multiplication.
+ *
+ * @returns a the size of the resulting matrix.
+ */
+template<typename MatT> inline size_t
+CheckedSquare(const MatT&, fixed_size_tag)
+{
+    CML_STATIC_REQUIRE_M(
+            ((size_t)MatT::array_rows == (size_t)MatT::array_cols),
+            square_matrix_arg_expected_error);
+    return (size_t)MatT::array_rows;
+}
+
+/** Verify the sizes of the argument matrices for matrix multiplication.
+ *
+ * @returns the size of the resulting matrix.
+ */
+template<typename MatT> inline size_t
+CheckedSquare(const MatT& m, dynamic_size_tag)
+{
+    matrix_size N = m.size();
+    et::GetCheckedSize<MatT,MatT,dynamic_size_tag>()
+        .equal_or_fail(N.first, N.second);
+    return N.first;
+}
+
+} // namespace et
+} // namespace cml
+
+#if defined(_MSC_VER) && _MSC_VER < 1400
+#pragma warning(pop)
+#endif
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/et/tags.h b/src/cml/et/tags.h
new file mode 100644 (file)
index 0000000..a940134
--- /dev/null
@@ -0,0 +1,55 @@
+/* -*- 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 et_tags_h
+#define et_tags_h
+
+namespace cml {
+namespace et {
+
+/** Tag an expression as returning a scalar. */
+struct scalar_result_tag {};
+
+/** Tag an expression as returning a vector. */
+struct vector_result_tag {};
+
+/** Tag an expression as returning a matrix. */
+struct matrix_result_tag {};
+
+/** Tag an expression as returning a quaternion. */
+struct quaternion_result_tag {};
+
+/** Marker for unary expression ops. */
+struct unary_expression {};
+
+/** Marker for biary expression ops. */
+struct binary_expression {};
+
+/** Marker for expression tree operator nodes. */
+struct expr_node_tag {};
+
+/** Marker for expression tree terminals (leaves). */
+struct expr_leaf_tag {};
+
+/** Marker for assignable types. */
+struct assignable_tag {};
+
+/** Marker for assignable types. */
+struct not_assignable_tag {};
+
+} // namespace et
+} // namespace cml
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/et/traits.h b/src/cml/et/traits.h
new file mode 100644 (file)
index 0000000..0e59e4b
--- /dev/null
@@ -0,0 +1,143 @@
+/* -*- 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 traits_h
+#define traits_h
+
+#include <cml/et/tags.h>
+
+/* XXX This is here temporarily, should be rolled into the traits classes
+ * once it's clear how to best specify scalar args
+ */
+//#define SCALAR_ARG_TYPE const ScalarT&
+//#define ELEMENT_ARG_TYPE const Element&
+#define SCALAR_ARG_TYPE ScalarT
+#define ELEMENT_ARG_TYPE Element
+
+namespace cml {
+namespace et {
+
+/** The expression traits class.
+ *
+ * The traits class is used to provide uniform access to expression
+ * objects, including scalars, when used in vector and matrix expressions.
+ * One especially useful property for scalars is that scalars are
+ * implicitly "promoted" to vectors or scalars as necessary via the
+ * ExprTraits's get() method.  Without this functionality, a separate
+ * expression tree node would be needed to hold a scalar, which would
+ * adversely affect performance.
+ *
+ * @internal This is also currently used for determining traits of scalar
+ * types from the scalar operators (+,-,etc.).  Really, a separate traits
+ * class should probably be used for this (e.g. ScalarTraits).
+ */
+template<typename T> struct ExprTraits
+#if defined(CML_NO_DEFAULT_EXPR_TRAITS)
+/* For testing, don't default to scalar traits: */
+#else
+{
+    /* Standard: */
+    typedef T expr_type;
+    typedef T value_type;
+    typedef T& reference;
+    typedef T const_reference;
+    typedef scalar_result_tag result_tag;
+    typedef fixed_memory_tag memory_tag;
+    typedef unit_size_tag size_tag;
+    typedef expr_type result_type;
+    typedef expr_leaf_tag node_tag;
+
+    /** Vector-like access, just returns the value. */
+    value_type get(const_reference v, size_t) const { return v; }
+
+    /** Matrix-like access, just returns the value. */
+    value_type get(const_reference v, size_t, size_t) const { return v; }
+
+    /** Size is always 1. */
+    size_t size(const_reference) const { return 1; }
+
+    /** Size is always 1. */
+    size_t rows(double) const { return 1; }
+
+    /** Size is always 1. */
+    size_t cols(double) const { return 1; }
+}
+#endif
+;
+
+#if defined(CML_NO_DEFAULT_EXPR_TRAITS)
+template<> struct ExprTraits<double>
+{
+    /* Standard: */
+    typedef double expr_type;
+    typedef double value_type;
+    typedef double& reference;
+    typedef double const_reference;
+    typedef scalar_result_tag result_tag;
+    typedef fixed_memory_tag memory_tag;
+    typedef unit_size_tag size_tag;
+    typedef double result_type;
+    typedef expr_leaf_tag node_tag;
+
+    /** Vector-like access, just returns the value. */
+    value_type get(double v, size_t) const { return v; }
+
+    /** Matrix-like access, just returns the value. */
+    value_type get(double v, size_t, size_t) const { return v; }
+
+    /** Size is always 1. */
+    size_t size(double) const { return 1; }
+
+    /** Size is always 1. */
+    size_t rows(double) const { return 1; }
+
+    /** Size is always 1. */
+    size_t cols(double) const { return 1; }
+};
+
+template<> struct ExprTraits<float>
+{
+    /* Standard: */
+    typedef float expr_type;
+    typedef float value_type;
+    typedef float& reference;
+    typedef float const_reference;
+    typedef scalar_result_tag result_tag;
+    typedef fixed_memory_tag memory_tag;
+    typedef unit_size_tag size_tag;
+    typedef float result_type;
+    typedef expr_leaf_tag node_tag;
+
+    /** Vector-like access, just returns the value. */
+    value_type get(float v, size_t) const { return v; }
+
+    /** Matrix-like access, just returns the value. */
+    value_type get(float v, size_t, size_t) const { return v; }
+
+    /** Size is always 1. */
+    size_t size(float) const { return 1; }
+
+    /** Size is always 1. */
+    size_t rows(float) const { return 1; }
+
+    /** Size is always 1. */
+    size_t cols(float) const { return 1; }
+};
+#endif
+
+} // namespace et
+} // namespace cml
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/external.h b/src/cml/external.h
new file mode 100644 (file)
index 0000000..cc052e7
--- /dev/null
@@ -0,0 +1,41 @@
+/* -*- 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 external_h
+#define external_h
+
+namespace cml {
+
+/** This is a selector for external 1D and 2D arrays.
+ *
+ * The external<> struct is used only to select a 1D or 2D array as the
+ * base class of a vector or matrix.  The rebind<> template is used by
+ * quaternion<> to select its vector length in a generic way.
+ *
+ * @sa fixed
+ * @sa dynamic
+ */
+template<int Dim1 = -1, int Dim2 = -1> struct external {
+
+    /** Rebind to a 1D type.
+     *
+     * This is used by quaternion<>.
+     */
+    template<int D> struct rebind { typedef external<D> other; };
+};
+
+} // namespace cml
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/fixed.h b/src/cml/fixed.h
new file mode 100644 (file)
index 0000000..536eb3c
--- /dev/null
@@ -0,0 +1,42 @@
+/* -*- 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 fixed_h
+#define fixed_h
+
+namespace cml {
+
+
+/** This is a selector for fixed 1D and 2D arrays.
+ *
+ * The fixed<> struct is used only to select a 1D or 2D array as the base
+ * class of a vector or matrix.  The rebind<> template is used by
+ * quaternion<> to select its vector length in a generic way.
+ *
+ * @sa dynamic
+ * @sa external
+ */
+template<int Dim1 = -1, int Dim2 = -1> struct fixed {
+
+    /** Rebind to a 1D type.
+     *
+     * This is used by quaternion<>.
+     */
+    template<int D> struct rebind { typedef fixed<D> other; };
+};
+
+} // namespace cml
+
+#endif
+
+// -------------------------------------------------------------------------
+// vim:ft=cpp
diff --git a/src/cml/mathlib/checking.h b/src/cml/mathlib/checking.h
new file mode 100644 (file)
index 0000000..36a7ced
--- /dev/null
@@ -0,0 +1,382 @@
+/* -*- 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 checking_h
+#define checking_h
+
+#include <cml/vector/vector_expr.h>
+#include <cml/matrix/matrix_expr.h>
+#include <cml/quaternion/quaternion_expr.h>
+
+/* Run- and compile-time checking of argument types, values and sizes. */
+
+struct function_expects_vector_arg_error;
+struct function_expects_matrix_arg_error;
+struct function_expects_quaternion_arg_error;
+
+struct function_expects_2D_vector_arg_error;
+struct function_expects_3D_vector_arg_error;
+struct function_expects_4D_vector_arg_error;
+struct function_expects_2D_or_3D_vector_arg_error;
+struct function_expects_2x2_matrix_arg_error;
+struct function_expects_3x3_matrix_arg_error;
+struct function_expects_4x4_matrix_arg_error;
+struct function_expects_square_matrix_arg_error;
+
+struct matrix_arg_fails_minimum_size_requirement;
+
+namespace cml {
+namespace detail {
+
+//////////////////////////////////////////////////////////////////////////////
+// Vector argument checking
+//////////////////////////////////////////////////////////////////////////////
+
+/** Compile-time check for a vector argument */
+template< class VecT > inline void
+CheckVec(const VecT&)
+{
+    typedef et::ExprTraits<VecT> vector_traits;
+    typedef typename vector_traits::result_tag result_type;
+    
+    CML_STATIC_REQUIRE_M(
+        (same_type<result_type, et::vector_result_tag>::is_true),
+        function_expects_vector_arg_error);
+}
+
+/** Compile-time check for a vector of size N */
+template< class VecT, size_t N, class ErrorT > inline void
+CheckVecN(const VecT& v, fixed_size_tag) {
+    CheckVec(v);
+
+    CML_STATIC_REQUIRE_M(((size_t)VecT::array_size == N), ErrorT);
+}
+
+/** Run-time check for a vector of size N */
+template< class VecT, size_t N, class /*ErrorT*/ > inline void
+CheckVecN(const VecT& v, dynamic_size_tag) {
+    CheckVec(v);
+
+    et::GetCheckedSize<VecT,VecT,dynamic_size_tag>()
+        .equal_or_fail(v.size(),size_t(N));
+}
+
+/** Check for a vector of size N */
+template< class VecT, size_t N, class ErrorT > inline void
+CheckVecN(const VecT& v) {
+    typedef et::ExprTraits<VecT> vector_traits;
+    typedef typename vector_traits::size_tag size_tag;
+
+    detail::CheckVecN<VecT,N,ErrorT>(v, size_tag());
+}
+
+/** Check for a vector of size 2 */
+template< class VecT > inline void
+CheckVec2(const VecT& v) {
+    detail::CheckVecN<VecT,2,function_expects_2D_vector_arg_error>(v);
+}
+
+/** Check for a vector of size 3 */
+template< class VecT > inline void
+CheckVec3(const VecT& v) {
+    detail::CheckVecN<VecT,3,function_expects_3D_vector_arg_error>(v);
+}
+
+/** Check for a vector of size 4 */
+template< class VecT > inline void
+CheckVec4(const VecT& v) {
+    CheckVecN<VecT,4,function_expects_4D_vector_arg_error>(v);
+}
+
+/** Compile-time check for a vector of size 2 or 3 */
+template< class VecT > inline void
+CheckVec2Or3(const VecT& v, fixed_size_tag) {
+    CheckVec(v);
+
+    CML_STATIC_REQUIRE_M(
+        (VecT::array_size == 2 || VecT::array_size == 3),
+        function_expects_2D_or_3D_vector_arg_error);
+}
+
+/** Run-time check for a vector of size 2 or 3 */
+template< class VecT > inline void
+CheckVec2Or3(const VecT& v, dynamic_size_tag) {
+    CheckVec(v);
+
+    if (v.size() != 2 && v.size() != 3) {
+        throw std::invalid_argument("2d or 3d vector arg expected");
+    }
+}
+
+/** Check for a vector of size 2 or 3 */
+template< class VecT > inline void
+CheckVec2Or3(const VecT& v) {
+    typedef et::ExprTraits<VecT> vector_traits;
+    typedef typename vector_traits::size_tag size_tag;
+
+    detail::CheckVec2Or3(v, size_tag());
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Matrix argument checking
+//////////////////////////////////////////////////////////////////////////////
+
+/** Compile-time check for a matrix argument */
+template< class MatT > inline void
+CheckMat(const MatT&)
+{
+    typedef et::ExprTraits<MatT> matrix_traits;
+    typedef typename matrix_traits::result_tag result_type;
+    
+    CML_STATIC_REQUIRE_M(
+        (same_type<result_type, et::matrix_result_tag>::is_true),
+        function_expects_matrix_arg_error);
+}
+
+/** Compile-time check for a matrix of size NxM */
+template< class MatT, size_t N, size_t M, class ErrorT > inline void
+CheckMatNxM(const MatT& m, fixed_size_tag) {
+    CheckMat(m);
+
+    CML_STATIC_REQUIRE_M(
+        (MatT::array_rows == N && MatT::array_cols == M), ErrorT);
+}
+
+/** Run-time check for a matrix of size NxM */
+template< class MatT, size_t N, size_t M, class /*ErrorT*/ > inline void
+CheckMatNxM(const MatT& m, dynamic_size_tag) {
+    CheckMat(m);
+
+    et::GetCheckedSize<MatT,MatT,dynamic_size_tag>()
+        .equal_or_fail(m.rows(),N);
+    et::GetCheckedSize<MatT,MatT,dynamic_size_tag>()
+        .equal_or_fail(m.cols(),M);
+}
+
+/** Check for a matrix of size NxM */
+template< class MatT, size_t N, size_t M, class ErrorT > inline void
+CheckMatNxM(const MatT& m) {
+    typedef et::ExprTraits<MatT> matrix_traits;
+    typedef typename matrix_traits::size_tag size_tag;
+
+    CheckMatNxM<MatT,N,M,ErrorT>(m, size_tag());
+}
+
+/** Check for a square matrix of size NxN */
+template< class MatT, size_t N, class ErrorT > inline void
+CheckMatN(const MatT& m) {
+    CheckMatNxM<MatT,N,N,ErrorT>(m);
+}
+
+/** Check for a square matrix of size 2x2 */
+template< class MatT > inline void
+CheckMat2x2(const MatT& m) {
+    CheckMatN<MatT,2,function_expects_2x2_matrix_arg_error>(m);
+}
+
+/** Check for a square matrix of size 3x3 */
+template< class MatT > inline void
+CheckMat3x3(const MatT& m) {
+    CheckMatN<MatT,3,function_expects_3x3_matrix_arg_error>(m);
+}
+
+/** Check for a square matrix of size 4x4 */
+template< class MatT > inline void
+CheckMat4x4(const MatT& m) {
+    CheckMatN<MatT,4,function_expects_4x4_matrix_arg_error>(m);
+}
+
+/** Compile-time check for a matrix with minimum dimensions NxM */
+template< class MatT, size_t N, size_t M, class ErrorT > inline void
+CheckMatMinNxM(const MatT& m, fixed_size_tag) {
+    CheckMat(m);
+
+    CML_STATIC_REQUIRE_M(
+        (MatT::array_rows >= N && MatT::array_cols >= M), ErrorT);
+}
+
+/** Run-time check for a matrix with minimum dimensions NxM */
+template< class MatT, size_t N, size_t M, class /*ErrorT*/ > inline void
+CheckMatMinNxM(const MatT& m, dynamic_size_tag) {
+    CheckMat(m);
+
+    if (m.rows() < N || m.cols() < M) {
+        throw std::invalid_argument(
+            "matrix does not meet minimum size requirement");
+    }
+}
+
+/** Check for a matrix with minimum dimensions NxM */
+template< class MatT, size_t N, size_t M, class ErrorT > inline void
+CheckMatMinNxM(const MatT& m) {
+    typedef et::ExprTraits<MatT> matrix_traits;
+    typedef typename matrix_traits::size_tag size_tag;
+
+    CheckMatMinNxM<MatT,N,M,ErrorT>(m, size_tag());
+}
+
+/** Check for a matrix with minimum dimensions NxN */
+template< class MatT, size_t N, class ErrorT > inline void
+CheckMatMinN(const MatT& m) {
+    CheckMatMinNxM<MatT,N,N,ErrorT>(m);
+}
+
+/** Check for a matrix with minimum dimensions 2x2 */
+template< class MatT > inline void
+CheckMatMin2x2(const MatT& m) {
+    CheckMatMinN<MatT,2,matrix_arg_fails_minimum_size_requirement>(m);
+}
+
+/** Check for a matrix with minimum dimensions 3x3 */
+template< class MatT > inline void
+CheckMatMin3x3(const MatT& m) {
+    CheckMatMinN<MatT,3,matrix_arg_fails_minimum_size_requirement>(m);
+}
+
+/** Check for a matrix with minimum dimensions 4x4 */
+template< class MatT > inline void
+CheckMatMin4x4(const MatT& m) {
+    CheckMatMinN<MatT,4,matrix_arg_fails_minimum_size_requirement>(m);
+}
+
+/** Check for a matrix that can represent a 3D linear transform */
+template< class MatT > inline void
+CheckMatLinear3D(const MatT& m) {
+    CheckMatMin3x3(m);
+}
+
+/** Check for a matrix that can represent a 2D linear transform */
+template< class MatT > inline void
+CheckMatLinear2D(const MatT& m) {
+    CheckMatMin2x2(m);
+}
+
+/** Check for a matrix that can represent a 3D row-basis affine transform */
+template< class MatT > inline void
+CheckMatAffine3D(const MatT& m, row_basis) {
+    CheckMatMinNxM<MatT,4,3,matrix_arg_fails_minimum_size_requirement>(m);
+}
+
+/** Check for a matrix that can represent a 3D col-basis affine transform */
+template< class MatT > inline void
+CheckMatAffine3D(const MatT& m, col_basis) {
+    CheckMatMinNxM<MatT,3,4,matrix_arg_fails_minimum_size_requirement>(m);
+}
+
+/** Check for a matrix that can represent a 2D row-basis affine transform */
+template< class MatT > inline void
+CheckMatAffine2D(const MatT& m, row_basis) {
+    CheckMatMinNxM<MatT,3,2,matrix_arg_fails_minimum_size_requirement>(m);
+}
+
+/** Check for a matrix that can represent a 2D col-basis affine transform */
+template< class MatT > inline void
+CheckMatAffine2D(const MatT& m, col_basis) {
+    CheckMatMinNxM<MatT,2,3,matrix_arg_fails_minimum_size_requirement>(m);
+}
+
+/** Check for a matrix that can represent a 3D affine transform */
+template< class MatT > inline void
+CheckMatAffine3D(const MatT& m) {
+    CheckMatAffine3D(m, typename MatT::basis_orient());
+}
+
+/** Check for a matrix that can represent a 2D affine transform */
+template< class MatT > inline void
+CheckMatAffine2D(const MatT& m) {
+    CheckMatAffine2D(m, typename MatT::basis_orient());
+}
+
+/** Check for a matrix that can represent a 3D homogenous transform */
+template< class MatT > inline void
+CheckMatHomogeneous3D(const MatT& m) {
+    CheckMatMin4x4(m);
+}
+
+/** Compile-time check for a square matrix */
+template< class MatT, class ErrorT> inline void
+CheckMatSquare(const MatT& m, fixed_size_tag) {
+    CheckMat(m);
+
+    CML_STATIC_REQUIRE_M(
+        (MatT::array_rows == MatT::array_cols), ErrorT);
+}
+
+/** Run-time check for a square matrix */
+template< class MatT, class /*ErrorT*/ > inline void
+CheckMatSquare(const MatT& m, dynamic_size_tag) {
+    CheckMat(m);
+
+    if (m.rows() != m.cols()) {
+        throw std::invalid_argument(
+            "function expects square matrix as argument");
+    }
+}
+
+/** Check for a square matrix */
+template< class MatT > inline void
+CheckMatSquare(const MatT& m) {
+    typedef et::ExprTraits<MatT> matrix_traits;
+    typedef typename matrix_traits::size_tag size_tag;
+
+    detail::CheckMatSquare<
+        MatT,function_expects_square_matrix_arg_error>(m, size_tag());
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Quaternion argument checking
+//////////////////////////////////////////////////////////////////////////////
+
+/** Compile-time check for a quaternion argument*/
+template< class QuatT > inline void
+CheckQuat(const QuatT& /*q*/)
+{
+    typedef et::ExprTraits<QuatT> quaternion_traits;
+    typedef typename quaternion_traits::result_tag result_type;
+    
+    CML_STATIC_REQUIRE_M(
+        (same_type<result_type, et::quaternion_result_tag>::is_true),
+        function_expects_quaternion_arg_error);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Index argument checking
+//////////////////////////////////////////////////////////////////////////////
+
+/** Run-time check for a valid argument */
+inline void CheckValidArg(bool valid)
+{
+    if (!valid) {
+        throw std::invalid_argument("invalid function argument");
+    }
+}
+
+/** Check for a valid integer index with value < N */
+template < size_t N >
+inline void CheckIndexN(size_t index) {
+    CheckValidArg(index < N);
+}
+
+/** Check for a valid integer index with value < 2 */
+inline void CheckIndex2(size_t index) {
+    CheckIndexN<2>(index);
+}
+
+/** Check for a valid integer index with value < 3 */
+inline void CheckIndex3(size_t index) {
+    CheckIndexN<3>(index);
+}
+
+} // namespace detail
+} // namespace cml
+
+#endif
diff --git a/src/cml/mathlib/coord_conversion.h b/src/cml/mathlib/coord_conversion.h
new file mode 100644 (file)
index 0000000..a42501d
--- /dev/null
@@ -0,0 +1,162 @@
+/* -*- 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 coord_conversion_h
+#define coord_conversion_h
+
+#include <cml/mathlib/checking.h>
+#include <cml/mathlib/epsilon.h>
+#include <cml/mathlib/helper.h>
+
+/* Functions for converting between Cartesian, polar, cylindrical and
+ * spherical coordinates.
+ *
+ * The 3D conversion functions take an integer axis index argument. For
+ * cylindrical coordinates this determines the axis of the cylinder, and for
+ * spherical it determines which cardinal axis is normal to the azimuth plane.
+ *
+ * For spherical coordinates the option of whether to treat phi as latitude
+ * or colatitude is also available. The 'type' argument takes either of the
+ * enumerants cml::latitude and cml::colatitude to reflect this.
+ */
+
+namespace cml {
+
+//////////////////////////////////////////////////////////////////////////////
+// Conversion to Cartesian coordinates
+//////////////////////////////////////////////////////////////////////////////
+
+/* Convert cylindrical coordinates to Cartesian coordinates in R3 */
+template < typename E, class A > void
+cylindrical_to_cartesian(
+    E radius, E theta, E height, size_t axis, vector<E,A>& v)
+{
+    typedef vector<E,A> vector_type;
+    typedef typename vector_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckVec3(v);
+    detail::CheckIndex3(axis);
+
+    size_t i, j, k;
+    cyclic_permutation(axis, i, j, k);
+    
+    v[i] = height;
+    v[j] = std::cos(theta) * radius;
+    v[k] = std::sin(theta) * radius;
+}
+
+/* Convert spherical coordinates to Cartesian coordinates in R3 */
+template < typename E, class A > void
+spherical_to_cartesian(E radius, E theta, E phi, size_t axis,
+    SphericalType type, vector<E,A>& v)
+{
+    typedef vector<E,A> vector_type;
+    typedef typename vector_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckVec3(v);
+    detail::CheckIndex3(axis);
+    
+    if (type == latitude) {
+        phi = constants<value_type>::pi_over_2() - phi;
+    }
+
+    value_type sin_phi = std::sin(phi);
+    value_type cos_phi = std::cos(phi);
+    value_type sin_phi_r = sin_phi * radius;
+
+    size_t i, j, k;
+    cyclic_permutation(axis, i, j, k);
+
+    v[i] = cos_phi * radius;
+    v[j] = sin_phi_r * std::cos(theta);
+    v[k] = sin_phi_r * std::sin(theta);
+}
+
+/* Convert polar coordinates to Cartesian coordinates in R2 */
+template < typename E, class A > void
+polar_to_cartesian(E radius, E theta, vector<E,A>& v)
+{
+    /* Checking handled by set() */
+    v.set(std::cos(theta) * double(radius), std::sin(theta) * double(radius));
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Conversion from Cartesian coordinates
+//////////////////////////////////////////////////////////////////////////////
+
+/* Convert Cartesian coordinates to cylindrical coordinates in R3  */
+template < class VecT, typename Real > void
+cartesian_to_cylindrical(const VecT& v, Real& radius, Real& theta,
+    Real& height, size_t axis, Real tolerance = epsilon<Real>::placeholder())
+{
+    typedef Real value_type;
+
+    /* Checking */
+    detail::CheckVec3(v);
+    detail::CheckIndex3(axis);
+
+    size_t i, j, k;
+    cyclic_permutation(axis, i, j, k);
+    
+    radius = length(v[j],v[k]);
+    theta = radius < tolerance ? value_type(0) : std::atan2(v[k],v[j]);
+    height = v[i];
+}
+
+/* Convert Cartesian coordinates to spherical coordinates in R3 */
+template < class VecT, typename Real > void
+cartesian_to_spherical(const VecT& v, Real& radius, Real& theta, Real& phi,
+    size_t axis, SphericalType type,
+    Real tolerance = epsilon<Real>::placeholder())
+{
+    typedef Real value_type;
+
+    /* Checking */
+    detail::CheckVec3(v);
+    detail::CheckIndex3(axis);
+
+    size_t i, j, k;
+    cyclic_permutation(axis, i, j, k);
+
+    value_type len = length(v[j],v[k]);
+    theta = len < tolerance ? value_type(0) : std::atan2(v[k],v[j]);
+    radius = length(v[i], len);
+    if (radius < tolerance) {
+        phi = value_type(0);
+    } else {
+        phi = std::atan2(len,v[i]);
+        //phi = type.convert(phi);
+        if (type == latitude) {
+            phi = constants<value_type>::pi_over_2() - phi;
+        }
+    }
+}
+
+/* Convert Cartesian coordinates to polar coordinates in R2 */
+template < class VecT, typename Real > void
+cartesian_to_polar(const VecT& v, Real& radius, Real& theta,
+    Real tolerance = epsilon<Real>::placeholder())
+{
+    typedef Real value_type;
+
+    /* Checking */
+    detail::CheckVec2(v);
+
+    radius = v.length();
+    theta = radius < tolerance ? value_type(0) : std::atan2(v[1],v[0]);
+}
+
+} // namespace cml
+
+#endif
diff --git a/src/cml/mathlib/epsilon.h b/src/cml/mathlib/epsilon.h
new file mode 100644 (file)
index 0000000..14059fa
--- /dev/null
@@ -0,0 +1,44 @@
+/* -*- 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 epsilon_h
+#define epsilon_h
+
+namespace cml {
+
+/* @todo: epsilon and tolerance handling.
+ *
+ * @note This is a placeholder for a more sophisticated epsilon/tolerance
+ * system.
+ */
+
+template < typename Real >
+struct epsilon
+{
+    typedef Real value_type;
+    
+private:
+
+    /** For convenience */
+    typedef value_type T;
+    
+public:
+
+    static T placeholder() {
+        /* Completely arbitrary placeholder value: */
+        return T(0.0001);
+    }
+};
+
+} // namespace cml
+
+#endif
diff --git a/src/cml/mathlib/frustum.h b/src/cml/mathlib/frustum.h
new file mode 100644 (file)
index 0000000..6a707af
--- /dev/null
@@ -0,0 +1,239 @@
+/* -*- 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 frustum_h
+#define frustum_h
+
+#include <cml/mathlib/matrix_concat.h>
+#include <cml/mathlib/checking.h>
+
+namespace cml {
+
+/* @todo: plane class, and perhaps named arguments instead of an array. */
+
+/* Extract the planes of a frustum given a modelview matrix and a projection
+ * matrix with the given near z-clipping range. The planes are normalized by
+ * default, but this can be turned off with the 'normalize' argument.
+ *
+ * The planes are in ax+by+cz+d = 0 form, and are in the order:
+ *     left
+ *     right
+ *     bottom
+ *     top
+ *     near
+ *     far
+ */
+
+template < class MatT, typename Real > void
+extract_frustum_planes(
+    const MatT& modelview,
+    const MatT& projection,
+    Real planes[6][4],
+    ZClip z_clip,
+    bool normalize = true)
+{
+    extract_frustum_planes(
+        detail::matrix_concat_transforms_4x4(modelview,projection),
+        planes,
+        z_clip,
+        normalize
+    );
+}
+
+/* Extract the planes of a frustum from a single matrix assumed to contain any
+ * model and view transforms followed by a projection transform with the given
+ * near z-cliping range. The planes are normalized by default, but this can be
+ * turned off with the 'normalize' argument.
+ *
+ * The planes are in ax+by+cz+d = 0 form, and are in the order:
+ *     left
+ *     right
+ *     bottom
+ *     top
+ *     near
+ *     far
+ */
+
+template < class MatT, typename Real > void
+extract_frustum_planes(
+    const MatT& m,
+    Real planes[6][4],
+    ZClip z_clip,
+    bool normalize = true)
+{
+    detail::CheckMatHomogeneous3D(m);
+
+    /* Left:   [03+00, 13+10, 23+20, 33+30] */
+    
+    planes[0][0] = m.basis_element(0,3) + m.basis_element(0,0);
+    planes[0][1] = m.basis_element(1,3) + m.basis_element(1,0);
+    planes[0][2] = m.basis_element(2,3) + m.basis_element(2,0);  
+    planes[0][3] = m.basis_element(3,3) + m.basis_element(3,0);
+    
+    /* Right:  [03-00, 13-10, 23-20, 33-30] */
+    
+    planes[1][0] = m.basis_element(0,3) - m.basis_element(0,0);
+    planes[1][1] = m.basis_element(1,3) - m.basis_element(1,0);
+    planes[1][2] = m.basis_element(2,3) - m.basis_element(2,0);  
+    planes[1][3] = m.basis_element(3,3) - m.basis_element(3,0);
+    
+    /* Bottom: [03+01, 13+11, 23+21, 33+31] */
+    
+    planes[2][0] = m.basis_element(0,3) + m.basis_element(0,1);
+    planes[2][1] = m.basis_element(1,3) + m.basis_element(1,1);
+    planes[2][2] = m.basis_element(2,3) + m.basis_element(2,1);  
+    planes[2][3] = m.basis_element(3,3) + m.basis_element(3,1);
+    
+    /* Top:    [03-01, 13-11, 23-21, 33-31] */
+    
+    planes[3][0] = m.basis_element(0,3) - m.basis_element(0,1);
+    planes[3][1] = m.basis_element(1,3) - m.basis_element(1,1);
+    planes[3][2] = m.basis_element(2,3) - m.basis_element(2,1);  
+    planes[3][3] = m.basis_element(3,3) - m.basis_element(3,1);
+    
+    /* Far:    [03-02, 13-12, 23-22, 33-32] */
+    
+    planes[5][0] = m.basis_element(0,3) - m.basis_element(0,2);
+    planes[5][1] = m.basis_element(1,3) - m.basis_element(1,2);
+    planes[5][2] = m.basis_element(2,3) - m.basis_element(2,2);  
+    planes[5][3] = m.basis_element(3,3) - m.basis_element(3,2);
+    
+    /* Near:   [03+02, 13+12, 23+22, 33+32] : [02, 12, 22, 32] */
+    
+    if (z_clip == z_clip_neg_one) {       
+        planes[4][0] = m.basis_element(0,3) + m.basis_element(0,2);
+        planes[4][1] = m.basis_element(1,3) + m.basis_element(1,2);
+        planes[4][2] = m.basis_element(2,3) + m.basis_element(2,2);  
+        planes[4][3] = m.basis_element(3,3) + m.basis_element(3,2);
+    } else { // z_clip == z_clip_zero
+        planes[4][0] = m.basis_element(0,2);
+        planes[4][1] = m.basis_element(1,2);
+        planes[4][2] = m.basis_element(2,2);  
+        planes[4][3] = m.basis_element(3,2);
+    }
+
+    /* @todo: This will be handled by the plane class */
+    if (normalize) {
+        for (size_t i = 0; i < 6; ++i) {
+            Real invl = inv_sqrt(planes[i][0] * planes[i][0] +
+                                 planes[i][1] * planes[i][1] +
+                                 planes[i][2] * planes[i][2]);
+                                
+            planes[i][0] *= invl;
+            planes[i][1] *= invl;
+            planes[i][2] *= invl;
+            planes[i][3] *= invl;
+        }
+    }
+}
+
+namespace detail {
+
+/* This is currently only in support of finding the corners of a frustum.
+ * The input planes are assumed to have a single unique intersection, so
+ * no tolerance is used.
+ */
+
+template < typename Real > vector< Real, fixed<3> >
+intersect_planes(Real p1[4], Real p2[4], Real p3[4])
+{
+    typedef vector< Real, fixed<3> > vector_type;
+    typedef typename vector_type::value_type value_type;
+
+    vector_type n1(p1[0],p1[1],p1[2]);
+    vector_type n2(p2[0],p2[1],p2[2]);
+    vector_type n3(p3[0],p3[1],p3[2]);
+    
+    value_type d1 = -p1[3];
+    value_type d2 = -p2[3];
+    value_type d3 = -p3[3];
+    
+    vector_type numer =
+        d1*cross(n2,n3) + d2*cross(n3,n1) + d3*cross(n1,n2);
+    value_type denom = triple_product(n1,n2,n3);
+    return numer/denom;
+}
+
+} // namespace detail
+
+/* Get the corners of a frustum defined by 6 planes. The planes are in
+ * ax+by+cz+d = 0 form, and are in the order:
+ *     left
+ *     right
+ *     bottom
+ *     top
+ *     near
+ *     far
+ *
+ * The corners are in CCW order starting in the lower-left, first at the near
+ * plane, then at the far plane.
+ */
+
+template < typename Real, typename E, class A > void
+get_frustum_corners(Real planes[6][4], vector<E,A> corners[8])
+{
+    // NOTE: Prefixed with 'PLANE_' due to symbol conflict with Windows
+    // macros PLANE_LEFT and PLANE_RIGHT.
+    enum {
+        PLANE_LEFT,
+        PLANE_RIGHT,
+        PLANE_BOTTOM,
+        PLANE_TOP,
+        PLANE_NEAR,
+        PLANE_FAR
+    };
+
+    corners[0] = detail::intersect_planes(
+        planes[PLANE_LEFT],
+        planes[PLANE_BOTTOM],
+        planes[PLANE_NEAR]
+    );
+    corners[1] = detail::intersect_planes(
+        planes[PLANE_RIGHT],
+        planes[PLANE_BOTTOM],
+        planes[PLANE_NEAR]
+    );
+    corners[2] = detail::intersect_planes(
+        planes[PLANE_RIGHT],
+        planes[PLANE_TOP],
+        planes[PLANE_NEAR]
+    );
+    corners[3] = detail::intersect_planes(
+        planes[PLANE_LEFT],
+        planes[PLANE_TOP],
+        planes[PLANE_NEAR]
+    );
+    corners[4] = detail::intersect_planes(
+        planes[PLANE_LEFT],
+        planes[PLANE_BOTTOM],
+        planes[PLANE_FAR]
+    );
+    corners[5] = detail::intersect_planes(
+        planes[PLANE_RIGHT],
+        planes[PLANE_BOTTOM],
+        planes[PLANE_FAR]
+    );
+    corners[6] = detail::intersect_planes(
+        planes[PLANE_RIGHT],
+        planes[PLANE_TOP],
+        planes[PLANE_FAR]
+    );
+    corners[7] = detail::intersect_planes(
+        planes[PLANE_LEFT],
+        planes[PLANE_TOP],
+        planes[PLANE_FAR]
+    );
+}
+
+} // namespace cml
+
+#endif
diff --git a/src/cml/mathlib/helper.h b/src/cml/mathlib/helper.h
new file mode 100644 (file)
index 0000000..054cb55
--- /dev/null
@@ -0,0 +1,158 @@
+/* -*- 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 helper_h
+#define helper_h
+
+#include <cstddef>
+#include <cml/constants.h>
+
+namespace cml {
+
+/* Helper classes for axis order, coordinate system handedness, z-clipping
+ * range and spherical coordinate type.
+ */
+//////////////////////////////////////////////////////////////////////////////
+// Euler order
+//////////////////////////////////////////////////////////////////////////////
+
+enum EulerOrder {
+    euler_order_xyz, // 0x00 [0000]
+    euler_order_xyx, // 0x01 [0001]
+    euler_order_xzy, // 0x02 [0010]
+    euler_order_xzx, // 0x03 [0011]
+    euler_order_yzx, // 0x04 [0100]
+    euler_order_yzy, // 0x05 [0101]
+    euler_order_yxz, // 0x06 [0110]
+    euler_order_yxy, // 0x07 [0111]
+    euler_order_zxy, // 0x08 [1000]
+    euler_order_zxz, // 0x09 [1001]
+    euler_order_zyx, // 0x0A [1010]
+    euler_order_zyz  // 0x0B [1011]
+};
+
+namespace detail {
+
+inline void unpack_euler_order(
+    EulerOrder order,
+    size_t& i,
+    size_t& j,
+    size_t& k,
+    bool& odd,
+    bool& repeat)
+{
+    enum { REPEAT = 0x01, ODD = 0x02, AXIS = 0x0C };
+
+    repeat = order & REPEAT;
+    odd = ((order & ODD) == ODD);
+    size_t offset = size_t(odd);
+    i = (order & AXIS) % 3;
+    j = (i + 1 + offset) % 3;
+    k = (i + 2 - offset) % 3;
+}
+
+} // namespace detail
+
+//////////////////////////////////////////////////////////////////////////////
+// Axis order
+//////////////////////////////////////////////////////////////////////////////
+
+enum AxisOrder {
+    axis_order_xyz = euler_order_xyz, // 0x00 [0000]
+    axis_order_xzy = euler_order_xzy, // 0x02 [0010]
+    axis_order_yzx = euler_order_yzx, // 0x04 [0100]
+    axis_order_yxz = euler_order_yxz, // 0x06 [0110]
+    axis_order_zxy = euler_order_zxy, // 0x08 [1000]
+    axis_order_zyx = euler_order_zyx, // 0x0A [1010]
+};
+
+namespace detail {
+
+inline void unpack_axis_order(
+    AxisOrder order,
+    size_t& i,
+    size_t& j,
+    size_t& k,
+    bool& odd)
+{
+    enum { ODD = 0x02, AXIS = 0x0C };
+
+    odd = ((order & ODD) == ODD);
+    size_t offset = size_t(odd);
+    i = (order & AXIS) % 3;
+    j = (i + 1 + offset) % 3;
+    k = (i + 2 - offset) % 3;
+}
+
+inline AxisOrder pack_axis_order(size_t i, bool odd) {
+    return AxisOrder((i << 2) | (size_t(odd) << 1));
+}
+
+inline AxisOrder swap_axis_order(AxisOrder order)
+{
+    size_t i, j, k;
+    bool odd;
+    unpack_axis_order(order, i, j, k, odd);
+    return pack_axis_order(j, !odd);
+}
+
+} // namespace detail
+
+//////////////////////////////////////////////////////////////////////////////
+// Axis order 2D
+//////////////////////////////////////////////////////////////////////////////
+
+enum AxisOrder2D {
+    axis_order_xy = axis_order_xyz, // 0x00 [0000]
+    axis_order_yx = axis_order_yxz, // 0x06 [0110]
+};
+
+namespace detail {
+
+inline void unpack_axis_order_2D(
+    AxisOrder2D order,
+    size_t& i,
+    size_t& j,
+    bool& odd)
+{
+    enum { ODD = 0x02, AXIS = 0x0C };
+
+    odd = ((order & ODD) == ODD);
+    size_t offset = size_t(odd);
+    i = (order & AXIS) % 3;
+    j = (i + 1 + offset) % 3;
+}
+
+} // namespace detail
+
+//////////////////////////////////////////////////////////////////////////////
+// Handedness
+//////////////////////////////////////////////////////////////////////////////
+
+enum Handedness { left_handed, right_handed };
+
+//////////////////////////////////////////////////////////////////////////////
+// Z clip
+//////////////////////////////////////////////////////////////////////////////
+
+enum ZClip { z_clip_neg_one, z_clip_zero };
+
+//////////////////////////////////////////////////////////////////////////////
+// Spherical coordinate type
+//////////////////////////////////////////////////////////////////////////////
+
+enum SphericalType { latitude, colatitude };
+
+} // namespace cml
+
+#endif
diff --git a/src/cml/mathlib/interpolation.h b/src/cml/mathlib/interpolation.h
new file mode 100644 (file)
index 0000000..4a9fd54
--- /dev/null
@@ -0,0 +1,1129 @@
+/* -*- 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 interpolation_h
+#define interpolation_h
+
+#include <cml/mathlib/matrix_rotation.h>
+
+/* Interpolation functions.
+ *
+ * @todo: This code works, but it needs a lot of cleanup.
+ */
+
+namespace cml {
+
+struct function_expects_args_of_same_type_error;
+
+namespace detail {
+
+//////////////////////////////////////////////////////////////////////////////
+// Helper struct to promote vectors, quaternions, and matrices
+//////////////////////////////////////////////////////////////////////////////
+
+template< class T1, class T2, class ResultT > struct TypePromote;
+
+template< class T >
+struct TypePromote< T,T,et::scalar_result_tag > {
+    typedef T temporary_type;
+};
+
+template< class T1, class T2 >
+struct TypePromote< T1,T2,et::scalar_result_tag > {
+    typedef et::ExprTraits<T1> traits_1;
+    typedef et::ExprTraits<T2> traits_2;
+    typedef typename traits_1::result_tag result_type_1;
+    typedef typename traits_2::result_tag result_type_2;
+    
+    /* Check that results are of the same type */
+    CML_STATIC_REQUIRE_M(
+        (same_type<result_type_1, result_type_2>::is_true),
+        function_expects_args_of_same_type_error);
+
+    typedef typename et::ScalarPromote<T1,T2>::type temporary_type;
+};
+
+template< class T1, class T2 >
+struct TypePromote< T1,T2,et::vector_result_tag > {
+    typedef et::ExprTraits<T1> traits_1;
+    typedef et::ExprTraits<T2> traits_2;
+    typedef typename traits_1::result_tag result_type_1;
+    typedef typename traits_2::result_tag result_type_2;
+    
+    /* Check that results are of the same type */
+    CML_STATIC_REQUIRE_M(
+        (same_type<result_type_1, result_type_2>::is_true),
+        function_expects_args_of_same_type_error);
+
+    /* @todo: This should be VectorPromote<> for symmetry with the other
+     * type promotions.
+     */
+    typedef typename CrossPromote<T1,T2>::promoted_vector temporary_type;
+};
+
+template< class T1, class T2 >
+struct TypePromote< T1,T2,et::matrix_result_tag > {
+    typedef et::ExprTraits<T1> traits_1;
+    typedef et::ExprTraits<T2> traits_2;
+    typedef typename traits_1::result_tag result_type_1;
+    typedef typename traits_2::result_tag result_type_2;
+    
+    /* Check that results are of the same type */
+    CML_STATIC_REQUIRE_M(
+        (same_type<result_type_1, result_type_2>::is_true),
+        function_expects_args_of_same_type_error);
+
+    typedef typename et::MatrixPromote2<T1,T2>::temporary_type temporary_type;
+};
+
+template< class T1, class T2 >
+struct TypePromote< T1,T2,et::quaternion_result_tag > {
+    typedef et::ExprTraits<T1> traits_1;
+    typedef et::ExprTraits<T2> traits_2;
+    typedef typename traits_1::result_tag result_type_1;
+    typedef typename traits_2::result_tag result_type_2;
+    
+    /* Check that results are of the same type */
+    CML_STATIC_REQUIRE_M(
+        (same_type<result_type_1, result_type_2>::is_true),
+        function_expects_args_of_same_type_error);
+
+    typedef typename et::QuaternionPromote2<T1,T2>::temporary_type
+        temporary_type;
+};
+
+template< class T1, class T2, class T3, class ResultT > struct TypePromote3;
+
+template< class T1, class T2, class T3 >
+struct TypePromote3< T1,T2,T3,et::matrix_result_tag > {
+    typedef et::ExprTraits<T1> traits_1;
+    typedef et::ExprTraits<T2> traits_2;
+    typedef et::ExprTraits<T3> traits_3;
+    typedef typename traits_1::result_tag result_type_1;
+    typedef typename traits_2::result_tag result_type_2;
+    typedef typename traits_3::result_tag result_type_3;
+    
+    /* Check that results are of the same type */
+    CML_STATIC_REQUIRE_M(
+        (same_type<result_type_1, result_type_2>::is_true),
+        function_expects_args_of_same_type_error);
+    CML_STATIC_REQUIRE_M(
+        (same_type<result_type_1, result_type_3>::is_true),
+        function_expects_args_of_same_type_error);
+
+    typedef typename et::MatrixPromote3<T1,T2,T3>::temporary_type
+        temporary_type;
+    typedef typename temporary_type::value_type value_type;
+};
+
+template< class T1, class T2, class T3 >
+struct TypePromote3< T1,T2,T3,et::quaternion_result_tag > {
+    typedef et::ExprTraits<T1> traits_1;
+    typedef et::ExprTraits<T2> traits_2;
+    typedef et::ExprTraits<T3> traits_3;
+    typedef typename traits_1::result_tag result_type_1;
+    typedef typename traits_2::result_tag result_type_2;
+    typedef typename traits_3::result_tag result_type_3;
+    
+    /* Check that results are of the same type */
+    CML_STATIC_REQUIRE_M(
+        (same_type<result_type_1, result_type_2>::is_true),
+        function_expects_args_of_same_type_error);
+    CML_STATIC_REQUIRE_M(
+        (same_type<result_type_1, result_type_3>::is_true),
+        function_expects_args_of_same_type_error);
+
+    typedef typename et::QuaternionPromote3<T1,T2,T3>::temporary_type
+        temporary_type;
+    typedef typename temporary_type::value_type value_type;
+};
+
+template <
+    class T1, class T2, class T3, class T4, class ResultT
+> struct TypePromote4;
+
+template< class T1, class T2, class T3, class T4 >
+struct TypePromote4< T1,T2,T3,T4,et::matrix_result_tag > {
+    typedef et::ExprTraits<T1> traits_1;
+    typedef et::ExprTraits<T2> traits_2;
+    typedef et::ExprTraits<T3> traits_3;
+    typedef et::ExprTraits<T4> traits_4;
+    typedef typename traits_1::result_tag result_type_1;
+    typedef typename traits_2::result_tag result_type_2;
+    typedef typename traits_3::result_tag result_type_3;
+    typedef typename traits_4::result_tag result_type_4;
+    
+    /* Check that results are of the same type */
+    CML_STATIC_REQUIRE_M(
+        (same_type<result_type_1, result_type_2>::is_true),
+        function_expects_args_of_same_type_error);
+    CML_STATIC_REQUIRE_M(
+        (same_type<result_type_1, result_type_3>::is_true),
+        function_expects_args_of_same_type_error);
+    CML_STATIC_REQUIRE_M(
+        (same_type<result_type_1, result_type_4>::is_true),
+        function_expects_args_of_same_type_error);
+
+    typedef typename et::MatrixPromote4<T1,T2,T3,T4>::temporary_type
+        temporary_type;
+    typedef typename temporary_type::value_type value_type;
+};
+
+template< class T1, class T2, class T3, class T4 >
+struct TypePromote4< T1,T2,T3,T4,et::quaternion_result_tag > {
+    typedef et::ExprTraits<T1> traits_1;
+    typedef et::ExprTraits<T2> traits_2;
+    typedef et::ExprTraits<T3> traits_3;
+    typedef et::ExprTraits<T4> traits_4;
+    typedef typename traits_1::result_tag result_type_1;
+    typedef typename traits_2::result_tag result_type_2;
+    typedef typename traits_3::result_tag result_type_3;
+    typedef typename traits_4::result_tag result_type_4;
+    
+    /* Check that results are of the same type */
+    CML_STATIC_REQUIRE_M(
+        (same_type<result_type_1, result_type_2>::is_true),
+        function_expects_args_of_same_type_error);
+    CML_STATIC_REQUIRE_M(
+        (same_type<result_type_1, result_type_3>::is_true),
+        function_expects_args_of_same_type_error);
+    CML_STATIC_REQUIRE_M(
+        (same_type<result_type_1, result_type_4>::is_true),
+        function_expects_args_of_same_type_error);
+
+    typedef typename et::QuaternionPromote4<T1,T2,T3,T4>::temporary_type
+        temporary_type;
+    typedef typename temporary_type::value_type value_type;
+};
+
+//////////////////////////////////////////////////////////////////////////////
+// Helper functions to resize a vector, quaternion or matrix
+//////////////////////////////////////////////////////////////////////////////
+
+// Should be able to catch all no-ops with a generic function template...
+
+template < class T1, class T2, class SizeTag > void
+InterpResize(T1& t1, const T2& t2, SizeTag) {}
+
+// Catch vector and matrix resizes...
+
+template< typename E, class A, class VecT > void
+InterpResize(vector<E,A>& v, const VecT& target, dynamic_size_tag) {
+    v.resize(target.size());
+}
+
+template< typename E, class A, class B, class L, class MatT > void
+InterpResize(matrix<E,A,B,L>& m, const MatT& target, dynamic_size_tag) {
+    m.resize(target.rows(),target.cols());
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Construction of 'intermediate' quaternions and matrices for use with squad
+//////////////////////////////////////////////////////////////////////////////
+
+#if 0
+template < class QuatT_1, class QuatT_2 >
+typename et::QuaternionPromote2<QuatT_1,QuatT_2>::temporary_type
+concatenate_quaternions(
+    const QuatT_1& q1,
+    const QuatT_2& q2,
+    positive_cross)
+{
+    return q2 * q1;
+}
+
+template < class QuatT_1, class QuatT_2 >
+typename et::QuaternionPromote2<QuatT_1,QuatT_2>::temporary_type
+concatenate_quaternions(
+    const QuatT_1& q1,
+    const QuatT_2& q2,
+    negative_cross)
+{
+    return q1 * q2;
+}
+
+template< class T1, class T2, class T3, class SizeT >
+typename detail::TypePromote3<
+    T1,T2,T3,typename et::ExprTraits<T1>::result_tag
+>::temporary_type
+squad_intermediate(
+    const T1& t1,
+    const T2& t2,
+    const T3& t3,
+    typename detail::TypePromote3<
+        T1, T2, T3, typename et::ExprTraits<T1>::result_tag
+    >::value_type tolerance,
+    et::quaternion_result_tag,
+    SizeT)
+{
+    typedef et::ExprTraits<T1> traits_1;
+    typedef typename traits_1::result_tag result_type_1;
+
+    typedef typename detail::TypePromote3<T1,T2,T3,result_type_1>::temporary_type
+        temporary_type;
+    typedef typename temporary_type::value_type value_type;
+    typedef typename temporary_type::cross_type cross_type;
+    typedef et::ExprTraits<temporary_type> result_traits;
+    typedef typename result_traits::size_tag size_tag;
+    
+    /**
+     * NOTE: It seems that the equation for computing an intermediate
+     * quaternion produces the same results regardless of whether 'standard'
+     * or 'reverse' multiplication order is used (I haven't proved this -
+     * I've just observed it). Still, just to be sure I've used a pair of
+     * helper functions to ensure that the quaternions are multiplied in the
+     * right order.
+     */
+
+    temporary_type result;
+    detail::InterpResize(result, t1, size_tag());
+
+    temporary_type t2_inverse = conjugate(t2);
+    temporary_type temp1 = concatenate_quaternions(t1, t2_inverse, cross_type());
+    temporary_type temp2 = concatenate_quaternions(t3, t2_inverse, cross_type());
+    result = concatenate_quaternions(
+        exp(-(log(temp1) + log(temp2)) * value_type(.25)), t2, cross_type());
+    return result;
+}
+
+/**
+ * NOTE: Construction of intermediate rotation matrices for use with squad
+ * is currently implemented in terms of quaternions. This is pretty
+ * inefficient (especially so in the 2-d case, which involves jumping through
+ * a lot of hoops to get to 3-d and back), and is inelegant as well.
+ *
+ * I imagine this could be streamlined to work directly with the matrices, but
+ * I'd need to dig a bit first (figure out the matrix equivalents of
+ * quaternion exp() and log(), figure out what shortcuts can be taken in
+ * 2-d, etc.), so for now it'll just have to remain as-is.
+ *
+ * In future versions of the CML, it might also be worth reconsidering
+ * whether it's wise to support slerp and squad for matrices. Although it
+ * can be done, it's not efficient, and may give the user a false sense of
+ * security with respect to the efficiency of the underlying operations.
+ */
+
+template< class MatT_1, class MatT_2, class MatT_3, size_t N >
+struct squad_intermediate_f;
+
+template< class MatT_1, class MatT_2, class MatT_3 >
+struct squad_intermediate_f<MatT_1,MatT_2,MatT_3,3>
+{
+    template< typename Real >
+    typename et::MatrixPromote3< MatT_1,MatT_2,MatT_3 >::temporary_type
+    operator()(
+        const MatT_1& m1,
+        const MatT_2& m2,
+        const MatT_3& m3,
+        Real tolerance)
+    {
+        typedef typename et::MatrixPromote3<
+            MatT_1,MatT_2,MatT_3 >::temporary_type temporary_type;
+        typedef typename temporary_type::value_type value_type;
+        typedef quaternion< value_type > quaternion_type;
+
+        quaternion_type q1, q2, q3;
+        quaternion_rotation_matrix(q1, m1);
+        quaternion_rotation_matrix(q2, m2);
+        quaternion_rotation_matrix(q3, m3);
+
+        quaternion_type q4 = squad_intermediate(q1, q2, q3, tolerance);
+
+        temporary_type m;
+        et::detail::Resize(m,3,3);
+        
+        matrix_rotation_quaternion(m, q4);
+        
+        return m;
+    }
+};
+
+template< class MatT_1, class MatT_2, class MatT_3 >
+struct squad_intermediate_f<MatT_1,MatT_2,MatT_3,2>
+{
+    template< typename Real >
+    typename et::MatrixPromote3< MatT_1,MatT_2,MatT_3 >::temporary_type
+    operator()(
+        const MatT_1& m1,
+        const MatT_2& m2,
+        const MatT_3& m3,
+        Real tolerance)
+    {
+        typedef typename et::MatrixPromote3<
+            MatT_1,MatT_2,MatT_3 >::temporary_type temporary_type;
+        typedef typename temporary_type::value_type value_type;
+        typedef quaternion< value_type > quaternion_type;
+        typedef vector< value_type, fixed<3> > vector_type;
+        
+        value_type angle1 = matrix_to_rotation_2D(m1);
+        value_type angle2 = matrix_to_rotation_2D(m2);
+        value_type angle3 = matrix_to_rotation_2D(m3);
+        vector_type axis(value_type(0), value_type(0), value_type(1));
+        
+        quaternion_type q1, q2, q3;
+        quaternion_rotation_axis_angle(q1, axis, angle1);
+        quaternion_rotation_axis_angle(q2, axis, angle2);
+        quaternion_rotation_axis_angle(q3, axis, angle3);
+
+        quaternion_type q4 = squad_intermediate(q1, q2, q3, tolerance);
+        
+        value_type angle;
+        quaternion_to_axis_angle(q4, axis, angle);
+
+        temporary_type m;
+        et::detail::Resize(m,2,2);
+        
+        matrix_rotation_2D(m, angle);
+        
+        return m;
+    }
+};
+
+template< class MatT_1, class MatT_2, class MatT_3, typename Real >
+typename et::MatrixPromote3< MatT_1,MatT_2,MatT_3 >::temporary_type
+squad_intermediate(
+    const MatT_1& m1,
+    const MatT_2& m2,
+    const MatT_3& m3,
+    Real tolerance,
+    et::matrix_result_tag,
+    fixed_size_tag)
+{
+    return squad_intermediate_f<MatT_1,MatT_2,MatT_3,MatT_1::array_rows>()(
+        m1,m2,m3,tolerance);
+}
+
+template< class MatT_1, class MatT_2, class MatT_3, typename Real >
+typename et::MatrixPromote3< MatT_1,MatT_2,MatT_3 >::temporary_type
+squad_intermediate(
+    const MatT_1& m1,
+    const MatT_2& m2,
+    const MatT_3& m3,
+    Real tolerance,
+    et::matrix_result_tag,
+    dynamic_size_tag)
+{
+    typedef typename et::MatrixPromote3<
+        MatT_1,MatT_2,MatT_3 >::temporary_type temporary_type;
+
+    temporary_type m;
+    et::detail::Resize(m,m1.rows(),m1.cols());
+    
+    switch (m1.rows()) {
+        case 3:
+            m = squad_intermediate_f<MatT_1,MatT_2,MatT_3,3>()(m1,m2,m3,tolerance);
+            break;
+        case 2:
+            m = squad_intermediate_f<MatT_1,MatT_2,MatT_3,2>()(m1,m2,m3,tolerance);
+            break;
+        default:
+            throw std::invalid_argument(
+                "matrix squad_intermediate_f() expects sizes 3x3 or 2x2");
+            break;
+    }
+    return m;
+}
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Spherical linear interpolation of two vectors of any size
+//////////////////////////////////////////////////////////////////////////////
+
+template< class VecT_1, class VecT_2, typename Real, class SizeT >
+typename detail::TypePromote<
+    VecT_1,VecT_2,typename et::ExprTraits<VecT_1>::result_tag
+>::temporary_type
+slerp(
+    const VecT_1& v1,
+    const VecT_2& v2,
+    Real t,
+    Real tolerance,
+    et::vector_result_tag,
+    SizeT)
+{
+    typedef et::ExprTraits<VecT_1> type_traits;
+    typedef typename type_traits::result_tag result_type;
+    typedef typename
+        detail::TypePromote<VecT_1,VecT_2,result_type>::temporary_type
+            temporary_type;
+    typedef typename temporary_type::value_type value_type;
+    typedef et::ExprTraits<temporary_type> result_traits;
+    typedef typename result_traits::size_tag size_tag;
+            
+    temporary_type result;
+    detail::InterpResize(result, v1, size_tag());
+
+    value_type omega = acos_safe(dot(v1,v2));
+    value_type s = std::sin(omega);
+    if (s < tolerance) {
+        result = nlerp(v1,v2,t);
+    } else {
+        result = (value_type(std::sin((value_type(1)-t)*omega))*v1 +
+            value_type(std::sin(t*omega))*v2) / s;
+    }
+    return result;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Spherical linear interpolation of two quaternions
+//////////////////////////////////////////////////////////////////////////////
+
+template< class QuatT_1, class QuatT_2, typename Real, class SizeT >
+typename detail::TypePromote<
+    QuatT_1,QuatT_2,typename et::ExprTraits<QuatT_1>::result_tag
+>::temporary_type
+slerp(
+    const QuatT_1& q1,
+    const QuatT_2& q2,
+    Real t,
+    Real tolerance,
+    et::quaternion_result_tag,
+    SizeT)
+{
+    typedef et::ExprTraits<QuatT_1> type_traits;
+    typedef typename type_traits::result_tag result_type;
+    typedef typename
+        detail::TypePromote<QuatT_1,QuatT_2,result_type>::temporary_type
+            temporary_type;
+    typedef typename temporary_type::value_type value_type;
+
+    temporary_type q3 = q2;
+    value_type c = dot(q1,q3);
+    if (c < value_type(0)) {
+        // Turning this off temporarily to test squad...
+        q3 = -q3;
+        c = -c;
+    }
+    
+    value_type omega = acos_safe(c);
+    value_type s = std::sin(omega);
+
+    return (s < tolerance) ?
+        normalize(lerp(q1,q3,t)) :
+        (value_type(std::sin((value_type(1) - t) * omega)) * q1+
+            value_type(std::sin(t * omega)) * q3) / s;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Helper struct for spherical linear interpolation of 3x3 and 2x2 matrices
+//////////////////////////////////////////////////////////////////////////////
+
+template< class MatT_1, class MatT_2, size_t N > struct slerp_f;
+
+template< class MatT_1, class MatT_2 > struct slerp_f<MatT_1,MatT_2,3>
+{
+    template< typename Real >
+    typename detail::TypePromote<
+        MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+    >::temporary_type
+    operator()(
+        const MatT_1& m1,
+        const MatT_2& m2,
+        Real t,
+        Real tolerance)
+    {
+        typedef typename detail::TypePromote<
+            MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+        >::temporary_type temporary_type;
+
+        temporary_type m;
+        et::detail::Resize(m,3,3);
+        m = matrix_rotation_difference(m1,m2);
+        matrix_scale_rotation_angle(m,t,tolerance);
+        m = detail::matrix_concat_rotations(m1,m);
+        return m;
+    }
+};
+
+template< class MatT_1, class MatT_2 > struct slerp_f<MatT_1,MatT_2,2>
+{
+    template< typename Real >
+    typename detail::TypePromote<
+        MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+    >::temporary_type
+    operator()(
+        const MatT_1& m1,
+        const MatT_2& m2,
+        Real t,
+        Real tolerance)
+    {
+        typedef typename detail::TypePromote<
+            MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+        >::temporary_type temporary_type;
+
+        temporary_type m;
+        et::detail::Resize(m,2,2);
+        m = matrix_rotation_difference_2D(m1,m2);
+        matrix_scale_rotation_angle_2D(m,t,tolerance);
+        m = detail::matrix_concat_rotations_2D(m1,m);
+        return m;
+    }
+};
+
+//////////////////////////////////////////////////////////////////////////////
+// Spherical linear interpolation of two matrices of size 3x3 or 2x2
+//////////////////////////////////////////////////////////////////////////////
+
+template< class MatT_1, class MatT_2, typename Real >
+typename detail::TypePromote<
+    MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+>::temporary_type
+slerp(
+    const MatT_1& m1,
+    const MatT_2& m2,
+    Real t,
+    Real tolerance,
+    et::matrix_result_tag,
+    fixed_size_tag)
+{
+    return slerp_f<MatT_1,MatT_2,MatT_1::array_rows>()(m1,m2,t,tolerance);
+}
+
+template< class MatT_1, class MatT_2, typename Real >
+typename detail::TypePromote<
+    MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+>::temporary_type
+slerp(
+    const MatT_1& m1,
+    const MatT_2& m2,
+    Real t,
+    Real tolerance,
+    et::matrix_result_tag,
+    dynamic_size_tag)
+{
+    typedef typename detail::TypePromote<
+        MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+    >::temporary_type temporary_type;
+
+    temporary_type m;
+    et::detail::Resize(m,m1.rows(),m1.cols());
+    
+    switch (m1.rows()) {
+        case 3:
+            m = slerp_f<MatT_1,MatT_2,3>()(m1,m2,t,tolerance);
+            break;
+        case 2:
+            m = slerp_f<MatT_1,MatT_2,2>()(m1,m2,t,tolerance);
+            break;
+        default:
+            throw std::invalid_argument(
+                "matrix slerp() expects sizes 3x3 or 2x2");
+            break;
+    }
+    return m;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Normalized linear interpolation of two vectors of any size
+//////////////////////////////////////////////////////////////////////////////
+
+template< class VecT_1, class VecT_2, typename Real, class SizeT >
+typename detail::TypePromote<
+    VecT_1,VecT_2,typename et::ExprTraits<VecT_1>::result_tag
+>::temporary_type
+nlerp(
+    const VecT_1& v1,
+    const VecT_2& v2,
+    Real t,
+    et::vector_result_tag,
+    SizeT)
+{
+    typedef et::ExprTraits<VecT_1> type_traits;
+    typedef typename type_traits::result_tag result_type;
+    typedef typename
+        detail::TypePromote<VecT_1,VecT_2,result_type>::temporary_type
+            temporary_type;
+    typedef typename temporary_type::value_type value_type;
+    typedef et::ExprTraits<temporary_type> result_traits;
+    typedef typename result_traits::size_tag size_tag;
+            
+    temporary_type result;
+    detail::InterpResize(result, v1, size_tag());
+
+    result = (value_type(1)-t)*v1+t*v2;
+    result.normalize();
+    return result;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Normalized linear interpolation of two quaternions
+//////////////////////////////////////////////////////////////////////////////
+
+template< class QuatT_1, class QuatT_2, typename Real, class SizeT >
+typename detail::TypePromote<
+    QuatT_1,QuatT_2,typename et::ExprTraits<QuatT_1>::result_tag
+>::temporary_type
+nlerp(
+    const QuatT_1& q1,
+    const QuatT_2& q2,
+    Real t,
+    et::quaternion_result_tag,
+    SizeT)
+{
+    typedef et::ExprTraits<QuatT_1> type_traits;
+    typedef typename type_traits::result_tag result_type;
+    typedef typename
+        detail::TypePromote<QuatT_1,QuatT_2,result_type>::temporary_type
+            temporary_type;
+    typedef typename temporary_type::value_type value_type;
+
+    return normalize(lerp(q1, (dot(q1,q2) < value_type(0)) ? -q2 : q2, t));
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Helper struct for normalized linear interpolation of 3x3 and 2x2 matrices
+//////////////////////////////////////////////////////////////////////////////
+
+template< class MatT_1, class MatT_2, size_t N > struct nlerp_f;
+
+template< class MatT_1, class MatT_2 > struct nlerp_f<MatT_1,MatT_2,3>
+{
+    template< typename Real >
+    typename detail::TypePromote<
+        MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+    >::temporary_type
+    operator()(
+        const MatT_1& m1,
+        const MatT_2& m2,
+        Real t)
+    {
+        typedef typename detail::TypePromote<
+            MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+        >::temporary_type temporary_type;
+        typedef typename temporary_type::value_type value_type;
+
+        temporary_type m;
+        et::detail::Resize(m,3,3);
+        m = lerp(m1,m2,t);
+        matrix_orthogonalize_3x3(m);
+        return m;
+    }
+};
+
+template< class MatT_1, class MatT_2 > struct nlerp_f<MatT_1,MatT_2,2>
+{
+    template< typename Real >
+    typename detail::TypePromote<
+        MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+    >::temporary_type
+    operator()(
+        const MatT_1& m1,
+        const MatT_2& m2,
+        Real t)
+    {
+        typedef typename detail::TypePromote<
+            MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+        >::temporary_type temporary_type;
+        typedef typename temporary_type::value_type value_type;
+
+        temporary_type m;
+        et::detail::Resize(m,2,2);
+        m = lerp(m1,m2,t);
+        matrix_orthogonalize_2x2(m);
+        return m;
+    }
+};
+
+//////////////////////////////////////////////////////////////////////////////
+// Normalized linear interpolation of two matrices of size 3x3 or 2x2
+//////////////////////////////////////////////////////////////////////////////
+
+template< class MatT_1, class MatT_2, typename Real >
+typename detail::TypePromote<
+    MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+>::temporary_type
+nlerp(
+    const MatT_1& m1,
+    const MatT_2& m2,
+    Real t,
+    et::matrix_result_tag,
+    fixed_size_tag)
+{
+    return nlerp_f<MatT_1,MatT_2,MatT_1::array_rows>()(m1,m2,t);
+}
+
+template< class MatT_1, class MatT_2, typename Real >
+typename detail::TypePromote<
+    MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+>::temporary_type
+nlerp(
+    const MatT_1& m1,
+    const MatT_2& m2,
+    Real t,
+    et::matrix_result_tag,
+    dynamic_size_tag)
+{
+    typedef typename detail::TypePromote<
+        MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+    >::temporary_type temporary_type;
+
+    temporary_type m;
+    et::detail::Resize(m,m1.rows(),m1.cols());
+    
+    switch (m1.rows()) {
+        case 3:
+            m = nlerp_f<MatT_1,MatT_2,3>()(m1,m2,t);
+            break;
+        case 2:
+            m = nlerp_f<MatT_1,MatT_2,2>()(m1,m2,t);
+            break;
+        default:
+            throw std::invalid_argument(
+                "matrix nlerp() expects sizes 3x3 or 2x2");
+            break;
+    }
+    return m;
+}
+
+} // namespace detail
+
+//////////////////////////////////////////////////////////////////////////////
+// Construction of 'intermediate' quaternions and matrices for use with squad
+//////////////////////////////////////////////////////////////////////////////
+
+/**
+ * NOTE: Computation of intermediate rotation matrices for matrix 'squad'
+ * doesn't seem to be working correctly. I'm not sure what the problem is
+ * (it might have to do with q and -q representing the same rotation), but
+ * in any case, I don't have time to get it sorted at the moment.
+ *
+ * In the meantime, I've just hacked in static assertions that will
+ * restrict squad usage to quats. For anyone reading these comments, don't
+ * worry: the quaternion verison of squad works just fine. However, you'll
+ * just have to live without matrix squad for the time being (which is
+ * probably just as well, given that matrix interpolation isn't terribly
+ * efficient).
+ */
+
+#if 0
+template< class T1, class T2, class T3 >
+typename detail::TypePromote3<
+    T1,T2,T3,typename et::ExprTraits<T1>::result_tag
+>::temporary_type
+squad_intermediate(
+    const T1& t1,
+    const T2& t2,
+    const T3& t3,
+    typename detail::TypePromote3<
+        T1, T2, T3, typename et::ExprTraits<T1>::result_tag
+    >::value_type tolerance =
+    epsilon <
+        typename detail::TypePromote3<
+            T1, T2, T3, typename et::ExprTraits<T1>::result_tag
+        >::value_type
+    >::placeholder())
+{
+    // HACK: See note above...
+    detail::CheckQuat(t1);
+    detail::CheckQuat(t2);
+    detail::CheckQuat(t3);
+
+    typedef et::ExprTraits<T1> traits_1;
+    typedef typename traits_1::result_tag result_type_1;
+
+    typedef typename detail::TypePromote3<T1,T2,T3,result_type_1>::temporary_type
+        temporary_type;
+    typedef et::ExprTraits<temporary_type> result_traits;
+    typedef typename result_traits::size_tag size_tag;
+
+    temporary_type result;
+    detail::InterpResize(result, t1, size_tag());
+
+    result = detail::squad_intermediate(
+        t1,t2,t3,tolerance,result_type_1(),size_tag());
+    return result;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Spherical quadrangle interpolation of two quaternions or matrices
+//////////////////////////////////////////////////////////////////////////////
+
+/**
+ * NOTE: The squad() impelementation is unfinished. I'm leaving the code
+ * here (but preprocessor'ed out) for future reference.
+ *
+ * Currently, it seems that:
+ *
+ * 1. Computation of intermediate matrices is incorrect.
+ * 2. The interpolated orientation sometimes 'jumps' while between nodes.
+ *
+ * I've observed that removing the 'shortest path' negation from the slerp
+ * function eliminates the second problem. Also, in another implementation
+ * of squad that I've seen, q1 and q2 are interpolated over the shortest
+ * path, while the helper quaternions are not. I've never seen this
+ * mentioned as a requirement of squad, but maybe they know something I
+ * don't.
+ *
+ * For anyone who happens to read these comments, all of the other
+ * interpolation functions (lerp, nlerp, slerp, etc.) should work fine -
+ * it's just squad() that's on hold.
+ */
+
+template< class T1, class T2, class T3, class T4, typename Real >
+typename detail::TypePromote4<
+    T1,T2,T3,T4,typename et::ExprTraits<T1>::result_tag
+>::temporary_type
+squad(
+    const T1& t1,
+    const T2& t1_intermediate,
+    const T3& t2_intermediate,
+    const T4& t2,
+    Real t,
+    Real tolerance = epsilon<Real>::placeholder())
+{
+    // HACK: See note above...
+    detail::CheckQuat(t1);
+    detail::CheckQuat(t1_intermediate);
+    detail::CheckQuat(t2_intermediate);
+    detail::CheckQuat(t2);
+
+    typedef et::ExprTraits<T1> traits_1;
+    typedef typename traits_1::result_tag result_type_1;
+
+    typedef typename detail::TypePromote4<
+        T1,T2,T3,T4,result_type_1>::temporary_type temporary_type;
+    typedef typename temporary_type::value_type value_type;
+    typedef et::ExprTraits<temporary_type> result_traits;
+    typedef typename result_traits::size_tag size_tag;
+
+    temporary_type result;
+    detail::InterpResize(result, t1, size_tag());
+    
+    result = slerp(
+        slerp(t1, t2, t, tolerance),
+        slerp(t1_intermediate, t2_intermediate, t, tolerance),
+        value_type(2) * t * (value_type(1) - t),
+        tolerance
+    );
+
+    return result;
+}
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Spherical linear interpolation of two vectors, quaternions or matrices
+//////////////////////////////////////////////////////////////////////////////
+
+template< class T1, class T2, typename Real >
+typename detail::TypePromote<
+    T1,T2,typename et::ExprTraits<T1>::result_tag
+>::temporary_type
+slerp(
+    const T1& t1,
+    const T2& t2,
+    Real t,
+    Real tolerance = epsilon<Real>::placeholder())
+{
+    typedef et::ExprTraits<T1> traits_1;
+    typedef typename traits_1::result_tag result_type_1;
+
+    typedef typename detail::TypePromote<T1,T2,result_type_1>::temporary_type
+        temporary_type;
+    typedef et::ExprTraits<temporary_type> result_traits;
+    typedef typename result_traits::size_tag size_tag;
+
+    temporary_type result;
+    detail::InterpResize(result, t1, size_tag());
+
+    result = detail::slerp(t1,t2,t,tolerance,result_type_1(),size_tag());
+    return result;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Normalized linear interpolation of two vectors, quaternions or matrices
+//////////////////////////////////////////////////////////////////////////////
+
+template< class T1, class T2, typename Real >
+typename detail::TypePromote<
+    T1,T2,typename et::ExprTraits<T1>::result_tag
+>::temporary_type
+nlerp(const T1& t1, const T2& t2, Real t)
+{
+    typedef et::ExprTraits<T1> traits_1;
+    typedef typename traits_1::result_tag result_type_1;
+
+    typedef typename detail::TypePromote<T1,T2,result_type_1>::temporary_type
+        temporary_type;
+    typedef et::ExprTraits<temporary_type> result_traits;
+    typedef typename result_traits::size_tag size_tag;
+
+    temporary_type result;
+    detail::InterpResize(result, t1, size_tag());
+
+    result = detail::nlerp(t1,t2,t,result_type_1(),size_tag());
+    return result;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Linear interpolation of two values of any qualified type
+//////////////////////////////////////////////////////////////////////////////
+
+/** Linear interpolation of 2 values.
+ *
+ * @note The data points are assumed to be sampled at u = 0 and u = 1, so
+ * for interpolation u must lie between 0 and 1.
+ */
+template< class T1, class T2, typename Scalar >
+typename detail::TypePromote<
+    T1,T2,typename et::ExprTraits<T1>::result_tag
+>::temporary_type
+lerp(const T1& val0, const T2& val1, Scalar u)
+{
+    typedef
+        typename detail::TypePromote<
+            T1,T2,typename et::ExprTraits<T1>::result_tag
+        >::temporary_type temporary_type;
+
+    typedef et::ExprTraits<temporary_type> result_traits;
+    typedef typename result_traits::size_tag size_tag;
+
+    temporary_type result;
+    detail::InterpResize(result, val1, size_tag());
+    
+    result = (Scalar(1) - u) * val0 + u * val1;
+    return result;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Bilinear interpolation of four values of any qualified type
+//////////////////////////////////////////////////////////////////////////////
+
+template < class T1, class T2, class T3, class T4, typename Scalar >
+typename detail::TypePromote<
+    typename detail::TypePromote<
+        T1,T2,typename et::ExprTraits<T1>::result_tag
+    >::temporary_type,
+    typename detail::TypePromote<
+        T3,T4,typename et::ExprTraits<T3>::result_tag
+    >::temporary_type,
+    typename et::ExprTraits<T1>::result_tag
+>::temporary_type
+bilerp(const T1& val00, const T2& val10,
+       const T3& val01, const T4& val11,
+       Scalar u, Scalar v)
+{
+    typedef
+        typename detail::TypePromote<
+            typename detail::TypePromote<
+                T1,T2,typename et::ExprTraits<T1>::result_tag
+            >::temporary_type,
+            typename detail::TypePromote<
+                T3,T4,typename et::ExprTraits<T1>::result_tag
+            >::temporary_type,
+            typename et::ExprTraits<T1>::result_tag
+        >::temporary_type temporary_type;
+
+    typedef et::ExprTraits<temporary_type> result_traits;
+    typedef typename result_traits::size_tag size_tag;
+
+    temporary_type result;
+    detail::InterpResize(result, val00, size_tag());
+
+    Scalar uv = u * v;
+    result = (
+        (Scalar(1.0) - u - v + uv) * val00 +
+                          (u - uv) * val10 +
+                          (v - uv) * val01 +
+                                uv * val11
+    );
+    return result;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Trilinear interpolation of eight values of any qualified type
+//////////////////////////////////////////////////////////////////////////////
+
+/** Trilinear interpolation of 8 values.
+ *
+ * @note The data values are assumed to be sampled at the corners of a unit
+ * cube, so for interpolation, u, v, and w must lie between 0 and 1.
+ */
+template < class T1, class T2, class T3, class T4,
+           class T5, class T6, class T7, class T8,
+           typename Scalar >
+typename detail::TypePromote<
+    typename detail::TypePromote<
+        typename detail::TypePromote<
+            T1,T2,typename et::ExprTraits<T1>::result_tag
+        >::temporary_type,
+        typename detail::TypePromote<
+            T3,T4,typename et::ExprTraits<T3>::result_tag
+        >::temporary_type,
+        typename et::ExprTraits<T1>::result_tag
+    >::temporary_type,
+    typename detail::TypePromote<
+        typename detail::TypePromote<
+            T5,T6,typename et::ExprTraits<T5>::result_tag
+        >::temporary_type,
+        typename detail::TypePromote<
+            T7,T8,typename et::ExprTraits<T7>::result_tag
+        >::temporary_type,
+        typename et::ExprTraits<T1>::result_tag
+    >::temporary_type,
+    typename et::ExprTraits<T1>::result_tag
+>::temporary_type
+trilerp(const T1& val000, const T2& val100,
+        const T3& val010, const T4& val110,
+        const T5& val001, const T6& val101,
+        const T7& val011, const T8& val111,
+        Scalar u, Scalar v, Scalar w)
+{
+    typedef
+        typename detail::TypePromote<
+            typename detail::TypePromote<
+                typename detail::TypePromote<
+                    T1,T2,typename et::ExprTraits<T1>::result_tag
+                >::temporary_type,
+                typename detail::TypePromote<
+                    T3,T4,typename et::ExprTraits<T1>::result_tag
+                >::temporary_type,
+                typename et::ExprTraits<T1>::result_tag
+            >::temporary_type,
+            typename detail::TypePromote<
+                typename detail::TypePromote<
+                    T5,T6,typename et::ExprTraits<T1>::result_tag
+                >::temporary_type,
+                typename detail::TypePromote<
+                    T7,T8,typename et::ExprTraits<T1>::result_tag
+                >::temporary_type,
+                typename et::ExprTraits<T1>::result_tag
+            >::temporary_type,
+            typename et::ExprTraits<T1>::result_tag
+        >::temporary_type temporary_type;
+
+    typedef et::ExprTraits<temporary_type> result_traits;
+    typedef typename result_traits::size_tag size_tag;
+
+    temporary_type result;
+    detail::InterpResize(result, val000, size_tag());
+
+    Scalar uv = u * v;
+    Scalar vw = v * w;
+    Scalar wu = w * u;
+    Scalar uvw = uv * w;
+    
+    result = (
+        (Scalar(1.0) - u - v - w + uv + vw + wu - uvw) * val000 +
+                                   (u - uv - wu + uvw) * val100 +
+                                   (v - uv - vw + uvw) * val010 +
+                                            (uv - uvw) * val110 +
+                                   (w - vw - wu + uvw) * val001 +
+                                            (wu - uvw) * val101 +
+                                            (vw - uvw) * val011 +
+                                                   uvw * val111
+    );
+    return result;
+}
+
+} // namespace cml
+
+#endif
diff --git a/src/cml/mathlib/mathlib.h b/src/cml/mathlib/mathlib.h
new file mode 100644 (file)
index 0000000..d15c5f2
--- /dev/null
@@ -0,0 +1,33 @@
+/* -*- 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 mathlib_h
+#define mathlib_h
+
+#include <cml/mathlib/typedef.h>
+#include <cml/mathlib/epsilon.h>
+#include <cml/mathlib/vector_angle.h>
+#include <cml/mathlib/vector_ortho.h>
+#include <cml/mathlib/vector_transform.h>
+#include <cml/mathlib/matrix_ortho.h>
+#include <cml/mathlib/matrix_rotation.h>
+#include <cml/mathlib/matrix_transform.h>
+#include <cml/mathlib/matrix_projection.h>
+#include <cml/mathlib/quaternion_basis.h>
+#include <cml/mathlib/quaternion_rotation.h>
+#include <cml/mathlib/coord_conversion.h>
+#include <cml/mathlib/interpolation.h>
+#include <cml/mathlib/frustum.h>
+#include <cml/mathlib/projection.h>
+#include <cml/mathlib/picking.h>
+
+#endif
diff --git a/src/cml/mathlib/matrix_basis.h b/src/cml/mathlib/matrix_basis.h
new file mode 100644 (file)
index 0000000..7732270
--- /dev/null
@@ -0,0 +1,364 @@
+/* -*- 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_basis_h
+#define matrix_basis_h
+
+#include <cml/mathlib/checking.h>
+
+/* This file contains functions for setting and retrieving the basis vectors
+ * or transposed basis vectors of a matrix representing a 3D or 2D transform,
+ * either by index (0,1,2) or name (x,y,z).
+ *
+ * In addition to being a convenience for the user, the functions are also
+ * in support of other matrix functions which are best implemented in vector
+ * form (such as orthogonalization and construction of orthonormal bases).
+ *
+ * Note that matrix expression arguments are allowed to have dimensions larger
+ * than the minimum requirement. For example, matrix_get_basis_vector() can be
+ * called on any NxM matrix with N,M >= 3.
+ *
+ * As with other matrix functions, the following template argument notation is
+ * used for conciseness:
+ *
+ * E = vector or matrix element type
+ * A = vector or matrix array storage type
+ * B = matrix basis orientation type
+ * L = matrix layout type
+ */
+
+namespace cml {
+
+//////////////////////////////////////////////////////////////////////////////
+// Functions for setting the basis vectors of a 3D or 2D transform matrix
+//////////////////////////////////////////////////////////////////////////////
+
+/** Set the i'th basis vector of a 3D transform */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_set_basis_vector(matrix<E,A,B,L>& m, size_t i, const VecT& v)
+{
+    /* Checking */
+    detail::CheckMatLinear3D(m);
+    detail::CheckVec3(v);
+    detail::CheckIndex3(i);
+
+    m.set_basis_element(i,0,v[0]);
+    m.set_basis_element(i,1,v[1]);
+    m.set_basis_element(i,2,v[2]);
+}
+
+/** Set the i'th transposed basis vector of a 3D transform */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_set_transposed_basis_vector(matrix<E,A,B,L>& m,size_t i,const VecT& v)
+{
+    /* Checking */
+    detail::CheckMatLinear3D(m);
+    detail::CheckVec3(v);
+    detail::CheckIndex3(i);
+
+    m.set_basis_element(0,i,v[0]);
+    m.set_basis_element(1,i,v[1]);
+    m.set_basis_element(2,i,v[2]);
+}
+
+/** Set the i'th basis vector of a 2D transform */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_set_basis_vector_2D(matrix<E,A,B,L>& m, size_t i, const VecT& v)
+{
+    /* Checking */
+    detail::CheckMatLinear2D(m);
+    detail::CheckVec2(v);
+    detail::CheckIndex2(i);
+
+    m.set_basis_element(i,0,v[0]);
+    m.set_basis_element(i,1,v[1]);
+}
+
+/** Set the i'th transposed basis vector of a 2D transform */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_set_transposed_basis_vector_2D(
+    matrix<E,A,B,L>& m, size_t i, const VecT& v)
+{
+    /* Checking */
+    detail::CheckMatLinear2D(m);
+    detail::CheckVec2(v);
+    detail::CheckIndex2(i);
+
+    m.set_basis_element(0,i,v[0]);
+    m.set_basis_element(1,i,v[1]);
+}
+
+/** Set the x basis vector of a 3D transform */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_set_x_basis_vector(matrix<E,A,B,L>& m, const VecT& x) {
+    matrix_set_basis_vector(m,0,x);
+}
+
+/** Set the y basis vector of a 3D transform */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_set_y_basis_vector(matrix<E,A,B,L>& m, const VecT& y) {
+    matrix_set_basis_vector(m,1,y);
+}
+
+/** Set the z basis vector of a 3D transform */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_set_z_basis_vector(matrix<E,A,B,L>& m, const VecT& z) {
+    matrix_set_basis_vector(m,2,z);
+}
+
+/** Set the transposed x basis vector of a 3D transform */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_set_transposed_x_basis_vector(matrix<E,A,B,L>& m, const VecT& x) {
+    matrix_set_transposed_basis_vector(m,0,x);
+}
+
+/** Set the transposed y basis vector of a 3D transform */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_set_transposed_y_basis_vector(matrix<E,A,B,L>& m, const VecT& y) {
+    matrix_set_transposed_basis_vector(m,1,y);
+}
+
+/** Set the transposed z basis vector of a 3D transform */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_set_transposed_z_basis_vector(matrix<E,A,B,L>& m, const VecT& z) {
+    matrix_set_transposed_basis_vector(m,2,z);
+}
+
+/** Set the x basis vector of a 2D transform */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_set_x_basis_vector_2D(matrix<E,A,B,L>& m, const VecT& x) {
+    matrix_set_basis_vector_2D(m,0,x);
+}
+
+/** Set the y basis vector of a 2D transform */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_set_y_basis_vector_2D(matrix<E,A,B,L>& m, const VecT& y) {
+    matrix_set_basis_vector_2D(m,1,y);
+}
+
+/** Set the transposed x basis vector of a 2D transform */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_set_transposed_x_basis_vector_2D(matrix<E,A,B,L>& m,const VecT& x) {
+    matrix_set_transposed_basis_vector_2D(m,0,x);
+}
+
+/** Set the transposed y basis vector of a 2D transform */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_set_transposed_y_basis_vector_2D(matrix<E,A,B,L>& m,const VecT& y) {
+    matrix_set_transposed_basis_vector_2D(m,1,y);
+}
+
+/** Set the basis vectors of a 3D transform */
+template < typename E, class A, class B, class L,
+    class VecT_1, class VecT_2, class VecT_3 > void
+matrix_set_basis_vectors(
+    matrix<E,A,B,L>& m, const VecT_1& x, const VecT_2& y, const VecT_3& z)
+{
+    matrix_set_x_basis_vector(m,x);
+    matrix_set_y_basis_vector(m,y);
+    matrix_set_z_basis_vector(m,z);
+}
+
+/** Set the transposed basis vectors of a 3D transform */
+template < typename E, class A, class B, class L,
+    class VecT_1, class VecT_2, class VecT_3 > void
+matrix_set_transposed_basis_vectors(
+    matrix<E,A,B,L>& m, const VecT_1& x, const VecT_2& y, const VecT_3& z)
+{
+    matrix_set_transposed_x_basis_vector(m,x);
+    matrix_set_transposed_y_basis_vector(m,y);
+    matrix_set_transposed_z_basis_vector(m,z);
+}
+
+/** Set the basis vectors of a 2D transform */
+template < typename E,class A,class B,class L,class VecT_1,class VecT_2 > void
+matrix_set_basis_vectors_2D(
+    matrix<E,A,B,L>& m, const VecT_1& x, const VecT_2& y)
+{
+    matrix_set_x_basis_vector_2D(m,x);
+    matrix_set_y_basis_vector_2D(m,y);
+}
+
+/** Set the transposed basis vectors of a 2D transform */
+template < typename E,class A,class B,class L,class VecT_1,class VecT_2 > void
+matrix_set_transposed_basis_vectors_2D(
+    matrix<E,A,B,L>& m, const VecT_1& x, const VecT_2& y)
+{
+    matrix_set_transposed_x_basis_vector_2D(m,x);
+    matrix_set_transposed_y_basis_vector_2D(m,y);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Functions for getting the basis vectors of a 3D or 2D transform matrix
+//////////////////////////////////////////////////////////////////////////////
+
+#define TEMP_VEC3 vector< typename MatT::value_type, fixed<3> >
+#define TEMP_VEC2 vector< typename MatT::value_type, fixed<2> >
+
+/** Get the i'th basis vector of a 3D transform */
+template < class MatT > TEMP_VEC3
+matrix_get_basis_vector(const MatT& m, size_t i)
+{
+    typedef TEMP_VEC3 vector_type;
+
+    /* Checking */
+    detail::CheckMatLinear3D(m);
+    detail::CheckIndex3(i);
+
+    return vector_type(
+        m.basis_element(i,0), m.basis_element(i,1), m.basis_element(i,2));
+}
+
+/** Get the i'th transposed basis vector of a 3D transform */
+template < class MatT > TEMP_VEC3
+matrix_get_transposed_basis_vector(const MatT& m, size_t i)
+{
+    typedef typename MatT::value_type value_type;
+    typedef vector< value_type, fixed<3> > vector_type;
+    
+    /* Checking */
+    detail::CheckMatLinear3D(m);
+    detail::CheckIndex3(i);
+
+    return vector_type(
+        m.basis_element(0,i), m.basis_element(1,i), m.basis_element(2,i));
+}
+
+/** Get the i'th basis vector of a 2D transform */
+template < class MatT > TEMP_VEC2
+matrix_get_basis_vector_2D(const MatT& m, size_t i)
+{
+    typedef TEMP_VEC2 vector_type;
+
+    /* Checking */
+    detail::CheckMatLinear2D(m);
+    detail::CheckIndex2(i);
+
+    return vector_type(m.basis_element(i,0), m.basis_element(i,1));
+}
+
+/** Get the i'th transposed basis vector of a 2D transform */
+template < class MatT > TEMP_VEC2
+matrix_get_transposed_basis_vector_2D(const MatT& m, size_t i)
+{
+    typedef TEMP_VEC2 vector_type;
+
+    /* Checking */
+    detail::CheckMatLinear2D(m);
+    detail::CheckIndex2(i);
+
+    return vector_type(m.basis_element(0,i), m.basis_element(1,i));
+}
+
+/** Get the x basis vector of a 3D transform */
+template < class MatT > TEMP_VEC3
+matrix_get_x_basis_vector(const MatT& m) {
+    return matrix_get_basis_vector(m,0);
+}
+
+/** Get the y basis vector of a 3D transform */
+template < class MatT > TEMP_VEC3
+matrix_get_y_basis_vector(const MatT& m) {
+    return matrix_get_basis_vector(m,1);
+}
+
+/** Get the z basis vector of a 3D transform */
+template < class MatT > TEMP_VEC3
+matrix_get_z_basis_vector(const MatT& m) {
+    return matrix_get_basis_vector(m,2);
+}
+
+/** Get the transposed x basis vector of a 3D transform */
+template < class MatT > TEMP_VEC3
+matrix_get_transposed_x_basis_vector(const MatT& m) {
+    return matrix_get_transposed_basis_vector(m,0);
+}
+
+/** Get the transposed y basis vector of a 3D transform */
+template < class MatT > TEMP_VEC3
+matrix_get_transposed_y_basis_vector(const MatT& m) {
+    return matrix_get_transposed_basis_vector(m,1);
+}
+
+/** Get the transposed z basis vector of a 3D transform */
+template < class MatT > TEMP_VEC3
+matrix_get_transposed_z_basis_vector(const MatT& m) {
+    return matrix_get_transposed_basis_vector(m,2);
+}
+
+/** Get the x basis vector of a 2D transform */
+template < class MatT > TEMP_VEC2
+matrix_get_x_basis_vector_2D(const MatT& m) {
+    return matrix_get_basis_vector_2D(m,0);
+}
+
+/** Get the y basis vector of a 2D transform */
+template < class MatT > TEMP_VEC2
+matrix_get_y_basis_vector_2D(const MatT& m) {
+    return matrix_get_basis_vector_2D(m,1);
+}
+
+/** Get the transposed x basis vector of a 2D transform */
+template < class MatT > TEMP_VEC2
+matrix_get_transposed_x_basis_vector_2D(const MatT& m) {
+    return matrix_get_transposed_basis_vector_2D(m,0);
+}
+
+/** Get the transposed y basis vector of a 2D transform */
+template < class MatT > TEMP_VEC2
+matrix_get_transposed_y_basis_vector_2D(const MatT& m) {
+    return matrix_get_transposed_basis_vector_2D(m,1);
+}
+
+/** Get the basis vectors of a 3D transform */
+template < class MatT, class E, class A > void
+matrix_get_basis_vectors(
+    const MatT& m, vector<E,A>& x, vector<E,A>& y, vector<E,A>& z)
+{
+    x = matrix_get_x_basis_vector(m);
+    y = matrix_get_y_basis_vector(m);
+    z = matrix_get_z_basis_vector(m);
+}
+
+/** Get the transposed basis vectors of a 3D transform */
+template < class MatT, typename E, class A > void
+matrix_get_transposed_basis_vectors(
+    const MatT& m, vector<E,A>& x, vector<E,A>& y, vector<E,A>& z)
+{
+    x = matrix_get_transposed_x_basis_vector(m);
+    y = matrix_get_transposed_y_basis_vector(m);
+    z = matrix_get_transposed_z_basis_vector(m);
+}
+
+/** Get the basis vectors of a 2D transform */
+template < class MatT, typename E, class A > void
+matrix_get_basis_vectors_2D(const MatT& m,vector<E,A>& x,vector<E,A>& y)
+{
+    x = matrix_get_x_basis_vector_2D(m);
+    y = matrix_get_y_basis_vector_2D(m);
+}
+
+/** Get the transposed basis vectors of a 2D transform */
+template < class MatT, typename E, class A > void
+matrix_get_transposed_basis_vectors_2D(
+    const MatT& m, vector<E,A>& x, vector<E,A>& y)
+{
+    x = matrix_get_transposed_x_basis_vector_2D(m);
+    y = matrix_get_transposed_y_basis_vector_2D(m);
+}
+
+#undef TEMP_VEC3
+#undef TEMP_VEC2
+
+} // namespace cml
+
+#endif
diff --git a/src/cml/mathlib/matrix_concat.h b/src/cml/mathlib/matrix_concat.h
new file mode 100644 (file)
index 0000000..8e789aa
--- /dev/null
@@ -0,0 +1,62 @@
+/* -*- 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_concat_h
+#define matrix_concat_h
+
+#include <cml/matrix/matrix_expr.h>
+
+/* This will all most likely be abstracted away in a future version of the
+ * CML. For now, this file provides support for functions that need to
+ * concatenate transformation matrices in a basis-independent manner.
+ *
+ * @todo: The 2x2 and 3x3 versions of these functions are currently in
+ * matrix_rotation.h. They should be moved here.
+ */
+
+namespace cml {
+namespace detail {
+
+/** A fixed-size temporary 4x4 matrix */
+#define MAT_TEMP_4X4 matrix<         \
+    typename et::ScalarPromote<      \
+        typename MatT_1::value_type, \
+        typename MatT_2::value_type  \
+    >::type,                         \
+    fixed<4,4>,                      \
+    typename MatT_1::basis_orient,   \
+    typename MatT_1::layout          \
+>
+
+template < class MatT_1, class MatT_2 > MAT_TEMP_4X4
+matrix_concat_transforms_4x4(const MatT_1& m1, const MatT_2& m2, row_basis) {
+    return m1*m2;
+}
+
+/** Concatenate two 3D col-basis rotation matrices in the order m1->m2 */
+template < class MatT_1, class MatT_2 > MAT_TEMP_4X4
+matrix_concat_transforms_4x4(const MatT_1& m1, const MatT_2& m2, col_basis) {
+    return m2*m1;
+}
+
+/** Concatenate two 3D rotation matrices in the order m1->m2 */
+template < class MatT_1, class MatT_2 > MAT_TEMP_4X4
+matrix_concat_transforms_4x4(const MatT_1& m1, const MatT_2& m2) {
+    return matrix_concat_transforms_4x4(m1,m2,typename MatT_1::basis_orient());
+}
+
+#undef MAT_TEMP_4x4
+
+} // namespace detail
+} // namespace cml
+
+#endif
diff --git a/src/cml/mathlib/matrix_misc.h b/src/cml/mathlib/matrix_misc.h
new file mode 100644 (file)
index 0000000..c36a399
--- /dev/null
@@ -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
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
@@ -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
diff --git a/src/cml/mathlib/matrix_projection.h b/src/cml/mathlib/matrix_projection.h
new file mode 100644 (file)
index 0000000..042b7b5
--- /dev/null
@@ -0,0 +1,346 @@
+/* -*- 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_projection_h
+#define matrix_projection_h
+
+#include <cml/mathlib/checking.h>
+#include <cml/mathlib/helper.h>
+
+/* Functions for building matrix transforms other than rotations
+ * (matrix_rotation.h) and viewing projections (matrix_projection.h).
+ *
+ * @todo: Clean up comments and documentation throughout.
+ */
+
+// NOTE: Changed 'near' and 'far' to 'n' and 'f' throughout to work around
+// windows.h 'near' and 'far' macros.
+
+namespace cml {
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D perspective projection from frustum
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a matrix representing a perspective projection, specified by frustum
+ *  bounds in l,r,b,t,n,f form, and with the given handedness and z clipping
+ *  range
+ */
+template < typename E, class A, class B, class L > void
+matrix_perspective(matrix<E,A,B,L>& m, E left, E right, E bottom, E top,
+    E n, E f, Handedness handedness,
+    ZClip z_clip)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckMatHomogeneous3D(m);
+
+    identity_transform(m);
+    
+    value_type inv_width = value_type(1) / (right - left);
+    value_type inv_height = value_type(1) / (top - bottom);
+    value_type inv_depth = value_type(1) / (f - n);
+    value_type near2 = value_type(2) * n;
+    value_type s = handedness == left_handed ? 1 : -1;
+
+    if (z_clip == z_clip_neg_one) {
+        m.set_basis_element(2,2,s * (f + n) * inv_depth);
+        m.set_basis_element(3,2,value_type(-2) * f * n * inv_depth);
+    } else { // z_clip == z_clip_zero
+        m.set_basis_element(2,2,s * f * inv_depth);
+        m.set_basis_element(3,2,-s * n * m.basis_element(2,2));
+    }
+    
+    m.set_basis_element(0,0,near2 * inv_width               );
+    m.set_basis_element(1,1,near2 * inv_height              );
+    m.set_basis_element(2,0,-s * (right + left) * inv_width );
+    m.set_basis_element(2,1,-s * (top + bottom) * inv_height);
+    m.set_basis_element(2,3,s                               );
+    m.set_basis_element(3,3,value_type(0)                   );
+}
+
+/** Build a matrix representing a perspective projection, specified by frustum
+ *  bounds in w,h,n,f form, and with the given handedness and z clipping
+ *  range
+ */
+template < typename E, class A, class B, class L > void
+matrix_perspective(matrix<E,A,B,L>& m, E width, E height, E n, E f,
+    Handedness handedness, ZClip z_clip)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+    
+    value_type half_width = width * value_type(.5);
+    value_type half_height = height * value_type(.5);
+    matrix_perspective(m, -half_width, half_width,
+        -half_height, half_height, n, f, handedness, z_clip);
+}
+
+/** Build a left-handedness frustum perspective matrix */
+template < typename E, class A, class B, class L > void
+matrix_perspective_LH(matrix<E,A,B,L>& m, E left, E right, E bottom,
+    E top, E n, E f, ZClip z_clip)
+{
+    matrix_perspective(m, left, right, bottom, top, n, f,
+        left_handed, z_clip);
+}
+
+/** Build a right-handedness frustum perspective matrix */
+template < typename E, class A, class B, class L > void
+matrix_perspective_RH(matrix<E,A,B,L>& m, E left, E right, E bottom,
+    E top, E n, E f, ZClip z_clip)
+{
+    matrix_perspective(m, left, right, bottom, top, n, f,
+        right_handed, z_clip);
+}
+
+/** Build a left-handedness frustum perspective matrix */
+template < typename E, class A, class B, class L > void
+matrix_perspective_LH(matrix<E,A,B,L>& m, E width, E height, E n,
+    E f, ZClip z_clip)
+{
+    matrix_perspective(m, width, height, n, f, left_handed, z_clip);
+}
+
+/** Build a right-handedness frustum perspective matrix */
+template < typename E, class A, class B, class L > void
+matrix_perspective_RH(matrix<E,A,B,L>& m, E width, E height, E n,
+    E f, ZClip z_clip)
+{
+    matrix_perspective(m, width, height, n, f, right_handed, z_clip);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D perspective projection from horizontal field of view
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a perspective matrix */
+template < typename E, class A, class B, class L > void
+matrix_perspective_xfov(matrix<E,A,B,L>& m, E xfov, E aspect, E n,
+    E f, Handedness handedness, ZClip z_clip)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+    
+    value_type width = value_type(2) * std::tan(xfov * value_type(.5)) * n;
+    matrix_perspective(m, width, width / aspect, n, f,
+        handedness, z_clip);
+}
+
+/** Build a left-handedness perspective matrix */
+template < typename E, class A, class B, class L > void
+matrix_perspective_xfov_LH(matrix<E,A,B,L>& m, E xfov, E aspect, E n,
+    E f, ZClip z_clip)
+{
+    matrix_perspective_xfov(m,xfov,aspect,n,f,left_handed,z_clip);
+}
+
+/** Build a right-handedness perspective matrix */
+template < typename E, class A, class B, class L > void
+matrix_perspective_xfov_RH(matrix<E,A,B,L>& m, E xfov, E aspect, E n,
+    E f, ZClip z_clip)
+{
+    matrix_perspective_xfov(m,xfov,aspect,n,f,right_handed,z_clip);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D perspective projection from vertical field of view
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a perspective matrix */
+template < typename E, class A, class B, class L > void
+matrix_perspective_yfov(matrix<E,A,B,L>& m, E yfov, E aspect, E n,
+    E f, Handedness handedness, ZClip z_clip)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+    
+    value_type height = value_type(2) * std::tan(yfov * value_type(.5)) * n;
+    matrix_perspective(m, height * aspect, height, n, f,
+        handedness, z_clip);
+}
+
+/** Build a left-handedness perspective matrix */
+template < typename E, class A, class B, class L > void
+matrix_perspective_yfov_LH(matrix<E,A,B,L>& m, E yfov, E aspect, E n,
+    E f, ZClip z_clip)
+{
+    matrix_perspective_yfov(m,yfov,aspect,n,f,left_handed,z_clip);
+}
+
+/** Build a right-handedness perspective matrix */
+template < typename E, class A, class B, class L > void
+matrix_perspective_yfov_RH(matrix<E,A,B,L>& m, E yfov, E aspect, E n,
+    E f, ZClip z_clip)
+{
+    matrix_perspective_yfov(m,yfov,aspect,n,f,right_handed,z_clip);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D orthographic projection from frustum
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a matrix representing an orthographic projection, specified by
+ *  frustum bounds in l,r,b,t,n,f form, and with the given handedness and z
+ *  clipping range
+ */
+
+template < typename E, class A, class B, class L > void
+matrix_orthographic(matrix<E,A,B,L>& m, E left, E right, E bottom, E top,
+    E n, E f, Handedness handedness,
+    ZClip z_clip)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckMatHomogeneous3D(m);
+
+    identity_transform(m);
+    
+    value_type inv_width = value_type(1) / (right - left);
+    value_type inv_height = value_type(1) / (top - bottom);
+    value_type inv_depth = value_type(1) / (f - n);
+    value_type s = handedness == left_handed ? 1 : -1;
+
+    if (z_clip == z_clip_neg_one) {
+        m.set_basis_element(2,2,s * value_type(2) * inv_depth);
+        m.set_basis_element(3,2,-(f + n) * inv_depth);
+    } else { // z_clip.z_clip() == 0
+        m.set_basis_element(2,2,s * inv_depth);
+        m.set_basis_element(3,2,-n * inv_depth);
+    }
+    
+    m.set_basis_element(0,0,value_type(2) * inv_width   );
+    m.set_basis_element(1,1,value_type(2) * inv_height  );
+    m.set_basis_element(3,0,-(right + left) * inv_width );
+    m.set_basis_element(3,1,-(top + bottom) * inv_height);
+}
+
+/** Build an orthographic projection matrix */
+template < typename E, class A, class B, class L > void
+matrix_orthographic(matrix<E,A,B,L>& m, E width, E height, E n, E f,
+    Handedness handedness, ZClip z_clip)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+    
+    value_type half_width = width * value_type(.5);
+    value_type half_height = height * value_type(.5);
+    matrix_orthographic(m, -half_width, half_width,
+        -half_height, half_height, n, f, handedness, z_clip);
+}
+
+/** Build a left-handedness orthographic projection matrix */
+template < typename E, class A, class B, class L > void
+matrix_orthographic_LH(matrix<E,A,B,L>& m, E left, E right, E bottom,
+    E top, E n, E f, ZClip z_clip)
+{
+    matrix_orthographic(m, left, right, bottom, top, n, f,
+        left_handed, z_clip);
+}
+
+/** Build a right-handedness orthographic projection matrix */
+template < typename E, class A, class B, class L > void
+matrix_orthographic_RH(matrix<E,A,B,L>& m, E left, E right, E bottom,
+    E top, E n, E f, ZClip z_clip)
+{
+    matrix_orthographic(m, left, right, bottom, top, n, f,
+        right_handed, z_clip);
+}
+
+/** Build a left-handedness orthographic projection matrix */
+template < typename E, class A, class B, class L > void
+matrix_orthographic_LH(matrix<E,A,B,L>& m, E width, E height, E n,
+    E f, ZClip z_clip)
+{
+    matrix_orthographic(m, width, height, n, f, left_handed,
+        z_clip);
+}
+
+/** Build a right-handedness orthographic projection matrix */
+template < typename E, class A, class B, class L > void
+matrix_orthographic_RH(matrix<E,A,B,L>& m, E width, E height, E n,
+    E f, ZClip z_clip)
+{
+    matrix_orthographic(m, width, height, n, f, right_handed,
+        z_clip);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D viewport
+//////////////////////////////////////////////////////////////////////////////
+
+/* Build a viewport matrix
+ *
+ * Note: A viewport matrix is in a sense the opposite of an orthographics
+ * projection matrix, and can be build by constructing and inverting the
+ * latter.
+ *
+ * @todo: Need to look into D3D viewport conventions and see if this needs to
+ * be adapted accordingly.
+ */
+
+template < typename E, class A, class B, class L > void
+matrix_viewport(matrix<E,A,B,L>& m, E left, E right, E bottom,
+    E top, ZClip z_clip, E n = E(0), E f = E(1))
+{
+    matrix_orthographic_LH(m, left, right, bottom, top, n, f, z_clip);
+    /* @todo: invert(m), when available */
+    m = inverse(m);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D picking volume
+//////////////////////////////////////////////////////////////////////////////
+
+/* Build a pick volume matrix
+ *
+ * When post-concatenated with a projection matrix, the pick matrix modifies
+ * the view volume to create a 'picking volume'. This volume corresponds to
+ * a screen rectangle centered at (pick_x, pick_y) and with dimensions
+ * pick_widthXpick_height.
+ *
+ * @todo: Representation of viewport between this function and
+ * matrix_viewport() is inconsistent (position and dimensions vs. bounds).
+ * Should this be addressed?
+ */
+
+template < typename E, class A, class B, class L > void
+matrix_pick(
+    matrix<E,A,B,L>& m, E pick_x, E pick_y, E pick_width, E pick_height,
+    E viewport_x, E viewport_y, E viewport_width, E viewport_height)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckMatHomogeneous3D(m);
+
+    identity_transform(m);
+    
+    value_type inv_width = value_type(1) / pick_width;
+    value_type inv_height = value_type(1) / pick_height;
+    
+    m.set_basis_element(0,0,viewport_width*inv_width);
+    m.set_basis_element(1,1,viewport_height*inv_height);
+    m.set_basis_element(3,0,
+        (viewport_width+value_type(2)*(viewport_x-pick_x))*inv_width);
+    m.set_basis_element(3,1,
+        (viewport_height+value_type(2)*(viewport_y-pick_y))*inv_height);
+}
+
+} // namespace cml
+
+#endif
diff --git a/src/cml/mathlib/matrix_rotation.h b/src/cml/mathlib/matrix_rotation.h
new file mode 100644 (file)
index 0000000..9c3821d
--- /dev/null
@@ -0,0 +1,872 @@
+/* -*- 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_rotation_h
+#define matrix_rotation_h
+
+#include <cml/mathlib/matrix_misc.h>
+#include <cml/mathlib/vector_ortho.h>
+
+/* Functions related to matrix rotations in 3D and 2D. */
+
+namespace cml {
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D rotation about world axes
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a matrix representing a 3D rotation about the given world axis */
+template < typename E, class A, class B, class L > void
+matrix_rotation_world_axis( matrix<E,A,B,L>& m, size_t axis, E angle)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckMatLinear3D(m);
+    detail::CheckIndex3(axis);
+
+    size_t i, j, k;
+    cyclic_permutation(axis, i, j, k);
+    
+    value_type s = value_type(std::sin(angle));
+    value_type c = value_type(std::cos(angle));
+    
+    identity_transform(m);
+
+    m.set_basis_element(j,j, c);
+    m.set_basis_element(j,k, s);
+    m.set_basis_element(k,j,-s);
+    m.set_basis_element(k,k, c);
+}
+
+/** Build a matrix representing a 3D rotation about the world x axis */
+template < typename E, class A, class B, class L > void
+matrix_rotation_world_x(matrix<E,A,B,L>& m, E angle) {
+    matrix_rotation_world_axis(m,0,angle);
+}
+
+/** Build a matrix representing a 3D rotation about the world y axis */
+template < typename E, class A, class B, class L > void
+matrix_rotation_world_y(matrix<E,A,B,L>& m, E angle) {
+    matrix_rotation_world_axis(m,1,angle);
+}
+
+/** Build a matrix representing a 3D rotation about the world z axis */
+template < typename E, class A, class B, class L > void
+matrix_rotation_world_z(matrix<E,A,B,L>& m, E angle) {
+    matrix_rotation_world_axis(m,2,angle);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D rotation from an axis-angle pair
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a rotation matrix from an axis-angle pair */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_rotation_axis_angle(matrix<E,A,B,L>& m, const VecT& axis, E angle)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckMatLinear3D(m);
+    detail::CheckVec3(axis);
+    
+    identity_transform(m);
+
+    value_type s = std::sin(angle);
+    value_type c = std::cos(angle);
+    value_type omc = value_type(1) - c;
+
+    value_type xomc = axis[0] * omc;
+    value_type yomc = axis[1] * omc;
+    value_type zomc = axis[2] * omc;
+    
+    value_type xxomc = axis[0] * xomc;
+    value_type yyomc = axis[1] * yomc;
+    value_type zzomc = axis[2] * zomc;
+    value_type xyomc = axis[0] * yomc;
+    value_type yzomc = axis[1] * zomc;
+    value_type zxomc = axis[2] * xomc;
+
+    value_type xs = axis[0] * s;
+    value_type ys = axis[1] * s;
+    value_type zs = axis[2] * s;
+
+    m.set_basis_element(0,0, xxomc + c );
+    m.set_basis_element(0,1, xyomc + zs);
+    m.set_basis_element(0,2, zxomc - ys);
+    m.set_basis_element(1,0, xyomc - zs);
+    m.set_basis_element(1,1, yyomc + c );
+    m.set_basis_element(1,2, yzomc + xs);
+    m.set_basis_element(2,0, zxomc + ys);
+    m.set_basis_element(2,1, yzomc - xs);
+    m.set_basis_element(2,2, zzomc + c );
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D rotation from a quaternion
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a rotation matrix from a quaternion */
+template < typename E, class A, class B, class L, class QuatT > void
+matrix_rotation_quaternion(matrix<E,A,B,L>& m, const QuatT& q)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef QuatT quaternion_type;
+    typedef typename quaternion_type::order_type order_type;
+    typedef typename matrix_type::value_type value_type;
+
+    enum {
+        W = order_type::W,
+        X = order_type::X,
+        Y = order_type::Y,
+        Z = order_type::Z
+    };
+    
+    /* Checking */
+    detail::CheckMatLinear3D(m);
+    detail::CheckQuat(q);
+
+    identity_transform(m);
+    
+    value_type x2 = q[X] + q[X];
+    value_type y2 = q[Y] + q[Y];
+    value_type z2 = q[Z] + q[Z];    
+
+    value_type xx2 = q[X] * x2;
+    value_type yy2 = q[Y] * y2;
+    value_type zz2 = q[Z] * z2;
+    value_type xy2 = q[X] * y2;
+    value_type yz2 = q[Y] * z2;
+    value_type zx2 = q[Z] * x2;
+    value_type xw2 = q[W] * x2;
+    value_type yw2 = q[W] * y2;
+    value_type zw2 = q[W] * z2;
+    
+    m.set_basis_element(0,0, value_type(1) - yy2 - zz2);
+    m.set_basis_element(0,1,                 xy2 + zw2);
+    m.set_basis_element(0,2,                 zx2 - yw2);
+    m.set_basis_element(1,0,                 xy2 - zw2);
+    m.set_basis_element(1,1, value_type(1) - zz2 - xx2);
+    m.set_basis_element(1,2,                 yz2 + xw2);
+    m.set_basis_element(2,0,                 zx2 + yw2);
+    m.set_basis_element(2,1,                 yz2 - xw2);
+    m.set_basis_element(2,2, value_type(1) - xx2 - yy2);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D rotation from Euler angles
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a rotation matrix from an Euler-angle triple
+ *
+ * The rotations are applied about the cardinal axes in the order specified by
+ * the 'order' argument, where 'order' is one of the following enumerants:
+ *
+ * euler_order_xyz
+ * euler_order_xzy
+ * euler_order_xyx
+ * euler_order_xzx
+ * euler_order_yzx
+ * euler_order_yxz
+ * euler_order_yzy
+ * euler_order_yxy
+ * euler_order_zxy
+ * euler_order_zyx
+ * euler_order_zxz
+ * euler_order_zyz
+ */
+
+template < typename E, class A, class B, class L > void
+matrix_rotation_euler(matrix<E,A,B,L>& m, E angle_0, E angle_1, E angle_2,
+    EulerOrder order)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckMatLinear3D(m);
+
+    identity_transform(m);
+    
+    size_t i, j, k;
+    bool odd, repeat;
+    detail::unpack_euler_order(order, i, j, k, odd, repeat);
+
+    if (odd) {
+        angle_0 = -angle_0;
+        angle_1 = -angle_1;
+        angle_2 = -angle_2;
+    }
+    
+    value_type s0 = std::sin(angle_0);
+    value_type c0 = std::cos(angle_0);
+    value_type s1 = std::sin(angle_1);
+    value_type c1 = std::cos(angle_1);
+    value_type s2 = std::sin(angle_2);
+    value_type c2 = std::cos(angle_2);
+    
+    value_type s0s2 = s0 * s2;
+    value_type s0c2 = s0 * c2;
+    value_type c0s2 = c0 * s2;
+    value_type c0c2 = c0 * c2;
+
+    if (repeat) {
+        m.set_basis_element(i,i, c1              );
+        m.set_basis_element(i,j, s1 * s2         );
+        m.set_basis_element(i,k,-s1 * c2         );
+        m.set_basis_element(j,i, s0 * s1         );
+        m.set_basis_element(j,j,-c1 * s0s2 + c0c2);
+        m.set_basis_element(j,k, c1 * s0c2 + c0s2);
+        m.set_basis_element(k,i, c0 * s1         );
+        m.set_basis_element(k,j,-c1 * c0s2 - s0c2);
+        m.set_basis_element(k,k, c1 * c0c2 - s0s2);
+    } else {
+        m.set_basis_element(i,i, c1 * c2         );
+        m.set_basis_element(i,j, c1 * s2         );
+        m.set_basis_element(i,k,-s1              );
+        m.set_basis_element(j,i, s1 * s0c2 - c0s2);
+        m.set_basis_element(j,j, s1 * s0s2 + c0c2);
+        m.set_basis_element(j,k, s0 * c1         );
+        m.set_basis_element(k,i, s1 * c0c2 + s0s2);
+        m.set_basis_element(k,j, s1 * c0s2 - s0c2);
+        m.set_basis_element(k,k, c0 * c1         );
+    }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D rotation to align with a vector, multiple vectors, or the view plane
+//////////////////////////////////////////////////////////////////////////////
+
+/** See vector_ortho.h for details */
+template < typename E,class A,class B,class L,class VecT_1,class VecT_2 > void
+matrix_rotation_align(
+    matrix<E,A,B,L>& m,
+    const VecT_1& align,
+    const VecT_2& reference,
+    bool normalize = true,
+    AxisOrder order = axis_order_zyx)
+{
+    typedef vector< E,fixed<3> > vector_type;
+
+    identity_transform(m);
+    
+    vector_type x, y, z;
+
+    orthonormal_basis(align, reference, x, y, z, normalize, order);
+    matrix_set_basis_vectors(m, x, y, z);
+}
+
+/** See vector_ortho.h for details */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_rotation_align(matrix<E,A,B,L>& m, const VecT& align,
+    bool normalize = true, AxisOrder order = axis_order_zyx)
+{
+    typedef vector< E,fixed<3> > vector_type;
+
+    identity_transform(m);
+    
+    vector_type x, y, z;
+
+    orthonormal_basis(align, x, y, z, normalize, order);
+    matrix_set_basis_vectors(m, x, y, z);
+}
+
+/** See vector_ortho.h for details */
+template < typename E,class A,class B,class L,class VecT_1,class VecT_2 > void
+matrix_rotation_align_axial(matrix<E,A,B,L>& m, const VecT_1& align,
+    const VecT_2& axis, bool normalize = true,
+    AxisOrder order = axis_order_zyx)
+{
+    typedef vector< E,fixed<3> > vector_type;
+
+    identity_transform(m);
+    
+    vector_type x, y, z;
+
+    orthonormal_basis_axial(align, axis, x, y, z, normalize, order);
+    matrix_set_basis_vectors(m, x, y, z);
+}
+
+/** See vector_ortho.h for details */
+template < typename E, class A, class B, class L, class MatT > void
+matrix_rotation_align_viewplane(
+    matrix<E,A,B,L>& m,
+    const MatT& view_matrix,
+    Handedness handedness,
+    AxisOrder order = axis_order_zyx)
+{
+    typedef vector< E, fixed<3> > vector_type;
+
+    identity_transform(m);
+    
+    vector_type x, y, z;
+
+    orthonormal_basis_viewplane(view_matrix, x, y, z, handedness, order);
+    matrix_set_basis_vectors(m, x, y, z);
+}
+
+/** See vector_ortho.h for details */
+template < typename E, class A, class B, class L, class MatT > void
+matrix_rotation_align_viewplane_LH(
+    matrix<E,A,B,L>& m,
+    const MatT& view_matrix,
+    AxisOrder order = axis_order_zyx)
+{
+    matrix_rotation_align_viewplane(
+        m,view_matrix,left_handed,order);
+}
+
+/** See vector_ortho.h for details */
+template < typename E, class A, class B, class L, class MatT > void
+matrix_rotation_align_viewplane_RH(
+    matrix<E,A,B,L>& m,
+    const MatT& view_matrix,
+    AxisOrder order = axis_order_zyx)
+{
+    matrix_rotation_align_viewplane(
+        m,view_matrix,right_handed,order);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D rotation to aim at a target
+//////////////////////////////////////////////////////////////////////////////
+
+/** See vector_ortho.h for details */
+template < typename E, class A, class B, class L,
+    class VecT_1, class VecT_2, class VecT_3 > void
+matrix_rotation_aim_at(
+    matrix<E,A,B,L>& m,
+    const VecT_1& pos,
+    const VecT_2& target,
+    const VecT_3& reference,
+    AxisOrder order = axis_order_zyx)
+{
+    matrix_rotation_align(m, target - pos, reference, true, order);
+}
+
+/** See vector_ortho.h for details */
+template < typename E, class A, class B, class L,
+    class VecT_1, class VecT_2 > void
+matrix_rotation_aim_at(
+    matrix<E,A,B,L>& m,
+    const VecT_1& pos,
+    const VecT_2& target,
+    AxisOrder order = axis_order_zyx)
+{
+    matrix_rotation_align(m, target - pos, true, order);
+}
+
+/** See vector_ortho.h for details */
+template < typename E, class A, class B, class L,
+    class VecT_1, class VecT_2, class VecT_3 > void
+matrix_rotation_aim_at_axial(
+    matrix<E,A,B,L>& m,
+    const VecT_1& pos,
+    const VecT_2& target,
+    const VecT_3& axis,
+    AxisOrder order = axis_order_zyx)
+{
+    matrix_rotation_align_axial(m, target - pos, axis, true, order);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 2D rotation
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a matrix representing a 2D rotation */
+template < typename E, class A, class B, class L > void
+matrix_rotation_2D( matrix<E,A,B,L>& m, E angle)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckMatLinear2D(m);
+
+    value_type s = value_type(std::sin(angle));
+    value_type c = value_type(std::cos(angle));
+    
+    identity_transform(m);
+
+    m.set_basis_element(0,0, c);
+    m.set_basis_element(0,1, s);
+    m.set_basis_element(1,0,-s);
+    m.set_basis_element(1,1, c);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 2D rotation to align with a vector
+//////////////////////////////////////////////////////////////////////////////
+
+/** See vector_ortho.h for details */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_rotation_align_2D(matrix<E,A,B,L>& m, const VecT& align,
+    bool normalize = true, AxisOrder2D order = axis_order_xy)
+{
+    typedef vector< E, fixed<2> > vector_type;
+
+    identity_transform(m);
+    
+    vector_type x, y;
+
+    orthonormal_basis_2D(align, x, y, normalize, order);
+    matrix_set_basis_vectors_2D(m, x, y);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D relative rotation about world axes
+//////////////////////////////////////////////////////////////////////////////
+
+/** Rotate a rotation matrix about the given world axis */
+template < typename E, class A, class B, class L > void
+matrix_rotate_about_world_axis(matrix<E,A,B,L>& m, size_t axis, E angle)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckMatLinear3D(m);
+    detail::CheckIndex3(axis);
+
+    size_t i, j, k;
+    cyclic_permutation(axis, i, j, k);
+
+    value_type s = value_type(std::sin(angle));
+    value_type c = value_type(std::cos(angle));
+
+    value_type ij = c * m.basis_element(i,j) - s * m.basis_element(i,k);
+    value_type jj = c * m.basis_element(j,j) - s * m.basis_element(j,k);
+    value_type kj = c * m.basis_element(k,j) - s * m.basis_element(k,k);
+    
+    m.set_basis_element(i,k, s*m.basis_element(i,j) + c*m.basis_element(i,k));
+    m.set_basis_element(j,k, s*m.basis_element(j,j) + c*m.basis_element(j,k));
+    m.set_basis_element(k,k, s*m.basis_element(k,j) + c*m.basis_element(k,k));
+    
+    m.set_basis_element(i,j,ij);
+    m.set_basis_element(j,j,jj);
+    m.set_basis_element(k,j,kj);
+}
+
+/** Rotate a rotation matrix about the world x axis */
+template < typename E, class A, class B, class L > void
+matrix_rotate_about_world_x(matrix<E,A,B,L>& m, E angle) {
+    matrix_rotate_about_world_axis(m,0,angle);
+}
+
+/** Rotate a rotation matrix about the world y axis */
+template < typename E, class A, class B, class L > void
+matrix_rotate_about_world_y(matrix<E,A,B,L>& m, E angle) {
+    matrix_rotate_about_world_axis(m,1,angle);
+}
+
+/** Rotate a rotation matrix about the world z axis */
+template < typename E, class A, class B, class L > void
+matrix_rotate_about_world_z(matrix<E,A,B,L>& m, E angle) {
+    matrix_rotate_about_world_axis(m,2,angle);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D relative rotation about local axes
+//////////////////////////////////////////////////////////////////////////////
+
+/** Rotate a rotation matrix about the given local axis */
+template < typename E, class A, class B, class L > void
+matrix_rotate_about_local_axis(matrix<E,A,B,L>& m, size_t axis, E angle)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckMatLinear3D(m);
+    detail::CheckIndex3(axis);
+
+    size_t i, j, k;
+    cyclic_permutation(axis, i, j, k);
+
+    value_type s = value_type(std::sin(angle));
+    value_type c = value_type(std::cos(angle));
+
+    value_type j0 = c * m.basis_element(j,0) + s * m.basis_element(k,0);
+    value_type j1 = c * m.basis_element(j,1) + s * m.basis_element(k,1);
+    value_type j2 = c * m.basis_element(j,2) + s * m.basis_element(k,2);
+
+    m.set_basis_element(k,0, c*m.basis_element(k,0) - s*m.basis_element(j,0));
+    m.set_basis_element(k,1, c*m.basis_element(k,1) - s*m.basis_element(j,1));
+    m.set_basis_element(k,2, c*m.basis_element(k,2) - s*m.basis_element(j,2));
+
+    m.set_basis_element(j,0,j0);
+    m.set_basis_element(j,1,j1);
+    m.set_basis_element(j,2,j2);
+}
+
+/** Rotate a rotation matrix about its local x axis */
+template < typename E, class A, class B, class L > void
+matrix_rotate_about_local_x(matrix<E,A,B,L>& m, E angle) {
+    matrix_rotate_about_local_axis(m,0,angle);
+}
+
+/** Rotate a rotation matrix about its local y axis */
+template < typename E, class A, class B, class L > void
+matrix_rotate_about_local_y(matrix<E,A,B,L>& m, E angle) {
+    matrix_rotate_about_local_axis(m,1,angle);
+}
+
+/** Rotate a rotation matrix about its local z axis */
+template < typename E, class A, class B, class L > void
+matrix_rotate_about_local_z(matrix<E,A,B,L>& m, E angle) {
+    matrix_rotate_about_local_axis(m,2,angle);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 2D relative rotation
+//////////////////////////////////////////////////////////////////////////////
+
+template < typename E, class A, class B, class L > void
+matrix_rotate_2D(matrix<E,A,B,L>& m, E angle)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckMatLinear2D(m);
+
+    value_type s = value_type(std::sin(angle));
+    value_type c = value_type(std::cos(angle));
+
+    value_type m00 = c * m.basis_element(0,0) - s * m.basis_element(0,1);
+    value_type m10 = c * m.basis_element(1,0) - s * m.basis_element(1,1);
+
+    m.set_basis_element(0,1, s*m.basis_element(0,0) + c*m.basis_element(0,1));
+    m.set_basis_element(1,1, s*m.basis_element(1,0) + c*m.basis_element(1,1));
+
+    m.set_basis_element(0,0,m00);
+    m.set_basis_element(1,0,m10);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Rotation from vector to vector
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a rotation matrix to rotate from one vector to another
+ *
+ * Note: The quaternion algorithm is more stable than the matrix algorithm, so
+ * we simply pass off to the quaternion function here.
+ */
+template < class E,class A,class B,class L,class VecT_1,class VecT_2 > void
+matrix_rotation_vec_to_vec(
+    matrix<E,A,B,L>& m,
+    const VecT_1& v1,
+    const VecT_2& v2,
+    bool unit_length_vectors = false)
+{
+    typedef quaternion< E,fixed<>,vector_first,positive_cross >
+        quaternion_type;
+    
+    quaternion_type q;
+    quaternion_rotation_vec_to_vec(q,v1,v2,unit_length_vectors);
+    matrix_rotation_quaternion(m,q);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Scale the angle of a rotation matrix
+//////////////////////////////////////////////////////////////////////////////
+
+/** Scale the angle of a 3D rotation matrix */
+template < typename E, class A, class B, class L > void
+matrix_scale_rotation_angle(matrix<E,A,B,L>& m, E t,
+    E tolerance = epsilon<E>::placeholder())
+{
+    typedef vector< E,fixed<3> > vector_type;
+    typedef typename vector_type::value_type value_type;
+    
+    vector_type axis;
+    value_type angle;
+    matrix_to_axis_angle(m, axis, angle, tolerance);
+    matrix_rotation_axis_angle(m, axis, angle * t);
+}
+
+/** Scale the angle of a 2D rotation matrix */
+template < typename E, class A, class B, class L > void
+matrix_scale_rotation_angle_2D(
+    matrix<E,A,B,L>& m, E t, E tolerance = epsilon<E>::placeholder())
+{
+    typedef vector< E,fixed<2> > vector_type;
+    typedef typename vector_type::value_type value_type;
+
+    value_type angle = matrix_to_rotation_2D(m);
+    matrix_rotation_2D(m, angle * t);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Support functions for uniform handling of row- and column-basis matrices
+//////////////////////////////////////////////////////////////////////////////
+
+/* Note: The matrix rotation slerp, difference and concatenation functions do
+ * not use et::MatrixPromote<M1,M2>::temporary_type as the return type, even
+ * though that is the return type of the underlying matrix multiplication.
+ * This is because the sizes of these matrices are known at compile time (3x3
+ * and 2x2), and using fixed<> obviates the need for resizing of intermediate
+ * temporaries.
+ *
+ * Also, no size- or type-checking is done on the arguments to these
+ * functions, as any such errors will be caught by the matrix multiplication
+ * and assignment to the 3x3 temporary.
+ */
+
+/** A fixed-size temporary 3x3 matrix */
+#define MAT_TEMP_3X3 matrix<         \
+    typename et::ScalarPromote<      \
+        typename MatT_1::value_type, \
+        typename MatT_2::value_type  \
+    >::type,                         \
+    fixed<3,3>,                      \
+    typename MatT_1::basis_orient,   \
+    row_major                        \
+>
+
+/** A fixed-size temporary 2x2 matrix */
+#define MAT_TEMP_2X2 matrix<         \
+    typename et::ScalarPromote<      \
+        typename MatT_1::value_type, \
+        typename MatT_2::value_type  \
+    >::type,                         \
+    fixed<2,2>,                      \
+    typename MatT_1::basis_orient,   \
+    row_major                        \
+>
+
+namespace detail {
+
+/** Concatenate two 3D row-basis rotation matrices in the order m1->m2 */
+template < class MatT_1, class MatT_2 > MAT_TEMP_3X3
+matrix_concat_rotations(const MatT_1& m1, const MatT_2& m2, row_basis) {
+    return m1*m2;
+}
+
+/** Concatenate two 3D col-basis rotation matrices in the order m1->m2 */
+template < class MatT_1, class MatT_2 > MAT_TEMP_3X3
+matrix_concat_rotations(const MatT_1& m1, const MatT_2& m2, col_basis) {
+    return m2*m1;
+}
+
+/** Concatenate two 3D rotation matrices in the order m1->m2 */
+template < class MatT_1, class MatT_2 > MAT_TEMP_3X3
+matrix_concat_rotations(const MatT_1& m1, const MatT_2& m2) {
+    return matrix_concat_rotations(m1,m2,typename MatT_1::basis_orient());
+}
+
+/** Concatenate two 2D row-basis rotation matrices in the order m1->m2 */
+template < class MatT_1, class MatT_2 > MAT_TEMP_2X2
+matrix_concat_rotations_2D(const MatT_1& m1, const MatT_2& m2, row_basis) {
+    return m1*m2;
+}
+
+/** Concatenate two 2D col-basis rotation matrices in the order m1->m2 */
+template < class MatT_1, class MatT_2 > MAT_TEMP_2X2
+matrix_concat_rotations_2D(const MatT_1& m1, const MatT_2& m2, col_basis) {
+    return m2*m1;
+}
+
+/** Concatenate two 2D rotation matrices in the order m1->m2 */
+template < class MatT_1, class MatT_2 > MAT_TEMP_2X2
+matrix_concat_rotations_2D(const MatT_1& m1, const MatT_2& m2) {
+    return matrix_concat_rotations_2D(m1,m2,typename MatT_1::basis_orient());
+}
+
+} // namespace detail
+
+//////////////////////////////////////////////////////////////////////////////
+// Matrix rotation difference
+//////////////////////////////////////////////////////////////////////////////
+
+/** Return the rotational 'difference' between two 3D rotation matrices */
+template < class MatT_1, class MatT_2 > MAT_TEMP_3X3
+matrix_rotation_difference(const MatT_1& m1, const MatT_2& m2) {
+    return detail::matrix_concat_rotations(transpose(m1),m2);
+}
+
+/** Return the rotational 'difference' between two 2D rotation matrices */
+template < class MatT_1, class MatT_2 > MAT_TEMP_2X2
+matrix_rotation_difference_2D(const MatT_1& m1, const MatT_2& m2) {
+    return detail::matrix_concat_rotations_2D(transpose(m1),m2);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Spherical linear interpolation of rotation matrices
+//////////////////////////////////////////////////////////////////////////////
+
+/* @todo: It might be as fast or faster to simply convert the matrices to
+ * quaternions, interpolate, and convert back.
+ *
+ * @todo: The behavior of matrix slerp is currently a little different than
+ * for quaternions: in the matrix function, when the two matrices are close
+ * to identical the first is returned, while in the quaternion function the
+ * quaternions are nlerp()'d in this case.
+ *
+ * I still need to do the equivalent of nlerp() for matrices, in which case
+ * these functions could be revised to pass off to nlerp() when the matrices
+ * are nearly aligned.
+*/
+
+/** Spherical linear interpolation of two 3D rotation matrices */
+template < class MatT_1, class MatT_2, typename E > MAT_TEMP_3X3
+matrix_slerp(const MatT_1& m1, const MatT_2& m2, E t,
+    E tolerance = epsilon<E>::placeholder())
+{
+    typedef MAT_TEMP_3X3 temporary_type;
+
+    temporary_type m = matrix_rotation_difference(m1,m2);
+    matrix_scale_rotation_angle(m,t,tolerance);
+    return detail::matrix_concat_rotations(m1,m);
+}
+
+/** Spherical linear interpolation of two 2D rotation matrices */
+template < class MatT_1, class MatT_2, typename E > MAT_TEMP_2X2
+matrix_slerp_2D(const MatT_1& m1, const MatT_2& m2, E t,
+    E tolerance = epsilon<E>::placeholder())
+{
+    typedef MAT_TEMP_2X2 temporary_type;
+
+    temporary_type m = matrix_rotation_difference_2D(m1,m2);
+    matrix_scale_rotation_angle_2D(m,t,tolerance);
+    return detail::matrix_concat_rotations_2D(m1,m);
+}
+
+#undef MAT_TEMP_3X3
+#undef MAT_TEMP_2X2
+
+//////////////////////////////////////////////////////////////////////////////
+// Conversions
+//////////////////////////////////////////////////////////////////////////////
+
+/** Convert a 3D rotation matrix to an axis-angle pair */
+template < class MatT, typename E, class A > void
+matrix_to_axis_angle(
+    const MatT& m,
+    vector<E,A >& axis,
+    E& angle,
+    E tolerance = epsilon<E>::placeholder())
+{
+    typedef MatT matrix_type;
+    typedef typename matrix_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckMatLinear3D(m);
+
+    axis.set(
+        m.basis_element(1,2) - m.basis_element(2,1),
+        m.basis_element(2,0) - m.basis_element(0,2),
+        m.basis_element(0,1) - m.basis_element(1,0)
+    );
+       value_type l = length(axis);
+    value_type tmo = trace_3x3(m) - value_type(1);
+
+       if (l > tolerance) {
+               axis /= l;
+        angle = std::atan2(l, tmo); // l=2sin(theta),tmo=2cos(theta)
+       } else if (tmo > value_type(0)) {
+               axis.zero();
+               angle = value_type(0);
+       } else {
+        size_t largest_diagonal_element =
+            index_of_max(
+                m.basis_element(0,0),
+                m.basis_element(1,1),
+                m.basis_element(2,2)
+            );
+        size_t i, j, k;
+        cyclic_permutation(largest_diagonal_element, i, j, k);
+               axis[i] =
+            std::sqrt(
+                m.basis_element(i,i) -
+                m.basis_element(j,j) -
+                m.basis_element(k,k) +
+                value_type(1)
+            ) * value_type(.5);
+               value_type s = value_type(.5) / axis[i];
+               axis[j] = m.basis_element(i,j) * s;
+               axis[k] = m.basis_element(i,k) * s;
+        angle = constants<value_type>::pi();
+       }
+}
+
+/** Convert a 3D rotation matrix to an Euler-angle triple */
+template < class MatT, typename Real >
+void matrix_to_euler(
+    const MatT& m,
+    Real& angle_0,
+    Real& angle_1,
+    Real& angle_2,
+    EulerOrder order,
+    Real tolerance = epsilon<Real>::placeholder())
+{
+    typedef MatT matrix_type;
+    typedef typename matrix_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckMatLinear3D(m);
+
+    size_t i, j, k;
+    bool odd, repeat;
+    detail::unpack_euler_order(order, i, j, k, odd, repeat);
+
+    if (repeat) {
+        value_type s1 = length(m.basis_element(j,i),m.basis_element(k,i));
+        value_type c1 = m.basis_element(i,i);
+
+        angle_1 = std::atan2(s1, c1);
+        if (s1 > tolerance) {
+            angle_0 = std::atan2(m.basis_element(j,i),m.basis_element(k,i));
+            angle_2 = std::atan2(m.basis_element(i,j),-m.basis_element(i,k));
+        } else {
+            angle_0 = value_type(0);
+            angle_2 = sign(c1) *
+                std::atan2(-m.basis_element(k,j),m.basis_element(j,j));
+        }
+    } else {
+        value_type s1 = -m.basis_element(i,k);
+        value_type c1 = length(m.basis_element(i,i),m.basis_element(i,j));
+
+        angle_1 = std::atan2(s1, c1);
+        if (c1 > tolerance) {
+            angle_0 = std::atan2(m.basis_element(j,k),m.basis_element(k,k));
+            angle_2 = std::atan2(m.basis_element(i,j),m.basis_element(i,i));
+        } else {
+            angle_0 = value_type(0);
+            angle_2 = -sign(s1) *
+                std::atan2(-m.basis_element(k,j),m.basis_element(j,j));
+        }
+    }
+    
+    if (odd) {
+        angle_0 = -angle_0;
+        angle_1 = -angle_1;
+        angle_2 = -angle_2;
+    }
+}
+
+/** Convert a 2D rotation matrix to a rotation angle */
+template < class MatT > typename MatT::value_type
+matrix_to_rotation_2D(const MatT& m)
+{
+    /* Checking */
+    detail::CheckMatLinear2D(m);
+    
+    return std::atan2(m.basis_element(0,1),m.basis_element(0,0));
+}
+
+} // namespace cml
+
+#endif
diff --git a/src/cml/mathlib/matrix_transform.h b/src/cml/mathlib/matrix_transform.h
new file mode 100644 (file)
index 0000000..359ab2d
--- /dev/null
@@ -0,0 +1,983 @@
+/* -*- 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_transform_h
+#define matrix_transform_h
+
+#include <cml/mathlib/matrix_basis.h>
+#include <cml/mathlib/matrix_rotation.h>
+#include <cml/mathlib/matrix_translation.h>
+
+/* Functions for building matrix transforms other than rotations
+ * (matrix_rotation.h) and viewing projections (matrix_projection.h).
+ */
+
+namespace cml {
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D translation
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a matrix representing a 3D translation */
+template < typename E, class A, class B, class L > void
+matrix_translation(matrix<E,A,B,L>& m, E x, E y, E z)
+{
+    identity_transform(m);
+    matrix_set_translation(m,x,y,z);
+}
+
+/** Build a matrix representing a 3D translation with z set to 0 */
+template < typename E, class A, class B, class L > void
+matrix_translation(matrix<E,A,B,L>& m, E x, E y)
+{
+    identity_transform(m);
+    matrix_set_translation(m,x,y);
+}
+
+/** Build a matrix representing a 3D translation */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_translation(matrix<E,A,B,L>& m, const VecT& translation)
+{
+    identity_transform(m);
+    matrix_set_translation(m,translation);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 2D translation
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a matrix representing a 2D translation */
+template < typename E, class A, class B, class L > void
+matrix_translation_2D(matrix<E,A,B,L>& m, E x, E y)
+{
+    identity_transform(m);
+    matrix_set_translation_2D(m,x,y);
+}
+
+/** Build a matrix representing a 2D translation */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_translation_2D(matrix<E,A,B,L>& m, const VecT& translation)
+{
+    identity_transform(m);
+    matrix_set_translation_2D(m, translation);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D scale
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a matrix representing a uniform 3D scale */
+template < typename E, class A, class B, class L > void
+matrix_uniform_scale(matrix<E,A,B,L>& m, E scale) {
+    matrix_scale(m,scale,scale,scale);
+}
+
+/** Build a matrix representing a non-uniform 3D scale */
+template < typename E, class A, class B, class L > void
+matrix_scale(matrix<E,A,B,L>& m, E scale_x, E scale_y, E scale_z)
+{
+    /* Checking */
+    detail::CheckMatLinear3D(m);
+    
+    identity_transform(m);
+    
+    m.set_basis_element(0,0,scale_x);
+    m.set_basis_element(1,1,scale_y);
+    m.set_basis_element(2,2,scale_z);
+}
+
+/** Build a matrix representing a non-uniform 3D scale */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_scale(matrix<E,A,B,L>& m, const VecT& scale)
+{
+    /* Checking */
+    detail::CheckVec3(scale);
+    
+    matrix_scale(m, scale[0], scale[1], scale[2]);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 2D scale
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a matrix representing a uniform 2D scale */
+template < typename E, class A, class B, class L > void
+matrix_uniform_scale_2D(matrix<E,A,B,L>& m, E scale) {
+    matrix_scale_2D(m,scale,scale);
+}
+
+/** Build a matrix representing a non-uniform 2D scale */
+template < typename E, class A, class B, class L > void
+matrix_scale_2D(matrix<E,A,B,L>& m, E scale_x, E scale_y)
+{
+    /* Checking */
+    detail::CheckMatLinear2D(m);
+    
+    identity_transform(m);
+    
+    m.set_basis_element(0,0,scale_x);
+    m.set_basis_element(1,1,scale_y);
+}
+
+/** Build a matrix representing a non-uniform 2D scale */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_scale_2D(matrix<E,A,B,L>& m, const VecT& scale)
+{
+    /* Checking */
+    detail::CheckVec2(scale);
+    
+    matrix_scale_2D(m, scale[0], scale[1]);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D scale along axis
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a matrix representing a 3D scale along an arbitrary axis */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_scale_along_axis(matrix<E,A,B,L>&m, const VecT& axis, E scale)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckVec3(axis);
+
+    matrix<E,fixed<3,3>,B,L> outer_p = outer(axis,axis)*(scale-value_type(1));
+    outer_p(0,0) += value_type(1);
+    outer_p(1,1) += value_type(1);
+    outer_p(2,2) += value_type(1);
+
+    matrix_linear_transform(m, outer_p);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 2D scale along axis
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a matrix representing a 2D scale along an arbitrary axis */
+template < typename E, class A, class B, class L, class VecT >
+void matrix_scale_along_axis_2D(matrix<E,A,B,L>&  m, const VecT& axis,
+    E scale)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckVec2(axis);
+
+    matrix<E,fixed<2,2>,B,L> outer_p = outer(axis,axis)*(scale-value_type(1));
+    outer_p(0,0) += value_type(1);
+    outer_p(1,1) += value_type(1);
+
+    matrix_linear_transform_2D(m, outer_p);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D shear
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a matrix representing a 3D shear along the specified world axis */
+template < typename E, class A, class B, class L > void
+matrix_shear(matrix<E,A,B,L>& m, size_t axis, E shear_s, E shear_t)
+{
+    /* Checking */
+    detail::CheckMatLinear3D(m);
+    detail::CheckIndex3(axis);
+
+    identity_transform(m);
+    
+    size_t i, j, k;
+    cyclic_permutation(axis, i, j, k);
+    
+    m.set_basis_element(i,j,shear_s);
+    m.set_basis_element(i,k,shear_t);
+}
+
+/** Build a matrix representing a 3D shear along the world x axis */
+template < typename E, class A, class B, class L > void
+matrix_shear_x(matrix<E,A,B,L>& m, E shear_s, E shear_t) {
+    matrix_shear(m,0,shear_s,shear_t);
+}
+
+/** Build a matrix representing a 3D shear along the world y axis */
+template < typename E, class A, class B, class L > void
+matrix_shear_y(matrix<E,A,B,L>& m, E shear_s, E shear_t) {
+    matrix_shear(m,1,shear_s,shear_t);
+}
+
+/** Build a matrix representing a 3D shear along the world z axis */
+template < typename E, class A, class B, class L > void
+matrix_shear_z(matrix<E,A,B,L>& m, E shear_s, E shear_t) {
+    matrix_shear(m,2,shear_s,shear_t);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 2D shear
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a matrix representing a 2D shear along the specified world axis */
+template < typename E, class A, class B, class L > void
+matrix_shear_2D(matrix<E,A,B,L>& m, size_t axis, E shear)
+{
+    /* Checking */
+    detail::CheckMatLinear2D(m);
+    detail::CheckIndex2(axis);
+
+    identity_transform(m);
+    
+    size_t i, j;
+    cyclic_permutation(axis, i, j);
+    
+    m.set_basis_element(i,j,shear);
+}
+
+/** Build a matrix representing a 2D shear along the world x axis */
+template < typename E, class A, class B, class L > void
+matrix_shear_x_2D(matrix<E,A,B,L>& m, E shear) {
+    matrix_shear_2D(m,0,shear);
+}
+
+/** Build a matrix representing a 2D shear along the world y axis */
+template < typename E, class A, class B, class L > void
+matrix_shear_y_2D(matrix<E,A,B,L>& m, E shear) {
+    matrix_shear_2D(m,1,shear);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D reflection
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a matrix representing a 3D reflection along the given world axis */
+template < typename E, class A, class B, class L > void
+matrix_reflect(matrix<E,A,B,L>& m, size_t axis)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckMatLinear3D(m);
+    detail::CheckIndex3(axis);
+
+    identity_transform(m);
+    
+    m(axis,axis) = value_type(-1);
+}
+
+/** Build a matrix representing a 3D reflection along the world x axis */
+template < typename E, class A, class B, class L > void
+matrix_reflect_x(matrix<E,A,B,L>& m) {
+    matrix_reflect(m,0);
+}
+
+/** Build a matrix representing a 3D reflection along the world y axis */
+template < typename E, class A, class B, class L > void
+matrix_reflect_y(matrix<E,A,B,L>& m) {
+    matrix_reflect(m,1);
+}
+
+/** Build a matrix representing a 3D reflection along the world z axis */
+template < typename E, class A, class B, class L > void
+matrix_reflect_z(matrix<E,A,B,L>& m) {
+    matrix_reflect(m,2);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 2D reflection
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a matrix representing a 2D reflection along the given world axis */
+template < typename E, class A, class B, class L > void
+matrix_reflect_2D(matrix<E,A,B,L>& m, size_t axis)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckMatLinear2D(m);
+    detail::CheckIndex2(axis);
+
+    identity_transform(m);
+    
+    m(axis,axis) = value_type(-1);
+}
+
+/** Build a matrix representing a 2D reflection along the world x axis */
+template < typename E, class A, class B, class L > void
+matrix_reflect_x_2D(matrix<E,A,B,L>& m) {
+    matrix_reflect_2D(m,0);
+}
+
+/** Build a matrix representing a 2D reflection along the world y axis */
+template < typename E, class A, class B, class L > void
+matrix_reflect_y_2D(matrix<E,A,B,L>& m) {
+    matrix_reflect_2D(m,1);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D reflection about hyperplane
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a matrix representing a 3D reflection about the given hyperplane */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_reflect_about_hplane(matrix<E,A,B,L>& m, const VecT& normal)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+
+    matrix_scale_along_axis(m, normal, value_type(-1));
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 2D reflection about hyperplane
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a matrix representing a 2D reflection about the given hyperplane */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_reflect_about_hplane_2D(matrix<E,A,B,L>&m, const VecT& normal)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+
+    matrix_scale_along_axis_2D(m, normal, value_type(-1));
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D orthographic projection to cardinal hyperplane
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a matrix representing an orthographic projection onto a plane */
+template < typename E, class A, class B, class L > void
+matrix_ortho_project(matrix<E,A,B,L>& m, size_t axis)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckMatLinear3D(m);
+    detail::CheckIndex3(axis);
+
+    identity_transform(m);
+    
+    m(axis,axis) = value_type(0);
+}
+
+/** Build a matrix representing an orthographic projection onto the yz plane*/
+template < typename E, class A, class B, class L > void
+matrix_ortho_project_yz(matrix<E,A,B,L>& m) {
+    matrix_ortho_project(m,0);
+}
+
+/** Build a matrix representing an orthographic projection onto the zx plane*/
+template < typename E, class A, class B, class L > void
+matrix_ortho_project_zx(matrix<E,A,B,L>& m) {
+    matrix_ortho_project(m,1);
+}
+
+/** Build a matrix representing an orthographic projection onto the zy plane*/
+template < typename E, class A, class B, class L > void
+matrix_ortho_project_xy(matrix<E,A,B,L>& m) {
+    matrix_ortho_project(m,2);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 2D orthographic projection to cardinal hyperplane
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a matrix representing a 2D orthographic projection */
+template < typename E, class A, class B, class L > void
+matrix_ortho_project_2D(matrix<E,A,B,L>& m, size_t axis)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckMatLinear2D(m);
+    detail::CheckIndex2(axis);
+
+    identity_transform(m);
+    
+    m(axis,axis) = value_type(0);
+}
+
+/** Build a matrix representing an orthographic projection onto the y axis */
+template < typename E, class A, class B, class L > void
+matrix_ortho_project_y_2D(matrix<E,A,B,L>& m) {
+    matrix_ortho_project_2D(m,0);
+}
+
+/** Build a matrix representing an orthographic projection onto the x axis */
+template < typename E, class A, class B, class L > void
+matrix_ortho_project_x_2D(matrix<E,A,B,L>& m) {
+    matrix_ortho_project_2D(m,1);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D orthographic projection to hyperplane
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a matrix representing a 3D orthographic projection about the given
+ * hyperplane passing through the origin.
+ */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_ortho_project_to_hplane(matrix<E,A,B,L>& m, const VecT& normal)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+
+    matrix_scale_along_axis(m, normal, value_type(0));
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 2D orthographic projection to hyperplane
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a matrix representing a 2D orthographic projection about the given
+ * hyperplane passing through the origin.
+ */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_ortho_project_to_hplane_2D(matrix<E,A,B,L>& m, const VecT& normal)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+
+    matrix_scale_along_axis_2D(m, normal, value_type(0));
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D 'aim at'
+//////////////////////////////////////////////////////////////////////////////
+
+/** See vector_ortho.h for details */
+template < typename E, class A, class B, class L,
+    class VecT_1, class VecT_2, class VecT_3 > void
+matrix_aim_at(matrix<E,A,B,L>& m, const VecT_1& pos, const VecT_2& target,
+    const VecT_3& reference,
+    AxisOrder order = axis_order_zyx)
+{
+    matrix_rotation_aim_at(m, pos, target, reference, order);
+    matrix_set_translation(m, pos);
+}
+
+/** See vector_ortho.h for details */
+template < typename E, class A, class B, class L,
+    class VecT_1, class VecT_2 > void
+matrix_aim_at(matrix<E,A,B,L>& m, const VecT_1& pos, const VecT_2& target,
+    AxisOrder order = axis_order_zyx)
+{
+    matrix_rotation_aim_at(m, pos, target, order);
+    matrix_set_translation(m, pos);
+}
+
+/** See vector_ortho.h for details */
+template < typename E, class A, class B, class L,
+    class VecT_1, class VecT_2, class VecT_3 > void
+matrix_aim_at_axial(
+    matrix<E,A,B,L>& m,
+    const VecT_1& pos,
+    const VecT_2& target,
+    const VecT_3& axis,
+    AxisOrder order = axis_order_zyx)
+{
+    matrix_rotation_aim_at_axial(m, pos, target, axis, order);
+    matrix_set_translation(m, pos);
+}
+
+/** See vector_ortho.h for details */
+template < typename E,class A,class B,class L,class VecT,class MatT > void
+matrix_aim_at_viewplane(
+    matrix<E,A,B,L>& m,
+    const VecT& pos,
+    const MatT& view_matrix,
+    Handedness handedness,
+    AxisOrder order = axis_order_zyx)
+{
+    matrix_rotation_align_viewplane(m, view_matrix, handedness, order);
+    matrix_set_translation(m, pos);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 2D 'aim at'
+//////////////////////////////////////////////////////////////////////////////
+
+/** See vector_ortho.h for details */
+template < typename E,class A,class B,class L,class VecT_1,class VecT_2 > void
+matrix_aim_at_2D(
+    matrix<E,A,B,L>& m,
+    const VecT_1& pos,
+    const VecT_2& target,
+    AxisOrder2D order = axis_order_xy)
+{
+    matrix_rotation_align_2D(m, target - pos, true, order);
+    matrix_set_translation_2D(m, pos);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D 'look at' view matrix
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a matrix representing a 'look at' view transform */
+template < typename E, class A, class B, class L,
+    class VecT_1, class VecT_2, class VecT_3 > void
+matrix_look_at(
+    matrix<E,A,B,L>& m,
+    const VecT_1& eye,
+    const VecT_2& target,
+    const VecT_3& up,
+    Handedness handedness)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef vector< E,fixed<3> > vector_type;
+    typedef typename matrix_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckMatAffine3D(m);
+
+    identity_transform(m);
+
+    value_type s = handedness == left_handed ? 1 : -1;
+    vector_type z = s * normalize(target - eye);
+    vector_type x = unit_cross(up,z);
+    vector_type y = cross(z,x);
+
+    matrix_set_transposed_basis_vectors(m,x,y,z);
+    matrix_set_translation(m,-dot(eye,x),-dot(eye,y),-dot(eye,z));
+}
+
+/** Build a matrix representing a left-handedness 'look at' view transform */
+template < typename E, class A, class B, class L,
+    class VecT_1, class VecT_2, class VecT_3 > void
+matrix_look_at_LH(matrix<E,A,B,L>& m, const VecT_1& eye,
+    const VecT_2& target, const VecT_3& up)
+{
+    matrix_look_at(m, eye, target, up, left_handed);
+}
+
+/** Build a matrix representing a right-handedness 'look at' view transform */
+template < typename E, class A, class B, class L,
+    class VecT_1, class VecT_2, class VecT_3 > void
+matrix_look_at_RH(matrix<E,A,B,L>& m, const VecT_1& eye,
+    const VecT_2& target, const VecT_3& up)
+{
+    matrix_look_at(m, eye, target, up, right_handed);
+}
+
+/** Build a matrix representing a 'look at' view transform */
+template < typename E, class A, class B, class L > void
+matrix_look_at(matrix<E,A,B,L>& m, E eye_x, E eye_y, E eye_z, E target_x,
+    E target_y, E target_z, E up_x, E up_y, E up_z,
+    Handedness handedness)
+{
+    typedef vector< E, fixed<3> > vector_type;
+    
+    matrix_look_at(m,
+        vector_type(eye_x,eye_y,eye_z),
+        vector_type(target_x,target_y,target_z),
+        vector_type(up_x,up_y,up_z),
+        handedness
+    );
+}
+
+/** Build a matrix representing a left-handed'look at' view transform */
+template < typename E, class A, class B, class L > void
+matrix_look_at_LH(matrix<E,A,B,L>& m, E eye_x, E eye_y, E eye_z,
+    E target_x, E target_y, E target_z, E up_x, E up_y, E up_z)
+{
+    matrix_look_at(m,eye_x,eye_y,eye_z,target_x,target_y,target_z,up_x,up_y,
+        up_z,left_handed);
+}
+
+/** Build a matrix representing a right-handed'look at' view transform */
+template < typename E, class A, class B, class L > void
+matrix_look_at_RH(matrix<E,A,B,L>& m, E eye_x, E eye_y, E eye_z,
+    E target_x, E target_y, E target_z, E up_x, E up_y, E up_z)
+{
+    matrix_look_at(m,eye_x,eye_y,eye_z,target_x,target_y,target_z,up_x,up_y,
+        up_z,right_handed);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D linear transform
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a matrix from the 3x3 linear transform part of another matrix */
+template < typename E, class A, class B, class L, class MatT > void
+matrix_linear_transform(matrix<E,A,B,L>& m, const MatT& linear)
+{
+    /* Checking */
+    detail::CheckMatLinear3D(m);
+    detail::CheckMatLinear3D(linear);
+    
+    identity_transform(m);
+    
+    for(size_t i = 0; i < 3; ++i) {
+        for(size_t j = 0; j < 3; ++j) {
+            m.set_basis_element(i,j,linear.basis_element(i,j));
+        }
+    }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 2D linear transform
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a matrix from the 2x2 linear transform part of another matrix */
+template < typename E, class A, class B, class L, class MatT > void
+matrix_linear_transform_2D(matrix<E,A,B,L>& m, const MatT& linear)
+{
+    /* Checking */
+    detail::CheckMatLinear2D(m);
+    detail::CheckMatLinear2D(linear);
+    
+    identity_transform(m);
+    
+    for(size_t i = 0; i < 2; ++i) {
+        for(size_t j = 0; j < 2; ++j) {
+            m.set_basis_element(i,j,linear.basis_element(i,j));
+        }
+    }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D affine transform
+//////////////////////////////////////////////////////////////////////////////
+
+/** 3D affine transform from three basis vectors and a translation */
+template <typename E, class A, class B, class L,
+    class VecT_1, class VecT_2, class VecT_3, class VecT_4 > void
+matrix_affine_transform(matrix<E,A,B,L>& m, const VecT_1& x, const VecT_2& y,
+    const VecT_3& z, const VecT_4& translation)
+{
+    identity_transform(m);
+    matrix_set_basis_vectors(m,x,y,z);
+    matrix_set_translation(m,translation);
+}
+
+/** 3D affine transform from a quaternion and a translation */
+template <
+    typename E, class A, class B, class L,
+    typename QE, class QA, class O, class C, class VecT > void
+matrix_affine_transform(
+    matrix<E,A,B,L>& m, const quaternion<QE,QA,O,C>& q,
+    const VecT& translation)
+{
+    matrix_rotation_quaternion(m,q);
+    matrix_set_translation(m,translation);
+}
+
+/** 3D affine transform from a quaternion expression and a translation */
+template < typename E,class A,class B,class L,class XprT,class VecT > void
+matrix_affine_transform(
+    matrix<E,A,B,L>& m, const et::QuaternionXpr<XprT>& q,
+    const VecT& translation)
+{
+    matrix_rotation_quaternion(m,q);
+    matrix_set_translation(m,translation);
+}
+
+/** 3D affine transform from an axis-angle pair and a translation */
+template <
+    typename E, class A, class B, class L, class VecT_1, class VecT_2 > void
+matrix_affine_transform(
+    matrix<E,A,B,L>& m,const VecT_1& axis,E angle,const VecT_2& translation)
+{
+    matrix_rotation_axis_angle(m,axis,angle);
+    matrix_set_translation(m,translation);
+}
+
+/** 3D affine transform from an Euler-angle triple and a translation */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_affine_transform(matrix<E,A,B,L>& m, E angle_0, E angle_1,
+    E angle_2, EulerOrder order, const VecT& translation)
+{
+    matrix_rotation_euler(m,angle_0,angle_1,angle_2,order);
+    matrix_set_translation(m,translation);
+}
+
+/** 3D affine transform from a matrix and a translation */
+template <
+    typename E, class A, class B, class L,
+    typename ME, class MA, class MB, class ML, class VecT > void
+matrix_affine_transform(matrix<E,A,B,L>& m,
+    const matrix<ME,MA,MB,ML>& linear, const VecT& translation)
+{
+    matrix_linear_transform(m,linear);
+    matrix_set_translation(m,translation);
+}
+
+/** 3D affine transform from a matrix expression and a translation */
+template < typename E,class A,class B,class L,class XprT,class VecT > void
+matrix_affine_transform(
+    matrix<E,A,B,L>& m, const et::MatrixXpr<XprT>& linear,
+    const VecT& translation)
+{
+    matrix_linear_transform(m,linear);
+    matrix_set_translation(m,translation);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 2D affine transform
+//////////////////////////////////////////////////////////////////////////////
+
+/** 2D affine transform from two basis vectors and a translation */
+template <typename E, class A, class B, class L,
+    class VecT_1, class VecT_2, class VecT_3 > void
+matrix_affine_transform_2D(matrix<E,A,B,L>& m, const VecT_1& x,
+    const VecT_2& y, const VecT_3& translation)
+{
+    identity_transform(m);
+    matrix_set_basis_vectors_2D(m,x,y);
+    matrix_set_translation_2D(m,translation);
+}
+
+/** 2D affine transform from a rotation angle and a translation */
+template <typename E, class A, class B, class L, class VecT >
+void matrix_affine_transform_2D(matrix<E,A,B,L>& m, E angle,
+    const VecT& translation)
+{
+    matrix_rotation_2D(m,angle);
+    matrix_set_translation_2D(m,translation);
+}
+
+/** 2D affine transform from a matrix and a translation */
+template < typename E,class A,class B,class L,class MatT,class VecT > void
+matrix_affine_transform_2D(
+    matrix<E,A,B,L>& m, const MatT& linear, const VecT& translation)
+{
+    matrix_linear_transform_2D(m, linear);
+    matrix_set_translation_2D(m,translation);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D affine from 2D affine
+//////////////////////////////////////////////////////////////////////////////
+
+/** Construct a 3D affine transform from a 2D affine transform */
+template < typename E, class A, class B, class L, class MatT > void
+matrix_3D_affine_from_2D_affine(matrix<E,A,B,L>& m, const MatT& affine_2D)
+{
+    typedef vector< E, fixed<2> > vector_type;
+
+    vector_type x = matrix_get_x_basis_vector_2D(affine_2D);
+    vector_type y = matrix_get_y_basis_vector_2D(affine_2D);
+    vector_type p = matrix_get_translation_2D(affine_2D);
+    
+    identity_transform(m);
+    
+    matrix_set_basis_vectors_2D(m,x,y);
+    matrix_set_translation(m,p);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// 3D affine from 3D affine
+//////////////////////////////////////////////////////////////////////////////
+
+/** Construct a 3D affine transform from another 3D affine transform */
+template < typename E, class A, class B, class L, class MatT > void
+matrix_3D_affine_from_3D_affine(matrix<E,A,B,L>& m, const MatT& affine_3D)
+{
+    typedef vector< E, fixed<3> > vector_type;
+
+    vector_type x = matrix_get_x_basis_vector(affine_3D);
+    vector_type y = matrix_get_y_basis_vector(affine_3D);
+    vector_type z = matrix_get_z_basis_vector(affine_3D);
+    vector_type p = matrix_get_translation(affine_3D);
+    
+    identity_transform(m);
+    
+    matrix_set_basis_vectors(m,x,y,z);
+    matrix_set_translation(m,p);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Matrix decomposition (scale->rotate->translate)
+//////////////////////////////////////////////////////////////////////////////
+
+/* 3x3 matrix version */
+template <
+    class MatT,
+    typename Real,
+    typename ME,
+    class MA,
+    class B,
+    class L,
+    typename VE,
+    class VA
+>
+void matrix_decompose_SRT(
+    const MatT& m,
+    Real& scale_x,
+    Real& scale_y,
+    Real& scale_z,
+    matrix<ME,MA,B,L>& rotation,
+    vector<VE,VA>& translation)
+{
+    typedef MatT matrix_type;
+    typedef typename matrix_type::value_type value_type;
+    typedef vector<value_type, fixed<3> > vector_type;
+
+    /* Checking */
+    detail::CheckMatAffine3D(m);
+    detail::CheckMatLinear3D(rotation);
+    
+    vector_type x, y, z;
+    matrix_get_basis_vectors(m, x, y, z);
+    
+    scale_x = x.length();
+    scale_y = y.length();
+    scale_z = z.length();
+    
+    x /= scale_x;
+    y /= scale_y;
+    z /= scale_z;
+    
+    matrix_set_basis_vectors(rotation, x, y, z);
+    translation = matrix_get_translation(m);
+}
+
+/* Quaternion version */
+template <
+    class MatT,
+    typename Real,
+    typename QE,
+    class QA,
+    class O,
+    class C,
+    typename VE,
+    class VA
+>
+void matrix_decompose_SRT(
+    const MatT& m,
+    Real& scale_x,
+    Real& scale_y,
+    Real& scale_z,
+    quaternion<QE,QA,O,C>& rotation,
+    vector<VE,VA>& translation)
+{
+    typedef MatT matrix_type;
+    typedef typename matrix_type::value_type value_type;
+    typedef matrix< value_type, fixed<3,3> > rotation_type;
+
+    rotation_type rotation_matrix;
+    matrix_decompose_SRT(
+        m, scale_x, scale_y, scale_z, rotation_matrix, translation);
+    quaternion_rotation_matrix(rotation, rotation_matrix);
+}
+
+/* Euler angle version */
+template < class MatT, typename Real, typename E, class A >
+void matrix_decompose_SRT(
+    const MatT& m,
+    Real& scale_x,
+    Real& scale_y,
+    Real& scale_z,
+    Real& angle_0,
+    Real& angle_1,
+    Real& angle_2,
+    EulerOrder order,
+    vector<E,A>& translation,
+    Real tolerance = epsilon<Real>::placeholder())
+{
+    typedef MatT matrix_type;
+    typedef typename matrix_type::value_type value_type;
+    typedef matrix< value_type, fixed<3,3> > rotation_type;
+
+    rotation_type rotation_matrix;
+    matrix_decompose_SRT(
+        m, scale_x, scale_y, scale_z, rotation_matrix, translation);
+    matrix_to_euler(
+        rotation_matrix, angle_0, angle_1, angle_2, order, tolerance);
+}
+
+/* Axis-angle version */
+template < class MatT, typename Real, typename E, class A >
+void matrix_decompose_SRT(
+    const MatT& m,
+    Real& scale_x,
+    Real& scale_y,
+    Real& scale_z,
+    vector<E,A>& axis,
+    Real& angle,
+    vector<E,A>& translation,
+    Real tolerance = epsilon<Real>::placeholder())
+{
+    typedef MatT matrix_type;
+    typedef typename matrix_type::value_type value_type;
+    typedef matrix< value_type, fixed<3,3> > rotation_type;
+
+    rotation_type rotation_matrix;
+    matrix_decompose_SRT(
+        m, scale_x, scale_y, scale_z, rotation_matrix, translation);
+    matrix_to_axis_angle(rotation_matrix, axis, angle, tolerance);
+}
+
+/* 2x2 matrix version, 2-d */
+template <
+    class MatT,
+    typename Real,
+    typename ME,
+    class MA,
+    class B,
+    class L,
+    typename VE,
+    class VA
+>
+void matrix_decompose_SRT_2D(
+    const MatT& m,
+    Real& scale_x,
+    Real& scale_y,
+    matrix<ME,MA,B,L>& rotation,
+    vector<VE,VA>& translation)
+{
+    typedef MatT matrix_type;
+    typedef typename matrix_type::value_type value_type;
+    typedef vector<value_type, fixed<2> > vector_type;
+
+    /* Checking */
+    detail::CheckMatAffine2D(m);
+    detail::CheckMatLinear2D(rotation);
+    
+    vector_type x, y;
+    matrix_get_basis_vectors_2D(m, x, y);
+    
+    scale_x = x.length();
+    scale_y = y.length();
+    
+    x /= scale_x;
+    y /= scale_y;
+    
+    matrix_set_basis_vectors_2D(rotation, x, y);
+    translation = matrix_get_translation_2D(m);
+}
+
+/* Angle version, 2-d */
+template < class MatT, typename Real, typename E, class A >
+void matrix_decompose_SRT_2D(
+    const MatT& m,
+    Real& scale_x,
+    Real& scale_y,
+    Real& angle,
+    vector<E,A>& translation)
+{
+    typedef MatT matrix_type;
+    typedef typename matrix_type::value_type value_type;
+    typedef matrix< value_type, fixed<2,2> > rotation_type;
+
+    rotation_type rotation_matrix;
+    matrix_decompose_SRT_2D(
+        m, scale_x, scale_y, rotation_matrix, translation);
+    angle = matrix_to_rotation_2D(rotation_matrix);
+}
+
+} // namespace cml
+
+#endif
diff --git a/src/cml/mathlib/matrix_translation.h b/src/cml/mathlib/matrix_translation.h
new file mode 100644 (file)
index 0000000..dadf4c5
--- /dev/null
@@ -0,0 +1,139 @@
+/* -*- 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_translation_h
+#define matrix_translation_h
+
+#include <cml/mathlib/checking.h>
+
+/* Functions for getting and setting the translation of a 3D or 2D affine
+ * transform.
+ */
+
+namespace cml {
+
+//////////////////////////////////////////////////////////////////////////////
+// Functions for setting the translation of a 3D or 2D affine transform matrix
+//////////////////////////////////////////////////////////////////////////////
+
+/** Set the translation of a 3D affine transform */
+template < typename E, class A, class B, class L > void
+matrix_set_translation(matrix<E,A,B,L>& m, E x, E y, E z)
+{
+    /* Checking */
+    detail::CheckMatAffine3D(m);
+    
+    m.set_basis_element(3,0,x);
+    m.set_basis_element(3,1,y);
+    m.set_basis_element(3,2,z);
+}
+
+/** Set the translation of a 3D affine transform with z set to 0 */
+template < typename E, class A, class B, class L > void
+matrix_set_translation(matrix<E,A,B,L>& m, E x, E y)
+{
+    typedef matrix<E,A,B,L> matrix_type;
+    typedef typename matrix_type::value_type value_type;
+    
+    matrix_set_translation(m, x, y, value_type(0));
+}
+
+/** Set the translation of a 3D affine transform from a 3D or 2D vector */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_set_translation(matrix<E,A,B,L>& m, const VecT& translation)
+{
+    /* Checking */
+    detail::CheckVec2Or3(translation);
+    
+    if (translation.size() == 3) {
+        matrix_set_translation(
+            m,translation[0], translation[1], translation[2]);
+    } else { // translation.size() == 2
+        matrix_set_translation(m, translation[0], translation[1]);
+    }
+}
+
+/** Set the translation of a 2D affine transform */
+template < typename E, class A, class B, class L > void
+matrix_set_translation_2D(matrix<E,A,B,L>& m, E x, E y)
+{
+    /* Checking */
+    detail::CheckMatAffine2D(m);
+    
+    m.set_basis_element(2,0,x);
+    m.set_basis_element(2,1,y);
+}
+
+/** Set the translation of a 2D affine transform from a 2D vector */
+template < typename E, class A, class B, class L, class VecT > void
+matrix_set_translation_2D(matrix<E,A,B,L>& m, const VecT& translation)
+{
+    /* Checking */
+    detail::CheckVec2(translation);
+    
+    matrix_set_translation_2D(m, translation[0], translation[1]);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Functions for getting the translation of a 3D or 2D affine transform matrix
+//////////////////////////////////////////////////////////////////////////////
+
+/** Get the translation of a 3D affine transform */
+template < class MatT > vector< typename MatT::value_type, fixed<3> >
+matrix_get_translation(const MatT& m)
+{
+    typedef typename MatT::value_type value_type;
+    typedef vector< value_type, fixed<3> > vector_type;
+
+    /* Checking */
+    detail::CheckMatAffine3D(m);
+    
+    return vector_type(
+        m.basis_element(3,0),
+        m.basis_element(3,1),
+        m.basis_element(3,2)
+    );
+}
+
+/** Get the translation of a 2D affine transform */
+template < class MatT > vector< typename MatT::value_type, fixed<2> >
+matrix_get_translation_2D(const MatT& m)
+{
+    typedef typename MatT::value_type value_type;
+    typedef vector< value_type, fixed<2> > vector_type;
+
+    /* Checking */
+    detail::CheckMatAffine2D(m);
+    
+    return vector_type(m.basis_element(2,0), m.basis_element(2,1));
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Function for getting the translation of a 3D view matrix
+//////////////////////////////////////////////////////////////////////////////
+
+/** Get the translation of a 3D affine transform */
+template < class MatT > vector< typename MatT::value_type, fixed<3> >
+matrix_get_view_translation(const MatT& m)
+{
+    typedef typename MatT::value_type value_type;
+    typedef vector< value_type, fixed<3> > vector_type;
+    
+    vector_type x, y, z;
+    matrix_get_basis_vectors(m,x,y,z);
+    vector_type p = matrix_get_translation(m);
+    return vector_type(-dot(p,x),-dot(p,y),-dot(p,z));
+}
+
+} // namespace cml
+
+#endif
diff --git a/src/cml/mathlib/misc.h b/src/cml/mathlib/misc.h
new file mode 100644 (file)
index 0000000..1017690
--- /dev/null
@@ -0,0 +1,210 @@
+/* -*- 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 misc_h
+#define misc_h
+
+#include <cml/mathlib/checking.h>
+
+/* A few miscellaneous functions and helper classes.
+ *
+ * @note: This is somewhat ad-hoc and will probably all be replaced in a future
+ * version of the CML (I don't think I even bothered to document these functions
+ * on the website).
+ */
+
+namespace cml {
+
+//////////////////////////////////////////////////////////////////////////////
+// N-d functions
+//////////////////////////////////////////////////////////////////////////////
+
+/** Return an N-d zero vector */
+template < size_t N >
+vector< double, fixed<N> > zero()
+{
+    typedef vector< double, fixed<N> > vector_type;
+
+    vector_type result;
+    result.zero();
+    return result;
+}
+
+/** Return an N-d cardinal axis by index */
+template < size_t N >
+vector< double, fixed<N> > axis(size_t i)
+{
+    /* Checking */
+    detail::CheckValidArg(i < N);
+
+    typedef vector< double, fixed<N> > vector_type;
+    vector_type result;
+    result.cardinal(i);
+    return result;
+}
+
+/** Return an NxM zero matrix */
+template < size_t N, size_t M >
+matrix< double, fixed<N,M>, row_basis, row_major > zero()
+{
+    typedef matrix< double, fixed<N,M>, row_basis, row_major > matrix_type;
+
+    matrix_type result;
+    result.zero();
+    return result;
+}
+
+/** Return an NxN identity matrix */
+template < size_t N >
+matrix< double, fixed<N,N>, row_basis, row_major > identity()
+{
+    typedef matrix< double, fixed<N,N>, row_basis, row_major > matrix_type;
+
+    matrix_type result;
+    result.identity();
+    return result;
+}
+
+/** Return an NxM identity transform */
+template < size_t N, size_t M >
+matrix< double, fixed<N,M>, row_basis, row_major > identity_transform()
+{
+    typedef matrix< double, fixed<N,M>, row_basis, row_major > matrix_type;
+
+    matrix_type result;
+    identity_transform(result);
+    return result;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Zero vector
+//////////////////////////////////////////////////////////////////////////////
+
+/** Return the 2D zero vector */
+inline vector< double, fixed<2> > zero_2D() {
+    return zero<2>();
+}
+
+/** Return the 3D zero vector */
+inline vector< double, fixed<3> > zero_3D() {
+    return zero<3>();
+}
+
+/** Return the 4D zero vector */
+inline vector< double, fixed<4> > zero_4D() {
+    return zero<4>();
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Cardinal axis
+//////////////////////////////////////////////////////////////////////////////
+
+/** Return a 2D cardinal axis by index */
+inline vector< double, fixed<2> > axis_2D(size_t i) {
+    return axis<2>(i);
+}
+
+/** Return a 3D cardinal axis by index */
+inline vector< double, fixed<3> > axis_3D(size_t i) {
+    return axis<3>(i);
+}
+
+/** Return a the 2D x cardinal axis */
+inline vector< double, fixed<2> > x_axis_2D() {
+    return axis_2D(0);
+}
+
+/** Return a the 2D y cardinal axis */
+inline vector< double, fixed<2> > y_axis_2D() {
+    return axis_2D(1);
+}
+
+/** Return a the 3D x cardinal axis */
+inline vector< double, fixed<3> > x_axis_3D() {
+    return axis_3D(0);
+}
+
+/** Return a the 3D y cardinal axis */
+inline vector< double, fixed<3> > y_axis_3D() {
+    return axis_3D(1);
+}
+
+/** Return a the 3D z cardinal axis */
+inline vector< double, fixed<3> > z_axis_3D() {
+    return axis_3D(2);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Zero matrix
+//////////////////////////////////////////////////////////////////////////////
+
+/** Return the 2x2 zero matrix */
+inline matrix< double, fixed<2,2>, row_basis, row_major > zero_2x2() {
+    return zero<2,2>();
+}
+
+/** Return the 3x3 zero matrix */
+inline matrix< double, fixed<3,3>, row_basis, row_major > zero_3x3() {
+    return zero<3,3>();
+}
+
+/** Return the 4x4 zero matrix */
+inline matrix< double, fixed<4,4>, row_basis, row_major > zero_4x4() {
+    return zero<4,4>();
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Identity matrix
+//////////////////////////////////////////////////////////////////////////////
+
+/** Return the 2x2 identity matrix */
+inline matrix< double, fixed<2,2>, row_basis, row_major > identity_2x2() {
+    return identity<2>();
+}
+
+/** Return the 3x3 identity matrix */
+inline matrix< double, fixed<3,3>, row_basis, row_major > identity_3x3() {
+    return identity<3>();
+}
+
+/** Return the 4x4 identity matrix */
+inline matrix< double, fixed<4,4>, row_basis, row_major > identity_4x4() {
+    return identity<4>();
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Identity transform matrix
+//////////////////////////////////////////////////////////////////////////////
+
+/** Return a 3x2 identity transform */
+inline matrix< double,fixed<3,2>,row_basis,row_major > identity_transform_3x2() {
+    return identity_transform<3,2>();
+}
+
+/** Return a 2x3 identity transform */
+inline matrix< double,fixed<2,3>,col_basis,col_major > identity_transform_2x3() {
+    return identity_transform<2,3>();
+}
+
+/** Return a 4x3 identity transform */
+inline matrix< double,fixed<4,3>,row_basis,row_major > identity_transform_4x3() {
+    return identity_transform<4,3>();
+}
+
+/** Return a 3x4 identity transform */
+inline matrix< double,fixed<3,4>,col_basis,col_major > identity_transform_3x4() {
+    return identity_transform<3,4>();
+}
+
+} // namespace cml
+
+#endif
diff --git a/src/cml/mathlib/picking.h b/src/cml/mathlib/picking.h
new file mode 100644 (file)
index 0000000..75d1697
--- /dev/null
@@ -0,0 +1,195 @@
+/* -*- 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 picking_h
+#define picking_h
+
+#include <cml/mathlib/projection.h>
+
+/* Functions for picking with rays, volumes, and drag-enclosed volumes. */
+
+namespace cml {
+
+/* Support function for extracting the near and far depth range values from
+ * a viewport matrix.
+  */
+namespace detail {
+
+// NOTE: Changed 'near' and 'far' to 'n' and 'f' to work around windows.h
+// 'near' and 'far' macros.
+
+template < class MatT, typename Real > void
+depth_range_from_viewport_matrix(const MatT& viewport, Real& n, Real& f)
+{
+    detail::CheckMatHomogeneous3D(viewport);
+    
+    n = viewport.basis_element(3,2);
+    f = viewport.basis_element(2,2) + n;
+}
+
+} // namespace detail
+
+/* Make a pick ray given screen coordinates and view, projection, and viewport
+ * matrices. The origin of the ray lies in the near plane of the frustum; the
+ * direction vector extends to the far plane if 'normalize' is false, and is
+ * made unit-length if 'normalize' is true (its default value).
+ *
+ * Note that the origin of the ray lies in the near plane rather than
+ * coinciding with the position of the virtual camera, as the latter gives
+ * incorrect results when the projection is orthographic.
+ *
+ * Note also that the screen y coordinate increases from bottom to top rather
+ * than top to bottom. If mouse coordinates are returned in window space where
+ * the y coordinate increases from top to bottom (as is often the case), the
+ * y value should be recomputed as 'y = <window height> - y' before being
+ * submitted to this function.
+ */
+
+template < class MatT_1, class MatT_2, class MatT_3, typename E, class A >
+void make_pick_ray(
+    E pick_x,
+    E pick_y,
+    const MatT_1& view,
+    const MatT_2& projection,
+    const MatT_3& viewport,
+    vector<E,A>& origin,
+    vector<E,A>& direction,
+    bool normalize = true)
+{
+    typedef vector<E,A> vector_type;
+    typedef typename vector_type::value_type value_type;
+
+    // NOTE: Changed 'near' and 'far' to 'n' and 'f' to work around
+    // windows.h 'near' and 'far' macros.
+    value_type n, f;
+    detail::depth_range_from_viewport_matrix(viewport, n, f);
+
+    origin =
+        unproject_point(
+            view,projection,viewport,vector_type(pick_x,pick_y,n)
+        );
+    direction =
+        unproject_point(
+            view,projection,viewport,vector_type(pick_x,pick_y,f)
+        ) - origin;
+    if (normalize) {
+        direction.normalize();
+    }
+}
+
+/* Make a pick volume given the screen coordinates of the center of the
+ * picking rect, the width and height of the picking rect, and view and
+ * projection matrices.
+ *
+ * The volume is loaded into the 'planes' array. The planes are of the form
+ * ax+by+cz+d = 0, and are in the order left, right, bottom, top, near, far.
+ *
+ * The z_clip argument should be either z_clip_neg_one or z_clip_zero, and
+ * should correspond to the near z-clipping range of the projection matrix
+ * argument.
+ *
+ * The 'normalize' argument indicates whether the output planes should be
+ * normalized; its default value is 'true'.
+ *
+ * Note that the screen y coordinate increases from bottom to top rather
+ * than top to bottom. If mouse coordinates are returned in window space where
+ * the y coordinate increases from top to bottom (as is often the case), the
+ * y value should be recomputed as 'y = <window height> - y' before being
+ * submitted to this function.
+ */
+
+template < class MatT_1, class MatT_2, typename Real >
+void make_pick_volume(
+    Real pick_x,
+    Real pick_y,
+    Real pick_width,
+    Real pick_height,
+    Real viewport_x,
+    Real viewport_y,
+    Real viewport_width,
+    Real viewport_height,
+    const MatT_1& view,
+    const MatT_2& projection,
+    Real planes[6][4],
+    ZClip z_clip,
+    bool normalize = true)
+{
+    // FIXME: Should be promoted type...
+    typedef matrix<
+        Real, fixed<4,4>,
+        typename MatT_1::basis_orient, typename MatT_1::layout >
+    matrix_type;
+    
+    matrix_type pick;
+    matrix_pick(
+        pick, pick_x, pick_y, pick_width, pick_height,
+        viewport_x, viewport_y, viewport_width, viewport_height
+    );
+    cml::extract_frustum_planes(
+        view,detail::matrix_concat_transforms_4x4(projection,pick),
+        planes,z_clip,normalize);
+}
+
+/* Make a pick volume given two opposite corners of a rectangle in screen
+ * space, and view and projection matrices. The corners of the screen rect
+ * need not be in any particular 'order' with regard to the values of the
+ * coordinates.
+ *
+ * The volume is loaded into the 'planes' array. The planes are of the form
+ * ax+by+cz+d = 0, and are in the order left, right, bottom, top, near, far.
+ *
+ * The z_clip argument should be either z_clip_neg_one or z_clip_zero, and
+ * should correspond to the near z-clipping range of the projection matrix
+ * argument.
+ *
+ * The 'normalize' argument indicates whether the output planes should be
+ * normalized; its default value is 'true'.
+ *
+ * Note that the screen y coordinate increases from bottom to top rather
+ * than top to bottom. If mouse coordinates are returned in window space where
+ * the y coordinate increases from top to bottom (as is often the case), the
+ * y value should be recomputed as 'y = <window height> - y' before being
+ * submitted to this function.
+ */
+
+template < class MatT_1, class MatT_2, typename Real >
+void make_pick_drag_volume(
+    Real pick_x1,
+    Real pick_y1,
+    Real pick_x2,
+    Real pick_y2,
+    Real viewport_x,
+    Real viewport_y,
+    Real viewport_width,
+    Real viewport_height,
+    const MatT_1& view,
+    const MatT_2& projection,
+    Real planes[6][4],
+    ZClip z_clip,
+    bool normalize = true)
+{
+    typedef Real value_type;
+
+    make_pick_volume(
+        (pick_x1+pick_x2)*value_type(.5),
+        (pick_y1+pick_y2)*value_type(.5),
+        std::fabs(pick_x2-pick_x1),
+        std::fabs(pick_y2-pick_y1),
+        viewport_x, viewport_y, viewport_width, viewport_height,
+        view, projection, planes, z_clip, normalize
+    );
+}
+
+} // namespace cml
+
+#endif
diff --git a/src/cml/mathlib/projection.h b/src/cml/mathlib/projection.h
new file mode 100644 (file)
index 0000000..7996851
--- /dev/null
@@ -0,0 +1,142 @@
+/* -*- 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 projection_h
+#define projection_h
+
+#include <cml/mathlib/matrix_concat.h>
+#include <cml/mathlib/vector_transform.h>
+
+/* Functions for projection and 'unprojection' of points in 3D. */
+
+namespace cml {
+
+namespace detail {
+
+template < typename E > void
+divide_by_w(vector< E,fixed<4> >& v) {
+    v *= E(1) / v[3];
+}
+
+} // namespace detail
+
+/* Project a point to screen space using the given model, view, projection,
+ * and viewport matrices. The z value of the returned point is a depth value
+ * in the range specified by the viewport matrix.
+ */
+
+template <class MatT_1, class MatT_2, class MatT_3, class MatT_4, class VecT>
+vector< typename VecT::value_type, fixed<3> > project_point(
+    const MatT_1& model,
+    const MatT_2& view,
+    const MatT_3& projection,
+    const MatT_4& viewport,
+    const VecT& p)
+{
+    return project_point(
+        detail::matrix_concat_transforms_4x4(model,view),
+        projection,
+        viewport,
+        p
+    );
+}
+
+/* Project a point to screen space using the given modelview, projection, and
+ * viewport matrices. The z value of the returned point is a depth value in
+ * the range specified by the viewport matrix.
+ */
+
+template < class MatT_1, class MatT_2, class MatT_3, class VecT >
+vector< typename VecT::value_type, fixed<3> > project_point(
+    const MatT_1& modelview,
+    const MatT_2& projection,
+    const MatT_3& viewport,
+    const VecT& p)
+{
+    typedef vector< typename VecT::value_type, fixed<3> > vector3_type;
+    typedef vector< typename VecT::value_type, fixed<4> > vector4_type;
+    typedef typename vector3_type::value_type value_type;
+
+    detail::CheckVec3(p);
+
+    vector4_type result = transform_vector_4D(
+        detail::matrix_concat_transforms_4x4(
+            modelview,
+            detail::matrix_concat_transforms_4x4(
+                projection,
+                viewport
+            )
+        ),
+        vector4_type(p[0],p[1],p[2],value_type(1))
+    );
+    detail::divide_by_w(result);
+    return vector3_type(result[0],result[1],result[2]);
+}
+
+/* 'Unproject' a point from screen space using the given model, view,
+ * projection, and viewport matrices. The z value of the input point is a
+ * depth value in the range specified by the viewport matrix.
+ */
+
+template <class MatT_1, class MatT_2, class MatT_3, class MatT_4, class VecT>
+vector< typename VecT::value_type, fixed<3> > unproject_point(
+    const MatT_1& model,
+    const MatT_2& view,
+    const MatT_3& projection,
+    const MatT_4& viewport,
+    const VecT& p)
+{
+    return unproject_point(
+        detail::matrix_concat_transforms_4x4(model,view),
+        projection,
+        viewport,
+        p
+    );
+}
+
+/* 'Unproject' a point from screen space using the given modelview,
+ * projection, and viewport matrices. The z value of the input point is a
+ * depth value in the range specified by the viewport matrix.
+ */
+template < class MatT_1, class MatT_2, class MatT_3, class VecT >
+vector< typename VecT::value_type, fixed<3> > unproject_point(
+    const MatT_1& modelview,
+    const MatT_2& projection,
+    const MatT_3& viewport,
+    const VecT& p)
+{
+    typedef vector< typename VecT::value_type, fixed<3> > vector3_type;
+    typedef vector< typename VecT::value_type, fixed<4> > vector4_type;
+    typedef typename vector3_type::value_type value_type;
+
+    detail::CheckVec3(p);
+
+    vector4_type result = transform_vector_4D(
+        inverse(
+            detail::matrix_concat_transforms_4x4(
+                modelview,
+                detail::matrix_concat_transforms_4x4(
+                    projection,
+                    viewport
+                )
+            )
+        ),
+        vector4_type(p[0],p[1],p[2],value_type(1))
+    );
+    detail::divide_by_w(result);
+    return vector3_type(result[0],result[1],result[2]);
+}
+
+} // namespace cml
+
+#endif
diff --git a/src/cml/mathlib/quaternion_basis.h b/src/cml/mathlib/quaternion_basis.h
new file mode 100644 (file)
index 0000000..5c4633e
--- /dev/null
@@ -0,0 +1,89 @@
+/* -*- 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 quaternion_basis_h
+#define quaternion_basis_h
+
+#include <cml/mathlib/checking.h>
+
+/* Functions for getting the basis vectors of a quaternion rotation. */
+
+namespace cml {
+
+/** Get the i'th basis vector of a quaternion rotation */
+template < class QuatT > vector< typename QuatT::value_type, fixed<3> >
+quaternion_get_basis_vector(const QuatT& q, size_t i)
+{
+    typedef QuatT quaternion_type;
+    typedef typename quaternion_type::value_type value_type;
+    typedef typename quaternion_type::order_type order_type;
+    typedef vector< value_type, fixed<3> > vector_type;
+
+    /* Checking */
+    detail::CheckQuat(q);
+    detail::CheckIndex3(i);
+
+    size_t j, k;
+    cyclic_permutation(i, i, j, k);
+    
+    /* @todo: Clean this up. */
+    const size_t W = order_type::W;
+    const size_t I = order_type::X + i;
+    const size_t J = order_type::X + j;
+    const size_t K = order_type::X + k;
+    
+    value_type j2 = q[J] + q[J];
+    value_type k2 = q[K] + q[K];
+    
+    /* @todo: use set_permuted() for the following when available. */
+
+    vector_type result;
+    result[i] = value_type(1) - q[J] * j2 - q[K] * k2;
+    result[j] = q[I] * j2 + q[W] * k2;
+    result[k] = q[I] * k2 - q[W] * j2;
+    return result;
+}
+
+/** Get the x basis vector of a quaternion rotation */
+template < class QuatT > vector< typename QuatT::value_type, fixed<3> >
+quaternion_get_x_basis_vector(const QuatT& q) {
+    return quaternion_get_basis_vector(q,0);
+}
+
+/** Get the y basis vector of a quaternion rotation */
+template < class QuatT > vector< typename QuatT::value_type, fixed<3> >
+quaternion_get_y_basis_vector(const QuatT& q) {
+    return quaternion_get_basis_vector(q,1);
+}
+
+/** Get the z basis vector of a quaternion rotation */
+template < class QuatT > vector< typename QuatT::value_type, fixed<3> >
+quaternion_get_z_basis_vector(const QuatT& q) {
+    return quaternion_get_basis_vector(q,2);
+}
+
+/** Get the basis vectors of a quaternion rotation */
+template < class QuatT, typename E, class A > void
+quaternion_get_basis_vectors(
+    const QuatT& q,
+    vector<E,A>& x,
+    vector<E,A>& y,
+    vector<E,A>& z)
+{
+    x = quaternion_get_x_basis_vector(q);
+    y = quaternion_get_y_basis_vector(q);
+    z = quaternion_get_z_basis_vector(q);
+}
+
+} // namespace cml
+
+#endif
diff --git a/src/cml/mathlib/quaternion_rotation.h b/src/cml/mathlib/quaternion_rotation.h
new file mode 100644 (file)
index 0000000..2e8c0e9
--- /dev/null
@@ -0,0 +1,635 @@
+/* -*- 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 quaternion_rotation_h
+#define quaternion_rotation_h
+
+#include <cml/mathlib/checking.h>
+
+/* Functions related to quaternion rotations.
+ *
+ * Note: A number of these functions simply wrap calls to the corresponding
+ * matrix functions. Some of them (the 'aim-at' and 'align' functions in
+ * particular) might be considered a bit superfluous, since the resulting
+ * quaternion will most likely be converted to a matrix at some point anyway.
+ * However, they're included here for completeness, and for convenience in
+ * cases where a quaternion is being used as the primary rotation
+ * representation.
+*/
+
+namespace cml {
+
+//////////////////////////////////////////////////////////////////////////////
+// Rotation about world axes
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a quaternion representing a rotation about the given world axis */
+template < class E, class A, class O, class C > void
+quaternion_rotation_world_axis(quaternion<E,A,O,C>& q, size_t axis, E angle)
+{
+    typedef quaternion<E,A,O,C> quaternion_type;
+    typedef typename quaternion_type::value_type value_type;
+    typedef typename quaternion_type::order_type order_type;
+
+    /* Checking */
+    detail::CheckIndex3(axis);
+
+    q.identity();
+
+    const size_t W = order_type::W;
+    const size_t I = order_type::X + axis;
+    
+    angle *= value_type(.5);
+    q[I] = std::sin(angle);
+    q[W] = std::cos(angle);
+}
+
+/** Build a quaternion representing a rotation about the world x axis */
+template < class E, class A, class O, class C > void
+quaternion_rotation_world_x(quaternion<E,A,O,C>& q, E angle) {
+    quaternion_rotation_world_axis(q,0,angle);
+}
+
+/** Build a quaternion representing a rotation about the world y axis */
+template < class E, class A, class O, class C > void
+quaternion_rotation_world_y(quaternion<E,A,O,C>& q, E angle) {
+    quaternion_rotation_world_axis(q,1,angle);
+}
+
+/** Build a quaternion representing a rotation about the world z axis */
+template < class E, class A, class O, class C > void
+quaternion_rotation_world_z(quaternion<E,A,O,C>& q, E angle) {
+    quaternion_rotation_world_axis(q,2,angle);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Rotation from an axis-angle pair
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a quaternion from an axis-angle pair */
+template < class E, class A, class O, class C, class VecT > void
+quaternion_rotation_axis_angle(
+    quaternion<E,A,O,C>& q, const VecT& axis, E angle)
+{
+    typedef quaternion<E,A,O,C> quaternion_type;
+    typedef typename quaternion_type::value_type value_type;
+    typedef typename quaternion_type::order_type order_type;
+    
+    /* Checking */
+    detail::CheckVec3(axis);
+
+    enum {
+        W = order_type::W,
+        X = order_type::X,
+        Y = order_type::Y,
+        Z = order_type::Z
+    };
+    
+    angle *= value_type(.5);
+    
+    /* @todo: If and when we have a set() function that takes a vector and a
+     * scalar, this can be written as:
+     *
+     * q.set(std::cos(angle), axis * std::sin(angle));
+     *
+     * In which case the enum will also not be necessary.
+     */
+    
+    q[W] = std::cos(angle);
+    value_type s = std::sin(angle);
+    q[X] = axis[0] * s;
+    q[Y] = axis[1] * s;
+    q[Z] = axis[2] * s;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Rotation from a matrix
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a quaternion from a rotation matrix */
+template < class E, class A, class O, class C, class MatT > void
+quaternion_rotation_matrix(quaternion<E,A,O,C>& q, const MatT& m)
+{
+    typedef quaternion<E,A,O,C> quaternion_type;
+    typedef typename quaternion_type::value_type value_type;
+    typedef typename quaternion_type::order_type order_type;
+
+    /* Checking */
+    detail::CheckMatLinear3D(m);
+
+    enum {
+        W = order_type::W,
+        X = order_type::X,
+        Y = order_type::Y,
+        Z = order_type::Z
+    };
+
+    value_type tr = trace_3x3(m);
+    if (tr >= value_type(0)) {
+        q[W] = std::sqrt(tr + value_type(1)) * value_type(.5);
+        value_type s = value_type(.25) / q[W];
+        q[X] = (m.basis_element(1,2) - m.basis_element(2,1)) * s;
+        q[Y] = (m.basis_element(2,0) - m.basis_element(0,2)) * s;
+        q[Z] = (m.basis_element(0,1) - m.basis_element(1,0)) * s;
+    } else {
+        size_t largest_diagonal_element =
+            index_of_max(
+                m.basis_element(0,0),
+                m.basis_element(1,1),
+                m.basis_element(2,2)
+            );
+        size_t i, j, k;
+        cyclic_permutation(largest_diagonal_element, i, j, k);
+        const size_t I = X + i;
+        const size_t J = X + j;
+        const size_t K = X + k;
+        q[I] =
+            std::sqrt(
+                m.basis_element(i,i) -
+                m.basis_element(j,j) -
+                m.basis_element(k,k) +
+                value_type(1)
+            ) * value_type(.5);
+        value_type s = value_type(.25) / q[I];
+        q[J] = (m.basis_element(i,j) + m.basis_element(j,i)) * s;
+        q[K] = (m.basis_element(i,k) + m.basis_element(k,i)) * s;
+        q[W] = (m.basis_element(j,k) - m.basis_element(k,j)) * s;
+    }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Rotation from Euler angles
+//////////////////////////////////////////////////////////////////////////////
+
+/** Build a quaternion from an Euler-angle triple */
+template < class E, class A, class O, class C > void
+quaternion_rotation_euler(
+    quaternion<E,A,O,C>& q, E angle_0, E angle_1, E angle_2,
+    EulerOrder order)
+{
+    typedef quaternion<E,A,O,C> quaternion_type;
+    typedef typename quaternion_type::value_type value_type;
+    typedef typename quaternion_type::order_type order_type;
+
+    size_t i, j, k;
+    bool odd, repeat;
+    detail::unpack_euler_order(order, i, j, k, odd, repeat);
+    
+    const size_t W = order_type::W;
+    const size_t I = order_type::X + i;
+    const size_t J = order_type::X + j;
+    const size_t K = order_type::X + k;
+
+    if (odd) {
+        angle_1 = -angle_1;
+    }
+
+    angle_0 *= value_type(.5);
+    angle_1 *= value_type(.5);
+    angle_2 *= value_type(.5);
+    
+    value_type s0 = std::sin(angle_0);
+    value_type c0 = std::cos(angle_0);
+    value_type s1 = std::sin(angle_1);
+    value_type c1 = std::cos(angle_1);
+    value_type s2 = std::sin(angle_2);
+    value_type c2 = std::cos(angle_2);
+    
+    value_type s0s2 = s0 * s2;
+    value_type s0c2 = s0 * c2;
+    value_type c0s2 = c0 * s2;
+    value_type c0c2 = c0 * c2;
+
+    if (repeat) {
+        q[I] = c1 * (c0s2 + s0c2);
+        q[J] = s1 * (c0c2 + s0s2);
+        q[K] = s1 * (c0s2 - s0c2);
+        q[W] = c1 * (c0c2 - s0s2);
+    } else {
+        q[I] = c1 * s0c2 - s1 * c0s2;
+        q[J] = c1 * s0s2 + s1 * c0c2;
+        q[K] = c1 * c0s2 - s1 * s0c2;
+        q[W] = c1 * c0c2 + s1 * s0s2;
+    }
+    if (odd) {
+        q[J] = -q[J];
+    }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Rotation to align with a vector, multiple vectors, or the view plane
+//////////////////////////////////////////////////////////////////////////////
+
+/** See vector_ortho.h for details */
+template < typename E,class A,class O,class C,class VecT_1,class VecT_2 > void
+quaternion_rotation_align(
+    quaternion<E,A,O,C>& q,
+    const VecT_1& align,
+    const VecT_2& reference,
+    bool normalize = true,
+    AxisOrder order = axis_order_zyx)
+{
+    typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
+    
+    matrix_type m;
+    matrix_rotation_align(m,align,reference,normalize,order);
+    quaternion_rotation_matrix(q,m);
+}
+
+/** See vector_ortho.h for details */
+template < typename E, class A, class O, class C, class VecT > void
+quaternion_rotation_align(quaternion<E,A,O,C>& q, const VecT& align,
+    bool normalize = true, AxisOrder order = axis_order_zyx)
+{
+    typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
+    
+    matrix_type m;
+    matrix_rotation_align(m,align,normalize,order);
+    quaternion_rotation_matrix(q,m);
+}
+
+/** See vector_ortho.h for details */
+template < typename E,class A,class O,class C,class VecT_1,class VecT_2 > void
+quaternion_rotation_align_axial(quaternion<E,A,O,C>& q, const VecT_1& align,
+    const VecT_2& axis, bool normalize = true,
+    AxisOrder order = axis_order_zyx)
+{
+    typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
+    
+    matrix_type m;
+    matrix_rotation_align_axial(m,align,axis,normalize,order);
+    quaternion_rotation_matrix(q,m);
+}
+
+/** See vector_ortho.h for details */
+template < typename E, class A, class O, class C, class MatT > void
+quaternion_rotation_align_viewplane(
+    quaternion<E,A,O,C>& q,
+    const MatT& view_matrix,
+    Handedness handedness,
+    AxisOrder order = axis_order_zyx)
+{
+    typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
+    
+    matrix_type m;
+    matrix_rotation_align_viewplane(m,view_matrix,handedness,order);
+    quaternion_rotation_matrix(q,m);
+}
+
+/** See vector_ortho.h for details */
+template < typename E, class A, class O, class C, class MatT > void
+quaternion_rotation_align_viewplane_LH(
+    quaternion<E,A,O,C>& q,
+    const MatT& view_matrix,
+    AxisOrder order = axis_order_zyx)
+{
+    typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
+    
+    matrix_type m;
+    matrix_rotation_align_viewplane_LH(m,view_matrix,order);
+    quaternion_rotation_matrix(q,m);
+}
+
+/** See vector_ortho.h for details */
+template < typename E, class A, class O, class C, class MatT > void
+quaternion_rotation_align_viewplane_RH(
+    quaternion<E,A,O,C>& q,
+    const MatT& view_matrix,
+    AxisOrder order = axis_order_zyx)
+{
+    typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
+    
+    matrix_type m;
+    matrix_rotation_align_viewplane_RH(m,view_matrix,order);
+    quaternion_rotation_matrix(q,m);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Rotation to aim at a target
+//////////////////////////////////////////////////////////////////////////////
+
+/** See vector_ortho.h for details */
+template < typename E, class A, class O, class C,
+    class VecT_1, class VecT_2, class VecT_3 > void
+quaternion_rotation_aim_at(
+    quaternion<E,A,O,C>& q,
+    const VecT_1& pos,
+    const VecT_2& target,
+    const VecT_3& reference,
+    AxisOrder order = axis_order_zyx)
+{
+    typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
+    
+    matrix_type m;
+    matrix_rotation_aim_at(m,pos,target,reference,order);
+    quaternion_rotation_matrix(q,m);
+}
+
+/** See vector_ortho.h for details */
+template < typename E, class A, class O, class C,
+    class VecT_1, class VecT_2 > void
+quaternion_rotation_aim_at(
+    quaternion<E,A,O,C>& q,
+    const VecT_1& pos,
+    const VecT_2& target,
+    AxisOrder order = axis_order_zyx)
+{
+    typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
+    
+    matrix_type m;
+    matrix_rotation_aim_at(m,pos,target,order);
+    quaternion_rotation_matrix(q,m);
+}
+
+/** See vector_ortho.h for details */
+template < typename E, class A, class O, class C,
+    class VecT_1, class VecT_2, class VecT_3 > void
+quaternion_rotation_aim_at_axial(
+    quaternion<E,A,O,C>& q,
+    const VecT_1& pos,
+    const VecT_2& target,
+    const VecT_3& axis,
+    AxisOrder order = axis_order_zyx)
+{
+    typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
+    
+    matrix_type m;
+    matrix_rotation_aim_at_axial(m,pos,target,axis,order);
+    quaternion_rotation_matrix(q,m);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Relative rotation about world axes
+//////////////////////////////////////////////////////////////////////////////
+
+/* Rotate a quaternion about the given world axis */
+template < class E, class A, class O, class C > void
+quaternion_rotate_about_world_axis(quaternion<E,A,O,C>& q,size_t axis,E angle)
+{
+    typedef quaternion<E,A,O,C> quaternion_type;
+    typedef typename quaternion_type::value_type value_type;
+    typedef typename quaternion_type::order_type order_type;
+
+    /* Checking */
+    detail::CheckIndex3(axis);
+
+    size_t i, j, k;
+    cyclic_permutation(axis, i, j, k);
+    
+    const size_t W = order_type::W;
+    const size_t I = order_type::X + i;
+    const size_t J = order_type::X + j;
+    const size_t K = order_type::X + k;
+    
+    angle *= value_type(.5);
+    value_type s = value_type(std::sin(angle));
+    value_type c = value_type(std::cos(angle));
+
+    quaternion_type result;
+    result[I] = c * q[I] + s * q[W];
+    result[J] = c * q[J] - s * q[K];
+    result[K] = c * q[K] + s * q[J];
+    result[W] = c * q[W] - s * q[I];
+    q = result;
+}
+
+/* Rotate a quaternion about the world x axis */
+template < class E, class A, class O, class C > void
+quaternion_rotate_about_world_x(quaternion<E,A,O,C>& q, E angle) {
+    quaternion_rotate_about_world_axis(q,0,angle);
+}
+
+/* Rotate a quaternion about the world y axis */
+template < class E, class A, class O, class C > void
+quaternion_rotate_about_world_y(quaternion<E,A,O,C>& q, E angle) {
+    quaternion_rotate_about_world_axis(q,1,angle);
+}
+
+/* Rotate a quaternion about the world z axis */
+template < class E, class A, class O, class C > void
+quaternion_rotate_about_world_z(quaternion<E,A,O,C>& q, E angle) {
+    quaternion_rotate_about_world_axis(q,2,angle);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Relative rotation about local axes
+//////////////////////////////////////////////////////////////////////////////
+
+/* Rotate a quaternion about the given local axis */
+template < class E, class A, class O, class C > void
+quaternion_rotate_about_local_axis(quaternion<E,A,O,C>& q,size_t axis,E angle)
+{
+    typedef quaternion<E,A,O,C> quaternion_type;
+    typedef typename quaternion_type::value_type value_type;
+    typedef typename quaternion_type::order_type order_type;
+
+    /* Checking */
+    detail::CheckIndex3(axis);
+
+    size_t i, j, k;
+    cyclic_permutation(axis, i, j, k);
+    
+    const size_t W = order_type::W;
+    const size_t I = order_type::X + i;
+    const size_t J = order_type::X + j;
+    const size_t K = order_type::X + k;
+    
+    angle *= value_type(.5);
+    value_type s = value_type(std::sin(angle));
+    value_type c = value_type(std::cos(angle));
+
+    quaternion_type result;
+    result[I] = c * q[I] + s * q[W];
+    result[J] = c * q[J] + s * q[K];
+    result[K] = c * q[K] - s * q[J];
+    result[W] = c * q[W] - s * q[I];
+    q = result;
+}
+
+/* Rotate a quaternion about its local x axis */
+template < class E, class A, class O, class C > void
+quaternion_rotate_about_local_x(quaternion<E,A,O,C>& q, E angle) {
+    quaternion_rotate_about_local_axis(q,0,angle);
+}
+
+/* Rotate a quaternion about its local y axis */
+template < class E, class A, class O, class C > void
+quaternion_rotate_about_local_y(quaternion<E,A,O,C>& q, E angle) {
+    quaternion_rotate_about_local_axis(q,1,angle);
+}
+
+/* Rotate a quaternion about its local z axis */
+template < class E, class A, class O, class C > void
+quaternion_rotate_about_local_z(quaternion<E,A,O,C>& q, E angle) {
+    quaternion_rotate_about_local_axis(q,2,angle);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Rotation from vector to vector
+//////////////////////////////////////////////////////////////////////////////
+
+/* http://www.martinb.com/maths/algebra/vectors/angleBetween/index.htm. */
+
+/** Build a quaternion to rotate from one vector to another */
+template < class E,class A,class O,class C,class VecT_1,class VecT_2 > void
+quaternion_rotation_vec_to_vec(
+    quaternion<E,A,O,C>& q,
+    const VecT_1& v1,
+    const VecT_2& v2,
+    bool unit_length_vectors = false)
+{
+    typedef quaternion<E,A,O,C> quaternion_type;
+    typedef typename quaternion_type::value_type value_type;
+    typedef vector< value_type, fixed<3> > vector_type;
+    
+    /* Checking handled by cross() */
+
+    /* @todo: If at some point quaternion<> has a set() function that takes a
+     * vector and a scalar, this can then be written as:
+     *
+     * if (...) {
+     *     q.set(value_type(1)+dot(v1,v2), cross(v1,v2));
+     * } else {
+     *     q.set(std::sqrt(...)+dot(v1,v2), cross(v1,v2));
+     * }
+     */
+     
+    vector_type c = cross(v1,v2);
+    if (unit_length_vectors) {
+        q = quaternion_type(value_type(1) + dot(v1,v2), c.data());
+    } else {
+        q = quaternion_type(
+            std::sqrt(v1.length_squared() * v2.length_squared()) + dot(v1,v2),
+            c/*.data()*/
+        );
+    }
+    q.normalize();
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Scale the angle of a rotation matrix
+//////////////////////////////////////////////////////////////////////////////
+
+template < typename E, class A, class O, class C > void
+quaternion_scale_angle(quaternion<E,A,O,C>& q, E t,
+    E tolerance = epsilon<E>::placeholder())
+{
+    typedef vector< E,fixed<3> > vector_type;
+    typedef typename vector_type::value_type value_type;
+    
+    vector_type axis;
+    value_type angle;
+    quaternion_to_axis_angle(q, axis, angle, tolerance);
+    quaternion_rotation_axis_angle(q, axis, angle * t);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Support functions for uniform handling of pos- and neg-cross quaternions
+//////////////////////////////////////////////////////////////////////////////
+
+namespace detail {
+
+/** Concatenate two quaternions in the order q1->q2 */
+template < class QuatT_1, class QuatT_2 >
+typename et::QuaternionPromote2<QuatT_1,QuatT_2>::temporary_type
+quaternion_rotation_difference(
+    const QuatT_1& q1, const QuatT_2& q2, positive_cross)
+{
+    return q2 * conjugate(q1);
+}
+
+/** Concatenate two quaternions in the order q1->q2 */
+template < class QuatT_1, class QuatT_2 >
+typename et::QuaternionPromote2<QuatT_1,QuatT_2>::temporary_type
+quaternion_rotation_difference(
+    const QuatT_1& q1, const QuatT_2& q2, negative_cross)
+{
+    return conjugate(q1) * q2;
+}
+
+} // namespace detail
+
+//////////////////////////////////////////////////////////////////////////////
+// Quaternions rotation difference
+//////////////////////////////////////////////////////////////////////////////
+
+/** Return the rotational 'difference' between two quaternions */
+template < class QuatT_1, class QuatT_2 >
+typename et::QuaternionPromote2<QuatT_1,QuatT_2>::temporary_type
+quaternion_rotation_difference(const QuatT_1& q1, const QuatT_2& q2) {
+    return detail::quaternion_rotation_difference(
+        q1, q2, typename QuatT_1::cross_type());
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Conversions
+//////////////////////////////////////////////////////////////////////////////
+
+/** Convert a quaternion to an axis-angle pair */
+template < class QuatT, typename E, class A > void
+quaternion_to_axis_angle(
+    const QuatT& q,
+    vector<E,A>& axis,
+    E& angle,
+    E tolerance = epsilon<E>::placeholder())
+{
+    typedef QuatT quaternion_type;
+    typedef typename quaternion_type::value_type value_type;
+    typedef typename quaternion_type::order_type order_type;
+    
+    /* Checking */
+    detail::CheckQuat(q);
+
+    axis = q.imaginary();
+    value_type l = length(axis);
+    if (l > tolerance) {
+        axis /= l;
+        angle = value_type(2) * std::atan2(l,q.real());
+    } else {
+        axis.zero();
+        angle = value_type(0);
+    }
+}
+
+/** Convert a quaternion to an Euler-angle triple
+ *
+ * Note: I've implemented direct quaternion-to-Euler conversion, but as far as
+ * I can tell it more or less reduces to converting the quaternion to a matrix
+ * as you go. The direct method is a little more efficient in that it doesn't
+ * require a temporary and only the necessary matrix elements need be
+ * computed. However, the implementation is complex and there's considerable
+ * opportunity for error, so from a development and debugging standpoint I
+ * think it's better to just perform the conversion via matrix_to_euler(),
+ * which is already known to be correct.
+*/
+
+template < class QuatT, typename Real > void
+quaternion_to_euler(
+    const QuatT& q,
+    Real& angle_0,
+    Real& angle_1,
+    Real& angle_2,
+    EulerOrder order,
+    Real tolerance = epsilon<Real>::placeholder())
+{
+    typedef QuatT quaternion_type;
+    typedef typename quaternion_type::value_type value_type;
+    typedef matrix< value_type,fixed<3,3>,row_basis,row_major > matrix_type;
+
+    matrix_type m;
+    matrix_rotation_quaternion(m, q);
+    matrix_to_euler(m, angle_0, angle_1, angle_2, order, tolerance);
+}
+
+} // namespace cml
+
+#endif
diff --git a/src/cml/mathlib/typedef.h b/src/cml/mathlib/typedef.h
new file mode 100644 (file)
index 0000000..f34843e
--- /dev/null
@@ -0,0 +1,108 @@
+/* -*- 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 typedef_h
+#define typedef_h
+
+#include <cml/vector.h>
+#include <cml/matrix.h>
+#include <cml/quaternion.h>
+#include <cml/constants.h>
+#include <cml/mathlib/epsilon.h>
+
+namespace cml {
+
+/* fixed-size vectors */
+typedef vector< int,    fixed<2> > vector2i;
+typedef vector< float,  fixed<2> > vector2f;
+typedef vector< double, fixed<2> > vector2d;
+
+typedef vector< int,    fixed<3> > vector3i;
+typedef vector< float,  fixed<3> > vector3f;
+typedef vector< double, fixed<3> > vector3d;
+
+typedef vector< int,    fixed<4> > vector4i;
+typedef vector< float,  fixed<4> > vector4f;
+typedef vector< double, fixed<4> > vector4d;
+
+/* fixed-size matrices */
+typedef matrix< int,    fixed<2,2>, row_basis, row_major > matrix22i_r;
+typedef matrix< int,    fixed<2,2>, col_basis, col_major > matrix22i_c;
+typedef matrix< float,  fixed<2,2>, row_basis, row_major > matrix22f_r;
+typedef matrix< float,  fixed<2,2>, col_basis, col_major > matrix22f_c;
+typedef matrix< double, fixed<2,2>, row_basis, row_major > matrix22d_r;
+typedef matrix< double, fixed<2,2>, col_basis, col_major > matrix22d_c;
+
+typedef matrix< int,    fixed<3,3>, row_basis, row_major > matrix33i_r;
+typedef matrix< int,    fixed<3,3>, col_basis, col_major > matrix33i_c;
+typedef matrix< float,  fixed<3,3>, row_basis, row_major > matrix33f_r;
+typedef matrix< float,  fixed<3,3>, col_basis, col_major > matrix33f_c;
+typedef matrix< double, fixed<3,3>, row_basis, row_major > matrix33d_r;
+typedef matrix< double, fixed<3,3>, col_basis, col_major > matrix33d_c;
+
+typedef matrix< int,    fixed<4,4>, row_basis, row_major > matrix44i_r;
+typedef matrix< int,    fixed<4,4>, col_basis, col_major > matrix44i_c;
+typedef matrix< float,  fixed<4,4>, row_basis, row_major > matrix44f_r;
+typedef matrix< float,  fixed<4,4>, col_basis, col_major > matrix44f_c;
+typedef matrix< double, fixed<4,4>, row_basis, row_major > matrix44d_r;
+typedef matrix< double, fixed<4,4>, col_basis, col_major > matrix44d_c;
+
+typedef matrix< int,    fixed<3,2>, row_basis, row_major > matrix32i_r;
+typedef matrix< float,  fixed<3,2>, row_basis, row_major > matrix32f_r;
+typedef matrix< double, fixed<3,2>, row_basis, row_major > matrix32d_r;
+
+typedef matrix< int,    fixed<2,3>, col_basis, col_major > matrix23i_c;
+typedef matrix< float,  fixed<2,3>, col_basis, col_major > matrix23f_c;
+typedef matrix< double, fixed<2,3>, col_basis, col_major > matrix23d_c;
+
+typedef matrix< int,    fixed<4,3>, row_basis, row_major > matrix43i_r;
+typedef matrix< float,  fixed<4,3>, row_basis, row_major > matrix43f_r;
+typedef matrix< double, fixed<4,3>, row_basis, row_major > matrix43d_r;
+
+typedef matrix< int,    fixed<3,4>, col_basis, col_major > matrix34i_c;
+typedef matrix< float,  fixed<3,4>, col_basis, col_major > matrix34f_c;
+typedef matrix< double, fixed<3,4>, col_basis, col_major > matrix34d_c;
+
+/* quaternions */
+typedef quaternion<float, fixed<>,vector_first,negative_cross>
+    quaternionf_n;
+typedef quaternion<float, fixed<>,vector_first,positive_cross>
+    quaternionf_p;
+typedef quaternion<double,fixed<>,vector_first,negative_cross>
+    quaterniond_n;
+typedef quaternion<double,fixed<>,vector_first,positive_cross>
+    quaterniond_p;
+
+/* dynamically resizable vectors */
+typedef vector< int,    dynamic<> > vectori;
+typedef vector< float,  dynamic<> > vectorf;
+typedef vector< double, dynamic<> > vectord;
+
+/* dynamically resizable matrices */
+typedef matrix< int,    dynamic<>, row_basis, row_major > matrixi_r;
+typedef matrix< int,    dynamic<>, col_basis, col_major > matrixi_c;
+typedef matrix< float,  dynamic<>, row_basis, row_major > matrixf_r;
+typedef matrix< float,  dynamic<>, col_basis, col_major > matrixf_c;
+typedef matrix< double, dynamic<>, row_basis, row_major > matrixd_r;
+typedef matrix< double, dynamic<>, col_basis, col_major > matrixd_c;
+
+/* constants */
+typedef constants<float>  constantsf;
+typedef constants<double> constantsd;
+
+/* epsilon/tolerance values (placeholder) */
+typedef epsilon<float>  epsilonf;
+typedef epsilon<double> epsilond;
+
+} // namespace cml
+
+#endif
diff --git a/src/cml/mathlib/vector_angle.h b/src/cml/mathlib/vector_angle.h
new file mode 100644 (file)
index 0000000..910817b
--- /dev/null
@@ -0,0 +1,69 @@
+/* -*- 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 vector_angle_h
+#define vector_angle_h
+
+#include <cml/mathlib/checking.h>
+
+/* Functions for finding the signed and unsigned angles between vectors in
+ * 3D and 2D.
+ *
+ * Note that the input vectors for these functions are not required to be
+ * unit length.
+ *
+ * @todo: Clean up promotions, conversions, and return types.
+ */
+
+namespace cml {
+
+/** Signed angle between two 3D vectors. */
+template< class VecT_1, class VecT_2, class VecT_3 >
+typename detail::DotPromote<
+    typename detail::CrossPromote<VecT_1,VecT_2>::promoted_vector, VecT_3
+>::promoted_scalar
+signed_angle(const VecT_1& v1, const VecT_2& v2, const VecT_3& reference)
+{
+    typedef typename detail::CrossPromote<VecT_1,VecT_2>::promoted_vector
+        vector_type;
+    typedef typename detail::DotPromote<vector_type,VecT_3>::promoted_scalar
+        value_type;
+
+    vector_type c = cross(v1,v2);
+    value_type angle = std::atan2(double(length(c)),double(dot(v1,v2)));
+    return dot(c,reference) < value_type(0) ? -angle : angle;
+}
+
+/** Unsigned angle between two 3D vectors. */
+template< class VecT_1, class VecT_2 >
+typename detail::DotPromote< VecT_1, VecT_2 >::promoted_scalar
+unsigned_angle(const VecT_1& v1, const VecT_2& v2) {
+    return std::atan2(double(length(cross(v1,v2))),double(dot(v1,v2)));
+}
+
+/** Signed angle between two 2D vectors. */
+template< class VecT_1, class VecT_2 >
+typename detail::DotPromote< VecT_1, VecT_2 >::promoted_scalar
+signed_angle_2D(const VecT_1& v1, const VecT_2& v2) {
+    return std::atan2(double(perp_dot(v1,v2)),double(dot(v1,v2)));
+}
+
+/** Unsigned angle between two 2D vectors. */
+template< class VecT_1, class VecT_2 >
+typename detail::DotPromote< VecT_1, VecT_2 >::promoted_scalar
+unsigned_angle_2D(const VecT_1& v1, const VecT_2& v2) {
+    return std::fabs(signed_angle_2D(v1,v2));
+}
+
+} // namespace cml
+
+#endif
diff --git a/src/cml/mathlib/vector_misc.h b/src/cml/mathlib/vector_misc.h
new file mode 100644 (file)
index 0000000..6553342
--- /dev/null
@@ -0,0 +1,304 @@
+/* -*- 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 vector_misc_h
+#define vector_misc_h
+
+#include <cml/mathlib/coord_conversion.h>
+
+/* Miscellaneous vector functions. */
+
+namespace cml {
+
+/* Function to project a vector v onto a hyperplane specified by a unit-length
+ * normal n.
+ *
+ * @todo: Clean up promotion code.
+ */
+
+template < class VecT_1, class VecT_2 >
+typename detail::CrossPromote<VecT_1,VecT_2>::promoted_vector
+project_to_hplane(const VecT_1& v, const VecT_2& n)
+{
+    typedef typename detail::CrossPromote<VecT_1,VecT_2>::promoted_vector
+        result_type;
+        
+    result_type result;
+    et::detail::Resize(result, v.size());
+
+    result = v - dot(v,n) * n;
+    return result;
+}
+
+/* Return a vector perpendicular (CCW) to a 2D vector. */
+template < class VecT > vector< typename VecT::value_type, fixed<2> >
+perp(const VecT& v)
+{
+    typedef vector< typename VecT::value_type, fixed<2> > temporary_type;
+    
+    /* Checking */
+    detail::CheckVec2(v);
+    
+    return temporary_type(-v[1],v[0]);
+}
+
+/* @todo: unit_cross() and cross_cardinal() should probably go in
+ * vector_products.h, but I'm trying to avoid modifying the existing codebase
+ * for now.
+ */
+
+/** Return normalized cross product of two vectors */
+template< class LeftT, class RightT >
+typename detail::CrossPromote<LeftT,RightT>::promoted_vector
+unit_cross(const LeftT& left, const RightT& right) {
+    /* @todo: This will probably break with dynamic<> vectors */
+    return normalize(cross(left,right));
+}
+
+/** Return the cross product of v and the i'th cardinal basis vector */
+template < class VecT > vector< typename VecT::value_type, fixed<3> >
+cross_cardinal(const VecT& v, size_t i)
+{
+    typedef vector< typename VecT::value_type, fixed<3> > vector_type;
+    typedef typename vector_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckVec3(v);
+    detail::CheckIndex3(i);
+
+    size_t j, k;
+    cyclic_permutation(i, i, j, k);
+    vector_type result;
+    result[i] = value_type(0);
+    result[j] = v[k];
+    result[k] = -v[j];
+    return result;
+}
+
+/** Return the cross product of the i'th cardinal basis vector and v */
+template < class VecT > vector< typename VecT::value_type, fixed<3> >
+cross_cardinal(size_t i, const VecT& v)
+{
+    typedef vector< typename VecT::value_type, fixed<3> > vector_type;
+    typedef typename vector_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckVec3(v);
+    detail::CheckIndex3(i);
+
+    size_t j, k;
+    cyclic_permutation(i, i, j, k);
+    vector_type result;
+    result[i] = value_type(0);
+    result[j] = -v[k];
+    result[k] = v[j];
+    return result;
+}
+
+/** Rotate a 3D vector v about a unit-length vector n. */
+template< class VecT_1, class VecT_2, typename Real >
+vector<
+    typename et::ScalarPromote<
+        typename VecT_1::value_type,
+        typename VecT_2::value_type
+    >::type,
+    fixed<3>
+>
+rotate_vector(const VecT_1& v, const VecT_2& n, Real angle)
+{
+    typedef vector<
+        typename et::ScalarPromote<
+            typename VecT_1::value_type,
+            typename VecT_2::value_type
+        >::type,
+        fixed<3>
+    > result_type;
+    
+    /* Checking */
+    detail::CheckVec3(v);
+    detail::CheckVec3(n);
+    
+    result_type parallel = dot(v,n)*n;
+    return (
+        std::cos(angle)*(v-parallel) + std::sin(angle)*cross(n,v) + parallel
+    );
+}
+
+/** Rotate a 2D vector v about a unit-length vector n. */
+template< class VecT, typename Real >
+vector< typename VecT::value_type, fixed<2> >
+rotate_vector_2D(const VecT& v, Real angle)
+{
+    typedef vector< typename VecT::value_type, fixed<2> > result_type;
+    typedef typename result_type::value_type value_type;
+    
+    /* Checking */
+    detail::CheckVec2(v);
+    
+    value_type s = std::sin(angle);
+    value_type c = std::cos(angle);
+    
+    return result_type(c * v[0] - s * v[1], s * v[0] + c * v[1]);
+}
+
+/** Random unit 3D or 2D vector
+ *
+ * @todo: This is just placeholder code for what will be a more thorough
+ * 'random unit' implementation:
+ *
+ * - All dimensions will be handled uniformly if practical, perhaps through
+ *   a normal distrubution PRNG.
+ *
+ * - Failing that (or perhaps even in this case), dimensions 2 and 3 will be
+ *   dispatched to special-case code, most likely implementing the algorithms
+ *   below.
+ *
+ * - Like the utility random functions, the option of using one's own PRGN
+ *   will be made available.
+ *
+ * @todo: Once N-d random vectors are supported, add a 'random unit
+ * quaternion' function that wraps a call to random_unit() with a 4D vector as
+ * the argument.
+ */
+template < typename E, class A > void
+random_unit(vector<E,A>& v)
+{
+    typedef vector<E,A> vector_type;
+    typedef typename vector_type::value_type value_type;
+    
+    switch (v.size()) {
+        case 3:
+        {
+            vector< E, fixed<3> > temp;
+            spherical_to_cartesian(
+                value_type(1),
+                value_type(random_unit() * constants<value_type>::two_pi()),
+                acos_safe(random_real(value_type(-1),value_type(1))),
+                2,
+                colatitude,
+                temp
+            );
+            v[0] = temp[0];
+            v[1] = temp[1];
+            v[2] = temp[2];
+            break;
+        }
+        case 2:
+        {
+            vector< E, fixed<2> > temp;
+            polar_to_cartesian(
+                value_type(1),
+                value_type(random_unit() * constants<value_type>::two_pi()),
+                temp
+            );
+            v[0] = temp[0];
+            v[1] = temp[1];
+            break;
+        }
+        default:
+            throw std::invalid_argument(
+                "random_unit() for N-d vectors not implemented yet");
+            break;
+    }
+}
+
+/* Random vector within a given angle of a unit-length axis, i.e. in a cone
+ * (3D) or wedge (2D).
+ *
+ * The same notes the appear above apply here too, more or less. One
+ * difference is that this is really only useful in 2D and 3D (presumably), so
+ * we'll probably just do a compile- or run-time dispatch as appropriate.
+ *
+ * Also, there may be a better algorithm for generating a random unit vector
+ * in a cone; need to look into that.
+ *
+ * All of this 'temp' stuff is because there's no compile-time dispatch for
+ * 3D and 2D vectors, but that'll be fixed soon.
+ */
+
+template < typename E, class A, class VecT > void
+random_unit(vector<E,A>& v, const VecT& axis, E theta)
+{
+    typedef vector<E,A> vector_type;
+    typedef typename vector_type::value_type value_type;
+    
+    switch (v.size()) {
+        case 3:
+        {
+            vector< E, fixed<3> > temp, n, temp_axis;
+            temp_axis[0] = axis[0];
+            temp_axis[1] = axis[1];
+            temp_axis[2] = axis[2];
+
+            /* @todo: Function for finding 'any perpendicular vector'? */
+            n = axis_3D(cml::index_of_min_abs(axis[0],axis[1],axis[2]));
+            n = cross(n,temp_axis);
+            
+            /* Rotate v 'away from' the axis by a random angle in the range
+             * [-theta,theta]
+             */
+            temp = rotate_vector(temp_axis,n,random_real(-theta,theta));
+             
+            /* Rotate v about the axis by a random angle in the range [-pi,pi]
+             */
+            temp = rotate_vector(
+                temp,
+                temp_axis,
+                random_real(
+                    -constants<value_type>::pi(),
+                     constants<value_type>::pi()
+                )
+            );
+
+            v[0] = temp[0];
+            v[1] = temp[1];
+            v[2] = temp[2];
+            break;
+        }
+        case 2:
+        {
+            vector< E, fixed<2> > temp, temp_axis;
+            temp_axis[0] = axis[0];
+            temp_axis[1] = axis[1];
+            temp = rotate_vector_2D(temp_axis, random_real(-theta,theta));
+            v[0] = temp[0];
+            v[1] = temp[1];
+            break;
+        }
+        default:
+            throw std::invalid_argument(
+                "random_unit(v,axis,theta) only implemented for 2D and 3D");
+            break;
+    }
+}
+
+/* NEW: Manhattan distance */
+
+template< class VecT_1, class VecT_2 >
+typename detail::DotPromote< VecT_1, VecT_2 >::promoted_scalar
+manhattan_distance(const VecT_1& v1, const VecT_2& v2) {
+    /* Check that a promotion exists */
+    typedef typename et::VectorPromote<
+        VecT_1,VecT_2>::temporary_type promoted_vector;
+        
+    typedef typename detail::DotPromote< VecT_1, VecT_2 >::promoted_scalar scalar_type;
+    
+    scalar_type sum = scalar_type(0);
+    for (size_t i = 0; i < v1.size(); ++i) {
+        sum += std::fabs(v2[i]-v1[i]);
+    }
+    return sum;
+}
+
+} // namespace cml
+
+#endif
diff --git a/src/cml/mathlib/vector_ortho.h b/src/cml/mathlib/vector_ortho.h
new file mode 100644 (file)
index 0000000..88506f5
--- /dev/null
@@ -0,0 +1,344 @@
+/* -*- 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 vector_ortho_h
+#define vector_ortho_h
+
+#include <cml/mathlib/vector_misc.h>
+#include <cml/mathlib/misc.h>
+
+/* Functions for orthonormalizing a set of basis vector in 3D or 2D, and for
+ * constructing an orthonormal basis given various input parameters.
+ */
+
+namespace cml {
+
+/* Orthonormalize 3 basis vectors in R3.
+ *
+ * Called with the default values, this function performs a single Gram-
+ * Schmidt step to orthonormalize the input vectors. By default, the direction
+ * of the 3rd basis vector is unchanged by this operation, but the unaffected
+ * axis can be specified via the 'stable_axis' parameter.
+ *
+ * The arguments 'num_iter' and 's' can be specified to an iterative Gram-
+ * Schmidt step. 'num_iter' is the number of iterations applied, and 's' is
+ * the fraction applied towards orthonormality each step.
+ *
+ * In most cases, the default arguments can be ignored, leaving only the three
+ * input vectors.
+ */
+
+//////////////////////////////////////////////////////////////////////////////
+// Orthonormalization in 3D and 2D
+//////////////////////////////////////////////////////////////////////////////
+
+template < typename E, class A > void
+orthonormalize(vector<E,A>& v0, vector<E,A>& v1, vector<E,A>& v2,
+    size_t stable_axis = 2, size_t num_iter = 0, E s = E(1))
+{
+    /* Checking */
+    detail::CheckVec3(v0);
+    detail::CheckVec3(v1);
+    detail::CheckVec3(v2);
+    detail::CheckIndex3(stable_axis);
+
+    typedef vector< E, fixed<3> > vector_type;
+    typedef typename vector_type::value_type value_type;
+
+    /* Iterative Gram-Schmidt; this step is skipped by default. */
+    
+    for (size_t i = 0; i < num_iter; ++i) {
+        value_type dot01 = dot(v0,v1);
+        value_type dot12 = dot(v1,v2);
+        value_type dot20 = dot(v2,v0);
+        value_type inv_dot00 = value_type(1) / dot(v0,v0);
+        value_type inv_dot11 = value_type(1) / dot(v1,v1);
+        value_type inv_dot22 = value_type(1) / dot(v2,v2);
+
+        vector_type temp0 = v0 - s*dot01*inv_dot11*v1 - s*dot20*inv_dot22*v2;
+        vector_type temp1 = v1 - s*dot12*inv_dot22*v2 - s*dot01*inv_dot00*v0;
+        vector_type temp2 = v2 - s*dot20*inv_dot00*v0 - s*dot12*inv_dot11*v1;
+        
+        v0 = temp0;
+        v1 = temp1;
+        v2 = temp2;
+    }
+
+    /* Final Gram-Schmidt step to ensure orthonormality. If no iterations
+     * have been requested (num_iter = 0), this is the only step. The step
+     * is performed such that the direction of the axis indexed by
+     * 'stable_axis' is unchanged.
+     */
+
+    size_t i, j, k;
+    cyclic_permutation(stable_axis, i, j, k);
+    vector_type v[] = { v0, v1, v2 };
+
+    v[i].normalize();
+    v[j] = normalize(project_to_hplane(v[j],v[i]));
+    v[k] = normalize(project_to_hplane(project_to_hplane(v[k],v[i]),v[j]));
+    
+    v0 = v[0];
+    v1 = v[1];
+    v2 = v[2];
+}
+
+/* Orthonormalize 2 basis vectors in R2 */
+template < typename E, class A > void
+orthonormalize(vector<E,A>& v0, vector<E,A>& v1,
+    size_t stable_axis = 0, size_t num_iter = 0, E s = E(1))
+{
+    typedef vector< E, fixed<2> > vector_type;
+    typedef typename vector_type::value_type value_type;
+
+    /* Checking */
+    detail::CheckVec2(v0);
+    detail::CheckVec2(v1);
+    detail::CheckIndex2(stable_axis);
+
+    /* Iterative Gram-Schmidt; this step is skipped by default. */
+    
+    for (size_t i = 0; i < num_iter; ++i) {
+        value_type dot01 = dot(v0,v1);
+
+        vector_type temp0 = v0 - (s * dot01 * v1) / dot(v1,v1);
+        vector_type temp1 = v1 - (s * dot01 * v0) / dot(v0,v0);
+        
+        v0 = temp0;
+        v1 = temp1;
+    }
+
+    /* Final Gram-Schmidt step to ensure orthonormality. If no iterations
+     * have been requested (num_iter = 0), this is the only step. The step
+     * is performed such that the direction of the axis indexed by
+     * 'stable_axis' is unchanged.
+     */
+
+    size_t i, j;
+    cyclic_permutation(stable_axis, i, j);
+    vector_type v[] = { v0, v1 };
+
+    v[i].normalize();
+    v[j] = normalize(project_to_hplane(v[j],v[i]));
+    
+    v0 = v[0];
+    v1 = v[1];
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Orthonormal basis construction in 3D and 2D
+//////////////////////////////////////////////////////////////////////////////
+
+/* This version of orthonormal_basis() ultimately does the work for all
+ * orthonormal_basis_*() functions. Given input vectors 'align' and
+ * 'reference', and an order 'axis_order_<i><j><k>', it constructs an
+ * orthonormal basis such that the i'th basis vector is aligned with (parallel
+ * to and pointing in the same direction as) 'align', and the j'th basis
+ * vector is maximally aligned with 'reference'. The k'th basis vector is
+ * chosen such that the basis has a determinant of +1.
+ *
+ * Note that the algorithm fails when 'align' is nearly parallel to
+ * 'reference'; this should be checked for and handled externally if it's a
+ * case that may occur.
+ */
+
+/* Note: This is an example of the 'non-const argument modification
+ * invalidates expression' gotcha. If x, y or z were to be assigned to before
+ * we were 'done' with align and reference, and if one of them were the same
+ * object as align or reference, then the algorithm could fail. As is the
+ * basis vectors are assigned at the end of the function from a temporary
+ * array, so all is well.
+ */
+
+template < class VecT_1, class VecT_2, typename E, class A > void
+orthonormal_basis(
+    const VecT_1& align,
+    const VecT_2& reference,
+    vector<E,A>& x,
+    vector<E,A>& y,
+    vector<E,A>& z,
+    bool normalize_align = true,
+    AxisOrder order = axis_order_zyx)
+{
+    typedef vector< E,fixed<3> > vector_type;
+    typedef typename vector_type::value_type value_type;
+    
+    /* Checking handled by cross() and assignment to fixed<3>. */
+    
+    size_t i, j, k;
+    bool odd;
+    detail::unpack_axis_order(order, i, j, k, odd);
+
+    vector_type axis[3];
+
+    axis[i] = normalize_align ? normalize(align) : align;
+    axis[k] = unit_cross(axis[i],reference);
+    axis[j] = cross(axis[k],axis[i]);
+    
+    if (odd) {
+        axis[k] = -axis[k];
+    }
+    
+    x = axis[0];
+    y = axis[1];
+    z = axis[2];
+}
+
+/* This version of orthonormal_basis() constructs in arbitrary basis given a
+ * vector with which to align the i'th basis vector. To avoid the failure
+ * case, the reference vector is always chosen so as to not be parallel to
+ * 'align'. This means the algorithm will always generate a valid basis, which
+ * can be useful in some circumstances; however, it should be noted that the
+ * basis will likely 'pop' as the alignment vector changes, and so may not be
+ * suitable for billboarding or other similar applications.
+ */
+
+template < class VecT, typename E, class A >
+void orthonormal_basis(
+    const VecT& align,
+    vector<E,A>& x,
+    vector<E,A>& y,
+    vector<E,A>& z,
+    bool normalize_align = true,
+    AxisOrder order = axis_order_zyx)
+{
+    /* Checking (won't be necessary with index_of_min_abs() member function */
+    detail::CheckVec3(align);
+
+    /* @todo: vector member function index_of_min_abs() would clean this up */
+    
+    orthonormal_basis(
+        align,
+        axis_3D(cml::index_of_min_abs(align[0],align[1],align[2])),
+        x, y, z, normalize_align, order
+    );
+}
+
+/* orthonormal_basis_axial() generates a basis in which the j'th basis vector
+ * is aligned with 'axis' and the i'th basis vector is maximally aligned (as
+ * 'aligned as possible') with 'align'. This can be used for e.g. axial
+ * billboarding for, say, trees or beam effects.
+ *
+ * Note that the implementation simply passes off to the 'reference' version
+ * of orthonormal_basis(), with the parameters adjusted so that the alignment
+ * is axial.
+ *
+ * With this algorithm the failure case is when 'align' and 'axis' are nearly
+ * parallel; if this is likely, it should be checked for and handled
+ * externally.
+ */
+
+template < class VecT_1, class VecT_2, typename E, class A >
+void orthonormal_basis_axial(
+    const VecT_1& align,
+    const VecT_2& axis,
+    vector<E,A>& x,
+    vector<E,A>& y,
+    vector<E,A>& z,
+    bool normalize_align = true,
+    AxisOrder order = axis_order_zyx)
+{
+    orthonormal_basis(
+        axis,
+        align,
+        x,
+        y,
+        z,
+        normalize_align,
+        detail::swap_axis_order(order));
+}
+
+/* orthonormal_basis_viewplane() builds a basis aligned with a viewplane, as
+ * extracted from the input view matrix. The function takes into account the
+ * handedness of the input view matrix and orients the basis accordingly.
+ *
+ * The generated basis will always be valid.
+ */
+template < class MatT, typename E, class A >
+void orthonormal_basis_viewplane(
+    const MatT& view_matrix,</