From 0fffd0097d7b496454413e57b398c903ecc252e4 Mon Sep 17 00:00:00 2001 From: Charles McGarvey Date: Thu, 9 Jul 2009 02:00:37 -0600 Subject: [PATCH] now using cml for vectors and math stuff --- src/cml/cml.h | 75 ++ src/cml/constants.h | 87 ++ src/cml/core/cml_assert.h | 97 ++ src/cml/core/cml_meta.h | 24 + src/cml/core/common.h | 87 ++ src/cml/core/dynamic_1D.h | 127 +++ src/cml/core/dynamic_2D.h | 178 +++ src/cml/core/external_1D.h | 216 ++++ src/cml/core/external_2D.h | 304 ++++++ src/cml/core/fixed_1D.h | 130 +++ src/cml/core/fixed_2D.h | 204 ++++ src/cml/core/fwd.h | 63 ++ src/cml/core/meta/common.h | 88 ++ src/cml/core/meta/if.h | 42 + src/cml/core/meta/switch.h | 173 +++ src/cml/defaults.h | 89 ++ src/cml/dynamic.h | 35 + src/cml/et/array_promotions.h | 288 +++++ src/cml/et/scalar_ops.h | 138 +++ src/cml/et/scalar_promotions.h | 120 +++ src/cml/et/size_checking.h | 426 ++++++++ src/cml/et/tags.h | 55 + src/cml/et/traits.h | 143 +++ src/cml/external.h | 41 + src/cml/fixed.h | 42 + src/cml/mathlib/checking.h | 382 +++++++ src/cml/mathlib/coord_conversion.h | 162 +++ src/cml/mathlib/epsilon.h | 44 + src/cml/mathlib/frustum.h | 239 +++++ src/cml/mathlib/helper.h | 158 +++ src/cml/mathlib/interpolation.h | 1129 ++++++++++++++++++++ src/cml/mathlib/mathlib.h | 33 + src/cml/mathlib/matrix_basis.h | 364 +++++++ src/cml/mathlib/matrix_concat.h | 62 ++ src/cml/mathlib/matrix_misc.h | 135 +++ src/cml/mathlib/matrix_ortho.h | 60 ++ src/cml/mathlib/matrix_projection.h | 346 ++++++ src/cml/mathlib/matrix_rotation.h | 872 +++++++++++++++ src/cml/mathlib/matrix_transform.h | 983 +++++++++++++++++ src/cml/mathlib/matrix_translation.h | 139 +++ src/cml/mathlib/misc.h | 210 ++++ src/cml/mathlib/picking.h | 195 ++++ src/cml/mathlib/projection.h | 142 +++ src/cml/mathlib/quaternion_basis.h | 89 ++ src/cml/mathlib/quaternion_rotation.h | 635 +++++++++++ src/cml/mathlib/typedef.h | 108 ++ src/cml/mathlib/vector_angle.h | 69 ++ src/cml/mathlib/vector_misc.h | 304 ++++++ src/cml/mathlib/vector_ortho.h | 344 ++++++ src/cml/mathlib/vector_transform.h | 156 +++ src/cml/matrix.h | 62 ++ src/cml/matrix/class_ops.h | 341 ++++++ src/cml/matrix/determinant.h | 192 ++++ src/cml/matrix/dynamic.h | 310 ++++++ src/cml/matrix/external.h | 531 +++++++++ src/cml/matrix/fixed.h | 275 +++++ src/cml/matrix/inverse.h | 443 ++++++++ src/cml/matrix/lu.h | 176 +++ src/cml/matrix/matop_macros.h | 236 ++++ src/cml/matrix/matrix_comparison.h | 245 +++++ src/cml/matrix/matrix_expr.h | 483 +++++++++ src/cml/matrix/matrix_functions.h | 43 + src/cml/matrix/matrix_mul.h | 205 ++++ src/cml/matrix/matrix_ops.h | 50 + src/cml/matrix/matrix_print.h | 59 + src/cml/matrix/matrix_promotions.h | 171 +++ src/cml/matrix/matrix_rowcol.h | 261 +++++ src/cml/matrix/matrix_traits.h | 49 + src/cml/matrix/matrix_transpose.h | 305 ++++++ src/cml/matrix/matrix_unroller.h | 285 +++++ src/cml/matvec/matvec_mul.h | 285 +++++ src/cml/matvec/matvec_promotions.h | 92 ++ src/cml/quaternion.h | 69 ++ src/cml/quaternion/conjugate.h | 197 ++++ src/cml/quaternion/inverse.h | 268 +++++ src/cml/quaternion/quaternion.h | 519 +++++++++ src/cml/quaternion/quaternion_comparison.h | 216 ++++ src/cml/quaternion/quaternion_dot.h | 73 ++ src/cml/quaternion/quaternion_expr.h | 614 +++++++++++ src/cml/quaternion/quaternion_functions.h | 172 +++ src/cml/quaternion/quaternion_mul.h | 140 +++ src/cml/quaternion/quaternion_ops.h | 51 + src/cml/quaternion/quaternion_print.h | 80 ++ src/cml/quaternion/quaternion_promotions.h | 140 +++ src/cml/quaternion/quaternion_traits.h | 45 + src/cml/quaternion/quatop_macros.h | 232 ++++ src/cml/util.h | 326 ++++++ src/cml/vector.h | 62 ++ src/cml/vector/class_ops.h | 238 +++++ src/cml/vector/dynamic.h | 173 +++ src/cml/vector/external.h | 305 ++++++ src/cml/vector/fixed.h | 171 +++ src/cml/vector/vecop_macros.h | 248 +++++ src/cml/vector/vector_comparison.h | 236 ++++ src/cml/vector/vector_expr.h | 458 ++++++++ src/cml/vector/vector_functions.h | 73 ++ src/cml/vector/vector_ops.h | 51 + src/cml/vector/vector_print.h | 49 + src/cml/vector/vector_products.h | 361 +++++++ src/cml/vector/vector_promotions.h | 77 ++ src/cml/vector/vector_traits.h | 47 + src/cml/vector/vector_unroller.h | 259 +++++ src/math.cc | 58 - src/math.hh | 21 +- src/matrix.hh | 373 ------- src/quaternion.hh | 188 ---- src/vector.hh | 372 ------- 107 files changed, 21697 insertions(+), 996 deletions(-) create mode 100644 src/cml/cml.h create mode 100644 src/cml/constants.h create mode 100644 src/cml/core/cml_assert.h create mode 100644 src/cml/core/cml_meta.h create mode 100644 src/cml/core/common.h create mode 100644 src/cml/core/dynamic_1D.h create mode 100644 src/cml/core/dynamic_2D.h create mode 100644 src/cml/core/external_1D.h create mode 100644 src/cml/core/external_2D.h create mode 100644 src/cml/core/fixed_1D.h create mode 100644 src/cml/core/fixed_2D.h create mode 100644 src/cml/core/fwd.h create mode 100644 src/cml/core/meta/common.h create mode 100644 src/cml/core/meta/if.h create mode 100644 src/cml/core/meta/switch.h create mode 100644 src/cml/defaults.h create mode 100644 src/cml/dynamic.h create mode 100644 src/cml/et/array_promotions.h create mode 100644 src/cml/et/scalar_ops.h create mode 100644 src/cml/et/scalar_promotions.h create mode 100644 src/cml/et/size_checking.h create mode 100644 src/cml/et/tags.h create mode 100644 src/cml/et/traits.h create mode 100644 src/cml/external.h create mode 100644 src/cml/fixed.h create mode 100644 src/cml/mathlib/checking.h create mode 100644 src/cml/mathlib/coord_conversion.h create mode 100644 src/cml/mathlib/epsilon.h create mode 100644 src/cml/mathlib/frustum.h create mode 100644 src/cml/mathlib/helper.h create mode 100644 src/cml/mathlib/interpolation.h create mode 100644 src/cml/mathlib/mathlib.h create mode 100644 src/cml/mathlib/matrix_basis.h create mode 100644 src/cml/mathlib/matrix_concat.h create mode 100644 src/cml/mathlib/matrix_misc.h create mode 100644 src/cml/mathlib/matrix_ortho.h create mode 100644 src/cml/mathlib/matrix_projection.h create mode 100644 src/cml/mathlib/matrix_rotation.h create mode 100644 src/cml/mathlib/matrix_transform.h create mode 100644 src/cml/mathlib/matrix_translation.h create mode 100644 src/cml/mathlib/misc.h create mode 100644 src/cml/mathlib/picking.h create mode 100644 src/cml/mathlib/projection.h create mode 100644 src/cml/mathlib/quaternion_basis.h create mode 100644 src/cml/mathlib/quaternion_rotation.h create mode 100644 src/cml/mathlib/typedef.h create mode 100644 src/cml/mathlib/vector_angle.h create mode 100644 src/cml/mathlib/vector_misc.h create mode 100644 src/cml/mathlib/vector_ortho.h create mode 100644 src/cml/mathlib/vector_transform.h create mode 100644 src/cml/matrix.h create mode 100644 src/cml/matrix/class_ops.h create mode 100644 src/cml/matrix/determinant.h create mode 100644 src/cml/matrix/dynamic.h create mode 100644 src/cml/matrix/external.h create mode 100644 src/cml/matrix/fixed.h create mode 100644 src/cml/matrix/inverse.h create mode 100644 src/cml/matrix/lu.h create mode 100644 src/cml/matrix/matop_macros.h create mode 100644 src/cml/matrix/matrix_comparison.h create mode 100644 src/cml/matrix/matrix_expr.h create mode 100644 src/cml/matrix/matrix_functions.h create mode 100644 src/cml/matrix/matrix_mul.h create mode 100644 src/cml/matrix/matrix_ops.h create mode 100644 src/cml/matrix/matrix_print.h create mode 100644 src/cml/matrix/matrix_promotions.h create mode 100644 src/cml/matrix/matrix_rowcol.h create mode 100644 src/cml/matrix/matrix_traits.h create mode 100644 src/cml/matrix/matrix_transpose.h create mode 100644 src/cml/matrix/matrix_unroller.h create mode 100644 src/cml/matvec/matvec_mul.h create mode 100644 src/cml/matvec/matvec_promotions.h create mode 100644 src/cml/quaternion.h create mode 100644 src/cml/quaternion/conjugate.h create mode 100644 src/cml/quaternion/inverse.h create mode 100644 src/cml/quaternion/quaternion.h create mode 100644 src/cml/quaternion/quaternion_comparison.h create mode 100644 src/cml/quaternion/quaternion_dot.h create mode 100644 src/cml/quaternion/quaternion_expr.h create mode 100644 src/cml/quaternion/quaternion_functions.h create mode 100644 src/cml/quaternion/quaternion_mul.h create mode 100644 src/cml/quaternion/quaternion_ops.h create mode 100644 src/cml/quaternion/quaternion_print.h create mode 100644 src/cml/quaternion/quaternion_promotions.h create mode 100644 src/cml/quaternion/quaternion_traits.h create mode 100644 src/cml/quaternion/quatop_macros.h create mode 100644 src/cml/util.h create mode 100644 src/cml/vector.h create mode 100644 src/cml/vector/class_ops.h create mode 100644 src/cml/vector/dynamic.h create mode 100644 src/cml/vector/external.h create mode 100644 src/cml/vector/fixed.h create mode 100644 src/cml/vector/vecop_macros.h create mode 100644 src/cml/vector/vector_comparison.h create mode 100644 src/cml/vector/vector_expr.h create mode 100644 src/cml/vector/vector_functions.h create mode 100644 src/cml/vector/vector_ops.h create mode 100644 src/cml/vector/vector_print.h create mode 100644 src/cml/vector/vector_products.h create mode 100644 src/cml/vector/vector_promotions.h create mode 100644 src/cml/vector/vector_traits.h create mode 100644 src/cml/vector/vector_unroller.h delete mode 100644 src/math.cc delete mode 100644 src/matrix.hh delete mode 100644 src/quaternion.hh delete mode 100644 src/vector.hh diff --git a/src/cml/cml.h b/src/cml/cml.h new file mode 100644 index 0000000..666bb07 --- /dev/null +++ b/src/cml/cml.h @@ -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 +#include +#include +#include +#include + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/constants.h b/src/cml/constants.h new file mode 100644 index 0000000..060705c --- /dev/null +++ b/src/cml/constants.h @@ -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 + +#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 +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 const F cml::constants::pi; + */ +/** Templated constants struct. + * + * Either float or double can be used. + */ +template +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 index 0000000..9b29c6d --- /dev/null +++ b/src/cml/core/cml_assert.h @@ -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 + +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 struct STATIC_ASSERTION_FAILURE; + +/** Struct instantiated when a true assertion is made at compile-time. */ +template<> struct STATIC_ASSERTION_FAILURE { + 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(,M), the compiler errors will + * contain the struct name at the point of the error. + */ +template struct STATIC_ASSERTION_FAILURE_M { + typename M::bogus result; +}; + +/** Instantiated for true assertions. */ +template struct STATIC_ASSERTION_FAILURE_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 index 0000000..679f761 --- /dev/null +++ b/src/cml/core/cml_meta.h @@ -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 +#include +#include + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/core/common.h b/src/cml/core/common.h new file mode 100644 index 0000000..01b3f7e --- /dev/null +++ b/src/cml/core/common.h @@ -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 +#ifndef _SSIZE_T_DEFINED +#ifdef _WIN64 +typedef __int64 ssize_t; +#else +typedef _W64 int ssize_t; +#endif +#define _SSIZE_T_DEFINED +#endif +#endif + +#include // for size_t +#include // for std::pair<> +#include + +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 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 index 0000000..2aae2c6 --- /dev/null +++ b/src/cml/core/dynamic_1D.h @@ -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 +#include +#include + +namespace cml { + +/** Dynamically-sized and allocated 1D array. + * + * @note The allocator should be an STL-compatible allocator. + * + * @internal The internal array type must have the proper copy + * semantics, otherwise copy construction will fail. + */ +template +class dynamic_1D +{ + public: + + /* Record the allocator type: */ + typedef typename Alloc::template rebind::other allocator_type; + + /* Record the generator: */ + typedef dynamic generator_type; + + /* Array implementation: */ + typedef std::vector 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 index 0000000..c6ea386 --- /dev/null +++ b/src/cml/core/dynamic_2D.h @@ -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 +#include +#include +#include + +namespace cml { + +/** Dynamically-sized and allocated 2D array. + * + * @note The allocator should be an STL-compatible allocator. + * + * @internal The internal array type must have the proper copy + * semantics, otherwise copy construction will fail. + * + * @internal This class does not need a destructor. + */ +template +class dynamic_2D +{ + public: + + /* Record the allocator type: */ + typedef typename Alloc::template rebind::other allocator_type; + + /* Record the generator: */ + typedef dynamic generator_type; + + /* Array implementation: */ + typedef std::vector 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 transposed_type; + + /* To simplify the matrix row and column operators: */ + typedef dynamic_1D row_array_type; + typedef dynamic_1D 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 index 0000000..92939e1 --- /dev/null +++ b/src/cml/core/external_1D.h @@ -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 +#include +#include +#include + +namespace cml { + +/** Fixed-size external 1D array. + * + * Both the memory and the size are fixed at compile time, and cannot be + * changed. + */ +template +class external_1D +{ + public: + + /* Require Size > 0: */ + CML_STATIC_REQUIRE(Size > 0); + + /* Record the generator: */ + 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; + + /* 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 +class external_1D +{ + 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 index 0000000..7461c51 --- /dev/null +++ b/src/cml/core/external_2D.h @@ -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 +#include +#include +#include +#include +#include + +namespace cml { + +/** Fixed-size external 2D array. + * + * Both the memory and the size are fixed at compile time, and cannot be + * changed. + */ +template +class external_2D +{ + public: + + /* Require Rows > 0, Cols > 0: */ + CML_STATIC_REQUIRE((Rows > 0) && (Cols > 0)); + + /* Record the generator: */ + 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 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 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 row_array_type; + typedef fixed_1D 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(&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 +class external_2D +{ + 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 + transposed_type; + + /* To simplify the matrix row and column operators: */ + typedef dynamic_1D row_array_type; + typedef dynamic_1D 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 index 0000000..eb691af --- /dev/null +++ b/src/cml/core/fixed_1D.h @@ -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 +#include +#include +#include + +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 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 not 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 +class fixed_1D +{ + public: + + /* Require Size > 0: */ + CML_STATIC_REQUIRE(Size > 0); + + /* Record the generator: */ + typedef fixed 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 index 0000000..44af7e4 --- /dev/null +++ b/src/cml/core/fixed_2D.h @@ -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 +#include + +/* 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 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 not 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 +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::is_true + || same_type::is_true), + invalid_layout_type_error); + + + /* Record the generator: */ + typedef fixed 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 transposed_type; + + /* To simplify the matrix row and column operators: */ + typedef fixed_1D row_array_type; + typedef fixed_1D 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 index 0000000..84fada0 --- /dev/null +++ b/src/cml/core/fwd.h @@ -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 class fixed_1D; + +/* cml/core/fixed_2D.h */ +template class fixed_2D; + +/* cml/core/dynamic_1D.h */ +template class dynamic_1D; + +/* cml/core/dynamic_2D.h */ +template class dynamic_2D; + +/* cml/core/external_1D.h */ +template class external_1D; + +/* cml/core/external_2D.h */ +template class external_2D; + +/* cml/fixed.h */ +template struct fixed; + +/* cml/dynamic.h */ +template struct dynamic; + +/* cml/external.h */ +template struct external; + +/* cml/vector.h */ +template class vector; + +/* cml/matrix.h */ +template class matrix; + +/* cml/quaternion.h */ +template 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 index 0000000..6d9b876 --- /dev/null +++ b/src/cml/core/meta/common.h @@ -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 struct is_true { + typedef false_type result; +}; + +template<> struct is_true { + typedef true_type result; +}; + +/** A "type pair". */ +template struct type_pair { + typedef T1 first; + typedef T2 second; +}; + +/** A "type quadruple". */ +template +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 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 struct same_type { + typedef true_type result; + enum { is_true = true, is_false = false }; +}; + +/** Match a type and any_type. */ +template struct same_type { + typedef true_type result; + enum { is_true = true, is_false = false }; +}; + +/** Match a type and any_type. */ +template struct same_type { + typedef true_type result; + enum { is_true = true, is_false = false }; +}; + +/** Disambiguate pair of any_type's. */ +template<> struct same_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 index 0000000..1ea3235 --- /dev/null +++ b/src/cml/core/meta/if.h @@ -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 + +namespace cml { + +/** Select argument type based upon truth value. */ +template struct select_if; + +/** Result is TrueT if true. */ +template +struct select_if { + typedef TrueT result; + enum { is_true = true }; +}; + +/** Result is FalseT if false. */ +template +struct select_if { + 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 index 0000000..6f4383a --- /dev/null +++ b/src/cml/core/meta/switch.h @@ -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 +#include + +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 +struct select_case +{ + template struct match { + typedef typename select_if< + same_type::is_true, + Result, + typename NextCase::template match::result + >::result result; + }; +}; + +/* Default case, returned when no match is found in a previous case: */ +template +struct select_case +{ + template struct match { + typedef Result result; + }; +}; + +/* The last case statement (if no match until now, the result is 'void'): */ +template +struct select_case +{ + template struct match { + typedef typename select_if< + same_type::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 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::result result; +}; + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/defaults.h b/src/cml/defaults.h new file mode 100644 index 0000000..4b7c4cf --- /dev/null +++ b/src/cml/defaults.h @@ -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: */ +#if !defined(CML_DEFAULT_ARRAY_ALLOC) +#include // for std::allocator +#define CML_DEFAULT_ARRAY_ALLOC std::allocator +#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 index 0000000..8c942b4 --- /dev/null +++ b/src/cml/dynamic.h @@ -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 + +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 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 index 0000000..418211d --- /dev/null +++ b/src/cml/et/array_promotions.h @@ -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 +#include + +namespace cml { +namespace et { + +#define VAL_MAX(_a_,_b_) ( ((_a_)>(_b_))?(_a_):(_b_) ) + +namespace detail { + +/* This is specialized for 1D and 2D promotions: */ +template struct promote; + +/* Promote 1D fixed-size arrays to a 1D fixed-size array: */ +template +struct promote +{ + 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 type; +}; + +/* Promote 1D dynamic arrays to a 1D dynamic array: */ +template +struct promote +{ + 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::other allocator; + + /* Finally, generate the promoted array type: */ + typedef dynamic_1D type; +}; + +/* Promote fixed 2D+1D array expressions to a fixed 1D array: */ +template +struct promote +{ + 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 type; +}; + +/* Promote fixed 1D+2D array expressions to a fixed 1D array: */ +template +struct promote +{ + 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 type; +}; + +/* Promote dynamic 2D+1D array expression to a 1D dynamic array: */ +template +struct promote +{ + 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::other allocator; + + /* Finally, generate the promoted array type: */ + typedef dynamic_1D type; +}; + +/* Promote dynamic 1D+2D array expression to a 1D dynamic array: */ +template +struct promote +{ + 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::other allocator; + + /* Finally, generate the promoted array type: */ + typedef dynamic_1D type; +}; + + +/* This is a helper to deduce the result of a promoted 2D array: */ +template 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::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 +struct promote +{ + 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 + ::promoted_layout promoted_layout; + + /* Finally, generate the promoted array type: */ + typedef fixed_2D type; +}; + +/* Promote 2D dynamic arrays to a 2D dynamic array: */ +template +struct promote +{ + 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::other allocator; + + /* Then deduce the array layout: */ + typedef typename A1::layout left_layout; + typedef typename A2::layout right_layout; + typedef typename deduce_layout + ::promoted_layout promoted_layout; + + /* Finally, generate the promoted array type: */ + typedef dynamic_2D 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 +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::is_true + && same_type::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 index 0000000..6bd236d --- /dev/null +++ b/src/cml/et/scalar_ops.h @@ -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 +#include + +/** Declare a unary scalar operator, like negation. */ +#define CML_UNARY_SCALAR_OP(_op_, _op_name_) \ +template struct _op_name_ { \ + typedef ExprTraits 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 struct _op_name_ { \ + typedef ExprTraits left_traits; \ + typedef ExprTraits 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::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::reference must specify + * a type that allows assignment. + */ +#define CML_BINARY_SCALAR_OP_ASSIGN(_op_, _op_name_) \ +template struct _op_name_ { \ + typedef ExprTraits left_traits; \ + typedef ExprTraits 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::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 struct _op_name_ { \ + typedef ExprTraits left_traits; \ + typedef ExprTraits 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 index 0000000..55d033c --- /dev/null +++ b/src/cml/et/scalar_promotions.h @@ -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 + +namespace cml { +namespace et { + +namespace detail { + +/** @class IntPromote + * @brief Helper template to int-promote a type. + */ +template struct IntPromote +{ + /* Signed -> signed int, unsigned -> unsigned int: */ + typedef typename select_switch::result result; +}; + +} // namespace detail + +/** @class ScalarPromote + * @brief Template for compile-time type promotion via C promotion rules. + */ +template struct ScalarPromote +{ + + /* Integral-promote the types (if possible). */ + typedef typename detail::IntPromote::result E1; + typedef typename detail::IntPromote::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::result uint_promotion; + + /* Do the selection on the promoted types: */ + typedef typename select_switch< + type_pair, + +#if defined(CML_USE_LONG_DOUBLE) + type_pair, long double, + type_pair, long double, + type_pair, long double, +#endif + + type_pair, double, + type_pair, double, + type_pair, double, + + type_pair, float, + type_pair, float, + type_pair, float, + + type_pair, void + + >::result float_filter; + + /* The promoted integral types really matter here: */ + typedef typename select_switch< + type_pair, + + type_pair, unsigned long, + type_pair, unsigned long, + type_pair, unsigned long, + + type_pair, long, + type_pair, uint_promotion, + type_pair, uint_promotion, + + type_pair, long, + type_pair, long, + + type_pair, unsigned int, + type_pair, unsigned int, + type_pair, unsigned int, + + type_pair, int, + type_pair, int, + type_pair, int, + + type_pair, void + + >::result int_filter; + + /* Deduce the final type: */ + typedef typename select_if< + same_type::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 index 0000000..323afb5 --- /dev/null +++ b/src/cml/et/size_checking.h @@ -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 +#include +#include +#include +#include + +#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 + struct GetCheckedSize; + +/* Checking for fixed-size expression: */ +template +struct GetCheckedSize +{ + /* Record argument traits: */ + typedef ExprTraits left_traits; + typedef ExprTraits right_traits; + + /* Result types: */ + typedef typename left_traits::result_tag left_result; + typedef typename right_traits::result_tag right_result; + + + /* For specialization below: */ + template struct impl; + + /* Check for two matrices (linear operators only): */ + template struct impl { + 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 struct impl { + 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 struct impl { + 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 struct impl { + 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 struct impl { + 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 struct impl { + 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 struct impl { + 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 struct impl { + 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 + struct impl { + 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 struct impl { + 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 struct impl { + 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 struct impl { + 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 struct impl { + 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 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 +struct GetCheckedSize +{ + /* Type of the size checker (for calling equal_or_fail): */ + typedef GetCheckedSize self; + + /* Record argument traits: */ + typedef ExprTraits left_traits; + typedef ExprTraits right_traits; + + /* Result types: */ + typedef typename left_traits::result_tag left_result; + typedef typename right_traits::result_tag right_result; + + + /* For specialization below: */ + template struct impl; + + /* Return the size if the same, or fail if different: */ + template 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 struct impl { + 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 struct impl { + 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 struct impl { + 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 struct impl { + 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 struct impl { + 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 struct impl { + 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 struct impl { + 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 struct impl { + 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 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 +inline typename et::GetCheckedSize::size_type +CheckedSize(const LeftT& left, const RightT& right, SizeTag) +{ + return et::GetCheckedSize()(left,right); +} + +/** Verify the sizes of the argument matrices for matrix multiplication. + * + * @returns a the size of the resulting matrix. + */ +template 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 inline size_t +CheckedSquare(const MatT& m, dynamic_size_tag) +{ + matrix_size N = m.size(); + et::GetCheckedSize() + .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 index 0000000..a940134 --- /dev/null +++ b/src/cml/et/tags.h @@ -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 index 0000000..0e59e4b --- /dev/null +++ b/src/cml/et/traits.h @@ -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 + +/* 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 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 +{ + /* 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 +{ + /* 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 index 0000000..cc052e7 --- /dev/null +++ b/src/cml/external.h @@ -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 struct external { + + /** Rebind to a 1D type. + * + * This is used by quaternion<>. + */ + template struct rebind { typedef external other; }; +}; + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/fixed.h b/src/cml/fixed.h new file mode 100644 index 0000000..536eb3c --- /dev/null +++ b/src/cml/fixed.h @@ -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 struct fixed { + + /** Rebind to a 1D type. + * + * This is used by quaternion<>. + */ + template struct rebind { typedef fixed 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 index 0000000..36a7ced --- /dev/null +++ b/src/cml/mathlib/checking.h @@ -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 +#include +#include + +/* 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 vector_traits; + typedef typename vector_traits::result_tag result_type; + + CML_STATIC_REQUIRE_M( + (same_type::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() + .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 vector_traits; + typedef typename vector_traits::size_tag size_tag; + + detail::CheckVecN(v, size_tag()); +} + +/** Check for a vector of size 2 */ +template< class VecT > inline void +CheckVec2(const VecT& v) { + detail::CheckVecN(v); +} + +/** Check for a vector of size 3 */ +template< class VecT > inline void +CheckVec3(const VecT& v) { + detail::CheckVecN(v); +} + +/** Check for a vector of size 4 */ +template< class VecT > inline void +CheckVec4(const VecT& v) { + CheckVecN(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 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 matrix_traits; + typedef typename matrix_traits::result_tag result_type; + + CML_STATIC_REQUIRE_M( + (same_type::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() + .equal_or_fail(m.rows(),N); + et::GetCheckedSize() + .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 matrix_traits; + typedef typename matrix_traits::size_tag size_tag; + + CheckMatNxM(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(m); +} + +/** Check for a square matrix of size 2x2 */ +template< class MatT > inline void +CheckMat2x2(const MatT& m) { + CheckMatN(m); +} + +/** Check for a square matrix of size 3x3 */ +template< class MatT > inline void +CheckMat3x3(const MatT& m) { + CheckMatN(m); +} + +/** Check for a square matrix of size 4x4 */ +template< class MatT > inline void +CheckMat4x4(const MatT& m) { + CheckMatN(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 matrix_traits; + typedef typename matrix_traits::size_tag size_tag; + + CheckMatMinNxM(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(m); +} + +/** Check for a matrix with minimum dimensions 2x2 */ +template< class MatT > inline void +CheckMatMin2x2(const MatT& m) { + CheckMatMinN(m); +} + +/** Check for a matrix with minimum dimensions 3x3 */ +template< class MatT > inline void +CheckMatMin3x3(const MatT& m) { + CheckMatMinN(m); +} + +/** Check for a matrix with minimum dimensions 4x4 */ +template< class MatT > inline void +CheckMatMin4x4(const MatT& m) { + CheckMatMinN(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(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(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(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(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 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 quaternion_traits; + typedef typename quaternion_traits::result_tag result_type; + + CML_STATIC_REQUIRE_M( + (same_type::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 index 0000000..a42501d --- /dev/null +++ b/src/cml/mathlib/coord_conversion.h @@ -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 +#include +#include + +/* 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& v) +{ + typedef vector 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& v) +{ + typedef vector vector_type; + typedef typename vector_type::value_type value_type; + + /* Checking */ + detail::CheckVec3(v); + detail::CheckIndex3(axis); + + if (type == latitude) { + phi = constants::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& 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::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::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::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::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 index 0000000..14059fa --- /dev/null +++ b/src/cml/mathlib/epsilon.h @@ -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 index 0000000..6a707af --- /dev/null +++ b/src/cml/mathlib/frustum.h @@ -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 +#include + +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 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 index 0000000..054cb55 --- /dev/null +++ b/src/cml/mathlib/helper.h @@ -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 +#include + +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 index 0000000..4a9fd54 --- /dev/null +++ b/src/cml/mathlib/interpolation.h @@ -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 + +/* 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 traits_1; + typedef et::ExprTraits 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::is_true), + function_expects_args_of_same_type_error); + + typedef typename et::ScalarPromote::type temporary_type; +}; + +template< class T1, class T2 > +struct TypePromote< T1,T2,et::vector_result_tag > { + typedef et::ExprTraits traits_1; + typedef et::ExprTraits 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::is_true), + function_expects_args_of_same_type_error); + + /* @todo: This should be VectorPromote<> for symmetry with the other + * type promotions. + */ + typedef typename CrossPromote::promoted_vector temporary_type; +}; + +template< class T1, class T2 > +struct TypePromote< T1,T2,et::matrix_result_tag > { + typedef et::ExprTraits traits_1; + typedef et::ExprTraits 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::is_true), + function_expects_args_of_same_type_error); + + typedef typename et::MatrixPromote2::temporary_type temporary_type; +}; + +template< class T1, class T2 > +struct TypePromote< T1,T2,et::quaternion_result_tag > { + typedef et::ExprTraits traits_1; + typedef et::ExprTraits 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::is_true), + function_expects_args_of_same_type_error); + + typedef typename et::QuaternionPromote2::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 traits_1; + typedef et::ExprTraits traits_2; + typedef et::ExprTraits 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::is_true), + function_expects_args_of_same_type_error); + CML_STATIC_REQUIRE_M( + (same_type::is_true), + function_expects_args_of_same_type_error); + + typedef typename et::MatrixPromote3::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 traits_1; + typedef et::ExprTraits traits_2; + typedef et::ExprTraits 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::is_true), + function_expects_args_of_same_type_error); + CML_STATIC_REQUIRE_M( + (same_type::is_true), + function_expects_args_of_same_type_error); + + typedef typename et::QuaternionPromote3::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 traits_1; + typedef et::ExprTraits traits_2; + typedef et::ExprTraits traits_3; + typedef et::ExprTraits 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::is_true), + function_expects_args_of_same_type_error); + CML_STATIC_REQUIRE_M( + (same_type::is_true), + function_expects_args_of_same_type_error); + CML_STATIC_REQUIRE_M( + (same_type::is_true), + function_expects_args_of_same_type_error); + + typedef typename et::MatrixPromote4::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 traits_1; + typedef et::ExprTraits traits_2; + typedef et::ExprTraits traits_3; + typedef et::ExprTraits 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::is_true), + function_expects_args_of_same_type_error); + CML_STATIC_REQUIRE_M( + (same_type::is_true), + function_expects_args_of_same_type_error); + CML_STATIC_REQUIRE_M( + (same_type::is_true), + function_expects_args_of_same_type_error); + + typedef typename et::QuaternionPromote4::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& 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& 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::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::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::result_tag +>::temporary_type +squad_intermediate( + const T1& t1, + const T2& t2, + const T3& t3, + typename detail::TypePromote3< + T1, T2, T3, typename et::ExprTraits::result_tag + >::value_type tolerance, + et::quaternion_result_tag, + SizeT) +{ + typedef et::ExprTraits traits_1; + typedef typename traits_1::result_tag result_type_1; + + typedef typename detail::TypePromote3::temporary_type + temporary_type; + typedef typename temporary_type::value_type value_type; + typedef typename temporary_type::cross_type cross_type; + typedef et::ExprTraits 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 +{ + 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 +{ + 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()( + 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()(m1,m2,m3,tolerance); + break; + case 2: + m = squad_intermediate_f()(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::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 type_traits; + typedef typename type_traits::result_tag result_type; + typedef typename + detail::TypePromote::temporary_type + temporary_type; + typedef typename temporary_type::value_type value_type; + typedef et::ExprTraits 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::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 type_traits; + typedef typename type_traits::result_tag result_type; + typedef typename + detail::TypePromote::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 +{ + template< typename Real > + typename detail::TypePromote< + MatT_1,MatT_2,typename et::ExprTraits::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::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 +{ + template< typename Real > + typename detail::TypePromote< + MatT_1,MatT_2,typename et::ExprTraits::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::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::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()(m1,m2,t,tolerance); +} + +template< class MatT_1, class MatT_2, typename Real > +typename detail::TypePromote< + MatT_1,MatT_2,typename et::ExprTraits::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::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()(m1,m2,t,tolerance); + break; + case 2: + m = slerp_f()(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::result_tag +>::temporary_type +nlerp( + const VecT_1& v1, + const VecT_2& v2, + Real t, + et::vector_result_tag, + SizeT) +{ + typedef et::ExprTraits type_traits; + typedef typename type_traits::result_tag result_type; + typedef typename + detail::TypePromote::temporary_type + temporary_type; + typedef typename temporary_type::value_type value_type; + typedef et::ExprTraits 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::result_tag +>::temporary_type +nlerp( + const QuatT_1& q1, + const QuatT_2& q2, + Real t, + et::quaternion_result_tag, + SizeT) +{ + typedef et::ExprTraits type_traits; + typedef typename type_traits::result_tag result_type; + typedef typename + detail::TypePromote::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 +{ + template< typename Real > + typename detail::TypePromote< + MatT_1,MatT_2,typename et::ExprTraits::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::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 +{ + template< typename Real > + typename detail::TypePromote< + MatT_1,MatT_2,typename et::ExprTraits::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::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::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()(m1,m2,t); +} + +template< class MatT_1, class MatT_2, typename Real > +typename detail::TypePromote< + MatT_1,MatT_2,typename et::ExprTraits::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::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()(m1,m2,t); + break; + case 2: + m = nlerp_f()(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::result_tag +>::temporary_type +squad_intermediate( + const T1& t1, + const T2& t2, + const T3& t3, + typename detail::TypePromote3< + T1, T2, T3, typename et::ExprTraits::result_tag + >::value_type tolerance = + epsilon < + typename detail::TypePromote3< + T1, T2, T3, typename et::ExprTraits::result_tag + >::value_type + >::placeholder()) +{ + // HACK: See note above... + detail::CheckQuat(t1); + detail::CheckQuat(t2); + detail::CheckQuat(t3); + + typedef et::ExprTraits traits_1; + typedef typename traits_1::result_tag result_type_1; + + typedef typename detail::TypePromote3::temporary_type + temporary_type; + typedef et::ExprTraits 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::result_tag +>::temporary_type +squad( + const T1& t1, + const T2& t1_intermediate, + const T3& t2_intermediate, + const T4& t2, + Real t, + Real tolerance = epsilon::placeholder()) +{ + // HACK: See note above... + detail::CheckQuat(t1); + detail::CheckQuat(t1_intermediate); + detail::CheckQuat(t2_intermediate); + detail::CheckQuat(t2); + + typedef et::ExprTraits 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 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::result_tag +>::temporary_type +slerp( + const T1& t1, + const T2& t2, + Real t, + Real tolerance = epsilon::placeholder()) +{ + typedef et::ExprTraits traits_1; + typedef typename traits_1::result_tag result_type_1; + + typedef typename detail::TypePromote::temporary_type + temporary_type; + typedef et::ExprTraits 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::result_tag +>::temporary_type +nlerp(const T1& t1, const T2& t2, Real t) +{ + typedef et::ExprTraits traits_1; + typedef typename traits_1::result_tag result_type_1; + + typedef typename detail::TypePromote::temporary_type + temporary_type; + typedef et::ExprTraits 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::result_tag +>::temporary_type +lerp(const T1& val0, const T2& val1, Scalar u) +{ + typedef + typename detail::TypePromote< + T1,T2,typename et::ExprTraits::result_tag + >::temporary_type temporary_type; + + typedef et::ExprTraits 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::result_tag + >::temporary_type, + typename detail::TypePromote< + T3,T4,typename et::ExprTraits::result_tag + >::temporary_type, + typename et::ExprTraits::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::result_tag + >::temporary_type, + typename detail::TypePromote< + T3,T4,typename et::ExprTraits::result_tag + >::temporary_type, + typename et::ExprTraits::result_tag + >::temporary_type temporary_type; + + typedef et::ExprTraits 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::result_tag + >::temporary_type, + typename detail::TypePromote< + T3,T4,typename et::ExprTraits::result_tag + >::temporary_type, + typename et::ExprTraits::result_tag + >::temporary_type, + typename detail::TypePromote< + typename detail::TypePromote< + T5,T6,typename et::ExprTraits::result_tag + >::temporary_type, + typename detail::TypePromote< + T7,T8,typename et::ExprTraits::result_tag + >::temporary_type, + typename et::ExprTraits::result_tag + >::temporary_type, + typename et::ExprTraits::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::result_tag + >::temporary_type, + typename detail::TypePromote< + T3,T4,typename et::ExprTraits::result_tag + >::temporary_type, + typename et::ExprTraits::result_tag + >::temporary_type, + typename detail::TypePromote< + typename detail::TypePromote< + T5,T6,typename et::ExprTraits::result_tag + >::temporary_type, + typename detail::TypePromote< + T7,T8,typename et::ExprTraits::result_tag + >::temporary_type, + typename et::ExprTraits::result_tag + >::temporary_type, + typename et::ExprTraits::result_tag + >::temporary_type temporary_type; + + typedef et::ExprTraits 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 index 0000000..d15c5f2 --- /dev/null +++ b/src/cml/mathlib/mathlib.h @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#endif diff --git a/src/cml/mathlib/matrix_basis.h b/src/cml/mathlib/matrix_basis.h new file mode 100644 index 0000000..7732270 --- /dev/null +++ b/src/cml/mathlib/matrix_basis.h @@ -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 + +/* 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& x, vector& y, vector& 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& x, vector& y, vector& 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& x,vector& 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& x, vector& 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 index 0000000..8e789aa --- /dev/null +++ b/src/cml/mathlib/matrix_concat.h @@ -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 + +/* 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 index 0000000..c36a399 --- /dev/null +++ b/src/cml/mathlib/matrix_misc.h @@ -0,0 +1,135 @@ +/* -*- C++ -*- ------------------------------------------------------------ + +Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/ + +The Configurable Math Library (CML) is distributed under the terms of the +Boost Software License, v1.0 (see cml/LICENSE for details). + + *-----------------------------------------------------------------------*/ +/** @file + * @brief + */ + +#ifndef matrix_misc_h +#define matrix_misc_h + +#include + +/* 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& m) +{ + typedef matrix 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& 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& 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& 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& 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 index 0000000..f0087f2 --- /dev/null +++ b/src/cml/mathlib/matrix_ortho.h @@ -0,0 +1,60 @@ +/* -*- C++ -*- ------------------------------------------------------------ + +Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/ + +The Configurable Math Library (CML) is distributed under the terms of the +Boost Software License, v1.0 (see cml/LICENSE for details). + + *-----------------------------------------------------------------------*/ +/** @file + * @brief + */ + +#ifndef matrix_ortho_h +#define matrix_ortho_h + +#include + +/* 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& 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& 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 index 0000000..042b7b5 --- /dev/null +++ b/src/cml/mathlib/matrix_projection.h @@ -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 +#include + +/* 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& m, E left, E right, E bottom, E top, + E n, E f, Handedness handedness, + ZClip z_clip) +{ + typedef matrix 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& m, E width, E height, E n, E f, + Handedness handedness, ZClip z_clip) +{ + typedef matrix 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& 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& 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& 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& 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& m, E xfov, E aspect, E n, + E f, Handedness handedness, ZClip z_clip) +{ + typedef matrix 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& 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& 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& m, E yfov, E aspect, E n, + E f, Handedness handedness, ZClip z_clip) +{ + typedef matrix 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& 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& 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& m, E left, E right, E bottom, E top, + E n, E f, Handedness handedness, + ZClip z_clip) +{ + typedef matrix 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& m, E width, E height, E n, E f, + Handedness handedness, ZClip z_clip) +{ + typedef matrix 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& 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& 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& 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& 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& 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& 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 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 index 0000000..9c3821d --- /dev/null +++ b/src/cml/mathlib/matrix_rotation.h @@ -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 +#include + +/* 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& m, size_t axis, E angle) +{ + typedef matrix 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& 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& 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& 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& m, const VecT& axis, E angle) +{ + typedef matrix 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& m, const QuatT& q) +{ + typedef matrix 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& m, E angle_0, E angle_1, E angle_2, + EulerOrder order) +{ + typedef matrix 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& m, E angle) +{ + typedef matrix 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& 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& m, size_t axis, E angle) +{ + typedef matrix 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& 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& 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& 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& m, size_t axis, E angle) +{ + typedef matrix 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& 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& 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& 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& m, E angle) +{ + typedef matrix 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& 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& m, E t, + E tolerance = epsilon::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& m, E t, E tolerance = epsilon::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::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::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::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& axis, + E& angle, + E tolerance = epsilon::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::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::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 index 0000000..359ab2d --- /dev/null +++ b/src/cml/mathlib/matrix_transform.h @@ -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 +#include +#include + +/* 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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&m, const VecT& axis, E scale) +{ + typedef matrix matrix_type; + typedef typename matrix_type::value_type value_type; + + /* Checking */ + detail::CheckVec3(axis); + + matrix,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& m, const VecT& axis, + E scale) +{ + typedef matrix matrix_type; + typedef typename matrix_type::value_type value_type; + + /* Checking */ + detail::CheckVec2(axis); + + matrix,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& 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& 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& 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& 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& 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& 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& 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& m, size_t axis) +{ + typedef matrix 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& 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& 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& 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& m, size_t axis) +{ + typedef matrix 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& 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& 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& m, const VecT& normal) +{ + typedef matrix 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&m, const VecT& normal) +{ + typedef matrix 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& m, size_t axis) +{ + typedef matrix 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& 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& 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& 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& m, size_t axis) +{ + typedef matrix 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& 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& 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& m, const VecT& normal) +{ + typedef matrix 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& m, const VecT& normal) +{ + typedef matrix 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& 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& 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& 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& 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& 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& m, + const VecT_1& eye, + const VecT_2& target, + const VecT_3& up, + Handedness handedness) +{ + typedef matrix 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& 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& 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& 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& 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& 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& 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& 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 void +matrix_affine_transform(matrix& 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& m, const quaternion& 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& m, const et::QuaternionXpr& 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& 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& 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& m, + const matrix& 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& m, const et::MatrixXpr& 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 void +matrix_affine_transform_2D(matrix& 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 +void matrix_affine_transform_2D(matrix& 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& 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& 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& 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& rotation, + vector& translation) +{ + typedef MatT matrix_type; + typedef typename matrix_type::value_type value_type; + typedef vector > 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& rotation, + vector& 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& translation, + Real tolerance = epsilon::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& axis, + Real& angle, + vector& translation, + Real tolerance = epsilon::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& rotation, + vector& translation) +{ + typedef MatT matrix_type; + typedef typename matrix_type::value_type value_type; + typedef vector > 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& 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 index 0000000..dadf4c5 --- /dev/null +++ b/src/cml/mathlib/matrix_translation.h @@ -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 + +/* 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& 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& m, E x, E y) +{ + typedef matrix 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& 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& 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& 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 index 0000000..1017690 --- /dev/null +++ b/src/cml/mathlib/misc.h @@ -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 + +/* 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 > zero() +{ + typedef vector< double, fixed > vector_type; + + vector_type result; + result.zero(); + return result; +} + +/** Return an N-d cardinal axis by index */ +template < size_t N > +vector< double, fixed > axis(size_t i) +{ + /* Checking */ + detail::CheckValidArg(i < N); + + typedef vector< double, fixed > 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, row_basis, row_major > zero() +{ + typedef matrix< double, fixed, 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, row_basis, row_major > identity() +{ + typedef matrix< double, fixed, 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, row_basis, row_major > identity_transform() +{ + typedef matrix< double, fixed, 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 index 0000000..75d1697 --- /dev/null +++ b/src/cml/mathlib/picking.h @@ -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 + +/* 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 = - 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& origin, + vector& direction, + bool normalize = true) +{ + typedef vector 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 = - 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 = - 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 index 0000000..7996851 --- /dev/null +++ b/src/cml/mathlib/projection.h @@ -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 +#include + +/* 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 +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 +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 index 0000000..5c4633e --- /dev/null +++ b/src/cml/mathlib/quaternion_basis.h @@ -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 + +/* 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& x, + vector& y, + vector& 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 index 0000000..2e8c0e9 --- /dev/null +++ b/src/cml/mathlib/quaternion_rotation.h @@ -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 + +/* 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& q, size_t axis, E angle) +{ + typedef quaternion 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& 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& 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& 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& q, const VecT& axis, E angle) +{ + typedef quaternion 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& q, const MatT& m) +{ + typedef quaternion 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& q, E angle_0, E angle_1, E angle_2, + EulerOrder order) +{ + typedef quaternion 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& q,size_t axis,E angle) +{ + typedef quaternion 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& 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& 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& 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& q,size_t axis,E angle) +{ + typedef quaternion 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& 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& 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& 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& q, + const VecT_1& v1, + const VecT_2& v2, + bool unit_length_vectors = false) +{ + typedef quaternion 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& q, E t, + E tolerance = epsilon::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::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::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::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& axis, + E& angle, + E tolerance = epsilon::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::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 index 0000000..f34843e --- /dev/null +++ b/src/cml/mathlib/typedef.h @@ -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 +#include +#include +#include +#include + +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,vector_first,negative_cross> + quaternionf_n; +typedef quaternion,vector_first,positive_cross> + quaternionf_p; +typedef quaternion,vector_first,negative_cross> + quaterniond_n; +typedef quaternion,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 constantsf; +typedef constants constantsd; + +/* epsilon/tolerance values (placeholder) */ +typedef epsilon epsilonf; +typedef epsilon epsilond; + +} // namespace cml + +#endif diff --git a/src/cml/mathlib/vector_angle.h b/src/cml/mathlib/vector_angle.h new file mode 100644 index 0000000..910817b --- /dev/null +++ b/src/cml/mathlib/vector_angle.h @@ -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 + +/* 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::promoted_vector, VecT_3 +>::promoted_scalar +signed_angle(const VecT_1& v1, const VecT_2& v2, const VecT_3& reference) +{ + typedef typename detail::CrossPromote::promoted_vector + vector_type; + typedef typename detail::DotPromote::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 index 0000000..6553342 --- /dev/null +++ b/src/cml/mathlib/vector_misc.h @@ -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 + +/* 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::promoted_vector +project_to_hplane(const VecT_1& v, const VecT_2& n) +{ + typedef typename detail::CrossPromote::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::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& v) +{ + typedef vector 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::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::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& v, const VecT& axis, E theta) +{ + typedef vector 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::pi(), + constants::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 index 0000000..88506f5 --- /dev/null +++ b/src/cml/mathlib/vector_ortho.h @@ -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 +#include + +/* 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& v0, vector& v1, vector& 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& v0, vector& 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_', 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& x, + vector& y, + vector& 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& x, + vector& y, + vector& 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& x, + vector& y, + vector& 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, + vector& x, + vector& y, + vector& z, + Handedness handedness, + AxisOrder order = axis_order_zyx) +{ + typedef MatT matrix_type; + typedef typename matrix_type::value_type value_type; + + orthonormal_basis( + -(handedness == left_handed ? value_type(1) : value_type(-1)) * + matrix_get_transposed_z_basis_vector(view_matrix), + matrix_get_transposed_y_basis_vector(view_matrix), + x, y, z, false, order + ); +} + +/** Build a viewplane-oriented basis from a left-handedness view matrix. */ +template < class MatT, typename E, class A > +void orthonormal_basis_viewplane_LH( + const MatT& view_matrix, + vector& x, + vector& y, + vector& z, + AxisOrder order = axis_order_zyx) +{ + orthonormal_basis_viewplane( + view_matrix,x,y,z,left_handed,order); +} + +/** Build a viewplane-oriented basis from a right-handedness view matrix. */ +template < class MatT, typename E, class A > +void orthonormal_basis_viewplane_RH( + const MatT& view_matrix, + vector& x, + vector& y, + vector& z, + AxisOrder order = axis_order_zyx) +{ + orthonormal_basis_viewplane( + view_matrix,x,y,z,right_handed,order); +} + +/* Build a 2D orthonormal basis. */ +template < class VecT, typename E, class A > +void orthonormal_basis_2D( + const VecT& align, + vector& x, + vector& y, + bool normalize_align = true, + AxisOrder2D order = axis_order_xy) +{ + typedef vector< E,fixed<2> > vector_type; + + /* Checking handled by perp() and assignment to fixed<2>. */ + + size_t i, j; + bool odd; + detail::unpack_axis_order_2D(order, i, j, odd); + + vector_type axis[2]; + + axis[i] = normalize_align ? normalize(align) : align; + axis[j] = perp(axis[i]); + + if (odd) { + axis[j] = -axis[j]; + } + + x = axis[0]; + y = axis[1]; +} + +} // namespace cml + +#endif diff --git a/src/cml/mathlib/vector_transform.h b/src/cml/mathlib/vector_transform.h new file mode 100644 index 0000000..1e71e81 --- /dev/null +++ b/src/cml/mathlib/vector_transform.h @@ -0,0 +1,156 @@ +/* -*- 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_transform_h +#define vector_transform_h + +#include + +/* Functions for transforming a vector, representing a geometric point or + * or vector, by an affine transfom. + * + * Note: This functionality may be provisional, depending on what architecture + * we settle on for the higher-level math functions. If we do keep these + * functions, then this code may ending up being a placeholder for expression + * template code. + */ + +namespace cml { + +/** A fixed-size temporary 4D vector */ +#define TEMP_VEC4 vector< \ + typename et::ScalarPromote< \ + typename MatT::value_type, \ + typename VecT::value_type \ + >::type, \ + fixed<4> \ +> + +/** A fixed-size temporary 3D vector */ +#define TEMP_VEC3 vector< \ + typename et::ScalarPromote< \ + typename MatT::value_type, \ + typename VecT::value_type \ + >::type, \ + fixed<3> \ +> + +/** A fixed-size temporary 2D vector */ +#define TEMP_VEC2 vector< \ + typename et::ScalarPromote< \ + typename MatT::value_type, \ + typename VecT::value_type \ + >::type, \ + fixed<2> \ +> + +namespace detail { + +template < class MatT, class VecT > TEMP_VEC4 +transform_vector_4D(const MatT& m, const VecT& v, row_basis) { + return v*m; +} + +template < class MatT, class VecT > TEMP_VEC4 +transform_vector_4D(const MatT& m, const VecT& v, col_basis) { + return m*v; +} + +} // namespace detail + +/** Apply a 4x4 homogeneous transform matrix to a 4D vector */ +template < class MatT, class VecT > TEMP_VEC4 +transform_vector_4D(const MatT& m, const VecT& v) { + return detail::transform_vector_4D(m,v,typename MatT::basis_orient()); +} + +/** Apply a 3D affine transform to a 3D point */ +template < class MatT, class VecT > TEMP_VEC3 +transform_point(const MatT& m, const VecT& v) +{ + typedef TEMP_VEC3 vector_type; + + /* Checking */ + detail::CheckMatAffine3D(m); + detail::CheckVec3(v); + + return vector_type( + m.basis_element(0,0)*v[0]+m.basis_element(1,0)*v[1]+ + m.basis_element(2,0)*v[2]+m.basis_element(3,0), + m.basis_element(0,1)*v[0]+m.basis_element(1,1)*v[1]+ + m.basis_element(2,1)*v[2]+m.basis_element(3,1), + m.basis_element(0,2)*v[0]+m.basis_element(1,2)*v[1]+ + m.basis_element(2,2)*v[2]+m.basis_element(3,2) + ); +} + +/** Apply a 3D affine transform to a 3D vector */ +template < class MatT, class VecT > TEMP_VEC3 +transform_vector(const MatT& m, const VecT& v) +{ + typedef TEMP_VEC3 vector_type; + + /* Checking */ + detail::CheckMatLinear3D(m); + detail::CheckVec3(v); + + return vector_type( + m.basis_element(0,0)*v[0]+m.basis_element(1,0)*v[1]+ + m.basis_element(2,0)*v[2], + m.basis_element(0,1)*v[0]+m.basis_element(1,1)*v[1]+ + m.basis_element(2,1)*v[2], + m.basis_element(0,2)*v[0]+m.basis_element(1,2)*v[1]+ + m.basis_element(2,2)*v[2] + ); +} + +/** Apply a 2D affine transform to a 2D point */ +template < class MatT, class VecT > TEMP_VEC2 +transform_point_2D(const MatT& m, const VecT& v) +{ + typedef TEMP_VEC2 vector_type; + + /* Checking */ + detail::CheckMatAffine2D(m); + detail::CheckVec2(v); + + return vector_type( + m.basis_element(0,0)*v[0]+m.basis_element(1,0)*v[1]+ + m.basis_element(2,0), + m.basis_element(0,1)*v[0]+m.basis_element(1,1)*v[1]+ + m.basis_element(2,1) + ); +} + +/** Apply a 2D affine transform to a 2D vector */ +template < class MatT, class VecT > TEMP_VEC2 +transform_vector_2D(const MatT& m, const VecT& v) +{ + typedef TEMP_VEC2 vector_type; + + /* Checking */ + detail::CheckMatLinear2D(m); + detail::CheckVec2(v); + + return vector_type( + m.basis_element(0,0)*v[0] + m.basis_element(1,0)*v[1], + m.basis_element(0,1)*v[0] + m.basis_element(1,1)*v[1] + ); +} + +#undef TEMP_VEC4 +#undef TEMP_VEC3 +#undef TEMP_VEC2 + +} // namespace cml + +#endif diff --git a/src/cml/matrix.h b/src/cml/matrix.h new file mode 100644 index 0000000..3573e38 --- /dev/null +++ b/src/cml/matrix.h @@ -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 + * + * The configurable matrix<> class. + */ + +#ifndef cml_matrix_h +#define cml_matrix_h + +#include + +namespace cml { + +/** A configurable matrix. + * + * This class encapsulates the notion of a matrix. The ArrayType template + * argument can be used to select the type of array to be used as internal + * storage for a 2D array of type Element. + * + * @internal Unlike the previous version, this uses specializations to better + * enable varied array and matrix types. For example, with the rebind method, + * it's difficult to support external<> matrix types that should not be + * assigned to. + * + * @internal All assignments to the matrix should go through UnrollAssignment, + * which ensures that the source expression and the destination matrix have + * the same size. This is particularly important for dynamically-sized + * matrices. + */ +template class matrix; + +} // namespace cml + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/matrix/class_ops.h b/src/cml/matrix/class_ops.h new file mode 100644 index 0000000..d2a8dbd --- /dev/null +++ b/src/cml/matrix/class_ops.h @@ -0,0 +1,341 @@ +/* -*- 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 + * + * @note GCC4 requires a copy constructor to elide---it won't elide a + * compiler-generated copy constructor! + */ + +#ifndef matrix_class_ops_h +#define matrix_class_ops_h + +#if defined(_MSC_VER) && _MSC_VER < 1400 +#pragma warning(disable:4003) +// XXX Horrible hack to turn off warnings about "not enough actual params" +// for the macros below. +#endif + +/* This is to circumvent the problem of commas in a macro argument. It's + * used to instantiate CML_ACCUMULATED_MATRIX_MULT: + */ +#define TEMPLATED_MATRIX_MACRO matrix + +/* XXX HACK!!! This is a hack to resize in the assign() functions only when + * auto resizing is turned off. + */ +#if !defined(CML_MATRIX_RESIZE_ON_ASSIGNMENT) +#define _DO_MATRIX_SET_RESIZE(_R_,_C_) cml::et::detail::Resize(*this,_R_,_C_) +#else +#define _DO_MATRIX_SET_RESIZE(_R_,_C_) +#endif + +/** Set a matrix from 2x2 values. + * + * The layout assumed for the values is that of the matrix being assigned. + */ +#define CML_ASSIGN_MAT_22 \ +matrix_type& \ +set( \ + ELEMENT_ARG_TYPE e00, ELEMENT_ARG_TYPE e01, \ + ELEMENT_ARG_TYPE e10, ELEMENT_ARG_TYPE e11 \ + ) \ +{ \ + _DO_MATRIX_SET_RESIZE(2,2); \ + /* This is overkill, but simplifies size checking: */ \ + value_type v[2][2] = {{e00,e01},{e10,e11}}; \ + typedef et::OpAssign OpT; \ + typedef const value_type element; \ + cml::matrix, basis_orient, row_major> \ + src(&v[0][0]); \ + et::UnrollAssignment(*this,src); \ + return *this; \ +} + +/** Create a matrix from 3x3 values. + * + * The layout assumed for the values is that of the matrix being assigned. + */ +#define CML_ASSIGN_MAT_33 \ +matrix_type& \ +set( \ + ELEMENT_ARG_TYPE e00, ELEMENT_ARG_TYPE e01, ELEMENT_ARG_TYPE e02, \ + ELEMENT_ARG_TYPE e10, ELEMENT_ARG_TYPE e11, ELEMENT_ARG_TYPE e12, \ + ELEMENT_ARG_TYPE e20, ELEMENT_ARG_TYPE e21, ELEMENT_ARG_TYPE e22 \ + ) \ +{ \ + _DO_MATRIX_SET_RESIZE(3,3); \ + /* This is overkill, but simplifies size checking: */ \ + value_type v[3][3] = { \ + {e00,e01,e02}, \ + {e10,e11,e12}, \ + {e20,e21,e22} \ + }; \ + typedef et::OpAssign OpT; \ + typedef const value_type element; \ + cml::matrix, basis_orient, row_major> \ + src(&v[0][0]); \ + et::UnrollAssignment(*this,src); \ + return *this; \ +} + +/** Create a matrix from 4x4 values. + * + * The layout assumed for the values is that of the matrix being assigned. + */ +#define CML_ASSIGN_MAT_44 \ +matrix_type& \ +set( \ + ELEMENT_ARG_TYPE e00, ELEMENT_ARG_TYPE e01, \ + ELEMENT_ARG_TYPE e02, ELEMENT_ARG_TYPE e03, \ + ELEMENT_ARG_TYPE e10, ELEMENT_ARG_TYPE e11, \ + ELEMENT_ARG_TYPE e12, ELEMENT_ARG_TYPE e13, \ + ELEMENT_ARG_TYPE e20, ELEMENT_ARG_TYPE e21, \ + ELEMENT_ARG_TYPE e22, ELEMENT_ARG_TYPE e23, \ + ELEMENT_ARG_TYPE e30, ELEMENT_ARG_TYPE e31, \ + ELEMENT_ARG_TYPE e32, ELEMENT_ARG_TYPE e33 \ + ) \ +{ \ + _DO_MATRIX_SET_RESIZE(4,4); \ + /* This is overkill, but simplifies size checking: */ \ + value_type v[4][4] = { \ + {e00,e01,e02,e03}, \ + {e10,e11,e12,e13}, \ + {e20,e21,e22,e23}, \ + {e30,e31,e32,e33} \ + }; \ + typedef et::OpAssign OpT; \ + typedef const value_type element; \ + cml::matrix, basis_orient, row_major> \ + src(&v[0][0]); \ + et::UnrollAssignment(*this,src); \ + return *this; \ +} + +/** Create a matrix from 2x2 values. + * + * The layout assumed for the values is that of the matrix being assigned. + */ +#define CML_CONSTRUCT_MAT_22 \ +matrix( \ + ELEMENT_ARG_TYPE e00, ELEMENT_ARG_TYPE e01, \ + ELEMENT_ARG_TYPE e10, ELEMENT_ARG_TYPE e11 \ + ) \ +{ \ + set( \ + e00,e01, \ + e10,e11 \ + ); \ +} + +/** Create a matrix from 3x3 values. + * + * The layout assumed for the values is that of the matrix being assigned. + */ +#define CML_CONSTRUCT_MAT_33 \ +matrix( \ + ELEMENT_ARG_TYPE e00, ELEMENT_ARG_TYPE e01, ELEMENT_ARG_TYPE e02, \ + ELEMENT_ARG_TYPE e10, ELEMENT_ARG_TYPE e11, ELEMENT_ARG_TYPE e12, \ + ELEMENT_ARG_TYPE e20, ELEMENT_ARG_TYPE e21, ELEMENT_ARG_TYPE e22 \ + ) \ +{ \ + set( \ + e00,e01,e02, \ + e10,e11,e12, \ + e20,e21,e22 \ + ); \ +} + +/** Create a matrix from 4x4 values. + * + * The layout assumed for the values is that of the matrix being assigned. + */ +#define CML_CONSTRUCT_MAT_44 \ +matrix( \ + ELEMENT_ARG_TYPE e00, ELEMENT_ARG_TYPE e01, \ + ELEMENT_ARG_TYPE e02, ELEMENT_ARG_TYPE e03, \ + ELEMENT_ARG_TYPE e10, ELEMENT_ARG_TYPE e11, \ + ELEMENT_ARG_TYPE e12, ELEMENT_ARG_TYPE e13, \ + ELEMENT_ARG_TYPE e20, ELEMENT_ARG_TYPE e21, \ + ELEMENT_ARG_TYPE e22, ELEMENT_ARG_TYPE e23, \ + ELEMENT_ARG_TYPE e30, ELEMENT_ARG_TYPE e31, \ + ELEMENT_ARG_TYPE e32, ELEMENT_ARG_TYPE e33 \ + ) \ +{ \ + set( \ + e00,e01,e02,e03, \ + e10,e11,e12,e13, \ + e20,e21,e22,e23, \ + e30,e31,e32,e33 \ + ); \ +} + +/** Copy-construct a matrix from a fixed-size array of values. */ +#define CML_MAT_COPY_FROM_FIXED_ARRAY(_R_,_C_) \ +matrix(const value_type m[_R_][_C_]) { \ + typedef et::OpAssign OpT; \ + cml::matrix, \ + basis_orient, row_major> src(&m[0][0]); \ + et::UnrollAssignment(*this,src); \ +} + +/** Copy-construct a matrix from a runtime-sized array of values. */ +#define CML_MAT_COPY_FROM_ARRAY(_add_) \ +matrix(const value_type* const v, size_t R, size_t C) _add_ { \ + typedef et::OpAssign OpT; \ + cml::matrix, basis_orient, \ + row_major > src(const_cast(v),R,C); \ + et::UnrollAssignment(*this,src); \ +} + +/** Copy this matrix from another using the given elementwise op. + * + * @internal This is required for GCC4, since it won't elide the default + * copy constructor. + */ +#define CML_MAT_COPY_FROM_MATTYPE \ +matrix(const matrix_type& m) : array_type() { \ + typedef et::OpAssign OpT; \ + et::UnrollAssignment(*this,m); \ +} + +/** Copy this matrix from another using the given elementwise op. + * + * This allows copies from arbitrary matrix types. + */ +#define CML_MAT_COPY_FROM_MAT \ +template \ +matrix(const TEMPLATED_MATRIX_MACRO& m) { \ + typedef et::OpAssign OpT; \ + et::UnrollAssignment(*this,m); \ +} + +/** Declare a function to copy this matrix from a matrix expression. */ +#define CML_MAT_COPY_FROM_MATXPR \ +template \ +matrix(MATXPR_ARG_TYPE e) { \ + /* Verify that a promotion exists at compile time: */ \ + typedef typename et::MatrixPromote< \ + matrix_type, typename XprT::result_type>::type result_type; \ + typedef typename XprT::value_type src_value_type; \ + typedef et::OpAssign OpT; \ + et::UnrollAssignment(*this,e); \ +} + +#if defined(CML_USE_GENERATED_MATRIX_ASSIGN_OP) +#define CML_MAT_ASSIGN_FROM_MATTYPE +#else +/** Assign from the same matrix type. + * + * @param m the matrix to copy from. + * + * @note This is required for GCC4, otherwise it generates a slow + * assignment operator using memcpy. + * + * @note ICC9/Linux-x86 seems to prefer its own assignment operator (need + * to figure out why). + */ +#define CML_MAT_ASSIGN_FROM_MATTYPE \ +matrix_type& operator=(const matrix_type& m) { \ + typedef et::OpAssign OpT; \ + et::UnrollAssignment(*this,m); \ + return *this; \ +} +#endif + + +/** Assign this matrix from another using the given elementwise op. + * + * This allows assignment from arbitrary matrix types. + * + * @param _op_ the operator (e.g. +=) + * @param _op_name_ the op functor (e.g. et::OpAssign) + */ +#define CML_MAT_ASSIGN_FROM_MAT(_op_, _op_name_) \ +template matrix_type& \ +operator _op_ (const TEMPLATED_MATRIX_MACRO& m) { \ + typedef _op_name_ OpT; \ + et::UnrollAssignment(*this,m); \ + return *this; \ +} + +/** Declare a function to assign this matrix from a matrix expression. + * + * @param _op_ the operator (e.g. +=) + * @param _op_name_ the op functor (e.g. et::OpAssign) + */ +#define CML_MAT_ASSIGN_FROM_MATXPR(_op_, _op_name_) \ +template matrix_type& \ +operator _op_ (MATXPR_ARG_TYPE e) { \ + /* Verify that a promotion exists at compile time: */ \ + typedef typename et::MatrixPromote< \ + matrix_type, typename XprT::result_type>::type result_type; \ + typedef typename XprT::value_type src_value_type; \ + typedef _op_name_ OpT; \ + et::UnrollAssignment(*this,e); \ + return *this; \ +} + +/** Declare a function to assign this matrix from a scalar. + * + * @param _op_ the operator (e.g. +=) + * @param _op_name_ the op functor (e.g. et::OpAssign) + * + * @internal This shouldn't be used for ops, like +=, which aren't + * defined in vector algebra. + */ +#define CML_MAT_ASSIGN_FROM_SCALAR(_op_, _op_name_) \ +matrix_type& operator _op_ (ELEMENT_ARG_TYPE s) { \ + typedef _op_name_ OpT; \ + et::UnrollAssignment(*this,s); \ + return *this; \ +} + + +/** Accumulated matrix multiplication. + * + * @throws std::invalid_argument if the matrices are not square. + */ +#define CML_ACCUMULATED_MATRIX_MULT(_arg_type_) \ +matrix_type& operator*=(_arg_type_ m) { \ + typedef typename et::MatrixPromote< \ + matrix_type, _arg_type_>::type result_type; \ + cml::et::CheckedSquare(*this, typename result_type::size_tag()); \ + return (*this = (*this)*m); \ +} + + +/* These should only be used for testing: */ +#define CML_MATRIX_BRACE_OPERATORS \ +template struct row_ref { \ + typedef typename Matrix::reference reference; \ + reference operator[](size_t col) { return m(row,col); } \ + Matrix& m; \ + size_t row; \ +}; \ + \ +template struct const_row_ref { \ + typedef typename Matrix::const_reference const_reference; \ + const_reference operator[](size_t col) const { return m(row,col); } \ + const Matrix& m; \ + size_t row; \ +}; \ + \ +row_ref operator[](size_t row) { \ + row_ref ref = { *this, row }; return ref; \ +} \ + \ +const_row_ref operator[](size_t row) const { \ + const_row_ref ref = { *this, row }; return ref; \ +} + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/matrix/determinant.h b/src/cml/matrix/determinant.h new file mode 100644 index 0000000..fa2cba3 --- /dev/null +++ b/src/cml/matrix/determinant.h @@ -0,0 +1,192 @@ +/* -*- 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 Compute the determinant of a square matrix using LU factorization. + * + * @todo This should be specialized on the matrix size for small matrices. + */ + +#ifndef determinant_h +#define determinant_h + +#include + +namespace cml { +namespace detail { + +/* Need to use a functional, since template functions cannot be + * specialized. N is used to differentiate dimension only, so this can be + * used for any matrix size type: + */ +template struct determinant_f; + +/* 2x2 determinant. Despite being marked for fixed_size matrices, this can + * be used for dynamic-sized ones also: + */ +template +struct determinant_f +{ + typename MatT::value_type operator()(const MatT& M) const + { + return M(0,0)*M(1,1) - M(1,0)*M(0,1); + } + +}; + +/* 3x3 determinant. Despite being marked for fixed_size matrices, this can + * be used for dynamic-sized ones also: + */ +template +struct determinant_f +{ + /* [00 01 02] + * M = [10 11 12] + * [20 21 22] + */ + typename MatT::value_type operator()(const MatT& M) const + { + return M(0,0)*(M(1,1)*M(2,2) - M(1,2)*M(2,1)) + + M(0,1)*(M(1,2)*M(2,0) - M(1,0)*M(2,2)) + + M(0,2)*(M(1,0)*M(2,1) - M(1,1)*M(2,0)); + } + +}; + +/* 4x4 determinant. Despite being marked for fixed_size matrices, this can + * be used for dynamic-sized ones also: + */ +template +struct determinant_f +{ + /* [00 01 02 03] + * M = [10 11 12 13] + * [20 21 22 23] + * [30 31 32 33] + * + * |11 12 13| |10 12 13| + * C00 = |21 22 23| C01 = |20 22 23| + * |31 32 33| |30 32 33| + * + * |10 11 13| |10 11 12| + * C02 = |20 21 23| C03 = |20 21 22| + * |30 31 33| |30 31 32| + * + * d00 = 11 * (22*33 - 23*32) d01 = 10 * (22*33 - 23*32) + * + 12 * (23*31 - 21*33) + 12 * (23*30 - 20*33) + * + 13 * (21*32 - 22*31) + 13 * (20*32 - 22*30) + * + * d02 = 10 * (21*33 - 23*31) d03 = 10 * (21*32 - 22*31) + * + 11 * (23*30 - 20*33) + 11 * (22*30 - 20*32) + * + 13 * (20*31 - 21*30) + 12 * (20*31 - 21*30) + */ + typename MatT::value_type operator()(const MatT& M) const + { + /* Shorthand. */ + typedef typename MatT::value_type value_type; + + /* Common cofactors: */ + value_type m_22_33_23_32 = M(2,2)*M(3,3) - M(2,3)*M(3,2); + value_type m_23_30_20_33 = M(2,3)*M(3,0) - M(2,0)*M(3,3); + value_type m_20_31_21_30 = M(2,0)*M(3,1) - M(2,1)*M(3,0); + value_type m_21_32_22_31 = M(2,1)*M(3,2) - M(2,2)*M(3,1); + value_type m_23_31_21_33 = M(2,3)*M(3,1) - M(2,1)*M(3,3); + value_type m_20_32_22_30 = M(2,0)*M(3,2) - M(2,2)*M(3,0); + + value_type d00 = M(0,0)*( + M(1,1) * m_22_33_23_32 + + M(1,2) * m_23_31_21_33 + + M(1,3) * m_21_32_22_31); + + value_type d01 = M(0,1)*( + M(1,0) * m_22_33_23_32 + + M(1,2) * m_23_30_20_33 + + M(1,3) * m_20_32_22_30); + + value_type d02 = M(0,2)*( + M(1,0) * - m_23_31_21_33 + + M(1,1) * m_23_30_20_33 + + M(1,3) * m_20_31_21_30); + + value_type d03 = M(0,3)*( + M(1,0) * m_21_32_22_31 + + M(1,1) * - m_20_32_22_30 + + M(1,2) * m_20_31_21_30); + + return d00 - d01 + d02 - d03; + } + +}; + +/* General NxN determinant by LU factorization: */ +template +struct determinant_f +{ + typename MatT::value_type operator()(const MatT& M) const + { + /* Compute the LU factorization: */ + typename MatT::temporary_type LU = lu(M); + + /* The product of the diagonal entries is the determinant: */ + typename MatT::value_type det = LU(0,0); + for(size_t i = 1; i < LU.rows(); ++ i) + det *= LU(i,i); + return det; + } + +}; + +/* Generator for the determinant functional for fixed-size matrices: */ +template typename MatT::value_type +determinant(const MatT& M, fixed_size_tag) +{ + /* Require a square matrix: */ + cml::et::CheckedSquare(M, fixed_size_tag()); + return determinant_f()(M); +} + +/* Generator for the determinant functional for dynamic-size matrices: */ +template typename MatT::value_type +determinant(const MatT& M, dynamic_size_tag) +{ + /* Require a square matrix: */ + cml::et::CheckedSquare(M, dynamic_size_tag()); + + /* Dispatch based upon the matrix dimension: */ + switch(M.rows()) { + case 2: return determinant_f()(M); + case 3: return determinant_f()(M); + case 4: return determinant_f()(M); + default: return determinant_f()(M); // > 4x4. + } +} + +} // namespace detail + +/** Determinant of a matrix. */ +template inline E +determinant(const matrix& M) +{ + typedef typename matrix::size_tag size_tag; + return detail::determinant(M,size_tag()); +} + +/** Determinant of a matrix expression. */ +template inline typename XprT::value_type +determinant(const et::MatrixXpr& e) +{ + typedef typename et::MatrixXpr::size_tag size_tag; + return detail::determinant(e,size_tag()); +} + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/matrix/dynamic.h b/src/cml/matrix/dynamic.h new file mode 100644 index 0000000..1362210 --- /dev/null +++ b/src/cml/matrix/dynamic.h @@ -0,0 +1,310 @@ +/* -*- 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_matrix_h +#define dynamic_matrix_h + +#include +#include +#include +#include + +namespace cml { + +/** Resizeable, dynamic-memory matrix. */ +template +class matrix,BasisOrient,Layout> +: public dynamic_2D +{ + public: + + /* Shorthand for the generator: */ + typedef dynamic generator_type; + + /* Shorthand for the array type: */ + typedef dynamic_2D array_type; + + /* Shorthand for the type of this matrix: */ + typedef matrix matrix_type; + + /* For integration into the expression template code: */ + typedef matrix_type expr_type; + + /* For integration into the expression template code: */ + typedef matrix_type temporary_type; + + /* Standard: */ + typedef typename array_type::value_type value_type; + typedef typename array_type::reference reference; + typedef typename array_type::const_reference const_reference; + + /* For integration into the expression templates code: */ + typedef matrix_type& expr_reference; + typedef const matrix_type& expr_const_reference; + + /* For matching by basis: */ + typedef BasisOrient basis_orient; + + /* For matching by memory layout: */ + typedef typename array_type::layout layout; + + /* For matching by storage type: */ + typedef typename array_type::memory_tag memory_tag; + + /* For matching by size type if necessary: */ + typedef typename array_type::size_tag size_tag; + + /* For matching by resizability: */ + typedef typename array_type::resizing_tag resizing_tag; + + /* For matching by result type: */ + typedef cml::et::matrix_result_tag result_tag; + + /* For matching by assignability: */ + typedef cml::et::assignable_tag assignable_tag; + + /* To simplify the matrix transpose operator: */ + typedef matrix< + Element, + typename array_type::transposed_type::generator_type, + BasisOrient, + Layout + > transposed_type; + + /* To simplify the matrix row and column operators: */ + typedef vector< + Element, + typename array_type::row_array_type::generator_type + > row_vector_type; + + typedef vector< + Element, + typename array_type::col_array_type::generator_type + > col_vector_type; + + + public: + + /** Set this matrix to zero. */ + matrix_type& zero() { + typedef cml::et::OpAssign OpT; + cml::et::UnrollAssignment(*this,Element(0)); + return *this; + } + + /** Set this matrix to the identity. + * + * This only makes sense for a square matrix, but no error will be + * signaled if the matrix is not square. + */ + matrix_type& identity() { + for(size_t i = 0; i < this->rows(); ++ i) { + for(size_t j = 0; j < this->cols(); ++ j) { + (*this)(i,j) = value_type((i == j)?1:0); + } + } + return *this; + } + + /** Set this matrix to its transpose. + * + * This only makes sense for a square matrix, but no error will be + * signaled if the matrix is not square. + */ + matrix_type& transpose() { + /* transpose() returns a temporary: */ + *this = cml::transpose(*this); + return *this; + } + + /** Set this matrix to its inverse. + * + * This only makes sense for a square matrix, but no error will be + * signaled if the matrix is not square. + */ + matrix_type& inverse() { + /* inverse() returns a temporary: */ + *this = cml::inverse(*this); + return *this; + } + + /* NOTE: minimize() and maximize() no longer supported (Jesse) */ + + #if 0 + /** Pairwise minimum of this matrix with another. */ + template + void minimize(const matrix& v) { + /* XXX This should probably use ScalarPromote: */ + for (size_t i = 0; i < this->rows(); ++i) { + for (size_t j = 0; j < this->cols(); ++j) { + (*this)(i,j) = std::min((*this)(i,j),v(i,j)); + } + } + } + + /** Pairwise maximum of this matrix with another. */ + template + void maximize(const matrix& v) { + /* XXX This should probably use ScalarPromote: */ + for (size_t i = 0; i < this->rows(); ++i) { + for (size_t j = 0; j < this->cols(); ++j) { + (*this)(i,j) = std::max((*this)(i,j),v(i,j)); + } + } + } + #endif + + /* Set each element to a random number in the range [min,max] */ + void random(ELEMENT_ARG_TYPE min, ELEMENT_ARG_TYPE max) { + for(size_t i = 0; i < this->rows(); ++i) { + for(size_t j = 0; j < this->cols(); ++j) { + (*this)(i,j) = cml::random_real(min,max); + } + } + } + + + public: + + /** Default constructor. */ + matrix() {} + + /** Constructor for dynamically-sized arrays. + * + * @param rows specify the number of rows. + * @param cols specify the number of cols. + */ + explicit matrix(size_t rows, size_t cols) + : array_type(rows,cols) {} + + + public: + + /** Return the matrix size as a pair. */ + matrix_size size() const { + return matrix_size(this->rows(),this->cols()); + } + + /** Return element j of basis vector i. */ + value_type basis_element(size_t i, size_t j) const { + return basis_element(i,j,basis_orient()); + } + + /** Set the given basis element. */ + void set_basis_element(size_t i, size_t j, ELEMENT_ARG_TYPE s) { + set_basis_element(i,j,s,basis_orient()); + } + + + public: + + /* Define common class operators: */ + + CML_CONSTRUCT_MAT_22 + CML_CONSTRUCT_MAT_33 + CML_CONSTRUCT_MAT_44 + + CML_MAT_COPY_FROM_ARRAY(: array_type()) + CML_MAT_COPY_FROM_MATTYPE + CML_MAT_COPY_FROM_MAT + CML_MAT_COPY_FROM_MATXPR + + CML_ASSIGN_MAT_22 + CML_ASSIGN_MAT_33 + CML_ASSIGN_MAT_44 + + CML_MAT_ASSIGN_FROM_MATTYPE + + CML_MAT_ASSIGN_FROM_MAT(=, et::OpAssign) + CML_MAT_ASSIGN_FROM_MAT(+=, et::OpAddAssign) + CML_MAT_ASSIGN_FROM_MAT(-=, et::OpSubAssign) + + CML_MAT_ASSIGN_FROM_MATXPR(=, et::OpAssign) + CML_MAT_ASSIGN_FROM_MATXPR(+=, et::OpAddAssign) + CML_MAT_ASSIGN_FROM_MATXPR(-=, et::OpSubAssign) + + CML_MAT_ASSIGN_FROM_SCALAR(*=, et::OpMulAssign) + CML_MAT_ASSIGN_FROM_SCALAR(/=, et::OpDivAssign) + + /** Accumulated matrix multiplication. + * + * This only makes sense for a square matrix, but no error will be + * signaled if the matrix is not square. + */ + matrix_type& operator*=(const matrix_type& m) { + /* Matrix multiplication returns a temporary: */ + *this = (*this)*m; + return *this; + } + + /** Accumulated matrix multiplication. + * + * This only makes sense for a square matrix, but no error will be + * signaled if the matrix is not square. + */ + template matrix_type& + operator*=(const matrix& m) { + /* Matrix multiplication returns a temporary: */ + *this = (*this)*m; + return *this; + } + + /** Accumulated matrix multiplication. + * + * This only makes sense for a square matrix, but no error will be + * signaled if the matrix is not square. + */ + template matrix_type& + operator*=(MATXPR_ARG_TYPE e) { + /* Verify that a promotion exists at compile time: */ + typedef typename et::MatrixPromote< + matrix_type, typename XprT::result_type>::type result_type; + /* Matrix multiplication returns a temporary: */ + *this = (*this)*e; + return *this; + } + + + protected: + + value_type basis_element(size_t i, size_t j, row_basis) const { + return (*this)(i,j); + } + + value_type basis_element(size_t i, size_t j, col_basis) const { + return (*this)(j,i); + } + + void set_basis_element(size_t i, size_t j, ELEMENT_ARG_TYPE s, row_basis) { + (*this)(i,j) = s; + } + + void set_basis_element(size_t i, size_t j, ELEMENT_ARG_TYPE s, col_basis) { + (*this)(j,i) = s; + } + + + public: + + /* Braces should only be used for testing: */ +#if defined(CML_ENABLE_MATRIX_BRACES) + CML_MATRIX_BRACE_OPERATORS +#endif +}; + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/matrix/external.h b/src/cml/matrix/external.h new file mode 100644 index 0000000..36c225a --- /dev/null +++ b/src/cml/matrix/external.h @@ -0,0 +1,531 @@ +/* -*- 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_matrix_h +#define external_matrix_h + +#include +#include +#include +#include +#include + +namespace cml { + +/** Fixed-size, external-memory matrix. */ +template +class matrix,BasisOrient,Layout> +: public external_2D +{ + public: + + /* Shorthand for the generator: */ + typedef external generator_type; + + /* Shorthand for the array type: */ + typedef external_2D array_type; + + /* Shorthand for the type of this matrix: */ + typedef matrix matrix_type; + + /* For integration into the expression template code: */ + typedef matrix_type expr_type; + + /* For integration into the expression template code: */ + typedef matrix,BasisOrient,Layout> temporary_type; + /* Note: this ensures that an external matrix is copied into the proper + * temporary; external<> temporaries are not allowed. + */ + + /* Standard: */ + typedef typename array_type::value_type value_type; + typedef typename array_type::reference reference; + typedef typename array_type::const_reference const_reference; + + typedef matrix_type& expr_reference; + typedef const matrix_type& expr_const_reference; + + /* For matching by basis: */ + typedef BasisOrient basis_orient; + + /* For matching by memory layout: */ + typedef typename array_type::layout layout; + + /* For matching by storage type if necessary: */ + typedef typename array_type::memory_tag memory_tag; + + /* For matching by size type if necessary: */ + typedef typename array_type::size_tag size_tag; + + /* For matching by resizability: */ + typedef typename array_type::resizing_tag resizing_tag; + + /* For matching by result-type: */ + typedef cml::et::matrix_result_tag result_tag; + + /* For matching by assignability: */ + typedef cml::et::assignable_tag assignable_tag; + + /* To simplify the matrix transpose operator: */ + typedef matrix< + Element, + typename array_type::transposed_type::generator_type, + BasisOrient, + Layout + > transposed_type; + + /* To simplify the matrix row and column operators: */ + typedef vector< + Element, + typename array_type::row_array_type::generator_type + > row_vector_type; + + typedef vector< + Element, + typename array_type::col_array_type::generator_type + > col_vector_type; + + + public: + + /** Set this matrix to zero. */ + matrix_type& zero() { + typedef cml::et::OpAssign OpT; + cml::et::UnrollAssignment(*this,Element(0)); + return *this; + } + + /** Set this matrix to the identity. + * + * This only makes sense for a square matrix, but no error will be + * signaled if the matrix is not square. + */ + matrix_type& identity() { + for(size_t i = 0; i < this->rows(); ++ i) { + for(size_t j = 0; j < this->cols(); ++ j) { + (*this)(i,j) = value_type((i == j)?1:0); + } + } + return *this; + } + + /** Set this matrix to its transpose. + * + * This only makes sense for a square matrix, but no error will be + * signaled if the matrix is not square. + */ + matrix_type& transpose() { + /* transpose() returns a temporary: */ + *this = transpose(*this); + return *this; + } + + /** Set this matrix to its inverse. + * + * This only makes sense for a square matrix, but no error will be + * signaled if the matrix is not square. + */ + matrix_type& inverse() { + /* inverse() returns a temporary: */ + *this = cml::inverse(*this); + return *this; + } + + /* NOTE: minimize() and maximize() no longer supported (Jesse) */ + + #if 0 + /** Pairwise minimum of this matrix with another. */ + template + void minimize(const matrix& v) { + /* XXX This should probably use ScalarPromote: */ + for (size_t i = 0; i < this->rows(); ++i) { + for (size_t j = 0; j < this->cols(); ++j) { + (*this)(i,j) = std::min((*this)(i,j),v(i,j)); + } + } + } + + /** Pairwise maximum of this matrix with another. */ + template + void maximize(const matrix& v) { + /* XXX This should probably use ScalarPromote: */ + for (size_t i = 0; i < this->rows(); ++i) { + for (size_t j = 0; j < this->cols(); ++j) { + (*this)(i,j) = std::max((*this)(i,j),v(i,j)); + } + } + } + #endif + + /* Set each element to a random number in the range [min,max] */ + void random(ELEMENT_ARG_TYPE min, ELEMENT_ARG_TYPE max) { + for(size_t i = 0; i < this->rows(); ++i) { + for(size_t j = 0; j < this->cols(); ++j) { + (*this)(i,j) = random_real(min,max); + } + } + } + + + public: + + /** Constructor for fixed-size external matrices. + * + * The array must be given as a pointer to Element*, not a + * multi-dimensional array. The caller owns the pointer, and is + * responsible for doing any necessary memory management. + * + * @param ptr specify the external pointer. + * + * @throws same as the ArrayType constructor. + */ + explicit matrix(value_type ptr[Rows][Cols]) : array_type(ptr) {} + + /** Constructor for fixed-size external matrices. + * + * The array must be given as a pointer to Element*, not a + * multi-dimensional array. The caller owns the pointer, and is + * responsible for doing any necessary memory management. + * + * @param ptr specify the external pointer. + * + * @throws same as the ArrayType constructor. + */ + explicit matrix(value_type* ptr) : array_type(ptr) {} + + + public: + + /** Return the matrix size as a pair. */ + matrix_size size() const { + return matrix_size(this->rows(),this->cols()); + } + + /** Return element j of basis vector i. */ + value_type basis_element(size_t i, size_t j) const { + return basis_element(i,j,basis_orient()); + } + + /** Set the given basis element. */ + void set_basis_element(size_t i, size_t j, ELEMENT_ARG_TYPE s) { + set_basis_element(i,j,s,basis_orient()); + } + + + public: + + CML_ASSIGN_MAT_22 + CML_ASSIGN_MAT_33 + CML_ASSIGN_MAT_44 + + /* Define class operators for external matrices. Note: external matrices + * cannot be copy-constructed, but they can be assigned to: + */ + CML_MAT_ASSIGN_FROM_MATTYPE + + CML_MAT_ASSIGN_FROM_MAT(=, et::OpAssign) + CML_MAT_ASSIGN_FROM_MAT(+=, et::OpAddAssign) + CML_MAT_ASSIGN_FROM_MAT(-=, et::OpSubAssign) + + CML_MAT_ASSIGN_FROM_MATXPR(=, et::OpAssign) + CML_MAT_ASSIGN_FROM_MATXPR(+=, et::OpAddAssign) + CML_MAT_ASSIGN_FROM_MATXPR(-=, et::OpSubAssign) + + CML_MAT_ASSIGN_FROM_SCALAR(*=, et::OpMulAssign) + CML_MAT_ASSIGN_FROM_SCALAR(/=, et::OpDivAssign) + + CML_ACCUMULATED_MATRIX_MULT(const matrix_type&) + + template + CML_ACCUMULATED_MATRIX_MULT(const TEMPLATED_MATRIX_MACRO&) + + template + CML_ACCUMULATED_MATRIX_MULT(MATXPR_ARG_TYPE) + + + protected: + + value_type basis_element(size_t i, size_t j, row_basis) const { + return (*this)(i,j); + } + + value_type basis_element(size_t i, size_t j, col_basis) const { + return (*this)(j,i); + } + + void set_basis_element(size_t i, size_t j, ELEMENT_ARG_TYPE s, row_basis) { + (*this)(i,j) = s; + } + + void set_basis_element(size_t i, size_t j, ELEMENT_ARG_TYPE s, col_basis) { + (*this)(j,i) = s; + } + + + public: + + /* Braces should only be used for testing: */ +#if defined(CML_ENABLE_MATRIX_BRACES) + CML_MATRIX_BRACE_OPERATORS +#endif +}; + +/** Dynamic-size, external-memory matrix. */ +template +class matrix,BasisOrient,Layout> +: public external_2D +{ + public: + + /* Shorthand for the generator: */ + typedef external<> generator_type; + + /* Shorthand for the array type: */ + typedef external_2D array_type; + + /* Shorthand for the type of this matrix: */ + typedef matrix matrix_type; + + /* For integration into the expression template code: */ + typedef matrix_type expr_type; + + /* For integration into the expression template code: */ + typedef matrix,BasisOrient,Layout> temporary_type; + /* Note: this ensures that an external matrix is copied into the proper + * temporary; external<> temporaries are not allowed. + */ + + /* Standard: */ + typedef typename array_type::value_type value_type; + typedef typename array_type::reference reference; + typedef typename array_type::const_reference const_reference; + + typedef matrix_type& expr_reference; + typedef const matrix_type& expr_const_reference; + + /* For matching by basis: */ + typedef BasisOrient basis_orient; + + /* For matching by memory layout: */ + typedef typename array_type::layout layout; + + /* For matching by storage type if necessary: */ + typedef typename array_type::memory_tag memory_tag; + + /* For matching by size type if necessary: */ + typedef typename array_type::size_tag size_tag; + + /* For matching by resizability: */ + typedef typename array_type::resizing_tag resizing_tag; + + /* For matching by result-type: */ + typedef cml::et::matrix_result_tag result_tag; + + /* For matching by assignability: */ + typedef cml::et::assignable_tag assignable_tag; + + /* To simplify the matrix transpose operator: */ + typedef matrix< + Element, + typename array_type::transposed_type::generator_type, + BasisOrient, + Layout + > transposed_type; + + /* To simplify the matrix row and column operators: */ + typedef vector< + Element, + typename array_type::row_array_type::generator_type + > row_vector_type; + + typedef vector< + Element, + typename array_type::col_array_type::generator_type + > col_vector_type; + + + public: + + /** Set this matrix to zero. */ + matrix_type& zero() { + typedef cml::et::OpAssign OpT; + cml::et::UnrollAssignment(*this,Element(0)); + return *this; + } + + /** Set this matrix to the identity. + * + * This only makes sense for a square matrix, but no error will be + * signaled if the matrix is not square. + */ + matrix_type& identity() { + for(size_t i = 0; i < this->rows(); ++ i) { + for(size_t j = 0; j < this->cols(); ++ j) { + (*this)(i,j) = value_type((i == j)?1:0); + } + } + return *this; + } + + /** Set this matrix to its transpose. + * + * This only makes sense for a square matrix, but no error will be + * signaled if the matrix is not square. + */ + matrix_type& transpose() { + /* transpose() returns a temporary: */ + *this = cml::transpose(*this); + return *this; + } + + /** Set this matrix to its inverse. + * + * This only makes sense for a square matrix, but no error will be + * signaled if the matrix is not square. + */ + matrix_type& inverse() { + /* inverse() returns a temporary: */ + *this = inverse(*this); + return *this; + } + + /** Pairwise minimum of this matrix with another. */ + template + void minimize(const matrix& v) { + /* XXX This should probably use ScalarPromote: */ + for (size_t i = 0; i < this->rows(); ++i) { + for (size_t j = 0; j < this->cols(); ++j) { + (*this)[i] = std::min((*this)(i,j),v(i,j)); + } + } + } + + /** Pairwise maximum of this matrix with another. */ + template + void maximize(const matrix& v) { + /* XXX This should probably use ScalarPromote: */ + for (size_t i = 0; i < this->rows(); ++i) { + for (size_t j = 0; j < this->cols(); ++j) { + (*this)[i] = std::max((*this)(i,j),v(i,j)); + } + } + } + + /* Set each element to a random number in the range [min,max] */ + void random(ELEMENT_ARG_TYPE min, ELEMENT_ARG_TYPE max) { + for(size_t i = 0; i < this->rows(); ++i) { + for(size_t j = 0; j < this->cols(); ++j) { + (*this)(i,j) = cml::random_real(min,max); + } + } + } + + + public: + + /** Constructor for fixed-size external matrices. + * + * The array must be given as a pointer to Element*, not a + * multi-dimensional array. The caller owns the pointer, and is + * responsible for doing any necessary memory management. + * + * @param ptr specify the external pointer. + * + * @throws same as the ArrayType constructor. + */ + explicit matrix(value_type* const ptr, size_t rows, size_t cols) + : array_type(ptr,rows,cols) {} + + + public: + + /** Return the matrix size as a pair. */ + matrix_size size() const { + return matrix_size(this->rows(),this->cols()); + } + + /** Return element j of basis vector i. */ + value_type basis_element(size_t i, size_t j) const { + return basis_element(i,j,basis_orient()); + } + + /** Set the given basis element. */ + void set_basis_element(size_t i, size_t j, ELEMENT_ARG_TYPE s) { + set_basis_element(i,j,s,basis_orient()); + } + + + public: + + CML_ASSIGN_MAT_22 + CML_ASSIGN_MAT_33 + CML_ASSIGN_MAT_44 + + /* Define class operators for external matrices. Note: external matrices + * cannot be copy-constructed, but they can be assigned to: + */ + CML_MAT_ASSIGN_FROM_MATTYPE + + CML_MAT_ASSIGN_FROM_MAT(=, et::OpAssign) + CML_MAT_ASSIGN_FROM_MAT(+=, et::OpAddAssign) + CML_MAT_ASSIGN_FROM_MAT(-=, et::OpSubAssign) + + CML_MAT_ASSIGN_FROM_MATXPR(=, et::OpAssign) + CML_MAT_ASSIGN_FROM_MATXPR(+=, et::OpAddAssign) + CML_MAT_ASSIGN_FROM_MATXPR(-=, et::OpSubAssign) + + CML_MAT_ASSIGN_FROM_SCALAR(*=, et::OpMulAssign) + CML_MAT_ASSIGN_FROM_SCALAR(/=, et::OpDivAssign) + + CML_ACCUMULATED_MATRIX_MULT(const matrix_type&) + + template + CML_ACCUMULATED_MATRIX_MULT(const TEMPLATED_MATRIX_MACRO&) + + template + CML_ACCUMULATED_MATRIX_MULT(MATXPR_ARG_TYPE) + + + protected: + + value_type basis_element(size_t i, size_t j, row_basis) const { + return (*this)(i,j); + } + + value_type basis_element(size_t i, size_t j, col_basis) const { + return (*this)(j,i); + } + + void set_basis_element(size_t i, size_t j, ELEMENT_ARG_TYPE s, row_basis) { + (*this)(i,j) = s; + } + + void set_basis_element(size_t i, size_t j, ELEMENT_ARG_TYPE s, col_basis) { + (*this)(j,i) = s; + } + + + public: + + /* Braces should only be used for testing: */ +#if defined(CML_ENABLE_MATRIX_BRACES) + CML_MATRIX_BRACE_OPERATORS +#endif +}; + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/matrix/fixed.h b/src/cml/matrix/fixed.h new file mode 100644 index 0000000..d20ccfa --- /dev/null +++ b/src/cml/matrix/fixed.h @@ -0,0 +1,275 @@ +/* -*- 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_matrix_h +#define fixed_matrix_h + +#include +#include +#include +#include + +namespace cml { + +/** Fixed-size, fixed-memory matrix. */ +template +class matrix,BasisOrient,Layout> +: public fixed_2D +{ + public: + + /* Shorthand for the generator: */ + typedef fixed generator_type; + + /* Shorthand for the array type: */ + typedef fixed_2D array_type; + + /* Shorthand for the type of this matrix: */ + typedef matrix matrix_type; + + /* For integration into the expression template code: */ + typedef matrix_type expr_type; + + /* For integration into the expression template code: */ + typedef matrix_type temporary_type; + + /* Standard: */ + typedef typename array_type::value_type value_type; + typedef typename array_type::reference reference; + typedef typename array_type::const_reference const_reference; + + typedef matrix_type& expr_reference; + typedef const matrix_type& expr_const_reference; + + /* For matching by basis: */ + typedef BasisOrient basis_orient; + + /* For matching by memory layout: */ + typedef typename array_type::layout layout; + + /* For matching by storage type if necessary: */ + typedef typename array_type::memory_tag memory_tag; + + /* For matching by size type if necessary: */ + typedef typename array_type::size_tag size_tag; + + /* For matching by result type: */ + typedef cml::et::matrix_result_tag result_tag; + + /* For matching by assignability: */ + typedef cml::et::assignable_tag assignable_tag; + + /* To simplify the matrix transpose operator: */ + typedef matrix< + Element, + typename array_type::transposed_type::generator_type, + BasisOrient, + Layout + > transposed_type; + + /* To simplify the matrix row and column operators: */ + typedef vector< + Element, + typename array_type::row_array_type::generator_type + > row_vector_type; + + typedef vector< + Element, + typename array_type::col_array_type::generator_type + > col_vector_type; + + + public: + + /** Set this matrix to zero. */ + matrix_type& zero() { + typedef cml::et::OpAssign OpT; + cml::et::UnrollAssignment(*this,Element(0)); + return *this; + } + + /** Set this matrix to the identity. + * + * This only makes sense for a square matrix, but no error will be + * signaled if the matrix is not square. + */ + matrix_type& identity() { + for(size_t i = 0; i < this->rows(); ++ i) { + for(size_t j = 0; j < this->cols(); ++ j) { + (*this)(i,j) = value_type((i == j)?1:0); + } + } + return *this; + } + + /** Set this matrix to its transpose. + * + * This only makes sense for a square matrix, but no error will be + * signaled if the matrix is not square. + */ + matrix_type& transpose() { + /* transpose() returns a temporary: */ + *this = cml::transpose(*this); + return *this; + } + + /** Set this matrix to its inverse. + * + * This only makes sense for a square matrix, but no error will be + * signaled if the matrix is not square. + */ + matrix_type& inverse() { + /* inverse() returns a temporary: */ + *this = cml::inverse(*this); + return *this; + } + + /* NOTE: minimize() and maximize() no longer supported (Jesse) */ + + #if 0 + /** Pairwise minimum of this matrix with another. */ + template + void minimize(const matrix& v) { + /* XXX This should probably use ScalarPromote: */ + for (size_t i = 0; i < this->rows(); ++i) { + for (size_t j = 0; j < this->cols(); ++j) { + (*this)(i,j) = std::min((*this)(i,j),v(i,j)); + } + } + } + + /** Pairwise maximum of this matrix with another. */ + template + void maximize(const matrix& v) { + /* XXX This should probably use ScalarPromote: */ + for (size_t i = 0; i < this->rows(); ++i) { + for (size_t j = 0; j < this->cols(); ++j) { + (*this)(i,j) = std::max((*this)(i,j),v(i,j)); + } + } + } + #endif + + /* Set each element to a random number in the range [min,max] */ + void random(ELEMENT_ARG_TYPE min, ELEMENT_ARG_TYPE max) { + for(size_t i = 0; i < this->rows(); ++i) { + for(size_t j = 0; j < this->cols(); ++j) { + (*this)(i,j) = cml::random_real(min,max); + } + } + } + + + public: + + /** Default constructor. + * + * @throws same as the ArrayType constructor. + * @sa cml::fixed + * @sa cml::dynamic + */ + matrix() {} + + + public: + + /** Return the matrix size as a pair. */ + matrix_size size() const { + return matrix_size(this->rows(),this->cols()); + } + + /** Return element j of basis vector i. */ + value_type basis_element(size_t i, size_t j) const { + return basis_element(i,j,basis_orient()); + } + + /** Set the given basis element. */ + void set_basis_element(size_t i, size_t j, ELEMENT_ARG_TYPE s) { + set_basis_element(i,j,s,basis_orient()); + } + + + public: + + /* Define common class operators: */ + + CML_CONSTRUCT_MAT_22 + CML_CONSTRUCT_MAT_33 + CML_CONSTRUCT_MAT_44 + + CML_MAT_COPY_FROM_FIXED_ARRAY( + array_type::array_rows, array_type::array_cols) + + CML_MAT_COPY_FROM_MATTYPE + CML_MAT_COPY_FROM_MAT + CML_MAT_COPY_FROM_MATXPR + + CML_ASSIGN_MAT_22 + CML_ASSIGN_MAT_33 + CML_ASSIGN_MAT_44 + + CML_MAT_ASSIGN_FROM_MATTYPE + + CML_MAT_ASSIGN_FROM_MAT(=, et::OpAssign) + CML_MAT_ASSIGN_FROM_MAT(+=, et::OpAddAssign) + CML_MAT_ASSIGN_FROM_MAT(-=, et::OpSubAssign) + + CML_MAT_ASSIGN_FROM_MATXPR(=, et::OpAssign) + CML_MAT_ASSIGN_FROM_MATXPR(+=, et::OpAddAssign) + CML_MAT_ASSIGN_FROM_MATXPR(-=, et::OpSubAssign) + + CML_MAT_ASSIGN_FROM_SCALAR(*=, et::OpMulAssign) + CML_MAT_ASSIGN_FROM_SCALAR(/=, et::OpDivAssign) + + CML_ACCUMULATED_MATRIX_MULT(const matrix_type&) + + template + CML_ACCUMULATED_MATRIX_MULT(const TEMPLATED_MATRIX_MACRO&) + + template + CML_ACCUMULATED_MATRIX_MULT(MATXPR_ARG_TYPE) + + + protected: + + value_type basis_element(size_t i, size_t j, row_basis) const { + return (*this)(i,j); + } + + value_type basis_element(size_t i, size_t j, col_basis) const { + return (*this)(j,i); + } + + void set_basis_element(size_t i, size_t j, ELEMENT_ARG_TYPE s, row_basis) { + (*this)(i,j) = s; + } + + void set_basis_element(size_t i, size_t j, ELEMENT_ARG_TYPE s, col_basis) { + (*this)(j,i) = s; + } + + + public: + + /* Braces should only be used for testing: */ +#if defined(CML_ENABLE_MATRIX_BRACES) + CML_MATRIX_BRACE_OPERATORS +#endif +}; + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/matrix/inverse.h b/src/cml/matrix/inverse.h new file mode 100644 index 0000000..4d80e6f --- /dev/null +++ b/src/cml/matrix/inverse.h @@ -0,0 +1,443 @@ +/* -*- 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 Compute the inverse of a matrix by LU factorization. + */ + +#ifndef matrix_inverse_h +#define matrix_inverse_h + +#include + +namespace cml { +namespace detail { + +/* Need to use a functional, since template functions cannot be + * specialized. _tag is used to specialize based upon dimension: + */ +template struct inverse_f; + +/* @todo: Reciprocal optimization for division by determinant. + */ + +/* 2x2 inverse. Despite being marked for fixed_size matrices, this can + * be used for dynamic-sized ones also: + */ +template +struct inverse_f +{ + typename MatT::temporary_type operator()(const MatT& M) const + { + typedef typename MatT::temporary_type temporary_type; + typedef typename temporary_type::value_type value_type; + + /* Matrix containing the inverse: */ + temporary_type Z; + cml::et::detail::Resize(Z,2,2); + + /* Compute determinant and inverse: */ + value_type D = value_type(1) / (M(0,0)*M(1,1) - M(0,1)*M(1,0)); + Z(0,0) = M(1,1)*D; Z(0,1) = - M(0,1)*D; + Z(1,0) = - M(1,0)*D; Z(1,1) = M(0,0)*D; + + return Z; + } +}; + +/* 3x3 inverse. Despite being marked for fixed_size matrices, this can + * be used for dynamic-sized ones also: + */ +template +struct inverse_f +{ + /* [00 01 02] + * M = [10 11 12] + * [20 21 22] + */ + typename MatT::temporary_type operator()(const MatT& M) const + { + /* Shorthand. */ + typedef typename MatT::value_type value_type; + + /* Compute cofactors for each entry: */ + value_type m_00 = M(1,1)*M(2,2) - M(1,2)*M(2,1); + value_type m_01 = M(1,2)*M(2,0) - M(1,0)*M(2,2); + value_type m_02 = M(1,0)*M(2,1) - M(1,1)*M(2,0); + + value_type m_10 = M(0,2)*M(2,1) - M(0,1)*M(2,2); + value_type m_11 = M(0,0)*M(2,2) - M(0,2)*M(2,0); + value_type m_12 = M(0,1)*M(2,0) - M(0,0)*M(2,1); + + value_type m_20 = M(0,1)*M(1,2) - M(0,2)*M(1,1); + value_type m_21 = M(0,2)*M(1,0) - M(0,0)*M(1,2); + value_type m_22 = M(0,0)*M(1,1) - M(0,1)*M(1,0); + + /* Compute determinant from the minors: */ + value_type D = + value_type(1) / (M(0,0)*m_00 + M(0,1)*m_01 + M(0,2)*m_02); + + /* Matrix containing the inverse: */ + typename MatT::temporary_type Z; + cml::et::detail::Resize(Z,3,3); + + /* Assign the inverse as (1/D) * (cofactor matrix)^T: */ + Z(0,0) = m_00*D; Z(0,1) = m_10*D; Z(0,2) = m_20*D; + Z(1,0) = m_01*D; Z(1,1) = m_11*D; Z(1,2) = m_21*D; + Z(2,0) = m_02*D; Z(2,1) = m_12*D; Z(2,2) = m_22*D; + + return Z; + } +}; + +/* 4x4 inverse. Despite being marked for fixed_size matrices, this can + * be used for dynamic-sized ones also: + */ +template +struct inverse_f +{ + /* [00 01 02 03] + * M = [10 11 12 13] + * [20 21 22 23] + * [30 31 32 33] + * + * |11 12 13| |10 12 13| + * C00 = |21 22 23| C01 = |20 22 23| + * |31 32 33| |30 32 33| + * + * |10 11 13| |10 11 12| + * C02 = |20 21 23| C03 = |20 21 22| + * |30 31 33| |30 31 32| + */ + typename MatT::temporary_type operator()(const MatT& M) const + { + /* Shorthand. */ + typedef typename MatT::value_type value_type; + + /* Common cofactors, rows 0,1: */ + value_type m_22_33_23_32 = M(2,2)*M(3,3) - M(2,3)*M(3,2); + value_type m_23_30_20_33 = M(2,3)*M(3,0) - M(2,0)*M(3,3); + value_type m_20_31_21_30 = M(2,0)*M(3,1) - M(2,1)*M(3,0); + value_type m_21_32_22_31 = M(2,1)*M(3,2) - M(2,2)*M(3,1); + value_type m_23_31_21_33 = M(2,3)*M(3,1) - M(2,1)*M(3,3); + value_type m_20_32_22_30 = M(2,0)*M(3,2) - M(2,2)*M(3,0); + + /* Compute minors: */ + value_type d00 + = M(1,1)*m_22_33_23_32+M(1,2)*m_23_31_21_33+M(1,3)*m_21_32_22_31; + + value_type d01 + = M(1,0)*m_22_33_23_32+M(1,2)*m_23_30_20_33+M(1,3)*m_20_32_22_30; + + value_type d02 + = M(1,0)*-m_23_31_21_33+M(1,1)*m_23_30_20_33+M(1,3)*m_20_31_21_30; + + value_type d03 + = M(1,0)*m_21_32_22_31+M(1,1)*-m_20_32_22_30+M(1,2)*m_20_31_21_30; + + /* Compute minors: */ + value_type d10 + = M(0,1)*m_22_33_23_32+M(0,2)*m_23_31_21_33+M(0,3)*m_21_32_22_31; + + value_type d11 + = M(0,0)*m_22_33_23_32+M(0,2)*m_23_30_20_33+M(0,3)*m_20_32_22_30; + + value_type d12 + = M(0,0)*-m_23_31_21_33+M(0,1)*m_23_30_20_33+M(0,3)*m_20_31_21_30; + + value_type d13 + = M(0,0)*m_21_32_22_31+M(0,1)*-m_20_32_22_30+M(0,2)*m_20_31_21_30; + + /* Common cofactors, rows 2,3: */ + value_type m_02_13_03_12 = M(0,2)*M(1,3) - M(0,3)*M(1,2); + value_type m_03_10_00_13 = M(0,3)*M(1,0) - M(0,0)*M(1,3); + value_type m_00_11_01_10 = M(0,0)*M(1,1) - M(0,1)*M(1,0); + value_type m_01_12_02_11 = M(0,1)*M(1,2) - M(0,2)*M(1,1); + value_type m_03_11_01_13 = M(0,3)*M(1,1) - M(0,1)*M(1,3); + value_type m_00_12_02_10 = M(0,0)*M(1,2) - M(0,2)*M(1,0); + + /* Compute minors (uses row 3 as the multipliers instead of row 0, + * which uses the same signs as row 0): + */ + value_type d20 + = M(3,1)*m_02_13_03_12+M(3,2)*m_03_11_01_13+M(3,3)*m_01_12_02_11; + + value_type d21 + = M(3,0)*m_02_13_03_12+M(3,2)*m_03_10_00_13+M(3,3)*m_00_12_02_10; + + value_type d22 + = M(3,0)*-m_03_11_01_13+M(3,1)*m_03_10_00_13+M(3,3)*m_00_11_01_10; + + value_type d23 + = M(3,0)*m_01_12_02_11+M(3,1)*-m_00_12_02_10+M(3,2)*m_00_11_01_10; + + /* Compute minors: */ + value_type d30 + = M(2,1)*m_02_13_03_12+M(2,2)*m_03_11_01_13+M(2,3)*m_01_12_02_11; + + value_type d31 + = M(2,0)*m_02_13_03_12+M(2,2)*m_03_10_00_13+M(2,3)*m_00_12_02_10; + + value_type d32 + = M(2,0)*-m_03_11_01_13+M(2,1)*m_03_10_00_13+M(2,3)*m_00_11_01_10; + + value_type d33 + = M(2,0)*m_01_12_02_11+M(2,1)*-m_00_12_02_10+M(2,2)*m_00_11_01_10; + + /* Finally, compute determinant from the minors, and assign the + * inverse as (1/D) * (cofactor matrix)^T: + */ + typename MatT::temporary_type Z; + cml::et::detail::Resize(Z,4,4); + + value_type D = value_type(1) / + (M(0,0)*d00 - M(0,1)*d01 + M(0,2)*d02 - M(0,3)*d03); + Z(0,0) = +d00*D; Z(0,1) = -d10*D; Z(0,2) = +d20*D; Z(0,3) = -d30*D; + Z(1,0) = -d01*D; Z(1,1) = +d11*D; Z(1,2) = -d21*D; Z(1,3) = +d31*D; + Z(2,0) = +d02*D; Z(2,1) = -d12*D; Z(2,2) = +d22*D; Z(2,3) = -d32*D; + Z(3,0) = -d03*D; Z(3,1) = +d13*D; Z(3,2) = -d23*D; Z(3,3) = +d33*D; + + return Z; + } +}; + +/* If more extensive general linear algebra functionality is offered in + * future versions it may be useful to make the elementary row and column + * operations separate functions. For now they're simply performed in place, + * but the commented-out lines of code show where the calls to these functions + * should go if and when they become available. + */ + +/* @todo: In-place version, and address memory allocation for pivot vector. + */ + +/* General NxN inverse by Gauss-Jordan elimination with full pivoting: */ +template +struct inverse_f +{ + typename MatT::temporary_type operator()(const MatT& M) const + { + /* Shorthand. */ + typedef typename MatT::value_type value_type; + + /* Size of matrix */ + size_t N = M.rows(); + + /* Matrix containing the inverse: */ + typename MatT::temporary_type Z; + cml::et::detail::Resize(Z,N,N); + Z = M; + + /* For tracking pivots */ + std::vector row_index(N); + std::vector col_index(N); + std::vector pivoted(N,0); + + /* For each column */ + for (size_t i = 0; i < N; ++i) { + + /* Find the pivot */ + size_t row = 0, col = 0; + value_type max = value_type(0); + for (size_t j = 0; j < N; ++j) { + if (!pivoted[j]) { + for (size_t k = 0; k < N; ++k) { + if (!pivoted[k]) { + value_type mag = std::fabs(Z(j,k)); + if (mag > max) { + max = mag; + row = j; + col = k; + } + } + } + } + } + + /* TODO: Check max against epsilon here to catch singularity */ + + row_index[i] = row; + col_index[i] = col; + + /* Swap rows if necessary */ + if (row != col) { + /*Z.row_op_swap(row,col);*/ + for (size_t j = 0; j < Z.cols(); ++j) { + std::swap(Z(row,j),Z(col,j)); + } + } + + /* Process pivot row */ + pivoted[col] = true; + value_type pivot = Z(col,col); + Z(col,col) = value_type(1); + /*Z.row_op_mult(col,value_type(1)/pivot);*/ + value_type k = value_type(1)/pivot; + for (size_t j = 0; j < Z.cols(); ++j) { + Z(col,j) *= k; + } + + /* Process other rows */ + for (size_t j = 0; j < N; ++j) { + if (j != col) { + value_type mult = -Z(j,col); + Z(j,col) = value_type(0); + /*Z.row_op_add_mult(col,j,mult);*/ + for (size_t k = 0; k < Z.cols(); ++k) { + Z(j,k) += mult * Z(col,k); + } + } + } + } + + /* Swap columns if necessary */ + for (int i = N-1; i >= 0; --i) { + if (row_index[i] != col_index[i]) { + /*Z.col_op_swap(row_index[i],col_index[i]);*/ + for (size_t j = 0; j < Z.rows(); ++j) { + std::swap(Z(j,row_index[i]),Z(j,col_index[i])); + } + } + } + + /* Return result */ + return Z; + } +}; + +/* Inversion by LU factorization is turned off for now due to lack of + * pivoting in the implementation, but we may switch back to it at some future + * time. + */ + +#if 0 + +/* General NxN inverse by LU factorization: */ +template +struct inverse_f +{ + typename MatT::temporary_type operator()(const MatT& M) const + { + /* Shorthand. */ + typedef typename MatT::value_type value_type; + + /* Compute LU factorization: */ + size_t N = M.rows(); + typename MatT::temporary_type LU; + cml::et::detail::Resize(LU,N,N); + LU = lu(M); + + /* Matrix containing the inverse: */ + typename MatT::temporary_type Z; + cml::et::detail::Resize(Z,N,N); + + typename MatT::col_vector_type v, x; + cml::et::detail::Resize(v,N); + cml::et::detail::Resize(x,N); + for(size_t i = 0; i < N; ++i) + v[i] = value_type(0); + /* XXX Need a fill() function here. */ + + /* Use lu_solve to solve M*x = v for x, where v = [0 ... 1 ... 0]^T: */ + for(size_t i = 0; i < N; ++i) { + v[i] = 1.; + x = lu_solve(LU,v); + + /* x is column i of the inverse of LU: */ + for(size_t k = 0; k < N; ++ k) { + Z(k,i) = x[k]; + } + v[i] = 0.; + } + + return Z; + } + +}; + +#endif + +/* Note: force_NxN is for checking general NxN inversion against the special- + * case 2x2, 3x3 and 4x4 code. I'm leaving it in for now since we may need to + * test the NxN code further if the implementation changes. At some future + * time when the implementation is stable, everything related to force_NxN can + * be taken out. + */ + +/* Note: Commenting the force_NxN stuff out, but leaving the code here in + * case we need to do more testing in the future. + */ + +/* Generator for the inverse functional for fixed-size matrices: */ +template typename MatT::temporary_type +inverse(const MatT& M, fixed_size_tag/*, bool force_NxN*/) +{ + /* Require a square matrix: */ + cml::et::CheckedSquare(M, fixed_size_tag()); + + /* + if (force_NxN) { + return inverse_f()(M); + } else { + */ + return inverse_f()(M); + /* + } + */ +} + +/* Generator for the inverse functional for dynamic-size matrices: */ +template typename MatT::temporary_type +inverse(const MatT& M, dynamic_size_tag/*, bool force_NxN*/) +{ + /* Require a square matrix: */ + cml::et::CheckedSquare(M, dynamic_size_tag()); + + /* + if (force_NxN) { + return inverse_f()(M); + } else { + */ + /* Dispatch based upon the matrix dimension: */ + switch(M.rows()) { + case 2: return inverse_f()(M); // 2x2 + case 3: return inverse_f()(M); // 3x3 + case 4: return inverse_f()(M); // 4x4 + default: return inverse_f()(M); // > 4x4 (or 1x1) + } + /* + } + */ +} + +} // namespace detail + +/** Inverse of a matrix. */ +template inline +typename matrix::temporary_type +inverse(const matrix& M/*, bool force_NxN = false*/) +{ + typedef typename matrix::size_tag size_tag; + return detail::inverse(M,size_tag()/*,force_NxN*/); +} + +/** Inverse of a matrix expression. */ +template inline +typename et::MatrixXpr::temporary_type +inverse(const et::MatrixXpr& e/*, bool force_NxN = false*/) +{ + typedef typename et::MatrixXpr::size_tag size_tag; + return detail::inverse(e,size_tag()/*,force_NxN*/); +} + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/matrix/lu.h b/src/cml/matrix/lu.h new file mode 100644 index 0000000..06e2835 --- /dev/null +++ b/src/cml/matrix/lu.h @@ -0,0 +1,176 @@ +/* -*- 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 Implements LU decomposition for square matrix expressions. + * + * @todo The LU implementation does not check for a zero diagonal entry + * (implying that the input has no LU factorization). + * + * @todo Should also have a pivoting implementation. + * + * @todo need to throw a numeric error if the determinant of the matrix + * given to lu(), lu_solve(), or inverse() is 0. + * + * @internal The implementation is the same for fixed- and dynamic-size + * matrices. It can be sped up for small matrices later. + */ + +#ifndef lu_h +#define lu_h + +#include +#include +#include + +/* This is used below to create a more meaningful compile-time error when + * lu is not provided with a matrix or MatrixExpr argument: + */ +struct lu_expects_a_matrix_arg_error; + +/* This is used below to create a more meaningful compile-time error when + * lu_inplace is not provided with an assignable matrix argument: + */ +struct lu_inplace_expects_an_assignable_matrix_arg_error; + +namespace cml { +namespace detail { + +/* Compute the LU decomposition in-place: */ +template inline +void lu_inplace(MatT& A) +{ + /* Shorthand: */ + typedef et::ExprTraits arg_traits; + typedef typename arg_traits::result_tag arg_result; + typedef typename arg_traits::assignable_tag arg_assignment; + typedef typename arg_traits::size_tag size_tag; + typedef typename arg_traits::value_type value_type; + + /* lu_inplace() requires an assignable matrix expression: */ + CML_STATIC_REQUIRE_M( + (same_type::is_true + && same_type::is_true), + lu_inplace_expects_an_assignable_matrix_arg_error); + /* Note: parens are required here so that the preprocessor ignores the + * commas. + */ + + /* Verify that the matrix is square, and get the size: */ + ssize_t N = (ssize_t) cml::et::CheckedSquare(A, size_tag()); + + + for(ssize_t k = 0; k < N-1; ++k) { + /* XXX Should check if A(k,k) = 0! */ + for(ssize_t i = k+1; i < N; ++i) { + value_type n = (A(i,k) /= A(k,k)); + for(ssize_t j = k+1; j < N; ++ j) { + A(i,j) -= n*A(k,j); + } + } + } +} + +/* Compute the LU decomposition, and return a copy of the result: */ +template +inline typename MatT::temporary_type +lu_copy(const MatT& M) +{ + /* Shorthand: */ + typedef et::ExprTraits arg_traits; + typedef typename arg_traits::result_tag arg_result; + + /* lu_with_copy() requires a matrix expression: */ + CML_STATIC_REQUIRE_M( + (same_type::is_true), + lu_expects_a_matrix_arg_error); + /* Note: parens are required here so that the preprocessor ignores the + * commas. + */ + + /* Use the in-place LU function, and return the result: */ + typename MatT::temporary_type A; + cml::et::detail::Resize(A,M.rows(),M.cols()); + A = M; + lu_inplace(A); + return A; +} + +} // namespace detail + +/** LU factorization for a matrix. */ +template +inline typename matrix::temporary_type +lu(const matrix& m) +{ + return detail::lu_copy(m); +} + +/** LU factorization for a matrix expression. */ +template +inline typename et::MatrixXpr::temporary_type +lu(const et::MatrixXpr& e) +{ + return detail::lu_copy(e); +} + +/** Solve y = LUx for x. + * + * This solves Lb = y for b by forward substitution, then Ux = b for x by + * backward substitution. + */ +template inline +typename et::MatVecPromote::temporary_type +lu_solve(const MatT& LU, const VecT& b) +{ + /* Shorthand. */ + typedef et::ExprTraits lu_traits; + typedef typename et::MatVecPromote::temporary_type vector_type; + typedef typename vector_type::value_type value_type; + + /* Verify that the matrix is square, and get the size: */ + ssize_t N = (ssize_t) cml::et::CheckedSquare( + LU, typename lu_traits::size_tag()); + + /* Verify that the matrix and vector have compatible sizes: */ + et::CheckedSize(LU, b, typename vector_type::size_tag()); + + /* Solve Ly = b for y by forward substitution. The entries below the + * diagonal of LU correspond to L, understood to be below a diagonal of + * 1's: + */ + vector_type y; cml::et::detail::Resize(y,N); + for(ssize_t i = 0; i < N; ++i) { + y[i] = b[i]; + for(ssize_t j = 0; j < i; ++j) { + y[i] -= LU(i,j)*y[j]; + } + } + + /* Solve Ux = y for x by backward substitution. The entries at and above + * the diagonal of LU correspond to U: + */ + vector_type x; cml::et::detail::Resize(x,N); + for(ssize_t i = N-1; i >= 0; --i) { + x[i] = y[i]; + for(ssize_t j = i+1; j < N; ++j) { + x[i] -= LU(i,j)*x[j]; + } + x[i] /= LU(i,i); + } + + /* Return x: */ + return x; +} + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/matrix/matop_macros.h b/src/cml/matrix/matop_macros.h new file mode 100644 index 0000000..f13b678 --- /dev/null +++ b/src/cml/matrix/matop_macros.h @@ -0,0 +1,236 @@ +/* -*- 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 various combinations of matrix expressions. + * + * Create unary and binary operators with macros. The available combinations + * are: + * + * Unary expressions: + * + * op Matrix -> Matrix + * op MatXpr -> Matrix + * + * Binary expressions: + * + * Matrix op Matrix -> Matrix + * MatXpr op Matrix -> MatXpr + * Matrix op MatXpr -> MatXpr + * MatXpr op MatXpr -> MatXpr + * + * Matrix op Scalar -> Matrix + * Scalar op Matrix -> Matrix + * MatXpr op Scalar -> MatXpr + * Scalar op MatXpr -> MatXpr + * + * All of the generator functions compress the expression tree by hoisting + * subexpressions into the containing expression. This has the effect of + * forcing only the root node of the expression tree to be a MatrixXpr. + * Every other node is a Unary or BinaryMatrixOp. + */ +#ifndef matop_macros_h +#define matop_macros_h + +/** Declare a unary operator taking a matrix operand. */ +#define CML_MAT_UNIOP(_op_, _OpT_) \ +template \ +inline et::MatrixXpr< \ + et::UnaryMatrixOp< matrix, _OpT_ > \ +> \ + \ +_op_ (const matrix& arg) \ +{ \ + typedef et::UnaryMatrixOp< \ + matrix, _OpT_ \ + > ExprT; \ + return et::MatrixXpr(ExprT(arg)); \ +} + +/** Declare a unary operator taking a et::MatrixXpr operand. */ +#define CML_MATXPR_UNIOP(_op_, _OpT_) \ +template \ +inline et::MatrixXpr< \ + et::UnaryMatrixOp > \ +> \ + \ +_op_ (MATXPR_ARG_TYPE arg) \ +{ \ + typedef et::UnaryMatrixOp< \ + XprT, _OpT_ \ + > ExprT; \ + return et::MatrixXpr(ExprT(arg.expression())); \ +} + +/** Declare an operator taking two matrix operands. */ +#define CML_MAT_MAT_BINOP(_op_, _OpT_) \ +template \ +inline et::MatrixXpr< \ + et::BinaryMatrixOp< \ + matrix, matrix, _OpT_ > \ +> \ + \ +_op_ ( \ + const matrix& left, \ + const matrix& right) \ +{ \ + typedef et::BinaryMatrixOp< \ + matrix, matrix, _OpT_ \ + > ExprT; \ + return et::MatrixXpr(ExprT(left,right)); \ +} + +/** Declare an operator taking a matrix and a et::MatrixXpr. */ +#define CML_MAT_MATXPR_BINOP(_op_, _OpT_) \ +template \ +inline et::MatrixXpr< \ + et::BinaryMatrixOp< \ + matrix, XprT, _OpT_ \ + > \ +> \ + \ +_op_ ( \ + const matrix& left, \ + MATXPR_ARG_TYPE right) \ +{ \ + typedef et::BinaryMatrixOp< \ + matrix, XprT, \ + _OpT_ \ + > ExprT; \ + return et::MatrixXpr(ExprT(left,right.expression())); \ +} + +/** Declare an operator taking a et::MatrixXpr and a matrix. */ +#define CML_MATXPR_MAT_BINOP(_op_, _OpT_) \ +template \ +inline et::MatrixXpr< \ + et::BinaryMatrixOp< \ + XprT, matrix, _OpT_ \ + > \ +> \ + \ +_op_ ( \ + MATXPR_ARG_TYPE left, \ + const matrix& right) \ +{ \ + typedef et::BinaryMatrixOp< \ + XprT, matrix, \ + _OpT_ \ + > ExprT; \ + return et::MatrixXpr(ExprT(left.expression(),right)); \ +} + +/** Declare an operator taking two et::MatrixXpr operands. */ +#define CML_MATXPR_MATXPR_BINOP(_op_, _OpT_) \ +template \ +inline et::MatrixXpr< \ + et::BinaryMatrixOp< \ + XprT1, XprT2, \ + _OpT_ < \ + typename XprT1::value_type, \ + typename XprT2::value_type \ + > \ + > \ +> \ + \ +_op_ ( \ + MATXPR_ARG_TYPE_N(1) left, \ + MATXPR_ARG_TYPE_N(2) right) \ +{ \ + typedef et::BinaryMatrixOp< \ + XprT1, XprT2, \ + _OpT_ < \ + typename XprT1::value_type, \ + typename XprT2::value_type> \ + > ExprT; \ + return et::MatrixXpr( \ + ExprT(left.expression(),right.expression())); \ +} + + +/** Declare an operator taking a matrix and a scalar. */ +#define CML_MAT_SCALAR_BINOP(_op_, _OpT_) \ +template\ +inline et::MatrixXpr< \ + et::BinaryMatrixOp< \ + matrix, ScalarT, _OpT_ \ + > \ +> \ + \ +_op_ ( \ + const matrix& left, \ + SCALAR_ARG_TYPE right) \ +{ \ + typedef et::BinaryMatrixOp< \ + matrix, ScalarT, _OpT_ \ + > ExprT; \ + return et::MatrixXpr(ExprT(left,right)); \ +} + +/** Declare an operator taking a scalar and a matrix. */ +#define CML_SCALAR_MAT_BINOP(_op_, _OpT_) \ +template\ +inline et::MatrixXpr< \ + et::BinaryMatrixOp< \ + ScalarT, matrix, _OpT_ \ + > \ +> \ + \ +_op_ ( \ + SCALAR_ARG_TYPE left, \ + const matrix& right) \ +{ \ + typedef et::BinaryMatrixOp< \ + ScalarT, matrix, _OpT_ \ + > ExprT; \ + return et::MatrixXpr(ExprT(left,right)); \ +} + +/** Declare an operator taking a et::MatrixXpr and a scalar. */ +#define CML_MATXPR_SCALAR_BINOP(_op_, _OpT_) \ +template \ +inline et::MatrixXpr< \ + et::BinaryMatrixOp< \ + XprT, ScalarT, _OpT_ \ + > \ +> \ + \ +_op_ ( \ + MATXPR_ARG_TYPE left, \ + SCALAR_ARG_TYPE right) \ +{ \ + typedef et::BinaryMatrixOp< \ + XprT, ScalarT, _OpT_ \ + > ExprT; \ + return et::MatrixXpr(ExprT(left.expression(),right)); \ +} + +/** Declare an operator taking a scalar and a et::MatrixXpr. */ +#define CML_SCALAR_MATXPR_BINOP(_op_, _OpT_) \ +template \ +inline et::MatrixXpr< \ + et::BinaryMatrixOp< \ + ScalarT, XprT, _OpT_ \ + > \ +> \ + \ +_op_ ( \ + SCALAR_ARG_TYPE left, \ + MATXPR_ARG_TYPE right) \ +{ \ + typedef et::BinaryMatrixOp< \ + ScalarT, XprT, _OpT_ \ + > ExprT; \ + return et::MatrixXpr(ExprT(left,right.expression())); \ +} + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/matrix/matrix_comparison.h b/src/cml/matrix/matrix_comparison.h new file mode 100644 index 0000000..d8772b0 --- /dev/null +++ b/src/cml/matrix/matrix_comparison.h @@ -0,0 +1,245 @@ +/* -*- 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 + * + * @todo The matrix and matrix order operators could probably be combined + * into a single templated implementation, since the only thing that is + * different is the access method. + */ + +#ifndef matrix_comparison_h +#define matrix_comparison_h + +#include +#include +#include + +/* This is used below to create a more meaningful compile-time error when + * matrix_comparison is not provided with matrix or MatrixExpr arguments: + */ +struct matrix_comparison_expects_matrix_args_error; + +#define CML_MAT_MAT_ORDER(_order_, _op_, _OpT_) \ +template \ +inline bool \ +_op_ ( \ + const matrix& left, \ + const matrix& right) \ +{ \ + return detail::matrix_##_order_ (left, right, _OpT_ ()); \ +} + +#define CML_MAT_MATXPR_ORDER(_order_, _op_, _OpT_) \ +template \ +inline bool \ +_op_ ( \ + const matrix& left, \ + MATXPR_ARG_TYPE right) \ +{ \ + return detail::matrix_##_order_ (left, right, \ + _OpT_ ()); \ +} + +#define CML_MATXPR_MAT_ORDER(_order_, _op_, _OpT_) \ +template \ +inline bool \ +_op_ ( \ + MATXPR_ARG_TYPE left, \ + const matrix& right) \ +{ \ + return detail::matrix_##_order_ (left, right, \ + _OpT_ ()); \ +} + +#define CML_MATXPR_MATXPR_ORDER(_order_, _op_, _OpT_) \ +template \ +inline bool \ +_op_ ( \ + MATXPR_ARG_TYPE_N(1) left, \ + MATXPR_ARG_TYPE_N(2) right) \ +{ \ + return detail::matrix_##_order_ (left, right, \ + _OpT_ < \ + typename XprT1::value_type, \ + typename XprT2::value_type>()); \ +} + + +namespace cml { +namespace detail { + +/** Matrix strict weak ordering relationship. + * + * OpT must implement a strict weak order on the matrix element type. + * operator< and operator> on integer and floating-point types are + * examples. + */ +template +inline bool +matrix_weak_order(const LeftT& left, const RightT& right, OpT) +{ + /* Shorthand: */ + typedef et::ExprTraits left_traits; + typedef et::ExprTraits right_traits; + + /* matrix_comparison() requires matrix expressions: */ + CML_STATIC_REQUIRE_M( + (et::MatrixExpressions::is_true), + matrix_comparison_expects_matrix_args_error); + /* Note: parens are required here so that the preprocessor ignores the + * commas: + */ + + typedef typename et::MatrixPromote< + typename left_traits::result_type, + typename right_traits::result_type + >::type result_type; + typedef typename result_type::size_tag size_tag; + + /* Verify expression size: */ + matrix_size N = et::CheckedSize(left,right,size_tag()); + for(ssize_t i = 0; i < N.first; ++ i) { + for(ssize_t j = 0; j < N.second; ++ j) { + if(OpT().apply( + left_traits().get(left,i,j), + right_traits().get(right,i,j) + )) + { + /* If weak order (a < b) is satisfied, return true: */ + return true; + } else if(OpT().apply( + right_traits().get(right,i,j), + left_traits().get(left,i,j) + )) + { + /* If !(b < a), then return false: */ + return false; + } else { + + /* Have !(a < b) && !(b < a) <=> (a >= b && b >= a) + * <=> (a == b). so need to test next element: + */ + continue; + } + } + } + /* XXX Can this be unrolled in any reasonable way? */ + + /* If we get here, then left == right: */ + return false; +} + +/** Matrix total order relationship. + * + * OpT must implement a total order on the matrix element type. operator<= + * and operator>= on integer and floating-point types are examples. + */ +template +inline bool +matrix_total_order(const LeftT& left, const RightT& right, OpT) +{ + /* Shorthand: */ + typedef et::ExprTraits left_traits; + typedef et::ExprTraits right_traits; + + /* matrix_comparison() requires matrix expressions: */ + CML_STATIC_REQUIRE_M( + (et::MatrixExpressions::is_true), + matrix_comparison_expects_matrix_args_error); + /* Note: parens are required here so that the preprocessor ignores the + * commas: + */ + + typedef typename et::MatrixPromote< + typename left_traits::result_type, + typename right_traits::result_type + >::type result_type; + typedef typename result_type::size_tag size_tag; + + /* Verify expression size: */ + matrix_size N = et::CheckedSize(left,right,size_tag()); + for(ssize_t i = 0; i < N.first; ++ i) { + for(ssize_t j = 0; j < N.second; ++ j) { + + /* Test total order: */ + if(OpT().apply( + left_traits().get(left,i,j), + right_traits().get(right,i,j) + )) + { + /* Automatically true if weak order (a <= b) && !(b <= a) + * <=> (a <= b) && (b > a) <=> (a < b) is satisfied: + */ + if(!OpT().apply( + right_traits().get(right,i,j), + left_traits().get(left,i,j) + )) + return true; + + /* Otherwise, have equality (a <= b) && (b <= a), so + * continue to next element: + */ + else + continue; + + } else { + + /* Total order isn't satisfied (a > b), so return false: */ + return false; + } + } + } + /* XXX Can this be unrolled in any reasonable way? */ + + /* Total (==) or weak (<) order was satisfied, so return true: */ + return true; +} + +} + +} // namespace cml + +/* XXX There is a better way to handle these with operator traits... */ + +CML_MAT_VEC_ORDER( total_order, operator==, et::OpEqual) +CML_MATXPR_MAT_ORDER( total_order, operator==, et::OpEqual) +CML_MAT_MATXPR_ORDER( total_order, operator==, et::OpEqual) +CML_MATXPR_VECXPR_ORDER( total_order, operator==, et::OpEqual) + +CML_MAT_VEC_ORDER( weak_order, operator!=, et::OpNotEqual) +CML_MATXPR_MAT_ORDER( weak_order, operator!=, et::OpNotEqual) +CML_MAT_MATXPR_ORDER( weak_order, operator!=, et::OpNotEqual) +CML_MATXPR_VECXPR_ORDER( weak_order, operator!=, et::OpNotEqual) + +CML_MAT_VEC_ORDER( weak_order, operator<, et::OpLess) +CML_MATXPR_MAT_ORDER( weak_order, operator<, et::OpLess) +CML_MAT_MATXPR_ORDER( weak_order, operator<, et::OpLess) +CML_MATXPR_VECXPR_ORDER( weak_order, operator<, et::OpLess) + +CML_MAT_VEC_ORDER( weak_order, operator>, et::OpGreater) +CML_MATXPR_MAT_ORDER( weak_order, operator>, et::OpGreater) +CML_MAT_MATXPR_ORDER( weak_order, operator>, et::OpGreater) +CML_MATXPR_VECXPR_ORDER( weak_order, operator>, et::OpGreater) + +CML_MAT_VEC_ORDER( total_order, operator<=, et::OpLessEqual) +CML_MATXPR_MAT_ORDER( total_order, operator<=, et::OpLessEqual) +CML_MAT_MATXPR_ORDER( total_order, operator<=, et::OpLessEqual) +CML_MATXPR_VECXPR_ORDER( total_order, operator<=, et::OpLessEqual) + +CML_MAT_VEC_ORDER( total_order, operator>=, et::OpGreaterEqual) +CML_MATXPR_MAT_ORDER( total_order, operator>=, et::OpGreaterEqual) +CML_MAT_MATXPR_ORDER( total_order, operator>=, et::OpGreaterEqual) +CML_MATXPR_VECXPR_ORDER( total_order, operator>=, et::OpGreaterEqual) + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/matrix/matrix_expr.h b/src/cml/matrix/matrix_expr.h new file mode 100644 index 0000000..00cb668 --- /dev/null +++ b/src/cml/matrix/matrix_expr.h @@ -0,0 +1,483 @@ +/* -*- 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 Matrix linear expression classes. + * + * @todo Dynamic resizing needs to be integrated more naturally into + * mul() and matrix transpose(): + */ + +#ifndef matrix_expr_h +#define matrix_expr_h + + +#include +#include +#include + +/* XXX Don't know which it should be just yet, since RVO seems to obviate the + * need for a reference type. However, copy by value copies the *entire + * expression tree rooted at the MatrixXpr<>, so this choice is bound to affect + * performance for some compiler or another: + */ +#define MATXPR_ARG_TYPE const et::MatrixXpr& +#define MATXPR_ARG_TYPE_N(_N_) const et::MatrixXpr& + +//#define MATXPR_ARG_TYPE const et::MatrixXpr +//#define MATXPR_ARG_TYPE_N(_N_) const et::MatrixXpr + +namespace cml { +namespace et { + +/** A placeholder for a matrix expression in the expression tree. */ +template +class MatrixXpr +{ + public: + + typedef MatrixXpr expr_type; + + /* Copy the expression by value into higher-up expressions: */ + typedef expr_type expr_const_reference; + + typedef typename ExprT::value_type value_type; + typedef matrix_result_tag result_tag; + typedef typename ExprT::size_tag size_tag; // Just inherit size type. + + /* Store the expression traits: */ + typedef ExprTraits expr_traits; + + /* Get the reference type: */ + typedef typename expr_traits::const_reference expr_reference; + + /* Get the result type: */ + typedef typename expr_traits::result_type result_type; + + /* Get the basis type: */ + typedef typename result_type::basis_orient basis_orient; + + /* Get the temporary type: */ + typedef typename result_type::temporary_type temporary_type; + + /* For matching by assignability: */ + typedef cml::et::not_assignable_tag assignable_tag; + + + public: + + /** Record result size as an enum (if applicable). */ + enum { array_rows = ExprT::array_rows, array_cols = ExprT::array_cols }; + + + public: + + /** Return the expression size as a pair. */ + matrix_size size() const { + return matrix_size(this->rows(),this->cols()); + } + + /** Return number of rows in the expression (same as subexpression). */ + size_t rows() const { + return expr_traits().rows(m_expr); + } + + /** Return number of columns in the expression (same as subexpression). */ + size_t cols() const { + return expr_traits().cols(m_expr); + } + + /** Return reference to contained expression. */ + expr_reference expression() const { return m_expr; } + + /** Compute value at index i,j of the result matrix. */ + value_type operator()(size_t i, size_t j) const { + return expr_traits().get(m_expr,i,j); + } + + /** Return element j of basis vector i. */ + value_type basis_element(size_t i, size_t j) const { + return basis_element(i,j,basis_orient()); + } + + + public: + + /** Construct from the subexpression to store. */ + explicit MatrixXpr(expr_reference expr) : m_expr(expr) {} + + /** Copy constructor. */ + MatrixXpr(const expr_type& e) : m_expr(e.m_expr) {} + + + protected: + + value_type basis_element(size_t i, size_t j, row_basis) const { + return (*this)(i,j); + } + + value_type basis_element(size_t i, size_t j, col_basis) const { + return (*this)(j,i); + } + + + protected: + + expr_reference m_expr; + + + private: + + /* Cannot be assigned to: */ + expr_type& operator=(const expr_type&); +}; + +/** Expression traits for MatrixXpr<>. */ +template +struct ExprTraits< MatrixXpr > +{ + typedef MatrixXpr expr_type; + typedef ExprT arg_type; + + typedef typename expr_type::value_type value_type; + typedef typename expr_type::expr_const_reference const_reference; + typedef typename expr_type::result_tag result_tag; + typedef typename expr_type::size_tag size_tag; + typedef typename expr_type::result_type result_type; + typedef typename expr_type::assignable_tag assignable_tag; + typedef expr_node_tag node_tag; + + value_type get(const expr_type& e, size_t i, size_t j) const { + return e(i,j); + } + + + matrix_size size(const expr_type& e) const { return e.size(); } + size_t rows(const expr_type& e) const { return e.rows(); } + size_t cols(const expr_type& e) const { return e.cols(); } +}; + + +/** A unary matrix expression operating on matrix elements as a list. + * + * The operator must take exactly one argument. + */ +template +class UnaryMatrixOp +{ + public: + + typedef UnaryMatrixOp expr_type; + + /* Record ary-ness of the expression: */ + typedef unary_expression expr_ary; + + /* Copy the expression by value into higher-up expressions: */ + typedef expr_type expr_const_reference; + + typedef typename OpT::value_type value_type; + typedef matrix_result_tag result_tag; + typedef typename ExprT::size_tag size_tag; + + /* Store the expression traits for the subexpression: */ + typedef ExprTraits expr_traits; + + /* Reference type for the subexpression: */ + typedef typename expr_traits::const_reference expr_reference; + + /* Get the result type: */ + typedef typename expr_traits::result_type result_type; + + /* Get the temporary type: */ + typedef typename result_type::temporary_type temporary_type; + + /* For matching by assignability: */ + typedef cml::et::not_assignable_tag assignable_tag; + + + public: + + /** Record result size as an enum (if applicable). */ + enum { array_rows = ExprT::array_rows, array_cols = ExprT::array_cols }; + + + public: + + /** Return the expression size as a pair. */ + matrix_size size() const { + return matrix_size(this->rows(),this->cols()); + } + + /** Return number of rows in the expression (same as argument). */ + size_t rows() const { + return expr_traits().rows(m_expr); + } + + /** Return number of columns in the expression (same as argument). */ + size_t cols() const { + return expr_traits().cols(m_expr); + } + + /** Compute value at index i,j of the result matrix. */ + value_type operator()(size_t i, size_t j) const { + + /* This uses the expression traits to figure out how to access the + * i,j'th element of the subexpression: + */ + return OpT().apply(expr_traits().get(m_expr,i,j)); + } + + + public: + + /** Construct from the subexpression. */ + explicit UnaryMatrixOp(expr_reference expr) : m_expr(expr) {} + + /** Copy constructor. */ + UnaryMatrixOp(const expr_type& e) : m_expr(e.m_expr) {} + + + protected: + + expr_reference m_expr; + + + private: + + /* Cannot be assigned to: */ + expr_type& operator=(const expr_type&); +}; + +/** Expression traits for UnaryMatrixOp<>. */ +template +struct ExprTraits< UnaryMatrixOp > +{ + typedef UnaryMatrixOp expr_type; + typedef ExprT arg_type; + + typedef typename expr_type::value_type value_type; + typedef typename expr_type::expr_const_reference const_reference; + typedef typename expr_type::result_tag result_tag; + typedef typename expr_type::size_tag size_tag; + typedef typename expr_type::result_type result_type; + typedef typename expr_type::assignable_tag assignable_tag; + typedef expr_node_tag node_tag; + + value_type get(const expr_type& e, size_t i, size_t j) const { + return e(i,j); + } + + matrix_size size(const expr_type& e) const { return e.size(); } + size_t rows(const expr_type& e) const { return e.rows(); } + size_t cols(const expr_type& e) const { return e.cols(); } +}; + + +/** A binary matrix expression. */ +template +class BinaryMatrixOp +{ + public: + + typedef BinaryMatrixOp expr_type; + + /* Copy the UnaryMatrixOp expression by value into parent + * expression tree nodes: + */ + typedef expr_type expr_const_reference; + + typedef typename OpT::value_type value_type; + typedef matrix_result_tag result_tag; + + /* For matching by assignability: */ + typedef cml::et::not_assignable_tag assignable_tag; + + /* Record the expression traits for the two subexpressions: */ + typedef ExprTraits left_traits; + typedef ExprTraits right_traits; + + /* Reference types for the two subexpressions: */ + typedef typename left_traits::const_reference left_reference; + typedef typename right_traits::const_reference right_reference; + + /* Figure out the expression's resulting (matrix) type: */ + typedef typename left_traits::result_type left_result; + typedef typename right_traits::result_type right_result; + typedef typename MatrixPromote::type result_type; + typedef typename result_type::size_tag size_tag; + + /* Get the temporary type: */ + typedef typename result_type::temporary_type temporary_type; + + /* Define a size checker: */ + typedef GetCheckedSize checked_size; + + + public: + + /** Record result size as an enum (if applicable). + * + * CheckExprSizes<> ensures that this works as expected. + */ + enum { + array_rows = result_type::array_rows, + array_cols = result_type::array_cols + }; + + + public: + + /** Return the expression size as a pair. */ + matrix_size size() const { + return CheckedSize(m_left,m_right,size_tag()); + } + + /** Return number of rows in the result. + * + * @note Because this calls size() internally, calling both rows() + * and cols() with CML_CHECK_MATRIX_EXPR_SIZES defined will cause the size + * checking code to be executed twice. + */ + size_t rows() const { +#if defined(CML_CHECK_MATRIX_EXPR_SIZES) + return this->size().first; +#else + return left_traits().rows(m_left); +#endif + } + + /** Return number of cols in the result. + * + * @note Because this calls size() internally, calling both rows() + * and cols() with CML_CHECK_MATRIX_EXPR_SIZES defined will cause the size + * checking code to be executed twice. + */ + size_t cols() const { +#if defined(CML_CHECK_MATRIX_EXPR_SIZES) + return this->size().second; +#else + return right_traits().cols(m_right); +#endif + } + + /** Compute value at index i,j of the result matrix. */ + value_type operator()(size_t i, size_t j) const { + + /* This uses the expression traits to figure out how to access the + * i'th index of the two subexpressions: + */ + return OpT().apply( + left_traits().get(m_left,i,j), + right_traits().get(m_right,i,j)); + } + + + public: + + /** Construct from the two subexpressions. + * + * @throws std::invalid_argument if the subexpression sizes don't + * match. + */ + explicit BinaryMatrixOp(left_reference left, right_reference right) + : m_left(left), m_right(right) {} + + /** Copy constructor. */ + BinaryMatrixOp(const expr_type& e) + : m_left(e.m_left), m_right(e.m_right) {} + + + protected: + + left_reference m_left; + right_reference m_right; + + + private: + + /* This ensures that a compile-time size check is executed: */ + typename checked_size::check_type _dummy; + + + private: + + /* Cannot be assigned to: */ + expr_type& operator=(const expr_type&); +}; + +/** Expression traits for BinaryMatrixOp<>. */ +template +struct ExprTraits< BinaryMatrixOp > +{ + typedef BinaryMatrixOp expr_type; + typedef LeftT left_type; + typedef RightT right_type; + + typedef typename expr_type::value_type value_type; + typedef typename expr_type::expr_const_reference const_reference; + typedef typename expr_type::result_tag result_tag; + typedef typename expr_type::size_tag size_tag; + typedef typename expr_type::result_type result_type; + typedef typename expr_type::assignable_tag assignable_tag; + typedef expr_node_tag node_tag; + + value_type get(const expr_type& e, size_t i, size_t j) const { + return e(i,j); + } + + matrix_size size(const expr_type& e) const { return e.size(); } + size_t rows(const expr_type& e) const { return e.rows(); } + size_t cols(const expr_type& e) const { return e.cols(); } +}; + +/* Helper struct to verify that both arguments are matrix expressions: */ +template +struct MatrixExpressions +{ + /* Require that both arguments are matrix expressions: */ + typedef typename LeftTraits::result_tag left_result; + typedef typename RightTraits::result_tag right_result; + enum { is_true = (same_type::is_true + && same_type::is_true) }; +}; + +namespace detail { + +/* XXX These are temporary helpers until dynamic resizing is integrated more + * naturally into mul() and matrix transpose(): + */ +template inline +void Resize(MatT&, size_t, size_t, fixed_size_tag, MT) {} + +template inline +void Resize(MatT& m, + size_t R, size_t C, dynamic_size_tag, dynamic_memory_tag) +{ + m.resize(R,C); +} + +template inline +void Resize(MatT& m, size_t R, size_t C) { + Resize(m, R, C, typename MatT::size_tag(), typename MatT::memory_tag()); +} + +template inline +void Resize(MatT& m, matrix_size N) { + Resize(m, N.first, N.second, + typename MatT::size_tag(), typename MatT::memory_tag()); +} + +} // namespace detail + +} // namespace et +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/matrix/matrix_functions.h b/src/cml/matrix/matrix_functions.h new file mode 100644 index 0000000..cd2c92a --- /dev/null +++ b/src/cml/matrix/matrix_functions.h @@ -0,0 +1,43 @@ +/* -*- 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_functions_h +#define matrix_functions_h + +namespace cml { + +/** Set the given matrix to the identity matrix. + * + * This only makes sense for a square matrix, but no error will be + * signaled if the matrix is not square. + * + * @todo This should return a MatrixXpr to allow loop unrolling, as should + * the class method. + */ +template +inline matrix +identity(const matrix& m) +{ + typename matrix::temporary_type result; + + /* This is a no-op for fixed-size matrices: */ + cml::et::detail::Resize(result, m.size()); + result.identity(); + return result; +} + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/matrix/matrix_mul.h b/src/cml/matrix/matrix_mul.h new file mode 100644 index 0000000..6d2dc11 --- /dev/null +++ b/src/cml/matrix/matrix_mul.h @@ -0,0 +1,205 @@ +/* -*- 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 Multiply two matrices. + * + * @todo Does it make sense to put mat-mat multiplication as a node into the + * expression tree? + * + * @internal This does not need to return an expression type, since the + * temporary generation for the matrix result is handled automatically by the + * compiler. i.e. when used in an expression, the result is automatically + * included in the expression tree as a temporary by the compiler. + */ + +#ifndef matrix_mul_h +#define matrix_mul_h + +#include +#include + +/* This is used below to create a more meaningful compile-time error when + * mul is not provided with matrix or MatrixExpr arguments: + */ +struct mul_expects_matrix_args_error; + +/* This is used below to create a more meaningful compile-time error when + * fixed-size arguments to mul() have the wrong size: + */ +struct mul_expressions_have_wrong_size_error; + +namespace cml { +namespace detail { + +/** Verify the sizes of the argument matrices for matrix multiplication. + * + * @returns a matrix_size containing the size of the resulting matrix. + */ +template inline matrix_size +MatMulCheckedSize(const LeftT&, const RightT&, fixed_size_tag) +{ + CML_STATIC_REQUIRE_M( + ((size_t)LeftT::array_cols == (size_t)RightT::array_rows), + mul_expressions_have_wrong_size_error); + return matrix_size(LeftT::array_rows,RightT::array_cols); +} + +/** Verify the sizes of the argument matrices for matrix multiplication. + * + * @returns a matrix_size containing the size of the resulting matrix. + */ +template inline matrix_size +MatMulCheckedSize(const LeftT& left, const RightT& right, dynamic_size_tag) +{ + matrix_size left_N = left.size(), right_N = right.size(); + et::GetCheckedSize() + .equal_or_fail(left_N.second, right_N.first); /* cols,rows */ + return matrix_size(left_N.first, right_N.second); /* rows,cols */ +} + + +/** Matrix multiplication. + * + * Computes C = A x B (O(N^3), non-blocked algorithm). + */ +template +inline typename et::MatrixPromote< + typename et::ExprTraits::result_type, + typename et::ExprTraits::result_type +>::temporary_type +mul(const LeftT& left, const RightT& right) +{ + /* Shorthand: */ + typedef et::ExprTraits left_traits; + typedef et::ExprTraits right_traits; + typedef typename left_traits::result_type left_result; + typedef typename right_traits::result_type right_result; + + /* First, require matrix expressions: */ + CML_STATIC_REQUIRE_M( + (et::MatrixExpressions::is_true), + mul_expects_matrix_args_error); + /* Note: parens are required here so that the preprocessor ignores the + * commas. + */ + + /* Deduce size type to ensure that a run-time check is performed if + * necessary: + */ + typedef typename et::MatrixPromote< + typename left_traits::result_type, + typename right_traits::result_type + >::type result_type; + typedef typename result_type::size_tag size_tag; + + /* Require that left has the same number of columns as right has rows. + * This automatically checks fixed-size matrices at compile time, and + * throws at run-time if the sizes don't match: + */ + matrix_size N = detail::MatMulCheckedSize(left, right, size_tag()); + + /* Create an array with the right size (resize() is a no-op for + * fixed-size matrices): + */ + result_type C; + cml::et::detail::Resize(C, N); + + /* XXX Specialize this for fixed-size matrices: */ + typedef typename result_type::value_type value_type; + for(size_t i = 0; i < left.rows(); ++i) { /* rows */ + for(size_t j = 0; j < right.cols(); ++j) { /* cols */ + value_type sum(left(i,0)*right(0,j)); + for(size_t k = 1; k < right.rows(); ++k) { + sum += (left(i,k)*right(k,j)); + } + C(i,j) = sum; + } + } + + return C; +} + +} // namespace detail + + +/** operator*() for two matrices. */ +template +inline typename et::MatrixPromote< + matrix, matrix +>::temporary_type +operator*(const matrix& left, + const matrix& right) +{ + return detail::mul(left,right); +} + +/** operator*() for a matrix and a MatrixXpr. */ +template +inline typename et::MatrixPromote< + matrix, typename XprT::result_type +>::temporary_type +operator*(const matrix& left, + const et::MatrixXpr& right) +{ + /* Generate a temporary, and compute the right-hand expression: */ + typedef typename et::MatrixXpr::temporary_type expr_tmp; + expr_tmp tmp; + cml::et::detail::Resize(tmp,right.rows(),right.cols()); + tmp = right; + + return detail::mul(left,tmp); +} + +/** operator*() for a MatrixXpr and a matrix. */ +template +inline typename et::MatrixPromote< + typename XprT::result_type , matrix +>::temporary_type +operator*(const et::MatrixXpr& left, + const matrix& right) +{ + /* Generate a temporary, and compute the left-hand expression: */ + typedef typename et::MatrixXpr::temporary_type expr_tmp; + expr_tmp tmp; + cml::et::detail::Resize(tmp,left.rows(),left.cols()); + tmp = left; + + return detail::mul(tmp,right); +} + +/** operator*() for two MatrixXpr's. */ +template +inline typename et::MatrixPromote< + typename XprT1::result_type, typename XprT2::result_type +>::temporary_type +operator*(const et::MatrixXpr& left, + const et::MatrixXpr& right) +{ + /* Generate temporaries and compute expressions: */ + typedef typename et::MatrixXpr::temporary_type left_tmp; + left_tmp ltmp; + cml::et::detail::Resize(ltmp,left.rows(),left.cols()); + ltmp = left; + + typedef typename et::MatrixXpr::temporary_type right_tmp; + right_tmp rtmp; + cml::et::detail::Resize(rtmp,right.rows(),right.cols()); + rtmp = right; + + return detail::mul(ltmp,rtmp); +} + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/matrix/matrix_ops.h b/src/cml/matrix/matrix_ops.h new file mode 100644 index 0000000..8188f87 --- /dev/null +++ b/src/cml/matrix/matrix_ops.h @@ -0,0 +1,50 @@ +/* -*- 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 matrix operators. + */ +#ifndef matrix_ops_h +#define matrix_ops_h + +#include +#include +#include + +namespace cml { + +CML_MAT_UNIOP( operator+, et::OpPos) +CML_MATXPR_UNIOP( operator+, et::OpPos) + +CML_MAT_UNIOP( operator-, et::OpNeg) +CML_MATXPR_UNIOP( operator-, et::OpNeg) + +CML_MAT_MAT_BINOP( operator+, et::OpAdd) +CML_MATXPR_MAT_BINOP( operator+, et::OpAdd) +CML_MAT_MATXPR_BINOP( operator+, et::OpAdd) +CML_MATXPR_MATXPR_BINOP( operator+, et::OpAdd) + +CML_MAT_MAT_BINOP( operator-, et::OpSub) +CML_MATXPR_MAT_BINOP( operator-, et::OpSub) +CML_MAT_MATXPR_BINOP( operator-, et::OpSub) +CML_MATXPR_MATXPR_BINOP( operator-, et::OpSub) + +CML_MAT_SCALAR_BINOP( operator*, et::OpMul) +CML_SCALAR_MAT_BINOP( operator*, et::OpMul) +CML_MATXPR_SCALAR_BINOP( operator*, et::OpMul) +CML_SCALAR_MATXPR_BINOP( operator*, et::OpMul) + +CML_MAT_SCALAR_BINOP( operator/, et::OpDiv) +CML_MATXPR_SCALAR_BINOP( operator/, et::OpDiv) + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/matrix/matrix_print.h b/src/cml/matrix/matrix_print.h new file mode 100644 index 0000000..0937afd --- /dev/null +++ b/src/cml/matrix/matrix_print.h @@ -0,0 +1,59 @@ +/* -*- 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_print_h +#define matrix_print_h + +#include + +namespace cml { + +/** Output a matrix to a std::ostream. */ +template inline std::ostream& +operator<<(std::ostream& os, const matrix& m) +{ + for(size_t i = 0; i < m.rows(); ++i) { + os << "["; + for(size_t j = 0; j < m.cols(); ++j) { + os << " " << m(i,j); + } + os << " ]"; + if (i != m.rows()-1) { + os << std::endl; + } + } + return os; +} + +/** Output a matrix expression to a std::ostream. */ +template< class XprT > inline std::ostream& +operator<<(std::ostream& os, const et::MatrixXpr& m) +{ + for(size_t i = 0; i < m.rows(); ++i) { + os << "["; + for(size_t j = 0; j < m.cols(); ++j) { + os << " " << m(i,j); + } + os << " ]"; + if (i != m.rows()-1) { + os << std::endl; + } + } + return os; +} + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/matrix/matrix_promotions.h b/src/cml/matrix/matrix_promotions.h new file mode 100644 index 0000000..18e94bb --- /dev/null +++ b/src/cml/matrix/matrix_promotions.h @@ -0,0 +1,171 @@ +/* -*- 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 for matrices used in matrix/matrix or matrix/scalar + * expressions. + * + * @sa UnaryMat4_TOp + * @sa BinaryMat4_TOp + */ + +#ifndef matrix_promotions_h +#define matrix_promotions_h + +#include +#include +#include +#include +#include + +/* This is used below to create a more meaningful compile-time error when + * either argument to OuterPromote has the wrong orientation. + */ +struct outer_promote_expects_properly_oriented_args_error; + +namespace cml { +namespace et { + +/* Default matrix type promotion template. */ +template struct MatrixPromote; + +/** Type promotion for two matrix types. + * + * @note This always uses the basis orientation of the left-hand matrix. + * @bug This always uses the basis orientation of the left-hand matrix, + * which is not always correct. + */ +template +struct MatrixPromote< cml::matrix, cml::matrix > +{ + /* Promote the arrays: */ + typedef typename ArrayPromote< + typename cml::matrix::array_type, + typename cml::matrix::array_type + >::type promoted_array; + + /* The deduced matrix result type: */ + typedef cml::matrix< + typename promoted_array::value_type, + typename promoted_array::generator_type, + BO1, + typename promoted_array::layout + > type; + + /* The deduced temporary type: */ + typedef typename type::temporary_type temporary_type; +}; + +/** + * NOTE: MatrixPromote* are somewhat ad hoc, and were added to + * simplify the code for matrix slerp/squad/etc. + */ + +/** Type promotion for two matrix types. */ +template < class Mat1_T, class Mat2_T > +struct MatrixPromote2 +{ + typedef typename MatrixPromote< + typename Mat1_T::temporary_type, typename Mat2_T::temporary_type + >::temporary_type temporary_type; + typedef typename temporary_type::value_type value_type; +}; + +/** Type promotion for three matrix types. */ +template < class Mat1_T, class Mat2_T, class Mat3_T > +struct MatrixPromote3 +{ + typedef typename MatrixPromote< + typename Mat1_T::temporary_type, + typename MatrixPromote< + typename Mat2_T::temporary_type, typename Mat3_T::temporary_type + >::temporary_type + >::temporary_type temporary_type; + typedef typename temporary_type::value_type value_type; +}; + +/** Type promotion for four matrix types. */ +template < class Mat1_T, class Mat2_T, class Mat3_T, class Mat4_T > +struct MatrixPromote4 +{ + typedef typename MatrixPromote< + typename Mat1_T::temporary_type, + typename MatrixPromote< + typename Mat2_T::temporary_type, + typename MatrixPromote< + typename Mat3_T::temporary_type, + typename Mat4_T::temporary_type + >::temporary_type + >::temporary_type + >::temporary_type temporary_type; + typedef typename temporary_type::value_type value_type; +}; + +/** Type promotion for a matrix and a scalar. */ +template +struct MatrixPromote, S> +{ + /* The deduced matrix result type (the array type is the same): */ + typedef cml::matrix::type, AT, BO, L> type; + + /* The deduced temporary type: */ + typedef typename type::temporary_type temporary_type; +}; + +/** Type promotion for a scalar and a matrix. */ +template +struct MatrixPromote > +{ + /* The deduced matrix result type (the array type is the same): */ + typedef cml::matrix::type, AT, BO, L> type; + + /* The deduced temporary type: */ + typedef typename type::temporary_type temporary_type; +}; + +/** Type promotion for outer product. */ +template +struct MatrixPromote< cml::vector, cml::vector > +{ + typedef cml::vector left_type; + typedef cml::vector right_type; + typedef CML_DEFAULT_BASIS_ORIENTATION basis_orient; + + /* Get matrix size: */ + enum { + array_rows = left_type::array_size, + array_cols = right_type::array_size + }; + + /* Deduce the corresponding matrix types for the vectors: */ + typedef CML_DEFAULT_ARRAY_LAYOUT layout; + typedef typename select_if< + array_rows == -1, dynamic<>, fixed + >::result left_storage; + typedef cml::matrix left_matrix; + + typedef typename select_if< + array_cols == -1, dynamic<>, fixed<1,array_cols> + >::result right_storage; + typedef cml::matrix right_matrix; + + /* Finally, promote the matrix types to get the result: */ + typedef typename et::MatrixPromote::type type; + typedef typename type::temporary_type temporary_type; +}; + +} // namespace et +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/matrix/matrix_rowcol.h b/src/cml/matrix/matrix_rowcol.h new file mode 100644 index 0000000..d324c92 --- /dev/null +++ b/src/cml/matrix/matrix_rowcol.h @@ -0,0 +1,261 @@ +/* -*- 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 Expressions to extract a row or column of a matrix. + */ + +#ifndef matrix_rowcol_h +#define matrix_rowcol_h + +#include +#include + +namespace cml { +namespace et { + +template +class MatrixRowOp +{ + public: + + typedef MatrixRowOp expr_type; + + /* Record ary-ness of the expression: */ + typedef unary_expression expr_ary; + + /* Copy the expression by value into higher-up expressions: */ + typedef expr_type expr_const_reference; + + typedef typename ExprT::value_type value_type; + typedef vector_result_tag result_tag; + typedef typename ExprT::size_tag size_tag; + + /* Store the expression traits: */ + typedef ExprTraits expr_traits; + + /* Get the reference type: */ + typedef typename expr_traits::const_reference expr_reference; + + /* Get the result vector type: */ + typedef typename expr_traits::result_type::row_vector_type result_type; + + /* Get the temporary type: */ + typedef typename result_type::temporary_type temporary_type; + + + public: + + /** Record result size as an enum. */ + enum { array_size = result_type::array_size }; + + + public: + + /** Return the expression size as a pair. */ + matrix_size size() const { + return expr_traits().rows(m_expr); + } + + /** Return reference to contained expression. */ + expr_reference expression() const { return m_expr; } + + /** Compute value at index i of the row vector. */ + value_type operator[](size_t i) const { + return expr_traits().get(m_expr,m_row,i); + } + + + public: + + /** Construct from the subexpression to store. */ + explicit MatrixRowOp(const ExprT& expr, size_t row) + : m_expr(expr), m_row(row) {} + + /** Copy constructor. */ + MatrixRowOp(const expr_type& e) + : m_expr(e.m_expr), m_row(e.m_row) {} + + + protected: + + expr_reference m_expr; + const size_t m_row; + + + private: + + /* Cannot be assigned to: */ + expr_type& operator=(const expr_type&); +}; + +/** Expression traits class for MatrixRowOp<>. */ +template +struct ExprTraits< MatrixRowOp > +{ + typedef MatrixRowOp expr_type; + typedef ExprT arg_type; + + typedef typename expr_type::value_type value_type; + typedef typename expr_type::expr_const_reference const_reference; + typedef typename expr_type::result_tag result_tag; + typedef typename expr_type::size_tag size_tag; + typedef typename expr_type::result_type result_type; + typedef expr_node_tag node_tag; + + value_type get(const expr_type& v, size_t i) const { return v[i]; } + size_t size(const expr_type& e) const { return e.size(); } +}; + +template +class MatrixColOp +{ + public: + + typedef MatrixColOp expr_type; + + /* Record ary-ness of the expression: */ + typedef unary_expression expr_ary; + + /* Copy the expression by value into higher-up expressions: */ + typedef expr_type expr_const_reference; + + typedef typename ExprT::value_type value_type; + typedef vector_result_tag result_tag; + typedef typename ExprT::size_tag size_tag; + + /* Store the expression traits: */ + typedef ExprTraits expr_traits; + + /* Get the reference type: */ + typedef typename expr_traits::const_reference expr_reference; + + /* Get the result vector type: */ + typedef typename expr_traits::result_type::col_vector_type result_type; + + /* Get the temporary type: */ + typedef typename result_type::temporary_type temporary_type; + + + public: + + /** Record result size as an enum. */ + enum { array_size = result_type::array_size }; + + + public: + + /** Return the expression size as a pair. */ + matrix_size size() const { + return expr_traits().cols(m_expr); + } + + /** Return reference to contained expression. */ + expr_reference expression() const { return m_expr; } + + /** Compute value at index i of the col vector. */ + value_type operator[](size_t i) const { + return expr_traits().get(m_expr,i,m_col); + } + + + public: + + /** Construct from the subexpression to store. */ + explicit MatrixColOp(const ExprT& expr, size_t col) + : m_expr(expr), m_col(col) {} + + /** Copy constructor. */ + MatrixColOp(const expr_type& e) + : m_expr(e.m_expr), m_col(e.m_col) {} + + + protected: + + expr_reference m_expr; + const size_t m_col; + + + private: + + /* Cannot be assigned to: */ + expr_type& operator=(const expr_type&); +}; + +/** Expression traits class for MatrixColOp<>. */ +template +struct ExprTraits< MatrixColOp > +{ + typedef MatrixColOp expr_type; + typedef ExprT arg_type; + + typedef typename expr_type::value_type value_type; + typedef typename expr_type::expr_const_reference const_reference; + typedef typename expr_type::result_tag result_tag; + typedef typename expr_type::size_tag size_tag; + typedef typename expr_type::result_type result_type; + typedef expr_node_tag node_tag; + + value_type get(const expr_type& v, size_t i) const { return v[i]; } + size_t size(const expr_type& e) const { return e.size(); } +}; + +} // namespace et + +/* Define the row and column operators in the cml namespace: */ + +/** Matrix row operator taking a matrix operand. */ +template +et::VectorXpr< et::MatrixRowOp< matrix > > +row(const matrix& expr, size_t i) +{ + typedef et::MatrixRowOp< matrix > ExprT; + return et::VectorXpr(ExprT(expr,i)); +} + +/** Matrix row operator taking an et::MatrixXpr operand. + * + * The parse tree is automatically compressed by hoisting the MatrixXpr's + * subexpression into the subexpression of the MatrixRowOp. + */ +template +et::VectorXpr< et::MatrixRowOp > +row(const et::MatrixXpr& expr, size_t i) +{ + typedef et::MatrixRowOp ExprT; + return et::MatrixXpr(ExprT(expr.expression(),i)); +} + +/** Matrix col operator taking a matrix operand. */ +template +et::VectorXpr< et::MatrixColOp< matrix > > +col(const matrix& expr, size_t i) +{ + typedef et::MatrixColOp< matrix > ExprT; + return et::VectorXpr(ExprT(expr,i)); +} + +/** Matrix col operator taking an et::MatrixXpr operand. + * + * The parse tree is automatically compressed by hoisting the MatrixXpr's + * subexpression into the subexpression of the MatrixColOp. + */ +template +et::VectorXpr< et::MatrixColOp > +col(const et::MatrixXpr& expr, size_t i) +{ + typedef et::MatrixColOp ExprT; + return et::VectorXpr(ExprT(expr.expression(),i)); +} + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/matrix/matrix_traits.h b/src/cml/matrix/matrix_traits.h new file mode 100644 index 0000000..7574ff6 --- /dev/null +++ b/src/cml/matrix/matrix_traits.h @@ -0,0 +1,49 @@ +/* -*- 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_traits_h +#define matrix_traits_h + +#include + +namespace cml { +namespace et { + +template +struct ExprTraits< cml::matrix > +{ + typedef typename cml::matrix expr_type; + typedef typename expr_type::value_type value_type; + typedef typename expr_type::expr_reference reference; + typedef typename expr_type::expr_const_reference const_reference; + typedef typename expr_type::result_tag result_tag; + typedef typename expr_type::size_tag size_tag; + typedef typename expr_type::assignable_tag assignable_tag; + typedef expr_type result_type; + typedef expr_leaf_tag node_tag; + + value_type get(const expr_type& m, size_t i, size_t j) const { + return m(i,j); + } + + matrix_size size(const expr_type& e) const { return e.size(); } + size_t rows(const expr_type& m) const { return m.rows(); } + size_t cols(const expr_type& m) const { return m.cols(); } +}; + +} // namespace et +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/matrix/matrix_transpose.h b/src/cml/matrix/matrix_transpose.h new file mode 100644 index 0000000..3f54843 --- /dev/null +++ b/src/cml/matrix/matrix_transpose.h @@ -0,0 +1,305 @@ +/* -*- 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 + * + * @todo Currently, the transpose() and T() functions copy the transposed + * result into a temporary, and return it to avoid aliasing problems, e.g. + * C = transpose(C). By checking for C on the right-hand side, this can + * be avoided, but experimentation is needed to determine the impact on + * performance. Another option is to use a function to explicitly specify + * when a temporary is needed; e.g. C = transpose(temp(C)). + */ + +#ifndef matrix_transpose_h +#define matrix_transpose_h + +#include + +#define MATRIX_TRANSPOSE_RETURNS_TEMP + +namespace cml { +namespace et { + +/** "Transpose" the given matrix expression. + * + * This does nothing more than change the result type of the expression + * into one with the opposite orientation (i.e. row->col, col->row). + */ +template +class MatrixTransposeOp +{ + public: + + typedef MatrixTransposeOp expr_type; + + /* Record ary-ness of the expression: */ + typedef unary_expression expr_ary; + + /* Copy the expression by value into higher-up expressions: */ + typedef expr_type expr_const_reference; + + typedef typename ExprT::value_type value_type; + typedef matrix_result_tag result_tag; + typedef typename ExprT::size_tag size_tag; + + /* Store the expression traits: */ + typedef ExprTraits expr_traits; + + /* Get the reference type: */ + typedef typename expr_traits::const_reference expr_reference; + + /* Swap the orientation: */ + typedef typename expr_traits::result_type::transposed_type result_type; + + /* Get the temporary type: */ + typedef typename result_type::temporary_type temporary_type; + + /* For matching by assignability: */ + typedef cml::et::not_assignable_tag assignable_tag; + + + public: + + /** Record result size as an enum. */ + enum { + array_rows = result_type::array_rows, + array_cols = result_type::array_cols + }; + + + public: + + /** Return the expression size as a pair. */ + matrix_size size() const { + return matrix_size(this->rows(),this->cols()); + } + + /** Return result rows. + * + * The tranpose has the same number of rows as the original has + * columns. + */ + size_t rows() const { + return expr_traits().cols(m_expr); + } + + /** Return result cols. + * + * The tranpose has the same number of columns as the original has + * rows. + */ + size_t cols() const { + return expr_traits().rows(m_expr); + } + + /** Return reference to contained expression. */ + expr_reference expression() const { return m_expr; } + + /** Compute value at index i of the result matrix. + * + * Element (i,j) of the transpose is element (j,i) of the original + * expression. + */ + value_type operator()(size_t i, size_t j) const { + return expr_traits().get(m_expr,j,i); + } + + + public: + + /** Construct from the subexpression to store. */ + explicit MatrixTransposeOp(const ExprT& expr) : m_expr(expr) {} + + /** Copy constructor. */ + MatrixTransposeOp(const expr_type& e) : m_expr(e.m_expr) {} + + + protected: + + expr_reference m_expr; + + + private: + + /* Cannot be assigned to: */ + expr_type& operator=(const expr_type&); +}; + +/** Expression traits class for VectorTransposeOp<>. */ +template +struct ExprTraits< MatrixTransposeOp > +{ + typedef MatrixTransposeOp expr_type; + typedef typename expr_type::value_type value_type; + typedef typename expr_type::expr_const_reference const_reference; + typedef typename expr_type::result_tag result_tag; + typedef typename expr_type::size_tag size_tag; + typedef typename expr_type::result_type result_type; + typedef typename expr_type::assignable_tag assignable_tag; + typedef expr_node_tag node_tag; + + value_type get(const expr_type& m, size_t i, size_t j) const { + return m(i,j); + } + + matrix_size size(const expr_type& e) const { return e.size(); } + size_t rows(const expr_type& e) const { return e.rows(); } + size_t cols(const expr_type& e) const { return e.cols(); } +}; + +} // namespace et + + +/* Define the transpose operators in the cml namespace: */ +#if defined(MATRIX_TRANSPOSE_RETURNS_TEMP) + +/** Matrix transpose operator taking a matrix operand. */ +template +typename et::MatrixTransposeOp< + matrix +>::temporary_type +transpose(const matrix& expr) +{ + /* Record the matrix type: */ + typedef matrix matrix_type; + + /* Record the type of the transpose op: */ + typedef et::MatrixTransposeOp Op; + + /* Determine the returned matrix type: */ + typedef typename et::MatrixTransposeOp< + matrix_type + >::temporary_type tmp_type; + + /* The expression to use to assign the temporary: */ + typedef et::MatrixXpr ExprT; + + /* Create the temporary and return it: */ + tmp_type tmp; + cml::et::detail::Resize(tmp,expr.rows(),expr.cols()); + tmp = ExprT(Op(expr)); + return tmp; +} + +/** Matrix transpose operator taking an et::MatrixXpr operand. + * + * The parse tree is automatically compressed by hoisting the MatrixXpr's + * subexpression into the subexpression of the MatrixTransposeOp. + */ +template +typename et::MatrixTransposeOp< + XprT +>::temporary_type +transpose(MATXPR_ARG_TYPE expr) +{ + /* Record the type of the transpose op: */ + typedef et::MatrixTransposeOp Op; + + /* Determine the returned matrix type: */ + typedef typename et::MatrixTransposeOp::temporary_type tmp_type; + + /* The expression to use to assign the temporary: */ + typedef et::MatrixXpr ExprT; + + /* Create the temporary and return it: */ + tmp_type tmp; + cml::et::detail::Resize(tmp,expr.rows(),expr.cols()); + tmp = ExprT(Op(expr.expression())); + return tmp; +} + + +/* For notational convenience: */ + +/** Matrix transpose operator taking a matrix operand. */ +template +typename et::MatrixTransposeOp< + matrix +>::temporary_type +T(const matrix& expr) +{ + return transpose(expr); +} + +/** Matrix transpose operator taking an et::MatrixXpr operand. + * + * The parse tree is automatically compressed by hoisting the MatrixXpr's + * subexpression into the subexpression of the MatrixTransposeOp. + */ +template +typename et::MatrixTransposeOp< + XprT +>::temporary_type +T(MATXPR_ARG_TYPE expr) +{ + return transpose(expr); +} + +#else + +/* XXX For this to work correctly, matrix assignment and copy have to be + * changed to either use a temporary all the time, or to create a temporary + * when the same matrix appears on both sides of an assignment, and a + * temporary was not already created on the RHS by the ET code. + */ + +/** Matrix transpose operator taking a matrix operand. */ +template +et::MatrixXpr< et::MatrixTransposeOp< matrix > > +transpose(const matrix& expr) +{ + typedef et::MatrixTransposeOp< matrix > ExprT; + return et::MatrixXpr(ExprT(expr)); +} + +/** Matrix transpose operator taking an et::MatrixXpr operand. + * + * The parse tree is automatically compressed by hoisting the MatrixXpr's + * subexpression into the subexpression of the MatrixTransposeOp. + */ +template +et::MatrixXpr< et::MatrixTransposeOp > +transpose(MATXPR_ARG_TYPE expr) +{ + typedef et::MatrixTransposeOp ExprT; + return et::MatrixXpr(ExprT(expr.expression())); +} + + +/* For notational convenience: */ + +/** Matrix transpose operator taking a matrix operand. */ +template +et::MatrixXpr< et::MatrixTransposeOp< matrix > > +T(const matrix& expr) +{ + return transpose(expr); +} + +/** Matrix transpose operator taking an et::MatrixXpr operand. + * + * The parse tree is automatically compressed by hoisting the MatrixXpr's + * subexpression into the subexpression of the MatrixTransposeOp. + */ +template +et::MatrixXpr< et::MatrixTransposeOp > +T(MATXPR_ARG_TYPE expr) +{ + return transpose(expr); +} + +#endif + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/matrix/matrix_unroller.h b/src/cml/matrix/matrix_unroller.h new file mode 100644 index 0000000..388abcf --- /dev/null +++ b/src/cml/matrix/matrix_unroller.h @@ -0,0 +1,285 @@ +/* -*- 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 + * + * @todo Need to implement unrolling for efficient col-major array access. + * + * @todo Does it make sense to unroll an assignment if either side of the + * assignment has a fixed size, or just when the target matrix is fixed + * size? + */ + +#ifndef matrix_unroller_h +#define matrix_unroller_h + +#include +#include +#include + +#if !defined(CML_2D_UNROLLER) && !defined(CML_NO_2D_UNROLLER) +#error "The matrix unroller has not been defined." +#endif + +#if defined(CML_2D_UNROLLER) && !defined(CML_MATRIX_UNROLL_LIMIT) +#error "CML_MATRIX_UNROLL_LIMIT is undefined." +#endif + +namespace cml { +namespace et { +namespace detail { + +/** Unroll a binary assignment operator on a fixed-size matrix. + * + * This uses a forward iteration to make better use of the cache. + * + * @sa cml::matrix + * @sa cml::et::OpAssign + * + * @bug Need to verify that OpT is actually an assignment operator. + * @bug The 2D unroller needs to be specified for efficient col-major + * access. + */ +template +class MatrixAssignmentUnroller +{ + protected: + + /* The matrix type being assigned to: */ + typedef cml::matrix matrix_type; + + /* Record traits for the arguments: */ + typedef ExprTraits dest_traits; + typedef ExprTraits src_traits; + +#if defined(CML_2D_UNROLLER) + + /* Forward declare: */ + template + struct Eval; + + /* XXX This needs to be specified for efficient col-major access also! */ + + /** Evaluate the binary operator at element R,C. */ + template + struct Eval { + void operator()(matrix_type& dest, const SrcT& src) const { + + /* Apply to current R,C: */ + OpT().apply(dest(R,C), src_traits().get(src,R,C)); + + /* Evaluate at R,C+1: */ + Eval()(dest,src); + } + }; + + /** Evaluate the binary operator at element R,LastCol. */ + template + struct Eval { + void operator()(matrix_type& dest, const SrcT& src) const { + + /* Apply to R,LastCol: */ + OpT().apply(dest(R,LastCol), src_traits().get(src,R,LastCol)); + + /* Evaluate at R+1,0; i.e. move to next row and start the + * col iteration from 0: + */ + Eval()(dest,src); + } + }; + + /** Evaluate the binary operator at element LastRow,C. */ + template + struct Eval { + void operator()(matrix_type& dest, const SrcT& src) const { + + /* Apply to LastRow,C: */ + OpT().apply(dest(LastRow,C), src_traits().get(src,LastRow,C)); + + /* Evaluate at LastRow,C+1: */ + Eval()(dest,src); + } + }; + + /** Evaluate the binary operator at element LastRow,LastCol. */ + template + struct Eval { + void operator()(matrix_type& dest, const SrcT& src) const { + + /* Apply to LastRow,LastCol: */ + OpT().apply( + dest(LastRow,LastCol), + src_traits().get(src,LastRow,LastCol)); + } + }; + + + /** Evaluate operators on large matrices using a loop. */ + template + struct Eval { + void operator()(matrix_type& dest, const SrcT& src) const { + for(size_t i = 0; i <= LastRow; ++i) { + for(size_t j = 0; j <= LastCol; ++j) { + OpT().apply(dest(i,j), src_traits().get(src,i,j)); + } + } + } + }; + +#endif // CML_2D_UNROLLER + +#if defined(CML_NO_2D_UNROLLER) + + /** Evaluate the binary operator using a loop. */ + template struct Eval { + void operator()(matrix_type& dest, const SrcT& src) const { + for(size_t i = 0; i <= LastRow; ++i) { + for(size_t j = 0; j <= LastCol; ++j) { + OpT().apply(dest(i,j), src_traits().get(src,i,j)); + } + } + } + }; + +#endif // CML_NO_2D_UNROLLER + + + public: + + /** Unroll assignment for a fixed-sized matrix. */ + void operator()( + cml::matrix& dest, const SrcT& src, cml::fixed_size_tag) + { + typedef cml::matrix matrix_type; + enum { + LastRow = matrix_type::array_rows-1, + LastCol = matrix_type::array_cols-1, + Max = (LastRow+1)*(LastCol+1) + }; + +#if defined(CML_2D_UNROLLER) + typedef typename MatrixAssignmentUnroller + ::template Eval<0, 0, LastRow, LastCol, + (Max <= CML_MATRIX_UNROLL_LIMIT)> Unroller; +#endif + +#if defined(CML_NO_2D_UNROLLER) + /* Use a loop: */ + typedef typename MatrixAssignmentUnroller + ::template Eval<0, 0, LastRow, LastCol> Unroller; +#endif + + /* Use a run-time check if src is a run-time sized expression: */ + typedef typename ExprTraits::size_tag src_size; + typedef typename select_if< + same_type::is_true, + dynamic_size_tag, fixed_size_tag>::result size_tag; + + /* Check the expression size (the returned size isn't needed): */ + CheckedSize(dest,src,size_tag()); + /* Note: for two fixed-size expressions, the if-statements and + * comparisons should be completely eliminated as dead code. If + * src is a dynamic-sized expression, the check will still happen. + */ + + Unroller()(dest,src); + } + + + private: + /* XXX Blah, a temp. hack to fix the auto-resizing stuff below. */ + matrix_size hack_actual_size(const SrcT& /*src*/, scalar_result_tag) { + return matrix_size(1,1); + } + + matrix_size hack_actual_size(const SrcT& src, matrix_result_tag) { + typedef ExprTraits src_traits; + return src_traits().size(src); + } + + matrix_size CheckOrResize( + matrix_type& dest, const SrcT& src, cml::resizable_tag) + { +#if defined(CML_AUTOMATIC_MATRIX_RESIZE_ON_ASSIGNMENT) + /* Get the size of src. This also causes src to check its size: */ + matrix_size N = hack_actual_size( + src, typename src_traits::result_tag()); + + /* Set the destination matrix's size: */ + dest.resize(N.first,N.second); +#else + matrix_size N = CheckedSize(dest,src,dynamic_size_tag()); +#endif + return N; + } + + matrix_size CheckOrResize( + matrix_type& dest, const SrcT& src, cml::not_resizable_tag) + { + return CheckedSize(dest,src,dynamic_size_tag()); + } + /* XXX Blah, a temp. hack to fix the auto-resizing stuff below. */ + public: + + + /** Use a loop for dynamic-sized matrix assignment. + * + * @note The target matrix must already have the correct size. + * + * @todo This needs to be specialized for efficient row-major or col-major + * layout access. + */ + void operator()(matrix_type& dest, const SrcT& src, cml::dynamic_size_tag) + { + typedef ExprTraits src_traits; + matrix_size N = this->CheckOrResize( + dest,src,typename matrix_type::resizing_tag()); + for(size_t i = 0; i < N.first; ++i) { + for(size_t j = 0; j < N.second; ++j) { + OpT().apply(dest(i,j), src_traits().get(src,i,j)); + /* Note: we don't need get(), since dest is a matrix. */ + } + } + } +}; + +} + +/** This constructs an assignment unroller for fixed-size arrays. + * + * The operator must be an assignment op (otherwise, this doesn't make any + * sense). Also, automatic unrolling is only performed for fixed-size + * matrices; a loop is used for dynamic-sized matrices. + * + * @sa cml::matrix + * @sa cml::et::OpAssign + * + * @bug Need to verify that OpT is actually an assignment operator. + */ +template +inline void UnrollAssignment(cml::matrix& dest, const SrcT& src) +{ + /* Record the destination matrix type, and the expression traits: */ + typedef cml::matrix matrix_type; + + /* Record the type of the unroller: */ + typedef detail::MatrixAssignmentUnroller unroller; + + /* Finally, do the unroll call: */ + unroller()(dest, src, typename matrix_type::size_tag()); + /* XXX It may make sense to unroll if either side is a fixed size. */ +} + +} // namespace et +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/matvec/matvec_mul.h b/src/cml/matvec/matvec_mul.h new file mode 100644 index 0000000..5f5ea92 --- /dev/null +++ b/src/cml/matvec/matvec_mul.h @@ -0,0 +1,285 @@ +/* -*- 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 Multiply a matrix and a vector. + * + * @todo Implement smarter temporary generation. + * + * @todo Does it make sense to put mat-vec multiplication as a node into the + * expression tree? + * + * @internal This does not need to return an expression type, since the + * temporary generation for the matrix result is handled automatically by the + * compiler. i.e. when used in an expression, the result is automatically + * included in the expression tree as a temporary by the compiler. + */ + +#ifndef matvec_mul_h +#define matvec_mul_h + +#include +#include +#include +#include + +/* This is used below to create a more meaningful compile-time error when + * mat-vec mul is not provided with the right arguments: + */ +struct mvmul_expects_one_matrix_and_one_vector_arg_error; +struct mvmul_expects_one_vector_and_one_matrix_arg_error; + +namespace cml { +namespace detail { + +/* For choosing the proper multiplication order: */ +typedef true_type mul_Ax; +typedef false_type mul_xA; + +/** Compute y = A*x. */ +template inline +typename et::MatVecPromote< + typename et::ExprTraits::result_type, + typename et::ExprTraits::result_type +>::temporary_type +mul(const LeftT& A, const RightT& x, mul_Ax) +{ + /* Shorthand: */ + typedef et::ExprTraits left_traits; + typedef et::ExprTraits right_traits; + typedef typename left_traits::result_tag left_result; + typedef typename right_traits::result_tag right_result; + + /* mul()[A*x] requires a matrix and a vector expression: */ + CML_STATIC_REQUIRE_M( + (same_type::is_true + && same_type::is_true), + mvmul_expects_one_matrix_and_one_vector_arg_error); + /* Note: parens are required here so that the preprocessor ignores the + * commas. + */ + + /* Get result type: */ + typedef typename et::MatVecPromote< + typename left_traits::result_type, + typename right_traits::result_type + >::temporary_type result_type; + + /* Record size type: */ + typedef typename result_type::size_tag size_tag; + + /* Check the size: */ + size_t N = et::CheckedSize(A, x, size_tag()); + + /* Initialize the new vector: */ + result_type y; cml::et::detail::Resize(y, N); + + /* Compute y = A*x: */ + typedef typename result_type::value_type sum_type; + for(size_t i = 0; i < N; ++i) { + /* XXX This should be unrolled. */ + sum_type sum(A(i,0)*x[0]); + for(size_t k = 1; k < x.size(); ++k) { + sum += (A(i,k)*x[k]); + } + y[i] = sum; + } + + return y; +} + +/** Compute y = x*A. */ +template inline +typename et::MatVecPromote< + typename et::ExprTraits::result_type, + typename et::ExprTraits::result_type +>::temporary_type +mul(const LeftT& x, const RightT& A, mul_xA) +{ + /* Shorthand: */ + typedef et::ExprTraits left_traits; + typedef et::ExprTraits right_traits; + typedef typename left_traits::result_tag left_result; + typedef typename right_traits::result_tag right_result; + + /* mul()[x*A] requires a vector and a matrix expression: */ + CML_STATIC_REQUIRE_M( + (same_type::is_true + && same_type::is_true), + mvmul_expects_one_vector_and_one_matrix_arg_error); + /* Note: parens are required here so that the preprocessor ignores the + * commas. + */ + + /* Get result type: */ + typedef typename et::MatVecPromote< + typename left_traits::result_type, + typename right_traits::result_type + >::temporary_type result_type; + + /* Record size type: */ + typedef typename result_type::size_tag size_tag; + + /* Check the size: */ + size_t N = et::CheckedSize(x, A, size_tag()); + + /* Initialize the new vector: */ + result_type y; cml::et::detail::Resize(y, N); + + /* Compute y = x*A: */ + typedef typename result_type::value_type sum_type; + for(size_t i = 0; i < N; ++i) { + /* XXX This should be unrolled. */ + sum_type sum(x[0]*A(0,i)); + for(size_t k = 1; k < x.size(); ++k) { + sum += (x[k]*A(k,i)); + } + y[i] = sum; + } + + return y; +} + +} // namespace detail + + +/** operator*() for a matrix and a vector. */ +template +inline typename et::MatVecPromote< + matrix, vector +>::temporary_type +operator*(const matrix& left, + const vector& right) +{ + return detail::mul(left,right,detail::mul_Ax()); +} + +/** operator*() for a matrix and a VectorXpr. */ +template +inline typename et::MatVecPromote< + matrix, typename XprT::result_type +>::temporary_type +operator*(const matrix& left, + const et::VectorXpr& right) +{ + /* Generate a temporary, and compute the right-hand expression: */ + typename et::VectorXpr::temporary_type right_tmp; + cml::et::detail::Resize(right_tmp,right.size()); + right_tmp = right; + + return detail::mul(left,right_tmp,detail::mul_Ax()); +} + +/** operator*() for a MatrixXpr and a vector. */ +template +inline typename et::MatVecPromote< + typename XprT::result_type, vector +>::temporary_type +operator*(const et::MatrixXpr& left, + const vector& right) +{ + /* Generate a temporary, and compute the left-hand expression: */ + typename et::MatrixXpr::temporary_type left_tmp; + cml::et::detail::Resize(left_tmp,left.rows(),left.cols()); + left_tmp = left; + + return detail::mul(left_tmp,right,detail::mul_Ax()); +} + +/** operator*() for a MatrixXpr and a VectorXpr. */ +template +inline typename et::MatVecPromote< + typename XprT1::result_type, typename XprT2::result_type +>::temporary_type +operator*(const et::MatrixXpr& left, + const et::VectorXpr& right) +{ + /* Generate a temporary, and compute the left-hand expression: */ + typename et::MatrixXpr::temporary_type left_tmp; + cml::et::detail::Resize(left_tmp,left.rows(),left.cols()); + left_tmp = left; + + /* Generate a temporary, and compute the right-hand expression: */ + typename et::VectorXpr::temporary_type right_tmp; + cml::et::detail::Resize(right_tmp,right.size()); + right_tmp = right; + + return detail::mul(left_tmp,right_tmp,detail::mul_Ax()); +} + +/** operator*() for a vector and a matrix. */ +template +inline typename et::MatVecPromote< + vector, matrix +>::temporary_type +operator*(const vector& left, + const matrix& right) +{ + return detail::mul(left,right,detail::mul_xA()); +} + +/** operator*() for a vector and a MatrixXpr. */ +template +inline typename et::MatVecPromote< + typename XprT::result_type, vector +>::temporary_type +operator*(const vector& left, + const et::MatrixXpr& right) +{ + /* Generate a temporary, and compute the right-hand expression: */ + typename et::MatrixXpr::temporary_type right_tmp; + cml::et::detail::Resize(right_tmp,right.rows(),right.cols()); + right_tmp = right; + + return detail::mul(left,right_tmp,detail::mul_xA()); +} + +/** operator*() for a VectorXpr and a matrix. */ +template +inline typename et::MatVecPromote< + typename XprT::result_type, matrix +>::temporary_type +operator*(const et::VectorXpr& left, + const matrix& right) +{ + /* Generate a temporary, and compute the left-hand expression: */ + typename et::VectorXpr::temporary_type left_tmp; + cml::et::detail::Resize(left_tmp,left.size()); + left_tmp = left; + + return detail::mul(left_tmp,right,detail::mul_xA()); +} + +/** operator*() for a VectorXpr and a MatrixXpr. */ +template +inline typename et::MatVecPromote< + typename XprT1::result_type, typename XprT2::result_type +>::temporary_type +operator*(const et::VectorXpr& left, + const et::MatrixXpr& right) +{ + /* Generate a temporary, and compute the left-hand expression: */ + typename et::VectorXpr::temporary_type left_tmp; + cml::et::detail::Resize(left_tmp,left.size()); + left_tmp = left; + + /* Generate a temporary, and compute the right-hand expression: */ + typename et::MatrixXpr::temporary_type right_tmp; + cml::et::detail::Resize(right_tmp,right.rows(),right.cols()); + right_tmp = right; + + return detail::mul(left_tmp,right_tmp,detail::mul_xA()); +} + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/matvec/matvec_promotions.h b/src/cml/matvec/matvec_promotions.h new file mode 100644 index 0000000..3b85d1f --- /dev/null +++ b/src/cml/matvec/matvec_promotions.h @@ -0,0 +1,92 @@ +/* -*- 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 for the vectors resulting from matrix/vector or + * vector/matrix ops. + * + * @sa matvec_ops::mvmul + */ + +#ifndef matvec_promotions_h +#define matvec_promotions_h + +#include +#include + +namespace cml { +namespace et { + +/* Default mat/vec type promotion template. */ +template struct MatVecPromote; + +/** Type promotion for a matrix and a vector. */ +template< + typename E1, class AT1, typename BO, typename L, + typename E2, class AT2> +struct MatVecPromote< cml::matrix, cml::vector > +{ + typedef cml::matrix matrix_type; + typedef cml::vector vector_type; + + /* Promote the arrays: */ + typedef typename ArrayPromote< + typename matrix_type::array_type, + typename vector_type::array_type + >::type promoted_array; + + /* The deduced vector result type: */ + typedef cml::vector< + typename promoted_array::value_type, + typename promoted_array::generator_type + > type; + + /* The deduced temporary type: */ + typedef typename type::temporary_type temporary_type; + /* Note: this is to avoid an "incomplete type" error from ICC9, which + * can't handle e.g. :::: when is a template type. + */ +}; + +/** Type promotion for a vector and a matrix. */ +template< + typename E1, class AT1, + typename E2, class AT2, typename BO, typename L> +struct MatVecPromote< cml::vector, cml::matrix > +{ + typedef cml::vector vector_type; + typedef cml::matrix matrix_type; + + /* Promote the arrays: */ + typedef typename ArrayPromote< + typename vector_type::array_type, + typename matrix_type::array_type + >::type promoted_array; + + /* The deduced vector result type: */ + typedef cml::vector< + typename promoted_array::value_type, + typename promoted_array::generator_type + > type; + + /* The deduced temporary type: */ + typedef typename type::temporary_type temporary_type; + /* Note: this is to avoid an "incomplete type" error from ICC9, which + * can't handle e.g. :::: when is a template type. + */ +}; + +} // namespace et +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/quaternion.h b/src/cml/quaternion.h new file mode 100644 index 0000000..595fe03 --- /dev/null +++ b/src/cml/quaternion.h @@ -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 cml_quaternion_h +#define cml_quaternion_h + +#include + +namespace cml { + +// NOTE: 'scale' constant no longer used. + +/** Helper to specify v1^v2 multiplication order. */ +struct positive_cross { + /*enum { scale = 1 };*/ +}; + +/** Helper to specify v2^v1 multiplication order. */ +struct negative_cross { + /*enum { scale = -1 };*/ +}; + +/** Helper to specify scalar-first quaternion ordering. */ +struct scalar_first { + enum { W, X, Y, Z }; +}; + +/** Helper to specify vector-first quaternion ordering. */ +struct vector_first { + enum { X, Y, Z, W }; +}; + +/** A configurable quaternion. + * + * This class encapsulates the notion of a quaternion. The ArrayType + * template argument can be used to select the type of array to be used as + * internal storage for the quaternion's coefficients. + * + * @note Quaternions with two different orders cannot be used in the same + * expression. + */ +template, + class Order = scalar_first, class Cross = positive_cross> class quaternion; + +} // namespace cml + +#include +#include +#include +#include +#include +#include +#include + +#include + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/quaternion/conjugate.h b/src/cml/quaternion/conjugate.h new file mode 100644 index 0000000..63bdcb0 --- /dev/null +++ b/src/cml/quaternion/conjugate.h @@ -0,0 +1,197 @@ +/* -*- 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 an operator for quaternion conjugation. + */ + +#ifndef conjugate_h +#define conjugate_h + +#include + +namespace cml { +namespace et { + +/** An expression node for conjugating a quaternion. */ +template +class ConjugateOp +{ + public: + + typedef ConjugateOp expr_type; + + /* Record ary-ness of the expression: */ + typedef unary_expression expr_ary; + + /* Copy the expression by value into higher-up expressions: */ + typedef expr_type expr_const_reference; + + typedef typename ExprT::value_type value_type; + typedef quaternion_result_tag result_tag; + typedef typename ExprT::size_tag size_tag; + + /* Store the expression traits for the subexpression: */ + typedef ExprTraits expr_traits; + + /* Reference type for the subexpression: */ + typedef typename expr_traits::const_reference expr_reference; + + /* Get the result type (same as for subexpression): */ + typedef typename expr_traits::result_type result_type; + + /* For matching by assignability: */ + typedef cml::et::not_assignable_tag assignable_tag; + + /* Get the temporary type: */ + typedef typename result_type::temporary_type temporary_type; + + /* Get the vector type: */ + typedef typename result_type::vector_type vector_type; + + /* Get the imaginary part type: */ + typedef typename vector_type::subvector_type imaginary_type; + + /* Record the order type: */ + typedef typename result_type::order_type order_type; + + + public: + + /** Record result size as an enum. */ + enum { array_size = ExprT::array_size }; + + /** Localize the ordering as an enum. */ + enum { + W = order_type::W, + X = order_type::X, + Y = order_type::Y, + Z = order_type::Z + }; + + + public: + + /** Return the real part of the expression. */ + value_type real() const { + return m_expr.real(); + } + + /** Return the vector part of the expression. */ + imaginary_type imaginary() const { + return -m_expr.imaginary(); + } + + /** Return the Cayley norm of the expression. */ + value_type norm() const { + return length_squared(); + } + + /** Return square of the quaternion length. */ + value_type length_squared() const { + return dot( + QuaternionXpr(*this), + QuaternionXpr(*this)); + } + + /** Return the quaternion length. */ + value_type length() const { + return std::sqrt(length_squared()); + } + + /** Return the result as a normalized quaternion. */ + temporary_type normalize() const { + temporary_type q(QuaternionXpr(*this)); + return q.normalize(); + } + + /** Compute conjugated result at index i. + * + * The conjugate of quaternion s + v is s - v. + */ + value_type operator[](size_t i) const { + return (i == W) ? m_expr[W] : - m_expr[i] ; + } + + + public: + + /** Return size of this expression (same as argument's size). */ + size_t size() const { + return m_expr.size(); + } + + /** Return reference to contained expression. */ + expr_reference expression() const { return m_expr; } + + + public: + + /** Construct from the subexpression. */ + explicit ConjugateOp(expr_reference expr) : m_expr(expr) {} + + /** Copy constructor. */ + ConjugateOp(const expr_type& e) : m_expr(e.m_expr) {} + + + protected: + + expr_reference m_expr; + + + private: + + /* Cannot be assigned to: */ + expr_type& operator=(const expr_type&); +}; + +/** Expression traits class for ConjugateOp<>. */ +template +struct ExprTraits< ConjugateOp > +{ + typedef ConjugateOp expr_type; + typedef ExprT arg_type; + + typedef typename expr_type::value_type value_type; + typedef typename expr_type::expr_const_reference const_reference; + typedef typename expr_type::result_tag result_tag; + typedef typename expr_type::size_tag size_tag; + typedef typename expr_type::result_type result_type; + typedef typename expr_type::assignable_tag assignable_tag; + typedef expr_node_tag node_tag; + + value_type get(const expr_type& v, size_t i) const { return v[i]; } + size_t size(const expr_type& e) const { return e.size(); } +}; + +} // namespace et + +/** Conjugation of a quaternion. */ +template inline +et::QuaternionXpr< et::ConjugateOp< quaternion > > +conjugate(const quaternion& arg) +{ + typedef et::ConjugateOp< quaternion > ExprT; + return et::QuaternionXpr(ExprT(arg)); +} + +/** Conjugation of a QuaternionXpr. */ +template inline +et::QuaternionXpr< et::ConjugateOp > +conjugate(QUATXPR_ARG_TYPE arg) +{ + typedef et::ConjugateOp ExprT; + return et::QuaternionXpr(ExprT(arg.expression())); +} + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/quaternion/inverse.h b/src/cml/quaternion/inverse.h new file mode 100644 index 0000000..8387c90 --- /dev/null +++ b/src/cml/quaternion/inverse.h @@ -0,0 +1,268 @@ +/* -*- 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 an operator for quaternion inverse. + */ + +#ifndef quaternion_inverse_h +#define quaternion_inverse_h + +#include +#include + +namespace cml { +namespace et { + +/** An expression node for inverting a quaternion. + * + * This internally creates a ConjugateOp node to process the conjugate + * of the given expression. The values produced by the ConjugateOp are then + * divided by the Cayley norm of the expression on the fly. + */ +template +class QuaternionInverseOp +{ + public: + + typedef QuaternionInverseOp expr_type; + + /* Record ary-ness of the expression: */ + typedef unary_expression expr_ary; + + /* Copy the expression by value into higher-up expressions: */ + typedef expr_type expr_const_reference; + + /* The subexpression is a ConjugateOp: */ + typedef et::ConjugateOp subexpression_type; + typedef ExprTraits expr_traits; + + /* Get traits for the ExprT: */ + typedef ExprTraits arg_traits; + typedef typename arg_traits::const_reference arg_reference; + + typedef typename subexpression_type::value_type value_type; + typedef quaternion_result_tag result_tag; + typedef typename subexpression_type::size_tag size_tag; + + /* Reference type for the subexpression: */ + typedef typename expr_traits::const_reference expr_reference; + + /* Get the result type (same as for subexpression): */ + typedef typename expr_traits::result_type result_type; + + /* For matching by assignability: */ + typedef cml::et::not_assignable_tag assignable_tag; + + /* Get the temporary type: */ + typedef typename result_type::temporary_type temporary_type; + + /* Get the vector type: */ + typedef typename result_type::vector_type vector_type; + + /* Get the imaginary part type: */ + typedef typename vector_type::subvector_type imaginary_type; + + /* Record the order type: */ + typedef typename result_type::order_type order_type; + + + public: + + /** Record result size as an enum. */ + enum { array_size = ExprT::array_size }; + + /** Localize the ordering as an enum. */ + enum { + W = order_type::W, + X = order_type::X, + Y = order_type::Y, + Z = order_type::Z + }; + + + public: + + /** Return the real part of the expression. */ + value_type real() const { + return m_expr.real()/m_norm; + } + + /** Return the vector part of the expression. + * + * @todo This could be returned as a VectorXpr also. + */ + imaginary_type imaginary() const { + return m_expr.imaginary()/m_norm; + } + + /** Return the Cayley norm of the expression. */ + value_type norm() const { + return length_squared(); + } + + /** Return square of the quaternion length. */ + value_type length_squared() const { + return dot( + QuaternionXpr(*this), + QuaternionXpr(*this)); + } + + /** Return the quaternion length. */ + value_type length() const { + return std::sqrt(length_squared()); + } + + /** Return the result as a normalized quaternion. */ + temporary_type normalize() const { + temporary_type q(QuaternionXpr(*this)); + return q.normalize(); + } + + /** Compute inverse result at index i. + * + * The inverse of a quaternion p is ~p/norm(p). + */ + value_type operator[](size_t i) const { + return m_expr[i]/m_norm; + } + + + public: + + /** Return size of this expression (same as argument's size). */ + size_t size() const { + return m_expr.size(); + } + + /** Return reference to contained expression. */ + expr_reference expression() const { return m_expr; } + + + public: + + /** Construct from an input expression. */ + explicit QuaternionInverseOp(arg_reference arg) + //: m_expr(arg), m_norm(cml::norm(arg)) {} + : m_expr(arg), m_norm(arg.norm()) {} + + /** Copy constructor. */ + QuaternionInverseOp(const expr_type& e) + : m_expr(e.m_expr), m_norm(e.m_norm) {} + + + protected: + + subexpression_type m_expr; + value_type m_norm; + + + private: + + /* Cannot be assigned to: */ + expr_type& operator=(const expr_type&); +}; + +/** Expression traits class for QuaternionInverseOp<>. */ +template +struct ExprTraits< QuaternionInverseOp > +{ + typedef QuaternionInverseOp expr_type; + typedef ExprT arg_type; + + typedef typename expr_type::value_type value_type; + typedef typename expr_type::expr_const_reference const_reference; + typedef typename expr_type::result_tag result_tag; + typedef typename expr_type::size_tag size_tag; + typedef typename expr_type::result_type result_type; + typedef typename expr_type::assignable_tag assignable_tag; + typedef expr_node_tag node_tag; + + value_type get(const expr_type& v, size_t i) const { return v[i]; } + size_t size(const expr_type& e) const { return e.size(); } +}; + +} // namespace et + +/** Inverse of a quaternion. */ +template inline +et::QuaternionXpr< et::QuaternionInverseOp< quaternion > > +inverse(const quaternion& arg) +{ + typedef et::QuaternionInverseOp< quaternion > ExprT; + return et::QuaternionXpr(ExprT(arg)); +} + +/** Inverse of a QuaternionXpr. */ +template inline +et::QuaternionXpr< et::QuaternionInverseOp > +inverse(QUATXPR_ARG_TYPE arg) +{ + typedef et::QuaternionInverseOp ExprT; + return et::QuaternionXpr(ExprT(arg.expression())); +} + +/* NOTE: Quaternion division no longer supported, but I'm leaving the + code here for reference (Jesse) */ + +#if 0 +/** Declare div taking two quaternion operands. */ +template +inline typename et::QuaternionPromote< + quaternion, quaternion +>::temporary_type +operator/( + const quaternion& left, + const quaternion& right) +{ + return left*inverse(right); +} + +/** Declare div taking a quaternion and a et::QuaternionXpr. */ +template +inline typename et::QuaternionPromote< + quaternion, typename XprT::result_type +>::temporary_type +operator/( + const quaternion& left, + QUATXPR_ARG_TYPE right) +{ + return left*inverse(right); +} + +/** Declare div taking an et::QuaternionXpr and a quaternion. */ +template +inline typename et::QuaternionPromote< + typename XprT::result_type, quaternion +>::temporary_type +operator/( + QUATXPR_ARG_TYPE left, + const quaternion& right) +{ + return left*inverse(right); +} + +/** Declare div taking two et::QuaternionXpr operands. */ +template +inline typename et::QuaternionPromote< + typename XprT1::result_type, typename XprT2::result_type +>::temporary_type +operator/( + QUATXPR_ARG_TYPE_N(1) left, + QUATXPR_ARG_TYPE_N(2) right) +{ + return left*inverse(right); +} +#endif + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/quaternion/quaternion.h b/src/cml/quaternion/quaternion.h new file mode 100644 index 0000000..771f34a --- /dev/null +++ b/src/cml/quaternion/quaternion.h @@ -0,0 +1,519 @@ +/* -*- 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 + * + * @todo Return a VectorXpr adaptor from the imaginary() method of + * quaternion and the expression node types. + * + * @todo swap multiplication order based upon template param + * + * @todo change element order based upon template param + */ + +#ifndef quaternion_h +#define quaternion_h + +#include +#include +#include +#include + +/* This is used below to create a more meaningful compile-time error when + * the quaternion class is not created with a fixed-size 4-vector: + */ +struct quaternion_requires_fixed_size_array_type_error; + +namespace cml { + +/** A configurable quaternion type. + * + * @note Quaternions with two different orders cannot be used in the same + * expression. + */ +template< + typename Element, + class ArrayType, + class Order, + class Cross +> +class quaternion +{ + /* The ArrayType must be fixed<> or external<>: */ + CML_STATIC_REQUIRE_M( + (same_type< ArrayType, fixed<> >::is_true + || same_type< ArrayType, external<> >::is_true), + quaternion_requires_fixed_size_array_type_error); + + public: + + /* Shorthand for the array type generator: */ + typedef ArrayType storage_type; + typedef typename ArrayType::template rebind<4>::other generator_type; + + /* Vector representing the quaternion. Use the rebinding template to + * set the vector size: + */ + typedef vector vector_type; + + /* Vector temporary type: */ + typedef typename vector_type::temporary_type vector_temporary; + + /* Quaternion order: */ + typedef Order order_type; + + /* Quaternion multiplication order: */ + typedef Cross cross_type; + + /* Scalar type representing the scalar part: */ + typedef typename vector_type::value_type value_type; + typedef typename vector_type::reference reference; + typedef typename vector_type::const_reference const_reference; + /* XXX Need to verify that this is a true scalar type. */ + + /* The quaternion type: */ + typedef quaternion + quaternion_type; + + /* For integration into the expression template code: */ + typedef quaternion_type expr_type; + + /* For integration into the expression template code: */ + typedef quaternion< + Element, typename vector_temporary::storage_type, + order_type, cross_type> temporary_type; + + /* For integration into the expression templates code: */ + typedef quaternion_type& expr_reference; + typedef const quaternion_type& expr_const_reference; + + /* For matching by storage type: */ + typedef typename vector_type::memory_tag memory_tag; + + /* For matching by size type: */ + typedef typename vector_type::size_tag size_tag; + + /* Get the imaginary part type: */ + typedef typename vector_temporary::subvector_type imaginary_type; + + /* For matching by result-type: */ + typedef cml::et::quaternion_result_tag result_tag; + + /* For matching by assignability: */ + typedef cml::et::assignable_tag assignable_tag; + + + public: + + /** Record result size as an enum. */ + enum { array_size = 4 }; + + /** Localize the ordering as an enum. */ + enum { + W = order_type::W, + X = order_type::X, + Y = order_type::Y, + Z = order_type::Z + }; + + + public: + + /** Return the scalar part. */ + value_type real() const { return m_q[W]; } + + /** Return the imaginary vector. */ + imaginary_type imaginary() const { + /* + imaginary_type v; + v[0] = m_q[X]; v[1] = m_q[Y]; v[2] = m_q[Z]; + return v; + */ + return imaginary_type(m_q[X], m_q[Y], m_q[Z]); + } + + /** Return the vector representing the quaternion. */ + const vector_type& as_vector() const { + return m_q; + } + + /** Return the Cayley norm of the quaternion. */ + value_type norm() const { + return length_squared(); + } + + /** Return square of the quaternion length. */ + value_type length_squared() const { + return cml::dot(*this,*this); + } + + /** Return the quaternion length. */ + value_type length() const { + return std::sqrt(length_squared()); + } + + /** Normalize this quaternion (divide by its length). + * + * @todo Make this return a QuaternionXpr. + */ + quaternion_type& normalize() { + return (*this /= length()); + } + + /** Set this quaternion to the conjugate. */ + quaternion_type& conjugate() { + return (*this) = cml::conjugate(*this); + } + + /** Set this quaternion to the inverse. */ + quaternion_type& inverse() { + return (*this) = cml::inverse(*this); + } + + /** Set this quaternion to the multiplicative identity. */ + quaternion_type& identity() { + m_q[W] = value_type(1); + m_q[X] = value_type(0); + m_q[Y] = value_type(0); + m_q[Z] = value_type(0); + return *this; + } + + /** Return the log of this quaternion. */ + temporary_type log( + value_type tolerance = epsilon::placeholder()) const + { + value_type a = acos_safe(real()); + value_type s = std::sin(a); + + if (s > tolerance) { + return temporary_type(value_type(0), imaginary() * (a / s)); + } else { + return temporary_type(value_type(0), imaginary()); + } + } + + /** + * Return the result of the exponential function as applied to + * this quaternion. + */ + temporary_type exp( + value_type tolerance = epsilon::placeholder()) const + { + imaginary_type v = imaginary(); + value_type a = cml::length(v); + + if (a > tolerance) { + return temporary_type(std::cos(a), v * (std::sin(a) / a)); + } else { + return temporary_type(std::cos(a), v); + } + } + + + /** Const access to the quaternion as a vector. */ + const_reference operator[](size_t i) const { return m_q[i]; } + + /** Mutable access to the quaternion as a vector. */ + reference operator[](size_t i) { return m_q[i]; } + + /** Fill quaternion with random elements. + * + * @warning This does not generate uniformly random rotations. + */ + void random(value_type min, value_type max) { + for (size_t i = 0; i < 4; ++i) { + m_q[i] = random_real(min,max); + } + } + + public: + + /** Default initializer. + * + * @note The default constructor cannot be used with an external<> + * array type. + */ + quaternion() {} + + /** Initializer for an external<> vector type. */ + quaternion(Element* const array) : m_q(array) {} + + /** Copy construct from the same type of quaternion. */ + quaternion(const quaternion_type& q) : m_q(q.m_q) {} + + /** Construct from a quaternion having a different array type. */ + template quaternion( + const quaternion& q) + : m_q(q.as_vector()) {} + + /** Copy construct from a QuaternionXpr. */ + template quaternion(QUATXPR_ARG_TYPE e) { + typedef typename XprT::order_type arg_order; + m_q[W] = e[arg_order::W]; + m_q[X] = e[arg_order::X]; + m_q[Y] = e[arg_order::Y]; + m_q[Z] = e[arg_order::Z]; + } + + + + /** Initialize from a 4-vector. + * + * If Order is scalar_first, then v[0] is the real part. Otherwise, + * v[3] is the real part. + */ + quaternion(const vector_type& v) : m_q(v) {} + + /** Initialize from an array of scalars. + * + * If Order is scalar_first, then v[0] is the real part. Otherwise, + * v[3] is the real part. + * + * @note The target vector must have CML_VEC_COPY_FROM_ARRAY + * implemented, so this cannot be used with external<> vectors. + */ + quaternion(const value_type v[4]) : m_q(v) {} + + /** Initialize from 4 scalars. + * + * If Order is scalar_first, then a is the real part, and (b,c,d) is + * the imaginary part. Otherwise, (a,b,c) is the imaginary part, and d + * is the real part. + */ + quaternion( + const value_type& a, const value_type& b, + const value_type& c, const value_type& d) + { + /* Call the overloaded assignment function: */ + assign(a, b, c, d, Order()); + } + + /** Initialize both the real and imaginary parts. + * + * The imaginary part is given by a 3-vector. Although the imaginary + * part is specified first, the proper coefficient order (vector or + * scalar first) is maintained. + */ + quaternion(const value_type& s, const imaginary_type& v) { + m_q[W] = s; m_q[X] = v[0]; m_q[Y] = v[1]; m_q[Z] = v[2]; + } + + /** Initialize both the real and imaginary parts. + * + * The imaginary part is given by a 3-vector. Although the imaginary + * part is specified second, the proper coefficient order (vector or + * scalar first) is maintained. + */ + quaternion(const imaginary_type& v, const value_type& s) { + m_q[W] = s; m_q[X] = v[0]; m_q[Y] = v[1]; m_q[Z] = v[2]; + } + + /** Initialize both the real and imaginary parts. + * + * The imaginary part is given by an array of scalars. Although the + * imaginary part is specified first, the proper coefficient order + * (vector or scalar first) is maintained. + */ + quaternion(const value_type v[3], const value_type& s) { + m_q[W] = s; m_q[X] = v[0]; m_q[Y] = v[1]; m_q[Z] = v[2]; + } + + /** Initialize both the real and imaginary parts. + * + * The imaginary part is given by an array of scalars. Although the + * imaginary part is specified second, the proper coefficient order + * (vector or scalar first) is maintained. + */ + quaternion(const value_type& s, const value_type v[3]) { + m_q[W] = s; m_q[X] = v[0]; m_q[Y] = v[1]; m_q[Z] = v[2]; + } + + + + /** Initialize from a VectorXpr. */ + template + quaternion(VECXPR_ARG_TYPE e) : m_q(e) {} + + /** Initialize both the real and imaginary parts. + * + * The imaginary part is initialized with a VectorXpr. + */ + template + quaternion(const value_type& s, VECXPR_ARG_TYPE e) { + m_q[W] = s; m_q[X] = e[0]; m_q[Y] = e[1]; m_q[Z] = e[2]; + } + + // @todo: Are we missing: + + // quaternion(VECXPR_ARG_TYPE e, const value_type& s) {} + + // Or is that covered elsewhere? + + /** In-place op from a quaternion. + * + * This assumes that _op_ is defined for both the quaternion's vector + * type and its scalar type. + */ +#define CML_QUAT_ASSIGN_FROM_QUAT(_op_) \ + template const quaternion_type& \ + operator _op_ (const quaternion& q) { \ + m_q[W] _op_ q[W]; \ + m_q[X] _op_ q[X]; \ + m_q[Y] _op_ q[Y]; \ + m_q[Z] _op_ q[Z]; \ + return *this; \ + } + + /** In-place op from a QuaternionXpr. + * + * This assumes that _op_ is defined for the quaternion's scalar type. + */ +#define CML_QUAT_ASSIGN_FROM_QUATXPR(_op_) \ + template quaternion_type& \ + operator _op_ (QUATXPR_ARG_TYPE e) { \ + typedef typename XprT::order_type arg_order; \ + m_q[W] _op_ e[arg_order::W]; \ + m_q[X] _op_ e[arg_order::X]; \ + m_q[Y] _op_ e[arg_order::Y]; \ + m_q[Z] _op_ e[arg_order::Z]; \ + return *this; \ + } + + /** In-place op from a scalar type. + * + * This assumes that _op_ is defined for the quaternion's scalar type. + */ +#define CML_QUAT_ASSIGN_FROM_SCALAR(_op_,_op_name_) \ + quaternion_type& operator _op_ (const value_type& s) { \ + typedef _op_name_ OpT; \ + OpT().apply(m_q[W],s); \ + OpT().apply(m_q[X],s); \ + OpT().apply(m_q[Y],s); \ + OpT().apply(m_q[Z],s); \ + return *this; \ + } + + CML_QUAT_ASSIGN_FROM_QUAT(=) + CML_QUAT_ASSIGN_FROM_QUAT(+=) + CML_QUAT_ASSIGN_FROM_QUAT(-=) + + CML_QUAT_ASSIGN_FROM_QUATXPR(=) + CML_QUAT_ASSIGN_FROM_QUATXPR(+=) + CML_QUAT_ASSIGN_FROM_QUATXPR(-=) + + CML_QUAT_ASSIGN_FROM_SCALAR(*=, cml::et::OpMulAssign) + CML_QUAT_ASSIGN_FROM_SCALAR(/=, cml::et::OpDivAssign) + +#undef CML_QUAT_ASSIGN_FROM_QUAT +#undef CML_QUAT_ASSIGN_FROM_QUATXPR +#undef CML_QUAT_ASSIGN_FROM_SCALAR + + /** Accumulated multiplication with a quaternion. + * + * Compute p = p * q for two quaternions p and q. + * + * @internal Using operator* here is okay, as long as cml/quaternion.h + * is included before using this method (the only supported case for + * end-user code). This is because modern compilers won't instantiate a + * method in a template class until it is used, and including the main + * header ensures all definitions are available before any possible use + * of this method. + */ + quaternion_type& operator*=(const quaternion_type& q) { + return (*this = *this * q); + } + + /** Accumulated multiplication with a quaternion expression. + * + * Compute p = p * e for a quaternion p and a quaternion expression e. + * + * @internal Using operator* here is okay, as long as cml/quaternion.h + * is included before using this method (the only supported case for + * end-user code). This is because modern compilers won't instantiate a + * method in a template class until it is used, and including the main + * header ensures all definitions are available before any possible use + * of this method. + */ + template quaternion_type& operator*=(QUATXPR_ARG_TYPE e) { + return (*this = *this * e); + } + + /* NOTE: Quaternion division no longer supported, but I'm leaving the + code here for reference (Jesse) */ + + #if 0 + /** Accumulated division with a quaternion. + * + * Compute p = p * inverse(q). + * + * @note Because quaternion multiplication is non-commutative, division + * is ambiguous. This method assumes a multiplication order consistent + * with the notational order; i.e. p = q / r means p = q*inverse(r). + * + * @internal Using operator* and cml::inverse here is okay, as long as + * cml/quaternion.h is included before using this method (the only + * supported case for end-user code). This is because modern compilers + * won't instantiate a method in a template class until it is used, and + * including the main header ensures all definitions are available + * before any possible use of this method. + */ + quaternion_type& operator/=(const quaternion_type& q) { + return (*this = *this * cml::inverse(q)); + } + + /** Accumulated division with a quaternion expression. + * + * Compute p = p * inverse(q). + * + * @note Because quaternion multiplication is non-commutative, division + * is ambiguous. This method assumes a multiplication order consistent + * with the notational order; i.e. p = q / r means p = q*inverse(r). + * + * @internal Using operator* and cml::inverse here is okay, as long as + * cml/quaternion.h is included before using this method (the only + * supported case for end-user code). This is because modern compilers + * won't instantiate a method in a template class until it is used, and + * including the main header ensures all definitions are available + * before any possible use of this method. + */ + template quaternion_type& operator/=(QUATXPR_ARG_TYPE e) { + return (*this = *this * cml::inverse(e)); + } + #endif + + + protected: + + /** Overloaded function to assign the quaternion from 4 scalars. */ + void assign(const value_type& a, const value_type& b, + const value_type& c, const value_type& d, scalar_first) + { + m_q[W] = a; m_q[X] = b; m_q[Y] = c; m_q[Z] = d; + } + + /** Overloaded function to assign the quaternion from 4 scalars. */ + void assign(const value_type& a, const value_type& b, + const value_type& c, const value_type& d, vector_first) + { + m_q[X] = a; m_q[Y] = b; m_q[Z] = c; m_q[W] = d; + } + + + protected: + + vector_type m_q; +}; + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/quaternion/quaternion_comparison.h b/src/cml/quaternion/quaternion_comparison.h new file mode 100644 index 0000000..c1e6d0c --- /dev/null +++ b/src/cml/quaternion/quaternion_comparison.h @@ -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 + */ + +#ifndef quaternion_comparison_h +#define quaternion_comparison_h + +#include +#include + +/* This is used below to create a more meaningful compile-time error when + * quaternion_comparison is not provided with quaternion or QuaternionExpr arguments: + */ +struct quaternion_comparison_expects_quaternion_args_error; + +#define CML_QUAT_QUAT_ORDER(_order_, _op_, _OpT_) \ +template< \ + typename E1, class AT1, typename E2, class AT2, class O, class C > \ +inline bool \ +_op_ ( \ + const quaternion& left, \ + const quaternion& right) \ +{ \ + return detail::quaternion_##_order_ (left, right, _OpT_ ()); \ +} + +#define CML_QUAT_QUATXPR_ORDER(_order_, _op_, _OpT_) \ +template \ +inline bool \ +_op_ ( \ + const quaternion& left, \ + QUATXPR_ARG_TYPE right) \ +{ \ + return detail::quaternion_##_order_ (left, right, \ + _OpT_ ()); \ +} + +#define CML_QUATXPR_QUAT_ORDER(_order_, _op_, _OpT_) \ +template \ +inline bool \ +_op_ ( \ + QUATXPR_ARG_TYPE left, \ + const quaternion& right) \ +{ \ + return detail::quaternion_##_order_ (left, right, \ + _OpT_ ()); \ +} + +#define CML_QUATXPR_QUATXPR_ORDER(_order_, _op_, _OpT_) \ +template \ +inline bool \ +_op_ ( \ + QUATXPR_ARG_TYPE_N(1) left, \ + QUATXPR_ARG_TYPE_N(2) right) \ +{ \ + return detail::quaternion_##_order_ (left, right, \ + _OpT_ < \ + typename XprT1::value_type, \ + typename XprT2::value_type>()); \ +} + + +namespace cml { +namespace detail { + +/** Quaternion strict weak ordering relationship. + * + * OpT must implement a strict weak order on the quaternion element type. + * operator< and operator> on integer and floating-point types are + * examples. + */ +template +inline bool +quaternion_weak_order(const LeftT& left, const RightT& right, OpT) +{ + /* Shorthand: */ + typedef et::ExprTraits left_traits; + typedef et::ExprTraits right_traits; + + /* quaternion_comparison() requires quaternion expressions: */ + CML_STATIC_REQUIRE_M( + (et::QuaternionExpressions::is_true), + quaternion_comparison_expects_quaternion_args_error); + /* Note: parens are required here so that the preprocessor ignores the + * commas: + */ + + typedef typename et::QuaternionPromote< + typename left_traits::result_type, + typename right_traits::result_type + >::type result_type; + + for(ssize_t i = 0; i < result_type::array_size; ++ i) { + + if(OpT().apply( + left_traits().get(left,i), + right_traits().get(right,i) + )) + { + /* If weak order (a < b) is satisfied, return true: */ + return true; + } else if(OpT().apply( + right_traits().get(right,i), + left_traits().get(left,i) + )) + { + /* If !(b < a), then return false: */ + return false; + } else { + + /* Have !(a < b) && !(b < a) <=> (a >= b && b >= a) <=> (a == b). + * so need to test next element: + */ + continue; + } + } + /* XXX Can this be unrolled in any reasonable way? */ + + /* If we get here, then left == right: */ + return false; +} + +/** Quaternion total order relationship. + * + * OpT must implement a total order on the quaternion element type. operator<= + * and operator>= on integer and floating-point types are examples. + */ +template +inline bool +quaternion_total_order(const LeftT& left, const RightT& right, OpT) +{ + /* Shorthand: */ + typedef et::ExprTraits left_traits; + typedef et::ExprTraits right_traits; + + /* quaternion_comparison() requires quaternion expressions: */ + CML_STATIC_REQUIRE_M( + (et::QuaternionExpressions::is_true), + quaternion_comparison_expects_quaternion_args_error); + /* Note: parens are required here so that the preprocessor ignores the + * commas: + */ + + typedef typename et::QuaternionPromote< + typename left_traits::result_type, + typename right_traits::result_type + >::type result_type; + + for(ssize_t i = 0; i < result_type::array_size; ++ i) { + + /* Test total order: */ + if(OpT().apply( + left_traits().get(left,i), + right_traits().get(right,i) + )) + { + /* Automatically true if weak order (a <= b) && !(b <= a) <=> + * (a <= b) && (b > a) <=> (a < b) is satisfied: + */ + if(!OpT().apply( + right_traits().get(right,i), + left_traits().get(left,i) + )) + return true; + + /* Otherwise, have equality (a <= b) && (b <= a), so continue + * to next element: + */ + else + continue; + + } else { + + /* Total order isn't satisfied (a > b), so return false: */ + return false; + } + } + /* XXX Can this be unrolled in any reasonable way? */ + + /* Total (==) or weak (<) order was satisfied, so return true: */ + return true; +} + +} + +/* XXX There is a better way to handle these with operator traits... */ + +CML_QUAT_QUAT_ORDER( total_order, operator==, et::OpEqual) +CML_QUATXPR_QUAT_ORDER( total_order, operator==, et::OpEqual) +CML_QUAT_QUATXPR_ORDER( total_order, operator==, et::OpEqual) +CML_QUATXPR_QUATXPR_ORDER( total_order, operator==, et::OpEqual) + +CML_QUAT_QUAT_ORDER( weak_order, operator!=, et::OpNotEqual) +CML_QUATXPR_QUAT_ORDER( weak_order, operator!=, et::OpNotEqual) +CML_QUAT_QUATXPR_ORDER( weak_order, operator!=, et::OpNotEqual) +CML_QUATXPR_QUATXPR_ORDER( weak_order, operator!=, et::OpNotEqual) + +CML_QUAT_QUAT_ORDER( weak_order, operator<, et::OpLess) +CML_QUATXPR_QUAT_ORDER( weak_order, operator<, et::OpLess) +CML_QUAT_QUATXPR_ORDER( weak_order, operator<, et::OpLess) +CML_QUATXPR_QUATXPR_ORDER( weak_order, operator<, et::OpLess) + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/quaternion/quaternion_dot.h b/src/cml/quaternion/quaternion_dot.h new file mode 100644 index 0000000..72a53e2 --- /dev/null +++ b/src/cml/quaternion/quaternion_dot.h @@ -0,0 +1,73 @@ +/* -*- 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_dot_h +#define quaternion_dot_h + +#include +#include + +namespace cml { +namespace detail { + +template inline +typename detail::DotPromote::promoted_scalar +quaternion_dot(const LeftT& p, const RightT& q) +{ + return p[0]*q[0] + p[1]*q[1] + p[2]*q[2] + p[3]*q[3]; +} + +} // namespace detail + +template +inline typename detail::DotPromote< + quaternion, quaternion +>::promoted_scalar +dot(const quaternion& p, + const quaternion& q) +{ + return detail::quaternion_dot(p,q); +} + +template +inline typename detail::DotPromote< + quaternion, et::QuaternionXpr +>::promoted_scalar +dot(const quaternion& p, QUATXPR_ARG_TYPE q) +{ + return detail::quaternion_dot(p,q); +} + +template +inline typename detail::DotPromote< + et::QuaternionXpr, quaternion +>::promoted_scalar +dot(QUATXPR_ARG_TYPE p, const quaternion& q) +{ + return detail::quaternion_dot(p,q); +} + +template inline +typename detail::DotPromote< + et::QuaternionXpr, et::QuaternionXpr +>::promoted_scalar +dot(QUATXPR_ARG_TYPE_N(1) p, QUATXPR_ARG_TYPE_N(2) q) +{ + return detail::quaternion_dot(p,q); +} + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/quaternion/quaternion_expr.h b/src/cml/quaternion/quaternion_expr.h new file mode 100644 index 0000000..dcec9fd --- /dev/null +++ b/src/cml/quaternion/quaternion_expr.h @@ -0,0 +1,614 @@ +/* -*- 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_expr_h +#define quaternion_expr_h + +#include +#include +#include +#include +#include + +#define QUATXPR_ARG_TYPE const et::QuaternionXpr& +#define QUATXPR_ARG_TYPE_N(_N_) const et::QuaternionXpr& + +namespace cml { +namespace et { + +/** A placeholder for a quaternion expression in an expression tree. */ +template +class QuaternionXpr +{ + public: + + typedef QuaternionXpr expr_type; + + /* Record ary-ness of the expression: */ + typedef typename ExprT::expr_ary expr_ary; + + /* Copy the expression by value into higher-up expressions: */ + typedef expr_type expr_const_reference; + + typedef typename ExprT::value_type value_type; + typedef quaternion_result_tag result_tag; + typedef typename ExprT::size_tag size_tag; + + /* Store the expression traits: */ + typedef ExprTraits expr_traits; + + /* Get the reference type: */ + typedef typename expr_traits::const_reference expr_reference; + + /* Get the result type: */ + typedef typename expr_traits::result_type result_type; + + /* Get the vector type: */ + typedef typename result_type::vector_type vector_type; + + /* Get the imaginary part type: */ + typedef typename vector_type::subvector_type imaginary_type; + + /* For matching by assignability: */ + typedef cml::et::not_assignable_tag assignable_tag; + + /* Get the temporary type: */ + typedef typename result_type::temporary_type temporary_type; + + /* Record the order type: */ + typedef typename result_type::order_type order_type; + + /* Record the cross type: */ + typedef typename result_type::cross_type cross_type; + + + public: + + /** Record result size as an enum. */ + enum { array_size = ExprT::array_size }; + + + public: + + /** Return the real part of the expression. */ + value_type real() const { + return m_expr.real(); + } + + /** Return the vector part of the expression. */ + imaginary_type imaginary() const { + return m_expr.imaginary(); + } + + /** Return the Cayley norm of the expression. */ + value_type norm() const { + return m_expr.length_squared(); + } + + /** Return square of the quaternion length. */ + value_type length_squared() const { + return m_expr.length_squared(); + } + + /** Return the quaternion length. */ + value_type length() const { + return m_expr.length(); + } + + /** Return the result as a normalized quaternion. */ + temporary_type normalize() const { + return m_expr.normalize(); + } + + /** Return the log of the expression. */ + temporary_type log( + value_type tolerance = epsilon::placeholder()) const + { + return m_expr.log(tolerance); + } + + /** + * Return the result of the exponential function as applied to + * this expression. + */ + temporary_type exp( + value_type tolerance = epsilon::placeholder()) const + { + return m_expr.exp(tolerance); + } + + /** Compute value at index i of the result quaternion. */ + value_type operator[](size_t i) const { + return m_expr[i]; + } + + + public: + + /** Return size of this expression (same as subexpression's size). */ + size_t size() const { + return m_expr.size(); + } + + /** Return reference to contained expression. */ + expr_reference expression() const { return m_expr; } + + + public: + + /** Construct from the subexpression to store. */ + explicit QuaternionXpr(expr_reference expr) : m_expr(expr) {} + + /** Copy constructor. */ + QuaternionXpr(const expr_type& e) : m_expr(e.m_expr) {} + + + protected: + + expr_reference m_expr; + + + private: + + /* Cannot be assigned to: */ + expr_type& operator=(const expr_type&); +}; + +/** Expression traits class for QuaternionXpr<>. */ +template +struct ExprTraits< QuaternionXpr > +{ + typedef QuaternionXpr expr_type; + typedef ExprT arg_type; + typedef typename expr_type::value_type value_type; + typedef typename expr_type::expr_const_reference const_reference; + typedef typename expr_type::result_tag result_tag; + typedef typename expr_type::size_tag size_tag; + typedef typename expr_type::result_type result_type; + typedef typename expr_type::assignable_tag not_assignable_tag; + typedef expr_node_tag node_tag; + + value_type get(const expr_type& v, size_t i) const { return v[i]; } + size_t size(const expr_type& e) const { return e.size(); } +}; + + +/** A unary quaternion expression. + * + * The operator's operator() method must take exactly one argument. + */ +template +class UnaryQuaternionOp +{ + public: + + typedef UnaryQuaternionOp expr_type; + + /* Record ary-ness of the expression: */ + typedef unary_expression expr_ary; + + /* Copy the expression by value into higher-up expressions: */ + typedef expr_type expr_const_reference; + + typedef typename OpT::value_type value_type; + typedef quaternion_result_tag result_tag; + typedef typename ExprT::size_tag size_tag; + + /* Store the expression traits for the subexpression: */ + typedef ExprTraits expr_traits; + + /* Reference type for the subexpression: */ + typedef typename expr_traits::const_reference expr_reference; + + /* Get the result type (same as for subexpression): */ + typedef typename expr_traits::result_type result_type; + + /* For matching by assignability: */ + typedef cml::et::not_assignable_tag assignable_tag; + + /* Get the temporary type: */ + typedef typename result_type::temporary_type temporary_type; + + /* Get the vector type: */ + typedef typename result_type::vector_type vector_type; + + /* Get the imaginary part type: */ + typedef typename vector_type::subvector_type imaginary_type; + + /* Record the order type: */ + typedef typename result_type::order_type order_type; + + + public: + + /** Record result size as an enum. */ + enum { array_size = ExprT::array_size }; + + /** Localize the ordering as an enum. */ + enum { + W = order_type::W, + X = order_type::X, + Y = order_type::Y, + Z = order_type::Z + }; + + + public: + + /** Return the real part of the expression. */ + value_type real() const { + return (*this)[W]; + } + + /** Return the vector part of the expression. */ + imaginary_type imaginary() const { + imaginary_type v; + v[0] = (*this)[X]; v[1] = (*this)[Y]; v[2] = (*this)[Z]; + return v; + } + + /** Return the Cayley norm of the expression. */ + value_type norm() const { + return length_squared(); + } + + /** Return square of the quaternion length. */ + value_type length_squared() const { + return dot( + QuaternionXpr(*this), + QuaternionXpr(*this)); + } + + /** Return the quaternion length. */ + value_type length() const { + return std::sqrt(length_squared()); + } + + /** Return the result as a normalized quaternion. */ + temporary_type normalize() const { + temporary_type q(QuaternionXpr(*this)); + return q.normalize(); + } + + /** Return the log of this expression. */ + temporary_type log( + value_type tolerance = epsilon::placeholder()) const + { + value_type a = acos_safe(real()); + value_type s = std::sin(a); + + if (s > tolerance) { + return temporary_type(value_type(0), imaginary() * (a / s)); + } else { + return temporary_type(value_type(0), imaginary()); + } + } + + /** + * Return the result of the exponential function as applied to + * this expression. + */ + temporary_type exp( + value_type tolerance = epsilon::placeholder()) const + { + imaginary_type v = imaginary(); + value_type a = cml::length(v); + + if (a > tolerance) { + return temporary_type(std::cos(a), v * (std::sin(a) / a)); + } else { + return temporary_type(std::cos(a), v); + } + } + + /** Compute value at index i of the result quaternion. */ + value_type operator[](size_t i) const { + + /* This uses the expression traits to figure out how to access the + * i'th index of the subexpression: + */ + return OpT().apply(expr_traits().get(m_expr,i)); + } + + + public: + + /** Return size of this expression (same as argument's size). */ + size_t size() const { + return m_expr.size(); + } + + /** Return reference to contained expression. */ + expr_reference expression() const { return m_expr; } + + + public: + + /** Construct from the subexpression. */ + explicit UnaryQuaternionOp(expr_reference expr) : m_expr(expr) {} + + /** Copy constructor. */ + UnaryQuaternionOp(const expr_type& e) : m_expr(e.m_expr) {} + + + protected: + + expr_reference m_expr; + + + private: + + /* Cannot be assigned to: */ + expr_type& operator=(const expr_type&); +}; + +/** Expression traits class for UnaryQuaternionOp<>. */ +template +struct ExprTraits< UnaryQuaternionOp > +{ + typedef UnaryQuaternionOp expr_type; + typedef ExprT arg_type; + + typedef typename expr_type::value_type value_type; + typedef typename expr_type::expr_const_reference const_reference; + typedef typename expr_type::result_tag result_tag; + typedef typename expr_type::size_tag size_tag; + typedef typename expr_type::result_type result_type; + typedef typename expr_type::assignable_tag not_assignable_tag; + typedef expr_node_tag node_tag; + + value_type get(const expr_type& v, size_t i) const { return v[i]; } + size_t size(const expr_type& e) const { return e.size(); } +}; + + +/** A binary quaternion expression. + * + * The operator's operator() method must take exactly two arguments. + */ +template +class BinaryQuaternionOp +{ + public: + + typedef BinaryQuaternionOp expr_type; + + /* Record ary-ness of the expression: */ + typedef binary_expression expr_ary; + + /* Copy the expression by value into higher-up expressions: */ + typedef expr_type expr_const_reference; + + typedef typename OpT::value_type value_type; + typedef quaternion_result_tag result_tag; + + /* Store the expression traits types for the two subexpressions: */ + typedef ExprTraits left_traits; + typedef ExprTraits right_traits; + + /* Reference types for the two subexpressions: */ + typedef typename left_traits::const_reference left_reference; + typedef typename right_traits::const_reference right_reference; + + /* Figure out the expression's resulting (quaternion) type: */ + typedef typename left_traits::result_type left_result; + typedef typename right_traits::result_type right_result; + typedef typename QuaternionPromote::type + result_type; + typedef typename result_type::size_tag size_tag; + + /* For matching by assignability: */ + typedef cml::et::not_assignable_tag assignable_tag; + + /* Get the temporary type: */ + typedef typename result_type::temporary_type temporary_type; + + /* Get the vector type: */ + typedef typename result_type::vector_type vector_type; + + /* Get the imaginary part type: */ + typedef typename vector_type::subvector_type imaginary_type; + + /* Record the order type: */ + typedef typename result_type::order_type order_type; + + /* Define a size checker: */ + typedef GetCheckedSize checked_size; + + + public: + + /** Record result size as an enum. */ + enum { array_size = 4 }; + + /** Localize the ordering as an enum. */ + enum { + W = order_type::W, + X = order_type::X, + Y = order_type::Y, + Z = order_type::Z + }; + + + public: + + /** Return the real part of the expression. */ + value_type real() const { + return (*this)[W]; + } + + /** Return the vector part of the expression. */ + imaginary_type imaginary() const { + imaginary_type v; + v[0] = (*this)[X]; v[1] = (*this)[Y]; v[2] = (*this)[Z]; + return v; + } + + /** Return the Cayley norm of the expression. */ + value_type norm() const { + return length_squared(); + } + + /** Return square of the quaternion length. */ + value_type length_squared() const { + return dot( + QuaternionXpr(*this), + QuaternionXpr(*this)); + } + + /** Return the quaternion length. */ + value_type length() const { + return std::sqrt(length_squared()); + } + + /** Return the result as a normalized quaternion. */ + temporary_type normalize() const { + temporary_type q(QuaternionXpr(*this)); + return q.normalize(); + } + + /** Return the log of this expression. */ + temporary_type log( + value_type tolerance = epsilon::placeholder()) const + { + value_type a = acos_safe(real()); + value_type s = std::sin(a); + + if (s > tolerance) { + return temporary_type(value_type(0), imaginary() * (a / s)); + } else { + return temporary_type(value_type(0), imaginary()); + } + } + + /** + * Return the result of the exponential function as applied to + * this expression. + */ + temporary_type exp( + value_type tolerance = epsilon::placeholder()) const + { + imaginary_type v = imaginary(); + value_type a = cml::length(v); + + if (a > tolerance) { + return temporary_type(std::cos(a), v * (std::sin(a) / a)); + } else { + return temporary_type(std::cos(a), v); + } + } + + /** Compute value at index i of the result quaternion. */ + value_type operator[](size_t i) const { + + /* This uses the expression traits to figure out how to access the + * i'th index of the two subexpressions: + */ + return OpT().apply( + left_traits().get(m_left,i), + right_traits().get(m_right,i)); + } + + + public: + + /** Return the size of the quaternion result. + * + * @throws std::invalid_argument if the expressions do not have the same + * size. + */ + size_t size() const { + /* Note: This actually does a check only if + * CML_CHECK_VECTOR_EXPR_SIZES is set: + */ + CheckedSize(m_left,m_right,size_tag()); + + /* The size is always 4: */ + return 4; + } + + /** Return reference to left expression. */ + left_reference left_expression() const { return m_left; } + + /** Return reference to right expression. */ + right_reference right_expression() const { return m_right; } + + + public: + + /** Construct from the two subexpressions. */ + explicit BinaryQuaternionOp(left_reference left, right_reference right) + : m_left(left), m_right(right) {} + + /** Copy constructor. */ + BinaryQuaternionOp(const expr_type& e) + : m_left(e.m_left), m_right(e.m_right) {} + + + protected: + + left_reference m_left; + right_reference m_right; + + + private: + + /* This ensures that a compile-time size check is executed: */ + typename checked_size::check_type _dummy; + + + private: + + /* Cannot be assigned to: */ + expr_type& operator=(const expr_type&); +}; + +/** Expression traits class for BinaryQuaternionOp<>. */ +template +struct ExprTraits< BinaryQuaternionOp > +{ + typedef BinaryQuaternionOp expr_type; + typedef LeftT left_type; + typedef RightT right_type; + + typedef typename expr_type::value_type value_type; + typedef typename expr_type::expr_const_reference const_reference; + typedef typename expr_type::result_tag result_tag; + typedef typename expr_type::size_tag size_tag; + typedef typename expr_type::result_type result_type; + typedef typename expr_type::imaginary_type imaginary_type; + typedef typename expr_type::assignable_tag not_assignable_tag; + typedef expr_node_tag node_tag; + + value_type get(const expr_type& v, size_t i) const { return v[i]; } + size_t size(const expr_type& e) const { return e.size(); } +}; + + +/* Helper struct to verify that both arguments are quaternion expressions: */ +template +struct QuaternionExpressions +{ + /* Require that both arguments are quaternion expressions: */ + typedef typename LeftTraits::result_tag left_result; + typedef typename RightTraits::result_tag right_result; + enum { is_true = (same_type::is_true + && same_type::is_true) }; +}; + +} // namespace et +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/quaternion/quaternion_functions.h b/src/cml/quaternion/quaternion_functions.h new file mode 100644 index 0000000..95abcb9 --- /dev/null +++ b/src/cml/quaternion/quaternion_functions.h @@ -0,0 +1,172 @@ +/* -*- 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 Functions on quaternions. + * + * @todo The functions that return quaternions and vectors should be changed + * to return quaternion expression nodes, as should the corresponding + * class methods. + */ + +#ifndef quaternion_functions_h +#define quaternion_functions_h + +#include // For CheckQuat() +#include +#include // For acos_safe() + +namespace cml { + +/** Returns the real part of the quaternion. */ +template +inline typename quaternion::value_type +real(const quaternion& q) +{ + return q.real(); +} + +/** Returns the real (scalar) part of the QuaternionXpr. */ +template +inline typename et::QuaternionXpr::value_type +real(const et::QuaternionXpr& e) +{ + return e.real(); +} + +/** Returns the imaginary (vector) part of the quaternion. */ +template +inline typename quaternion::imaginary_type +imaginary(const quaternion& q) +{ + return q.imaginary(); +} + +/** Returns the imaginary (vector) part of the QuaternionXpr. */ +template +//inline typename et::QuaternionXpr::temporary_type +inline typename et::QuaternionXpr::imaginary_type +imaginary(const et::QuaternionXpr& e) +{ + return e.imaginary(); +} + +/** Cayley norm of a quaternion. */ +template +inline typename quaternion::value_type +norm(const quaternion& arg) +{ + return arg.length_squared(); +} + +/** Cayley norm of a QuaternionXpr. */ +template +inline typename XprT::value_type +norm(QUATXPR_ARG_TYPE arg) +{ + return arg.length_squared(); +} + +/** Squared length of a quaternion. */ +template +inline typename quaternion::value_type +length_squared(const quaternion& arg) +{ + return arg.length_squared(); +} + +/** Squared length of a quaternion expr. */ +template +inline typename XprT::value_type +length_squared(QUATXPR_ARG_TYPE arg) +{ + return arg.length_squared(); +} + +/** Length of a quaternion. */ +template +inline typename quaternion::value_type +length(const quaternion& arg) +{ + return arg.length(); +} + +/** Length of a quaternion expr. */ +template +inline typename XprT::value_type +length(QUATXPR_ARG_TYPE arg) +{ + return arg.length(); +} + +/** Normalize a quaternion. + * + * The input quaternion is not changed. + */ +template +inline quaternion +normalize(const quaternion& arg) +{ + typename quaternion::temporary_type result(arg); + result.normalize(); + return result; +} + +/** Normalize a quaternion expr. */ +template +inline typename XprT::temporary_type +normalize(QUATXPR_ARG_TYPE arg) +{ + return arg.normalize(); +} + +/** Set a quaternion to the multiplicative identity. + * + * The input quaternion is not changed. + */ +template +inline quaternion +identity(const quaternion& arg) +{ + typename quaternion::temporary_type result(arg); + result.identity(); + return result; +} + +/** Log of a quaternion or quaternion expression. + */ +template < class QuatT > +typename QuatT::temporary_type log( + const QuatT& q, + typename QuatT::value_type tolerance = + epsilon::placeholder()) +{ + detail::CheckQuat(q); + + return q.log(); +} + +/** Exponential function of a quaternion or quaternion expression. + */ +template < class QuatT > +typename QuatT::temporary_type exp( + const QuatT& q, + typename QuatT::value_type tolerance = + epsilon::placeholder()) +{ + detail::CheckQuat(q); + + return q.exp(); +} + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/quaternion/quaternion_mul.h b/src/cml/quaternion/quaternion_mul.h new file mode 100644 index 0000000..bfb533b --- /dev/null +++ b/src/cml/quaternion/quaternion_mul.h @@ -0,0 +1,140 @@ +/* -*- 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 Multiplication of two quaternions, p*q. + * + * This uses the expression tree, since the result is closed-form and can be + * computed by index. + */ + +#ifndef quaternion_mul_h +#define quaternion_mul_h + +#include +#include + +namespace cml { +namespace detail { + +template < class CrossType, class Real > struct SumOp; + +template < class Real > struct SumOp< positive_cross, Real > { + Real operator()(Real a, Real b) const { + return a + b; + } +}; + +template < class Real > struct SumOp< negative_cross, Real > { + Real operator()(Real a, Real b) const { + return a - b; + } +}; + +template < class Quat1_T, class Quat2_T > +typename et::QuaternionPromote< + typename Quat1_T::temporary_type, typename Quat2_T::temporary_type +>::temporary_type +QuaternionMult(const Quat1_T& q1, const Quat2_T& q2) +{ + detail::CheckQuat(q1); + detail::CheckQuat(q2); + + typedef typename et::QuaternionPromote< + typename Quat1_T::temporary_type, typename Quat2_T::temporary_type + >::temporary_type temporary_type; + + typedef typename temporary_type::value_type value_type; + typedef typename temporary_type::order_type order_type; + typedef typename temporary_type::cross_type cross_type; + + typedef detail::SumOp sum_op; + + enum { + W = order_type::W, + X = order_type::X, + Y = order_type::Y, + Z = order_type::Z + }; + + temporary_type result; + + /* s1*s2-dot(v1,v2): */ + result[W] = + q1[W]*q2[W] - q1[X]*q2[X] - q1[Y]*q2[Y] - q1[Z]*q2[Z]; + + /* (s1*v2 + s2*v1 + v1^v2) i: */ + result[X] = + sum_op()(q1[W]*q2[X] + q2[W]*q1[X], q1[Y]*q2[Z] - q1[Z]*q2[Y]); + + /* (s1*v2 + s2*v1 + v1^v2) j: */ + result[Y] = + sum_op()(q1[W]*q2[Y] + q2[W]*q1[Y], q1[Z]*q2[X] - q1[X]*q2[Z]); + + /* (s1*v2 + s2*v1 + v1^v2) k: */ + result[Z] = + sum_op()(q1[W]*q2[Z] + q2[W]*q1[Z], q1[X]*q2[Y] - q1[Y]*q2[X]); + + return result; +} + +} // namespace detail + +/** Declare mul taking two quaternion operands. */ +template +inline typename et::QuaternionPromote< + typename quaternion::temporary_type, + typename quaternion::temporary_type +>::temporary_type operator*( + const quaternion& left, + const quaternion& right) +{ + return detail::QuaternionMult(left, right); +} + +/** Declare mul taking a quaternion and a et::QuaternionXpr. */ +template +inline typename et::QuaternionPromote< + typename quaternion::temporary_type, + typename XprT::temporary_type +>::temporary_type operator*( + const quaternion& left, + QUATXPR_ARG_TYPE right) +{ + return detail::QuaternionMult(left, right); +} + +/** Declare mul taking an et::QuaternionXpr and a quaternion. */ +template +inline typename et::QuaternionPromote< + typename XprT::temporary_type, + typename quaternion::temporary_type +>::temporary_type operator*( + QUATXPR_ARG_TYPE left, + const quaternion& right) +{ + return detail::QuaternionMult(left, right); +} + +/** Declare mul taking two et::QuaternionXpr operands. */ +template +inline typename et::QuaternionPromote< + typename XprT1::temporary_type, typename XprT2::temporary_type +>::temporary_type operator*( + QUATXPR_ARG_TYPE_N(1) left, + QUATXPR_ARG_TYPE_N(2) right) +{ + return detail::QuaternionMult(left, right); +} + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/quaternion/quaternion_ops.h b/src/cml/quaternion/quaternion_ops.h new file mode 100644 index 0000000..e973f9b --- /dev/null +++ b/src/cml/quaternion/quaternion_ops.h @@ -0,0 +1,51 @@ +/* -*- 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 linear quaternion ops. + */ + +#ifndef quaternion_ops_h +#define quaternion_ops_h + +#include +#include +#include + +namespace cml { + +CML_QUAT_UNIOP( operator+, et::OpPos) +CML_QUATXPR_UNIOP( operator+, et::OpPos) + +CML_QUAT_UNIOP( operator-, et::OpNeg) +CML_QUATXPR_UNIOP( operator-, et::OpNeg) + +CML_QUAT_QUAT_BINOP( operator+, et::OpAdd) +CML_QUATXPR_QUAT_BINOP( operator+, et::OpAdd) +CML_QUAT_QUATXPR_BINOP( operator+, et::OpAdd) +CML_QUATXPR_QUATXPR_BINOP( operator+, et::OpAdd) + +CML_QUAT_QUAT_BINOP( operator-, et::OpSub) +CML_QUATXPR_QUAT_BINOP( operator-, et::OpSub) +CML_QUAT_QUATXPR_BINOP( operator-, et::OpSub) +CML_QUATXPR_QUATXPR_BINOP( operator-, et::OpSub) + +CML_QUAT_SCALAR_BINOP( operator*, et::OpMul) +CML_SCALAR_QUAT_BINOP( operator*, et::OpMul) +CML_QUATXPR_SCALAR_BINOP( operator*, et::OpMul) +CML_SCALAR_QUATXPR_BINOP( operator*, et::OpMul) + +CML_QUAT_SCALAR_BINOP( operator/, et::OpDiv) +CML_QUATXPR_SCALAR_BINOP( operator/, et::OpDiv) + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/quaternion/quaternion_print.h b/src/cml/quaternion/quaternion_print.h new file mode 100644 index 0000000..dd73d3c --- /dev/null +++ b/src/cml/quaternion/quaternion_print.h @@ -0,0 +1,80 @@ +/* -*- 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_print_h +#define quaternion_print_h + +#include + +namespace cml { + +/* NOTE: Made 'plain' quaternion output the default (Jesse) */ + +/* #if !defined(CML_PLAIN_QUATERNION_OUTPUT) */ +#if defined(CML_COMPLEX_QUATERNION_OUTPUT) + +template std::ostream& +operator<<(std::ostream& os, const cml::quaternion& q) +{ + os << ((q[0] < 0)?" - ":"") << std::fabs(q[0]); + os << ((q[1] < 0)?" - ":" + ") << std::fabs(q[1]) << "i"; + os << ((q[2] < 0)?" - ":" + ") << std::fabs(q[2]) << "j"; + os << ((q[3] < 0)?" - ":" + ") << std::fabs(q[3]) << "k"; + return os; +} + +template std::ostream& +operator<<(std::ostream& os, const cml::quaternion& q) +{ + os << ((q[0] < 0)?" - ":"") << std::fabs(q[0]) << "i"; + os << ((q[1] < 0)?" - ":" + ") << std::fabs(q[1]) << "j"; + os << ((q[2] < 0)?" - ":" + ") << std::fabs(q[2]) << "k"; + os << ((q[3] < 0)?" - ":" + ") << std::fabs(q[3]); + return os; +} + +#else + +/** Output a quaternion to a std::ostream. */ +template std::ostream& +operator<<(std::ostream& os, const cml::quaternion& q) +{ + os << "["; + for (size_t i = 0; i < 4; ++i) { + os << " " << q[i]; + } + os << " ]"; + return os; +} + +#endif + +/** Output a quaternion expression to a std::ostream. */ +template< class XprT > inline std::ostream& +operator<<(std::ostream& os, const et::QuaternionXpr& q) +{ + typedef typename et::QuaternionXpr::result_type quaternion_type; + + os << quaternion_type(q); + /* XXX This temporary can be removed by templating the stream insertion + * operators above. + */ + + return os; +} + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/quaternion/quaternion_promotions.h b/src/cml/quaternion/quaternion_promotions.h new file mode 100644 index 0000000..049a1b3 --- /dev/null +++ b/src/cml/quaternion/quaternion_promotions.h @@ -0,0 +1,140 @@ +/* -*- 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_promotions_h +#define quaternion_promotions_h + +#include +#include + +namespace cml { +namespace et { + +/* Default quaternion type promotion templates. */ +template struct QuaternionPromote; + +/** Type promotion for two quaternion types. */ +template +struct QuaternionPromote< + cml::quaternion, + cml::quaternion +> +{ + /* The deduced vector type: */ + typedef typename VectorPromote< + typename cml::quaternion::vector_type, + typename cml::quaternion::vector_type + >::type promoted_vector; + + /* The deduced element and storage types: */ + typedef typename promoted_vector::value_type value_type; + typedef typename promoted_vector::storage_type storage_type; + + /* The deduced quaternion result type: */ + typedef cml::quaternion type; + + /* The temporary type: */ + typedef typename type::temporary_type temporary_type; +}; + +/** + * NOTE: QuaternionPromote* are somewhat ad hoc, and were added to + * simplify the code for quaternion slerp/squad/etc. + */ + +/** Type promotion for two quaternion types. */ +template < class Quat1_T, class Quat2_T > +struct QuaternionPromote2 +{ + typedef typename QuaternionPromote< + typename Quat1_T::temporary_type, typename Quat2_T::temporary_type + >::temporary_type temporary_type; + typedef typename temporary_type::value_type value_type; +}; + +/** Type promotion for three quaternion types. */ +template < class Quat1_T, class Quat2_T, class Quat3_T > +struct QuaternionPromote3 +{ + typedef typename QuaternionPromote< + typename Quat1_T::temporary_type, + typename QuaternionPromote< + typename Quat2_T::temporary_type, typename Quat3_T::temporary_type + >::temporary_type + >::temporary_type temporary_type; + typedef typename temporary_type::value_type value_type; +}; + +/** Type promotion for four quaternion types. */ +template < class Quat1_T, class Quat2_T, class Quat3_T, class Quat4_T > +struct QuaternionPromote4 +{ + typedef typename QuaternionPromote< + typename Quat1_T::temporary_type, + typename QuaternionPromote< + typename Quat2_T::temporary_type, + typename QuaternionPromote< + typename Quat3_T::temporary_type, + typename Quat4_T::temporary_type + >::temporary_type + >::temporary_type + >::temporary_type temporary_type; + typedef typename temporary_type::value_type value_type; +}; + +/** Type promotion for a quaternion and a scalar. */ +template +struct QuaternionPromote, S> +{ + /* The deduced vector type: */ + typedef typename VectorPromote< + typename quaternion::vector_type, S + >::type promoted_vector; + + /* The deduced element and storage types: */ + typedef typename promoted_vector::value_type value_type; + typedef typename promoted_vector::storage_type storage_type; + + /* The deduced quaternion result type: */ + typedef cml::quaternion type; + + /* The temporary type: */ + typedef typename type::temporary_type temporary_type; +}; + +/** Type promotion for a scalar and a quaternion. */ +template +struct QuaternionPromote > +{ + /* The deduced vector type: */ + typedef typename VectorPromote< + S, typename quaternion::vector_type + >::type promoted_vector; + + /* The deduced element and storage types: */ + typedef typename promoted_vector::value_type value_type; + typedef typename promoted_vector::storage_type storage_type; + + /* The deduced quaternion result type: */ + typedef cml::quaternion type; + + /* The temporary type: */ + typedef typename type::temporary_type temporary_type; +}; + +} // namespace et +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/quaternion/quaternion_traits.h b/src/cml/quaternion/quaternion_traits.h new file mode 100644 index 0000000..fbf23cf --- /dev/null +++ b/src/cml/quaternion/quaternion_traits.h @@ -0,0 +1,45 @@ +/* -*- 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_traits_h +#define quaternion_traits_h + +#include + +namespace cml { +namespace et { + +/** Expression traits for a quaternion<> type. */ +template +struct ExprTraits< cml::quaternion > +{ + typedef typename cml::quaternion::expr_type expr_type; + typedef typename expr_type::value_type value_type; + typedef typename expr_type::expr_reference reference; + typedef typename expr_type::expr_const_reference const_reference; + typedef typename expr_type::result_tag result_tag; + typedef typename expr_type::size_tag size_tag; + typedef typename expr_type::assignable_tag assignable_tag; + typedef expr_type result_type; + typedef expr_leaf_tag node_tag; + + value_type get(const expr_type& v, size_t i) const { return v[i]; } + size_t size(const expr_type& v) const { return 4; } +}; + +} // namespace et +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/quaternion/quatop_macros.h b/src/cml/quaternion/quatop_macros.h new file mode 100644 index 0000000..112e348 --- /dev/null +++ b/src/cml/quaternion/quatop_macros.h @@ -0,0 +1,232 @@ +/* -*- 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 + * + * Create unary and binary operators with macros. + * + * These macros work just like those in cml/quaternion/vecop_macros.h. + */ + +#ifndef quatop_macros_h +#define quatop_macros_h + +/** Declare a unary operator taking a quaternion operand. */ +#define CML_QUAT_UNIOP(_op_, _OpT_) \ +template \ +inline et::QuaternionXpr< \ + et::UnaryQuaternionOp< quaternion, _OpT_ > \ +> \ + \ +_op_ (const quaternion& arg) \ +{ \ + typedef et::UnaryQuaternionOp< \ + quaternion, _OpT_ \ + > ExprT; \ + return et::QuaternionXpr(ExprT(arg)); \ +} + + +/** Declare a unary operator taking a et::QuaternionXpr operand. */ +#define CML_QUATXPR_UNIOP(_op_, _OpT_) \ +template \ +inline et::QuaternionXpr< \ + et::UnaryQuaternionOp< XprT, _OpT_ > \ +> \ + \ +_op_ (QUATXPR_ARG_TYPE arg) \ +{ \ + typedef et::UnaryQuaternionOp< \ + XprT, _OpT_ \ + > ExprT; \ + return et::QuaternionXpr(ExprT(arg.expression())); \ +} + + + +/** Declare an operator taking two quaternion operands. */ +#define CML_QUAT_QUAT_BINOP(_op_, _OpT_) \ +template \ +inline et::QuaternionXpr< \ + et::BinaryQuaternionOp< \ + quaternion, quaternion, \ + _OpT_ \ + > \ +> \ + \ +_op_ ( \ + const quaternion& left, \ + const quaternion& right) \ +{ \ + typedef et::BinaryQuaternionOp< \ + quaternion, quaternion, \ + _OpT_ \ + > ExprT; \ + return et::QuaternionXpr(ExprT(left,right)); \ +} + + +/** Declare an operator taking a quaternion and a et::QuaternionXpr. */ +#define CML_QUAT_QUATXPR_BINOP(_op_, _OpT_) \ +template \ +inline et::QuaternionXpr< \ + et::BinaryQuaternionOp< \ + quaternion, XprT, \ + _OpT_ \ + > \ +> \ + \ +_op_ ( \ + const quaternion& left, \ + QUATXPR_ARG_TYPE right) \ +{ \ + typedef et::BinaryQuaternionOp< \ + quaternion, XprT, \ + _OpT_ \ + > ExprT; \ + return et::QuaternionXpr(ExprT(left,right.expression())); \ +} + + +/** Declare an operator taking an et::QuaternionXpr and a quaternion. */ +#define CML_QUATXPR_QUAT_BINOP(_op_, _OpT_) \ +template \ +inline et::QuaternionXpr< \ + et::BinaryQuaternionOp< \ + XprT, quaternion, \ + _OpT_ \ + > \ +> \ + \ +_op_ ( \ + QUATXPR_ARG_TYPE left, \ + const quaternion& right) \ +{ \ + typedef et::BinaryQuaternionOp< \ + XprT, quaternion, \ + _OpT_ \ + > ExprT; \ + return et::QuaternionXpr(ExprT(left.expression(),right)); \ +} + + +/** Declare an operator taking two et::QuaternionXpr operands. */ +#define CML_QUATXPR_QUATXPR_BINOP(_op_, _OpT_) \ +template \ +inline et::QuaternionXpr< \ + et::BinaryQuaternionOp< \ + XprT1, XprT2, \ + _OpT_ < \ + typename XprT1::value_type, \ + typename XprT2::value_type \ + > \ + > \ +> \ + \ +_op_ ( \ + QUATXPR_ARG_TYPE_N(1) left, \ + QUATXPR_ARG_TYPE_N(2) right) \ +{ \ + typedef et::BinaryQuaternionOp< \ + XprT1, XprT2, \ + _OpT_ < \ + typename XprT1::value_type, \ + typename XprT2::value_type> \ + > ExprT; \ + return et::QuaternionXpr( \ + ExprT(left.expression(),right.expression())); \ +} + + +/** Declare an operator taking a quaternion and a scalar. */ +#define CML_QUAT_SCALAR_BINOP(_op_, _OpT_) \ +template \ +inline et::QuaternionXpr< \ + et::BinaryQuaternionOp< \ + quaternion, ScalarT, \ + _OpT_ \ + > \ +> \ + \ +_op_ ( \ + const quaternion& left, \ + SCALAR_ARG_TYPE right) \ +{ \ + typedef et::BinaryQuaternionOp< \ + quaternion, ScalarT, _OpT_ \ + > ExprT; \ + return et::QuaternionXpr(ExprT(left,right)); \ +} + + +/** Declare an operator taking a scalar and a quaternion. */ +#define CML_SCALAR_QUAT_BINOP(_op_, _OpT_) \ +template \ +inline et::QuaternionXpr< \ + et::BinaryQuaternionOp< \ + ScalarT, quaternion, _OpT_ \ + > \ +> \ + \ +_op_ ( \ + SCALAR_ARG_TYPE left, \ + const quaternion& right) \ +{ \ + typedef et::BinaryQuaternionOp< \ + ScalarT, quaternion, _OpT_ \ + > ExprT; \ + return et::QuaternionXpr(ExprT(left,right)); \ +} + + +/** Declare an operator taking a et::QuaternionXpr and a scalar. */ +#define CML_QUATXPR_SCALAR_BINOP(_op_, _OpT_) \ +template \ +inline et::QuaternionXpr< \ + et::BinaryQuaternionOp< \ + XprT, ScalarT, _OpT_ \ + > \ +> \ + \ +_op_ ( \ + QUATXPR_ARG_TYPE left, \ + SCALAR_ARG_TYPE right) \ +{ \ + typedef et::BinaryQuaternionOp< \ + XprT, ScalarT, _OpT_ \ + > ExprT; \ + return et::QuaternionXpr(ExprT(left.expression(),right)); \ +} + + +/** Declare an operator taking a scalar and a et::QuaternionXpr. */ +#define CML_SCALAR_QUATXPR_BINOP(_op_, _OpT_) \ +template \ +inline et::QuaternionXpr< \ + et::BinaryQuaternionOp< \ + ScalarT, XprT, _OpT_ \ + > \ +> \ + \ +_op_ ( \ + SCALAR_ARG_TYPE left, \ + QUATXPR_ARG_TYPE right) \ +{ \ + typedef et::BinaryQuaternionOp< \ + ScalarT, XprT, \ + _OpT_ \ + > ExprT; \ + return et::QuaternionXpr(ExprT(left,right.expression())); \ +} + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/util.h b/src/cml/util.h new file mode 100644 index 0000000..0fcca00 --- /dev/null +++ b/src/cml/util.h @@ -0,0 +1,326 @@ +/* -*- 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 cml_util_h +#define cml_util_h + +#include // For std::min and std::max. +#include // For std::rand. +#include + +namespace cml { + +/** Sign of input value as double. */ +template < typename T > +double sign(T value) { + return value < T(0) ? -1.0 : (value > T(0) ? 1.0 : 0.0); +} + +/** Clamp input value to the range [min, max]. */ +template < typename T > +T clamp(T value, T min, T max) { + return std::max(std::min(value, max), min); +} + +/** Test input value for inclusion in [min, max]. */ +template < typename T > +bool in_range(T value, T min, T max) { + return value >= min && value <= max; +} + +/** Map input value from [min1, max1] to [min2, max2]. */ +template < typename T > +T map_range(T value, T min1, T max1, T min2, T max2) { + return min2 + ((value - min1) / (max1 - min1)) * (max2 - min2); +} + + +/** Wrap std::acos() and clamp argument to [-1, 1]. */ +template < typename T > +T acos_safe(T theta) { + return T(std::acos(clamp(theta, T(-1.0), T(1.0)))); +} + +/** Wrap std::asin() and clamp argument to [-1, 1]. */ +template < typename T > +T asin_safe(T theta) { + return T(std::asin(clamp(theta, T(-1.0), T(1.0)))); +} + +/** Wrap std::sqrt() and clamp argument to [0, inf). */ +template < typename T > +T sqrt_safe(T value) { + return T(std::sqrt(std::max(value, T(0.0)))); +} + + +/** For convenient squaring of expressions. */ +template < typename T > +T sqr(T value) { + return value * value; +} + +/** Inverse square root. */ +template < typename T > +T inv_sqrt(T value) { + return T(1.0 / std::sqrt(value)); +} + + +/* The next few functions deal with indexing. next() and prev() are useful + * for operations involving the vertices of a polygon or other cyclic set, + * and cyclic_permutation() is used by various functions that deal with + * axes or basis vectors in a generic way. As these functions are only + * relevant for unsigned integer types, I've just used size_t, but there + * may be reasons I haven't thought of that they should be templated. + */ + +/** Return next, with cycling, in a series of N non-negative integers. */ +inline size_t next(size_t i, size_t N) { + return (i + 1) % N; +} + +/** Return previous, with cycling, in a series of N non-negative integers. */ +inline size_t prev(size_t i, size_t N) { + return i ? (i - 1) : (N - 1); +} + +/** Cyclic permutation of the set { 0, 1 }, starting with 'first'. */ +inline void cyclic_permutation(size_t first, size_t& i, size_t& j) { + i = first; + j = next(i, 2); +} + +/** Cyclic permutation of the set { 0, 1, 2 }, starting with 'first'. */ +inline void cyclic_permutation(size_t first, size_t& i, size_t& j, size_t& k) +{ + i = first; + j = next(i, 3); + k = next(j, 3); +} + +/** Cyclic permutation of the set { 0, 1, 2, 3 }, starting with 'first'. */ +inline void cyclic_permutation( + size_t first, size_t& i, size_t& j, size_t& k, size_t& l) +{ + i = first; + j = next(i, 4); + k = next(j, 4); + l = next(k, 4); +} + + +/** Convert radians to degrees. */ +template < typename T > +T deg(T theta) { + return theta * constants::deg_per_rad(); +} + +/** Convert degrees to radians. */ +template < typename T > +T rad(T theta) { + return theta * constants::rad_per_deg(); +} + +/* Note: Moving interpolation functions to interpolation.h */ + +#if 0 +/** 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 +T lerp(const T& f0, const T& f1, Scalar u) { + return (Scalar(1.0) - u) * f0 + u * f1; +} +#endif + +#if 0 +/** Bilinear interpolation of 4 values. + * + * @note The data points are assumed to be sampled at the corners of a unit + * square, so for interpolation u and v must lie between 0 and 1, + */ +template +T bilerp(const T& f00, const T& f10, + const T& f01, const T& f11, + Scalar u, Scalar v) +{ + Scalar uv = u * v; + return ( + (Scalar(1.0) - u - v + uv) * f00 + + (u - uv) * f10 + + (v - uv) * f01 + + uv * f11 + ); +} +#endif + +#if 0 +/** 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 +T trilerp(const T& f000, const T& f100, + const T& f010, const T& f110, + const T& f001, const T& f101, + const T& f011, const T& f111, + Scalar u, Scalar v, Scalar w) +{ + Scalar uv = u * v; + Scalar vw = v * w; + Scalar wu = w * u; + Scalar uvw = uv * w; + + return ( + (Scalar(1.0) - u - v - w + uv + vw + wu - uvw) * f000 + + (u - uv - wu + uvw) * f100 + + (v - uv - vw + uvw) * f010 + + (uv - uvw) * f110 + + (w - vw - wu + uvw) * f001 + + (wu - uvw) * f101 + + (vw - uvw) * f011 + + uvw * f111 + ); +} +#endif + +/** Random binary (0,1) value. */ +inline size_t random_binary() { + return std::rand() % 2; +} + +/** Random polar (-1,1) value. */ +inline int random_polar() { + return random_binary() ? 1 : -1; +} + +/** Random real in [0,1]. */ +inline double random_unit() { + return double(std::rand()) / double(RAND_MAX); +} + +/* Random integer in the range [min, max] */ +inline long random_integer(long min, long max) { + return min + std::rand() % (max - min + 1); +} + +/* Random real number in the range [min, max] */ +template < typename T > +T random_real(T min, T max) { + return min + random_unit() * (max - min); +} + +/** Squared length in R2. */ +template < typename T > +T length_squared(T x, T y) { + return x * x + y * y; +} + +/** Squared length in R3. */ +template < typename T > +T length_squared(T x, T y, T z) { + return x * x + y * y + z * z; +} + +/** Length in R2. */ +template < typename T > +T length(T x, T y) { + return std::sqrt(length_squared(x,y)); +} + +/** Length in R3. */ +template < typename T > +T length(T x, T y, T z) { + return std::sqrt(length_squared(x,y,z)); +} + +/** Index of maximum of 3 values. */ +template < typename T > +size_t index_of_max(T a, T b, T c) { + return a > b ? (c > a ? 2 : 0) : (b > c ? 1 : 2); +} + +/** Index of maximum of 3 values by magnitude. */ +template < typename T > +size_t index_of_max_abs(T a, T b, T c) { + return index_of_max(std::fabs(a),std::fabs(b),std::fabs(c)); +} + +/** Index of minimum of 3 values. */ +template < typename T > +size_t index_of_min(T a, T b, T c) { + return a < b ? (c < a ? 2 : 0) : (b < c ? 1 : 2); +} + +/** Index of minimum of 3 values by magnitude. */ +template < typename T > +size_t index_of_min_abs(T a, T b, T c) { + return index_of_min(std::fabs(a),std::fabs(b),std::fabs(c)); +} + +/** Wrap input value to the range [min,max]. */ +template < typename T > +T wrap(T value, T min, T max) { + max -= min; + value = std::fmod(value - min, max); + if (value < T(0)) { + value += max; + } + return min + value; +} + +/** Convert horizontal field of view to vertical field of view. */ +template < typename T > +T xfov_to_yfov(T xfov, T aspect) { + return T(2.0 * std::atan(std::tan(xfov * T(.5)) / double(aspect))); +} + +/** Convert vertical field of view to horizontal field of view. */ +template < typename T > +T yfov_to_xfov(T yfov, T aspect) { + return T(2.0 * std::atan(std::tan(yfov * T(.5)) * double(aspect))); +} + +/** Convert horizontal zoom to vertical zoom. */ +template < typename T > +T xzoom_to_yzoom(T xzoom, T aspect) { + return xzoom * aspect; +} + +/** Convert vertical zoom to horizontal zoom. */ +template < typename T > +T yzoom_to_xzoom(T yzoom, T aspect) { + return yzoom / aspect; +} + +/** Convert zoom factor to field of view. */ +template < typename T > +T zoom_to_fov(T zoom) { + return T(2) * T(std::atan(T(1) / zoom)); +} + +/** Convert field of view to zoom factor. */ +template < typename T > +T fov_to_zoom(T fov) { + return T(1) / T(std::tan(fov * T(.5))); +} + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/vector.h b/src/cml/vector.h new file mode 100644 index 0000000..58c3c91 --- /dev/null +++ b/src/cml/vector.h @@ -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 + * + * The configurable vector<> class. + */ + +#ifndef cml_vector_h +#define cml_vector_h + +#include + +namespace cml { + +/** A configurable vector type. + * + * This class encapsulates the notion of a vector. The ArrayType template + * argument can be used to select the type of array to be used as internal + * storage for a list of type Element. The vector orientation determines + * how vectors are used arithmetically in expressions; i.e. a*b, when a is + * a row vector and b is a column vector, is the dot (inner) product, while + * a*b, when a is a column vector and b is a row vector, is the matrix + * (outer) product of a and b. + * + * @internal Unlike the previous version, this uses specializations to + * better enable varied array and vector types. For example, with the + * rebind method, it's difficult to support external<> vector types that + * should not be assigned to. + * + * @internal All assignments to the vector should go through UnrollAssignment, + * which ensures that the source expression and the destination vector have + * the same size. This is particularly important for dynamically-sized + * vectors. + */ +template class vector; + +} // namespace cml + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/vector/class_ops.h b/src/cml/vector/class_ops.h new file mode 100644 index 0000000..0b7a361 --- /dev/null +++ b/src/cml/vector/class_ops.h @@ -0,0 +1,238 @@ +/* -*- 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 Vector class operators. + */ + +#ifndef vector_class_ops_h +#define vector_class_ops_h + +#if defined(_MSC_VER) && _MSC_VER < 1400 +#pragma warning(disable:4003) +// XXX Horrible hack to turn off warnings about "not enough actual params" +// for the macros below. +#endif + +/* XXX HACK!!! This is a hack to resize in the assign() functions only when + * auto resizing is turned off. + */ +#if !defined(CML_VECTOR_RESIZE_ON_ASSIGNMENT) +#define _DO_VECTOR_SET_RESIZE(_N_) cml::et::detail::Resize(*this,_N_) +#else +#define _DO_VECTOR_SET_RESIZE(_N_) +#endif + +/** Set a vector from 2 values. */ +#define CML_ASSIGN_VEC_2 \ +vector_type& \ +set(ELEMENT_ARG_TYPE e0, ELEMENT_ARG_TYPE e1) { \ + _DO_VECTOR_SET_RESIZE(2); \ + /* This is overkill, but simplifies size checking: */ \ + value_type v[] = {e0,e1}; \ + typedef et::OpAssign OpT; \ + cml::vector< const value_type, external<2> > src(v); \ + et::UnrollAssignment(*this,src); \ + return *this; \ +} + +/** Set a vector from 3 values. */ +#define CML_ASSIGN_VEC_3 \ +vector_type& \ +set( \ + ELEMENT_ARG_TYPE e0, \ + ELEMENT_ARG_TYPE e1, \ + ELEMENT_ARG_TYPE e2 \ + ) \ +{ \ + _DO_VECTOR_SET_RESIZE(3); \ + /* This is overkill, but simplifies size checking: */ \ + value_type v[] = {e0,e1,e2}; \ + typedef et::OpAssign OpT; \ + cml::vector< const value_type, external<3> > src(v); \ + et::UnrollAssignment(*this,src); \ + return *this; \ +} + +/** Create a vector from 4 values. */ +#define CML_ASSIGN_VEC_4 \ +vector_type& \ +set( \ + ELEMENT_ARG_TYPE e0, \ + ELEMENT_ARG_TYPE e1, \ + ELEMENT_ARG_TYPE e2, \ + ELEMENT_ARG_TYPE e3 \ + ) \ +{ \ + _DO_VECTOR_SET_RESIZE(4); \ + /* This is overkill, but simplifies size checking: */ \ + value_type v[] = {e0,e1,e2,e3}; \ + typedef et::OpAssign OpT; \ + cml::vector< const value_type, external<4> > src(v); \ + et::UnrollAssignment(*this,src); \ + return *this; \ +} + + +/** Create a vector from 2 values. */ +#define CML_CONSTRUCT_VEC_2(_add_) \ +vector(ELEMENT_ARG_TYPE e0, ELEMENT_ARG_TYPE e1) _add_ { \ + set(e0,e1); \ +} + +/** Create a vector from 3 values. */ +#define CML_CONSTRUCT_VEC_3(_add_) \ +vector( \ + ELEMENT_ARG_TYPE e0, \ + ELEMENT_ARG_TYPE e1, \ + ELEMENT_ARG_TYPE e2 \ + ) _add_ \ +{ \ + set(e0,e1,e2); \ +} + +/** Create a vector from 4 values. */ +#define CML_CONSTRUCT_VEC_4(_add_) \ +vector( \ + ELEMENT_ARG_TYPE e0, \ + ELEMENT_ARG_TYPE e1, \ + ELEMENT_ARG_TYPE e2, \ + ELEMENT_ARG_TYPE e3 \ + ) _add_ \ +{ \ + set(e0,e1,e2,e3); \ +} + +/** Create a (fixed-size) N vector from an N-1-vector and a scalar. */ +#define CML_CONSTRUCT_FROM_SUBVEC(_add_) \ +vector( \ + const subvector_type& s, \ + ELEMENT_ARG_TYPE e \ + ) _add_ \ +{ \ + _DO_VECTOR_SET_RESIZE(s.size()+1); \ + for(size_t i = 0; i < s.size(); ++ i) \ + (*this)[i] = s[i]; \ + (*this)[s.size()] = e; \ +} + +/** Copy-construct a vector from a fixed-size array of values. */ +#define CML_VEC_COPY_FROM_FIXED_ARRAY(_N_,_add_) \ +vector(const value_type v[_N_]) _add_ { \ + typedef et::OpAssign OpT; \ + cml::vector< const value_type, external<_N_> > src(v); \ + et::UnrollAssignment(*this,src); \ +} + +/** Copy-construct a vector from a runtime-sized array of values. */ +#define CML_VEC_COPY_FROM_ARRAY(_add_) \ +vector(const value_type* const v, size_t N) _add_ { \ + typedef et::OpAssign OpT; \ + cml::vector > src(v,N); \ + et::UnrollAssignment(*this,src); \ +} + +/** Copy-construct a vector. + * + * @internal This is required for GCC4, since it won't elide the default + * copy constructor. + */ +#define CML_VEC_COPY_FROM_VECTYPE(_add_) \ +vector(const vector_type& v) _add_ { \ + typedef et::OpAssign OpT; \ + et::UnrollAssignment(*this,v); \ +} + +/** Construct from an arbitrary vector. + * + * @param v the vector to copy from. + */ +#define CML_VEC_COPY_FROM_VEC \ +template \ +vector(const vector& m) { \ + typedef et::OpAssign OpT; \ + et::UnrollAssignment(*this,m); \ +} + +/** Construct from a vector expression. + * + * @param expr the expression to copy from. + */ +#define CML_VEC_COPY_FROM_VECXPR \ +template \ +vector(VECXPR_ARG_TYPE e) { \ + /* Verify that a promotion exists at compile time: */ \ + typedef typename et::VectorPromote< \ + vector_type, typename XprT::result_type>::type result_type; \ + typedef typename XprT::value_type src_value_type; \ + typedef et::OpAssign OpT; \ + et::UnrollAssignment(*this,e); \ +} + +/** Assign from the same vector type. + * + * @param v the vector to copy from. + */ +#define CML_VEC_ASSIGN_FROM_VECTYPE \ +vector_type& operator=(const vector_type& v) { \ + typedef et::OpAssign OpT; \ + et::UnrollAssignment(*this,v); \ + return *this; \ +} + +/** Assign this vector from another using the given elementwise op. + * + * This allows assignment from arbitrary vector types. + * + * @param _op_ the operator (e.g. +=) + * @param _op_name_ the op functor (e.g. et::OpAssign) + */ +#define CML_VEC_ASSIGN_FROM_VEC(_op_, _op_name_) \ +template vector_type& \ +operator _op_ (const cml::vector& m) { \ + typedef _op_name_ OpT; \ + cml::et::UnrollAssignment(*this,m); \ + return *this; \ +} + +/** Declare a function to assign this vector from a vector expression. + * + * @param _op_ the operator (e.g. +=) + * @param _op_name_ the op functor (e.g. et::OpAssign) + */ +#define CML_VEC_ASSIGN_FROM_VECXPR(_op_, _op_name_) \ +template vector_type& \ +operator _op_ (VECXPR_ARG_TYPE e) { \ + /* Verify that a promotion exists at compile time: */ \ + typedef typename et::VectorPromote< \ + vector_type, typename XprT::result_type>::type result_type; \ + typedef typename XprT::value_type src_value_type; \ + typedef _op_name_ OpT; \ + cml::et::UnrollAssignment(*this,e); \ + return *this; \ +} + +/** Declare a function to assign this vector from a scalar. + * + * @param _op_ the operator (e.g. *=) + * @param _op_name_ the op functor (e.g. et::OpAssign) + * + * @internal This shouldn't be used for ops, like +=, which aren't + * defined in vector algebra. + */ +#define CML_VEC_ASSIGN_FROM_SCALAR(_op_, _op_name_) \ +vector_type& operator _op_ (ELEMENT_ARG_TYPE s) { \ + typedef _op_name_ OpT; \ + cml::et::UnrollAssignment(*this,s); \ + return *this; \ +} + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/vector/dynamic.h b/src/cml/vector/dynamic.h new file mode 100644 index 0000000..0fa800c --- /dev/null +++ b/src/cml/vector/dynamic.h @@ -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 Specialization for resizeable, dynamic-memory vector. + */ + +#ifndef dynamic_vector_h +#define dynamic_vector_h + +#include +#include +#include +#include + +namespace cml { + +/** Resizeable, dynamic-memory vector. */ +template +class vector< Element, dynamic > +: public dynamic_1D +{ + public: + + /* Shorthand for the generator: */ + typedef dynamic<> storage_type; + typedef dynamic generator_type; + + /* Shorthand for the array type: */ + typedef dynamic_1D array_type; + + /* Shorthand for the type of this vector: */ + typedef vector vector_type; + + /* For integration into the expression template code: */ + typedef vector_type expr_type; + + /* For integration into the expression template code: */ + typedef vector_type temporary_type; + + /* Standard: */ + typedef typename array_type::value_type value_type; + typedef typename array_type::reference reference; + typedef typename array_type::const_reference const_reference; + + /* For integration into the expression templates code: */ + typedef vector_type& expr_reference; + typedef const vector_type& expr_const_reference; + + /* For matching by storage type: */ + typedef typename array_type::memory_tag memory_tag; + + /* For matching by size type: */ + typedef typename array_type::size_tag size_tag; + + /* For matching by resizability: */ + typedef typename array_type::resizing_tag resizing_tag; + + /* For matching by result-type: */ + typedef cml::et::vector_result_tag result_tag; + + /* For matching by assignability: */ + typedef cml::et::assignable_tag assignable_tag; + + + public: + + /** Return square of the length. */ + value_type length_squared() const { + return cml::dot(*this,*this); + } + + /** Return the length. */ + value_type length() const { + return std::sqrt(length_squared()); + } + + /** Normalize the vector. */ + vector_type& normalize() { + return (*this /= length()); + } + + /** Set this vector to [0]. */ + vector_type& zero() { + typedef cml::et::OpAssign OpT; + cml::et::UnrollAssignment(*this,Element(0)); + return *this; + } + + /** Set this vector to a cardinal vector. */ + vector_type& cardinal(size_t i) { + zero(); + (*this)[i] = Element(1); + return *this; + } + + /** Pairwise minimum of this vector with another. */ + template + void minimize(const vector& v) { + /* XXX This should probably use ScalarPromote: */ + for (size_t i = 0; i < this->size(); ++i) { + (*this)[i] = std::min((*this)[i],v[i]); + } + } + + /** Pairwise maximum of this vector with another. */ + template + void maximize(const vector& v) { + /* XXX This should probably use ScalarPromote: */ + for (size_t i = 0; i < this->size(); ++i) { + (*this)[i] = std::max((*this)[i],v[i]); + } + } + + /** Fill vector with random elements. */ + void random(value_type min, value_type max) { + for (size_t i = 0; i < this->size(); ++i) { + (*this)[i] = cml::random_real(min,max); + } + } + + + public: + + /** Default constructor. */ + vector() : array_type() {} + + /** Construct given array size. */ + vector(size_t N) : array_type(N) {} + + + public: + + /* Define common class operators: */ + + CML_CONSTRUCT_VEC_2(: array_type()) + CML_CONSTRUCT_VEC_3(: array_type()) + CML_CONSTRUCT_VEC_4(: array_type()) + + CML_VEC_COPY_FROM_ARRAY(: array_type()) + CML_VEC_COPY_FROM_VECTYPE(: array_type()) + CML_VEC_COPY_FROM_VEC + CML_VEC_COPY_FROM_VECXPR + + CML_ASSIGN_VEC_2 + CML_ASSIGN_VEC_3 + CML_ASSIGN_VEC_4 + + CML_VEC_ASSIGN_FROM_VECTYPE + + CML_VEC_ASSIGN_FROM_VEC(=, cml::et::OpAssign) + CML_VEC_ASSIGN_FROM_VEC(+=, cml::et::OpAddAssign) + CML_VEC_ASSIGN_FROM_VEC(-=, cml::et::OpSubAssign) + + CML_VEC_ASSIGN_FROM_VECXPR(=, cml::et::OpAssign) + CML_VEC_ASSIGN_FROM_VECXPR(+=, cml::et::OpAddAssign) + CML_VEC_ASSIGN_FROM_VECXPR(-=, cml::et::OpSubAssign) + + CML_VEC_ASSIGN_FROM_SCALAR(*=, cml::et::OpMulAssign) + CML_VEC_ASSIGN_FROM_SCALAR(/=, cml::et::OpDivAssign) +}; + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/vector/external.h b/src/cml/vector/external.h new file mode 100644 index 0000000..bc9bc3f --- /dev/null +++ b/src/cml/vector/external.h @@ -0,0 +1,305 @@ +/* -*- 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 Specializations for external-memory vectors. + * + * @note Copy-constructing one external<> vector from another is not + * supported, since an external<> vector is essentially a wrapper for a + * pointer and has no allocated storage of its own. + */ + +#ifndef external_vector_h +#define external_vector_h + +#include +#include +#include +#include +#include + +namespace cml { + +/** Fixed-size, fixed-memory vector. */ +template +class vector< Element, external > +: public external_1D +{ + public: + + /* Shorthand for the generator: */ + typedef external<> storage_type; + typedef external generator_type; + + /* Shorthand for the array type: */ + typedef external_1D array_type; + + /* Shorthand for the type of this vector: */ + typedef vector vector_type; + + /* For integration into the expression template code: */ + typedef vector_type expr_type; + + /* For integration into the expression template code: */ + typedef vector< Element,fixed > temporary_type; + typedef typename temporary_type::subvector_type subvector_type; + /* Note: this ensures that an external vector is copied into the proper + * temporary; external<> temporaries are not allowed. + */ + + /* Standard: */ + typedef typename array_type::value_type value_type; + typedef typename array_type::reference reference; + typedef typename array_type::const_reference const_reference; + + /* For integration into the expression templates code: */ + typedef vector_type& expr_reference; + typedef const vector_type& expr_const_reference; + + /* For matching by storage type: */ + typedef typename array_type::memory_tag memory_tag; + + /* For matching by size type: */ + typedef typename array_type::size_tag size_tag; + + /* For matching by result-type: */ + typedef cml::et::vector_result_tag result_tag; + + /* For matching by assignability: */ + typedef cml::et::assignable_tag assignable_tag; + + + public: + + /** Return square of the length. */ + value_type length_squared() const { + return cml::dot(*this,*this); + } + + /** Return the length. */ + value_type length() const { + return std::sqrt(length_squared()); + } + + /** Normalize the vector. */ + vector_type& normalize() { + return (*this /= length()); + } + + /** Set this vector to [0]. */ + vector_type& zero() { + typedef cml::et::OpAssign OpT; + cml::et::UnrollAssignment(*this,Element(0)); + return *this; + } + + /** Set this vector to a cardinal vector. */ + vector_type& cardinal(size_t i) { + zero(); + (*this)[i] = Element(1); + return *this; + } + + /** Pairwise minimum of this vector with another. */ + template + void minimize(const vector& v) { + /* XXX This should probably use ScalarPromote: */ + for (size_t i = 0; i < this->size(); ++i) { + (*this)[i] = std::min((*this)[i],v[i]); + } + } + + /** Pairwise maximum of this vector with another. */ + template + void maximize(const vector& v) { + /* XXX This should probably use ScalarPromote: */ + for (size_t i = 0; i < this->size(); ++i) { + (*this)[i] = std::max((*this)[i],v[i]); + } + } + + /** Fill vector with random elements. */ + void random(value_type min, value_type max) { + for (size_t i = 0; i < this->size(); ++i) { + (*this)[i] = cml::random_real(min,max); + } + } + + + public: + + /** Construct from an array of values. */ + vector(Element* const array) : array_type(array) {} + + + public: + + CML_ASSIGN_VEC_2 + CML_ASSIGN_VEC_3 + CML_ASSIGN_VEC_4 + + CML_VEC_ASSIGN_FROM_VECTYPE + + /* Only assignment operators can be used to copy from other types: */ + CML_VEC_ASSIGN_FROM_VEC(=, cml::et::OpAssign) + CML_VEC_ASSIGN_FROM_VEC(+=, cml::et::OpAddAssign) + CML_VEC_ASSIGN_FROM_VEC(-=, cml::et::OpSubAssign) + + CML_VEC_ASSIGN_FROM_VECXPR(=, cml::et::OpAssign) + CML_VEC_ASSIGN_FROM_VECXPR(+=, cml::et::OpAddAssign) + CML_VEC_ASSIGN_FROM_VECXPR(-=, cml::et::OpSubAssign) + + CML_VEC_ASSIGN_FROM_SCALAR(*=, cml::et::OpMulAssign) + CML_VEC_ASSIGN_FROM_SCALAR(/=, cml::et::OpDivAssign) +}; + +/** Run-time sized vector. */ +template +class vector< Element, external<> > +: public external_1D +{ + public: + + /* Shorthand for the generator: */ + typedef external<> storage_type; + typedef external<> generator_type; + + /* Shorthand for the array type: */ + typedef external_1D array_type; + + /* Shorthand for the type of this vector: */ + typedef vector vector_type; + + /* For integration into the expression template code: */ + typedef vector_type expr_type; + + /* For integration into the expression template code: */ + typedef vector< Element, dynamic<> > temporary_type; + /* Note: this ensures that an external vector is copied into the proper + * temporary; external<> temporaries are not allowed. + */ + + /* Standard: */ + typedef typename array_type::value_type value_type; + typedef typename array_type::reference reference; + typedef typename array_type::const_reference const_reference; + + /* For integration into the expression templates code: */ + typedef vector_type& expr_reference; + typedef const vector_type& expr_const_reference; + + /* For matching by storage type: */ + typedef typename array_type::memory_tag memory_tag; + + /* For matching by size type: */ + typedef typename array_type::size_tag size_tag; + + /* For matching by resizability: */ + typedef typename array_type::resizing_tag resizing_tag; + + /* For matching by result-type: */ + typedef cml::et::vector_result_tag result_tag; + + /* For matching by assignability: */ + typedef cml::et::assignable_tag assignable_tag; + + + public: + + /** Return square of the length. */ + value_type length_squared() const { + return dot(*this,*this); + } + + /** Return the length. */ + value_type length() const { + return std::sqrt(length_squared()); + } + + /** Normalize the vector. */ + vector_type& normalize() { + return (*this /= length()); + } + + /** Set this vector to [0]. */ + vector_type& zero() { + typedef cml::et::OpAssign OpT; + cml::et::UnrollAssignment(*this,Element(0)); + return *this; + } + + /** Set this vector to a cardinal vector. */ + vector_type& cardinal(size_t i) { + zero(); + (*this)[i] = Element(1); + return *this; + } + + /** Pairwise minimum of this vector with another. */ + template + void minimize(const vector& v) { + /* XXX This should probably use ScalarPromote: */ + for (size_t i = 0; i < this->size(); ++i) { + (*this)[i] = std::min((*this)[i],v[i]); + } + } + + /** Pairwise maximum of this vector with another. */ + template + void maximize(const vector& v) { + /* XXX This should probably use ScalarPromote: */ + for (size_t i = 0; i < this->size(); ++i) { + (*this)[i] = std::max((*this)[i],v[i]); + } + } + + /** Fill vector with random elements. */ + void random(value_type min, value_type max) { + for (size_t i = 0; i < this->size(); ++i) { + (*this)[i] = random_real(min,max); + } + } + + + public: + + /** Construct from an array of values and the size. */ + vector(Element* const array, size_t size) + : array_type(array, size) {} + + + public: + + /* Define class operators for external vectors. Note: external vectors + * cannot be copy-constructed, but they can be assigned to: + */ + CML_ASSIGN_VEC_2 + CML_ASSIGN_VEC_3 + CML_ASSIGN_VEC_4 + + CML_VEC_ASSIGN_FROM_VECTYPE + + /* Only assignment operators can be used to copy from other types: */ + CML_VEC_ASSIGN_FROM_VEC(=, cml::et::OpAssign) + CML_VEC_ASSIGN_FROM_VEC(+=, cml::et::OpAddAssign) + CML_VEC_ASSIGN_FROM_VEC(-=, cml::et::OpSubAssign) + + CML_VEC_ASSIGN_FROM_VECXPR(=, cml::et::OpAssign) + CML_VEC_ASSIGN_FROM_VECXPR(+=, cml::et::OpAddAssign) + CML_VEC_ASSIGN_FROM_VECXPR(-=, cml::et::OpSubAssign) + + CML_VEC_ASSIGN_FROM_SCALAR(*=, cml::et::OpMulAssign) + CML_VEC_ASSIGN_FROM_SCALAR(/=, cml::et::OpDivAssign) +}; + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/vector/fixed.h b/src/cml/vector/fixed.h new file mode 100644 index 0000000..b065938 --- /dev/null +++ b/src/cml/vector/fixed.h @@ -0,0 +1,171 @@ +/* -*- 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 Specialization for fixed-size, fixed-memory vectors. + */ + +#ifndef fixed_vector_h +#define fixed_vector_h + +#include +#include +#include +#include +#include +#include + +namespace cml { + +/** Fixed-size, fixed-memory vector. */ +template +class vector< Element, fixed > +: public fixed_1D +{ + public: + + /* Shorthand for the generator: */ + typedef fixed<> storage_type; + typedef fixed generator_type; + + /* Shorthand for the array type: */ + typedef fixed_1D array_type; + + /* Shorthand for the type of this vector: */ + typedef vector vector_type; + + /* For integration into the expression template code: */ + typedef vector_type expr_type; + + /* For integration into the expression template code: */ + typedef vector_type temporary_type; + typedef vector< Element, fixed > subvector_type; + + /* Standard: */ + typedef typename array_type::value_type value_type; + typedef typename array_type::reference reference; + typedef typename array_type::const_reference const_reference; + + /* For integration into the expression templates code: */ + typedef vector_type& expr_reference; + typedef const vector_type& expr_const_reference; + + /* For matching by storage type: */ + typedef typename array_type::memory_tag memory_tag; + + /* For matching by size type: */ + typedef typename array_type::size_tag size_tag; + + /* For matching by result-type: */ + typedef cml::et::vector_result_tag result_tag; + + /* For matching by assignability: */ + typedef cml::et::assignable_tag assignable_tag; + + + public: + + /** Return square of the length. */ + value_type length_squared() const { + return cml::dot(*this,*this); + } + + /** Return the length. */ + value_type length() const { + return std::sqrt(length_squared()); + } + + /** Normalize the vector. */ + vector_type& normalize() { + return (*this /= length()); + } + + /** Set this vector to [0]. */ + vector_type& zero() { + typedef cml::et::OpAssign OpT; + cml::et::UnrollAssignment(*this,Element(0)); + return *this; + } + + /** Set this vector to a cardinal vector. */ + vector_type& cardinal(size_t i) { + zero(); + (*this)[i] = Element(1); + return *this; + } + + /** Pairwise minimum of this vector with another. */ + template + void minimize(const vector& v) { + /* XXX This should probably use ScalarPromote: */ + for (size_t i = 0; i < this->size(); ++i) { + (*this)[i] = std::min((*this)[i],v[i]); + } + } + + /** Pairwise maximum of this vector with another. */ + template + void maximize(const vector& v) { + /* XXX This should probably use ScalarPromote: */ + for (size_t i = 0; i < this->size(); ++i) { + (*this)[i] = std::max((*this)[i],v[i]); + } + } + + /** Fill vector with random elements. */ + void random(value_type min, value_type max) { + for (size_t i = 0; i < this->size(); ++i) { + (*this)[i] = cml::random_real(min,max); + } + } + + + public: + + vector() : array_type() {} + + + public: + + /* Define common class operators: */ + + CML_CONSTRUCT_VEC_2(/**/) + CML_CONSTRUCT_VEC_3(/**/) + CML_CONSTRUCT_VEC_4(/**/) + + CML_CONSTRUCT_FROM_SUBVEC(/**/) + + CML_VEC_COPY_FROM_FIXED_ARRAY(array_type::array_size,/**/) + CML_VEC_COPY_FROM_VECTYPE(: array_type()) + CML_VEC_COPY_FROM_VEC + CML_VEC_COPY_FROM_VECXPR + + CML_ASSIGN_VEC_2 + CML_ASSIGN_VEC_3 + CML_ASSIGN_VEC_4 + + CML_VEC_ASSIGN_FROM_VECTYPE + + CML_VEC_ASSIGN_FROM_VEC(=, cml::et::OpAssign) + CML_VEC_ASSIGN_FROM_VEC(+=, cml::et::OpAddAssign) + CML_VEC_ASSIGN_FROM_VEC(-=, cml::et::OpSubAssign) + + CML_VEC_ASSIGN_FROM_VECXPR(=, cml::et::OpAssign) + CML_VEC_ASSIGN_FROM_VECXPR(+=, cml::et::OpAddAssign) + CML_VEC_ASSIGN_FROM_VECXPR(-=, cml::et::OpSubAssign) + + CML_VEC_ASSIGN_FROM_SCALAR(*=, cml::et::OpMulAssign) + CML_VEC_ASSIGN_FROM_SCALAR(/=, cml::et::OpDivAssign) +}; + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/vector/vecop_macros.h b/src/cml/vector/vecop_macros.h new file mode 100644 index 0000000..a82c29a --- /dev/null +++ b/src/cml/vector/vecop_macros.h @@ -0,0 +1,248 @@ +/* -*- 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 various combinations of vector expressions. + * + * Create unary and binary operators with macros. The available combinations + * are: + * + * Unary expressions: + * + * op Vector -> Vector + * op VecXpr -> VecXpr + * + * Binary expressions: + * + * Vector op Vector -> Vector + * VecXpr op Vector -> VecXpr + * Vector op VecXpr -> VecXpr + * VecXpr op VecXpr -> VecXpr + * + * Vector op Scalar -> Vector + * Scalar op Vector -> Vector + * VecXpr op Scalar -> VecXpr + * Scalar op VecXpr -> VecXpr + * + * All of the generator functions compress the expression tree by hoisting + * subexpressions into the containing expression. This has the effect of + * forcing only the root node of the expression tree to be a VectorXpr. + * Every other node is a Unary or BinaryVectorOp. + * + * @todo Should ScalarT in expressions be passed by reference or by value? + */ + +#ifndef vecop_macros_h +#define vecop_macros_h + +/** Declare a unary operator taking a vector operand. */ +#define CML_VEC_UNIOP(_op_, _OpT_) \ +template \ +inline et::VectorXpr< \ + et::UnaryVectorOp< vector, _OpT_ > \ +> \ + \ +_op_ (const vector& arg) \ +{ \ + typedef et::UnaryVectorOp< \ + vector, _OpT_ \ + > ExprT; \ + return et::VectorXpr(ExprT(arg)); \ +} + + +/** Declare a unary operator taking a et::VectorXpr operand. */ +#define CML_VECXPR_UNIOP(_op_, _OpT_) \ +template \ +inline et::VectorXpr< \ + et::UnaryVectorOp< XprT, _OpT_ > \ +> \ + \ +_op_ (VECXPR_ARG_TYPE arg) \ +{ \ + typedef et::UnaryVectorOp< \ + XprT, _OpT_ \ + > ExprT; \ + return et::VectorXpr(ExprT(arg.expression())); \ +} + + +/** Declare an operator taking two vector operands. */ +#define CML_VEC_VEC_BINOP(_op_, _OpT_) \ +template \ +inline et::VectorXpr< \ + et::BinaryVectorOp< \ + vector, vector, _OpT_ \ + > \ +> \ + \ +_op_ ( \ + const vector& left, \ + const vector& right) \ +{ \ + typedef et::BinaryVectorOp< \ + vector, vector, _OpT_ \ + > ExprT; \ + return et::VectorXpr(ExprT(left,right)); \ +} + + +/** Declare an operator taking a vector and a et::VectorXpr. */ +#define CML_VEC_VECXPR_BINOP(_op_, _OpT_) \ +template \ +inline et::VectorXpr< \ + et::BinaryVectorOp< \ + vector, XprT, _OpT_ \ + > \ +> \ + \ +_op_ ( \ + const vector& left, \ + VECXPR_ARG_TYPE right) \ +{ \ + typedef et::BinaryVectorOp< \ + vector, XprT, \ + _OpT_ \ + > ExprT; \ + return et::VectorXpr(ExprT(left,right.expression())); \ +} + + +/** Declare an operator taking an et::VectorXpr and a vector. */ +#define CML_VECXPR_VEC_BINOP(_op_, _OpT_) \ +template \ +inline et::VectorXpr< \ + et::BinaryVectorOp< \ + XprT, vector, _OpT_ \ + > \ +> \ + \ +_op_ ( \ + VECXPR_ARG_TYPE left, \ + const vector& right) \ +{ \ + typedef et::BinaryVectorOp< \ + XprT, vector, \ + _OpT_ \ + > ExprT; \ + return et::VectorXpr(ExprT(left.expression(),right)); \ +} + + +/** Declare an operator taking two et::VectorXpr operands. */ +#define CML_VECXPR_VECXPR_BINOP(_op_, _OpT_) \ +template \ +inline et::VectorXpr< \ + et::BinaryVectorOp< \ + XprT1, XprT2, \ + _OpT_ < \ + typename XprT1::value_type, \ + typename XprT2::value_type \ + > \ + > \ +> \ + \ +_op_ ( \ + VECXPR_ARG_TYPE_N(1) left, \ + VECXPR_ARG_TYPE_N(2) right) \ +{ \ + typedef et::BinaryVectorOp< \ + XprT1, XprT2, \ + _OpT_ < \ + typename XprT1::value_type, \ + typename XprT2::value_type> \ + > ExprT; \ + return et::VectorXpr( \ + ExprT(left.expression(),right.expression())); \ +} + + +/** Declare an operator taking a vector and a scalar. */ +#define CML_VEC_SCALAR_BINOP(_op_, _OpT_) \ +template \ +inline et::VectorXpr< \ + et::BinaryVectorOp< \ + vector, ScalarT, _OpT_ \ + > \ +> \ + \ +_op_ ( \ + const vector& left, \ + SCALAR_ARG_TYPE right) \ +{ \ + typedef et::BinaryVectorOp< \ + vector, ScalarT, _OpT_ \ + > ExprT; \ + return et::VectorXpr(ExprT(left,right)); \ +} + + +/** Declare an operator taking a scalar and a vector. */ +#define CML_SCALAR_VEC_BINOP(_op_, _OpT_) \ +template \ +inline et::VectorXpr< \ + et::BinaryVectorOp< \ + ScalarT, vector, _OpT_ \ + > \ +> \ + \ +_op_ ( \ + SCALAR_ARG_TYPE left, \ + const vector& right) \ +{ \ + typedef et::BinaryVectorOp< \ + ScalarT, vector, _OpT_ \ + > ExprT; \ + return et::VectorXpr(ExprT(left,right)); \ +} + + +/** Declare an operator taking a et::VectorXpr and a scalar. */ +#define CML_VECXPR_SCALAR_BINOP(_op_, _OpT_) \ +template \ +inline et::VectorXpr< \ + et::BinaryVectorOp< \ + XprT, ScalarT, _OpT_ \ + > \ +> \ + \ +_op_ ( \ + VECXPR_ARG_TYPE left, \ + SCALAR_ARG_TYPE right) \ +{ \ + typedef et::BinaryVectorOp< \ + XprT, ScalarT, _OpT_ \ + > ExprT; \ + return et::VectorXpr(ExprT(left.expression(),right)); \ +} + + +/** Declare an operator taking a scalar and a et::VectorXpr. */ +#define CML_SCALAR_VECXPR_BINOP(_op_, _OpT_) \ +template \ +inline et::VectorXpr< \ + et::BinaryVectorOp< \ + ScalarT, XprT, _OpT_ \ + > \ +> \ + \ +_op_ ( \ + SCALAR_ARG_TYPE left, \ + VECXPR_ARG_TYPE right) \ +{ \ + typedef et::BinaryVectorOp< \ + ScalarT, XprT, \ + _OpT_ \ + > ExprT; \ + return et::VectorXpr(ExprT(left,right.expression())); \ +} + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/vector/vector_comparison.h b/src/cml/vector/vector_comparison.h new file mode 100644 index 0000000..c30a07f --- /dev/null +++ b/src/cml/vector/vector_comparison.h @@ -0,0 +1,236 @@ +/* -*- 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_comparison_h +#define vector_comparison_h + +#include +#include +#include + +/* This is used below to create a more meaningful compile-time error when + * vector_comparison is not provided with vector or VectorExpr arguments: + */ +struct vector_comparison_expects_vector_args_error; + +#define CML_VEC_VEC_ORDER(_order_, _op_, _OpT_) \ +template \ +inline bool \ +_op_ ( \ + const vector& left, \ + const vector& right) \ +{ \ + return detail::vector_##_order_ (left, right, _OpT_ ()); \ +} + +#define CML_VEC_VECXPR_ORDER(_order_, _op_, _OpT_) \ +template \ +inline bool \ +_op_ ( \ + const vector& left, \ + VECXPR_ARG_TYPE right) \ +{ \ + return detail::vector_##_order_ (left, right, \ + _OpT_ ()); \ +} + +#define CML_VECXPR_VEC_ORDER(_order_, _op_, _OpT_) \ +template \ +inline bool \ +_op_ ( \ + VECXPR_ARG_TYPE left, \ + const vector& right) \ +{ \ + return detail::vector_##_order_ (left, right, \ + _OpT_ ()); \ +} + +#define CML_VECXPR_VECXPR_ORDER(_order_, _op_, _OpT_) \ +template \ +inline bool \ +_op_ ( \ + VECXPR_ARG_TYPE_N(1) left, \ + VECXPR_ARG_TYPE_N(2) right) \ +{ \ + return detail::vector_##_order_ (left, right, \ + _OpT_ < \ + typename XprT1::value_type, \ + typename XprT2::value_type>()); \ +} + + +namespace cml { +namespace detail { + +/** Vector strict weak ordering relationship. + * + * OpT must implement a strict weak order on the vector element type. + * operator< and operator> on integer and floating-point types are + * examples. + */ +template +inline bool +vector_weak_order(const LeftT& left, const RightT& right, OpT) +{ + /* Shorthand: */ + typedef et::ExprTraits left_traits; + typedef et::ExprTraits right_traits; + + /* vector_comparison() requires vector expressions: */ + CML_STATIC_REQUIRE_M( + (et::VectorExpressions::is_true), + vector_comparison_expects_vector_args_error); + /* Note: parens are required here so that the preprocessor ignores the + * commas: + */ + + typedef typename et::VectorPromote< + typename left_traits::result_type, + typename right_traits::result_type + >::type result_type; + typedef typename result_type::size_tag size_tag; + + /* Verify expression size: */ + ssize_t N = (ssize_t) et::CheckedSize(left,right,size_tag()); + for(ssize_t i = 0; i < N; ++ i) { + if(OpT().apply( + left_traits().get(left,i), + right_traits().get(right,i) + )) + { + /* If weak order (a < b) is satisfied, return true: */ + return true; + } else if(OpT().apply( + right_traits().get(right,i), + left_traits().get(left,i) + )) + { + /* If !(b < a), then return false: */ + return false; + } else { + + /* Have !(a < b) && !(b < a) <=> (a >= b && b >= a) <=> (a == b). + * so need to test next element: + */ + continue; + } + } + /* XXX Can this be unrolled in any reasonable way? */ + + /* If we get here, then left == right: */ + return false; +} + +/** Vector total order relationship. + * + * OpT must implement a total order on the vector element type. operator<= + * and operator>= on integer and floating-point types are examples. + */ +template +inline bool +vector_total_order(const LeftT& left, const RightT& right, OpT) +{ + /* Shorthand: */ + typedef et::ExprTraits left_traits; + typedef et::ExprTraits right_traits; + + /* vector_comparison() requires vector expressions: */ + CML_STATIC_REQUIRE_M( + (et::VectorExpressions::is_true), + vector_comparison_expects_vector_args_error); + /* Note: parens are required here so that the preprocessor ignores the + * commas: + */ + + typedef typename et::VectorPromote< + typename left_traits::result_type, + typename right_traits::result_type + >::type result_type; + typedef typename result_type::size_tag size_tag; + + /* Verify expression size: */ + ssize_t N = (ssize_t) et::CheckedSize(left,right,size_tag()); + for(ssize_t i = 0; i < N; ++ i) { + + /* Test total order: */ + if(OpT().apply( + left_traits().get(left,i), + right_traits().get(right,i) + )) + { + /* Automatically true if weak order (a <= b) && !(b <= a) <=> + * (a <= b) && (b > a) <=> (a < b) is satisfied: + */ + if(!OpT().apply( + right_traits().get(right,i), + left_traits().get(left,i) + )) + return true; + + /* Otherwise, have equality (a <= b) && (b <= a), so continue + * to next element: + */ + else + continue; + + } else { + + /* Total order isn't satisfied (a > b), so return false: */ + return false; + } + } + /* XXX Can this be unrolled in any reasonable way? */ + + /* Total (==) or weak (<) order was satisfied, so return true: */ + return true; +} + +} + +/* XXX There is a better way to handle these with operator traits... */ + +CML_VEC_VEC_ORDER( total_order, operator==, et::OpEqual) +CML_VECXPR_VEC_ORDER( total_order, operator==, et::OpEqual) +CML_VEC_VECXPR_ORDER( total_order, operator==, et::OpEqual) +CML_VECXPR_VECXPR_ORDER( total_order, operator==, et::OpEqual) + +CML_VEC_VEC_ORDER( weak_order, operator!=, et::OpNotEqual) +CML_VECXPR_VEC_ORDER( weak_order, operator!=, et::OpNotEqual) +CML_VEC_VECXPR_ORDER( weak_order, operator!=, et::OpNotEqual) +CML_VECXPR_VECXPR_ORDER( weak_order, operator!=, et::OpNotEqual) + +CML_VEC_VEC_ORDER( weak_order, operator<, et::OpLess) +CML_VECXPR_VEC_ORDER( weak_order, operator<, et::OpLess) +CML_VEC_VECXPR_ORDER( weak_order, operator<, et::OpLess) +CML_VECXPR_VECXPR_ORDER( weak_order, operator<, et::OpLess) + +CML_VEC_VEC_ORDER( weak_order, operator>, et::OpGreater) +CML_VECXPR_VEC_ORDER( weak_order, operator>, et::OpGreater) +CML_VEC_VECXPR_ORDER( weak_order, operator>, et::OpGreater) +CML_VECXPR_VECXPR_ORDER( weak_order, operator>, et::OpGreater) + +CML_VEC_VEC_ORDER( total_order, operator<=, et::OpLessEqual) +CML_VECXPR_VEC_ORDER( total_order, operator<=, et::OpLessEqual) +CML_VEC_VECXPR_ORDER( total_order, operator<=, et::OpLessEqual) +CML_VECXPR_VECXPR_ORDER( total_order, operator<=, et::OpLessEqual) + +CML_VEC_VEC_ORDER( total_order, operator>=, et::OpGreaterEqual) +CML_VECXPR_VEC_ORDER( total_order, operator>=, et::OpGreaterEqual) +CML_VEC_VECXPR_ORDER( total_order, operator>=, et::OpGreaterEqual) +CML_VECXPR_VECXPR_ORDER( total_order, operator>=, et::OpGreaterEqual) + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/vector/vector_expr.h b/src/cml/vector/vector_expr.h new file mode 100644 index 0000000..9082520 --- /dev/null +++ b/src/cml/vector/vector_expr.h @@ -0,0 +1,458 @@ +/* -*- 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 Vector linear expression classes. + */ + +#ifndef vector_expr_h +#define vector_expr_h + +#include +#include +#include +#include + +/* XXX Don't know which it should be just yet, since RVO seems to obviate need + * for a reference type. However, copy by value copies the *entire expression + * tree rooted at the VectorXpr<>, so this choice is bound to affect + * performace for some compiler or another: + */ +#define VECXPR_ARG_TYPE const et::VectorXpr& +#define VECXPR_ARG_TYPE_N(_N_) const et::VectorXpr& + +//#define VECXPR_ARG_TYPE const et::VectorXpr +//#define VECXPR_ARG_TYPE_N(_N_) const et::VectorXpr + +namespace cml { +namespace et { + +/** A placeholder for a vector expression in an expression tree. */ +template +class VectorXpr +{ + public: + + typedef VectorXpr expr_type; + + /* Record ary-ness of the expression: */ + typedef typename ExprT::expr_ary expr_ary; + + /* Copy the expression by value into higher-up expressions: */ + typedef expr_type expr_const_reference; + + typedef typename ExprT::value_type value_type; + typedef vector_result_tag result_tag; + typedef typename ExprT::size_tag size_tag; + + /* Store the expression traits: */ + typedef ExprTraits expr_traits; + + /* Get the reference type: */ + typedef typename expr_traits::const_reference expr_reference; + + /* Get the result type: */ + typedef typename expr_traits::result_type result_type; + + /* For matching by assignability: */ + typedef cml::et::not_assignable_tag assignable_tag; + + /* Get the temporary type: */ + typedef typename result_type::temporary_type temporary_type; + + + public: + + /** Record result size as an enum. */ + enum { array_size = ExprT::array_size }; + + + public: + + /** Return square of the length. */ + value_type length_squared() const { + return m_expr.length_squared(); + } + + /** Return the length. */ + value_type length() const { + return m_expr.length(); + } + + /** Return the result as a normalized vector. */ + result_type normalize() const { + return m_expr.normalize(); + } + + /** Compute value at index i of the result vector. */ + value_type operator[](size_t i) const { + return m_expr[i]; + } + + + public: + + /** Return size of this expression (same as subexpression's size). */ + size_t size() const { + return m_expr.size(); + } + + /** Return reference to contained expression. */ + expr_reference expression() const { return m_expr; } + + + public: + + /** Construct from the subexpression to store. */ + explicit VectorXpr(expr_reference expr) : m_expr(expr) {} + + /** Copy constructor. */ + VectorXpr(const expr_type& e) : m_expr(e.m_expr) {} + + + protected: + + expr_reference m_expr; + + + private: + + /* Cannot be assigned to: */ + expr_type& operator=(const expr_type&); +}; + +/** Expression traits class for VectorXpr<>. */ +template +struct ExprTraits< VectorXpr > +{ + typedef VectorXpr expr_type; + typedef ExprT arg_type; + typedef typename expr_type::value_type value_type; + typedef typename expr_type::expr_const_reference const_reference; + typedef typename expr_type::result_tag result_tag; + typedef typename expr_type::size_tag size_tag; + typedef typename expr_type::result_type result_type; + typedef typename expr_type::assignable_tag assignable_tag; + typedef expr_node_tag node_tag; + + value_type get(const expr_type& v, size_t i) const { return v[i]; } + size_t size(const expr_type& e) const { return e.size(); } +}; + + +/** A unary vector expression. + * + * The operator's operator() method must take exactly one argument. + */ +template +class UnaryVectorOp +{ + public: + + typedef UnaryVectorOp expr_type; + + /* Record ary-ness of the expression: */ + typedef unary_expression expr_ary; + + /* Copy the expression by value into higher-up expressions: */ + typedef expr_type expr_const_reference; + + typedef typename OpT::value_type value_type; + typedef vector_result_tag result_tag; + typedef typename ExprT::size_tag size_tag; + + /* Store the expression traits for the subexpression: */ + typedef ExprTraits expr_traits; + + /* Reference type for the subexpression: */ + typedef typename expr_traits::const_reference expr_reference; + + /* Get the result type (same as for subexpression): */ + typedef typename expr_traits::result_type result_type; + + /* For matching by assignability: */ + typedef cml::et::not_assignable_tag assignable_tag; + + /* Get the temporary type: */ + typedef typename result_type::temporary_type temporary_type; + + + public: + + /** Record result size as an enum. */ + enum { array_size = ExprT::array_size }; + + + public: + + /** Return square of the length. */ + value_type length_squared() const { + return dot( + VectorXpr(*this), + VectorXpr(*this)); + } + + /** Return the length. */ + value_type length() const { + return std::sqrt(length_squared()); + } + + /** Return the result as a normalized vector. */ + result_type normalize() const { + result_type v(VectorXpr(*this)); + return v.normalize(); + } + + /** Compute value at index i of the result vector. */ + value_type operator[](size_t i) const { + + /* This uses the expression traits to figure out how to access the + * i'th index of the subexpression: + */ + return OpT().apply(expr_traits().get(m_expr,i)); + } + + + public: + + /** Return size of this expression (same as argument's size). */ + size_t size() const { + return m_expr.size(); + } + + /** Return reference to contained expression. */ + expr_reference expression() const { return m_expr; } + + + public: + + /** Construct from the subexpression. */ + explicit UnaryVectorOp(expr_reference expr) : m_expr(expr) {} + + /** Copy constructor. */ + UnaryVectorOp(const expr_type& e) : m_expr(e.m_expr) {} + + + protected: + + expr_reference m_expr; + + + private: + + /* Cannot be assigned to: */ + expr_type& operator=(const expr_type&); +}; + +/** Expression traits class for UnaryVectorOp<>. */ +template +struct ExprTraits< UnaryVectorOp > +{ + typedef UnaryVectorOp expr_type; + typedef ExprT arg_type; + + typedef typename expr_type::value_type value_type; + typedef typename expr_type::expr_const_reference const_reference; + typedef typename expr_type::result_tag result_tag; + typedef typename expr_type::size_tag size_tag; + typedef typename expr_type::result_type result_type; + typedef typename expr_type::assignable_tag assignable_tag; + typedef expr_node_tag node_tag; + + value_type get(const expr_type& v, size_t i) const { return v[i]; } + size_t size(const expr_type& e) const { return e.size(); } +}; + + +/** A binary vector expression. + * + * The operator's operator() method must take exactly two arguments. + */ +template +class BinaryVectorOp +{ + public: + + typedef BinaryVectorOp expr_type; + + /* Record ary-ness of the expression: */ + typedef binary_expression expr_ary; + + /* Copy the expression by value into higher-up expressions: */ + typedef expr_type expr_const_reference; + + typedef typename OpT::value_type value_type; + typedef vector_result_tag result_tag; + + /* Store the expression traits types for the two subexpressions: */ + typedef ExprTraits left_traits; + typedef ExprTraits right_traits; + + /* Reference types for the two subexpressions: */ + typedef typename left_traits::const_reference left_reference; + typedef typename right_traits::const_reference right_reference; + + /* Figure out the expression's resulting (vector) type: */ + typedef typename left_traits::result_type left_result; + typedef typename right_traits::result_type right_result; + typedef typename VectorPromote::type result_type; + typedef typename result_type::size_tag size_tag; + + /* For matching by assignability: */ + typedef cml::et::not_assignable_tag assignable_tag; + + /* Get the temporary type: */ + typedef typename result_type::temporary_type temporary_type; + + /* Define a size checker: */ + typedef GetCheckedSize checked_size; + + + public: + + /** Record result size as an enum (if applicable). */ + enum { array_size = result_type::array_size }; + + + public: + + /** Return square of the length. */ + value_type length_squared() const { + return dot( + VectorXpr(*this), + VectorXpr(*this)); + } + + /** Return the length. */ + value_type length() const { + return std::sqrt(length_squared()); + } + + /** Return the result as a normalized vector. */ + result_type normalize() const { + result_type v(VectorXpr(*this)); + return v.normalize(); + } + + /** Compute value at index i of the result vector. */ + value_type operator[](size_t i) const { + + /* This uses the expression traits to figure out how to access the + * i'th index of the two subexpressions: + */ + return OpT().apply( + left_traits().get(m_left,i), + right_traits().get(m_right,i)); + } + + + public: + + /** Return the size of the vector result. + * + * @throws std::invalid_argument if the expressions do not have the same + * size. + */ + size_t size() const { + /* Note: This actually does a check only if + * CML_CHECK_VECTOR_EXPR_SIZES is set: + */ + return CheckedSize(m_left,m_right,size_tag()); + } + + /** Return reference to left expression. */ + left_reference left_expression() const { return m_left; } + + /** Return reference to right expression. */ + right_reference right_expression() const { return m_right; } + + + public: + + /** Construct from the two subexpressions. */ + explicit BinaryVectorOp(left_reference left, right_reference right) + : m_left(left), m_right(right) {} + + /** Copy constructor. */ + BinaryVectorOp(const expr_type& e) + : m_left(e.m_left), m_right(e.m_right) {} + + + protected: + + left_reference m_left; + right_reference m_right; + + + private: + + /* This ensures that a compile-time size check is executed: */ + typename checked_size::check_type _dummy; + + + private: + + /* Cannot be assigned to: */ + expr_type& operator=(const expr_type&); +}; + +/** Expression traits class for BinaryVectorOp<>. */ +template +struct ExprTraits< BinaryVectorOp > +{ + typedef BinaryVectorOp expr_type; + typedef LeftT left_type; + typedef RightT right_type; + + typedef typename expr_type::value_type value_type; + typedef typename expr_type::expr_const_reference const_reference; + typedef typename expr_type::result_tag result_tag; + typedef typename expr_type::size_tag size_tag; + typedef typename expr_type::result_type result_type; + typedef typename expr_type::assignable_tag assignable_tag; + typedef expr_node_tag node_tag; + + value_type get(const expr_type& v, size_t i) const { return v[i]; } + size_t size(const expr_type& e) const { return e.size(); } +}; + +/* Helper struct to verify that both arguments are vector expressions: */ +template +struct VectorExpressions +{ + /* Require that both arguments are vector expressions: */ + typedef typename LeftTraits::result_tag left_result; + typedef typename RightTraits::result_tag right_result; + enum { is_true = (same_type::is_true + && same_type::is_true) }; +}; + +namespace detail { + +template inline +void Resize(VecT&,size_t,RT,MT) {} + +template inline +void Resize(VecT& v, size_t S, resizable_tag, dynamic_memory_tag) { + v.resize(S); +} + +template inline +void Resize(VecT& v, size_t S) { + Resize(v, S, typename VecT::resizing_tag(), typename VecT::memory_tag()); +} + +} // namespace detail + +} // namespace et +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/vector/vector_functions.h b/src/cml/vector/vector_functions.h new file mode 100644 index 0000000..ce92371 --- /dev/null +++ b/src/cml/vector/vector_functions.h @@ -0,0 +1,73 @@ +/* -*- 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_functions_h +#define vector_functions_h + +namespace cml { + +/** Squared length of a vector. */ +template +inline typename vector::value_type +length_squared(const vector& arg) +{ + return arg.length_squared(); +} + +/** Squared length of a vector expr. */ +template +inline typename XprT::value_type +length_squared(VECXPR_ARG_TYPE arg) +{ + return arg.length_squared(); +} + +/** Length of a vector. */ +template +inline typename vector::value_type +length(const vector& arg) +{ + return arg.length(); +} + +/** Length of a vector expr. */ +template +inline typename XprT::value_type +length(VECXPR_ARG_TYPE arg) +{ + return arg.length(); +} + +/** Normalize a vector. */ +template +inline vector +normalize(const vector& arg) +{ + vector result(arg); + result.normalize(); + return result; +} + +/** Normalize a vector expr. */ +template +inline typename XprT::result_type +normalize(VECXPR_ARG_TYPE arg) +{ + return arg.normalize(); +} + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/vector/vector_ops.h b/src/cml/vector/vector_ops.h new file mode 100644 index 0000000..e7920d1 --- /dev/null +++ b/src/cml/vector/vector_ops.h @@ -0,0 +1,51 @@ +/* -*- 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 vector operators. + */ + +#ifndef vector_ops_h +#define vector_ops_h + +#include +#include +#include + +namespace cml { + +CML_VEC_UNIOP( operator+, et::OpPos) +CML_VECXPR_UNIOP( operator+, et::OpPos) + +CML_VEC_UNIOP( operator-, et::OpNeg) +CML_VECXPR_UNIOP( operator-, et::OpNeg) + +CML_VEC_VEC_BINOP( operator+, et::OpAdd) +CML_VECXPR_VEC_BINOP( operator+, et::OpAdd) +CML_VEC_VECXPR_BINOP( operator+, et::OpAdd) +CML_VECXPR_VECXPR_BINOP( operator+, et::OpAdd) + +CML_VEC_VEC_BINOP( operator-, et::OpSub) +CML_VECXPR_VEC_BINOP( operator-, et::OpSub) +CML_VEC_VECXPR_BINOP( operator-, et::OpSub) +CML_VECXPR_VECXPR_BINOP( operator-, et::OpSub) + +CML_VEC_SCALAR_BINOP( operator*, et::OpMul) +CML_SCALAR_VEC_BINOP( operator*, et::OpMul) +CML_VECXPR_SCALAR_BINOP( operator*, et::OpMul) +CML_SCALAR_VECXPR_BINOP( operator*, et::OpMul) + +CML_VEC_SCALAR_BINOP( operator/, et::OpDiv) +CML_VECXPR_SCALAR_BINOP( operator/, et::OpDiv) + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/vector/vector_print.h b/src/cml/vector/vector_print.h new file mode 100644 index 0000000..cbb4764 --- /dev/null +++ b/src/cml/vector/vector_print.h @@ -0,0 +1,49 @@ +/* -*- 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_print_h +#define vector_print_h + +#include + +namespace cml { + +/** Output a vector to a std::ostream. */ +template inline std::ostream& +operator<<(std::ostream& os, const vector& v) +{ + os << "["; + for (size_t i = 0; i < v.size(); ++i) { + os << " " << v[i]; + } + os << " ]"; + return os; +} + +/** Output a vector expression to a std::ostream. */ +template< class XprT > inline std::ostream& +operator<<(std::ostream& os, const et::VectorXpr& v) +{ + os << "["; + for (size_t i = 0; i < v.size(); ++i) { + os << " " << v[i]; + } + os << " ]"; + return os; +} + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/vector/vector_products.h b/src/cml/vector/vector_products.h new file mode 100644 index 0000000..89f5edc --- /dev/null +++ b/src/cml/vector/vector_products.h @@ -0,0 +1,361 @@ +/* -*- 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 vector dot and outer products. + * + * @todo Figure out if the source or destination size type should trigger + * unrolling. May need a per-compiler compile-time option for this. + */ + +#ifndef vector_products_h +#define vector_products_h + +#include +#include +#include +#include +#include +#include + +/* This is used below to create a more meaningful compile-time error when + * dot() is not provided with vector or VectorExpr arguments: + */ +struct dot_expects_vector_args_error; + +/* This is used below to create a more meaningful compile-time error when + * perp_dot() is not provided with 2D vector or VectorExpr arguments: + */ +struct perp_dot_expects_vector_args_error; +struct perp_dot_expects_2D_vector_args_error; + +/* This is used below to create a more meaningful compile-time error when + * outer() is not provided with vector or VectorExpr arguments: + */ +struct outer_expects_vector_args_error; + +/* This is used below to create a more meaningful compile-time error when + * cross() is not provided with 3D vector or VectorExpr arguments: + */ +struct cross_expects_vector_args_error; +struct cross_expects_3D_vector_args_error; + + +namespace cml { +namespace detail { + +template +struct DotPromote +{ + /* Shorthand: */ + typedef et::ExprTraits left_traits; + typedef et::ExprTraits right_traits; + typedef typename left_traits::value_type left_value; + typedef typename right_traits::value_type right_value; + + /* Deduce the promoted scalar type: */ + typedef et::OpMul op_mul; + typedef typename et::OpAdd< + typename op_mul::value_type, + typename op_mul::value_type> op_add; + typedef typename op_add::value_type promoted_scalar; +}; + +template +struct CrossPromote +{ + /* Shorthand: */ + typedef et::ExprTraits left_traits; + typedef et::ExprTraits right_traits; + typedef typename left_traits::result_type left_type; + typedef typename right_traits::result_type right_type; + + /* Deduce the matrix result type: */ + typedef typename et::VectorPromote< + left_type,right_type>::temporary_type promoted_vector; +}; + +template +struct OuterPromote +{ + /* Shorthand: */ + typedef et::ExprTraits left_traits; + typedef et::ExprTraits right_traits; + typedef typename left_traits::result_type left_type; + typedef typename right_traits::result_type right_type; + + /* Deduce the matrix result type: */ + typedef typename et::MatrixPromote< + left_type,right_type>::temporary_type promoted_matrix; +}; + +/** Construct a dot unroller for fixed-size arrays. + * + * @note This should only be called for vectors. + * + * @sa cml::dot + */ +template +inline typename DotPromote::promoted_scalar +UnrollDot(const LeftT& left, const RightT& right, fixed_size_tag) +{ + /* Shorthand: */ + typedef DotPromote dot_helper; + + /* Compile-type vector size check: */ + typedef typename et::GetCheckedSize + ::check_type check_sizes; + + /* Get the fixed array size using the helper: */ + enum { Len = check_sizes::array_size }; + + /* Record the unroller type: */ + typedef typename dot_helper::op_mul op_mul; + typedef typename dot_helper::op_add op_add; + typedef typename et::detail::VectorAccumulateUnroller< + op_add,op_mul,LeftT,RightT>::template + Eval<0, Len-1, (Len <= CML_VECTOR_DOT_UNROLL_LIMIT)> Unroller; + /* Note: Len is the array size, so Len-1 is the last element. */ + + /* Now, call the unroller: */ + return Unroller()(left,right); +} + +/** Use a loop to compute the dot product for dynamic arrays. + * + * @note This should only be called for vectors. + * + * @sa cml::dot + */ +template +inline typename DotPromote::promoted_scalar +UnrollDot(const LeftT& left, const RightT& right, dynamic_size_tag) +{ + /* Shorthand: */ + typedef DotPromote dot_helper; + typedef et::ExprTraits left_traits; + typedef et::ExprTraits right_traits; + typedef typename dot_helper::op_mul op_mul; + typedef typename dot_helper::op_add op_add; + + /* Record the return type: */ + typedef typename dot_helper::promoted_scalar sum_type; + + /* Verify expression sizes: */ + const size_t N = et::CheckedSize(left,right,dynamic_size_tag()); + + /* Initialize the sum. Left and right must be vector expressions, so + * it's okay to use array notation here: + */ + sum_type sum(op_mul().apply(left[0],right[0])); + for(size_t i = 1; i < N; ++i) { + /* XXX This might not be optimized properly by some compilers. + * but to do anything else requires changing the requirements + * of a scalar operator, or requires defining a new class of scalar + * = operators. + */ + sum = op_add().apply(sum, op_mul().apply(left[i], right[i])); + /* Note: we don't need get(), since both arguments are required to + * be vector expressions. + */ + } + return sum; +} + +/** For cross(): compile-time check for a 3D vector. */ +template inline void +Require3D(const VecT&, fixed_size_tag) { + CML_STATIC_REQUIRE_M( + ((size_t)VecT::array_size == 3), + cross_expects_3D_vector_args_error); +} + +/** For cross(): run-time check for a 3D vector. */ +template inline void +Require3D(const VecT& v, dynamic_size_tag) { + et::GetCheckedSize() + .equal_or_fail(v.size(),size_t(3)); +} + +/** For perp_dot(): compile-time check for a 2D vector. */ +template inline void +Require2D(const VecT& v, fixed_size_tag) { + CML_STATIC_REQUIRE_M( + ((size_t)VecT::array_size == 2), + perp_dot_expects_2D_vector_args_error); +} + +/** For perp_dot(): run-time check for a 2D vector. */ +template inline void +Require2D(const VecT& v, dynamic_size_tag) { + et::GetCheckedSize() + .equal_or_fail(v.size(),size_t(2)); +} + +} // namespace detail + + +/** Vector dot (inner) product implementation. + */ +template +inline typename detail::DotPromote::promoted_scalar +dot(const LeftT& left, const RightT& right) +{ + /* Shorthand: */ + typedef detail::DotPromote dot_helper; + typedef et::ExprTraits left_traits; + typedef et::ExprTraits right_traits; + typedef typename left_traits::result_type left_type; + typedef typename right_traits::result_type right_type; + typedef typename left_traits::size_tag left_size; + typedef typename right_traits::size_tag right_size; + + /* dot() requires vector expressions: */ + CML_STATIC_REQUIRE_M( + (et::VectorExpressions::is_true), + dot_expects_vector_args_error); + /* Note: parens are required here so that the preprocessor ignores the + * commas: + */ + + /* Figure out the unroller to use (fixed or dynamic): */ + typedef typename et::VectorPromote< + left_type, right_type>::temporary_type promoted_vector; + typedef typename promoted_vector::size_tag size_tag; + + /* Call unroller: */ + return detail::UnrollDot(left,right,size_tag()); +} + +/** perp_dot() + */ +template +inline typename detail::DotPromote::promoted_scalar +perp_dot(const LeftT& left, const RightT& right) +{ + /* Shorthand: */ + typedef et::ExprTraits left_traits; + typedef et::ExprTraits right_traits; + typedef typename left_traits::result_tag left_result; + typedef typename right_traits::result_tag right_result; + + /* perp_dot() requires vector expressions: */ + CML_STATIC_REQUIRE_M( + (same_type::is_true + && same_type::is_true), + perp_dot_expects_vector_args_error); + /* Note: parens are required here so that the preprocessor ignores the + * commas. + */ + + /* Make sure arguments are 2D vectors: */ + detail::Require2D(left, typename left_traits::size_tag()); + detail::Require2D(right, typename right_traits::size_tag()); + + /* Get result type: */ + typedef typename detail::DotPromote< + LeftT,RightT>::promoted_scalar result_type; + + /* Compute and return: */ + return result_type(left[0]*right[1]-left[1]*right[0]); +} + +template +inline typename detail::CrossPromote::promoted_vector +cross(const LeftT& left, const RightT& right) +{ + /* Shorthand: */ + typedef et::ExprTraits left_traits; + typedef et::ExprTraits right_traits; + typedef typename left_traits::result_tag left_result; + typedef typename right_traits::result_tag right_result; + + /* outer() requires vector expressions: */ + CML_STATIC_REQUIRE_M( + (same_type::is_true + && same_type::is_true), + cross_expects_vector_args_error); + /* Note: parens are required here so that the preprocessor ignores the + * commas. + */ + + /* Make sure arguments are 3D vectors: */ + detail::Require3D(left, typename left_traits::size_tag()); + detail::Require3D(right, typename right_traits::size_tag()); + + /* Get result type: */ + typedef typename detail::CrossPromote< + LeftT,RightT>::promoted_vector result_type; + + /* Now, compute and return the cross product: */ + result_type result( + left[1]*right[2] - left[2]*right[1], + left[2]*right[0] - left[0]*right[2], + left[0]*right[1] - left[1]*right[0] + ); + return result; +} + +/** Return the triple product of three 3D vectors. + * + * No checking is done here, as dot() and cross() will catch any size or + * type errors. + */ + +template < class VecT_1, class VecT_2, class VecT_3 > +typename detail::DotPromote< + VecT_1, typename detail::CrossPromote< VecT_2, VecT_3 >::promoted_vector +>::promoted_scalar +triple_product(const VecT_1& v1, const VecT_2& v2, const VecT_3& v3) { + return dot(v1,cross(v2,v3)); +} + +template +inline typename detail::OuterPromote::promoted_matrix +outer(const LeftT& left, const RightT& right) +{ + /* Shorthand: */ + typedef et::ExprTraits left_traits; + typedef et::ExprTraits right_traits; + typedef typename left_traits::result_tag left_result; + typedef typename right_traits::result_tag right_result; + + /* outer() requires vector expressions: */ + CML_STATIC_REQUIRE_M( + (same_type::is_true + && same_type::is_true), + dot_expects_vector_args_error); + /* Note: parens are required here so that the preprocessor ignores the + * commas. + */ + + /* Create a matrix with the right size (resize() is a no-op for + * fixed-size matrices): + */ + typename detail::OuterPromote::promoted_matrix C; + cml::et::detail::Resize(C, left.size(), right.size()); + + /* Now, compute the outer product: */ + for(size_t i = 0; i < left.size(); ++i) { + for(size_t j = 0; j < right.size(); ++j) { + C(i,j) = left[i]*right[j]; + /* Note: both arguments are vectors, so array notation + * is okay here. + */ + } + } + + return C; +} + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/vector/vector_promotions.h b/src/cml/vector/vector_promotions.h new file mode 100644 index 0000000..49e575d --- /dev/null +++ b/src/cml/vector/vector_promotions.h @@ -0,0 +1,77 @@ +/* -*- 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 for vectors used in vector/vector or vector/scalar + * expressions. + * + * @sa BinaryVectorOp + */ + +#ifndef vector_promotions_h +#define vector_promotions_h + +#include +#include + +namespace cml { +namespace et { + +/* Default vector type promotion template. */ +template struct VectorPromote; + +/** Type promotion for two vector types. */ +template +struct VectorPromote< cml::vector, cml::vector > +{ + typedef typename ArrayPromote< + typename cml::vector::array_type, + typename cml::vector::array_type + >::type promoted_array; + + /* The deduced vector result type: */ + typedef cml::vector< + typename promoted_array::value_type, + typename promoted_array::generator_type + > type; + + /* The deduced temporary type: */ + typedef typename type::temporary_type temporary_type; +}; + +/** Type promotion for a vector and a scalar. */ +template +struct VectorPromote, S> +{ + /* The deduced vector result type (the array type is the same): */ + typedef cml::vector::type, AT> type; + + /* The deduced temporary type: */ + typedef typename type::temporary_type temporary_type; +}; + +/** Type promotion for a scalar and a vector. */ +template +struct VectorPromote > +{ + /* The deduced vector result type (the array type is the same): */ + typedef cml::vector::type, AT> type; + + /* The deduced temporary type: */ + typedef typename type::temporary_type temporary_type; +}; + +} // namespace et +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/vector/vector_traits.h b/src/cml/vector/vector_traits.h new file mode 100644 index 0000000..51b4228 --- /dev/null +++ b/src/cml/vector/vector_traits.h @@ -0,0 +1,47 @@ +/* -*- 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_traits_h +#define vector_traits_h + +#include + +namespace cml { +namespace et { + +/** Expression traits for a vector<> type. */ +template +struct ExprTraits< cml::vector > +{ + typedef typename cml::vector::expr_type expr_type; + typedef typename expr_type::value_type value_type; + typedef typename expr_type::expr_reference reference; + typedef typename expr_type::expr_const_reference const_reference; + typedef typename expr_type::result_tag result_tag; + typedef typename expr_type::size_tag size_tag; + typedef typename expr_type::resizing_tag resizing_tag; + typedef typename expr_type::assignable_tag assignable_tag; + typedef expr_type result_type; + typedef expr_leaf_tag node_tag; + + value_type get(const expr_type& v, size_t i) const { return v[i]; } + size_t size(const expr_type& v) const { return v.size(); } +}; + +} // namespace et +} // namespace cml + + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/cml/vector/vector_unroller.h b/src/cml/vector/vector_unroller.h new file mode 100644 index 0000000..eed6da6 --- /dev/null +++ b/src/cml/vector/vector_unroller.h @@ -0,0 +1,259 @@ +/* -*- 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 vector unrollers. + * + * @todo Add unrolling for dynamic vectors, and for vectors longer than + * CML_VECTOR_UNROLL_LIMIT. + * + * @todo Does it make sense to unroll an assignment if either side of the + * assignment has a fixed size, or just when the target vector is fixed + * size? + */ + +#ifndef vector_unroller_h +#define vector_unroller_h + +#include +#include +#include + +#if !defined(CML_VECTOR_UNROLL_LIMIT) +#error "CML_VECTOR_UNROLL_LIMIT is undefined." +#endif + +namespace cml { +namespace et { +namespace detail { + +/** Unroll a binary assignment operator on a fixed-size vector. + * + * This uses forward iteration to make efficient use of the cache. + * + * @sa cml::vector + * @sa cml::et::OpAssign + * + * @bug Need to verify that OpT is actually an assignment operator. + */ +template +class VectorAssignmentUnroller +{ + protected: + + /* Forward declare: */ + template struct Eval; + + /* The vector type being assigned to: */ + typedef cml::vector vector_type; + + /* Record traits for the arguments: */ + typedef ExprTraits dest_traits; + typedef ExprTraits src_traits; + + /** Evaluate the binary operator for the first Len-1 elements. */ + template struct Eval { + void operator()(vector_type& dest, const SrcT& src) const { + + /* Apply to current N: */ + OpT().apply(dest[N], src_traits().get(src,N)); + /* Note: we don't need get(), since dest is a vector. */ + + /* Apply to N+1: */ + Eval()(dest, src); + } + }; + + /** Evaluate the binary operator at element Last. */ + template struct Eval { + void operator()(vector_type& dest, const SrcT& src) const { + + /* Apply to last element: */ + OpT().apply(dest[Last], src_traits().get(src,Last)); + /* Note: we don't need get(), since dest is a vector. */ + } + }; + + + /** Evaluate the binary operator using a loop. + * + * This is used when the vector's length is longer than + * CML_VECTOR_UNROLL_LIMIT + */ + template struct Eval { + void operator()(vector_type& dest, const SrcT& src) const { + for(size_t i = 0; i <= Last; ++i) { + OpT().apply(dest[i], src_traits().get(src,i)); + /* Note: we don't need get(), since dest is a vector. */ + } + } + }; + + + public: + + /** Unroll assignment to a fixed-sized vector. */ + void operator()(vector_type& dest, const SrcT& src, cml::fixed_size_tag) + { + typedef cml::vector vector_type; + enum { Len = vector_type::array_size }; + typedef typename VectorAssignmentUnroller::template + Eval<0, Len-1, (Len <= CML_VECTOR_UNROLL_LIMIT)> Unroller; + /* Note: Len is the array size, so Len-1 is the last element. */ + + /* Use a run-time check if src is a run-time sized expression: */ + typedef typename ExprTraits::size_tag src_size; + typedef typename select_if< + same_type::is_true, + dynamic_size_tag, fixed_size_tag>::result size_tag; + + /* Check the expression size (the returned size isn't needed): */ + CheckedSize(dest,src,size_tag()); + /* Note: for two fixed-size expressions, the if-statements and + * comparisons should be completely eliminated as dead code. If src + * is a dynamic-sized expression, the check will still happen. + */ + + /* Now, call the unroller: */ + Unroller()(dest,src); + } + + + private: + /* XXX Blah, a temp. hack to fix the auto-resizing stuff below. */ + size_t CheckOrResize( + vector_type& dest, const SrcT& src, cml::resizable_tag) + { +#if defined(CML_AUTOMATIC_VECTOR_RESIZE_ON_ASSIGNMENT) + /* Get the size of src. This also causes src to check its size: */ + size_t N = std::max(dest.size(),src_traits().size(src)); + + /* Set the destination vector's size: */ + cml::et::detail::Resize(dest,N); +#else + size_t N = CheckedSize(dest,src,dynamic_size_tag()); +#endif + + return N; + } + + size_t CheckOrResize( + vector_type& dest, const SrcT& src, cml::not_resizable_tag) + { + return CheckedSize(dest,src,dynamic_size_tag()); + } + /* XXX Blah, a temp. hack to fix the auto-resizing stuff below. */ + public: + + + /** Just use a loop to assign to a runtime-sized vector. */ + void operator()(vector_type& dest, const SrcT& src, cml::dynamic_size_tag) + { + /* Shorthand: */ + typedef ExprTraits src_traits; + size_t N = this->CheckOrResize( + dest,src,typename vector_type::resizing_tag()); + for(size_t i = 0; i < N; ++i) { + OpT().apply(dest[i], src_traits().get(src,i)); + /* Note: we don't need get(), since dest is a vector. */ + } + } + +}; + +/** Unroll a vector accumulation/reduction operator. + * + * This uses forward iteration to make efficient use of the cache. + */ +template +struct VectorAccumulateUnroller +{ + /* Forward declare: */ + template struct Eval; + + /* Record traits for the arguments: */ + typedef ExprTraits left_traits; + typedef ExprTraits right_traits; + + /* Figure out the return type: */ + typedef typename AccumT::value_type result_type; + + /** Evaluate for the first Len-1 elements. */ + template struct Eval { + result_type operator()( + const LeftT& left, const RightT& right) const + { + /* Apply to last value: */ + return AccumT().apply( + OpT().apply(left[N], right_traits().get(right,N)), + Eval()(left, right)); + /* Note: we don't need get(), since dest is a vector. */ + } + }; + + /** Evaluate the binary operator at element Last. */ + template struct Eval { + result_type operator()( + const LeftT& left, const RightT& right) const + { + return OpT().apply(left[Last],right_traits().get(right,Last)); + /* Note: we don't need get(), since dest is a vector. */ + } + }; + + /** Evaluate using a loop. */ + template struct Eval { + result_type operator()( + const LeftT& left, const RightT& right) const + { + result_type accum = OpT().apply(left[0],right[0]); + for(size_t i = 1; i <= Last; ++i) { + /* XXX This might not be optimized properly by some compilers, + * but to do anything else requires changing the requirements + * of a scalar operator. + */ + accum = AccumT().apply(accum, OpT().apply( + left[i],right_traits().get(right,i))); + /* Note: we don't need get(), since dest is a vector. */ + } + } + }; +}; + +} + +/** Construct an assignment unroller. + * + * The operator must be an assignment op, otherwise, this doesn't make any + * sense. + * + * @bug Need to verify that OpT is actually an assignment operator. + */ +template inline +void UnrollAssignment(cml::vector& dest, const SrcT& src) +{ + /* Record the destination vector type, and the expression traits: */ + typedef cml::vector vector_type; + + /* Record the type of the unroller: */ + typedef detail::VectorAssignmentUnroller unroller; + + /* Do the unroll call: */ + unroller()(dest, src, typename vector_type::size_tag()); + /* XXX It may make sense to unroll if either side is a fixed size. */ +} + +} // namespace et +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp diff --git a/src/math.cc b/src/math.cc deleted file mode 100644 index c81c874..0000000 --- a/src/math.cc +++ /dev/null @@ -1,58 +0,0 @@ - -/******************************************************************************* - - Copyright (c) 2009, Charles McGarvey - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -*******************************************************************************/ - -#include "math.hh" -#include "vector.hh" -#include "matrix.hh" - - -namespace dc { - - -vector2 vector2::zero(0.0, 0.0); -vector3 vector3::zero(0.0, 0.0, 0.0); - -matrix3 matrix3::zero(0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0); -matrix3 matrix3::identity(1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0); - -matrix4 matrix4::zero(0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0); -matrix4 matrix4::identity(1.0, 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 0.0, 1.0); - - -} // namespace dc - diff --git a/src/math.hh b/src/math.hh index 78f4efd..313ecc1 100644 --- a/src/math.hh +++ b/src/math.hh @@ -26,21 +26,33 @@ *******************************************************************************/ +#ifndef _MATH_HH_ +#define _MATH_HH_ + /** * @file math.hh * General math-related types and functions. */ -#ifndef _MATH_HH_ -#define _MATH_HH_ - #include +#include namespace dc { -typedef double scalar; ///< Scalar variable. +typedef double scalar; ///< Scalar variable. + +typedef cml::vector2d vector2; +typedef cml::vector3d vector3; +typedef cml::vector4d vector4; + +typedef cml::matrix33d_c matrix3; +typedef cml::matrix44d_c matrix4; + +typedef cml::quaterniond_p quaternion; + +typedef cml::vector4f color; // Here's a simple way to check the equality of floating-point variables more @@ -61,6 +73,5 @@ inline bool equals(scalar a, scalar b, scalar epsilon = default_epsilon) } // namespace dc - #endif // _MATH_HH_ diff --git a/src/matrix.hh b/src/matrix.hh deleted file mode 100644 index 49bdd4c..0000000 --- a/src/matrix.hh +++ /dev/null @@ -1,373 +0,0 @@ - -/******************************************************************************* - - Copyright (c) 2009, Charles McGarvey - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -*******************************************************************************/ - -#ifndef _MATRIX_HH_ -#define _MATRIX_HH_ - -/** - * @file matrix.hh - * Matrix classes. - */ - -#include - -#include "math.hh" - - -namespace dc { - -/** - * 3x3 matrix. - */ - -struct matrix3 -{ - matrix3() {} - matrix3(scalar M11, scalar M12, scalar M13, - scalar M21, scalar M22, scalar M23, - scalar M31, scalar M32, scalar M33) - { - m11 = M11; m12 = M12; m13 = M13; - m21 = M21; m22 = M22; m23 = M23; - m31 = M31; m32 = M32; m33 = M33; - } - matrix3(scalar m[9]) - { - array[0] = m[0]; array[1] = m[1]; array[2] = m[2]; - array[3] = m[3]; array[4] = m[4]; array[5] = m[5]; - array[6] = m[6]; array[7] = m[7]; array[8] = m[8]; - } - - const scalar& operator[](int i) const - { - assert(i >= 0 && i <= 8 && "Index into matrix3 out of bounds."); - return array[i]; - } - scalar& operator[](int i) - { - assert(i >= 0 && i <= 8 && "Index into matrix3 out of bounds."); - return array[i]; - } - - matrix3 operator+(const matrix3& m) const - { - return matrix3(m11 + m.m11, m12 + m.m12, m13 + m.m13, - m21 + m.m21, m22 + m.m22, m23 + m.m23, - m31 + m.m31, m32 + m.m32, m33 + m.m33); - } - matrix3 operator-(const matrix3& m) const - { - return matrix3(m11 - m.m11, m12 - m.m12, m13 - m.m13, - m21 - m.m21, m22 - m.m22, m23 - m.m23, - m31 - m.m31, m32 - m.m32, m33 - m.m33); - } - matrix3 operator*(const matrix3& m) const - { - return matrix3(m11 * m.m11 + m12 * m.m21 + m13 * m.m31, - m11 * m.m12 + m12 * m.m22 + m13 * m.m32, - m11 * m.m13 + m12 * m.m23 + m13 * m.m33, - m21 * m.m11 + m22 * m.m21 + m23 * m.m31, - m21 * m.m12 + m22 * m.m22 + m23 * m.m32, - m21 * m.m13 + m22 * m.m23 + m23 * m.m33, - m31 * m.m11 + m32 * m.m21 + m33 * m.m31, - m31 * m.m12 + m32 * m.m22 + m33 * m.m32, - m32 * m.m13 + m32 * m.m23 + m33 * m.m33); - } - matrix3 operator*(scalar s) const - { - return matrix3(m11 * s, m12 * s, m13 * s, - m21 * s, m22 * s, m23 * s, - m31 * s, m32 * s, m33 * s); - } - matrix3 operator/(scalar s) const - { - return matrix3(m11 / s, m12 / s, m13 / s, - m21 / s, m22 / s, m23 / s, - m31 / s, m32 / s, m33 / s); - } - - matrix3& operator+=(const matrix3& m) - { - m11 += m.m11; m12 += m.m12; m13 += m.m13; - m21 += m.m21; m22 += m.m22; m23 += m.m23; - m31 += m.m31; m32 += m.m32; m33 += m.m33; - return *this; - } - matrix3& operator-=(const matrix3& m) - { - m11 -= m.m11; m12 -= m.m12; m13 -= m.m13; - m21 -= m.m21; m22 -= m.m22; m23 -= m.m23; - m31 -= m.m31; m32 -= m.m32; m33 -= m.m33; - return *this; - } - matrix3& operator*=(const matrix3& m) - { - return *this = *this * m; - } - matrix3& operator*=(scalar s) - { - m11 *= s; m12 *= s; m13 *= s; - m21 *= s; m22 *= s; m23 *= s; - m31 *= s; m32 *= s; m33 *= s; - return *this; - } - matrix3& operator/=(scalar s) - { - m11 /= s; m12 /= s; m13 /= s; - m21 /= s; m22 /= s; m23 /= s; - m31 /= s; m32 /= s; m33 /= s; - return *this; - } - - matrix3 operator-() const - { - return matrix3(-m11, -m12, -m13, - -m21, -m22, -m23, - -m31, -m32, -m33); - } - - bool operator==(const matrix3& m) const - { - return equals(m11,m.m11) && equals(m12,m.m12) && equals(m13,m.m13) && - equals(m21,m.m21) && equals(m22,m.m22) && equals(m23,m.m23) && - equals(m31,m.m31) && equals(m32,m.m32) && equals(m33,m.m33); - } - bool operator!=(const matrix3& m) const - { - return !(*this == m); - } - - - union - { - struct - { - scalar m11, m21, m31; - scalar m12, m22, m32; - scalar m13, m23, m33; ///< scalars - }; - scalar array[9]; ///< array - }; - - static matrix3 zero; ///< zero matrix - static matrix3 identity; ///< identity matrix -}; - -inline matrix3 operator*(scalar s, const matrix3& m) -{ - return m * s; -} -inline matrix3 operator/(scalar s, const matrix3& m) -{ - return matrix3(s / m.m11, s / m.m12, s / m.m13, - s / m.m21, s / m.m22, s / m.m23, - s / m.m31, s / m.m32, s / m.m33); -} - - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -/** - * 4x4 matrix. - */ - -struct matrix4 -{ - matrix4() {} - matrix4(scalar M11, scalar M12, scalar M13, scalar M14, - scalar M21, scalar M22, scalar M23, scalar M24, - scalar M31, scalar M32, scalar M33, scalar M34, - scalar M41, scalar M42, scalar M43, scalar M44) - { - m11 = M11; m12 = M12; m13 = M13; m14 = M14; - m21 = M21; m22 = M22; m23 = M23; m24 = M24; - m31 = M31; m32 = M32; m33 = M33; m34 = M34; - m41 = M41; m42 = M42; m43 = M43; m44 = M44; - } - matrix4(scalar m[15]) - { - array[0] = m[0]; array[1] = m[1]; array[2] = m[2]; array[3] = m[3]; - array[4] = m[4]; array[5] = m[5]; array[6] = m[6]; array[7] = m[7]; - array[8] = m[8]; array[9] = m[9]; array[10] = m[10]; array[11] = m[11]; - array[12] = m[12]; array[13] = m[13]; array[14] = m[14]; array[15] = m[15]; - } - - const scalar& operator[](int i) const - { - assert(i >= 0 && i <= 15 && "Index into matrix4 out of bounds."); - return array[i]; - } - scalar& operator[](int i) - { - assert(i >= 0 && i <= 15 && "Index into matrix4 out of bounds."); - return array[i]; - } - - matrix4 operator+(const matrix4& m) const - { - return matrix4(m11 + m.m11, m12 + m.m12, m13 + m.m13, m14 + m.m14, - m21 + m.m21, m22 + m.m22, m23 + m.m23, m24 + m.m24, - m31 + m.m31, m32 + m.m32, m33 + m.m33, m34 + m.m34, - m41 + m.m41, m42 + m.m42, m43 + m.m43, m44 + m.m44); - } - matrix4 operator-(const matrix4& m) const - { - return matrix4(m11 - m.m11, m12 - m.m12, m13 - m.m13, m14 - m.m14, - m21 - m.m21, m22 - m.m22, m23 - m.m23, m24 - m.m24, - m31 - m.m31, m32 - m.m32, m33 - m.m33, m34 - m.m34, - m41 - m.m41, m42 - m.m42, m43 - m.m43, m44 - m.m44); - } - matrix4 operator*(const matrix4& m) const // TODO ?? - { - return matrix4(m11 * m.m11 + m12 * m.m21 + m13 * m.m31 + m14 * m.m41, - m11 * m.m12 + m12 * m.m22 + m13 * m.m32 + m14 * m.m42, - m11 * m.m13 + m12 * m.m23 + m13 * m.m33 + m14 * m.m43, - m11 * m.m14 + m12 * m.m24 + m13 * m.m34 + m14 * m.m44, - m21 * m.m11 + m22 * m.m21 + m23 * m.m31 + m24 * m.m41, - m21 * m.m12 + m22 * m.m22 + m23 * m.m32 + m24 * m.m42, - m21 * m.m13 + m22 * m.m23 + m23 * m.m33 + m24 * m.m43, - m21 * m.m14 + m22 * m.m24 + m23 * m.m34 + m24 * m.m44, - m31 * m.m11 + m32 * m.m21 + m33 * m.m31 + m34 * m.m41, - m31 * m.m12 + m32 * m.m22 + m33 * m.m32 + m34 * m.m42, - m31 * m.m13 + m32 * m.m23 + m33 * m.m33 + m34 * m.m43, - m31 * m.m14 + m32 * m.m24 + m33 * m.m34 + m34 * m.m44, - m41 * m.m11 + m42 * m.m21 + m43 * m.m31 + m44 * m.m41, - m41 * m.m12 + m42 * m.m22 + m43 * m.m32 + m44 * m.m42, - m41 * m.m13 + m42 * m.m23 + m43 * m.m33 + m44 * m.m43, - m41 * m.m14 + m42 * m.m24 + m43 * m.m34 + m44 * m.m44); - } - matrix4 operator*(scalar s) const - { - return matrix4(m11 * s, m12 * s, m13 * s, m14 * s, - m21 * s, m22 * s, m23 * s, m24 * s, - m31 * s, m32 * s, m33 * s, m34 * s, - m41 * s, m42 * s, m43 * s, m44 * s); - } - matrix4 operator/(scalar s) const - { - return matrix4(m11 / s, m12 / s, m13 / s, m14 / s, - m21 / s, m22 / s, m23 / s, m24 / s, - m31 / s, m32 / s, m33 / s, m34 / s, - m41 / s, m42 / s, m43 / s, m44 / s); - } - - matrix4& operator+=(const matrix4& m) - { - m11 += m.m11; m12 += m.m12; m13 += m.m13; m14 += m.m14; - m21 += m.m21; m22 += m.m22; m23 += m.m23; m24 += m.m24; - m31 += m.m31; m32 += m.m32; m33 += m.m33; m34 += m.m34; - m41 += m.m41; m42 += m.m42; m43 += m.m43; m44 += m.m44; - return *this; - } - matrix4& operator-=(const matrix4& m) - { - m11 -= m.m11; m12 -= m.m12; m13 -= m.m13; m14 -= m.m14; - m21 -= m.m21; m22 -= m.m22; m23 -= m.m23; m24 -= m.m24; - m31 -= m.m31; m32 -= m.m32; m33 -= m.m33; m34 -= m.m34; - m41 -= m.m41; m42 -= m.m42; m43 -= m.m43; m44 -= m.m44; - return *this; - } - matrix4& operator*=(const matrix4& m) - { - *this = *this * m; - return *this; - } - matrix4& operator*=(scalar s) - { - m11 *= s; m12 *= s; m13 *= s; m14 *= s; - m21 *= s; m22 *= s; m23 *= s; m24 *= s; - m31 *= s; m32 *= s; m33 *= s; m34 *= s; - m41 *= s; m42 *= s; m43 *= s; m44 *= s; - return *this; - } - matrix4& operator/=(scalar s) - { - m11 /= s; m12 /= s; m13 /= s; m14 /= s; - m21 /= s; m22 /= s; m23 /= s; m24 /= s; - m31 /= s; m32 /= s; m33 /= s; m34 /= s; - m41 /= s; m42 /= s; m43 /= s; m44 /= s; - return *this; - } - - matrix4 operator-() const - { - return matrix4(-m11, -m12, -m13, -m14, - -m21, -m22, -m23, -m24, - -m31, -m32, -m33, -m34, - -m41, -m42, -m43, -m44); - } - - bool operator==(const matrix4& m) const - { - return equals(m11,m.m11) && equals(m12,m.m12) && equals(m13,m.m13) && - equals(m14,m.m14) && equals(m21,m.m21) && equals(m22,m.m22) && - equals(m23,m.m23) && equals(m24,m.m24) && equals(m31,m.m31) && - equals(m32,m.m32) && equals(m33,m.m33) && equals(m34,m.m34) && - equals(m41,m.m41) && equals(m42,m.m42) && equals(m43,m.m43) && - equals(m44,m.m44); - } - bool operator!=(const matrix4& m) const - { - return !(*this == m); - } - - - union - { - struct - { - scalar m11, m21, m31, m41; - scalar m12, m22, m32, m42; - scalar m13, m23, m33, m43; - scalar m14, m24, m34, m44; ///< scalars - }; - scalar array[16]; ///< array - }; - - static matrix4 zero; ///< zero matrix - static matrix4 identity; ///< identity matrix -}; - -inline matrix4 operator*(scalar s, const matrix4& m) -{ - return m * s; -} -inline matrix4 operator/(scalar s, const matrix4& m) -{ - return matrix4(s / m.m11, s / m.m12, s / m.m13, s / m.m14, - s / m.m21, s / m.m22, s / m.m23, s / m.m24, - s / m.m31, s / m.m32, s / m.m33, s / m.m34, - s / m.m41, s / m.m42, s / m.m43, s / m.m44); -} - - -} // namespace dc - - -#endif // _MATRIX_HH_ - diff --git a/src/quaternion.hh b/src/quaternion.hh deleted file mode 100644 index daf3114..0000000 --- a/src/quaternion.hh +++ /dev/null @@ -1,188 +0,0 @@ - -/******************************************************************************* - - Copyright (c) 2009, Charles McGarvey - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -*******************************************************************************/ - -#ifndef _QUATERNION_HH_ -#define _QUATERNION_HH_ - -#include - -#include "math.hh" -#include "matrix.hh" -#include "vector.hh" - - -// -// Quaternion -> 3x3 Matrix -// -// | w^2 + x^2 - y^2 - z^2 2xy - 2wz 2xz + 2yw | -// | 2xy + 2wz w^2 - x^2 + y^2 - z^2 2yz - 2wx | -// | 2xz - 2wy 2yz - 2wx w^2 - x^2 - y^2 + z^2 | -// - -namespace dc { - - -class quaternion -{ -public: - // Constructors. - quaternion() {} - quaternion(scalar X, scalar Y, scalar Z, scalar W) { - vec.x = X; vec.y = Y; vec.z = Z; w = W; - } - quaternion(vector3 v, scalar W = 1.0) { - vec = v; w = W; - } - quaternion(scalar q[4]) { - vec.x = q[0]; vec.y = q[1]; vec.z = q[2]; w = q[3]; - } - - // Accessible by index. - const scalar& operator [] (int i) const { - assert(i >= 0 && i <= 3 && "Index into quaternion out of bounds."); - return *((&vec.x) + i); - } - scalar& operator [] (int i) { - assert(i >= 0 && i <= 3 && "Index into quaternion out of bounds."); - //return vec[i]; - return *((&vec.x) + i); - } - - // Basic maths. - quaternion operator + (const quaternion& q) const { - return quaternion(vec + q.vec, w + q.w); - } - quaternion operator - (const quaternion& q) const { - return quaternion(vec - q.vec, w - q.w); - } - quaternion operator * (const quaternion& q) const { - return quaternion(q.w * vec + w * q.vec + q.vec.cross(vec), - w * q.w - q.vec.dot(vec)); - } - quaternion operator * (scalar s) const { - return quaternion(vec * s, w * s); - } - quaternion operator / (scalar s) const { - return quaternion(vec / s, w / s); - } - - quaternion& operator += (const quaternion& q) { - vec += q.vec; w += q.w; - return *this; - } - quaternion& operator -= (const quaternion& q) { - vec -= q.vec; w -= q.w; - return *this; - } - quaternion& operator *= (const quaternion& q) { - scalar W = w; - w = W * q.w - q.vec.dot(vec); - vec = q.w * vec + W * q.vec + q.vec.cross(vec); - return *this; - } - quaternion& operator *= (scalar s) { - vec *= s; w *= s; - return *this; - } - quaternion& operator /= (scalar s) { - vec /= s; w /= s; - return *this; - } - - // Unary operators. - quaternion operator - () const { - return quaternion(-vec, -w); - } - - // Quaternion operations. - quaternion conjugate() const { - return quaternion(-vec, w); - } - void normalize(const vector3& v) - { - scalar len = length(); - if (len != 0.0) { *this /= len; } - } - - scalar length2() const { - return vec.x * vec.x + vec.y * vec.y + vec.z * vec.z + w * w; - } - scalar length() const { - return std::sqrt(length2()); - } - - // Converting to other types. - matrix3 matrix() const { - scalar x2 = vec.x * vec.x; - scalar y2 = vec.y * vec.y; - scalar z2 = vec.z * vec.z; - scalar w2 = w * w; - scalar xy = vec.x * vec.y; - scalar xz = vec.x * vec.z; - scalar yzwx = 2 * (vec.y * vec.z - w * vec.x); - scalar wy = w * vec.y; - scalar wz = w * vec.z; - return matrix3(x2 - y2 - z2 + w2, 2 * (xy - wz), 2 * (xz + wy), - 2 * (xy + wz), w2 - x2 + y2 - z2, yzwx, - 2 * (xz - wy), yzwx, w2 - x2 - y2 + z2); - } - vector3 axis(scalar& angle) { // Axis of rotation, w/ angle of rotation. - vector3 axisVec = axis(); - angle = 2 * std::acos(w); - return axisVec; - } - vector3 axis() { - scalar sa = std::sqrt(1 - w * w); - return vec / sa; - } - - // Checking equality. - bool operator == (const quaternion& q) const { - return vec == q.vec && equals(w,q.w); - } - bool operator != (const quaternion& q) const { - return !(*this == q); - } - - // Data. - vector3 vec; - scalar w; -}; - -inline quaternion operator * (scalar s, const quaternion& q) { - return q * s; -} -inline quaternion operator / (scalar s, const quaternion& q) { - return quaternion(s / q.vec, s / q.w); -} - -} // namespace dc - - -#endif // _QUATERNION_HH_ - diff --git a/src/vector.hh b/src/vector.hh deleted file mode 100644 index f5c18ba..0000000 --- a/src/vector.hh +++ /dev/null @@ -1,372 +0,0 @@ - -/******************************************************************************* - - Copyright (c) 2009, Charles McGarvey - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -*******************************************************************************/ - -#ifndef _VECTOR_HH_ -#define _VECTOR_HH_ - -/** - * @file vector.hh - * Vector classes. - */ - -#include -#include - -#include "math.hh" -#include "random.hh" - - -// TODO: project, reflect, random, normal (of plane), orthonormal - -namespace dc { - -/** - * 2-dimensional vector. - */ - -struct vector2 -{ - vector2() {} - vector2(scalar X, scalar Y) : x(X), y(Y) {} - vector2(scalar v[2]) : x(v[0]), y(v[1]) {} - - const scalar& operator[](int i) const - { - assert(i >= 0 && i <= 1 && "Index into vector2 out of bounds."); - return array[i]; - } - scalar& operator[](int i) - { - assert(i >= 0 && i <= 1 && "Index into vector2 out of bounds."); - return array[i]; - } - - vector2 operator+(const vector2& v) const - { - return vector2(x + v.x, y + v.y); - } - vector2 operator-(const vector2& v) const - { - return vector2(x - v.x, y - v.y); - } - vector2 operator*(scalar s) const - { - return vector2(x * s, y * s); - } - vector2 operator/(scalar s) const - { - return vector2(x / s, y / s); - } - - vector2& operator+=(const vector2& v) - { - x += v.x; y += v.y; - return *this; - } - vector2& operator-=(const vector2& v) - { - x -= v.x; y -= v.y; - return *this; - } - vector2& operator*=(scalar s) - { - x *= s; y *= s; - return *this; - } - vector2& operator/=(scalar s) - { - x /= s; y /= s; - return *this; - } - - vector2 operator-() const - { - return vector2(-x, -y); - } - - scalar dot(const vector2& v) const /// dot product - { - return x * v.x + y * v.y; - } - scalar angleBetween(const vector2& v) const - { - scalar lens = length() * v.length(); - if (lens == 0.0) { return HUGE_VAL; } - return std::acos(dot(v) / lens); - } - - void normalize() /// scale such that length is one - { - scalar len = length(); - if (len != 0.0) { *this /= len; } - } - vector2 normalized() const - { - scalar len = length(); - if (len != 0.0) { return *this / len; } - return vector2(0.0, 0.0); - } - - scalar length2() const /// magnitude squared - { - return dot(*this); - } - scalar length() const /// magnitude - { - return std::sqrt(length2()); - } - - bool operator==(const vector2& v) const - { - return equals(x, v.x) && equals(y, v.y); - } - bool operator!=(const vector2& v) const - { - return !(*this == v); - } - - - static vector2 random() - { - vector2 v = random(-1.0, 1.0); - v.normalize(); - return v; - } - - static vector2 random(scalar lower, scalar upper) - { - return vector2(rng::get(lower, upper), - rng::get(lower, upper)); - } - - - union - { - struct - { - scalar x, y; ///< euler coordinates - }; - scalar array[2]; ///< array - }; - - static vector2 zero; ///< zero vector -}; - -inline vector2 operator*(scalar s, const vector2& v) -{ - return v * s; -} -inline vector2 operator/(scalar s, const vector2& v) -{ - return vector2(s / v.x, s / v.y); -} - -inline std::ostream& operator<<(std::ostream& output, const vector2& v) -{ - output << "(" << v.x << "," << v.y << ")"; - return output; -} - - -typedef vector2 point; - - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -/** - * 3-dimensional vector. - */ - -struct vector3 -{ - vector3() {} - vector3(scalar X, scalar Y, scalar Z) : x(X), y(Y), z(Z) {} - vector3(scalar v[3]) : x(v[0]), y(v[1]), z(v[2]) {} - - const scalar& operator[](int i) const - { - assert(i >= 0 && i <= 2 && "Index into vector3 out of bounds."); - return array[i]; - } - scalar& operator[](int i) - { - assert(i >= 0 && i <= 2 && "Index into vector3 out of bounds."); - return array[i]; - } - - vector3 operator+(const vector3& v) const - { - return vector3(x + v.x, y + v.y, z + v.z); - } - vector3 operator-(const vector3& v) const - { - return vector3(x - v.x, y - v.y, z - v.z); - } - vector3 operator*(scalar s) const - { - return vector3(x * s, y * s, z * s); - } - vector3 operator/(scalar s) const - { - return vector3(x / s, y / s, z / s); - } - - vector3& operator+=(const vector3& v) - { - x += v.x; y += v.y; z += v.z; - return *this; - } - vector3& operator-=(const vector3& v) - { - x -= v.x; y -= v.y; z -= v.z; - return *this; - } - vector3& operator*=(scalar s) - { - x *= s; y *= s; z *= s; - return *this; - } - vector3& operator/=(scalar s) - { - x /= s; y /= s; z /= s; - return *this; - } - - vector3 operator-() const - { - return vector3(-x, -y, -z); - } - - scalar dot(const vector3& v) const /// dot product - { - return x * v.x + y * v.y + z * v.z; - } - vector3 cross(const vector3& v) const /// cross product - { - return vector3(y * v.z - z * v.y, - z * v.x - x * v.z, - x * v.y - y * v.x); - } - - scalar angleBetween(const vector3& v) const - { - scalar lens = length() * v.length(); - if (lens == 0.0) { return HUGE_VAL; } - return std::acos(dot(v) / lens); - } - - void normalize() /// scale such that length is one - { - scalar len = length(); - if (len != 0.0) { *this /= len; } - } - vector3 normalized() const - { - scalar len = length(); - if (len != 0.0) { return *this / len; } - return vector3(0.0, 0.0, 0.0); - } - - scalar length2() const /// magnitude squared - { - return dot(*this); - } - scalar length() const /// magnitude - { - return std::sqrt(length2()); - } - - bool operator==(const vector3& v) const - { - return equals(x, v.x) && equals(y, v.y) && equals(z, v.z); - } - bool operator!=(const vector3& v) const - { - return !(*this == v); - } - - - static vector3 random() - { - vector3 v = random(-1.0, 1.0); - v.normalize(); - return v; - } - - static vector3 random(scalar lower, scalar upper) - { - return vector3(rng::get(lower, upper), - rng::get(lower, upper), - rng::get(lower, upper)); - } - - - union - { - struct - { - scalar x, y, z; ///< euler coordinates - }; - struct - { - scalar u, v, w; ///< texture coordinates - }; - struct - { - scalar r, g, b; ///< color intensities - }; - scalar array[3]; ///< array - }; - - static vector3 zero; ///< zero vector -}; - -inline vector3 operator*(scalar s, const vector3& v) -{ - return v * s; -} -inline vector3 operator/(scalar s, const vector3& v) -{ - return vector3(s / v.x, s / v.y, s / v.z); -} - -inline std::ostream& operator<<(std::ostream& output, const vector3& v) -{ - output << "(" << v.x << "," << v.y << "," << v.z << ")"; - return output; -} - - -typedef vector3 color; - - -} // namespace dc - - -#endif // _VECTOR_HH_ - -- 2.43.0