martin
/
protot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Deleted old SimpleMath, updated SimpleMathV2

opengl3
Martin Felis 2019-04-23 11:15:00 +02:00
parent 369d6a634b
commit ffe4e3bec0
12 changed files with 562 additions and 2956 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

702
src/SimpleMath/SimpleMath.h
View File

@ -1,3 +1,38 @@
/*
* SimpleMath - A simple highly inefficient single header C++ math library
* Copyright (c) 2019 Martin Felis <martin@fysx.org>
*
* 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 <sstream>
@ -32,10 +67,16 @@ struct Transpose;
typedef Matrix<float, 3, 3> Matrix33f;
typedef Matrix<float, 3, 1> Vector3f;
template <typename Derived, typename ScalarType, int NumRows, int NumCols>
template <typename Derived>
class LLT;
template <typename Derived>
class PartialPivLU;
template <typename Derived>
class HouseholderQR;
template <typename Derived, typename ScalarType, int NumRows, int NumCols>
template <typename Derived>
class ColPivHouseholderQR;
@ -48,10 +89,18 @@ struct MatrixBase {
typedef MatrixBase<Derived, ScalarType, Rows, Cols> MatrixType;
typedef ScalarType value_type;
enum {
RowsAtCompileTime = Rows,
ColsAtCompileTime = Cols
};
Derived& operator=(const Derived& other) {
if (static_cast<const void*>(this) != static_cast<const void*>(&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 <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
Derived& operator=(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) {
if (static_cast<const void*>(this) != static_cast<const void*>(&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<ScalarType, Rows, Cols> operator*(const double& scalar) const {
Matrix<ScalarType, Rows, Cols> 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<ScalarType>(scalar);
}
}
@ -91,10 +142,10 @@ struct MatrixBase {
Matrix<ScalarType, Rows, Cols> operator*(const float& scalar) const {
Matrix<ScalarType, Rows, Cols> 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<ScalarType>(scalar);
}
}
@ -106,9 +157,10 @@ struct MatrixBase {
//
template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
bool operator==(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& 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<ScalarType, Rows, OtherCols>
operator*(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols> &other) const {
Matrix<ScalarType, Rows, OtherCols> result(Matrix<ScalarType, Rows, OtherCols>::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 <typename OtherDerived>
Derived operator*=(const OtherDerived &other) {
Derived operator*=(const MatrixBase<OtherDerived, typename OtherDerived:: value_type, OtherDerived::RowsAtCompileTime, OtherDerived::ColsAtCompileTime> &other) {
Derived copy (*static_cast<const Derived*>(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<ScalarType, Rows, Cols> operator-() const {
Matrix<ScalarType, Rows, Cols> copy (*static_cast<const Derived*>(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<ScalarType>(-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<const Derived*>(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<const Derived*>(this)->operator()(i,0);
}
ScalarType& operator[](const size_t& i) {
assert(cols() == 1);
assert(cols() == 1);
return static_cast<Derived*>(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<ScalarType>(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<ScalarType>(0.0);
Derived result(rows(), cols());
for (unsigned int i = 0; i < rows(); i++) {
result += operator()(i,i);
ScalarType detinv = static_cast<ScalarType>(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<ScalarType>(0.0);
for (unsigned int i = 0; i < rows(); i++) {
result += operator()(i,i);
}
// TODO: implement Cholesky decompositioe
const HouseholderQR<Derived, ScalarType, Rows, Cols> llt() const {
std::cerr << "LLT decomposition uses householder!" << std::endl;
return HouseholderQR<Derived, ScalarType, Rows, Cols>(*this);
return result;
}
ScalarType mean() const {
assert(rows() == 1 || cols() == 1);
ScalarType result = static_cast<ScalarType>(0.0);
for (unsigned int i = 0; i < rows(); i++) {
result += operator[](i);
}
const HouseholderQR<Derived, ScalarType, Rows, Cols> householderQr() const {
return HouseholderQR<Derived, ScalarType, Rows, Cols>(*this);
}
return result / static_cast<ScalarType>(rows() * cols());
}
const ColPivHouseholderQR<Derived, ScalarType, Rows, Cols> colPivHouseholderQr() const {
return ColPivHouseholderQR<Derived, ScalarType, Rows, Cols>(*this);
}
const LLT<Derived> llt() const {
return LLT<Derived>(*this);
}
const PartialPivLU<Derived> partialPivLu() const {
return PartialPivLU<Derived>(*this);
}
const HouseholderQR<Derived> householderQr() const {
return HouseholderQR<Derived>(*this);
}
const ColPivHouseholderQR<Derived> colPivHouseholderQr() const {
return ColPivHouseholderQR<Derived>(*this);
}
ScalarType* data() {
return static_cast<Derived*>(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<ScalarType, 0, Dynamic, NumCols> {
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, 0, Dynamic, NumCols> {
}
}
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<ScalarType, 0, Dynamic, Dynamic> {
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, 0, Dynamic, Dynamic> {
}
}
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 <typename ScalarType, int NumRows, int NumCols>
@ -800,21 +923,45 @@ struct Matrix : public MatrixBase<Matrix<ScalarType, NumRows, NumCols>, 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<typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
Matrix(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols> &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<Matrix<ScalarType, NumRows, NumCols>, 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<Matrix<ScalarType, NumRows, NumCols>, 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<Matrix<ScalarType, NumRows, NumCols>, 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<Matrix<ScalarType, NumRows, NumCols>, 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<Derived, ScalarType, NumRows, Num
template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
Transpose& operator=(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) {
if (static_cast<const void*>(this) != static_cast<const void*>(&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<Block<Derived, ScalarType, NumRows, NumCols>, 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<Block<Derived, ScalarType, NumRows, NumCols>, S
template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
Block& operator=(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& 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<Block<Derived, ScalarType, NumRows, NumCols>, S
return *this;
}
// template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
// Matrix<ScalarType, NumRows, OtherCols>& operator=(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& 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 <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
Matrix<ScalarType, NumRows, OtherCols> operator*(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) const {
Matrix<ScalarType, NumRows, OtherCols> 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<Block<Derived, ScalarType, NumRows, NumCols>, S
Matrix<ScalarType, NumRows, OtherCols> operator+(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) const {
Matrix<ScalarType, NumRows, OtherCols> 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<Block<Derived, ScalarType, NumRows, NumCols>, S
}
};
//
// LLT Decomposition
//
template <typename Derived>
class LLT {
public:
typedef typename Derived::value_type value_type;
typedef MatrixBase<Derived, value_type, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> MatrixType;
LLT() :
mIsFactorized(false)
{}
private:
typedef Matrix<value_type> VectorXd;
typedef Matrix<value_type> MatrixXXd;
typedef Matrix<value_type, Derived::RowsAtCompileTime, 1> ColumnVector;
bool mIsFactorized;
Matrix<value_type, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> 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 <typename Derived>
class PartialPivLU {
public:
typedef typename Derived::value_type value_type;
typedef MatrixBase<Derived, value_type, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> MatrixType;
PartialPivLU() :
mIsFactorized(false)
{}
private:
typedef Matrix<value_type> VectorXd;
typedef Matrix<value_type> MatrixXXd;
typedef Matrix<value_type, Derived::RowsAtCompileTime, 1> ColumnVector;
typedef Matrix<value_type, 1, Derived::ColsAtCompileTime> 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<double>::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 <typename Derived, typename ScalarType, int NumRows, int NumCols>
template <typename Derived>
class HouseholderQR {
public:
typedef MatrixBase<Derived, ScalarType, NumRows, NumCols> MatrixType;
typedef typename MatrixType::value_type value_type;
typedef typename Derived::value_type value_type;
typedef MatrixBase<Derived, value_type, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> MatrixType;
HouseholderQR() :
mIsFactorized(false)
@ -1383,10 +1805,10 @@ public:
private:
typedef Matrix<value_type> VectorXd;
typedef Matrix<value_type> MatrixXXd;
typedef Matrix<ScalarType, NumRows, 1> ColumnVector;
typedef Matrix<value_type, Derived::RowsAtCompileTime, 1> ColumnVector;
bool mIsFactorized;
Matrix<ScalarType, NumRows, NumRows> mQ;
Matrix<value_type, Derived::RowsAtCompileTime, Derived::RowsAtCompileTime> mQ;
Derived mR;
public:
@ -1452,7 +1874,7 @@ public:
}
if (fabs(mR(i,i)) < std::numeric_limits<value_type>::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 <typename Derived, typename ScalarType, int NumRows, int NumCols>
template <typename Derived>
class ColPivHouseholderQR {
public:
typedef MatrixBase<Derived, ScalarType, NumRows, NumCols> MatrixType;
typedef typename MatrixType::value_type value_type;
typedef typename Derived::value_type value_type;
typedef MatrixBase<Derived, value_type, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> MatrixType;
private:
typedef Matrix<value_type> VectorXd;
typedef Matrix<value_type> MatrixXXd;
typedef Matrix<ScalarType, NumRows, 1> ColumnVector;
typedef Matrix<value_type, Derived::RowsAtCompileTime, 1> ColumnVector;
bool mIsFactorized;
Matrix<ScalarType, NumRows, NumRows> mQ;
Matrix<value_type, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> mQ;
Derived mR;
unsigned int *mPermutations;
@ -1638,7 +2060,7 @@ public:
}
if (fabs(mR(i,i)) < std::numeric_limits<value_type>::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);

11
src/SimpleMathOld/SimpleMath.h
View File

@ -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 */

