Deleted old SimpleMath, updated SimpleMathV2
Martin Felis
parent
369d6a634b
commit
ffe4e3bec0
|
|
@ -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();
|
||||
}
|
||||
|
|
@ -491,6 +543,57 @@ struct MatrixBase {
|
|||
}
|
||||
|
||||
Derived inverse() const {
|
||||
if (rows() == cols()) {
|
||||
if (rows() == 1) {
|
||||
Derived result(rows(), cols());
|
||||
result(0,0) = static_cast<ScalarType>(1.) / operator()(0,0);
|
||||
|
||||
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);
|
||||
|
||||
Derived result(rows(), cols());
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
|
|
@ -506,18 +609,30 @@ struct MatrixBase {
|
|||
return result;
|
||||
}
|
||||
|
||||
// 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);
|
||||
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() {
|
||||
|
|
@ -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++) {
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
@ -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 */
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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 */
|
||||
|
|
@ -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 */
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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 */
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
Loading…
Reference in New Issue