From 09db06fbfd83c0db4dab8ba29fb6cdcac69fe6f6 Mon Sep 17 00:00:00 2001 From: Martin Felis Date: Fri, 28 Sep 2018 09:17:22 +0200 Subject: [PATCH] Working on single header simple math version --- 3rdparty/glfw/CMakeLists.txt | 12 - src/Serializer.h | 10 +- src/SimpleMath/README | 10 - src/SimpleMath/SimpleMath.h | 1721 ++++++++++++++++++- src/SimpleMath/SimpleMath.h.orig | 24 - src/SimpleMath/SimpleMathBlock.h | 194 --- src/SimpleMath/SimpleMathBlock.h.orig | 220 --- src/SimpleMath/SimpleMathCholesky.h | 94 - src/SimpleMath/SimpleMathCommaInitializer.h | 69 - src/SimpleMath/SimpleMathDynamic.h | 667 ------- src/SimpleMath/SimpleMathDynamic.h.orig | 619 ------- src/SimpleMath/SimpleMathFixed.h | 885 ---------- src/SimpleMath/SimpleMathFixed.h.orig | 825 --------- src/SimpleMath/SimpleMathGL.h | 346 ---- src/SimpleMath/SimpleMathLU.h | 107 -- src/SimpleMath/SimpleMathMap.h | 22 - src/SimpleMath/SimpleMathMixed.h | 138 -- src/SimpleMath/SimpleMathQR.h | 324 ---- src/SimpleMath/compileassert.h | 39 - src/math_types.h | 28 +- src/modules/RenderModule.cc | 6 +- src/modules/RenderUtils.cc | 1 - src/modules/TestModule.cc | 8 +- 23 files changed, 1738 insertions(+), 4631 deletions(-) delete mode 100644 src/SimpleMath/README delete mode 100644 src/SimpleMath/SimpleMath.h.orig delete mode 100644 src/SimpleMath/SimpleMathBlock.h delete mode 100644 src/SimpleMath/SimpleMathBlock.h.orig delete mode 100644 src/SimpleMath/SimpleMathCholesky.h delete mode 100644 src/SimpleMath/SimpleMathCommaInitializer.h delete mode 100644 src/SimpleMath/SimpleMathDynamic.h delete mode 100644 src/SimpleMath/SimpleMathDynamic.h.orig delete mode 100644 src/SimpleMath/SimpleMathFixed.h delete mode 100644 src/SimpleMath/SimpleMathFixed.h.orig delete mode 100644 src/SimpleMath/SimpleMathGL.h delete mode 100644 src/SimpleMath/SimpleMathLU.h delete mode 100644 src/SimpleMath/SimpleMathMap.h delete mode 100644 src/SimpleMath/SimpleMathMixed.h delete mode 100644 src/SimpleMath/SimpleMathQR.h delete mode 100644 src/SimpleMath/compileassert.h diff --git a/3rdparty/glfw/CMakeLists.txt b/3rdparty/glfw/CMakeLists.txt index b1476bd..831166d 100644 --- a/3rdparty/glfw/CMakeLists.txt +++ b/3rdparty/glfw/CMakeLists.txt @@ -376,18 +376,6 @@ configure_file(src/glfw3.pc.in src/glfw3.pc @ONLY) #-------------------------------------------------------------------- add_subdirectory(src) -if (GLFW_BUILD_EXAMPLES) - add_subdirectory(examples) -endif() - -if (GLFW_BUILD_TESTS) - add_subdirectory(tests) -endif() - -if (DOXYGEN_FOUND AND GLFW_BUILD_DOCS) - add_subdirectory(docs) -endif() - #-------------------------------------------------------------------- # Install files other than the library # The library is installed by src/CMakeLists.txt diff --git a/src/Serializer.h b/src/Serializer.h index 2f2733e..3dab10c 100644 --- a/src/Serializer.h +++ b/src/Serializer.h @@ -1,6 +1,6 @@ #pragma once -#include "SimpleMath/SimpleMath.h" +#include "math_types.h" #include #include #include @@ -134,11 +134,11 @@ bool SerializedUint16 (Serializer &serializer, const std::string &key, uint16_t& } template -bool SerializeVec3 (Serializer &serializer, const std::string &key, SimpleMath::Vector3f& value) { - return serializer.SerializeData(key, reinterpret_cast(&value), sizeof(SimpleMath::Vector3f)); +bool SerializeVec3 (Serializer &serializer, const std::string &key, Vector3f& value) { + return serializer.SerializeData(key, reinterpret_cast(&value), sizeof(Vector3f)); } template -bool SerializeVec4 (Serializer &serializer, const std::string &key, SimpleMath::Vector4f& value) { - return serializer.SerializeData(key, reinterpret_cast(&value), sizeof(SimpleMath::Vector4f)); +bool SerializeVec4 (Serializer &serializer, const std::string &key, Vector4f& value) { + return serializer.SerializeData(key, reinterpret_cast(&value), sizeof(Vector4f)); } diff --git a/src/SimpleMath/README b/src/SimpleMath/README deleted file mode 100644 index e49e343..0000000 --- a/src/SimpleMath/README +++ /dev/null @@ -1,10 +0,0 @@ -This is a highly inefficient math library. It was conceived by Martin -Felis while he was waiting for code to -compile which used a highly efficient math library. - -It is intended to be used as a fast compiling substitute for the -blazingly fast Eigen3 http://eigen.tuxfamily.org/index.php?title=Main_Page -library and tries to mimic its API to a certain extent. - -Feel free to use it wherever you like. However, no guarantees are given -that this code does what it says it would. diff --git a/src/SimpleMath/SimpleMath.h b/src/SimpleMath/SimpleMath.h index 22324bd..6182884 100644 --- a/src/SimpleMath/SimpleMath.h +++ b/src/SimpleMath/SimpleMath.h @@ -1,10 +1,1715 @@ -#ifndef _SIMPLEMATH_H -#define _SIMPLEMATH_H +#pragma once -#include "SimpleMathFixed.h" -#include "SimpleMathDynamic.h" -#include "SimpleMathMixed.h" -#include "SimpleMathQR.h" -#include "SimpleMathCommaInitializer.h" +#include +#include +#include +#include // for memcpy +#include +#include -#endif /* _SIMPLEMATH_H */ +namespace SimpleMath { + +// +// Forward Declarations +// +template +struct CommaInitializer; + +template +struct Fixed; + +template +struct Dynamic; + +template +struct Block; + +template +struct Transpose; + +//template +//class HouseholderQR; + +//template +//class ColPivHouseholderQR; + + +// +// Main MatrixBase class which defines all functions available on the +// derived matrix types. +// +template +struct MatrixBase { + typedef MatrixBase MatrixType; + typedef ScalarType value_type; + + template + Derived& operator=(const OtherDerived& other) { + Derived result (other.rows(), other.cols()); + + unsigned int i,j; + for (i = 0; i < rows(); i++) { + for (j = 0; j < cols(); j++) { + this->operator()(i,j) = other(i,j); + } + } + + return result; + } + + // + // operators with other matrices + // + template + void operator=(const OtherDerived& other) { + assert (cols() == other.cols()); + assert (rows() == other.rows()); + + std::cout << "here!!" << std::endl; + + for (int i = 0, nr = rows(); i < nr; ++i) { + for (int j = 0, nc = cols(); j < nc; ++j) { + operator()(i,j) = other(i,j); + } + } + } + + template + bool operator==(const OtherDerived& other) const { + unsigned int i,j; + for (i = 0; i < rows(); i++) { + for (j = 0; j < cols(); j++) { + if (this->operator()(i,j) != other(i,j)) + return false; + } + } + + return true; + } + + CommaInitializer operator<< (const value_type& value) { + return CommaInitializer (*(static_cast(this)), value); + } + + template + Derived operator+(const OtherDerived& other) const { + Derived result (*(static_cast(this))); + result += other; + return result; + } + + template + Derived operator-(const OtherDerived& other) { + Derived result (*(static_cast(this))); + result -= other; + return result; + } + + template + Derived operator-(const OtherDerived& other) const { + Derived result (*(static_cast(this))); + result -= other; + return result; + } + + template + Derived operator*(const OtherDerived& other) const { + MatrixBase()); + + unsigned int i,j,k; + for (i = 0; i < rows(); i++) { + for (j = 0; j < other.cols(); j++) { + for (k = 0; k < other.rows(); k++) { + result (i,j) += operator()(i,k) * other(k,j); + } + } + } + return result; + } + + template + Derived operator*=(const OtherDerived& other) { + Derived copy (*static_cast(this)); +// Derived result (Derived::Zero(rows(), other.cols())); + + unsigned int i,j,k; + for (i = 0; i < rows(); i++) { + for (j = 0; j < other.cols(); j++) { + for (k = 0; k < other.rows(); k++) { + operator()(i,j) += copy.operator()(i,k) * other(k,j); + } + } + } + + return *this; + } + + void set(const ScalarType& v0) { + static_assert(cols() * rows() == 1); + data()[0] = v0; + } + + void set( + const ScalarType& v0, + const ScalarType& v1, + const ScalarType& v2 + ) { + assert(cols() * rows() == 3); + + data()[0] = v0; + data()[1] = v1; + data()[2] = v2; + } + + void set( + const ScalarType& v0, + const ScalarType& v1, + const ScalarType& v2, + const ScalarType& v3 + ) { + assert(cols() * rows() == 4); + + data()[0] = v0; + data()[1] = v1; + data()[2] = v2; + data()[3] = v3; + } + + // + // operators with scalars + // + Derived operator*(const ScalarType& scalar) const { + Derived result (rows(), cols()); + + unsigned int i,j; + for (i = 0; i < rows(); i++) { + for (j = 0; j < cols(); j++) { + result (i,j) = operator()(i,j) * scalar; + } + } + return result; + } + + size_t rows() const { + return static_cast(this)->rows(); + } + size_t cols() const { + return static_cast(this)->cols(); + } + + const ScalarType& operator()(const size_t& i, const size_t& j) const { + return static_cast(this)->operator()(i,j); + } + ScalarType& operator()(const size_t& i, const size_t& j) { + return static_cast(this)->operator()(i,j); + } + + const ScalarType& operator[](const size_t& i) const { + static_assert(Cols == 1); + return static_cast(this)->operator()(i,0); + } + ScalarType& operator[](const size_t& i) { + static_assert(Cols == 1); + return static_cast(this)->operator()(i,0); + } + + // + // Numerical functions + // + + // TODO: separate functions for float or double matrices + double dot(const Derived& other) const { + assert ((rows() == 1 || cols() == 1) && (other.rows() == 1 || other.cols() == 1)); + + double result = 0.0; + + size_t n = rows() * cols(); + for (size_t i = 0; i < n; ++i) { + result += operator[](i) * other[i]; + } + + return result; + } + + // TODO: separate functions for float or double matrices + double squaredNorm() const { + double result = 0.0; + + size_t nr = rows(); + size_t nc = cols(); + for (size_t i = 0; i < nr; ++i) { + for (size_t j = 0; j < nc; ++j) { + result += operator()(i, j) * operator()(i, j); + } + } + + return result; + } + + // TODO: separate functions for float or double matrices + double norm() const { + return std::sqrt(squaredNorm()); + } + + // TODO: separate functions for float or double matrices + Derived normalized() const { + Derived result (rows(), cols()); + + double norm = squaredNorm(); + unsigned int i,j; + for (i = 0; i < rows(); i++) { + for (j = 0; j < cols(); j++) { + result (i,j) = operator()(i,j) / norm; + } + } + return result; + } + + // TODO: separate functions for float or double matrices + Derived normalize() { + double norm = squaredNorm(); + unsigned int i,j; + for (i = 0; i < rows(); i++) { + for (j = 0; j < cols(); j++) { + operator() (i,j) /= norm; + } + } + + return *this; + } + + Derived cross(const Derived& other) const { + assert(cols() * rows() == 3); + + Derived result(cols(), rows()); + result[0] = operator[](1) * other[2] - operator[](2) * other[1]; + result[1] = operator[](2) * other[0] - operator[](0) * other[2]; + result[2] = operator[](0) * other[1] - operator[](1) * other[0]; + + return result; + } + + Derived inverse() const { + assert(false); + + Derived result(cols(), rows()); + + return result; + } + + /* + Derived inverse() const { + return colPivHouseholderQr().inverse(); + } + + const HouseholderQR householderQr() const { + return HouseholderQR(*this); + } + const ColPivHouseholderQR colPivHouseholderQr() const { + return ColPivHouseholderQR(*this); + } + */ + + ScalarType* data() { + return static_cast(this)->data(); + } + + const ScalarType* data() const { + return static_cast(this)->data(); + } + + // + // Special Constructors + // + static Derived Zero(size_t NumRows = Rows, size_t NumCols = Cols) { + Derived result (NumRows, NumCols); + + for (size_t i = 0; i < NumRows; i++) { + for (size_t j = 0; j < NumCols; j++) { + result(i,j) = static_cast(0.0); + } + } + + return result; + } + + static Derived Identity(size_t NumRows = Rows, size_t NumCols = Cols) { + Derived result (NumRows, NumCols); + + for (size_t i = 0; i < NumRows; i++) { + for (size_t j = 0; j < NumCols; j++) { + result(i,j) = static_cast(1.0); + } + } + + return result; + } + + // TODO: Random() + + // + // Block accessors + // + template < + int block_rows, + int block_cols + > + Block< + Derived, + ScalarType, + block_rows, + block_cols + > block(int block_row_index, int block_col_index) { + assert(block_row_index + block_rows <= rows()); + assert(block_col_index + block_cols <= cols()); + return Block(static_cast(this), block_row_index, block_col_index); + } + + template < + int block_rows, + int block_cols + > + const Block< + Derived, + ScalarType, + block_rows, + block_cols + > block(int block_row_index, int block_col_index) const { + assert(block_row_index + block_rows <= rows()); + assert(block_col_index + block_cols <= cols()); + return Block(const_cast(static_cast(this)), block_row_index, block_col_index); + } + + Block< + Derived, + ScalarType + > block(int block_row_index, int block_col_index, + int block_num_rows, int block_num_cols) { + assert(block_row_index + block_num_rows <= rows()); + assert(block_col_index + block_num_cols <= cols()); + return Block(static_cast(this), block_row_index, block_col_index, block_num_rows, block_num_cols); + } + + const Block< + Derived, + ScalarType + > block(int block_row_index, int block_col_index, + int block_num_rows, int block_num_cols) const { + assert(block_row_index + block_num_rows <= rows()); + assert(block_col_index + block_num_cols <= cols()); + return Block(static_cast(this), block_row_index, block_col_index, block_num_rows, block_num_cols); + } + + // TODO: head, tail + + // + // Transpose + // + Transpose transpose() { + return Transpose(static_cast(this)); + } + + const Transpose transpose() const { + return Transpose(static_cast(this)); + } + + template + Transpose transpose() { + return Transpose(static_cast(this)); + } +}; + +template +inline Derived operator*(const ScalarType& scalar, const MatrixBase &matrix) { + return matrix * scalar; +} + +template +inline Derived operator*(const MatrixBase &matrix, const ScalarType& scalar) { + return matrix * scalar; +} + +template +inline Derived operator/(const MatrixBase &matrix, const ScalarType& scalar) { + return matrix * (1.0 / scalar); +} + +// +// CommaInitializer +// +template +struct CommaInitializer { + typedef typename Derived::value_type value_type; + + private: + CommaInitializer() {} + + Derived *mParentMatrix; + unsigned int mRowIndex; + unsigned int mColIndex; + bool mElementWasAdded; + + public: + + CommaInitializer(Derived &matrix, const value_type &value) : + mParentMatrix(&matrix), + mRowIndex(0), + mColIndex(0), + mElementWasAdded(false) + { + assert (matrix.rows() > 0 && matrix.cols() > 0); + mParentMatrix->operator()(0,0) = value; + } + + CommaInitializer(Derived &matrix, unsigned int row_index, unsigned int col_index) : + mParentMatrix(&matrix), + mRowIndex(row_index), + mColIndex(col_index), + mElementWasAdded(false) + { + assert (matrix.rows() > 0 && matrix.cols() > 0); + } + + ~CommaInitializer() { + if (!mElementWasAdded + && (mColIndex + 1 < mParentMatrix->cols() + || mRowIndex + 1 < mParentMatrix->rows())) { + std::cerr + << "Error: too few elements passed to CommaInitializer Expected " + << mParentMatrix->rows() * mParentMatrix->cols() + << " but was given " + << mRowIndex * mParentMatrix->cols() + mColIndex + 1 << std::endl; + abort(); + } + } + + CommaInitializer operator, (const value_type &value) { + mColIndex++; + if (mColIndex >= mParentMatrix->cols()) { + mRowIndex++; + mColIndex = 0; + } + + if (mRowIndex == mParentMatrix->rows() && mColIndex == 0) { + std::cerr + << "Error: too many elements passed to CommaInitializer!Expected " + << mParentMatrix->rows() * mParentMatrix->cols() + << " but was given " + << mRowIndex *mParentMatrix->cols() + mColIndex + 1 << std::endl; + abort(); + } + (*mParentMatrix)(mRowIndex, mColIndex) = value; + mElementWasAdded = true; + + return CommaInitializer(*mParentMatrix, mRowIndex, mColIndex); + } +}; + +// +// Transpose +// +template +struct Transpose : public MatrixBase, ScalarType, NumRows, NumCols> { + typedef MatrixBase MatrixType; + Derived* mTransposeSource; + + Transpose(Derived* transpose_source) : + mTransposeSource(transpose_source) + { } + + Transpose(const Transpose &other) : + mTransposeSource(other.mTransposeSource) + { } + + template + Transpose& operator=(const OtherDerived& other) { + unsigned int i,j; + for (i = 0; i < rows(); i++) { + for (j = 0; j < cols(); j++) { + this->operator()(i,j) = other(i,j); + } + } + + return *this; + } + + size_t rows() const { + return static_cast(mTransposeSource)->cols(); + } + size_t cols() const { + return static_cast(mTransposeSource)->rows(); + } + + const ScalarType& operator()(const size_t& i, const size_t& j) const { + return static_cast(mTransposeSource)->operator()(j, i); + } + ScalarType& operator()(const size_t& i, const size_t& j) { + return static_cast(mTransposeSource)->operator()(j, i); + } +}; + +// +// Block +// +template +struct Block : public MatrixBase, ScalarType, NumRows, NumCols> { + typedef Block matrix_type; + + Derived* mBlockSource; + int row_index; + int col_index; + int nrows; + int ncols; + + Block(Derived* block_source, int row_index, int col_index) : + mBlockSource(block_source), + row_index(row_index), + col_index(col_index), + nrows(NumRows), ncols(NumCols) + { + static_assert(NumRows != -1 && NumCols != -1, "Invalid block specifications: unknown number of rows and columns!"); + } + + Block(Derived* block_source, int row_index, int col_index, int num_rows, int num_cols) : + mBlockSource(block_source), + row_index(row_index), + col_index(col_index), + nrows (num_rows), + ncols (num_cols) + { + } + + Block(const Block &other) : + mBlockSource(other.mBlockSource), + row_index(other.row_index), + col_index(other.col_index) + { } + + template + Derived& operator=(const OtherDerived& other) { + unsigned int i,j; + for (i = 0; i < rows(); i++) { + for (j = 0; j < cols(); j++) { + this->operator()(i,j) = other(i,j); + } + } + + return *mBlockSource; + } + + template + Dynamic operator*(const OtherDerived& other) { + Dynamic result (Dynamic::Zero(rows(), other.cols())); + + unsigned int i,j,k; + for (i = 0; i < rows(); i++) { + for (j = 0; j < other.cols(); j++) { + for (k = 0; k < other.rows(); k++) { + result (i,j) += operator()(i,k) * other(k,j); + } + } + } + return result; + } + + + size_t rows() const { + return nrows; + } + size_t cols() const { + return ncols; + } + + const ScalarType& operator()(const size_t& i, const size_t& j) const { + return static_cast(mBlockSource)->operator()(row_index + i, col_index + j); + } + ScalarType& operator()(const size_t& i, const size_t& j) { + return static_cast(mBlockSource)->operator()(row_index + i,col_index + j); + } + + template + Dynamic operator+(const OtherDerived& other) { + Dynamic result (*this); + result += other; + return result; + } + + private: + Block() { assert(0 && "Invalid call!"); }; + + ScalarType* data() { + assert("invalid call"); + return NULL; + } + + const ScalarType* data() const { + assert("invalid call"); + return NULL; + } +}; + +// +// Fixed Matrices (i.e. size defined at compile time) +// +template +struct Fixed : public MatrixBase, val_type, NumRows, NumCols> { + typedef Fixed matrix_type; + typedef val_type value_type; + + val_type mData[NumRows * NumCols]; + + Fixed() { + for (size_t i = 0; i < NumRows * NumCols; i++) { + mData[i] = static_cast (0.); + } + } + + Fixed (const Fixed &other) { + assert (NumRows == other.rows() && NumCols == other.cols() && "Error: matrix dimensions do not match!"); + memcpy (mData, other.data(), sizeof (val_type) * rows() * cols()); + } + + template + Fixed(const OtherDerived &other) { + std::cout << "FCC" << std::endl; + assert (other.rows() == NumRows && other.cols() == NumCols); + + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < cols(); j++) { + this->operator()(i,j) = other(i,j); + } + } + } + + template + Fixed& operator=(const OtherDerived& other) { + if (static_cast(this) != static_cast(&other)) { + std::cout << "Fixed AO" << std::endl; + + for (size_t i = 0; i < other.rows(); i++) { + for (size_t j = 0; j < other.cols(); j++) { + this->operator()(i,j) = other(i,j); + } + } + } + + return *this; + } + + explicit Fixed (size_t rows, size_t cols) { + assert (rows == NumRows); + assert (cols == cols); + } + + + // + // Constructor for vectors + // + + Fixed ( + const val_type& v0 + ) { + static_assert (NumRows * NumCols == 1); + + mData[0] = v0; + } + + Fixed ( + const val_type& v0, + const val_type& v1 + ) { + static_assert (NumRows * NumCols == 2); + + mData[0] = v0; + mData[1] = v1; + } + + Fixed ( + const val_type& v0, + const val_type& v1, + const val_type& v2 + ) { + static_assert (NumRows * NumCols == 3); + + mData[0] = v0; + mData[1] = v1; + mData[2] = v2; + } + + Fixed ( + const val_type& v0, + const val_type& v1, + const val_type& v2, + const val_type& v3 + ) { + static_assert (NumRows * NumCols == 4); + + mData[0] = v0; + mData[1] = v1; + mData[2] = v2; + mData[3] = v3; + } + + // + // Constructor for matrices + // + Fixed ( + const val_type& v00, + const val_type& v01, + const val_type& v02, + const val_type& v10, + const val_type& v11, + const val_type& v12, + const val_type& v20, + const val_type& v21, + const val_type& v22 + ) { + static_assert (NumRows == 3 && NumCols == 3); + + operator()(0,0) = v00; + operator()(0,1) = v01; + operator()(0,2) = v02; + + operator()(1,0) = v10; + operator()(1,1) = v11; + operator()(1,2) = v12; + + operator()(2,0) = v20; + operator()(2,1) = v21; + operator()(2,2) = v22; + } + + Fixed ( + const val_type& v00, + const val_type& v01, + const val_type& v02, + const val_type& v03, + const val_type& v10, + const val_type& v11, + const val_type& v12, + const val_type& v13, + const val_type& v20, + const val_type& v21, + const val_type& v22, + const val_type& v23, + const val_type& v30, + const val_type& v31, + const val_type& v32, + const val_type& v33 + ) { + static_assert (NumRows == 4 && NumCols == 4); + + operator()(0,0) = v00; + operator()(0,1) = v01; + operator()(0,2) = v02; + operator()(0,3) = v03; + + operator()(1,0) = v10; + operator()(1,1) = v11; + operator()(1,2) = v12; + operator()(1,3) = v13; + + operator()(2,0) = v20; + operator()(2,1) = v21; + operator()(2,2) = v22; + operator()(2,3) = v23; + + operator()(3,0) = v30; + operator()(3,1) = v31; + operator()(3,2) = v32; + operator()(3,3) = v33; + } + + + + template + Fixed& operator+=(const OtherDerived& other) { + assert (NumRows == other.rows() && NumCols == other.cols() && "Error: matrix dimensions do not match!"); + + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < cols(); j++) { + this->operator()(i,j) += other(i,j); + } + } + return *this; + } + + template + Fixed& operator-=(const OtherDerived& other) { + assert (NumRows == other.rows() && NumCols == other.cols() && "Error: matrix dimensions do not match!"); + + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < cols(); j++) { + this->operator()(i,j) -= other(i,j); + } + } + return *this; + } + + val_type& operator()(const size_t& i, const size_t& j) { + return mData[i*NumCols + j]; + } + + val_type* data() { + return mData; + } + + const val_type* data() const { + return mData; + } + + const val_type& operator()(const size_t& i, const size_t& j) const { + return mData[i*NumCols + j]; + } + + size_t cols() const { + return NumCols; + } + + size_t rows() const { + return NumRows; + } +}; + +// +// Dynamic Matrices +// +template +struct Dynamic : public MatrixBase, ScalarType, -1, -1> { + typedef Dynamic matrix_type; + + ScalarType* mData; + int mNumRows; + int mNumCols; + + Dynamic() : + mData (NULL), + mNumRows (0), + mNumCols (0) + { + } + + Dynamic(int num_rows, int num_cols) : + mNumRows (num_rows), + mNumCols (num_cols) { + mData = new ScalarType[mNumRows * mNumCols]; + + for (size_t i = 0; i < (size_t) mNumRows * (size_t) mNumCols; i++) { + mData[i] = static_cast (0.); + } + } + + ~Dynamic() { + if (mData != NULL) { + delete[] mData; + mData = NULL; + } + } + + Dynamic(const Dynamic &other) : + mNumRows (other.rows()), + mNumCols (other.cols()) { + mData = new ScalarType[mNumRows * mNumCols]; + assert (mNumRows == (int) other.rows() && mNumCols == (int) other.cols() && "Error: matrix dimensions do not match!"); + memcpy (mData, other.data(), sizeof (ScalarType) * rows() * cols()); + } + + template + Dynamic(const OtherDerived &other) : + mNumRows (other.rows()), + mNumCols (other.cols()) { + mData = new ScalarType[mNumRows * mNumCols]; + assert (mNumRows == other.rows() && mNumCols == other.cols() && "Error: matrix dimensions do not match!"); + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < cols(); j++) { + this->operator()(i,j) = other(i,j); + } + } + } + + Dynamic& operator=(const Dynamic &other) { + if (static_cast(this) != static_cast(&other)) { + assert (mData != NULL); + delete[] mData; + + mNumRows = other.rows(); + mNumCols = other.cols(); + mData = new ScalarType[mNumRows * mNumCols]; + + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < cols(); j++) { + this->operator()(i,j) = other(i,j); + } + } + } + + return *this; + } + + template + Dynamic& operator=(const OtherDerived &other) { + if (static_cast(this) != static_cast(&other)) { + assert (mData != NULL); + delete[] mData; + + mNumRows = other.rows(); + mNumCols = other.cols(); + mData = new ScalarType[mNumRows * mNumCols]; + + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < cols(); j++) { + this->operator()(i,j) = other(i,j); + } + } + } + + return *this; + } + + template + Dynamic& operator+=(const OtherDerived& other) { + assert (mNumRows == (int) other.rows() && mNumCols == (int) other.cols() && "Error: matrix dimensions do not match!"); + + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < cols(); j++) { + this->operator()(i,j) += other(i,j); + } + } + + return *this; + } + + template + Dynamic& operator-=(const OtherDerived& other) { + assert (mNumRows == (int) other.rows() && mNumCols == (int) other.cols() && "Error: matrix dimensions do not match!"); + + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < cols(); j++) { + this->operator()(i,j) -= other(i,j); + } + } + + return *this; + } + + ScalarType& operator[](const size_t& i) { + return mData[i]; + } + + ScalarType& operator()(const size_t& i, const size_t& j) { + return mData[i*cols() + j]; + } + + const ScalarType& operator()(const size_t& i, const size_t& j) const { + return mData[i*cols() + j]; + } + + ScalarType* data() { + return mData; + } + + const ScalarType* data() const { + return mData; + } + + size_t cols() const { + return mNumCols; + } + + size_t rows() const { + return mNumRows; + } +}; + +// +// QR Decomposition +// + template + class HouseholderQR { + public: + typedef typename matrix_type::value_type value_type; + HouseholderQR() : + mIsFactorized(false) + {} + + private: + typedef Dynamic VectorXd; + typedef Dynamic MatrixXXd; + + bool mIsFactorized; + MatrixXXd mQ; + MatrixXXd mR; + + public: + HouseholderQR(const matrix_type &matrix) : + mIsFactorized(false), + mQ(matrix.rows(), matrix.rows()) + { + compute(matrix); + } + HouseholderQR compute(const matrix_type &matrix) { + mR = matrix; + mQ = Dynamic::Identity (mR.rows(), mR.rows()); + + for (unsigned int i = 0; i < mR.cols(); i++) { + unsigned int block_rows = mR.rows() - i; + unsigned int block_cols = mR.cols() - i; + + MatrixXXd current_block = mR.block(i,i, block_rows, block_cols); + VectorXd column = current_block.block(0, 0, block_rows, 1); + + value_type alpha = - column.norm(); + if (current_block(0,0) < 0) { + alpha = - alpha; + } + + VectorXd v = current_block.block(0, 0, block_rows, 1); + v[0] = v[0] - alpha; + + MatrixXXd Q (MatrixXXd::Identity(mR.rows(), mR.rows())); + Q.block(i, i, block_rows, block_rows) = MatrixXXd (Q.block(i, i, block_rows, block_rows)) + - MatrixXXd(v * v.transpose() / (v.squaredNorm() * 0.5)); + + mR = Q * mR; + + // Normalize so that we have positive diagonal elements + if (mR(i,i) < 0) { + mR.block(i,i,block_rows, block_cols) = MatrixXXd(mR.block(i,i,block_rows, block_cols)) * -1.; + Q.block(i,i,block_rows, block_rows) = MatrixXXd(Q.block(i,i,block_rows, block_rows)) * -1.; + } + + mQ = mQ * Q; + } + + mIsFactorized = true; + + return *this; + } + Dynamic solve ( + const Dynamic &rhs + ) const { + assert (mIsFactorized); + + VectorXd y = mQ.transpose() * rhs; + VectorXd x = VectorXd::Zero(mR.cols()); + + for (int i = mR.cols() - 1; i >= 0; --i) { + value_type z = y[i]; + + for (unsigned int j = i + 1; j < mR.cols(); j++) { + z = z - x[j] * mR(i,j); + } + + if (fabs(mR(i,i)) < std::numeric_limits::epsilon() * 10) { + std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero!" << std::endl; + abort(); + } + x[i] = z / mR(i,i); + } + + return x; + } + Dynamic inverse() const { + assert (mIsFactorized); + + VectorXd rhs_temp = VectorXd::Zero(mQ.cols()); + MatrixXXd result (mQ.cols(), mQ.cols()); + + for (unsigned int i = 0; i < mQ.cols(); i++) { + rhs_temp[i] = 1.; + + result.block(0, i, mQ.cols(), 1) = solve(rhs_temp); + + rhs_temp[i] = 0.; + } + + return result; + } + Dynamic householderQ () const { + return mQ; + } + Dynamic matrixR () const { + return mR; + } + }; + +template +class ColPivHouseholderQR { + public: + typedef typename matrix_type::value_type value_type; + private: + typedef Dynamic MatrixXXd; + typedef Dynamic VectorXd; + + bool mIsFactorized; + MatrixXXd mQ; + MatrixXXd mR; + unsigned int *mPermutations; + value_type mThreshold; + unsigned int mRank; + + public: + ColPivHouseholderQR(): + mIsFactorized(false) { + mPermutations = new unsigned int[1]; + } + + ColPivHouseholderQR (const ColPivHouseholderQR& other) { + mIsFactorized = other.mIsFactorized; + mQ = other.mQ; + mR = other.mR; + mPermutations = new unsigned int[mQ.cols()]; + mThreshold = other.mThreshold; + mRank = other.mRank; + } + + ColPivHouseholderQR& operator= (const ColPivHouseholderQR& other) { + if (this != &other) { + mIsFactorized = other.mIsFactorized; + mQ = other.mQ; + mR = other.mR; + delete[] mPermutations; + mPermutations = new unsigned int[mQ.cols()]; + mThreshold = other.mThreshold; + mRank = other.mRank; + } + + return *this; + } + + ColPivHouseholderQR(const matrix_type &matrix) : + mIsFactorized(false), + mQ(matrix.rows(), matrix.rows()), + mThreshold (std::numeric_limits::epsilon() * matrix.cols()) { + mPermutations = new unsigned int [matrix.cols()]; + for (unsigned int i = 0; i < matrix.cols(); i++) { + mPermutations[i] = i; + } + compute(matrix); + } + ~ColPivHouseholderQR() { + delete[] mPermutations; + } + + ColPivHouseholderQR& setThreshold (const value_type& threshold) { + mThreshold = threshold; + + return *this; + } + ColPivHouseholderQR& compute(const matrix_type &matrix) { + mR = matrix; + mQ = Dynamic::Identity (mR.rows(), mR.rows()); + + for (unsigned int i = 0; i < mR.cols(); i++) { + unsigned int block_rows = mR.rows() - i; + unsigned int block_cols = mR.cols() - i; + + // find and swap the column with the highest norm + unsigned int col_index_norm_max = i; + value_type col_norm_max = VectorXd(mR.block(i,i, block_rows, 1)).squaredNorm(); + + for (unsigned int j = i + 1; j < mR.cols(); j++) { + VectorXd column = mR.block(i, j, block_rows, 1); + + if (column.squaredNorm() > col_norm_max) { + col_index_norm_max = j; + col_norm_max = column.squaredNorm(); + } + } + + if (col_norm_max < mThreshold) { + // if all entries of the column is close to zero, we bail out + break; + } + + + if (col_index_norm_max != i) { + VectorXd temp_col = mR.block(0, i, mR.rows(), 1); + mR.block(0,i,mR.rows(),1) = mR.block(0, col_index_norm_max, mR.rows(), 1); + mR.block(0, col_index_norm_max, mR.rows(), 1) = temp_col; + + unsigned int temp_index = mPermutations[i]; + mPermutations[i] = mPermutations[col_index_norm_max]; + mPermutations[col_index_norm_max] = temp_index; + } + + MatrixXXd current_block = mR.block(i,i, block_rows, block_cols); + VectorXd column = current_block.block(0, 0, block_rows, 1); + + value_type alpha = - column.norm(); + if (current_block(0,0) < 0) { + alpha = - alpha; + } + + VectorXd v = current_block.block(0, 0, block_rows, 1); + v[0] = v[0] - alpha; + + MatrixXXd Q (MatrixXXd::Identity(mR.rows(), mR.rows())); + + Q.block(i, i, block_rows, block_rows) = MatrixXXd (Q.block(i, i, block_rows, block_rows)) + - MatrixXXd(v * v.transpose() / (v.squaredNorm() * static_cast(0.5))); + + mR = Q * mR; + + // Normalize so that we have positive diagonal elements + if (mR(i,i) < 0) { + mR.block(i,i,block_rows, block_cols) = MatrixXXd(mR.block(i,i,block_rows, block_cols)) * -1.; + Q.block(i,i,block_rows, block_rows) = MatrixXXd(Q.block(i,i,block_rows, block_rows)) * -1.; + } + + mQ = mQ * Q; + } + + mIsFactorized = true; + + return *this; + } + Dynamic solve ( + const Dynamic &rhs + ) const { + assert (mIsFactorized); + + VectorXd y = mQ.transpose() * rhs; + VectorXd x = VectorXd::Zero(mR.cols()); + + for (int i = mR.cols() - 1; i >= 0; --i) { + value_type z = y[i]; + + for (unsigned int j = i + 1; j < mR.cols(); j++) { + z = z - x[mPermutations[j]] * mR(i,j); + } + + if (fabs(mR(i,i)) < std::numeric_limits::epsilon() * 10) { + std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero!" << std::endl; + abort(); + } + x[mPermutations[i]] = z / mR(i,i); + } + + return x; + } + Dynamic inverse() const { + assert (mIsFactorized); + + VectorXd rhs_temp = VectorXd::Zero(mQ.cols()); + MatrixXXd result (mQ.cols(), mQ.cols()); + + for (unsigned int i = 0; i < mQ.cols(); i++) { + rhs_temp[i] = 1.; + + result.block(0, i, mQ.cols(), 1) = solve(rhs_temp); + + rhs_temp[i] = 0.; + } + + return result; + } + + Dynamic householderQ () const { + return mQ; + } + Dynamic matrixR () const { + return mR; + } + Dynamic matrixP () const { + MatrixXXd P = MatrixXXd::Identity(mR.cols(), mR.cols()); + MatrixXXd identity = MatrixXXd::Identity(mR.cols(), mR.cols()); + for (unsigned int i = 0; i < mR.cols(); i++) { + P.block(0,i,mR.cols(),1) = identity.block(0,mPermutations[i], mR.cols(), 1); + } + return P; + } + + unsigned int rank() const { + value_type abs_threshold = fabs(mR(0,0)) * mThreshold; + + for (unsigned int i = 1; i < mR.cols(); i++) { + if (fabs(mR(i,i)) < abs_threshold) + return i; + } + + return mR.cols(); + } + }; + + +// +// OpenGL Matrices and Quaternions +// + +typedef Fixed Vector3f; +typedef Fixed Matrix33f; + +typedef Fixed Vector4f; +typedef Fixed Matrix44f; + +inline Matrix33f RotateMat33 (float rot_deg, float x, float y, float z) { + float c = cosf (rot_deg * M_PI / 180.f); + float s = sinf (rot_deg * M_PI / 180.f); + return Matrix33f ( + x * x * (1.0f - c) + c, + y * x * (1.0f - c) + z * s, + x * z * (1.0f - c) - y * s, + + x * y * (1.0f - c) - z * s, + y * y * (1.0f - c) + c, + y * z * (1.0f - c) + x * s, + + x * z * (1.0f - c) + y * s, + y * z * (1.0f - c) - x * s, + z * z * (1.0f - c) + c + + ); +} + + +inline Matrix44f RotateMat44 (float rot_deg, float x, float y, float z) { + float c = cosf (rot_deg * M_PI / 180.f); + float s = sinf (rot_deg * M_PI / 180.f); + return Matrix44f ( + x * x * (1.0f - c) + c, + y * x * (1.0f - c) + z * s, + x * z * (1.0f - c) - y * s, + 0.f, + + x * y * (1.0f - c) - z * s, + y * y * (1.0f - c) + c, + y * z * (1.0f - c) + x * s, + 0.f, + + x * z * (1.0f - c) + y * s, + y * z * (1.0f - c) - x * s, + z * z * (1.0f - c) + c, + 0.f, + + 0.f, 0.f, 0.f, 1.f + ); +} + +inline Matrix44f TranslateMat44 (float x, float y, float z) { + return Matrix44f ( + 1.f, 0.f, 0.f, 0.f, + 0.f, 1.f, 0.f, 0.f, + 0.f, 0.f, 1.f, 0.f, + x, y, z, 1.f + ); +} + +inline Matrix44f ScaleMat44 (float x, float y, float z) { + return Matrix44f ( + x, 0.f, 0.f, 0.f, + 0.f, y, 0.f, 0.f, + 0.f, 0.f, z, 0.f, + 0.f, 0.f, 0.f, 1.f + ); +} + +inline Matrix44f Ortho( + float left, float right, + float bottom, float top, + float near, float far) { + float tx = -(right + left) / (right - left); + float ty = -(top + bottom) / (top - bottom); + float tz = -(far + near) / (far - near); + return Matrix44f( + 2.0f / (right - left), 0.0f, 0.0f, 0.0f, + 0, 2.0f / (top - bottom), 0.0f, 0.0f, + 0.0f, 0.0f, -2.0f / (far - near), 0.0f, + tx, ty, tz, 1.0f + ); +} + +inline Matrix44f Perspective(float fovy, float aspect, + float near, float far) { + float x = (fovy * M_PI / 180.0) / 2.0f; + float f = cos(x) / sin(x); + + return Matrix44f( + f / aspect, 0.0f, 0.0f, 0.0f, + 0.0f, f, 0.0f, 0.0f, + 0.0f, 0.0f, (far + near) / (near - far), -1.0f, + 0.0f, 0.0f, (2.0f * far * near) / (near - far), 0.0f + ); +} + +inline Matrix44f Frustum(float left, float right, + float bottom, float top, + float near, float far) { + float A = (right + left) / (right - left); + float B = (top + bottom) / (top - bottom); + float C = -(far + near) / (far - near); + float D = - (2.0f * far * near) / (far - near); + + return Matrix44f( + 2.0f * near / (right - left), 0.0f, 0.0f, 0.0f, + 0.0f, 2.0f * near / (top - bottom), 0.0f, 0.0f, + A, B, C, -1.0f, + 0.0f, 0.0f, D, 0.0f + ); +} + +inline Matrix44f LookAt( + const Vector3f& eye, + const Vector3f& poi, + const Vector3f& up) { + Vector3f d = (poi - eye).normalized(); + Vector3f s = d.cross(up.normalized()).normalized(); + Vector3f u = s.cross(d).normalized(); + + return TranslateMat44(-eye[0], -eye[1], -eye[2]) * Matrix44f( + s[0], u[0], -d[0], 0.0f, + s[1], u[1], -d[1], 0.0f, + s[2], u[2], -d[2], 0.0f, + 0.0f, 0.0f, 0.0f, 1.0f + ); +} + +/** Quaternion + * + * order: x,y,z,w + */ +class Quaternion : public Vector4f { + public: + Quaternion () : + Vector4f (0.f, 0.f, 0.f, 1.f) + {} + Quaternion (const Vector4f vec4) : + Vector4f (vec4) + {} + Quaternion (float x, float y, float z, float w): + Vector4f (x, y, z, w) + {} + /** This function is equivalent to multiplicate their corresponding rotation matrices */ + Quaternion operator* (const Quaternion &q) const { + return Quaternion ( + (*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1], + (*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2], + (*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0], + (*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2] + ); + } + Quaternion& operator*=(const Quaternion &q) { + set ( + (*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1], + (*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2], + (*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0], + (*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2] + ); + return *this; + } + + static Quaternion fromGLRotate (float angle, float x, float y, float z) { + float st = sinf (angle * M_PI / 360.f); + return Quaternion ( + st * x, + st * y, + st * z, + cosf (angle * M_PI / 360.f) + ); + } + + Quaternion normalize() { + return Vector4f::normalize(); + } + + Quaternion slerp (float alpha, const Quaternion &quat) const { + // check whether one of the two has 0 length + float s = sqrt (squaredNorm() * quat.squaredNorm()); + + // division by 0.f is unhealthy! + assert (s != 0.f); + + float angle = acos (dot(quat) / s); + if (angle == 0.f || std::isnan(angle)) { + return *this; + } + assert(!std::isnan(angle)); + + float d = 1.f / sinf (angle); + float p0 = sinf ((1.f - alpha) * angle); + float p1 = sinf (alpha * angle); + + if (dot (quat) < 0.f) { + return Quaternion( ((*this) * p0 - quat * p1) * d); + } + return Quaternion( ((*this) * p0 + quat * p1) * d); + } + + Matrix44f toGLMatrix() const { + float x = (*this)[0]; + float y = (*this)[1]; + float z = (*this)[2]; + float w = (*this)[3]; + return Matrix44f ( + 1 - 2*y*y - 2*z*z, + 2*x*y + 2*w*z, + 2*x*z - 2*w*y, + 0.f, + + 2*x*y - 2*w*z, + 1 - 2*x*x - 2*z*z, + 2*y*z + 2*w*x, + 0.f, + + 2*x*z + 2*w*y, + 2*y*z - 2*w*x, + 1 - 2*x*x - 2*y*y, + 0.f, + + 0.f, + 0.f, + 0.f, + 1.f); + } + + static Quaternion fromGLMatrix(const Matrix44f &mat) { + float w = sqrt (1.f + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5f; + return Quaternion ( + -(mat(2,1) - mat(1,2)) / (w * 4.f), + -(mat(0,2) - mat(2,0)) / (w * 4.f), + -(mat(1,0) - mat(0,1)) / (w * 4.f), + w); + } + + static Quaternion fromMatrix (const Matrix33f &mat) { + float w = sqrt (1.f + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5f; + return Quaternion ( + (mat(2,1) - mat(1,2)) / (w * 4.f), + (mat(0,2) - mat(2,0)) / (w * 4.f), + (mat(1,0) - mat(0,1)) / (w * 4.f), + w); + } + + static Quaternion fromAxisAngle (const Vector3f &axis, double angle_rad) { + double d = axis.norm(); + double s2 = std::sin (angle_rad * 0.5) / d; + return Quaternion ( + axis[0] * s2, + axis[1] * s2, + axis[2] * s2, + std::cos(angle_rad * 0.5) + ); + } + + static Quaternion fromEulerZYX (const Vector3f &zyx_angles) { + return Quaternion::fromAxisAngle (Vector3f (0., 0., 1.), zyx_angles[0]) + * Quaternion::fromAxisAngle (Vector3f (0., 1., 0.), zyx_angles[1]) + * Quaternion::fromAxisAngle (Vector3f (1., 0., 0.), zyx_angles[2]); + } + + static Quaternion fromEulerYXZ (const Vector3f &yxz_angles) { + return Quaternion::fromAxisAngle (Vector3f (0., 1., 0.), yxz_angles[0]) + * Quaternion::fromAxisAngle (Vector3f (1., 0., 0.), yxz_angles[1]) + * Quaternion::fromAxisAngle (Vector3f (0., 0., 1.), yxz_angles[2]); + } + + static Quaternion fromEulerXYZ (const Vector3f &xyz_angles) { + return Quaternion::fromAxisAngle (Vector3f (0., 0., 01.), xyz_angles[2]) + * Quaternion::fromAxisAngle (Vector3f (0., 1., 0.), xyz_angles[1]) + * Quaternion::fromAxisAngle (Vector3f (1., 0., 0.), xyz_angles[0]); + } + + Vector3f toEulerZYX () const { + return Vector3f (1.0f, 2.0f, 3.0f + ); + } + + Vector3f toEulerYXZ() const { + return Vector3f ( + atan2 (-2.f * (*this)[0] * (*this)[2] + 2.f * (*this)[3] * (*this)[1], + (*this)[2] * (*this)[2] - (*this)[1] * (*this)[1] + -(*this)[0] * (*this)[0] + (*this)[3] * (*this)[3]), + asin (2.f * (*this)[1] * (*this)[2] + 2.f * (*this)[3] * (*this)[0]), + atan2 (-2.f * (*this)[0] * (*this)[1] + 2.f * (*this)[3] * (*this)[2], + (*this)[1] * (*this)[1] - (*this)[2] * (*this)[2] + +(*this)[3] * (*this)[3] - (*this)[0] * (*this)[0] + ) + ); + } + + Matrix33f toMatrix() const { + float x = (*this)[0]; + float y = (*this)[1]; + float z = (*this)[2]; + float w = (*this)[3]; + return Matrix33f ( + 1 - 2*y*y - 2*z*z, + 2*x*y - 2*w*z, + 2*x*z + 2*w*y, + + 2*x*y + 2*w*z, + 1 - 2*x*x - 2*z*z, + 2*y*z - 2*w*x, + + 2*x*z - 2*w*y, + 2*y*z + 2*w*x, + 1 - 2*x*x - 2*y*y + ); + } + + Quaternion conjugate() const { + return Quaternion ( + -(*this)[0], + -(*this)[1], + -(*this)[2], + (*this)[3]); + } + + Vector3f rotate (const Vector3f &vec) const { + Vector3f vn (vec); + Quaternion vec_quat (vn[0], vn[1], vn[2], 0.f), res_quat; + + res_quat = (*this) * vec_quat; + res_quat = res_quat * conjugate(); + + return Vector3f (res_quat[0], res_quat[1], res_quat[2]); + } +}; + + +// +// Stream operators +// +template +inline std::ostream& operator<<(std::ostream& output, const MatrixBase &matrix) { + size_t max_width = 0; + size_t out_width = output.width(); + + // get the widest number + for (size_t i = 0; i < matrix.rows(); i++) { + for (size_t j = 0; j < matrix.cols(); j++) { + std::stringstream out_stream; + out_stream << matrix(i,j); + max_width = std::max (out_stream.str().size(),max_width); + } + } + + // overwrite width if it was explicitly prescribed + if (out_width != 0) { + max_width = out_width; + } + + for (unsigned int i = 0; i < matrix.rows(); i++) { + output.width(0); + output << "[ "; + output.width(out_width); + for (unsigned int j = 0; j < matrix.cols(); j++) { + std::stringstream out_stream; + out_stream.width (max_width); + out_stream << matrix(i,j); + output << out_stream.str(); + + if (j < matrix.cols() - 1) + output << ", "; + } + output << " ]"; + + if (matrix.rows() > 1 && i < matrix.rows() - 1) + output << std::endl; + } + return output; +} + +} diff --git a/src/SimpleMath/SimpleMath.h.orig b/src/SimpleMath/SimpleMath.h.orig deleted file mode 100644 index 4fd753f..0000000 --- a/src/SimpleMath/SimpleMath.h.orig +++ /dev/null @@ -1,24 +0,0 @@ -#ifndef _SIMPLEMATH_H -#define _SIMPLEMATH_H - -#include "SimpleMathFixed.h" -#include "SimpleMathDynamic.h" -#include "SimpleMathMixed.h" -#include "SimpleMathQR.h" -#include "SimpleMathCommaInitializer.h" - -typedef SimpleMath::Fixed::Matrix Vector3i; - -typedef SimpleMath::Fixed::Matrix Vector3d; -typedef SimpleMath::Fixed::Matrix Matrix33d; - -typedef SimpleMath::Fixed::Matrix Vector4d; - -typedef SimpleMath::Fixed::Matrix Vector3f; -typedef SimpleMath::Fixed::Matrix Vector4f; -typedef SimpleMath::Fixed::Matrix Matrix33f; -typedef SimpleMath::Fixed::Matrix Matrix44f; - -typedef SimpleMath::Dynamic::Matrix VectorNd; - -#endif /* _SIMPLEMATH_H */ diff --git a/src/SimpleMath/SimpleMathBlock.h b/src/SimpleMath/SimpleMathBlock.h deleted file mode 100644 index cb2e472..0000000 --- a/src/SimpleMath/SimpleMathBlock.h +++ /dev/null @@ -1,194 +0,0 @@ -/** - * This is a highly inefficient math library. It was conceived by Martin - * Felis while he was compiling code - * that uses a highly efficient math library. - * - * It is intended to be used as a fast compiling substitute for the - * blazingly fast Eigen3 library and tries to mimic its API to a certain - * extend. - * - * Feel free to use it wherever you like. However, no guarantees are given - * that this code does what it says it would. - */ - -#ifndef SIMPLEMATHBLOCK_H -#define SIMPLEMATHBLOCK_H - -#include -#include -#include -#include - -#include "compileassert.h" - -// #include "SimpleMathQR.h" - -/** \brief Namespace for a highly inefficient math library - * - */ -namespace SimpleMath { - -/** \brief Namespace for fixed size elements - */ -// forward declaration -template -class Matrix; - -template -class Block { - public: - typedef val_type value_type; - - Block() : - mParentRows(0), - mParentCols(0), - mParentRowStart(0), - mParentColStart(0) - { } - Block (const matrix_type &matrix, const unsigned int row_start, const unsigned int col_start, const unsigned int row_count, const unsigned int col_count) : - mParentRows (matrix.rows()), - mParentCols (matrix.cols()), - mParentRowStart (row_start), - mParentColStart (col_start), - mRowCount (row_count), - mColCount (col_count), - mTransposed (false) { - assert (mParentRows >= mParentRowStart + mRowCount); - assert (mParentCols >= mParentColStart + mColCount); - - // without the following line we could not create blocks from const - // matrices - mParentMatrix = const_cast(&matrix); - } - - // copy data from the other block into this - Block& operator=(const Block &other) { - if (this != &other) { - if (mRowCount != other.rows() || mColCount != other.cols()) { - std::cerr << "Error: cannot assign blocks of different size (left is " << mRowCount << "x" << mColCount << " right is " << other.rows() << "x" << other.cols() << ")!" << std::endl; - abort(); - } - - value_type* temp_values = new value_type [mRowCount * mColCount]; - - for (unsigned int i = 0; i < mRowCount; i++) { - for (unsigned int j = 0; j < mColCount; j++) { - temp_values[i * mColCount + j] = static_cast(other(i,j)); - } - } - - for (unsigned int i = 0; i < mRowCount; i++) { - for (unsigned int j = 0; j < mColCount; j++) { - (*this)(i,j) = temp_values[i * mColCount + j]; - } - } - - delete[] temp_values; - } - - return *this; - } - - template - // copy data from the other block into this - Block& operator=(const other_matrix_type &other) { - if (mRowCount != other.rows() || mColCount != other.cols()) { - std::cerr << "Error: cannot assign blocks of different size (left is " << mRowCount << "x" << mColCount << " right is " << other.rows() << "x" << other.cols() << ")!" << std::endl; - abort(); - } - - value_type *temp_values = new value_type[mRowCount * mColCount]; - - for (unsigned int i = 0; i < mRowCount; i++) { - for (unsigned int j = 0; j < mColCount; j++) { - temp_values[i * mColCount + j] = static_cast(other(i,j)); - } - } - - for (unsigned int i = 0; i < mRowCount; i++) { - for (unsigned int j = 0; j < mColCount; j++) { - (*this)(i,j) = temp_values[i * mColCount + j]; - } - } - - delete[] temp_values; - - return *this; - } - - unsigned int rows() const { - if (!mTransposed) - return mRowCount; - - return mColCount; - } - unsigned int cols() const { - if (!mTransposed) - return mColCount; - - return mRowCount; - } - const val_type& operator() (const unsigned int i, const unsigned int j) const { - - if (!mTransposed) { - assert (i < mRowCount); - assert (j < mColCount); - return (*mParentMatrix) (i + mParentRowStart, j + mParentColStart); - } - - return (*mParentMatrix) (j + mParentRowStart, i + mParentColStart); - } - - val_type& operator() (const unsigned int i, const unsigned int j) { - if (!mTransposed) { - assert (i < mRowCount); - assert (j < mColCount); - return (*mParentMatrix) (i + mParentRowStart, j + mParentColStart); - } - - assert (j < mRowCount); - assert (i < mColCount); - return (*mParentMatrix) (j + mParentRowStart, i + mParentColStart); - } - - Block transpose() const { - Block result (*this); - result.mTransposed = mTransposed ^ true; - return result; - } - - private: - matrix_type *mParentMatrix; - const unsigned int mParentRows; - const unsigned int mParentCols; - const unsigned int mParentRowStart; - const unsigned int mParentColStart; - const unsigned int mRowCount; - const unsigned int mColCount; - bool mTransposed; -}; - -template -inline std::ostream& operator<<(std::ostream& output, const Block &block) { - unsigned int i,j; - for (i = 0; i < block.rows(); i++) { - output << "[ "; - for (j = 0; j < block.cols(); j++) { - output << block(i,j); - - if (j < block.cols() - 1) - output << ", "; - } - output << " ]"; - - if (block.rows() > 1 && i < block.rows() - 1) - output << std::endl; - } - - return output; -} - - -} - -#endif /* SIMPLEMATHBLOCK_H */ diff --git a/src/SimpleMath/SimpleMathBlock.h.orig b/src/SimpleMath/SimpleMathBlock.h.orig deleted file mode 100644 index 93753a1..0000000 --- a/src/SimpleMath/SimpleMathBlock.h.orig +++ /dev/null @@ -1,220 +0,0 @@ -/** - * This is a highly inefficient math library. It was conceived by Martin - * Felis while he was compiling code - * that uses a highly efficient math library. - * - * It is intended to be used as a fast compiling substitute for the - * blazingly fast Eigen3 library and tries to mimic its API to a certain - * extend. - * - * Feel free to use it wherever you like. However, no guarantees are given - * that this code does what it says it would. - */ - -#ifndef SIMPLEMATHBLOCK_H -#define SIMPLEMATHBLOCK_H - -#include -#include -#include -#include - -#include "compileassert.h" - -// #include "SimpleMathQR.h" - -/** \brief Namespace for a highly inefficient math library - * - */ -namespace SimpleMath { - -/** \brief Namespace for fixed size elements - */ -// forward declaration -template -class Matrix; - -template -class Block { - public: - typedef val_type value_type; - - Block() : - mParentRows(0), - mParentCols(0), - mParentRowStart(0), - mParentColStart(0) - { } - Block (const matrix_type &matrix, const unsigned int row_start, const unsigned int col_start, const unsigned int row_count, const unsigned int col_count) : - mParentRows (matrix.rows()), - mParentCols (matrix.cols()), - mParentRowStart (row_start), - mParentColStart (col_start), - mRowCount (row_count), - mColCount (col_count), - mTransposed (false) { - assert (mParentRows >= mParentRowStart + mRowCount); - assert (mParentCols >= mParentColStart + mColCount); - - // without the following line we could not create blocks from const - // matrices - mParentMatrix = const_cast(&matrix); - } - - // copy data from the other block into this - Block& operator=(const Block &other) { - if (this != &other) { - if (mRowCount != other.rows() || mColCount != other.cols()) { - std::cerr << "Error: cannot assign blocks of different size (left is " << mRowCount << "x" << mColCount << " right is " << other.rows() << "x" << other.cols() << ")!" << std::endl; - abort(); - } - - value_type* temp_values = new value_type [mRowCount * mColCount]; - - for (unsigned int i = 0; i < mRowCount; i++) { - for (unsigned int j = 0; j < mColCount; j++) { - temp_values[i * mColCount + j] = static_cast(other(i,j)); - } - } - - for (unsigned int i = 0; i < mRowCount; i++) { - for (unsigned int j = 0; j < mColCount; j++) { - (*this)(i,j) = temp_values[i * mColCount + j]; - } - } - - delete[] temp_values; - } - - return *this; - } - - template - // copy data from the other block into this - Block& operator=(const other_matrix_type &other) { - if (mRowCount != other.rows() || mColCount != other.cols()) { - std::cerr << "Error: cannot assign blocks of different size (left is " << mRowCount << "x" << mColCount << " right is " << other.rows() << "x" << other.cols() << ")!" << std::endl; - abort(); - } - - value_type *temp_values = new value_type[mRowCount * mColCount]; - - for (unsigned int i = 0; i < mRowCount; i++) { - for (unsigned int j = 0; j < mColCount; j++) { - temp_values[i * mColCount + j] = static_cast(other(i,j)); - } - } - - for (unsigned int i = 0; i < mRowCount; i++) { - for (unsigned int j = 0; j < mColCount; j++) { - (*this)(i,j) = temp_values[i * mColCount + j]; - } - } - - delete[] temp_values; - - return *this; - } - - template - other_matrix_type operator+(const other_matrix_type& other) { - other_matrix_type result (this); - - for (unsigned int i = 0; i < mRowCount; i++) { - for (unsigned int j = 0; j < mColCount; j++) { - result += other(i,j); - } - } - - return result; - } - - template - other_matrix_type operator-(const other_matrix_type& other) { - other_matrix_type result (this); - - for (unsigned int i = 0; i < mRowCount; i++) { - for (unsigned int j = 0; j < mColCount; j++) { - result -= other(i,j); - } - } - - return result; - } - - unsigned int rows() const { - if (!mTransposed) - return mRowCount; - - return mColCount; - } - unsigned int cols() const { - if (!mTransposed) - return mColCount; - - return mRowCount; - } - const val_type& operator() (const unsigned int i, const unsigned int j) const { - - if (!mTransposed) { - assert (i < mRowCount); - assert (j < mColCount); - return (*mParentMatrix) (i + mParentRowStart, j + mParentColStart); - } - - return (*mParentMatrix) (j + mParentRowStart, i + mParentColStart); - } - - val_type& operator() (const unsigned int i, const unsigned int j) { - if (!mTransposed) { - assert (i < mRowCount); - assert (j < mColCount); - return (*mParentMatrix) (i + mParentRowStart, j + mParentColStart); - } - - assert (j < mRowCount); - assert (i < mColCount); - return (*mParentMatrix) (j + mParentRowStart, i + mParentColStart); - } - - Block transpose() const { - Block result (*this); - result.mTransposed = mTransposed ^ true; - return result; - } - - private: - matrix_type *mParentMatrix; - const unsigned int mParentRows; - const unsigned int mParentCols; - const unsigned int mParentRowStart; - const unsigned int mParentColStart; - const unsigned int mRowCount; - const unsigned int mColCount; - bool mTransposed; -}; - -template -inline std::ostream& operator<<(std::ostream& output, const Block &block) { - unsigned int i,j; - for (i = 0; i < block.rows(); i++) { - output << "[ "; - for (j = 0; j < block.cols(); j++) { - output << block(i,j); - - if (j < block.cols() - 1) - output << ", "; - } - output << " ]"; - - if (block.rows() > 1 && i < block.rows() - 1) - output << std::endl; - } - - return output; -} - - -} - -#endif /* SIMPLEMATHBLOCK_H */ diff --git a/src/SimpleMath/SimpleMathCholesky.h b/src/SimpleMath/SimpleMathCholesky.h deleted file mode 100644 index b51a361..0000000 --- a/src/SimpleMath/SimpleMathCholesky.h +++ /dev/null @@ -1,94 +0,0 @@ -#ifndef _SIMPLE_MATH_CHOLESKY_H -#define _SIMPLE_MATH_CHOLESKY_H - -#include -#include - -#include "SimpleMathDynamic.h" - -namespace SimpleMath { - - template - class LLT { - public: - typedef typename matrix_type::value_type value_type; - - private: - LLT () {} - - typedef Dynamic::Matrix MatrixXXd; - typedef Dynamic::Matrix VectorXd; - - bool mIsFactorized; - MatrixXXd mL; - - public: - LLT (const matrix_type &matrix) : - mIsFactorized(false), - mL(matrix) { - compute(); - } - LLT compute() { - for (int i = 0; i < mL.rows(); i++) { - for (int j = 0; j < mL.rows(); j++) { - if (j > i) { - mL(i,j) = 0.; - continue; - } - double s = mL(i,j); - for (int k = 0; k < j; k++) { - s = s - mL(i,k) * mL(j,k); - } - if (i > j) { - mL(i,j) = s / mL(j,j); - } else if (s > 0.) { - mL (i,i) = sqrt (s); - } else { - std::cerr << "Error computing Cholesky decomposition: matrix not symmetric positive definite!" << std::endl; - assert (false); - } - } - } - - mIsFactorized = true; - - return *this; - } - Dynamic::Matrix solve ( - const Dynamic::Matrix &rhs - ) const { - assert (mIsFactorized); - - VectorXd y (mL.rows()); - for (unsigned int i = 0; i < mL.rows(); i++) { - double temp = rhs[i]; - - for (unsigned int j = 0; j < i; j++) { - temp = temp - mL(i,j) * y[j]; - } - - y[i] = temp / mL(i,i); - } - - VectorXd x (mL.rows()); - for (int i = mL.rows() - 1; i >= 0; i--) { - double temp = y[i]; - - for (unsigned int j = i + 1; j < mL.rows(); j++) { - temp = temp - mL(j, i) * x[j]; - } - - x[i] = temp / mL(i,i); - } - - return x; - } - Dynamic::Matrix matrixL() const { - return mL; - } - }; - -} - -/* _SIMPLE_MATH_CHOLESKY_H */ -#endif diff --git a/src/SimpleMath/SimpleMathCommaInitializer.h b/src/SimpleMath/SimpleMathCommaInitializer.h deleted file mode 100644 index 85657ba..0000000 --- a/src/SimpleMath/SimpleMathCommaInitializer.h +++ /dev/null @@ -1,69 +0,0 @@ -#ifndef _SIMPLE_MATH_COMMA_INITIALIZER_H -#define _SIMPLE_MATH_COMMA_INITIALIZER_H - -#include -#include - -#include "SimpleMathFixed.h" -#include "SimpleMathDynamic.h" - -namespace SimpleMath { - - template - class CommaInitializer { - public: - typedef typename matrix_type::value_type value_type; - - CommaInitializer(matrix_type &matrix, const value_type &value) : - mElementWasAdded (false) { - assert (matrix.cols() > 0 && matrix.rows() > 0); - mParentMatrix = &matrix; - mRowIndex = 0; - mColIndex = 0; - (*mParentMatrix)(mRowIndex, mColIndex) = value; - } - CommaInitializer(matrix_type &matrix, unsigned int row_index, unsigned int col_index) : - mRowIndex (row_index), - mColIndex (col_index), - mElementWasAdded (false) { - assert (matrix.cols() > 0 && matrix.rows() > 0); - mParentMatrix = &matrix; - mRowIndex = row_index; - mColIndex = col_index; - } - ~CommaInitializer() { - if (!mElementWasAdded - && (mColIndex + 1 < mParentMatrix->cols() || mRowIndex + 1 < mParentMatrix->rows())) { - std::cerr << "Error: too few elements passed to CommaInitializer! Expected " << mParentMatrix->size() << " but was given " << mRowIndex * mParentMatrix->cols() + mColIndex + 1 << std::endl; - abort(); - } - } - CommaInitializer operator, (const value_type &value) { - mColIndex++; - if (mColIndex >= mParentMatrix->cols()) { - mRowIndex++; - mColIndex = 0; - } - if (mRowIndex == mParentMatrix->rows() && mColIndex == 0 ) { - std::cerr << "Error: too many elements passed to CommaInitializer! Expected " << mParentMatrix->size() << " but was given " << mRowIndex * mParentMatrix->cols() + mColIndex + 1 << std::endl; - abort(); - } - (*mParentMatrix)(mRowIndex, mColIndex) = value; - mElementWasAdded = true; - - return CommaInitializer (*mParentMatrix, mRowIndex, mColIndex); - } - - private: - CommaInitializer() {} - - matrix_type *mParentMatrix; - unsigned int mRowIndex; - unsigned int mColIndex; - bool mElementWasAdded; - }; - -} - -/* _SIMPLE_MATH_COMMA_INITIALIZER_H */ -#endif diff --git a/src/SimpleMath/SimpleMathDynamic.h b/src/SimpleMath/SimpleMathDynamic.h deleted file mode 100644 index 62975e8..0000000 --- a/src/SimpleMath/SimpleMathDynamic.h +++ /dev/null @@ -1,667 +0,0 @@ -/** - * This is a highly inefficient math library. It was conceived by Martin - * Felis while he was compiling code - * that uses a highly efficient math library. - * - * It is intended to be used as a fast compiling substitute for the - * blazingly fast Eigen3 library and tries to mimic its API to a certain - * extend. - * - * Feel free to use it wherever you like. However, no guarantees are given - * that this code does what it says it would. - */ - -#ifndef SIMPLEMATHDYNAMIC_H -#define SIMPLEMATHDYNAMIC_H - -#include -#include -#include -#include - -#include "compileassert.h" -#include "SimpleMathBlock.h" - -/** \brief Namespace for a highly inefficient math library - * - */ -namespace SimpleMath { - -template -class LLT; - -template -class HouseholderQR; - -template -class ColPivHouseholderQR; - -template -class CommaInitializer; - -namespace Fixed { - template class Matrix; -} - - -/** \brief Namespace for elements of varying size. - */ -namespace Dynamic { - -// forward declaration -template -class Matrix; - -/** \brief Class for both matrices and vectors. - */ -template -class Matrix { - public: - typedef Matrix matrix_type; - typedef val_type value_type; - - Matrix() : - nrows (0), - ncols (0), - mapped_data (false), - mData (NULL) {}; - Matrix(unsigned int rows) : - nrows (rows), - ncols (1), - mapped_data (false) { - mData = new val_type[rows]; - } - Matrix(unsigned int rows, unsigned int cols) : - nrows (rows), - ncols (cols), - mapped_data (false) { - mData = new val_type[rows * cols]; - } - Matrix(unsigned int rows, unsigned int cols, val_type *data_ptr) : - nrows (rows), - ncols (cols), - mapped_data (true) { - mData = data_ptr; - } - - unsigned int rows() const { - return nrows; - } - - unsigned int cols() const { - return ncols; - } - - unsigned int size() const { - return nrows * ncols; - } - void resize (unsigned int rows, unsigned int cols=1) { - if (nrows * ncols > 0 && mData != NULL && mapped_data == false) { - delete[] mData; - } - - nrows = rows; - ncols = cols; - - mData = new val_type[nrows * ncols]; - } - - void conservativeResize (unsigned int rows, unsigned int cols = 1) { - Matrix result = Matrix::Zero(rows, cols); - - unsigned int arows = std::min (rows, nrows); - unsigned int acols = std::min (cols, ncols); - - for (unsigned int i = 0; i < arows; i++) { - for (unsigned int j = 0; j < acols; j++) { - result(i,j) = (*this)(i,j); - } - } - - *this = result; - } - - Matrix(const Matrix &matrix) : - nrows (matrix.nrows), - ncols (matrix.ncols), - mapped_data (false) { - unsigned int i; - - mData = new val_type[nrows * ncols]; - - for (i = 0; i < nrows * ncols; i++) - mData[i] = matrix.mData[i]; - } - Matrix& operator=(const Matrix &matrix) { - if (this != &matrix) { - if (!mapped_data) { - delete[] mData; - - nrows = matrix.nrows; - ncols = matrix.ncols; - mapped_data = false; - - mData = new val_type[nrows * ncols]; - - unsigned int i; - for (i = 0; i < nrows * ncols; i++) - mData[i] = matrix.mData[i]; - } else { - // we overwrite any existing data - nrows = matrix.nrows; - ncols = matrix.ncols; - mapped_data = true; - - unsigned int i; - for (i = 0; i < nrows * ncols; i++) - mData[i] = matrix.mData[i]; - } - } - return *this; - } - - CommaInitializer operator<< (const val_type& value) { - return CommaInitializer (*this, value); - } - - // conversion different val_types - template - Matrix (const Matrix &matrix) : - nrows (matrix.rows()), - ncols (matrix.cols()), - mapped_data(false) { - - mData = new val_type[nrows * ncols]; - - for (unsigned int i = 0; i < nrows; i++) { - for (unsigned int j = 0; j < ncols; j++) { - (*this)(i,j) = static_cast(matrix(i,j)); - } - } - } - - // conversion from a fixed size matrix - template - Matrix (const Fixed::Matrix &fixed_matrix) : - nrows (fnrows), - ncols (fncols), - mapped_data (false), - mData (NULL) { - mData = new val_type[nrows * ncols]; - - for (unsigned int i = 0; i < nrows; i++) { - for (unsigned int j = 0; j < ncols; j++) { - (*this)(i,j) = static_cast(fixed_matrix(i,j)); - } - } - } - - template - Matrix (const Block &block) : - nrows(block.rows()), - ncols(block.cols()), - mapped_data (false) { - mData = new val_type[nrows * ncols]; - - for (unsigned int i = 0; i < nrows; i++) { - for (unsigned int j = 0; j < ncols; j++) { - (*this)(i,j) = static_cast(block(i,j)); - } - } - - } - - ~Matrix() { - if (nrows * ncols > 0 && mData != NULL && mapped_data == false) { - delete[] mData; - mData = NULL; - } - - nrows = 0; - ncols = 0; - }; - - // comparison - bool operator==(const Matrix &matrix) const { - if (nrows != matrix.nrows || ncols != matrix.ncols) - return false; - - for (unsigned int i = 0; i < nrows * ncols; i++) { - if (mData[i] != matrix.mData[i]) - return false; - } - return true; - } - bool operator!=(const Matrix &matrix) const { - if (nrows != matrix.nrows || ncols != matrix.ncols) - return true; - - for (unsigned int i = 0; i < nrows * ncols; i++) { - if (mData[i] != matrix.mData[i]) - return true; - } - return false; - } - - // access operators - const val_type& operator[](const unsigned int &index) const { - assert (index >= 0); - assert (index < nrows * ncols); - return mData[index]; - }; - val_type& operator[](const unsigned int &index) { - assert (index >= 0 && index < nrows * ncols); - return mData[index]; - } - - const val_type& operator()(const unsigned int &row, const unsigned int &col) const { - if (!(row >= 0 && row < nrows && col >= 0 && col < ncols)) { - std::cout << "row = " << row << " col = " << col << std::endl; - std::cout << "nrows = " << nrows << " ncols = " << ncols << std::endl; - std::cout << "invalid read = " << mData[100000] << std::endl; - } - assert (row >= 0 && row < nrows && col >= 0 && col < ncols); - return mData[row*ncols + col]; - }; - val_type& operator()(const unsigned int &row, const unsigned int &col) { - assert (row >= 0 && row < nrows && col >= 0 && col < ncols); - return mData[row*ncols + col]; - }; - - void zero() { - for (unsigned int i = 0; i < ncols * nrows; i++) - mData[i] = 0.; - } - void setZero() { - zero(); - } - - val_type norm() const { - return sqrt(this->squaredNorm()); - } - - void normalize() { - val_type length = this->norm(); - - for (unsigned int i = 0; i < ncols * nrows; i++) - mData[i] /= length; - } - - Matrix normalized() const { - return Matrix (*this) / this->norm(); - } - - Matrix cross(const Matrix &other_vector) { - assert (nrows * ncols == 3); - assert (other_vector.nrows * other_vector.ncols == 3); - - Matrix result (3, 1); - result[0] = mData[1] * other_vector[2] - mData[2] * other_vector[1]; - result[1] = mData[2] * other_vector[0] - mData[0] * other_vector[2]; - result[2] = mData[0] * other_vector[1] - mData[1] * other_vector[0]; - - return result; - } - - val_type trace() const { - assert (rows() == cols()); - val_type result = 0.; - - for (unsigned int i = 0; i < rows(); i++) { - result += operator()(i,i); - } - - return result; - } - - val_type mean() const { - assert (rows() == 1 || cols() == 1); - - val_type result = 0.; - for (unsigned int i = 0; i < rows() * cols(); i++) { - result += operator[](i); - } - - return result / static_cast(rows() * cols()); - } - - static matrix_type Zero() { - matrix_type result; - result.setZero(); - return result; - } - - static matrix_type Zero(int rows, int cols = 1) { - matrix_type result (rows, cols); - result.setZero(); - return result; - } - - static matrix_type Constant (int rows, val_type value) { - matrix_type result (rows, 1); - unsigned int i; - for (i = 0; i < result.size(); i++) - result[i] = value; - - return result; - } - - static matrix_type Constant (int rows, int cols, val_type value) { - matrix_type result (rows, cols); - unsigned int i; - for (i = 0; i < result.size(); i++) - result[i] = value; - - return result; - } - - static matrix_type Identity (int rows, int cols = 1) { - assert (rows == cols); - - matrix_type result (rows, cols); - result.identity(); - - return result; - } - - void identity() { - assert (nrows == ncols); - - setZero(); - for (unsigned int i = 0; i < ncols; i++) - mData[i * ncols + i] = 1.; - } - - void random() { - for (unsigned int i = 0; i < nrows * ncols; i++) - mData[i] = static_cast (rand()) / static_cast (RAND_MAX); - } - - val_type squaredNorm() const { - assert (ncols == 1 || nrows == 1); - val_type result = 0; - - for (unsigned int i = 0; i < nrows * ncols; i++) - result += mData[i] * mData[i]; - - return result; - } - - val_type dot(const matrix_type &matrix) const { - assert (ncols == 1 || nrows == 1); - val_type result = 0; - - for (unsigned int i = 0; i < nrows * ncols; i++) - result += mData[i] * matrix[i]; - - return result; - } - - // Blocks - Block - block (unsigned int row_start, unsigned int col_start, unsigned int row_count, unsigned int col_count) { - return Block(*this, row_start, col_start, row_count, col_count); - } - - template - Block - block (unsigned int row_start, unsigned int col_start) { - return Block(*this, row_start, col_start, row_count, col_count); - } - - Block - block (unsigned int row_start, unsigned int col_start, unsigned int row_count, unsigned int col_count) const { - return Block(*this, row_start, col_start, row_count, col_count); - } - - template - Block - block (unsigned int row_start, unsigned int col_start) const { - return Block(*this, row_start, col_start, row_count, col_count); - } - - // row and col accessors - Block - col(unsigned int index) const { - return Block(*this, 0, index, rows(), 1); - } - - Block - col(unsigned int index) { - return Block(*this, 0, index, rows(), 1); - } - - Block - row(unsigned int index) const { - return Block(*this, index, 0, 1, cols()); - } - - Block - row(unsigned int index) { - return Block(*this, index, 0, 1, cols()); - } - - // Operators with scalars - void operator*=(const val_type &scalar) { - for (unsigned int i = 0; i < nrows * ncols; i++) - mData[i] *= scalar; - }; - void operator/=(const val_type &scalar) { - for (unsigned int i = 0; i < nrows * ncols; i++) - mData[i] /= scalar; - } - Matrix operator/(const val_type& scalar) const { - matrix_type result (*this); - - for (unsigned int i = 0; i < nrows * ncols; i++) - result[i] /= scalar; - - return result; - } - - // Operators with other matrices - Matrix operator+(const Matrix &matrix) const { - matrix_type result (*this); - - for (unsigned int i = 0; i < nrows * ncols; i++) - result[i] += matrix[i]; - - return result; - } - void operator+=(const matrix_type &matrix) { - for (unsigned int i = 0; i < nrows * ncols; i++) - mData[i] += matrix.mData[i]; - } - Matrix operator-(const Matrix &matrix) const { - matrix_type result (*this); - - for (unsigned int i = 0; i < nrows * ncols; i++) - result[i] -= matrix[i]; - - return result; - } - void operator-=(const Matrix &matrix) { - for (unsigned int i = 0; i < nrows * ncols; i++) - mData[i] -= matrix.mData[i]; - } - - Matrix operator*(const Matrix &other_matrix) const { - assert (ncols == other_matrix.nrows); - - Matrix result(nrows, other_matrix.ncols); - - result.setZero(); - - unsigned int i,j, k; - for (i = 0; i < nrows; i++) { - for (j = 0; j < other_matrix.cols(); j++) { - for (k = 0; k < other_matrix.rows(); k++) { - result(i,j) += mData[i * ncols + k] * other_matrix(k,j); - } - } - } - - return result; - } - - template - Matrix operator*(const Fixed::Matrix &other_matrix) const { - assert (ncols == other_matrix.rows()); - - Matrix result(nrows, other_matrix.cols()); - - result.setZero(); - - unsigned int i,j, k; - for (i = 0; i < nrows; i++) { - for (j = 0; j < other_matrix.cols(); j++) { - for (k = 0; k < other_matrix.rows(); k++) { - result(i,j) += mData[i * ncols + k] * other_matrix(k,j); - } - } - } - - return result; - } - - Matrix operator*(const Block &other_matrix) const { - assert (ncols == other_matrix.rows()); - - Matrix result(nrows, other_matrix.cols()); - - result.setZero(); - - unsigned int i,j, k; - for (i = 0; i < nrows; i++) { - for (j = 0; j < other_matrix.cols(); j++) { - for (k = 0; k < other_matrix.rows(); k++) { - result(i,j) += mData[i * ncols + k] * other_matrix(k,j); - } - } - } - - return result; - } - - void operator*=(const Matrix &matrix) { - matrix_type temp (*this); - *this = temp * matrix; - } - - // Special operators - val_type *data(){ - return mData; - } - - // regular transpose of a 6 dimensional matrix - Matrix transpose() const { - Matrix result(ncols, nrows); - - for (unsigned int i = 0; i < nrows; i++) { - for (unsigned int j = 0; j < ncols; j++) { - result(j,i) = mData[i * ncols + j]; - } - } - - return result; - } - - operator val_type() { - - assert (nrows == 1); - assert (nrows == 1); - - return mData[0]; - } - - Matrix operator-() const { - return *this * -1.0; - }; - - Matrix inverse() const { - return colPivHouseholderQr().inverse(); - } - - const LLT llt() const { - return LLT(*this); - } - - const HouseholderQR householderQr() const { - return HouseholderQR(*this); - } - const ColPivHouseholderQR colPivHouseholderQr() const { - return ColPivHouseholderQR(*this); - } - - private: - unsigned int nrows; - unsigned int ncols; - bool mapped_data; - - val_type* mData; -}; - -template -inline Matrix operator*(val_type scalar, const Matrix &matrix) { - Matrix result (matrix); - - for (unsigned int i = 0; i < matrix.rows() * matrix.cols(); i++) - result.data()[i] *= scalar; - - return result; -} - -template -inline Matrix operator*(const Matrix &matrix, other_type scalar) { - Matrix result (matrix); - - for (unsigned int i = 0; i < matrix.rows() * matrix.cols(); i++) - result.data()[i] *= static_cast(scalar); - - return result; -} - -template -inline std::ostream& operator<<(std::ostream& output, const Matrix &matrix) { - size_t max_width = 0; - size_t out_width = output.width(); - - // get the widest number - for (size_t i = 0; i < matrix.rows(); i++) { - for (size_t j = 0; j < matrix.cols(); j++) { - std::stringstream out_stream; - out_stream << matrix(i,j); - max_width = std::max (out_stream.str().size(),max_width); - } - } - - // overwrite width if it was explicitly prescribed - if (out_width != 0) { - max_width = out_width; - } - - for (unsigned int i = 0; i < matrix.rows(); i++) { - output.width(0); - output << "[ "; - output.width(out_width); - for (unsigned int j = 0; j < matrix.cols(); j++) { - std::stringstream out_stream; - out_stream.width (max_width); - out_stream << matrix(i,j); - output << out_stream.str(); - - if (j < matrix.cols() - 1) - output << ", "; - } - output << " ]"; - - if (matrix.rows() > 1 && i < matrix.rows() - 1) - output << std::endl; - } - return output;} - -} - -} - -#endif /* SIMPLEMATHDYNAMIC_H */ diff --git a/src/SimpleMath/SimpleMathDynamic.h.orig b/src/SimpleMath/SimpleMathDynamic.h.orig deleted file mode 100644 index b86108b..0000000 --- a/src/SimpleMath/SimpleMathDynamic.h.orig +++ /dev/null @@ -1,619 +0,0 @@ -/** - * This is a highly inefficient math library. It was conceived by Martin - * Felis while he was compiling code - * that uses a highly efficient math library. - * - * It is intended to be used as a fast compiling substitute for the - * blazingly fast Eigen3 library and tries to mimic its API to a certain - * extend. - * - * Feel free to use it wherever you like. However, no guarantees are given - * that this code does what it says it would. - */ - -#ifndef SIMPLEMATHDYNAMIC_H -#define SIMPLEMATHDYNAMIC_H - -#include -#include -#include -#include - -#include "compileassert.h" -#include "SimpleMathBlock.h" - -/** \brief Namespace for a highly inefficient math library - * - */ -namespace SimpleMath { - -template -class LLT; - -template -class HouseholderQR; - -template -class ColPivHouseholderQR; - -template -class CommaInitializer; - -namespace Fixed { - template class Matrix; -} - - -/** \brief Namespace for elements of varying size. - */ -namespace Dynamic { - -// forward declaration -template -class Matrix; - -/** \brief Class for both matrices and vectors. - */ -template -class Matrix { - public: - typedef Matrix matrix_type; - typedef val_type value_type; - - Matrix() : - nrows (0), - ncols (0), - mapped_data (false), - mData (NULL) {}; - Matrix(unsigned int rows) : - nrows (rows), - ncols (1), - mapped_data (false) { - mData = new val_type[rows]; - } - Matrix(unsigned int rows, unsigned int cols) : - nrows (rows), - ncols (cols), - mapped_data (false) { - mData = new val_type[rows * cols]; - } - Matrix(unsigned int rows, unsigned int cols, val_type *data_ptr) : - nrows (rows), - ncols (cols), - mapped_data (true) { - mData = data_ptr; - } - - unsigned int rows() const { - return nrows; - } - - unsigned int cols() const { - return ncols; - } - - unsigned int size() const { - return nrows * ncols; - } - void resize (unsigned int rows, unsigned int cols=1) { - if (nrows * ncols > 0 && mData != NULL && mapped_data == false) { - delete[] mData; - } - - nrows = rows; - ncols = cols; - - mData = new val_type[nrows * ncols]; - } - - void conservativeResize (unsigned int rows, unsigned int cols = 1) { - Matrix result = Matrix::Zero(rows, cols); - - unsigned int arows = std::min (rows, nrows); - unsigned int acols = std::min (cols, ncols); - - for (unsigned int i = 0; i < arows; i++) { - for (unsigned int j = 0; j < acols; j++) { - result(i,j) = (*this)(i,j); - } - } - - *this = result; - } - - Matrix(const Matrix &matrix) : - nrows (matrix.nrows), - ncols (matrix.ncols), - mapped_data (false) { - unsigned int i; - - mData = new val_type[nrows * ncols]; - - for (i = 0; i < nrows * ncols; i++) - mData[i] = matrix.mData[i]; - } - Matrix& operator=(const Matrix &matrix) { - if (this != &matrix) { - if (!mapped_data) { - delete[] mData; - - nrows = matrix.nrows; - ncols = matrix.ncols; - mapped_data = false; - - mData = new val_type[nrows * ncols]; - - unsigned int i; - for (i = 0; i < nrows * ncols; i++) - mData[i] = matrix.mData[i]; - } else { - // we overwrite any existing data - nrows = matrix.nrows; - ncols = matrix.ncols; - mapped_data = true; - - unsigned int i; - for (i = 0; i < nrows * ncols; i++) - mData[i] = matrix.mData[i]; - } - } - return *this; - } - - CommaInitializer operator<< (const val_type& value) { - return CommaInitializer (*this, value); - } - - // conversion different val_types - template - Matrix (const Matrix &matrix) : - nrows (matrix.rows()), - ncols (matrix.cols()), - mapped_data(false) { - - mData = new val_type[nrows * ncols]; - - for (unsigned int i = 0; i < nrows; i++) { - for (unsigned int j = 0; j < ncols; j++) { - (*this)(i,j) = static_cast(matrix(i,j)); - } - } - } - - // conversion from a fixed size matrix - template - Matrix (const Fixed::Matrix &fixed_matrix) : - nrows (fnrows), - ncols (fncols), - mapped_data (false), - mData (NULL) { - mData = new val_type[nrows * ncols]; - - for (unsigned int i = 0; i < nrows; i++) { - for (unsigned int j = 0; j < ncols; j++) { - (*this)(i,j) = static_cast(fixed_matrix(i,j)); - } - } - } - - template - explicit Matrix (const Block &block) : - nrows(block.rows()), - ncols(block.cols()), - mapped_data (false) { - mData = new val_type[nrows * ncols]; - - for (unsigned int i = 0; i < nrows; i++) { - for (unsigned int j = 0; j < ncols; j++) { - (*this)(i,j) = static_cast(block(i,j)); - } - } - - } - - ~Matrix() { - if (nrows * ncols > 0 && mData != NULL && mapped_data == false) { - delete[] mData; - mData = NULL; - } - - nrows = 0; - ncols = 0; - }; - - // comparison - bool operator==(const Matrix &matrix) const { - if (nrows != matrix.nrows || ncols != matrix.ncols) - return false; - - for (unsigned int i = 0; i < nrows * ncols; i++) { - if (mData[i] != matrix.mData[i]) - return false; - } - return true; - } - bool operator!=(const Matrix &matrix) const { - if (nrows != matrix.nrows || ncols != matrix.ncols) - return true; - - for (unsigned int i = 0; i < nrows * ncols; i++) { - if (mData[i] != matrix.mData[i]) - return true; - } - return false; - } - - // access operators - const val_type& operator[](const unsigned int &index) const { - assert (index >= 0); - assert (index < nrows * ncols); - return mData[index]; - }; - val_type& operator[](const unsigned int &index) { - assert (index >= 0 && index < nrows * ncols); - return mData[index]; - } - - const val_type& operator()(const unsigned int &row, const unsigned int &col) const { - if (!(row >= 0 && row < nrows && col >= 0 && col < ncols)) { - std::cout << "row = " << row << " col = " << col << std::endl; - std::cout << "nrows = " << nrows << " ncols = " << ncols << std::endl; - std::cout << "invalid read = " << mData[100000] << std::endl; - } - assert (row >= 0 && row < nrows && col >= 0 && col < ncols); - return mData[row*ncols + col]; - }; - val_type& operator()(const unsigned int &row, const unsigned int &col) { - assert (row >= 0 && row < nrows && col >= 0 && col < ncols); - return mData[row*ncols + col]; - }; - - void zero() { - for (unsigned int i = 0; i < ncols * nrows; i++) - mData[i] = 0.; - } - void setZero() { - zero(); - } - - val_type norm() const { - return sqrt(this->squaredNorm()); - } - - void normalize() { - val_type length = this->norm(); - - for (unsigned int i = 0; i < ncols * nrows; i++) - mData[i] /= length; - } - - Matrix normalized() const { - return Matrix (*this) / this->norm(); - } - - Matrix cross(const Matrix &other_vector) { - assert (nrows * ncols == 3); - assert (other_vector.nrows * other_vector.ncols == 3); - - Matrix result (3, 1); - result[0] = mData[1] * other_vector[2] - mData[2] * other_vector[1]; - result[1] = mData[2] * other_vector[0] - mData[0] * other_vector[2]; - result[2] = mData[0] * other_vector[1] - mData[1] * other_vector[0]; - - return result; - } - - static matrix_type Zero() { - matrix_type result; - result.setZero(); - return result; - } - - static matrix_type Zero(int rows, int cols = 1) { - matrix_type result (rows, cols); - result.setZero(); - return result; - } - - static matrix_type Constant (int rows, val_type value) { - matrix_type result (rows, 1); - unsigned int i; - for (i = 0; i < result.size(); i++) - result[i] = value; - - return result; - } - - static matrix_type Constant (int rows, int cols, val_type value) { - matrix_type result (rows, cols); - unsigned int i; - for (i = 0; i < result.size(); i++) - result[i] = value; - - return result; - } - - static matrix_type Identity (int rows, int cols = 1) { - assert (rows == cols); - - matrix_type result (rows, cols); - result.identity(); - - return result; - } - - void identity() { - assert (nrows == ncols); - - setZero(); - for (unsigned int i = 0; i < ncols; i++) - mData[i * ncols + i] = 1.; - } - - void random() { - for (unsigned int i = 0; i < nrows * ncols; i++) - mData[i] = static_cast (rand()) / static_cast (RAND_MAX); - } - - val_type squaredNorm() const { - assert (ncols == 1 || nrows == 1); - val_type result = 0; - - for (unsigned int i = 0; i < nrows * ncols; i++) - result += mData[i] * mData[i]; - - return result; - } - - val_type dot(const matrix_type &matrix) const { - assert (ncols == 1 || nrows == 1); - val_type result = 0; - - for (unsigned int i = 0; i < nrows * ncols; i++) - result += mData[i] * matrix[i]; - - return result; - } - - // Blocks - Block - block (unsigned int row_start, unsigned int col_start, unsigned int row_count, unsigned int col_count) { - return Block(*this, row_start, col_start, row_count, col_count); - } - - template - Block - block (unsigned int row_start, unsigned int col_start) { - return Block(*this, row_start, col_start, row_count, col_count); - } - - Block - block (unsigned int row_start, unsigned int col_start, unsigned int row_count, unsigned int col_count) const { - return Block(*this, row_start, col_start, row_count, col_count); - } - - template - Block - block (unsigned int row_start, unsigned int col_start) const { - return Block(*this, row_start, col_start, row_count, col_count); - } - - // Operators with scalars - void operator*=(const val_type &scalar) { - for (unsigned int i = 0; i < nrows * ncols; i++) - mData[i] *= scalar; - }; - void operator/=(const val_type &scalar) { - for (unsigned int i = 0; i < nrows * ncols; i++) - mData[i] /= scalar; - } - Matrix operator/(const val_type& scalar) const { - matrix_type result (*this); - - for (unsigned int i = 0; i < nrows * ncols; i++) - result[i] /= scalar; - - return result; - } - - // Operators with other matrices - Matrix operator+(const Matrix &matrix) const { - matrix_type result (*this); - - for (unsigned int i = 0; i < nrows * ncols; i++) - result[i] += matrix[i]; - - return result; - } - void operator+=(const matrix_type &matrix) { - for (unsigned int i = 0; i < nrows * ncols; i++) - mData[i] += matrix.mData[i]; - } - Matrix operator-(const Matrix &matrix) const { - matrix_type result (*this); - - for (unsigned int i = 0; i < nrows * ncols; i++) - result[i] -= matrix[i]; - - return result; - } - void operator-=(const Matrix &matrix) { - for (unsigned int i = 0; i < nrows * ncols; i++) - mData[i] -= matrix.mData[i]; - } - - Matrix operator*(const Matrix &other_matrix) const { - assert (ncols == other_matrix.nrows); - - Matrix result(nrows, other_matrix.ncols); - - result.setZero(); - - unsigned int i,j, k; - for (i = 0; i < nrows; i++) { - for (j = 0; j < other_matrix.cols(); j++) { - for (k = 0; k < other_matrix.rows(); k++) { - result(i,j) += mData[i * ncols + k] * other_matrix(k,j); - } - } - } - - return result; - } - - template - Matrix operator*(const Fixed::Matrix &other_matrix) const { - assert (ncols == other_matrix.rows()); - - Matrix result(nrows, other_matrix.cols()); - - result.setZero(); - - unsigned int i,j, k; - for (i = 0; i < nrows; i++) { - for (j = 0; j < other_matrix.cols(); j++) { - for (k = 0; k < other_matrix.rows(); k++) { - result(i,j) += mData[i * ncols + k] * other_matrix(k,j); - } - } - } - - return result; - } - - Matrix operator*(const Block &other_matrix) const { - assert (ncols == other_matrix.rows()); - - Matrix result(nrows, other_matrix.cols()); - - result.setZero(); - - unsigned int i,j, k; - for (i = 0; i < nrows; i++) { - for (j = 0; j < other_matrix.cols(); j++) { - for (k = 0; k < other_matrix.rows(); k++) { - result(i,j) += mData[i * ncols + k] * other_matrix(k,j); - } - } - } - - return result; - } - - void operator*=(const Matrix &matrix) { - matrix_type temp (*this); - *this = temp * matrix; - } - - // Special operators - val_type *data(){ - return mData; - } - - // regular transpose of a 6 dimensional matrix - Matrix transpose() const { - Matrix result(ncols, nrows); - - for (unsigned int i = 0; i < nrows; i++) { - for (unsigned int j = 0; j < ncols; j++) { - result(j,i) = mData[i * ncols + j]; - } - } - - return result; - } - - operator val_type() { - assert (nrows == 1); - assert (nrows == 1); - - return mData[0]; - } - - Matrix inverse() const { - return colPivHouseholderQr().inverse(); - } - - const LLT llt() const { - return LLT(*this); - } - - const HouseholderQR householderQr() const { - return HouseholderQR(*this); - } - const ColPivHouseholderQR colPivHouseholderQr() const { - return ColPivHouseholderQR(*this); - } - - private: - unsigned int nrows; - unsigned int ncols; - bool mapped_data; - - val_type* mData; -}; - -template -inline Matrix operator*(val_type scalar, const Matrix &matrix) { - Matrix result (matrix); - - for (unsigned int i = 0; i < matrix.rows() * matrix.cols(); i++) - result.data()[i] *= scalar; - - return result; -} - -template -inline Matrix operator*(const Matrix &matrix, other_type scalar) { - Matrix result (matrix); - - for (unsigned int i = 0; i < matrix.rows() * matrix.cols(); i++) - result.data()[i] *= static_cast(scalar); - - return result; -} - -template -inline std::ostream& operator<<(std::ostream& output, const Matrix &matrix) { - size_t max_width = 0; - size_t out_width = output.width(); - - // get the widest number - for (size_t i = 0; i < matrix.rows(); i++) { - for (size_t j = 0; j < matrix.cols(); j++) { - std::stringstream out_stream; - out_stream << matrix(i,j); - max_width = std::max (out_stream.str().size(),max_width); - } - } - - // overwrite width if it was explicitly prescribed - if (out_width != 0) { - max_width = out_width; - } - - for (unsigned int i = 0; i < matrix.rows(); i++) { - output.width(0); - output << "[ "; - output.width(out_width); - for (unsigned int j = 0; j < matrix.cols(); j++) { - std::stringstream out_stream; - out_stream.width (max_width); - out_stream << matrix(i,j); - output << out_stream.str(); - - if (j < matrix.cols() - 1) - output << ", "; - } - output << " ]"; - - if (matrix.rows() > 1 && i < matrix.rows() - 1) - output << std::endl; - } - return output;} - -} - -} - -#endif /* SIMPLEMATHDYNAMIC_H */ diff --git a/src/SimpleMath/SimpleMathFixed.h b/src/SimpleMath/SimpleMathFixed.h deleted file mode 100644 index 0e43713..0000000 --- a/src/SimpleMath/SimpleMathFixed.h +++ /dev/null @@ -1,885 +0,0 @@ -/** - * This is a highly inefficient math library. It was conceived by Martin - * Felis while he was compiling code - * that uses a highly efficient math library. - * - * It is intended to be used as a fast compiling substitute for the - * blazingly fast Eigen3 library and tries to mimic its API to a certain - * extend. - * - * Feel free to use it wherever you like. However, no guarantees are given - * that this code does what it says it would. - */ - -#ifndef SIMPLEMATHFIXED_H -#define SIMPLEMATHFIXED_H - -#include -#include -#include -#include -#include -#include - -#include "compileassert.h" -#include "SimpleMathBlock.h" - -/** \brief Namespace for a highly inefficient math library - * - */ -namespace SimpleMath { - -template -class LLT; - -template -class HouseholderQR; - -template -class ColPivHouseholderQR; - -template -class CommaInitializer; - -namespace Dynamic { -template class Matrix; -} - -/** \brief Namespace for fixed size elements - */ -namespace Fixed { - -// forward declaration -template -class Matrix; - -/** \brief Fixed size matrix class - */ -template -class Matrix { - public: - typedef Matrix matrix_type; - typedef val_type value_type; - - unsigned int rows() const { - return nrows; - } - - unsigned int cols() const { - return ncols; - } - - unsigned int size() const { - return nrows * ncols; - } - - Matrix() {}; - Matrix(const Matrix &matrix) { - unsigned int i; - for (i = 0; i < nrows * ncols; i++) - mData[i] = matrix.mData[i]; - } - Matrix(unsigned int _nrows, unsigned int _ncols, const value_type* values) { - assert(nrows == _nrows); - assert(ncols == _ncols); - memcpy (mData, values, sizeof(value_type) * nrows * ncols); - } - - Matrix& operator=(const Matrix &matrix) { - if (this != &matrix) { - unsigned int i; - for (i = 0; i < nrows * ncols; i++) - mData[i] = matrix.mData[i]; - } - return *this; - } - - // conversion different val_types - - template - Matrix (const Block &block) { - assert (nrows == block.rows()); - assert (ncols == block.cols()); - - for (unsigned int i = 0; i < nrows; i++) { - for (unsigned int j = 0; j < ncols; j++) { - (*this)(i,j) = static_cast(block(i,j)); - } - } - } - template - Matrix& operator= (const Block &block) { - assert (nrows == block.rows()); - assert (ncols == block.cols()); - - for (unsigned int i = 0; i < nrows; i++) { - for (unsigned int j = 0; j < ncols; j++) { - (*this)(i,j) = static_cast(block(i,j)); - } - } - - return *this; - } - - template - Matrix (const Matrix &matrix) { - for (unsigned int i = 0; i < nrows; i++) { - for (unsigned int j = 0; j < ncols; j++) { - (*this)(i,j) = static_cast(matrix(i,j)); - } - } - } - - template - Matrix& operator=(const Matrix &matrix) { - for (unsigned int i = 0; i < nrows; i++) { - for (unsigned int j = 0; j < ncols; j++) { - (*this)(i,j) = static_cast(matrix(i,j)); - } - } - - return *this; - } - - CommaInitializer operator<< (const val_type& value) { - return CommaInitializer (*this, value); - } - - // conversion Dynamic->Fixed - Matrix(const Dynamic::Matrix &dynamic_matrix); - Matrix& operator=(const Dynamic::Matrix &dynamic_matrix); - - ~Matrix() {}; - - Matrix ( - const val_type &v00, const val_type &v01 - ) { - assert (nrows == 2); - assert (ncols == 1); - - mData[0] = v00; - mData[1] = v01; - } - - void set( - const val_type &v00, const val_type &v01 - ) { - COMPILE_ASSERT (nrows * ncols == 2); - - mData[0] = v00; - mData[1] = v01; - } - - Matrix ( - const val_type &v00, const val_type &v01, const val_type &v02 - ) { - assert (nrows == 3); - assert (ncols == 1); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - } - - void set( - const val_type &v00, const val_type &v01, const val_type &v02 - ) { - COMPILE_ASSERT (nrows * ncols == 3); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - } - - Matrix ( - const val_type &v00, const val_type &v01, const val_type &v02, - const val_type &v10, const val_type &v11, const val_type &v12, - const val_type &v20, const val_type &v21, const val_type &v22 - ) { - COMPILE_ASSERT (nrows == 3); - COMPILE_ASSERT (ncols == 3); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - - mData[1 * 3 + 0] = v10; - mData[1 * 3 + 1] = v11; - mData[1 * 3 + 2] = v12; - - mData[2 * 3 + 0] = v20; - mData[2 * 3 + 1] = v21; - mData[2 * 3 + 2] = v22; - } - - void set( - const val_type v00, const val_type v01, const val_type v02, - const val_type v10, const val_type v11, const val_type v12, - const val_type v20, const val_type v21, const val_type v22 - ) { - COMPILE_ASSERT (nrows == 3); - COMPILE_ASSERT (ncols == 3); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - - mData[1 * 3 + 0] = v10; - mData[1 * 3 + 1] = v11; - mData[1 * 3 + 2] = v12; - - mData[2 * 3 + 0] = v20; - mData[2 * 3 + 1] = v21; - mData[2 * 3 + 2] = v22; - } - - Matrix ( - const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03 - ) { - assert (nrows == 4); - assert (ncols == 1); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - mData[3] = v03; - } - - void set( - const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03 - ) { - COMPILE_ASSERT (nrows * ncols == 4); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - mData[3] = v03; - } - - Matrix ( - const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03, - const val_type &v10, const val_type &v11, const val_type &v12, const val_type &v13, - const val_type &v20, const val_type &v21, const val_type &v22, const val_type &v23, - const val_type &v30, const val_type &v31, const val_type &v32, const val_type &v33 - ) { - COMPILE_ASSERT (nrows == 4); - COMPILE_ASSERT (ncols == 4); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - mData[3] = v03; - - mData[1 * 4 + 0] = v10; - mData[1 * 4 + 1] = v11; - mData[1 * 4 + 2] = v12; - mData[1 * 4 + 3] = v13; - - mData[2 * 4 + 0] = v20; - mData[2 * 4 + 1] = v21; - mData[2 * 4 + 2] = v22; - mData[2 * 4 + 3] = v23; - - mData[3 * 4 + 0] = v30; - mData[3 * 4 + 1] = v31; - mData[3 * 4 + 2] = v32; - mData[3 * 4 + 3] = v33; - } - - void set( - const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03, - const val_type &v10, const val_type &v11, const val_type &v12, const val_type &v13, - const val_type &v20, const val_type &v21, const val_type &v22, const val_type &v23, - const val_type &v30, const val_type &v31, const val_type &v32, const val_type &v33 - ) { - COMPILE_ASSERT (nrows == 4); - COMPILE_ASSERT (ncols == 4); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - mData[3] = v03; - - mData[1 * 4 + 0] = v10; - mData[1 * 4 + 1] = v11; - mData[1 * 4 + 2] = v12; - mData[1 * 4 + 3] = v13; - - mData[2 * 4 + 0] = v20; - mData[2 * 4 + 1] = v21; - mData[2 * 4 + 2] = v22; - mData[2 * 4 + 3] = v23; - - mData[3 * 4 + 0] = v30; - mData[3 * 4 + 1] = v31; - mData[3 * 4 + 2] = v32; - mData[3 * 4 + 3] = v33; - } - - Matrix ( - const val_type &v00, const val_type &v01, const val_type &v02, - const val_type &v03, const val_type &v04, const val_type &v05 - ) { - COMPILE_ASSERT (nrows == 6); - COMPILE_ASSERT (ncols == 1); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - mData[3] = v03; - mData[4] = v04; - mData[5] = v05; - } - - void set( - const val_type &v00, const val_type &v01, const val_type &v02, - const val_type &v03, const val_type &v04, const val_type &v05 - ) { - COMPILE_ASSERT (nrows * ncols == 6); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - mData[3] = v03; - mData[4] = v04; - mData[5] = v05; - } - - Matrix ( - const val_type &v00, const val_type &v01, const val_type &v02, - const val_type &v03, const val_type &v04, const val_type &v05, - - const val_type &v10, const val_type &v11, const val_type &v12, - const val_type &v13, const val_type &v14, const val_type &v15, - - const val_type &v20, const val_type &v21, const val_type &v22, - const val_type &v23, const val_type &v24, const val_type &v25, - - const val_type &v30, const val_type &v31, const val_type &v32, - const val_type &v33, const val_type &v34, const val_type &v35, - - const val_type &v40, const val_type &v41, const val_type &v42, - const val_type &v43, const val_type &v44, const val_type &v45, - - const val_type &v50, const val_type &v51, const val_type &v52, - const val_type &v53, const val_type &v54, const val_type &v55 - ) { - COMPILE_ASSERT (nrows == 6); - COMPILE_ASSERT (ncols == 6); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - mData[3] = v03; - mData[4] = v04; - mData[5] = v05; - - mData[6 + 0] = v10; - mData[6 + 1] = v11; - mData[6 + 2] = v12; - mData[6 + 3] = v13; - mData[6 + 4] = v14; - mData[6 + 5] = v15; - - mData[12 + 0] = v20; - mData[12 + 1] = v21; - mData[12 + 2] = v22; - mData[12 + 3] = v23; - mData[12 + 4] = v24; - mData[12 + 5] = v25; - - mData[18 + 0] = v30; - mData[18 + 1] = v31; - mData[18 + 2] = v32; - mData[18 + 3] = v33; - mData[18 + 4] = v34; - mData[18 + 5] = v35; - - mData[24 + 0] = v40; - mData[24 + 1] = v41; - mData[24 + 2] = v42; - mData[24 + 3] = v43; - mData[24 + 4] = v44; - mData[24 + 5] = v45; - - mData[30 + 0] = v50; - mData[30 + 1] = v51; - mData[30 + 2] = v52; - mData[30 + 3] = v53; - mData[30 + 4] = v54; - mData[30 + 5] = v55; - }; - - void set( - const val_type v00, const val_type v01, const val_type v02, - const val_type v03, const val_type v04, const val_type v05, - - const val_type v10, const val_type v11, const val_type v12, - const val_type v13, const val_type v14, const val_type v15, - - const val_type v20, const val_type v21, const val_type v22, - const val_type v23, const val_type v24, const val_type v25, - - const val_type v30, const val_type v31, const val_type v32, - const val_type v33, const val_type v34, const val_type v35, - - const val_type v40, const val_type v41, const val_type v42, - const val_type v43, const val_type v44, const val_type v45, - - const val_type v50, const val_type v51, const val_type v52, - const val_type v53, const val_type v54, const val_type v55 - ) { - COMPILE_ASSERT (nrows == 6); - COMPILE_ASSERT (ncols == 6); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - mData[3] = v03; - mData[4] = v04; - mData[5] = v05; - - mData[6 + 0] = v10; - mData[6 + 1] = v11; - mData[6 + 2] = v12; - mData[6 + 3] = v13; - mData[6 + 4] = v14; - mData[6 + 5] = v15; - - mData[12 + 0] = v20; - mData[12 + 1] = v21; - mData[12 + 2] = v22; - mData[12 + 3] = v23; - mData[12 + 4] = v24; - mData[12 + 5] = v25; - - mData[18 + 0] = v30; - mData[18 + 1] = v31; - mData[18 + 2] = v32; - mData[18 + 3] = v33; - mData[18 + 4] = v34; - mData[18 + 5] = v35; - - mData[24 + 0] = v40; - mData[24 + 1] = v41; - mData[24 + 2] = v42; - mData[24 + 3] = v43; - mData[24 + 4] = v44; - mData[24 + 5] = v45; - - mData[30 + 0] = v50; - mData[30 + 1] = v51; - mData[30 + 2] = v52; - mData[30 + 3] = v53; - mData[30 + 4] = v54; - mData[30 + 5] = v55; - } - - // comparison - bool operator==(const Matrix &matrix) const { - for (unsigned int i = 0; i < nrows * ncols; i++) { - if (mData[i] != matrix.mData[i]) - return false; - } - return true; - } - bool operator!=(const Matrix &matrix) const { - for (unsigned int i = 0; i < nrows * ncols; i++) { - if (mData[i] != matrix.mData[i]) - return true; - } - return false; - } - - // access operators - const val_type& operator[](const unsigned int &index) const { - assert (index >= 0 && index < nrows * ncols); - return mData[index]; - }; - val_type& operator[](const unsigned int &index) { - assert (index >= 0 && index < nrows * ncols); - return mData[index]; - } - - const val_type& operator()(const unsigned int &row, const unsigned int &col) const { - assert (row >= 0 && row < nrows && col >= 0 && col < ncols); - return mData[row*ncols + col]; - }; - val_type& operator()(const unsigned int &row, const unsigned int &col) { - assert (row >= 0 && row < nrows && col >= 0 && col < ncols); - return mData[row*ncols + col]; - }; - - void zero() { - for (unsigned int i = 0; i < ncols * nrows; i++) - mData[i] = 0.; - } - void setZero() { - zero(); - } - - val_type norm() const { - return sqrt(this->squaredNorm()); - } - - matrix_type normalize() { - val_type length = this->norm(); - - for (unsigned int i = 0; i < ncols * nrows; i++) - mData[i] /= length; - - return *this; - } - - matrix_type normalized() const { - return matrix_type (*this) / this->norm(); - } - - Matrix cross(const Matrix &other_vector) const { - COMPILE_ASSERT (nrows * ncols == 3); - - Matrix result; - result[0] = mData[1] * other_vector[2] - mData[2] * other_vector[1]; - result[1] = mData[2] * other_vector[0] - mData[0] * other_vector[2]; - result[2] = mData[0] * other_vector[1] - mData[1] * other_vector[0]; - - return result; - } - - val_type trace() const { - COMPILE_ASSERT(nrows == ncols); - val_type result = 0.; - - for (unsigned int i = 0; i < rows(); i++) { - result += operator()(i,i); - } - - return result; - } - - val_type mean() const { - COMPILE_ASSERT(nrows == 1 || ncols == 1); - - val_type result = 0.; - for (unsigned int i = 0; i < rows() * cols(); i++) { - result += operator[](i); - } - - return result / static_cast(nrows * ncols); - } - - static matrix_type Zero() { - matrix_type result; - result.setZero(); - return result; - } - - static matrix_type Zero(int ignore_me) { - matrix_type result; - result.setZero(); - return result; - } - - static matrix_type Zero(int ignore_me, int ignore_me_too) { - matrix_type result; - result.setZero(); - return result; - } - - static matrix_type Identity() { - matrix_type result; - result.identity(); - return result; - } - - static matrix_type Identity(int ignore_me, int ignore_me_too) { - matrix_type result; - result.identity(); - return result; - } - - void identity() { - COMPILE_ASSERT (nrows == ncols); - - setZero(); - for (unsigned int i = 0; i < ncols; i++) - mData[i * ncols + i] = 1.; - } - - void random() { - for (unsigned int i = 0; i < nrows * ncols; i++) - mData[i] = static_cast (rand()) / static_cast (RAND_MAX); - } - - val_type squaredNorm() const { - COMPILE_ASSERT (ncols == 1 || nrows == 1); - val_type result = 0; - - for (unsigned int i = 0; i < nrows * ncols; i++) - result += mData[i] * mData[i]; - - return result; - } - - val_type dot(const matrix_type &matrix) const { - COMPILE_ASSERT (ncols == 1 || nrows == 1); - val_type result = 0; - - for (unsigned int i = 0; i < nrows * ncols; i++) - result += mData[i] * matrix[i]; - - return result; - } - - // Blocks using block(i,j,r,c) syntax - Block - block (unsigned int row_start, unsigned int col_start, unsigned int row_count, unsigned int col_count) { - return Block(*this, row_start, col_start, row_count, col_count); - } - - const Block - block (unsigned int row_start, unsigned int col_start, unsigned int row_count, unsigned int col_count) const { - return Block(*this, row_start, col_start, row_count, col_count); - } - - // Blocks using block(i,j) syntax - template - Block - block (unsigned int row_start, unsigned int col_start) { - return Block(*this, row_start, col_start, block_row_count, block_col_count); - } - - template - const Block - block (unsigned int row_start, unsigned int col_start) const { - return Block(*this, row_start, col_start, block_row_count, block_col_count); - } - - // row and col accessors - Block - col(unsigned int index) const { - return Block(*this, 0, index, rows(), 1); - } - - Block - col(unsigned int index) { - return Block(*this, 0, index, rows(), 1); - } - - Block - row(unsigned int index) const { - return Block(*this, index, 0, 1, cols()); - } - - Block - row(unsigned int index) { - return Block(*this, index, 0, 1, cols()); - } - - // Operators with scalars - void operator*=(const val_type &scalar) { - for (unsigned int i = 0; i < nrows * ncols; i++) - mData[i] *= scalar; - }; - void operator/=(const val_type &scalar) { - for (unsigned int i = 0; i < nrows * ncols; i++) - mData[i] /= scalar; - } - Matrix operator/(const val_type& scalar) const { - matrix_type result (*this); - - for (unsigned int i = 0; i < nrows * ncols; i++) - result[i] /= scalar; - - return result; - } - - // Operators with other matrices - Matrix operator+(const Matrix &matrix) const { - matrix_type result (*this); - - for (unsigned int i = 0; i < nrows * ncols; i++) - result[i] += matrix[i]; - - return result; - } - void operator+=(const matrix_type &matrix) { - for (unsigned int i = 0; i < nrows * ncols; i++) - mData[i] += matrix.mData[i]; - } - Matrix operator-(const Matrix &matrix) const { - matrix_type result (*this); - - for (unsigned int i = 0; i < nrows * ncols; i++) - result[i] -= matrix[i]; - - return result; - } - void operator-=(const Matrix &matrix) { - for (unsigned int i = 0; i < nrows * ncols; i++) - mData[i] -= matrix.mData[i]; - } - - template - Matrix operator*(const Matrix &matrix) const { - COMPILE_ASSERT (ncols == other_rows); - - Matrix result; - - result.setZero(); - - unsigned int i,j, k; - for (i = 0; i < nrows; i++) { - for (j = 0; j < other_cols; j++) { - for (k = 0; k < other_rows; k++) { - result(i,j) += mData[i * ncols + k] * matrix(k,j); - } - } - } - - return result; - } - - // multiplication with dynamic sized matrix - template - Dynamic::Matrix operator*(const Dynamic::Matrix &other_matrix) { - assert (ncols == other_matrix.rows()); - - Dynamic::Matrix result(nrows, other_matrix.cols()); - - result.setZero(); - - unsigned int i,j, k; - for (i = 0; i < nrows; i++) { - for (j = 0; j < other_matrix.cols(); j++) { - for (k = 0; k < other_matrix.rows(); k++) { - result(i,j) += mData[i * ncols + k] * static_cast(other_matrix(k,j)); - } - } - } - - return result; - } - - void operator*=(const Matrix &matrix) { - matrix_type temp (*this); - *this = temp * matrix; - } - - // Special operators - val_type *data(){ - return mData; - } - - const val_type *data() const{ - return mData; - } - - // regular transpose of a 6 dimensional matrix - Matrix transpose() const { - Matrix result; - - for (unsigned int i = 0; i < nrows; i++) { - for (unsigned int j = 0; j < ncols; j++) { - result(j,i) = mData[i * ncols + j]; - } - } - - return result; - } - - operator val_type() { - COMPILE_ASSERT (nrows == 1); - COMPILE_ASSERT (nrows == 1); - - return mData[0]; - } - - Matrix operator-() const { - return *this * -1.; - } - - Matrix inverse() const { - return colPivHouseholderQr().inverse(); - } - - const LLT llt() const { - return LLT(*this); - } - - const HouseholderQR householderQr() const { - return HouseholderQR(*this); - } - const ColPivHouseholderQR colPivHouseholderQr() const { - return ColPivHouseholderQR(*this); - } - - private: - val_type mData[nrows * ncols]; -}; - -template -inline Matrix operator*(val_type scalar, const Matrix &matrix) { - Matrix result (matrix); - - for (unsigned int i = 0; i < nrows * ncols; i++) - result.data()[i] *= scalar; - - return result; -} - -template -inline Matrix operator*(const Matrix &matrix, other_type scalar) { - Matrix result (matrix); - - for (unsigned int i = 0; i < nrows * ncols; i++) - result.data()[i] *= static_cast (scalar); - - return result; -} - -template -inline std::ostream& operator<<(std::ostream& output, const Matrix &matrix) { - size_t max_width = 0; - size_t out_width = output.width(); - - // get the widest number - for (size_t i = 0; i < matrix.rows(); i++) { - for (size_t j = 0; j < matrix.cols(); j++) { - std::stringstream out_stream; - out_stream << matrix(i,j); - max_width = std::max (out_stream.str().size(),max_width); - } - } - - // overwrite width if it was explicitly prescribed - if (out_width != 0) { - max_width = out_width; - } - - for (unsigned int i = 0; i < matrix.rows(); i++) { - output.width(0); - output << "[ "; - output.width(out_width); - for (unsigned int j = 0; j < matrix.cols(); j++) { - std::stringstream out_stream; - out_stream.width (max_width); - out_stream << matrix(i,j); - output << out_stream.str(); - - if (j < matrix.cols() - 1) - output << ", "; - } - output << " ]"; - - if (matrix.rows() > 1 && i < matrix.rows() - 1) - output << std::endl; - } - return output; -} - -} - -} - -#endif /* SIMPLEMATHFIXED_H */ diff --git a/src/SimpleMath/SimpleMathFixed.h.orig b/src/SimpleMath/SimpleMathFixed.h.orig deleted file mode 100644 index 132f075..0000000 --- a/src/SimpleMath/SimpleMathFixed.h.orig +++ /dev/null @@ -1,825 +0,0 @@ -/** - * This is a highly inefficient math library. It was conceived by Martin - * Felis while he was compiling code - * that uses a highly efficient math library. - * - * It is intended to be used as a fast compiling substitute for the - * blazingly fast Eigen3 library and tries to mimic its API to a certain - * extend. - * - * Feel free to use it wherever you like. However, no guarantees are given - * that this code does what it says it would. - */ - -#ifndef SIMPLEMATHFIXED_H -#define SIMPLEMATHFIXED_H - -#include -#include -#include -#include -#include - -#include "compileassert.h" -#include "SimpleMathBlock.h" - -/** \brief Namespace for a highly inefficient math library - * - */ -namespace SimpleMath { - -template -class LLT; - -template -class PartialPivLU; - -template -class HouseholderQR; - -template -class ColPivHouseholderQR; - -template -class CommaInitializer; - -namespace Dynamic { -template class Matrix; -} - -/** \brief Namespace for fixed size elements - */ -namespace Fixed { - -// forward declaration -template -class Matrix; - -/** \brief Fixed size matrix class - */ -template -class Matrix { - public: - typedef Matrix matrix_type; - typedef val_type value_type; - - unsigned int rows() const { - return nrows; - } - - unsigned int cols() const { - return ncols; - } - - unsigned int size() const { - return nrows * ncols; - } - - Matrix() {}; - Matrix(const Matrix &matrix) { - unsigned int i; - for (i = 0; i < nrows * ncols; i++) - mData[i] = matrix.mData[i]; - } - Matrix& operator=(const Matrix &matrix) { - if (this != &matrix) { - unsigned int i; - for (i = 0; i < nrows * ncols; i++) - mData[i] = matrix.mData[i]; - } - return *this; - } - - // conversion different val_types - - template - Matrix (const Block &block) { - assert (nrows == block.rows()); - assert (ncols == block.cols()); - - for (unsigned int i = 0; i < nrows; i++) { - for (unsigned int j = 0; j < ncols; j++) { - (*this)(i,j) = static_cast(block(i,j)); - } - } - } - template - Matrix& operator= (const Block &block) { - assert (nrows == block.rows()); - assert (ncols == block.cols()); - - for (unsigned int i = 0; i < nrows; i++) { - for (unsigned int j = 0; j < ncols; j++) { - (*this)(i,j) = static_cast(block(i,j)); - } - } - - return *this; - } - - template - Matrix (const Matrix &matrix) { - for (unsigned int i = 0; i < nrows; i++) { - for (unsigned int j = 0; j < ncols; j++) { - (*this)(i,j) = static_cast(matrix(i,j)); - } - } - } - - template - Matrix& operator=(const Matrix &matrix) { - for (unsigned int i = 0; i < nrows; i++) { - for (unsigned int j = 0; j < ncols; j++) { - (*this)(i,j) = static_cast(matrix(i,j)); - } - } - - return *this; - } - - CommaInitializer operator<< (const val_type& value) { - return CommaInitializer (*this, value); - } - - // conversion Dynamic->Fixed - Matrix(const Dynamic::Matrix &dynamic_matrix); - Matrix& operator=(const Dynamic::Matrix &dynamic_matrix); - - ~Matrix() {}; - - Matrix ( - const val_type &v00, const val_type &v01, const val_type &v02 - ) { - assert (nrows == 3); - assert (ncols == 1); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - } - - void set( - const val_type &v00, const val_type &v01, const val_type &v02 - ) { - COMPILE_ASSERT (nrows * ncols == 3); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - } - - Matrix ( - const val_type &v00, const val_type &v01, const val_type &v02, - const val_type &v10, const val_type &v11, const val_type &v12, - const val_type &v20, const val_type &v21, const val_type &v22 - ) { - COMPILE_ASSERT (nrows == 3); - COMPILE_ASSERT (ncols == 3); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - - mData[1 * 3 + 0] = v10; - mData[1 * 3 + 1] = v11; - mData[1 * 3 + 2] = v12; - - mData[2 * 3 + 0] = v20; - mData[2 * 3 + 1] = v21; - mData[2 * 3 + 2] = v22; - } - - void set( - const val_type v00, const val_type v01, const val_type v02, - const val_type v10, const val_type v11, const val_type v12, - const val_type v20, const val_type v21, const val_type v22 - ) { - COMPILE_ASSERT (nrows == 3); - COMPILE_ASSERT (ncols == 3); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - - mData[1 * 3 + 0] = v10; - mData[1 * 3 + 1] = v11; - mData[1 * 3 + 2] = v12; - - mData[2 * 3 + 0] = v20; - mData[2 * 3 + 1] = v21; - mData[2 * 3 + 2] = v22; - } - - Matrix ( - const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03 - ) { - assert (nrows == 4); - assert (ncols == 1); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - mData[3] = v03; - } - - void set( - const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03 - ) { - COMPILE_ASSERT (nrows * ncols == 4); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - mData[3] = v03; - } - - Matrix ( - const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03, - const val_type &v10, const val_type &v11, const val_type &v12, const val_type &v13, - const val_type &v20, const val_type &v21, const val_type &v22, const val_type &v23, - const val_type &v30, const val_type &v31, const val_type &v32, const val_type &v33 - ) { - COMPILE_ASSERT (nrows == 4); - COMPILE_ASSERT (ncols == 4); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - mData[3] = v03; - - mData[1 * 4 + 0] = v10; - mData[1 * 4 + 1] = v11; - mData[1 * 4 + 2] = v12; - mData[1 * 4 + 3] = v13; - - mData[2 * 4 + 0] = v20; - mData[2 * 4 + 1] = v21; - mData[2 * 4 + 2] = v22; - mData[2 * 4 + 3] = v23; - - mData[3 * 4 + 0] = v30; - mData[3 * 4 + 1] = v31; - mData[3 * 4 + 2] = v32; - mData[3 * 4 + 3] = v33; - } - - void set( - const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03, - const val_type &v10, const val_type &v11, const val_type &v12, const val_type &v13, - const val_type &v20, const val_type &v21, const val_type &v22, const val_type &v23, - const val_type &v30, const val_type &v31, const val_type &v32, const val_type &v33 - ) { - COMPILE_ASSERT (nrows == 4); - COMPILE_ASSERT (ncols == 4); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - mData[3] = v03; - - mData[1 * 4 + 0] = v10; - mData[1 * 4 + 1] = v11; - mData[1 * 4 + 2] = v12; - mData[1 * 4 + 3] = v13; - - mData[2 * 4 + 0] = v20; - mData[2 * 4 + 1] = v21; - mData[2 * 4 + 2] = v22; - mData[2 * 4 + 3] = v23; - - mData[3 * 4 + 0] = v30; - mData[3 * 4 + 1] = v31; - mData[3 * 4 + 2] = v32; - mData[3 * 4 + 3] = v33; - } - - Matrix ( - const val_type &v00, const val_type &v01, const val_type &v02, - const val_type &v03, const val_type &v04, const val_type &v05 - ) { - COMPILE_ASSERT (nrows == 6); - COMPILE_ASSERT (ncols == 1); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - mData[3] = v03; - mData[4] = v04; - mData[5] = v05; - } - - void set( - const val_type &v00, const val_type &v01, const val_type &v02, - const val_type &v03, const val_type &v04, const val_type &v05 - ) { - COMPILE_ASSERT (nrows * ncols == 6); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - mData[3] = v03; - mData[4] = v04; - mData[5] = v05; - } - - Matrix ( - const val_type &v00, const val_type &v01, const val_type &v02, - const val_type &v03, const val_type &v04, const val_type &v05, - - const val_type &v10, const val_type &v11, const val_type &v12, - const val_type &v13, const val_type &v14, const val_type &v15, - - const val_type &v20, const val_type &v21, const val_type &v22, - const val_type &v23, const val_type &v24, const val_type &v25, - - const val_type &v30, const val_type &v31, const val_type &v32, - const val_type &v33, const val_type &v34, const val_type &v35, - - const val_type &v40, const val_type &v41, const val_type &v42, - const val_type &v43, const val_type &v44, const val_type &v45, - - const val_type &v50, const val_type &v51, const val_type &v52, - const val_type &v53, const val_type &v54, const val_type &v55 - ) { - COMPILE_ASSERT (nrows == 6); - COMPILE_ASSERT (ncols == 6); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - mData[3] = v03; - mData[4] = v04; - mData[5] = v05; - - mData[6 + 0] = v10; - mData[6 + 1] = v11; - mData[6 + 2] = v12; - mData[6 + 3] = v13; - mData[6 + 4] = v14; - mData[6 + 5] = v15; - - mData[12 + 0] = v20; - mData[12 + 1] = v21; - mData[12 + 2] = v22; - mData[12 + 3] = v23; - mData[12 + 4] = v24; - mData[12 + 5] = v25; - - mData[18 + 0] = v30; - mData[18 + 1] = v31; - mData[18 + 2] = v32; - mData[18 + 3] = v33; - mData[18 + 4] = v34; - mData[18 + 5] = v35; - - mData[24 + 0] = v40; - mData[24 + 1] = v41; - mData[24 + 2] = v42; - mData[24 + 3] = v43; - mData[24 + 4] = v44; - mData[24 + 5] = v45; - - mData[30 + 0] = v50; - mData[30 + 1] = v51; - mData[30 + 2] = v52; - mData[30 + 3] = v53; - mData[30 + 4] = v54; - mData[30 + 5] = v55; - }; - - void set( - const val_type v00, const val_type v01, const val_type v02, - const val_type v03, const val_type v04, const val_type v05, - - const val_type v10, const val_type v11, const val_type v12, - const val_type v13, const val_type v14, const val_type v15, - - const val_type v20, const val_type v21, const val_type v22, - const val_type v23, const val_type v24, const val_type v25, - - const val_type v30, const val_type v31, const val_type v32, - const val_type v33, const val_type v34, const val_type v35, - - const val_type v40, const val_type v41, const val_type v42, - const val_type v43, const val_type v44, const val_type v45, - - const val_type v50, const val_type v51, const val_type v52, - const val_type v53, const val_type v54, const val_type v55 - ) { - COMPILE_ASSERT (nrows == 6); - COMPILE_ASSERT (ncols == 6); - - mData[0] = v00; - mData[1] = v01; - mData[2] = v02; - mData[3] = v03; - mData[4] = v04; - mData[5] = v05; - - mData[6 + 0] = v10; - mData[6 + 1] = v11; - mData[6 + 2] = v12; - mData[6 + 3] = v13; - mData[6 + 4] = v14; - mData[6 + 5] = v15; - - mData[12 + 0] = v20; - mData[12 + 1] = v21; - mData[12 + 2] = v22; - mData[12 + 3] = v23; - mData[12 + 4] = v24; - mData[12 + 5] = v25; - - mData[18 + 0] = v30; - mData[18 + 1] = v31; - mData[18 + 2] = v32; - mData[18 + 3] = v33; - mData[18 + 4] = v34; - mData[18 + 5] = v35; - - mData[24 + 0] = v40; - mData[24 + 1] = v41; - mData[24 + 2] = v42; - mData[24 + 3] = v43; - mData[24 + 4] = v44; - mData[24 + 5] = v45; - - mData[30 + 0] = v50; - mData[30 + 1] = v51; - mData[30 + 2] = v52; - mData[30 + 3] = v53; - mData[30 + 4] = v54; - mData[30 + 5] = v55; - } - - // comparison - bool operator==(const Matrix &matrix) const { - for (unsigned int i = 0; i < nrows * ncols; i++) { - if (mData[i] != matrix.mData[i]) - return false; - } - return true; - } - bool operator!=(const Matrix &matrix) const { - for (unsigned int i = 0; i < nrows * ncols; i++) { - if (mData[i] != matrix.mData[i]) - return true; - } - return false; - } - - // access operators - const val_type& operator[](const unsigned int &index) const { - assert (index >= 0 && index < nrows * ncols); - return mData[index]; - }; - val_type& operator[](const unsigned int &index) { - assert (index >= 0 && index < nrows * ncols); - return mData[index]; - } - - const val_type& operator()(const unsigned int &row, const unsigned int &col) const { - assert (row >= 0 && row < nrows && col >= 0 && col < ncols); - return mData[row*ncols + col]; - }; - val_type& operator()(const unsigned int &row, const unsigned int &col) { - assert (row >= 0 && row < nrows && col >= 0 && col < ncols); - return mData[row*ncols + col]; - }; - - void zero() { - for (unsigned int i = 0; i < ncols * nrows; i++) - mData[i] = 0.; - } - void setZero() { - zero(); - } - - val_type norm() const { - return sqrt(this->squaredNorm()); - } - - matrix_type normalize() { - val_type length = this->norm(); - - for (unsigned int i = 0; i < ncols * nrows; i++) - mData[i] /= length; - - return *this; - } - - matrix_type normalized() const { - return matrix_type (*this) / this->norm(); - } - - Matrix cross(const Matrix &other_vector) const { - COMPILE_ASSERT (nrows * ncols == 3); - - Matrix result; - result[0] = mData[1] * other_vector[2] - mData[2] * other_vector[1]; - result[1] = mData[2] * other_vector[0] - mData[0] * other_vector[2]; - result[2] = mData[0] * other_vector[1] - mData[1] * other_vector[0]; - - return result; - } - - static matrix_type Zero() { - matrix_type result; - result.setZero(); - return result; - } - - static matrix_type Zero(int ignore_me) { - matrix_type result; - result.setZero(); - return result; - } - - static matrix_type Zero(int ignore_me, int ignore_me_too) { - matrix_type result; - result.setZero(); - return result; - } - - static matrix_type Identity() { - matrix_type result; - result.identity(); - return result; - } - - static matrix_type Identity(int ignore_me, int ignore_me_too) { - matrix_type result; - result.identity(); - return result; - } - - void identity() { - COMPILE_ASSERT (nrows == ncols); - - setZero(); - for (unsigned int i = 0; i < ncols; i++) - mData[i * ncols + i] = 1.; - } - - void random() { - for (unsigned int i = 0; i < nrows * ncols; i++) - mData[i] = static_cast (rand()) / static_cast (RAND_MAX); - } - - val_type squaredNorm() const { - COMPILE_ASSERT (ncols == 1 || nrows == 1); - val_type result = 0; - - for (unsigned int i = 0; i < nrows * ncols; i++) - result += mData[i] * mData[i]; - - return result; - } - - val_type dot(const matrix_type &matrix) const { - COMPILE_ASSERT (ncols == 1 || nrows == 1); - val_type result = 0; - - for (unsigned int i = 0; i < nrows * ncols; i++) - result += mData[i] * matrix[i]; - - return result; - } - - // Blocks using block(i,j,r,c) syntax - Block - block (unsigned int row_start, unsigned int col_start, unsigned int row_count, unsigned int col_count) { - return Block(*this, row_start, col_start, row_count, col_count); - } - - const Block - block (unsigned int row_start, unsigned int col_start, unsigned int row_count, unsigned int col_count) const { - return Block(*this, row_start, col_start, row_count, col_count); - } - - // Blocks using block(i,j) syntax - template - Block - block (unsigned int row_start, unsigned int col_start) { - return Block(*this, row_start, col_start, block_row_count, block_col_count); - } - - template - const Block - block (unsigned int row_start, unsigned int col_start) const { - return Block(*this, row_start, col_start, block_row_count, block_col_count); - } - - // Operators with scalars - void operator*=(const val_type &scalar) { - for (unsigned int i = 0; i < nrows * ncols; i++) - mData[i] *= scalar; - }; - void operator/=(const val_type &scalar) { - for (unsigned int i = 0; i < nrows * ncols; i++) - mData[i] /= scalar; - } - Matrix operator/(const val_type& scalar) const { - matrix_type result (*this); - - for (unsigned int i = 0; i < nrows * ncols; i++) - result[i] /= scalar; - - return result; - } - - // Operators with other matrices - Matrix operator+(const Matrix &matrix) const { - matrix_type result (*this); - - for (unsigned int i = 0; i < nrows * ncols; i++) - result[i] += matrix[i]; - - return result; - } - void operator+=(const matrix_type &matrix) { - for (unsigned int i = 0; i < nrows * ncols; i++) - mData[i] += matrix.mData[i]; - } - Matrix operator-(const Matrix &matrix) const { - matrix_type result (*this); - - for (unsigned int i = 0; i < nrows * ncols; i++) - result[i] -= matrix[i]; - - return result; - } - void operator-=(const Matrix &matrix) { - for (unsigned int i = 0; i < nrows * ncols; i++) - mData[i] -= matrix.mData[i]; - } - - template - Matrix operator*(const Matrix &matrix) const { - COMPILE_ASSERT (ncols == other_rows); - - Matrix result; - - result.setZero(); - - unsigned int i,j, k; - for (i = 0; i < nrows; i++) { - for (j = 0; j < other_cols; j++) { - for (k = 0; k < other_rows; k++) { - result(i,j) += mData[i * ncols + k] * matrix(k,j); - } - } - } - - return result; - } - - // multiplication with dynamic sized matrix - template - Dynamic::Matrix operator*(const Dynamic::Matrix &other_matrix) { - assert (ncols == other_matrix.rows()); - - Dynamic::Matrix result(nrows, other_matrix.cols()); - - result.setZero(); - - unsigned int i,j, k; - for (i = 0; i < nrows; i++) { - for (j = 0; j < other_matrix.cols(); j++) { - for (k = 0; k < other_matrix.rows(); k++) { - result(i,j) += mData[i * ncols + k] * static_cast(other_matrix(k,j)); - } - } - } - - return result; - } - - - void operator*=(const Matrix &matrix) { - matrix_type temp (*this); - *this = temp * matrix; - } - - // Special operators - val_type *data(){ - return mData; - } - - const val_type *data() const{ - return mData; - } - - // regular transpose of a 6 dimensional matrix - Matrix transpose() const { - Matrix result; - - for (unsigned int i = 0; i < nrows; i++) { - for (unsigned int j = 0; j < ncols; j++) { - result(j,i) = mData[i * ncols + j]; - } - } - - return result; - } - - operator val_type() { - COMPILE_ASSERT (nrows == 1); - COMPILE_ASSERT (nrows == 1); - - return mData[0]; - } - - Matrix operator-() const { - return *this * -1.; - } - - Matrix inverse() const { - return colPivHouseholderQr().inverse(); - } - - const PartialPivLU partialPivLU() const { - return PartialPivLU(*this); - } - - const LLT llt() const { - return LLT(*this); - } - - const HouseholderQR householderQr() const { - return HouseholderQR(*this); - } - - const ColPivHouseholderQR colPivHouseholderQr() const { - return ColPivHouseholderQR(*this); - } - - private: - val_type mData[nrows * ncols]; -}; - -template -inline Matrix operator*(val_type scalar, const Matrix &matrix) { - Matrix result (matrix); - - for (unsigned int i = 0; i < nrows * ncols; i++) - result.data()[i] *= scalar; - - return result; -} - -template -inline Matrix operator*(const Matrix &matrix, other_type scalar) { - Matrix result (matrix); - - for (unsigned int i = 0; i < nrows * ncols; i++) - result.data()[i] *= static_cast (scalar); - - return result; -} - -template -inline std::ostream& operator<<(std::ostream& output, const Matrix &matrix) { - size_t max_width = 0; - size_t out_width = output.width(); - - // get the widest number - for (size_t i = 0; i < matrix.rows(); i++) { - for (size_t j = 0; j < matrix.cols(); j++) { - std::stringstream out_stream; - out_stream << matrix(i,j); - max_width = std::max (out_stream.str().size(),max_width); - } - } - - // overwrite width if it was explicitly prescribed - if (out_width != 0) { - max_width = out_width; - } - - for (unsigned int i = 0; i < matrix.rows(); i++) { - output.width(0); - output << "[ "; - output.width(out_width); - for (unsigned int j = 0; j < matrix.cols(); j++) { - std::stringstream out_stream; - out_stream.width (max_width); - out_stream << matrix(i,j); - output << out_stream.str(); - - if (j < matrix.cols() - 1) - output << ", "; - } - output << " ]"; - - if (matrix.rows() > 1 && i < matrix.rows() - 1) - output << std::endl; - } - return output; -} - -} - -} - -#endif /* SIMPLEMATHFIXED_H */ diff --git a/src/SimpleMath/SimpleMathGL.h b/src/SimpleMath/SimpleMathGL.h deleted file mode 100644 index a4c948e..0000000 --- a/src/SimpleMath/SimpleMathGL.h +++ /dev/null @@ -1,346 +0,0 @@ -#ifndef _SIMPLEMATHGL_H_ -#define _SIMPLEMATHGL_H_ - -#include "SimpleMath.h" -#include - -namespace SimpleMath { - -typedef SimpleMath::Fixed::Matrix Vector3f; -typedef SimpleMath::Fixed::Matrix Matrix33f; - -typedef SimpleMath::Fixed::Matrix Vector4f; -typedef SimpleMath::Fixed::Matrix Matrix44f; - -namespace GL { - -inline Matrix33f RotateMat33 (float rot_deg, float x, float y, float z) { - float c = cosf (rot_deg * M_PI / 180.f); - float s = sinf (rot_deg * M_PI / 180.f); - return Matrix33f ( - x * x * (1.0f - c) + c, - y * x * (1.0f - c) + z * s, - x * z * (1.0f - c) - y * s, - - x * y * (1.0f - c) - z * s, - y * y * (1.0f - c) + c, - y * z * (1.0f - c) + x * s, - - x * z * (1.0f - c) + y * s, - y * z * (1.0f - c) - x * s, - z * z * (1.0f - c) + c - - ); -} - - -inline Matrix44f RotateMat44 (float rot_deg, float x, float y, float z) { - float c = cosf (rot_deg * M_PI / 180.f); - float s = sinf (rot_deg * M_PI / 180.f); - return Matrix44f ( - x * x * (1.0f - c) + c, - y * x * (1.0f - c) + z * s, - x * z * (1.0f - c) - y * s, - 0.f, - - x * y * (1.0f - c) - z * s, - y * y * (1.0f - c) + c, - y * z * (1.0f - c) + x * s, - 0.f, - - x * z * (1.0f - c) + y * s, - y * z * (1.0f - c) - x * s, - z * z * (1.0f - c) + c, - 0.f, - - 0.f, 0.f, 0.f, 1.f - ); -} - -inline Matrix44f TranslateMat44 (float x, float y, float z) { - return Matrix44f ( - 1.f, 0.f, 0.f, 0.f, - 0.f, 1.f, 0.f, 0.f, - 0.f, 0.f, 1.f, 0.f, - x, y, z, 1.f - ); -} - -inline Matrix44f ScaleMat44 (float x, float y, float z) { - return Matrix44f ( - x, 0.f, 0.f, 0.f, - 0.f, y, 0.f, 0.f, - 0.f, 0.f, z, 0.f, - 0.f, 0.f, 0.f, 1.f - ); -} - -inline Matrix44f Ortho(float left, float right, - float bottom, float top, - float near, float far) { - float tx = -(right + left) / (right - left); - float ty = -(top + bottom) / (top - bottom); - float tz = -(far + near) / (far - near); - return Matrix44f( - 2.0f / (right - left), 0.0f, 0.0f, 0.0f, - 0, 2.0f / (top - bottom), 0.0f, 0.0f, - 0.0f, 0.0f, -2.0f / (far - near), 0.0f, - tx, ty, tz, 1.0f - ); -} - -inline Matrix44f Perspective(float fovy, float aspect, - float near, float far) { - float x = (fovy * M_PI / 180.0) / 2.0f; - float f = cos(x) / sin(x); - - return Matrix44f( - f / aspect, 0.0f, 0.0f, 0.0f, - 0.0f, f, 0.0f, 0.0f, - 0.0f, 0.0f, (far + near) / (near - far), -1.0f, - 0.0f, 0.0f, (2.0f * far * near) / (near - far), 0.0f - ); -} - -inline Matrix44f Frustum(float left, float right, - float bottom, float top, - float near, float far) { - float A = (right + left) / (right - left); - float B = (top + bottom) / (top - bottom); - float C = -(far + near) / (far - near); - float D = - (2.0f * far * near) / (far - near); - - return Matrix44f( - 2.0f * near / (right - left), 0.0f, 0.0f, 0.0f, - 0.0f, 2.0f * near / (top - bottom), 0.0f, 0.0f, - A, B, C, -1.0f, - 0.0f, 0.0f, D, 0.0f - ); -} - -inline Matrix44f LookAt( - const Vector3f& eye, - const Vector3f& poi, - const Vector3f& up) { - Vector3f d = (poi - eye).normalized(); - Vector3f s = d.cross(up.normalized()).normalized(); - Vector3f u = s.cross(d).normalized(); - - return TranslateMat44(-eye[0], -eye[1], -eye[2]) * Matrix44f( - s[0], u[0], -d[0], 0.0f, - s[1], u[1], -d[1], 0.0f, - s[2], u[2], -d[2], 0.0f, - 0.0f, 0.0f, 0.0f, 1.0f - ); -} - -/** Quaternion - * - * order: x,y,z,w - */ -class Quaternion : public Vector4f { - public: - Quaternion () : - Vector4f (0.f, 0.f, 0.f, 1.f) - {} - Quaternion (const Vector4f vec4) : - Vector4f (vec4) - {} - Quaternion (float x, float y, float z, float w): - Vector4f (x, y, z, w) - {} - /** This function is equivalent to multiplicate their corresponding rotation matrices */ - Quaternion operator* (const Quaternion &q) const { - return Quaternion ( - (*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1], - (*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2], - (*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0], - (*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2] - ); - } - Quaternion& operator*=(const Quaternion &q) { - set ( - (*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1], - (*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2], - (*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0], - (*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2] - ); - return *this; - } - - static Quaternion fromGLRotate (float angle, float x, float y, float z) { - float st = sinf (angle * M_PI / 360.f); - return Quaternion ( - st * x, - st * y, - st * z, - cosf (angle * M_PI / 360.f) - ); - } - - Quaternion normalize() { - return Vector4f::normalize(); - } - - Quaternion slerp (float alpha, const Quaternion &quat) const { - // check whether one of the two has 0 length - float s = sqrt (squaredNorm() * quat.squaredNorm()); - - // division by 0.f is unhealthy! - assert (s != 0.f); - - float angle = acos (dot(quat) / s); - if (angle == 0.f || std::isnan(angle)) { - return *this; - } - assert(!std::isnan(angle)); - - float d = 1.f / sinf (angle); - float p0 = sinf ((1.f - alpha) * angle); - float p1 = sinf (alpha * angle); - - if (dot (quat) < 0.f) { - return Quaternion( ((*this) * p0 - quat * p1) * d); - } - return Quaternion( ((*this) * p0 + quat * p1) * d); - } - - Matrix44f toGLMatrix() const { - float x = (*this)[0]; - float y = (*this)[1]; - float z = (*this)[2]; - float w = (*this)[3]; - return Matrix44f ( - 1 - 2*y*y - 2*z*z, - 2*x*y + 2*w*z, - 2*x*z - 2*w*y, - 0.f, - - 2*x*y - 2*w*z, - 1 - 2*x*x - 2*z*z, - 2*y*z + 2*w*x, - 0.f, - - 2*x*z + 2*w*y, - 2*y*z - 2*w*x, - 1 - 2*x*x - 2*y*y, - 0.f, - - 0.f, - 0.f, - 0.f, - 1.f); - } - - static Quaternion fromGLMatrix(const Matrix44f &mat) { - float w = sqrt (1.f + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5f; - return Quaternion ( - -(mat(2,1) - mat(1,2)) / (w * 4.f), - -(mat(0,2) - mat(2,0)) / (w * 4.f), - -(mat(1,0) - mat(0,1)) / (w * 4.f), - w); - } - - static Quaternion fromMatrix (const Matrix33f &mat) { - float w = sqrt (1.f + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5f; - return Quaternion ( - (mat(2,1) - mat(1,2)) / (w * 4.f), - (mat(0,2) - mat(2,0)) / (w * 4.f), - (mat(1,0) - mat(0,1)) / (w * 4.f), - w); - } - - static Quaternion fromAxisAngle (const Vector3f &axis, double angle_rad) { - double d = axis.norm(); - double s2 = std::sin (angle_rad * 0.5) / d; - return Quaternion ( - axis[0] * s2, - axis[1] * s2, - axis[2] * s2, - std::cos(angle_rad * 0.5) - ); - } - - static Quaternion fromEulerZYX (const Vector3f &zyx_angles) { - return Quaternion::fromAxisAngle (Vector3f (0., 0., 1.), zyx_angles[0]) - * Quaternion::fromAxisAngle (Vector3f (0., 1., 0.), zyx_angles[1]) - * Quaternion::fromAxisAngle (Vector3f (1., 0., 0.), zyx_angles[2]); - } - - static Quaternion fromEulerYXZ (const Vector3f &yxz_angles) { - return Quaternion::fromAxisAngle (Vector3f (0., 1., 0.), yxz_angles[0]) - * Quaternion::fromAxisAngle (Vector3f (1., 0., 0.), yxz_angles[1]) - * Quaternion::fromAxisAngle (Vector3f (0., 0., 1.), yxz_angles[2]); - } - - static Quaternion fromEulerXYZ (const Vector3f &xyz_angles) { - return Quaternion::fromAxisAngle (Vector3f (0., 0., 01.), xyz_angles[2]) - * Quaternion::fromAxisAngle (Vector3f (0., 1., 0.), xyz_angles[1]) - * Quaternion::fromAxisAngle (Vector3f (1., 0., 0.), xyz_angles[0]); - } - - Vector3f toEulerZYX () const { - return Vector3f (1.0f, 2.0f, 3.0f - ); - } - - Vector3f toEulerYXZ() const { - return Vector3f ( - atan2 (-2.f * (*this)[0] * (*this)[2] + 2.f * (*this)[3] * (*this)[1], - (*this)[2] * (*this)[2] - (*this)[1] * (*this)[1] - -(*this)[0] * (*this)[0] + (*this)[3] * (*this)[3]), - asin (2.f * (*this)[1] * (*this)[2] + 2.f * (*this)[3] * (*this)[0]), - atan2 (-2.f * (*this)[0] * (*this)[1] + 2.f * (*this)[3] * (*this)[2], - (*this)[1] * (*this)[1] - (*this)[2] * (*this)[2] - +(*this)[3] * (*this)[3] - (*this)[0] * (*this)[0] - ) - ); - } - - Matrix33f toMatrix() const { - float x = (*this)[0]; - float y = (*this)[1]; - float z = (*this)[2]; - float w = (*this)[3]; - return Matrix33f ( - 1 - 2*y*y - 2*z*z, - 2*x*y - 2*w*z, - 2*x*z + 2*w*y, - - 2*x*y + 2*w*z, - 1 - 2*x*x - 2*z*z, - 2*y*z - 2*w*x, - - 2*x*z - 2*w*y, - 2*y*z + 2*w*x, - 1 - 2*x*x - 2*y*y - ); - } - - Quaternion conjugate() const { - return Quaternion ( - -(*this)[0], - -(*this)[1], - -(*this)[2], - (*this)[3]); - } - - Vector3f rotate (const Vector3f &vec) const { - Vector3f vn (vec); - Quaternion vec_quat (vn[0], vn[1], vn[2], 0.f), res_quat; - - res_quat = (*this) * vec_quat; - res_quat = res_quat * conjugate(); - - return Vector3f (res_quat[0], res_quat[1], res_quat[2]); - } -}; - -// namespace GL -} - -// namespace SimpleMath -} - -/* _SIMPLEMATHGL_H_ */ -#endif diff --git a/src/SimpleMath/SimpleMathLU.h b/src/SimpleMath/SimpleMathLU.h deleted file mode 100644 index 5766a2b..0000000 --- a/src/SimpleMath/SimpleMathLU.h +++ /dev/null @@ -1,107 +0,0 @@ -#ifndef _SIMPLE_MATH_LU_H -#define _SIMPLE_MATH_LU_H - -#include -#include - -#include "SimpleMathFixed.h" -#include "SimpleMathDynamic.h" -#include "SimpleMathBlock.h" - -namespace SimpleMath { - - template - class PartialPivLU { - public: - typedef typename matrix_type::value_type value_type; - - private: - PartialPivLU() {} - - typedef Dynamic::Matrix MatrixXXd; - typedef Dynamic::Matrix VectorXd; - - bool mIsFactorized; - unsigned int *mPermutations; - MatrixXXd matrixLU; - - public: - PartialPivLU(const matrix_type &matrix) : - mIsFactorized(false), - matrixLU(matrix) { - // We can only solve quadratic systems - assert (matrixLU.rows() == matrixLU.cols()); - - mPermutations = new unsigned int [matrix.cols()]; - for (unsigned int i = 0; i < matrix.cols(); i++) { - mPermutations[i] = i; - } - compute(); - } - PartialPivLU compute() { - unsigned int n = matrixLU.rows(); - unsigned int pi; - - unsigned int i,j,k; - - for (j = 0; j < n; j++) { - double pv = fabs (matrixLU(j,mPermutations[j])); - - // LOG << "j = " << j << " pv = " << pv << std::endl; - // find the pivot - for (k = j; k < n; k++) { - double pt = fabs (matrixLU(j,mPermutations[k])); - if (pt > pv) { - pv = pt; - pi = k; - unsigned int p_swap = mPermutations[j]; - mPermutations[j] = mPermutations[pi]; - mPermutations[pi] = p_swap; - // LOG << "swap " << j << " with " << pi << std::endl; - // LOG << "j = " << j << " pv = " << pv << std::endl; - } - } - - for (i = j + 1; i < n; i++) { - if (fabs(A(j,pivot[j])) <= std::numeric_limits::epsilon()) { - std::cerr << "Error: pivoting failed for matrix A = " << std::endl; - std::cerr << "A = " << std::endl << A << std::endl; - std::cerr << "b = " << b << std::endl; - } - // assert (fabs(A(j,pivot[j])) > std::numeric_limits::epsilon()); - double d = A(i,pivot[j])/A(j,pivot[j]); - } - - for (k = j; k < n; k++) { - A(i,pivot[k]) -= A(j,pivot[k]) * d; - } - - } - - mIsFactorized = true; - - return *this; - } - Dynamic::Matrix solve ( - const Dynamic::Matrix &rhs - ) const { - assert (mIsFactorized); - - // temporary result vector which contains the pivoted result - VectorXd px = rhs; - - for (int i = mR.cols() - 1; i >= 0; --i) { - for (j = i + 1; j < n; j++) { - px[i] += A(i, pivot[j]) * px[j]; - } - px[i] = (b[i] - px[i]) / A (i, pivot[i]); - } - - return x; - } - }; - -} - -/* _SIMPLE_MATH_LU_H */ -#endif diff --git a/src/SimpleMath/SimpleMathMap.h b/src/SimpleMath/SimpleMathMap.h deleted file mode 100644 index dbb757a..0000000 --- a/src/SimpleMath/SimpleMathMap.h +++ /dev/null @@ -1,22 +0,0 @@ -#ifndef SIMPLEMATHMAP_H -#define SIMPLEMATHMAP_H - -#include "compileassert.h" - -namespace SimpleMath { - -/** \brief \brief Wraps a varying size matrix type around existing data - * - * \warning If you create a matrix using the map function and then assign - * a bigger matrix you invalidate the original memory! - * - */ -template < typename MatrixType > -MatrixType Map (typename MatrixType::value_type *data, unsigned int rows, unsigned int cols) { - return MatrixType (rows, cols, data); -} - -} - -// SIMPLEMATHMAP_H -#endif diff --git a/src/SimpleMath/SimpleMathMixed.h b/src/SimpleMath/SimpleMathMixed.h deleted file mode 100644 index 777670f..0000000 --- a/src/SimpleMath/SimpleMathMixed.h +++ /dev/null @@ -1,138 +0,0 @@ -/** - * This is a highly inefficient math library. It was conceived by Martin - * Felis while he was compiling code - * that uses a highly efficient math library. - * - * It is intended to be used as a fast compiling substitute for the - * blazingly fast Eigen3 library and tries to mimic its API to a certain - * extend. - * - * Feel free to use it wherever you like. However, no guarantees are given - * that this code does what it says it would. - */ - -#ifndef SIMPLEMATHMIXED_H -#define SIMPLEMATHMIXED_H - -#include -#include -#include -#include - -#include "compileassert.h" - -/** \brief Namespace for a highly inefficient math library - * - */ -namespace SimpleMath { - -// conversion Dynamic->Fixed -template -inline Fixed::Matrix::Matrix(const Dynamic::Matrix &dynamic_matrix) { - if (dynamic_matrix.cols() != ncols - || dynamic_matrix.rows() != nrows) { - std::cerr << "Error: cannot assign a dynamic sized matrix of size " << dynamic_matrix.rows() << "x" << dynamic_matrix.cols() << " to a fixed size matrix of size " << nrows << "x" << ncols << "!" << std::endl; - abort(); - } - - for (unsigned int i = 0; i < nrows * ncols; i++) { - mData[i] = dynamic_matrix[i]; - } -} - -template -inline Fixed::Matrix& Fixed::Matrix::operator=(const Dynamic::Matrix &dynamic_matrix) { - if (dynamic_matrix.cols() != ncols - || dynamic_matrix.rows() != nrows) { - std::cerr << "Error: cannot assign a dynamic sized matrix of size " << dynamic_matrix.rows() << "x" << dynamic_matrix.cols() << " to a fixed size matrix of size " << nrows << "x" << ncols << "!" << std::endl; - abort(); - } - - for (unsigned int i = 0; i < nrows * ncols; i++) { - mData[i] = dynamic_matrix[i]; - } - - return *this; -} - -// multiplication -template -inline Dynamic::Matrix operator*( - const Fixed::Matrix &matrix_a, - const Dynamic::Matrix &matrix_b) { - assert (matrix_a.cols() == matrix_b.rows()); - - Dynamic::Matrix result (nrows, matrix_b.cols()); - - result.setZero(); - - unsigned int i,j, k; - for (i = 0; i < nrows; i++) { - for (j = 0; j < matrix_b.cols(); j++) { - for (k = 0; k < matrix_b.rows(); k++) { - result(i,j) += matrix_a(i,k) * matrix_b(k,j); - } - } - } - - return result; -} - -template -inline Dynamic::Matrix operator*( - const Dynamic::Matrix &matrix_a, - const Fixed::Matrix &matrix_b) { - assert (matrix_a.cols() == matrix_b.rows()); - - Dynamic::Matrix result (matrix_a.rows(), ncols); - - result.setZero(); - - unsigned int i,j, k; - for (i = 0; i < matrix_a.rows(); i++) { - for (j = 0; j < matrix_b.cols(); j++) { - for (k = 0; k < matrix_b.rows(); k++) { - result(i,j) += matrix_a(i,k) * matrix_b(k,j); - } - } - } - - return result; -} - -// equality -template -inline bool operator==( - const Fixed::Matrix &matrix_a, - const Dynamic::Matrix &matrix_b) { - assert (nrows == matrix_a.rows()); - assert (ncols == matrix_a.cols()); - - unsigned int i; - for (i = 0; i < matrix_a.size(); i++) { - if (matrix_a[i] != matrix_b[i]) - return false; - } - - return true; -} - -template -inline bool operator==( - const Dynamic::Matrix &matrix_b, - const Fixed::Matrix &matrix_a) { - assert (nrows == matrix_a.rows()); - assert (ncols == matrix_a.cols()); - - unsigned int i; - for (i = 0; i < matrix_a.size(); i++) { - if (matrix_a[i] != matrix_b[i]) - return false; - } - - return true; -} - -} - -#endif /* SIMPLEMATHMIXED_H */ diff --git a/src/SimpleMath/SimpleMathQR.h b/src/SimpleMath/SimpleMathQR.h deleted file mode 100644 index 31b0add..0000000 --- a/src/SimpleMath/SimpleMathQR.h +++ /dev/null @@ -1,324 +0,0 @@ -#ifndef _SIMPLE_MATH_QR_H -#define _SIMPLE_MATH_QR_H - -#include -#include - -#include "SimpleMathFixed.h" -#include "SimpleMathDynamic.h" -#include "SimpleMathBlock.h" - -namespace SimpleMath { - - template - class HouseholderQR { - public: - typedef typename matrix_type::value_type value_type; - HouseholderQR() : - mIsFactorized(false) - {} - - private: - typedef Dynamic::Matrix MatrixXXd; - typedef Dynamic::Matrix VectorXd; - - bool mIsFactorized; - MatrixXXd mQ; - MatrixXXd mR; - - public: - HouseholderQR(const matrix_type &matrix) : - mIsFactorized(false), - mQ(matrix.rows(), matrix.rows()) - { - compute(matrix); - } - HouseholderQR compute(const matrix_type &matrix) { - mR = matrix; - mQ = Dynamic::Matrix::Identity (mR.rows(), mR.rows()); - - for (unsigned int i = 0; i < mR.cols(); i++) { - unsigned int block_rows = mR.rows() - i; - unsigned int block_cols = mR.cols() - i; - - MatrixXXd current_block = mR.block(i,i, block_rows, block_cols); - VectorXd column = current_block.block(0, 0, block_rows, 1); - - value_type alpha = - column.norm(); - if (current_block(0,0) < 0) { - alpha = - alpha; - } - - VectorXd v = current_block.block(0, 0, block_rows, 1); - v[0] = v[0] - alpha; - - MatrixXXd Q (MatrixXXd::Identity(mR.rows(), mR.rows())); - Q.block(i, i, block_rows, block_rows) = MatrixXXd (Q.block(i, i, block_rows, block_rows)) - - MatrixXXd(v * v.transpose() / (v.squaredNorm() * 0.5)); - - mR = Q * mR; - - // Normalize so that we have positive diagonal elements - if (mR(i,i) < 0) { - mR.block(i,i,block_rows, block_cols) = MatrixXXd(mR.block(i,i,block_rows, block_cols)) * -1.; - Q.block(i,i,block_rows, block_rows) = MatrixXXd(Q.block(i,i,block_rows, block_rows)) * -1.; - } - - mQ = mQ * Q; - } - - mIsFactorized = true; - - return *this; - } - Dynamic::Matrix solve ( - const Dynamic::Matrix &rhs - ) const { - assert (mIsFactorized); - - VectorXd y = mQ.transpose() * rhs; - VectorXd x = VectorXd::Zero(mR.cols()); - - for (int i = mR.cols() - 1; i >= 0; --i) { - value_type z = y[i]; - - for (unsigned int j = i + 1; j < mR.cols(); j++) { - z = z - x[j] * mR(i,j); - } - - if (fabs(mR(i,i)) < std::numeric_limits::epsilon() * 10) { - std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero!" << std::endl; - abort(); - } - x[i] = z / mR(i,i); - } - - return x; - } - Dynamic::Matrix inverse() const { - assert (mIsFactorized); - - VectorXd rhs_temp = VectorXd::Zero(mQ.cols()); - MatrixXXd result (mQ.cols(), mQ.cols()); - - for (unsigned int i = 0; i < mQ.cols(); i++) { - rhs_temp[i] = 1.; - - result.block(0, i, mQ.cols(), 1) = solve(rhs_temp); - - rhs_temp[i] = 0.; - } - - return result; - } - Dynamic::Matrix householderQ () const { - return mQ; - } - Dynamic::Matrix matrixR () const { - return mR; - } - }; - - template - class ColPivHouseholderQR { - public: - typedef typename matrix_type::value_type value_type; - private: - typedef Dynamic::Matrix MatrixXXd; - typedef Dynamic::Matrix VectorXd; - - bool mIsFactorized; - MatrixXXd mQ; - MatrixXXd mR; - unsigned int *mPermutations; - value_type mThreshold; - unsigned int mRank; - - public: - ColPivHouseholderQR(): - mIsFactorized(false) { - mPermutations = new unsigned int[1]; - } - - ColPivHouseholderQR (const ColPivHouseholderQR& other) { - mIsFactorized = other.mIsFactorized; - mQ = other.mQ; - mR = other.mR; - mPermutations = new unsigned int[mQ.cols()]; - mThreshold = other.mThreshold; - mRank = other.mRank; - } - - ColPivHouseholderQR& operator= (const ColPivHouseholderQR& other) { - if (this != &other) { - mIsFactorized = other.mIsFactorized; - mQ = other.mQ; - mR = other.mR; - delete[] mPermutations; - mPermutations = new unsigned int[mQ.cols()]; - mThreshold = other.mThreshold; - mRank = other.mRank; - } - - return *this; - } - - ColPivHouseholderQR(const matrix_type &matrix) : - mIsFactorized(false), - mQ(matrix.rows(), matrix.rows()), - mThreshold (std::numeric_limits::epsilon() * matrix.cols()) { - mPermutations = new unsigned int [matrix.cols()]; - for (unsigned int i = 0; i < matrix.cols(); i++) { - mPermutations[i] = i; - } - compute(matrix); - } - ~ColPivHouseholderQR() { - delete[] mPermutations; - } - - ColPivHouseholderQR& setThreshold (const value_type& threshold) { - mThreshold = threshold; - - return *this; - } - ColPivHouseholderQR& compute(const matrix_type &matrix) { - mR = matrix; - mQ = Dynamic::Matrix::Identity (mR.rows(), mR.rows()); - - for (unsigned int i = 0; i < mR.cols(); i++) { - unsigned int block_rows = mR.rows() - i; - unsigned int block_cols = mR.cols() - i; - - // find and swap the column with the highest norm - unsigned int col_index_norm_max = i; - value_type col_norm_max = VectorXd(mR.block(i,i, block_rows, 1)).squaredNorm(); - - for (unsigned int j = i + 1; j < mR.cols(); j++) { - VectorXd column = mR.block(i, j, block_rows, 1); - - if (column.squaredNorm() > col_norm_max) { - col_index_norm_max = j; - col_norm_max = column.squaredNorm(); - } - } - - if (col_norm_max < mThreshold) { - // if all entries of the column is close to zero, we bail out - break; - } - - - if (col_index_norm_max != i) { - VectorXd temp_col = mR.block(0, i, mR.rows(), 1); - mR.block(0,i,mR.rows(),1) = mR.block(0, col_index_norm_max, mR.rows(), 1); - mR.block(0, col_index_norm_max, mR.rows(), 1) = temp_col; - - unsigned int temp_index = mPermutations[i]; - mPermutations[i] = mPermutations[col_index_norm_max]; - mPermutations[col_index_norm_max] = temp_index; - } - - MatrixXXd current_block = mR.block(i,i, block_rows, block_cols); - VectorXd column = current_block.block(0, 0, block_rows, 1); - - value_type alpha = - column.norm(); - if (current_block(0,0) < 0) { - alpha = - alpha; - } - - VectorXd v = current_block.block(0, 0, block_rows, 1); - v[0] = v[0] - alpha; - - MatrixXXd Q (MatrixXXd::Identity(mR.rows(), mR.rows())); - - Q.block(i, i, block_rows, block_rows) = MatrixXXd (Q.block(i, i, block_rows, block_rows)) - - MatrixXXd(v * v.transpose() / (v.squaredNorm() * static_cast(0.5))); - - mR = Q * mR; - - // Normalize so that we have positive diagonal elements - if (mR(i,i) < 0) { - mR.block(i,i,block_rows, block_cols) = MatrixXXd(mR.block(i,i,block_rows, block_cols)) * -1.; - Q.block(i,i,block_rows, block_rows) = MatrixXXd(Q.block(i,i,block_rows, block_rows)) * -1.; - } - - mQ = mQ * Q; - } - - mIsFactorized = true; - - return *this; - } - Dynamic::Matrix solve ( - const Dynamic::Matrix &rhs - ) const { - assert (mIsFactorized); - - VectorXd y = mQ.transpose() * rhs; - VectorXd x = VectorXd::Zero(mR.cols()); - - for (int i = mR.cols() - 1; i >= 0; --i) { - value_type z = y[i]; - - for (unsigned int j = i + 1; j < mR.cols(); j++) { - z = z - x[mPermutations[j]] * mR(i,j); - } - - if (fabs(mR(i,i)) < std::numeric_limits::epsilon() * 10) { - std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero!" << std::endl; - abort(); - } - x[mPermutations[i]] = z / mR(i,i); - } - - return x; - } - Dynamic::Matrix inverse() const { - assert (mIsFactorized); - - VectorXd rhs_temp = VectorXd::Zero(mQ.cols()); - MatrixXXd result (mQ.cols(), mQ.cols()); - - for (unsigned int i = 0; i < mQ.cols(); i++) { - rhs_temp[i] = 1.; - - result.block(0, i, mQ.cols(), 1) = solve(rhs_temp); - - rhs_temp[i] = 0.; - } - - return result; - } - - Dynamic::Matrix householderQ () const { - return mQ; - } - Dynamic::Matrix matrixR () const { - return mR; - } - Dynamic::Matrix matrixP () const { - MatrixXXd P = MatrixXXd::Identity(mR.cols(), mR.cols()); - MatrixXXd identity = MatrixXXd::Identity(mR.cols(), mR.cols()); - for (unsigned int i = 0; i < mR.cols(); i++) { - P.block(0,i,mR.cols(),1) = identity.block(0,mPermutations[i], mR.cols(), 1); - } - return P; - } - - unsigned int rank() const { - value_type abs_threshold = fabs(mR(0,0)) * mThreshold; - - for (unsigned int i = 1; i < mR.cols(); i++) { - if (fabs(mR(i,i)) < abs_threshold) - return i; - } - - return mR.cols(); - } - }; - -} - -/* _SIMPLE_MATH_QR_H */ -#endif diff --git a/src/SimpleMath/compileassert.h b/src/SimpleMath/compileassert.h deleted file mode 100644 index 4d12fdb..0000000 --- a/src/SimpleMath/compileassert.h +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef _COMPILE_ASSERT_H -#define _COMPILE_ASSERT_H - -/* - * This is a simple compile time assertion tool taken from: - * http://blogs.msdn.com/b/abhinaba/archive/2008/10/27/c-c-compile-time-asserts.aspx - * written by Abhinaba Basu! - * - * Thanks! - */ - -#ifdef __cplusplus - -#define JOIN( X, Y ) JOIN2(X,Y) -#define JOIN2( X, Y ) X##Y - -namespace static_assert_compat -{ - template struct STATIC_ASSERT_FAILURE; - template <> struct STATIC_ASSERT_FAILURE { enum { value = 1 }; }; - - template struct static_assert_test{}; -} - -#define COMPILE_ASSERT(x) \ - typedef ::static_assert_compat::static_assert_test<\ - sizeof(::static_assert_compat::STATIC_ASSERT_FAILURE< (bool)( x ) >)>\ - JOIN(_static_assert_typedef, __LINE__) - -#else // __cplusplus - -#define COMPILE_ASSERT(x) extern int __dummy[(int)x] - -#endif // __cplusplus - -#define VERIFY_EXPLICIT_CAST(from, to) COMPILE_ASSERT(sizeof(from) == sizeof(to)) - -// _COMPILE_ASSERT_H_ -#endif diff --git a/src/math_types.h b/src/math_types.h index 7f6c06c..f744b51 100644 --- a/src/math_types.h +++ b/src/math_types.h @@ -2,27 +2,25 @@ #define MESHUP_CONFIG_H #include "SimpleMath/SimpleMath.h" -#include "SimpleMath/SimpleMathGL.h" -#include "SimpleMath/SimpleMathMap.h" -typedef SimpleMath::Fixed::Matrix Vector2f; -typedef SimpleMath::Fixed::Matrix Matrix22f; +typedef SimpleMath::Fixed Vector2f; +typedef SimpleMath::Fixed Matrix22f; -typedef SimpleMath::Fixed::Matrix Vector3f; -typedef SimpleMath::Fixed::Matrix Matrix33f; +typedef SimpleMath::Fixed Vector3f; +typedef SimpleMath::Fixed Matrix33f; -typedef SimpleMath::Fixed::Matrix Vector4f; -typedef SimpleMath::Fixed::Matrix Matrix44f; +typedef SimpleMath::Fixed Vector4f; +typedef SimpleMath::Fixed Matrix44f; -typedef SimpleMath::GL::Quaternion Quaternion; +typedef SimpleMath::Quaternion Quaternion; -typedef SimpleMath::Dynamic::Matrix VectorNf; -typedef SimpleMath::Dynamic::Matrix MatrixNNf; +typedef SimpleMath::Dynamic VectorNf; +typedef SimpleMath::Dynamic MatrixNNf; -typedef SimpleMath::Fixed::Matrix Vector3d; -typedef SimpleMath::Fixed::Matrix Matrix33d; +typedef SimpleMath::Fixed Vector3d; +typedef SimpleMath::Fixed Matrix33d; -typedef SimpleMath::Dynamic::Matrix VectorNd; -typedef SimpleMath::Dynamic::Matrix MatrixNNd; +typedef SimpleMath::Dynamic VectorNd; +typedef SimpleMath::Dynamic MatrixNNd; #endif diff --git a/src/modules/RenderModule.cc b/src/modules/RenderModule.cc index c1bc7c2..a1bb16d 100644 --- a/src/modules/RenderModule.cc +++ b/src/modules/RenderModule.cc @@ -10,7 +10,7 @@ #include "imgui/imgui.h" #include "imgui_dock.h" -using namespace SimpleMath::GL; +using namespace SimpleMath; struct Renderer; @@ -180,10 +180,10 @@ extern "C" { const struct module_api MODULE_API = { .init = module_init, + .finalize = module_finalize, .reload = module_reload, - .step = module_step, .unload = module_unload, - .finalize = module_finalize + .step = module_step }; } diff --git a/src/modules/RenderUtils.cc b/src/modules/RenderUtils.cc index 3257471..9b6c595 100644 --- a/src/modules/RenderUtils.cc +++ b/src/modules/RenderUtils.cc @@ -13,7 +13,6 @@ #include "imgui_dock.h" using namespace SimpleMath; -using namespace SimpleMath::GL; typedef tinygltf::TinyGLTF GLTFLoader; diff --git a/src/modules/TestModule.cc b/src/modules/TestModule.cc index d8763f1..c0814d7 100644 --- a/src/modules/TestModule.cc +++ b/src/modules/TestModule.cc @@ -46,10 +46,10 @@ void handle_mouse (struct module_state *state) { view_dir = (poi - eye).normalized(); Vector3f right = camera_rot_inv.block<1,3>(0,0).transpose(); right = view_dir.cross (Vector3f (0.f, 1.f, 0.f)); - Matrix33f rot_matrix_y = SimpleMath::GL::RotateMat33( + Matrix33f rot_matrix_y = SimpleMath::RotateMat33( gGuiInputState->mousedY * 0.4f, right[0], right[1], right[2]); - Matrix33f rot_matrix_x = SimpleMath::GL::RotateMat33( + Matrix33f rot_matrix_x = SimpleMath::RotateMat33( gGuiInputState->mousedX * 0.4f, 0.f, 1.f, 0.f); poi = eye + rot_matrix_x * rot_matrix_y * view_dir; @@ -169,9 +169,9 @@ extern "C" { const struct module_api MODULE_API = { .init = module_init, + .finalize = module_finalize, .reload = module_reload, - .step = module_step, .unload = module_unload, - .finalize = module_finalize + .step = module_step }; }