194
src/SimpleMathOld/SimpleMathBlock.h
View File

@ -1,194 +0,0 @@
/**
* This is a highly inefficient math library. It was conceived by Martin
* Felis <martin.felis@iwr.uni-heidelberg.de> 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 <cstdlib>
#include <cmath>
#include <iostream>
#include <assert.h>
#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 <typename val_type, unsigned int nrows, unsigned int ncols>
class Matrix;
template <typename matrix_type, typename val_type>
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_type*>(&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<value_type>(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 <typename other_matrix_type>
// 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<value_type>(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 <typename matrix_type, typename val_type>
inline std::ostream& operator<<(std::ostream& output, const Block<matrix_type, val_type> &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 */

94
src/SimpleMathOld/SimpleMathCholesky.h
View File

@ -1,94 +0,0 @@
#ifndef _SIMPLE_MATH_CHOLESKY_H
#define _SIMPLE_MATH_CHOLESKY_H
#include <iostream>
#include <limits>
#include "SimpleMathDynamic.h"
namespace SimpleMath {
template <typename matrix_type>
class LLT {
public:
typedef typename matrix_type::value_type value_type;
private:
LLT () {}
typedef Dynamic::Matrix<value_type> MatrixXXd;
typedef Dynamic::Matrix<value_type> 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<value_type> solve (
const Dynamic::Matrix<value_type> &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<value_type> matrixL() const {
return mL;
}
};
}
/* _SIMPLE_MATH_CHOLESKY_H */
#endif

69
src/SimpleMathOld/SimpleMathCommaInitializer.h
View File

@ -1,69 +0,0 @@
#ifndef _SIMPLE_MATH_COMMA_INITIALIZER_H
#define _SIMPLE_MATH_COMMA_INITIALIZER_H
#include <iostream>
#include <limits>
#include "SimpleMathFixed.h"
#include "SimpleMathDynamic.h"
namespace SimpleMath {
template <typename matrix_type>
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<matrix_type> 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

678
src/SimpleMathOld/SimpleMathDynamic.h
View File

@ -1,678 +0,0 @@
/**
* This is a highly inefficient math library. It was conceived by Martin
* Felis <martin.felis@iwr.uni-heidelberg.de> 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 <sstream>
#include <cstdlib>
#include <assert.h>
#include <algorithm>
#include "compileassert.h"
#include "SimpleMathBlock.h"
/** \brief Namespace for a highly inefficient math library
*
*/
namespace SimpleMath {
template <typename matrix_type>
class LLT;
template <typename matrix_type>
class HouseholderQR;
template <typename matrix_type>
class ColPivHouseholderQR;
template <typename matrix_type>
class CommaInitializer;
namespace Fixed {
template <typename val_type, unsigned int ncols, unsigned int nrows> class Matrix;
}
/** \brief Namespace for elements of varying size.
*/
namespace Dynamic {
// forward declaration
template <typename val_type>
class Matrix;
/** \brief Class for both matrices and vectors.
*/
template <typename val_type>
class Matrix {
public:
typedef Matrix<val_type> 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 <val_type> result = Matrix<val_type>::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<matrix_type> operator<< (const val_type& value) {
return CommaInitializer<matrix_type> (*this, value);
}
// conversion different val_types
template <typename other_type>
Matrix (const Matrix<other_type> &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<val_type>(matrix(i,j));
}
}
}
// conversion from a fixed size matrix
template <typename other_type, unsigned int fnrows, unsigned int fncols>
Matrix (const Fixed::Matrix<other_type, fnrows, fncols> &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<val_type>(fixed_matrix(i,j));
}
}
}
template <typename other_matrix_type>
Matrix (const Block<other_matrix_type, value_type> &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<val_type>(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<val_type> normalized() const {
return Matrix<val_type> (*this) / this->norm();
}
Matrix<val_type> cross(const Matrix<val_type> &other_vector) {
assert (nrows * ncols == 3);
assert (other_vector.nrows * other_vector.ncols == 3);
Matrix<val_type> 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<val_type>(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<value_type>(rand()) / static_cast<value_type>(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<val_type> (rand()) / static_cast<val_type> (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<matrix_type, val_type>
block (unsigned int row_start, unsigned int col_start, unsigned int row_count, unsigned int col_count) {
return Block<matrix_type, val_type>(*this, row_start, col_start, row_count, col_count);
}
template <unsigned int row_count, unsigned int col_count>
Block<matrix_type, val_type>
block (unsigned int row_start, unsigned int col_start) {
return Block<matrix_type, val_type>(*this, row_start, col_start, row_count, col_count);
}
Block<matrix_type, val_type>
block (unsigned int row_start, unsigned int col_start, unsigned int row_count, unsigned int col_count) const {
return Block<matrix_type, val_type>(*this, row_start, col_start, row_count, col_count);
}
template <unsigned int row_count, unsigned int col_count>
Block<matrix_type, val_type>
block (unsigned int row_start, unsigned int col_start) const {
return Block<matrix_type, val_type>(*this, row_start, col_start, row_count, col_count);
}
// row and col accessors
Block<matrix_type, val_type>
col(unsigned int index) const {
return Block<matrix_type, val_type>(*this, 0, index, rows(), 1);
}
Block<matrix_type, val_type>
col(unsigned int index) {
return Block<matrix_type, val_type>(*this, 0, index, rows(), 1);
}
Block<matrix_type, val_type>
row(unsigned int index) const {
return Block<matrix_type, val_type>(*this, index, 0, 1, cols());
}
Block<matrix_type, val_type>
row(unsigned int index) {
return Block<matrix_type, val_type>(*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<val_type> operator*(const Matrix<val_type> &other_matrix) const {
assert (ncols == other_matrix.nrows);
Matrix<val_type> 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 <unsigned int _nrows, unsigned int _ncols>
Matrix<val_type> operator*(const Fixed::Matrix<val_type, _nrows, _ncols> &other_matrix) const {
assert (ncols == other_matrix.rows());
Matrix<val_type> 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<val_type> operator*(const Block<matrix_type, val_type> &other_matrix) const {
assert (ncols == other_matrix.rows());
Matrix<val_type> 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<val_type> transpose() const {
Matrix<val_type> 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<matrix_type> llt() const {
return LLT<matrix_type>(*this);
}
const HouseholderQR<matrix_type> householderQr() const {
return HouseholderQR<matrix_type>(*this);
}
const ColPivHouseholderQR<matrix_type> colPivHouseholderQr() const {
return ColPivHouseholderQR<matrix_type>(*this);
}
private:
unsigned int nrows;
unsigned int ncols;
bool mapped_data;
val_type* mData;
};
template <typename val_type>
inline Matrix<val_type> operator*(val_type scalar, const Matrix<val_type> &matrix) {
Matrix<val_type> result (matrix);
for (unsigned int i = 0; i < matrix.rows() * matrix.cols(); i++)
result.data()[i] *= scalar;
return result;
}
template <typename val_type, typename other_type>
inline Matrix<val_type> operator*(const Matrix<val_type> &matrix, other_type scalar) {
Matrix<val_type> result (matrix);
for (unsigned int i = 0; i < matrix.rows() * matrix.cols(); i++)
result.data()[i] *= static_cast<val_type>(scalar);
return result;
}
template <typename val_type>
inline std::ostream& operator<<(std::ostream& output, const Matrix<val_type> &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 */

899
src/SimpleMathOld/SimpleMathFixed.h
View File

@ -1,899 +0,0 @@
/**
* This is a highly inefficient math library. It was conceived by Martin
* Felis <martin.felis@iwr.uni-heidelberg.de> 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 <sstream>
#include <cstdlib>
#include <cmath>
#include <assert.h>
#include <algorithm>
#include <string.h>
#include "compileassert.h"
#include "SimpleMathBlock.h"
/** \brief Namespace for a highly inefficient math library
*
*/
namespace SimpleMath {
template <typename matrix_type>
class LLT;
template <typename matrix_type>
class HouseholderQR;
template <typename matrix_type>
class ColPivHouseholderQR;
template <typename matrix_type>
class CommaInitializer;
namespace Dynamic {
template <typename val_type> class Matrix;
}
/** \brief Namespace for fixed size elements
*/
namespace Fixed {
// forward declaration
template <typename val_type, unsigned int nrows, unsigned int ncols>
class Matrix;
/** \brief Fixed size matrix class
*/
template <typename val_type, unsigned int nrows, unsigned int ncols>
class Matrix {
public:
typedef Matrix<val_type, nrows, ncols> 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 <typename other_matrix_type>
Matrix (const Block<other_matrix_type, val_type> &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<val_type>(block(i,j));
}
}
}
template <typename other_matrix_type>
Matrix& operator= (const Block<other_matrix_type, val_type> &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<value_type>(block(i,j));
}
}
return *this;
}
template <typename other_type>
Matrix (const Matrix<other_type, nrows, ncols> &matrix) {
for (unsigned int i = 0; i < nrows; i++) {
for (unsigned int j = 0; j < ncols; j++) {
(*this)(i,j) = static_cast<val_type>(matrix(i,j));
}
}
}
template <typename other_type>
Matrix& operator=(const Matrix<other_type, nrows, ncols> &matrix) {
for (unsigned int i = 0; i < nrows; i++) {
for (unsigned int j = 0; j < ncols; j++) {
(*this)(i,j) = static_cast<val_type>(matrix(i,j));
}
}
return *this;
}
CommaInitializer<matrix_type> operator<< (const val_type& value) {
return CommaInitializer<matrix_type> (*this, value);
}
// conversion Dynamic->Fixed
Matrix(const Dynamic::Matrix<val_type> &dynamic_matrix);
Matrix& operator=(const Dynamic::Matrix<val_type> &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<val_type, 3, 1> cross(const Matrix<val_type, 3, 1> &other_vector) const {
COMPILE_ASSERT (nrows * ncols == 3);
Matrix<val_type, 3, 1> 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<val_type>(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<value_type>(rand()) / static_cast<value_type>(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<val_type> (rand()) / static_cast<val_type> (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<matrix_type, val_type>
block (unsigned int row_start, unsigned int col_start, unsigned int row_count, unsigned int col_count) {
return Block<matrix_type, val_type>(*this, row_start, col_start, row_count, col_count);
}
const Block<matrix_type, val_type>
block (unsigned int row_start, unsigned int col_start, unsigned int row_count, unsigned int col_count) const {
return Block<matrix_type, val_type>(*this, row_start, col_start, row_count, col_count);
}
// Blocks using block<r,c>(i,j) syntax
template <unsigned int block_row_count, unsigned int block_col_count>
Block<matrix_type, val_type>
block (unsigned int row_start, unsigned int col_start) {
return Block<matrix_type, val_type>(*this, row_start, col_start, block_row_count, block_col_count);
}
template <unsigned int block_row_count, unsigned int block_col_count>
const Block<matrix_type, val_type>
block (unsigned int row_start, unsigned int col_start) const {
return Block<matrix_type, val_type>(*this, row_start, col_start, block_row_count, block_col_count);
}
// row and col accessors
Block<matrix_type, val_type>
col(unsigned int index) const {
return Block<matrix_type, val_type>(*this, 0, index, rows(), 1);
}
Block<matrix_type, val_type>
col(unsigned int index) {
return Block<matrix_type, val_type>(*this, 0, index, rows(), 1);
}
Block<matrix_type, val_type>
row(unsigned int index) const {
return Block<matrix_type, val_type>(*this, index, 0, 1, cols());
}
Block<matrix_type, val_type>
row(unsigned int index) {
return Block<matrix_type, val_type>(*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 <unsigned int other_rows, unsigned int other_cols>
Matrix<val_type, nrows, other_cols> operator*(const Matrix<val_type, other_rows, other_cols> &matrix) const {
COMPILE_ASSERT (ncols == other_rows);
Matrix<val_type, nrows, other_cols> 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 <typename other_type>
Dynamic::Matrix<val_type> operator*(const Dynamic::Matrix<other_type> &other_matrix) {
assert (ncols == other_matrix.rows());
Dynamic::Matrix<val_type> 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<val_type>(other_matrix(k,j));
}
}
}
return result;
}
// Multiplication with a block
template <typename other_matrix_type>
Dynamic::Matrix<val_type> operator*(const Block<other_matrix_type, val_type> &block) {
assert (ncols == block.rows());
Dynamic::Matrix<val_type> 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<val_type>(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<val_type, ncols, nrows> transpose() const {
Matrix<val_type, ncols, nrows> 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<matrix_type> llt() const {
return LLT<matrix_type>(*this);
}
const HouseholderQR<matrix_type> householderQr() const {
return HouseholderQR<matrix_type>(*this);
}
const ColPivHouseholderQR<matrix_type> colPivHouseholderQr() const {
return ColPivHouseholderQR<matrix_type>(*this);
}
private:
val_type mData[nrows * ncols];
};
template <typename val_type, unsigned int nrows, unsigned int ncols>
inline Matrix<val_type, nrows, ncols> operator*(val_type scalar, const Matrix<val_type, nrows, ncols> &matrix) {
Matrix<val_type, nrows, ncols> result (matrix);
for (unsigned int i = 0; i < nrows * ncols; i++)
result.data()[i] *= scalar;
return result;
}
template <typename val_type, typename other_type, unsigned int nrows, unsigned int ncols>
inline Matrix<val_type, nrows, ncols> operator*(const Matrix<val_type, nrows, ncols> &matrix, other_type scalar) {
Matrix<val_type, nrows, ncols> result (matrix);
for (unsigned int i = 0; i < nrows * ncols; i++)
result.data()[i] *= static_cast<val_type> (scalar);
return result;
}
template <typename val_type, unsigned int nrows, unsigned int ncols>
inline std::ostream& operator<<(std::ostream& output, const Matrix<val_type, nrows, ncols> &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 */

348
src/SimpleMathOld/SimpleMathGL.h
View File

@ -1,348 +0,0 @@
#ifndef _SIMPLEMATHGL_H_
#define _SIMPLEMATHGL_H_
#include "SimpleMath.h"
#include <cmath>
namespace SimpleMath {
typedef SimpleMath::Fixed::Matrix<float, 3, 1> Vector3f;
typedef SimpleMath::Fixed::Matrix<float, 3, 3> Matrix33f;
typedef SimpleMath::Fixed::Matrix<float, 4, 1> Vector4f;
typedef SimpleMath::Fixed::Matrix<float, 4, 4> 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

22
src/SimpleMathOld/SimpleMathMap.h
View File

@ -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

138
src/SimpleMathOld/SimpleMathMixed.h
View File

@ -1,138 +0,0 @@
/**
* This is a highly inefficient math library. It was conceived by Martin
* Felis <martin.felis@iwr.uni-heidelberg.de> 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 <sstream>
#include <cstdlib>
#include <assert.h>
#include <iostream>
#include "compileassert.h"
/** \brief Namespace for a highly inefficient math library
*
*/
namespace SimpleMath {
// conversion Dynamic->Fixed
template <typename val_type, unsigned int nrows, unsigned int ncols>
inline Fixed::Matrix<val_type, nrows, ncols>::Matrix(const Dynamic::Matrix<val_type> &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 <typename val_type, unsigned int nrows, unsigned int ncols>
inline Fixed::Matrix<val_type, nrows, ncols>& Fixed::Matrix<val_type, nrows, ncols>::operator=(const Dynamic::Matrix<val_type> &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 <typename val_type, unsigned int nrows, unsigned int ncols>
inline Dynamic::Matrix<val_type> operator*(
const Fixed::Matrix<val_type, nrows, ncols> &matrix_a,
const Dynamic::Matrix<val_type> &matrix_b) {
assert (matrix_a.cols() == matrix_b.rows());
Dynamic::Matrix<val_type> 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 <typename val_type, unsigned int nrows, unsigned int ncols>
inline Dynamic::Matrix<val_type> operator*(
const Dynamic::Matrix<val_type> &matrix_a,
const Fixed::Matrix<val_type, nrows, ncols> &matrix_b) {
assert (matrix_a.cols() == matrix_b.rows());
Dynamic::Matrix<val_type> 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 <typename val_type, unsigned int nrows, unsigned int ncols>
inline bool operator==(
const Fixed::Matrix<val_type, nrows, ncols> &matrix_a,
const Dynamic::Matrix<val_type> &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 <typename val_type, unsigned int nrows, unsigned int ncols>
inline bool operator==(
const Dynamic::Matrix<val_type> &matrix_b,
const Fixed::Matrix<val_type, nrows, ncols> &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 */

324
src/SimpleMathOld/SimpleMathQR.h
View File

@ -1,324 +0,0 @@
#ifndef _SIMPLE_MATH_QR_H
#define _SIMPLE_MATH_QR_H
#include <iostream>
#include <limits>
#include "SimpleMathFixed.h"
#include "SimpleMathDynamic.h"
#include "SimpleMathBlock.h"
namespace SimpleMath {
template <typename matrix_type>
class HouseholderQR {
public:
typedef typename matrix_type::value_type value_type;
HouseholderQR() :
mIsFactorized(false)
{}
private:
typedef Dynamic::Matrix<value_type> MatrixXXd;
typedef Dynamic::Matrix<value_type> 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<value_type>::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<value_type> solve (
const Dynamic::Matrix<value_type> &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<value_type>::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<value_type> 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<value_type> householderQ () const {
return mQ;
}
Dynamic::Matrix<value_type> matrixR () const {
return mR;
}
};
template <typename matrix_type>
class ColPivHouseholderQR {
public:
typedef typename matrix_type::value_type value_type;
private:
typedef Dynamic::Matrix<value_type> MatrixXXd;
typedef Dynamic::Matrix<value_type> 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<value_type>::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<value_type>::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<value_type>(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<value_type> solve (
const Dynamic::Matrix<value_type> &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<value_type>::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<value_type> 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<value_type> householderQ () const {
return mQ;
}
Dynamic::Matrix<value_type> matrixR () const {
return mR;
}
Dynamic::Matrix<value_type> 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

39
src/SimpleMathOld/compileassert.h
View File

@ -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 <bool> struct STATIC_ASSERT_FAILURE;
template <> struct STATIC_ASSERT_FAILURE<true> { enum { value = 1 }; };
template<int x> 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