diff --git a/src/SimpleMath/SimpleMath.h b/src/SimpleMath/SimpleMath.h index 31c09c8..3196221 100644 --- a/src/SimpleMath/SimpleMath.h +++ b/src/SimpleMath/SimpleMath.h @@ -1,3 +1,38 @@ +/* + * SimpleMath - A simple highly inefficient single header C++ math library + * Copyright (c) 2019 Martin Felis + * + * This is a highly inefficient math library. It was conceived 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 (even claim it as yours!). However, + * no guarantees are given that this code does what it says it would. + * + * Should you need a more formal license go with the following (zlib license): + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + * +*/ + #pragma once #include @@ -32,10 +67,16 @@ struct Transpose; typedef Matrix Matrix33f; typedef Matrix Vector3f; -template +template +class LLT; + +template +class PartialPivLU; + +template class HouseholderQR; -template +template class ColPivHouseholderQR; @@ -48,10 +89,18 @@ struct MatrixBase { typedef MatrixBase MatrixType; typedef ScalarType value_type; + enum { + RowsAtCompileTime = Rows, + ColsAtCompileTime = Cols + }; + + Derived& operator=(const Derived& other) { if (static_cast(this) != static_cast(&other)) { - for (size_t i = 0; i < other.rows(); i++) { - for (size_t j = 0; j < other.cols(); j++) { + int i, j, in = other.rows(), jn = other.cols(); + + for (size_t i = 0; i < in; i++) { + for (size_t j = 0; j < jn; j++) { this->operator()(i,j) = other(i,j); } } @@ -64,8 +113,10 @@ struct MatrixBase { template Derived& operator=(const MatrixBase& other) { if (static_cast(this) != static_cast(&other)) { - for (size_t i = 0; i < other.rows(); i++) { - for (size_t j = 0; j < other.cols(); j++) { + int i, j, in = other.rows(), jn = other.cols(); + + for (size_t i = 0; i < in; i++) { + for (size_t j = 0; j < jn; j++) { this->operator()(i,j) = other(i,j); } } @@ -79,10 +130,10 @@ struct MatrixBase { // Matrix operator*(const double& scalar) const { Matrix result (rows(), cols()); + int i, j, in = rows(), jn = cols(); - unsigned int i,j; - for (i = 0; i < rows(); i++) { - for (j = 0; j < cols(); j++) { + for (i = 0; i < in; i++) { + for (j = 0; j < jn; j++) { result (i,j) = operator()(i,j) * static_cast(scalar); } } @@ -91,10 +142,10 @@ struct MatrixBase { Matrix operator*(const float& scalar) const { Matrix result (rows(), cols()); + int i, j, in = rows(), jn = cols(); - unsigned int i,j; - for (i = 0; i < rows(); i++) { - for (j = 0; j < cols(); j++) { + for (i = 0; i < in; i++) { + for (j = 0; j < jn; j++) { result (i,j) = operator()(i,j) * static_cast(scalar); } } @@ -106,9 +157,10 @@ struct MatrixBase { // template bool operator==(const MatrixBase& other) const { - unsigned int i,j; - for (i = 0; i < rows(); i++) { - for (j = 0; j < cols(); j++) { + int i, j, in = rows(), jn = cols(); + + for (i = 0; i < in; i++) { + for (j = 0; j < jn; j++) { if (this->operator()(i,j) != other(i,j)) return false; } @@ -151,11 +203,11 @@ struct MatrixBase { Matrix operator*(const MatrixBase &other) const { Matrix result(Matrix::Zero(rows(), other.cols())); + int i, j, k, in = rows(), jn = other.cols(), kn = other.rows(); - 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++) { + for (i = 0; i < in; i++) { + for (j = 0; j < jn; j++) { + for (k = 0; k < kn; k++) { result(i, j) += operator()(i, k) * other(k, j); } } @@ -165,14 +217,15 @@ struct MatrixBase { } template - Derived operator*=(const OtherDerived &other) { + Derived operator*=(const MatrixBase &other) { Derived copy (*static_cast(this)); this->setZero(); - 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++) { + int i, j, k, in = rows(), jn = other.cols(), kn = other.rows(); + + for (i = 0; i < in; i++) { + for (j = 0; j < jn; j++) { + for (k = 0; k < kn; k++) { this->operator()(i, j) += copy.operator()(i, k) * other(k, j); } } @@ -182,8 +235,10 @@ struct MatrixBase { Matrix operator-() const { Matrix copy (*static_cast(this)); - for (int i = 0; i < rows(); i++) { - for (int j = 0; j < cols(); j++) { + int i, j, in = rows(), jn = cols(); + + for (int i = 0; i < in; i++) { + for (int j = 0; j < jn; j++) { copy(i,j) *= static_cast(-1.); } } @@ -191,9 +246,10 @@ struct MatrixBase { } Derived operator*=(const ScalarType& s) { - unsigned int i,j; - for (i = 0; i < rows(); i++) { - for (j = 0; j < cols(); j++) { + int i, j, in = rows(), jn = cols(); + + for (int i = 0; i < in; i++) { + for (int j = 0; j < jn; j++) { operator()(i,j) *= s; } } @@ -213,7 +269,6 @@ struct MatrixBase { void conservativeResize(unsigned int nrows, unsigned int ncols = 1) { static_assert(Rows == Dynamic, "Resize of fixed size matrices not allowed."); - Derived copy(*this); unsigned int arows = std::min(nrows, (unsigned int) rows()); @@ -375,11 +430,8 @@ struct MatrixBase { operator()(5,3) = v53; operator()(5,4) = v54; operator()(5,5) = v55; - } - - size_t rows() const { return static_cast(this)->rows(); } @@ -400,11 +452,11 @@ struct MatrixBase { } const ScalarType& operator[](const size_t& i) const { - assert(cols() == 1); + assert(cols() == 1); return static_cast(this)->operator()(i,0); } ScalarType& operator[](const size_t& i) { - assert(cols() == 1); + assert(cols() == 1); return static_cast(this)->operator()(i,0); } @@ -490,35 +542,98 @@ struct MatrixBase { return result; } - Derived inverse() const { - return colPivHouseholderQr().inverse(); - } + Derived inverse() const { + if (rows() == cols()) { + if (rows() == 1) { + Derived result(rows(), cols()); + result(0,0) = static_cast(1.) / operator()(0,0); - ScalarType trace() const { - assert(rows() == cols()); + return result; + } else if (rows() == 2) { + const ScalarType& a = operator()(0,0); + const ScalarType& b = operator()(0,1); + const ScalarType& c = operator()(1,0); + const ScalarType& d = operator()(1,1); - ScalarType result = static_cast(0.0); + Derived result(rows(), cols()); - for (unsigned int i = 0; i < rows(); i++) { - result += operator()(i,i); + ScalarType detinv = static_cast(1.) / (a * d - b * c); + + result(0,0) = d * detinv; + result(0,1) = -b * detinv; + result(1,0) = -c * detinv; + result(1,1) = d * detinv; + + return result; + } else if (rows() == 3) { + // source: + // https://stackoverflow.com/questions/983999/simple-3x3-matrix-inverse-code-c + + // computes the inverse of a matrix m + ScalarType det = operator()(0, 0) * (operator()(1, 1) * operator()(2, 2) + - operator()(2, 1) * operator()(1, 2)) + - operator()(0, 1) * (operator()(1, 0) * operator()(2, 2) + - operator()(1, 2) * operator()(2, 0)) + + operator()(0, 2) * (operator()(1, 0) * operator()(2, 1) + - operator()(1, 1) * operator()(2, 0)); + + ScalarType invdet = 1. / det; + + Derived result(rows(), cols()); + + result(0,0) = (operator()(1, 1) * operator()(2, 2) - operator()(2, 1) * operator()(1, 2)) * invdet; + result(0,1) = (operator()(0, 2) * operator()(2, 1) - operator()(0, 1) * operator()(2, 2)) * invdet; + result(0,2) = (operator()(0, 1) * operator()(1, 2) - operator()(0, 2) * operator()(1, 1)) * invdet; + result(1,0) = (operator()(1, 2) * operator()(2, 0) - operator()(1, 0) * operator()(2, 2)) * invdet; + result(1,1) = (operator()(0, 0) * operator()(2, 2) - operator()(0, 2) * operator()(2, 0)) * invdet; + result(1,2) = (operator()(1, 0) * operator()(0, 2) - operator()(0, 0) * operator()(1, 2)) * invdet; + result(2,0) = (operator()(1, 0) * operator()(2, 1) - operator()(2, 0) * operator()(1, 1)) * invdet; + result(2,1) = (operator()(2, 0) * operator()(0, 1) - operator()(0, 0) * operator()(2, 1)) * invdet; + result(2,2) = (operator()(0, 0) * operator()(1, 1) - operator()(1, 0) * operator()(0, 1)) * invdet; + + return result; } + } + return colPivHouseholderQr().inverse(); + } - return result; + ScalarType trace() const { + assert(rows() == cols()); + + ScalarType result = static_cast(0.0); + + for (unsigned int i = 0; i < rows(); i++) { + result += operator()(i,i); } - // TODO: implement Cholesky decompositioe - const HouseholderQR llt() const { - std::cerr << "LLT decomposition uses householder!" << std::endl; - return HouseholderQR(*this); + return result; + } + + ScalarType mean() const { + assert(rows() == 1 || cols() == 1); + ScalarType result = static_cast(0.0); + for (unsigned int i = 0; i < rows(); i++) { + result += operator[](i); } - const HouseholderQR householderQr() const { - return HouseholderQR(*this); - } + return result / static_cast(rows() * cols()); + } - const ColPivHouseholderQR colPivHouseholderQr() const { - return ColPivHouseholderQR(*this); - } + const LLT llt() const { + return LLT(*this); + } + + const PartialPivLU partialPivLu() const { + return PartialPivLU(*this); + } + + const HouseholderQR householderQr() const { + return HouseholderQR(*this); + } + + const ColPivHouseholderQR colPivHouseholderQr() const { + return ColPivHouseholderQR(*this); + } ScalarType* data() { return static_cast(this)->data(); @@ -673,9 +788,9 @@ struct Storage { resize(rows, cols); } - size_t rows() const { return NumRows; } + inline size_t rows() const { return NumRows; } - size_t cols() const { return NumCols; } + inline size_t cols() const { return NumCols; } void resize(int num_rows, int num_cols) { // Resizing of fixed size matrices not allowed @@ -689,15 +804,15 @@ struct Storage { assert (num_rows == NumRows && num_cols == NumCols); } - ScalarType& coeff(int row_index, int col_index) { - assert (row_index >= 0 && row_index <= NumRows); - assert (col_index >= 0 && col_index <= NumCols); + inline ScalarType& coeff(int row_index, int col_index) { +// assert (row_index >= 0 && row_index <= NumRows); +// assert (col_index >= 0 && col_index <= NumCols); return mData[row_index * NumCols + col_index]; } - const ScalarType& coeff(int row_index, int col_index) const { - assert (row_index >= 0 && row_index <= NumRows); - assert (col_index >= 0 && col_index <= NumCols); + inline const ScalarType& coeff(int row_index, int col_index) const { +// assert (row_index >= 0 && row_index <= NumRows); +// assert (col_index >= 0 && col_index <= NumCols); return mData[row_index * NumCols + col_index]; } }; @@ -710,12 +825,16 @@ struct Storage { Storage() {} + ~Storage() { + delete[] mData; + } + Storage(int rows, int cols) { resize(rows, cols); } - size_t rows() const { return mRows; } - size_t cols() const { return mCols; } + inline size_t rows() const { return mRows; } + inline size_t cols() const { return mCols; } void resize(int num_rows, int num_cols) { if (mRows != num_rows || mCols != num_cols) { @@ -729,14 +848,14 @@ struct Storage { } } - ScalarType& coeff(int row_index, int col_index) { - assert (row_index >= 0 && row_index <= mRows); - assert (col_index >= 0 && col_index <= mCols); + inline ScalarType& coeff(int row_index, int col_index) { +// assert (row_index >= 0 && row_index <= mRows); +// assert (col_index >= 0 && col_index <= mCols); return mData[row_index * mCols + col_index]; } - const ScalarType& coeff(int row_index, int col_index) const { - assert (row_index >= 0 && row_index <= mRows); - assert (col_index >= 0 && col_index <= mCols); + inline const ScalarType& coeff(int row_index, int col_index) const { +// assert (row_index >= 0 && row_index <= mRows); +// assert (col_index >= 0 && col_index <= mCols); return mData[row_index * mCols + col_index]; } }; @@ -750,12 +869,16 @@ struct Storage { Storage() {} + ~Storage() { + delete[] mData; + } + Storage(int rows, int cols) { resize(rows, cols); } - size_t rows() const { return mRows; } - size_t cols() const { return mCols; } + inline size_t rows() const { return mRows; } + inline size_t cols() const { return mCols; } void resize(int num_rows, int num_cols) { if (mRows != num_rows || mCols != num_cols) { @@ -769,17 +892,17 @@ struct Storage { } } - ScalarType& coeff(int row_index, int col_index) { - assert (row_index >= 0 && row_index <= mRows); - assert (col_index >= 0 && col_index <= mCols); + inline ScalarType& coeff(int row_index, int col_index) { +// assert (row_index >= 0 && row_index <= mRows); +// assert (col_index >= 0 && col_index <= mCols); return mData[row_index * mCols + col_index]; } - const ScalarType& coeff(int row_index, int col_index) const { - assert (row_index >= 0 && row_index <= mRows); - assert (col_index >= 0 && col_index <= mCols); + inline const ScalarType& coeff(int row_index, int col_index) const { +// assert (row_index >= 0 && row_index <= mRows); +// assert (col_index >= 0 && col_index <= mCols); return mData[row_index * mCols + col_index]; } - }; +}; template @@ -800,21 +923,45 @@ struct Matrix : public MatrixBase, ScalarTy SizeAtCompileTime / RowsAtCompileTime ) {} - explicit Matrix(int rows, int cols = 1) : + explicit Matrix(int rows) : mStorage (rows, 1) {} + explicit Matrix(unsigned int rows) : mStorage (rows, 1) {} + explicit Matrix(size_t rows) : mStorage (rows, 1) {} + + explicit Matrix(int rows, int cols) : mStorage(rows, cols) {} - explicit Matrix(unsigned int rows, unsigned int cols = 1) : + explicit Matrix(int rows, unsigned int cols) : mStorage(rows, cols) {} - explicit Matrix (size_t rows, size_t cols = 1) : + explicit Matrix(int rows, size_t cols) : + mStorage(rows, cols) {} + + explicit Matrix(unsigned int rows, int cols) : + mStorage(rows, cols) {} + + explicit Matrix(unsigned int rows, unsigned int cols) : + mStorage(rows, cols) {} + + explicit Matrix(unsigned int rows, size_t cols) : + mStorage(rows, cols) {} + + explicit Matrix(size_t rows, int cols) : + mStorage(rows, cols) {} + + explicit Matrix(size_t rows, unsigned int cols) : + mStorage(rows, cols) {} + + explicit Matrix(size_t rows, size_t cols) : mStorage(rows, cols) {} template Matrix(const MatrixBase &other) { mStorage.resize(other.rows(), other.cols()); - for (size_t i = 0; i < rows(); i++) { - for (size_t j = 0; j < cols(); j++) { + int i, j, in = rows(), jn = cols(); + + for (size_t i = 0; i < in; i++) { + for (size_t j = 0; j < jn; j++) { this->operator()(i, j) = other(i, j); } } @@ -837,15 +984,7 @@ struct Matrix : public MatrixBase, ScalarTy // // Constructor for vectors // - Matrix ( - const ScalarType& v0 - ) { - static_assert (NumRows * NumCols == 1, "Invalid matrix size"); - - operator()(0,0) = v0; - } - - Matrix ( + explicit Matrix ( const ScalarType& v0, const ScalarType& v1 ) { @@ -1061,8 +1200,10 @@ struct Matrix : public MatrixBase, ScalarTy Matrix& operator+=(const OtherDerived& other) { assert (rows() == other.rows() && cols() == other.cols() && "Error: matrix dimensions do not match!"); - for (size_t i = 0; i < rows(); i++) { - for (size_t j = 0; j < cols(); j++) { + int i, j, in = rows(), jn = cols(); + + for (size_t i = 0; i < in; i++) { + for (size_t j = 0; j < jn; j++) { this->operator()(i,j) += other(i,j); } } @@ -1073,15 +1214,21 @@ struct Matrix : public MatrixBase, ScalarTy Matrix& operator-=(const OtherDerived& other) { assert (rows() == other.rows() && cols() == 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); + int i, j, in = rows(), jn = cols(); + + for (size_t i = 0; i < in; i++) { + for (size_t j = 0; j < jn; j++) { + this->operator()(i,j) -= other(i,j); } } return *this; } - ScalarType& operator()(const size_t& i, const size_t& j) { + inline ScalarType& operator()(const size_t& i, const size_t& j) { + return mStorage.coeff(i, j); + } + + inline const ScalarType& operator()(const size_t& i, const size_t& j) const { return mStorage.coeff(i, j); } @@ -1093,10 +1240,6 @@ struct Matrix : public MatrixBase, ScalarTy return mStorage.mData; } - const ScalarType& operator()(const size_t& i, const size_t& j) const { - return mStorage.coeff(i, j); - } - size_t cols() const { return mStorage.cols(); } @@ -1215,8 +1358,10 @@ struct Transpose : public MatrixBase Transpose& operator=(const MatrixBase& other) { if (static_cast(this) != static_cast(&other)) { - for (size_t i = 0; i < other.rows(); i++) { - for (size_t j = 0; j < other.cols(); j++) { + int i, j, in = other.rows(), jn = other.cols(); + + for (size_t i = 0; i < in; i++) { + for (size_t j = 0; j < jn; j++) { this->operator()(i,j) = other(i,j); } } @@ -1278,9 +1423,10 @@ struct Block : public MatrixBase, S { } Block& operator=(const Block &other) { - unsigned int i,j; - for (i = 0; i < rows(); i++) { - for (j = 0; j < cols(); j++) { + int i, j, in = rows(), jn = cols(); + + for (i = 0; i < in; i++) { + for (j = 0; j < jn; j++) { this->operator()(i,j) = other(i,j); } } @@ -1290,9 +1436,10 @@ struct Block : public MatrixBase, S template Block& operator=(const MatrixBase& other) { - unsigned int i,j; - for (i = 0; i < rows(); i++) { - for (j = 0; j < cols(); j++) { + int i, j, in = rows(), jn = cols(); + + for (i = 0; i < in; i++) { + for (j = 0; j < jn; j++) { this->operator()(i,j) = other(i,j); } } @@ -1300,25 +1447,15 @@ struct Block : public MatrixBase, S return *this; } -// template -// Matrix& operator=(const MatrixBase& other) { -// unsigned int i,j,k; -// for (i = 0; i < rows(); i++) { -// for (j = 0; j < other.cols(); j++) { -// operator()(i,k) = other(k,j); -// } -// } -// return *this; -// } template Matrix operator*(const MatrixBase& other) const { Matrix result (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++) { + int i, j, k, in = rows(), jn = other.cols(), kn = other.rows(); + for (i = 0; i < in; i++) { + for (j = 0; j < jn; j++) { + for (k = 0; k < kn; k++) { result (i,j) += operator()(i,k) * other(k,j); } } @@ -1344,9 +1481,9 @@ struct Block : public MatrixBase, S Matrix operator+(const MatrixBase& other) const { Matrix result (rows(), other.cols()); - unsigned int i,j,k; - for (i = 0; i < rows(); i++) { - for (j = 0; j < other.cols(); j++) { + int i, j, in = rows(), jn = other.cols(); + for (i = 0; i < in; i++) { + for (j = 0; j < jn; j++) { result (i,j) = operator()(i,j) + other(i,j); } } @@ -1367,14 +1504,299 @@ struct Block : public MatrixBase, S } }; +// +// LLT Decomposition +// +template +class LLT { +public: + typedef typename Derived::value_type value_type; + typedef MatrixBase MatrixType; + + LLT() : + mIsFactorized(false) + {} + +private: + typedef Matrix VectorXd; + typedef Matrix MatrixXXd; + typedef Matrix ColumnVector; + + bool mIsFactorized; + Matrix mQ; + Derived mL; + +public: + LLT(const Derived &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; + } + ColumnVector solve ( + const ColumnVector &rhs + ) const { + assert (mIsFactorized); + + ColumnVector 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); + } + + ColumnVector 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; + } + Derived 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; + } + Derived matrixL () const { + return mL; + } +}; + + +// +// Partial Pivoting LU Decomposition +// +template +class PartialPivLU { +public: + typedef typename Derived::value_type value_type; + typedef MatrixBase MatrixType; + PartialPivLU() : + mIsFactorized(false) + {} +private: + typedef Matrix VectorXd; + typedef Matrix MatrixXXd; + typedef Matrix ColumnVector; + typedef Matrix RowVector; + bool mIsFactorized; + unsigned int *mPermutations = nullptr; + Derived mLU; + +public: + ~PartialPivLU() { + delete[] mPermutations; + } + + PartialPivLU(const Derived &matrix) : + mIsFactorized(false), + mLU (matrix) + { + mPermutations = new unsigned int [matrix.cols() + 1]; + for (unsigned int i = 0; i <= matrix.cols(); i++) { + mPermutations[i] = i; + } + compute(matrix); + } + + PartialPivLU& compute(const Derived &matrix) { + unsigned int n = matrix.rows(); + + double v_abs; + RowVector temp_vec; + + unsigned int i,j,k; + + // over all columns + for (i = 0; i < n; i++) { + double max_v = 0.0; + unsigned int max_i = i; + + // Find the row pivoting index + for (k = i; k < n; k++) { + if ((v_abs = fabs(mLU(k, i))) > max_v) { + max_v = v_abs; + max_i = k; + } + } + + if (max_v < std::numeric_limits::epsilon()) { + std::cerr << "Error: pivoting failed for matrix A = " << std::endl; + std::cerr << "A = " << matrix << std::endl; + abort(); + } + + // Perform the permutation + if (max_i != i) { + // update permutation vector + j = mPermutations[i]; + mPermutations[i] = mPermutations[max_i]; + mPermutations[max_i] = j; + + // swap columns + temp_vec = mLU.block(i,0,1,n); + mLU.block(i, 0, 1, n) = mLU.block(max_i, 0, 1, n); + mLU.block(max_i, 0, 1, n) = temp_vec; + + // Increase number of permutations + mPermutations[n]++; + } + + // eliminate i'th column of k'th row + for (int k = i+1; k < n; k++) { + mLU(k,i) = mLU(k,i) / mLU(i,i); + + // iterate over all columns + for (int j = i+1; j < n; j++) { + mLU(k,j) = mLU(k,j) - mLU(i,j) * mLU(k,i); + } + } + } + + mIsFactorized = true; + + return *this; + } + + Derived matrixL() const { + Derived result (Derived::Zero(mLU.rows(), mLU.cols())); + + unsigned int n = mLU.rows(); + + for (int i = 0; i < n; i++) { + for (int j = 0; j < i; j++) { + result(i,j) = mLU(i,j); + } + + result(i,i) = 1.0; + } + + return result; + } + + Derived matrixU() const { + Derived result (Derived::Zero(mLU.rows(), mLU.cols())); + + unsigned int n = mLU.rows(); + + for (int i = 0; i < n; i++) { + for (int j = i; j < n; j++) { + result(i,j) = mLU(i,j); + } + } + + return result; + } + + Derived matrixP() const { + Derived result(Derived::Zero(mLU.rows(), mLU.cols())); + + unsigned int n = mLU.rows(); + for (int i = 0; i < n; i++) { + result(i, mPermutations[i]) = 1.0; + } + + return result; + } + + ColumnVector solve ( + const ColumnVector &rhs + ) const { + assert (mIsFactorized); + + unsigned int n = mLU.rows(); + + // Backsolve L^-1 * rhs + ColumnVector result(n, 1); + + for (int i = 0; i < n; i++) { + result[i] = rhs[mPermutations[i]]; + for (int j = 0; j < i; j++) { + result[i] = result[i] - result[j] * mLU(i,j); + } + } + + // Solve U^-1 * result + for (int i = n - 1; i >= 0; i--) { + for (int j = i + 1; j < n; j++) { + result[i] = result[i] - result[j] * mLU(i,j); + } + + result[i] = result[i] / mLU(i,i); + } + + return result; + } + + Derived inverse() const { + assert (mIsFactorized); + VectorXd rhs_temp = VectorXd::Zero(mLU.cols()); + MatrixXXd result (mLU.cols(), mLU.cols()); + for (unsigned int i = 0; i < mLU.cols(); i++) { + rhs_temp[i] = 1.; + result.block(0, i, mLU.cols(), 1) = solve(rhs_temp); + rhs_temp[i] = 0.; + } + return result; + } +}; + // // QR Decomposition // -template +template class HouseholderQR { public: - typedef MatrixBase MatrixType; - typedef typename MatrixType::value_type value_type; + typedef typename Derived::value_type value_type; + typedef MatrixBase MatrixType; HouseholderQR() : mIsFactorized(false) @@ -1383,10 +1805,10 @@ public: private: typedef Matrix VectorXd; typedef Matrix MatrixXXd; - typedef Matrix ColumnVector; + typedef Matrix ColumnVector; bool mIsFactorized; - Matrix mQ; + Matrix mQ; Derived mR; public: @@ -1452,7 +1874,7 @@ public: } if (fabs(mR(i,i)) < std::numeric_limits::epsilon() * 10) { - std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero!" << std::endl; + std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero:" << fabs(mR(i,i))<< std::endl; abort(); } x[i] = z / mR(i,i); @@ -1486,20 +1908,20 @@ public: } }; -template +template class ColPivHouseholderQR { public: - typedef MatrixBase MatrixType; - typedef typename MatrixType::value_type value_type; + typedef typename Derived::value_type value_type; + typedef MatrixBase MatrixType; private: typedef Matrix VectorXd; typedef Matrix MatrixXXd; - typedef Matrix ColumnVector; + typedef Matrix ColumnVector; bool mIsFactorized; - Matrix mQ; + Matrix mQ; Derived mR; unsigned int *mPermutations; @@ -1638,7 +2060,7 @@ public: } if (fabs(mR(i,i)) < std::numeric_limits::epsilon() * 10) { - std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero!" << std::endl; + std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero:" << fabs(mR(i,i))<< std::endl; abort(); } x[mPermutations[i]] = z / mR(i,i); diff --git a/src/SimpleMathOld/SimpleMath.h b/src/SimpleMathOld/SimpleMath.h deleted file mode 100644 index 89afabf..0000000 --- a/src/SimpleMathOld/SimpleMath.h +++ /dev/null @@ -1,11 +0,0 @@ -#ifndef _SIMPLEMATH_H -#define _SIMPLEMATH_H - -#include "SimpleMathFixed.h" -#include "SimpleMathDynamic.h" -#include "SimpleMathMixed.h" -#include "SimpleMathQR.h" -#include "SimpleMathCommaInitializer.h" -#include "SimpleMathGL.h" - -#endif /* _SIMPLEMATH_H */ diff --git a/src/SimpleMathOld/SimpleMathBlock.h b/src/SimpleMathOld/SimpleMathBlock.h deleted file mode 100644 index cb2e472..0000000 --- a/src/SimpleMathOld/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/SimpleMathOld/SimpleMathCholesky.h b/src/SimpleMathOld/SimpleMathCholesky.h deleted file mode 100644 index b51a361..0000000 --- a/src/SimpleMathOld/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/SimpleMathOld/SimpleMathCommaInitializer.h b/src/SimpleMathOld/SimpleMathCommaInitializer.h deleted file mode 100644 index 85657ba..0000000 --- a/src/SimpleMathOld/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/SimpleMathOld/SimpleMathDynamic.h b/src/SimpleMathOld/SimpleMathDynamic.h deleted file mode 100644 index 0ee6e57..0000000 --- a/src/SimpleMathOld/SimpleMathDynamic.h +++ /dev/null @@ -1,678 +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 Random (int rows, int cols = 1) { - matrix_type result (rows, cols); - for (int i = 0; i < rows; i++) { - for (int j = 0; j < cols; j++) { - result(i,j) = (static_cast(rand()) / static_cast(RAND_MAX)) * 2.0 - 1.0; - } - } - - 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/SimpleMathOld/SimpleMathFixed.h b/src/SimpleMathOld/SimpleMathFixed.h deleted file mode 100644 index b5985c9..0000000 --- a/src/SimpleMathOld/SimpleMathFixed.h +++ /dev/null @@ -1,899 +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, 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; - } - - static matrix_type Random (int rows = 1, int cols = 1) { - matrix_type result; - for (int i = 0; i < nrows; i++) { - for (int j = 0; j < ncols; j++) { - result(i,j) = (static_cast(rand()) / static_cast(RAND_MAX)) * 2.0 - 1.0; - } - } - - 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; - } - - // Multiplication with a block - template - Dynamic::Matrix operator*(const Block &block) { - assert (ncols == block.rows()); - - Dynamic::Matrix result(nrows, block.cols()); - - result.setZero(); - - unsigned int i,j, k; - for (i = 0; i < nrows; i++) { - for (j = 0; j < block.cols(); j++) { - for (k = 0; k < block.rows(); k++) { - result(i,j) += mData[i * ncols + k] * static_cast(block(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/SimpleMathOld/SimpleMathGL.h b/src/SimpleMathOld/SimpleMathGL.h deleted file mode 100644 index fcdfd19..0000000 --- a/src/SimpleMathOld/SimpleMathGL.h +++ /dev/null @@ -1,348 +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/SimpleMathOld/SimpleMathMap.h b/src/SimpleMathOld/SimpleMathMap.h deleted file mode 100644 index dbb757a..0000000 --- a/src/SimpleMathOld/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/SimpleMathOld/SimpleMathMixed.h b/src/SimpleMathOld/SimpleMathMixed.h deleted file mode 100644 index 777670f..0000000 --- a/src/SimpleMathOld/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/SimpleMathOld/SimpleMathQR.h b/src/SimpleMathOld/SimpleMathQR.h deleted file mode 100644 index 31b0add..0000000 --- a/src/SimpleMathOld/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/SimpleMathOld/compileassert.h b/src/SimpleMathOld/compileassert.h deleted file mode 100644 index 4d12fdb..0000000 --- a/src/SimpleMathOld/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