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
|
#pragma once
|
||||||
|
|
||||||
#include <sstream>
|
#include <sstream>
|
||||||
|
|
@ -32,10 +67,16 @@ struct Transpose;
|
||||||
typedef Matrix<float, 3, 3> Matrix33f;
|
typedef Matrix<float, 3, 3> Matrix33f;
|
||||||
typedef Matrix<float, 3, 1> Vector3f;
|
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;
|
class HouseholderQR;
|
||||||
|
|
||||||
template <typename Derived, typename ScalarType, int NumRows, int NumCols>
|
template <typename Derived>
|
||||||
class ColPivHouseholderQR;
|
class ColPivHouseholderQR;
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -48,10 +89,18 @@ struct MatrixBase {
|
||||||
typedef MatrixBase<Derived, ScalarType, Rows, Cols> MatrixType;
|
typedef MatrixBase<Derived, ScalarType, Rows, Cols> MatrixType;
|
||||||
typedef ScalarType value_type;
|
typedef ScalarType value_type;
|
||||||
|
|
||||||
|
enum {
|
||||||
|
RowsAtCompileTime = Rows,
|
||||||
|
ColsAtCompileTime = Cols
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
Derived& operator=(const Derived& other) {
|
Derived& operator=(const Derived& other) {
|
||||||
if (static_cast<const void*>(this) != static_cast<const void*>(&other)) {
|
if (static_cast<const void*>(this) != static_cast<const void*>(&other)) {
|
||||||
for (size_t i = 0; i < other.rows(); i++) {
|
int i, j, in = other.rows(), jn = other.cols();
|
||||||
for (size_t j = 0; j < other.cols(); j++) {
|
|
||||||
|
for (size_t i = 0; i < in; i++) {
|
||||||
|
for (size_t j = 0; j < jn; j++) {
|
||||||
this->operator()(i,j) = other(i,j);
|
this->operator()(i,j) = other(i,j);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -64,8 +113,10 @@ struct MatrixBase {
|
||||||
template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
|
template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
|
||||||
Derived& operator=(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) {
|
Derived& operator=(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) {
|
||||||
if (static_cast<const void*>(this) != static_cast<const void*>(&other)) {
|
if (static_cast<const void*>(this) != static_cast<const void*>(&other)) {
|
||||||
for (size_t i = 0; i < other.rows(); i++) {
|
int i, j, in = other.rows(), jn = other.cols();
|
||||||
for (size_t j = 0; j < other.cols(); j++) {
|
|
||||||
|
for (size_t i = 0; i < in; i++) {
|
||||||
|
for (size_t j = 0; j < jn; j++) {
|
||||||
this->operator()(i,j) = other(i,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> operator*(const double& scalar) const {
|
||||||
Matrix<ScalarType, Rows, Cols> result (rows(), cols());
|
Matrix<ScalarType, Rows, Cols> result (rows(), cols());
|
||||||
|
int i, j, in = rows(), jn = cols();
|
||||||
|
|
||||||
unsigned int i,j;
|
for (i = 0; i < in; i++) {
|
||||||
for (i = 0; i < rows(); i++) {
|
for (j = 0; j < jn; j++) {
|
||||||
for (j = 0; j < cols(); j++) {
|
|
||||||
result (i,j) = operator()(i,j) * static_cast<ScalarType>(scalar);
|
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> operator*(const float& scalar) const {
|
||||||
Matrix<ScalarType, Rows, Cols> result (rows(), cols());
|
Matrix<ScalarType, Rows, Cols> result (rows(), cols());
|
||||||
|
int i, j, in = rows(), jn = cols();
|
||||||
|
|
||||||
unsigned int i,j;
|
for (i = 0; i < in; i++) {
|
||||||
for (i = 0; i < rows(); i++) {
|
for (j = 0; j < jn; j++) {
|
||||||
for (j = 0; j < cols(); j++) {
|
|
||||||
result (i,j) = operator()(i,j) * static_cast<ScalarType>(scalar);
|
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>
|
template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
|
||||||
bool operator==(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) const {
|
bool operator==(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) const {
|
||||||
unsigned int i,j;
|
int i, j, in = rows(), jn = cols();
|
||||||
for (i = 0; i < rows(); i++) {
|
|
||||||
for (j = 0; j < cols(); j++) {
|
for (i = 0; i < in; i++) {
|
||||||
|
for (j = 0; j < jn; j++) {
|
||||||
if (this->operator()(i,j) != other(i,j))
|
if (this->operator()(i,j) != other(i,j))
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
@ -151,11 +203,11 @@ struct MatrixBase {
|
||||||
Matrix<ScalarType, Rows, OtherCols>
|
Matrix<ScalarType, Rows, OtherCols>
|
||||||
operator*(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols> &other) const {
|
operator*(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols> &other) const {
|
||||||
Matrix<ScalarType, Rows, OtherCols> result(Matrix<ScalarType, Rows, OtherCols>::Zero(rows(), other.cols()));
|
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 < in; i++) {
|
||||||
for (i = 0; i < rows(); i++) {
|
for (j = 0; j < jn; j++) {
|
||||||
for (j = 0; j < other.cols(); j++) {
|
for (k = 0; k < kn; k++) {
|
||||||
for (k = 0; k < other.rows(); k++) {
|
|
||||||
result(i, j) += operator()(i, k) * other(k, j);
|
result(i, j) += operator()(i, k) * other(k, j);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -165,14 +217,15 @@ struct MatrixBase {
|
||||||
}
|
}
|
||||||
|
|
||||||
template <typename OtherDerived>
|
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));
|
Derived copy (*static_cast<const Derived*>(this));
|
||||||
this->setZero();
|
this->setZero();
|
||||||
|
|
||||||
unsigned int i, j, k;
|
int i, j, k, in = rows(), jn = other.cols(), kn = other.rows();
|
||||||
for (i = 0; i < rows(); i++) {
|
|
||||||
for (j = 0; j < other.cols(); j++) {
|
for (i = 0; i < in; i++) {
|
||||||
for (k = 0; k < other.rows(); k++) {
|
for (j = 0; j < jn; j++) {
|
||||||
|
for (k = 0; k < kn; k++) {
|
||||||
this->operator()(i, j) += copy.operator()(i, k) * other(k, j);
|
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> operator-() const {
|
||||||
Matrix<ScalarType, Rows, Cols> copy (*static_cast<const Derived*>(this));
|
Matrix<ScalarType, Rows, Cols> copy (*static_cast<const Derived*>(this));
|
||||||
for (int i = 0; i < rows(); i++) {
|
int i, j, in = rows(), jn = cols();
|
||||||
for (int j = 0; j < cols(); j++) {
|
|
||||||
|
for (int i = 0; i < in; i++) {
|
||||||
|
for (int j = 0; j < jn; j++) {
|
||||||
copy(i,j) *= static_cast<ScalarType>(-1.);
|
copy(i,j) *= static_cast<ScalarType>(-1.);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -191,9 +246,10 @@ struct MatrixBase {
|
||||||
}
|
}
|
||||||
|
|
||||||
Derived operator*=(const ScalarType& s) {
|
Derived operator*=(const ScalarType& s) {
|
||||||
unsigned int i,j;
|
int i, j, in = rows(), jn = cols();
|
||||||
for (i = 0; i < rows(); i++) {
|
|
||||||
for (j = 0; j < cols(); j++) {
|
for (int i = 0; i < in; i++) {
|
||||||
|
for (int j = 0; j < jn; j++) {
|
||||||
operator()(i,j) *= s;
|
operator()(i,j) *= s;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -213,7 +269,6 @@ struct MatrixBase {
|
||||||
void conservativeResize(unsigned int nrows, unsigned int ncols = 1) {
|
void conservativeResize(unsigned int nrows, unsigned int ncols = 1) {
|
||||||
static_assert(Rows == Dynamic, "Resize of fixed size matrices not allowed.");
|
static_assert(Rows == Dynamic, "Resize of fixed size matrices not allowed.");
|
||||||
|
|
||||||
|
|
||||||
Derived copy(*this);
|
Derived copy(*this);
|
||||||
|
|
||||||
unsigned int arows = std::min(nrows, (unsigned int) rows());
|
unsigned int arows = std::min(nrows, (unsigned int) rows());
|
||||||
|
|
@ -375,11 +430,8 @@ struct MatrixBase {
|
||||||
operator()(5,3) = v53;
|
operator()(5,3) = v53;
|
||||||
operator()(5,4) = v54;
|
operator()(5,4) = v54;
|
||||||
operator()(5,5) = v55;
|
operator()(5,5) = v55;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
size_t rows() const {
|
size_t rows() const {
|
||||||
return static_cast<const Derived*>(this)->rows();
|
return static_cast<const Derived*>(this)->rows();
|
||||||
}
|
}
|
||||||
|
|
@ -400,11 +452,11 @@ struct MatrixBase {
|
||||||
}
|
}
|
||||||
|
|
||||||
const ScalarType& operator[](const size_t& i) const {
|
const ScalarType& operator[](const size_t& i) const {
|
||||||
assert(cols() == 1);
|
assert(cols() == 1);
|
||||||
return static_cast<const Derived*>(this)->operator()(i,0);
|
return static_cast<const Derived*>(this)->operator()(i,0);
|
||||||
}
|
}
|
||||||
ScalarType& operator[](const size_t& i) {
|
ScalarType& operator[](const size_t& i) {
|
||||||
assert(cols() == 1);
|
assert(cols() == 1);
|
||||||
return static_cast<Derived*>(this)->operator()(i,0);
|
return static_cast<Derived*>(this)->operator()(i,0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -490,35 +542,98 @@ struct MatrixBase {
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
Derived inverse() const {
|
Derived inverse() const {
|
||||||
return colPivHouseholderQr().inverse();
|
if (rows() == cols()) {
|
||||||
}
|
if (rows() == 1) {
|
||||||
|
Derived result(rows(), cols());
|
||||||
|
result(0,0) = static_cast<ScalarType>(1.) / operator()(0,0);
|
||||||
|
|
||||||
ScalarType trace() const {
|
return result;
|
||||||
assert(rows() == cols());
|
} else if (rows() == 2) {
|
||||||
|
const ScalarType& a = operator()(0,0);
|
||||||
|
const ScalarType& b = operator()(0,1);
|
||||||
|
const ScalarType& c = operator()(1,0);
|
||||||
|
const ScalarType& d = operator()(1,1);
|
||||||
|
|
||||||
ScalarType result = static_cast<ScalarType>(0.0);
|
Derived result(rows(), cols());
|
||||||
|
|
||||||
for (unsigned int i = 0; i < rows(); i++) {
|
ScalarType detinv = static_cast<ScalarType>(1.) / (a * d - b * c);
|
||||||
result += operator()(i,i);
|
|
||||||
|
result(0,0) = d * detinv;
|
||||||
|
result(0,1) = -b * detinv;
|
||||||
|
result(1,0) = -c * detinv;
|
||||||
|
result(1,1) = d * detinv;
|
||||||
|
|
||||||
|
return result;
|
||||||
|
} else if (rows() == 3) {
|
||||||
|
// source:
|
||||||
|
// https://stackoverflow.com/questions/983999/simple-3x3-matrix-inverse-code-c
|
||||||
|
|
||||||
|
// computes the inverse of a matrix m
|
||||||
|
ScalarType det = operator()(0, 0) * (operator()(1, 1) * operator()(2, 2)
|
||||||
|
- operator()(2, 1) * operator()(1, 2))
|
||||||
|
- operator()(0, 1) * (operator()(1, 0) * operator()(2, 2)
|
||||||
|
- operator()(1, 2) * operator()(2, 0))
|
||||||
|
+ operator()(0, 2) * (operator()(1, 0) * operator()(2, 1)
|
||||||
|
- operator()(1, 1) * operator()(2, 0));
|
||||||
|
|
||||||
|
ScalarType invdet = 1. / det;
|
||||||
|
|
||||||
|
Derived result(rows(), cols());
|
||||||
|
|
||||||
|
result(0,0) = (operator()(1, 1) * operator()(2, 2) - operator()(2, 1) * operator()(1, 2)) * invdet;
|
||||||
|
result(0,1) = (operator()(0, 2) * operator()(2, 1) - operator()(0, 1) * operator()(2, 2)) * invdet;
|
||||||
|
result(0,2) = (operator()(0, 1) * operator()(1, 2) - operator()(0, 2) * operator()(1, 1)) * invdet;
|
||||||
|
result(1,0) = (operator()(1, 2) * operator()(2, 0) - operator()(1, 0) * operator()(2, 2)) * invdet;
|
||||||
|
result(1,1) = (operator()(0, 0) * operator()(2, 2) - operator()(0, 2) * operator()(2, 0)) * invdet;
|
||||||
|
result(1,2) = (operator()(1, 0) * operator()(0, 2) - operator()(0, 0) * operator()(1, 2)) * invdet;
|
||||||
|
result(2,0) = (operator()(1, 0) * operator()(2, 1) - operator()(2, 0) * operator()(1, 1)) * invdet;
|
||||||
|
result(2,1) = (operator()(2, 0) * operator()(0, 1) - operator()(0, 0) * operator()(2, 1)) * invdet;
|
||||||
|
result(2,2) = (operator()(0, 0) * operator()(1, 1) - operator()(1, 0) * operator()(0, 1)) * invdet;
|
||||||
|
|
||||||
|
return result;
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
return colPivHouseholderQr().inverse();
|
||||||
|
}
|
||||||
|
|
||||||
return result;
|
ScalarType trace() const {
|
||||||
|
assert(rows() == cols());
|
||||||
|
|
||||||
|
ScalarType result = static_cast<ScalarType>(0.0);
|
||||||
|
|
||||||
|
for (unsigned int i = 0; i < rows(); i++) {
|
||||||
|
result += operator()(i,i);
|
||||||
}
|
}
|
||||||
|
|
||||||
// TODO: implement Cholesky decompositioe
|
return result;
|
||||||
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 result / static_cast<ScalarType>(rows() * cols());
|
||||||
return HouseholderQR<Derived, ScalarType, Rows, Cols>(*this);
|
}
|
||||||
}
|
|
||||||
|
|
||||||
const ColPivHouseholderQR<Derived, ScalarType, Rows, Cols> colPivHouseholderQr() const {
|
const LLT<Derived> llt() const {
|
||||||
return ColPivHouseholderQR<Derived, ScalarType, Rows, Cols>(*this);
|
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() {
|
ScalarType* data() {
|
||||||
return static_cast<Derived*>(this)->data();
|
return static_cast<Derived*>(this)->data();
|
||||||
|
|
@ -673,9 +788,9 @@ struct Storage {
|
||||||
resize(rows, cols);
|
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) {
|
void resize(int num_rows, int num_cols) {
|
||||||
// Resizing of fixed size matrices not allowed
|
// Resizing of fixed size matrices not allowed
|
||||||
|
|
@ -689,15 +804,15 @@ struct Storage {
|
||||||
assert (num_rows == NumRows && num_cols == NumCols);
|
assert (num_rows == NumRows && num_cols == NumCols);
|
||||||
}
|
}
|
||||||
|
|
||||||
ScalarType& coeff(int row_index, int col_index) {
|
inline ScalarType& coeff(int row_index, int col_index) {
|
||||||
assert (row_index >= 0 && row_index <= NumRows);
|
// assert (row_index >= 0 && row_index <= NumRows);
|
||||||
assert (col_index >= 0 && col_index <= NumCols);
|
// assert (col_index >= 0 && col_index <= NumCols);
|
||||||
return mData[row_index * NumCols + col_index];
|
return mData[row_index * NumCols + col_index];
|
||||||
}
|
}
|
||||||
|
|
||||||
const ScalarType& coeff(int row_index, int col_index) const {
|
inline const ScalarType& coeff(int row_index, int col_index) const {
|
||||||
assert (row_index >= 0 && row_index <= NumRows);
|
// assert (row_index >= 0 && row_index <= NumRows);
|
||||||
assert (col_index >= 0 && col_index <= NumCols);
|
// assert (col_index >= 0 && col_index <= NumCols);
|
||||||
return mData[row_index * NumCols + col_index];
|
return mData[row_index * NumCols + col_index];
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
@ -710,12 +825,16 @@ struct Storage<ScalarType, 0, Dynamic, NumCols> {
|
||||||
|
|
||||||
Storage() {}
|
Storage() {}
|
||||||
|
|
||||||
|
~Storage() {
|
||||||
|
delete[] mData;
|
||||||
|
}
|
||||||
|
|
||||||
Storage(int rows, int cols) {
|
Storage(int rows, int cols) {
|
||||||
resize(rows, cols);
|
resize(rows, cols);
|
||||||
}
|
}
|
||||||
|
|
||||||
size_t rows() const { return mRows; }
|
inline size_t rows() const { return mRows; }
|
||||||
size_t cols() const { return mCols; }
|
inline size_t cols() const { return mCols; }
|
||||||
|
|
||||||
void resize(int num_rows, int num_cols) {
|
void resize(int num_rows, int num_cols) {
|
||||||
if (mRows != num_rows || mCols != 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) {
|
inline ScalarType& coeff(int row_index, int col_index) {
|
||||||
assert (row_index >= 0 && row_index <= mRows);
|
// assert (row_index >= 0 && row_index <= mRows);
|
||||||
assert (col_index >= 0 && col_index <= mCols);
|
// assert (col_index >= 0 && col_index <= mCols);
|
||||||
return mData[row_index * mCols + col_index];
|
return mData[row_index * mCols + col_index];
|
||||||
}
|
}
|
||||||
const ScalarType& coeff(int row_index, int col_index) const {
|
inline const ScalarType& coeff(int row_index, int col_index) const {
|
||||||
assert (row_index >= 0 && row_index <= mRows);
|
// assert (row_index >= 0 && row_index <= mRows);
|
||||||
assert (col_index >= 0 && col_index <= mCols);
|
// assert (col_index >= 0 && col_index <= mCols);
|
||||||
return mData[row_index * mCols + col_index];
|
return mData[row_index * mCols + col_index];
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
@ -750,12 +869,16 @@ struct Storage<ScalarType, 0, Dynamic, Dynamic> {
|
||||||
|
|
||||||
Storage() {}
|
Storage() {}
|
||||||
|
|
||||||
|
~Storage() {
|
||||||
|
delete[] mData;
|
||||||
|
}
|
||||||
|
|
||||||
Storage(int rows, int cols) {
|
Storage(int rows, int cols) {
|
||||||
resize(rows, cols);
|
resize(rows, cols);
|
||||||
}
|
}
|
||||||
|
|
||||||
size_t rows() const { return mRows; }
|
inline size_t rows() const { return mRows; }
|
||||||
size_t cols() const { return mCols; }
|
inline size_t cols() const { return mCols; }
|
||||||
|
|
||||||
void resize(int num_rows, int num_cols) {
|
void resize(int num_rows, int num_cols) {
|
||||||
if (mRows != num_rows || mCols != 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) {
|
inline ScalarType& coeff(int row_index, int col_index) {
|
||||||
assert (row_index >= 0 && row_index <= mRows);
|
// assert (row_index >= 0 && row_index <= mRows);
|
||||||
assert (col_index >= 0 && col_index <= mCols);
|
// assert (col_index >= 0 && col_index <= mCols);
|
||||||
return mData[row_index * mCols + col_index];
|
return mData[row_index * mCols + col_index];
|
||||||
}
|
}
|
||||||
const ScalarType& coeff(int row_index, int col_index) const {
|
inline const ScalarType& coeff(int row_index, int col_index) const {
|
||||||
assert (row_index >= 0 && row_index <= mRows);
|
// assert (row_index >= 0 && row_index <= mRows);
|
||||||
assert (col_index >= 0 && col_index <= mCols);
|
// assert (col_index >= 0 && col_index <= mCols);
|
||||||
return mData[row_index * mCols + col_index];
|
return mData[row_index * mCols + col_index];
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
template <typename ScalarType, int NumRows, int NumCols>
|
template <typename ScalarType, int NumRows, int NumCols>
|
||||||
|
|
@ -800,21 +923,45 @@ struct Matrix : public MatrixBase<Matrix<ScalarType, NumRows, NumCols>, ScalarTy
|
||||||
SizeAtCompileTime / RowsAtCompileTime
|
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) {}
|
mStorage(rows, cols) {}
|
||||||
|
|
||||||
explicit Matrix(unsigned int rows, unsigned int cols = 1) :
|
explicit Matrix(int rows, unsigned int cols) :
|
||||||
mStorage(rows, 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) {}
|
mStorage(rows, cols) {}
|
||||||
|
|
||||||
template<typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
|
template<typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
|
||||||
Matrix(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols> &other) {
|
Matrix(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols> &other) {
|
||||||
mStorage.resize(other.rows(), other.cols());
|
mStorage.resize(other.rows(), other.cols());
|
||||||
|
|
||||||
for (size_t i = 0; i < rows(); i++) {
|
int i, j, in = rows(), jn = cols();
|
||||||
for (size_t j = 0; j < cols(); j++) {
|
|
||||||
|
for (size_t i = 0; i < in; i++) {
|
||||||
|
for (size_t j = 0; j < jn; j++) {
|
||||||
this->operator()(i, j) = other(i, j);
|
this->operator()(i, j) = other(i, j);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -837,15 +984,7 @@ struct Matrix : public MatrixBase<Matrix<ScalarType, NumRows, NumCols>, ScalarTy
|
||||||
//
|
//
|
||||||
// Constructor for vectors
|
// Constructor for vectors
|
||||||
//
|
//
|
||||||
Matrix (
|
explicit Matrix (
|
||||||
const ScalarType& v0
|
|
||||||
) {
|
|
||||||
static_assert (NumRows * NumCols == 1, "Invalid matrix size");
|
|
||||||
|
|
||||||
operator()(0,0) = v0;
|
|
||||||
}
|
|
||||||
|
|
||||||
Matrix (
|
|
||||||
const ScalarType& v0,
|
const ScalarType& v0,
|
||||||
const ScalarType& v1
|
const ScalarType& v1
|
||||||
) {
|
) {
|
||||||
|
|
@ -1061,8 +1200,10 @@ struct Matrix : public MatrixBase<Matrix<ScalarType, NumRows, NumCols>, ScalarTy
|
||||||
Matrix& operator+=(const OtherDerived& other) {
|
Matrix& operator+=(const OtherDerived& other) {
|
||||||
assert (rows() == other.rows() && cols() == other.cols() && "Error: matrix dimensions do not match!");
|
assert (rows() == other.rows() && cols() == other.cols() && "Error: matrix dimensions do not match!");
|
||||||
|
|
||||||
for (size_t i = 0; i < rows(); i++) {
|
int i, j, in = rows(), jn = cols();
|
||||||
for (size_t j = 0; j < cols(); j++) {
|
|
||||||
|
for (size_t i = 0; i < in; i++) {
|
||||||
|
for (size_t j = 0; j < jn; j++) {
|
||||||
this->operator()(i,j) += other(i,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) {
|
Matrix& operator-=(const OtherDerived& other) {
|
||||||
assert (rows() == other.rows() && cols() == other.cols() && "Error: matrix dimensions do not match!");
|
assert (rows() == other.rows() && cols() == other.cols() && "Error: matrix dimensions do not match!");
|
||||||
|
|
||||||
for (size_t i = 0; i < rows(); i++) {
|
int i, j, in = rows(), jn = cols();
|
||||||
for (size_t j = 0; j < cols(); j++) {
|
|
||||||
this->operator()(i,j) -= other(i,j);
|
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;
|
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);
|
return mStorage.coeff(i, j);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -1093,10 +1240,6 @@ struct Matrix : public MatrixBase<Matrix<ScalarType, NumRows, NumCols>, ScalarTy
|
||||||
return mStorage.mData;
|
return mStorage.mData;
|
||||||
}
|
}
|
||||||
|
|
||||||
const ScalarType& operator()(const size_t& i, const size_t& j) const {
|
|
||||||
return mStorage.coeff(i, j);
|
|
||||||
}
|
|
||||||
|
|
||||||
size_t cols() const {
|
size_t cols() const {
|
||||||
return mStorage.cols();
|
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>
|
template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
|
||||||
Transpose& operator=(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) {
|
Transpose& operator=(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) {
|
||||||
if (static_cast<const void*>(this) != static_cast<const void*>(&other)) {
|
if (static_cast<const void*>(this) != static_cast<const void*>(&other)) {
|
||||||
for (size_t i = 0; i < other.rows(); i++) {
|
int i, j, in = other.rows(), jn = other.cols();
|
||||||
for (size_t j = 0; j < other.cols(); j++) {
|
|
||||||
|
for (size_t i = 0; i < in; i++) {
|
||||||
|
for (size_t j = 0; j < jn; j++) {
|
||||||
this->operator()(i,j) = other(i,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) {
|
Block& operator=(const Block &other) {
|
||||||
unsigned int i,j;
|
int i, j, in = rows(), jn = cols();
|
||||||
for (i = 0; i < rows(); i++) {
|
|
||||||
for (j = 0; j < cols(); j++) {
|
for (i = 0; i < in; i++) {
|
||||||
|
for (j = 0; j < jn; j++) {
|
||||||
this->operator()(i,j) = other(i,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>
|
template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
|
||||||
Block& operator=(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) {
|
Block& operator=(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) {
|
||||||
unsigned int i,j;
|
int i, j, in = rows(), jn = cols();
|
||||||
for (i = 0; i < rows(); i++) {
|
|
||||||
for (j = 0; j < cols(); j++) {
|
for (i = 0; i < in; i++) {
|
||||||
|
for (j = 0; j < jn; j++) {
|
||||||
this->operator()(i,j) = other(i,j);
|
this->operator()(i,j) = other(i,j);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -1300,25 +1447,15 @@ struct Block : public MatrixBase<Block<Derived, ScalarType, NumRows, NumCols>, S
|
||||||
return *this;
|
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>
|
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> operator*(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) const {
|
||||||
Matrix<ScalarType, NumRows, OtherCols> result (rows(), other.cols());
|
Matrix<ScalarType, NumRows, OtherCols> result (rows(), other.cols());
|
||||||
|
|
||||||
unsigned int i,j,k;
|
int i, j, k, in = rows(), jn = other.cols(), kn = other.rows();
|
||||||
for (i = 0; i < rows(); i++) {
|
for (i = 0; i < in; i++) {
|
||||||
for (j = 0; j < other.cols(); j++) {
|
for (j = 0; j < jn; j++) {
|
||||||
for (k = 0; k < other.rows(); k++) {
|
for (k = 0; k < kn; k++) {
|
||||||
result (i,j) += operator()(i,k) * other(k,j);
|
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> operator+(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) const {
|
||||||
Matrix<ScalarType, NumRows, OtherCols> result (rows(), other.cols());
|
Matrix<ScalarType, NumRows, OtherCols> result (rows(), other.cols());
|
||||||
|
|
||||||
unsigned int i,j,k;
|
int i, j, in = rows(), jn = other.cols();
|
||||||
for (i = 0; i < rows(); i++) {
|
for (i = 0; i < in; i++) {
|
||||||
for (j = 0; j < other.cols(); j++) {
|
for (j = 0; j < jn; j++) {
|
||||||
result (i,j) = operator()(i,j) + other(i,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
|
// QR Decomposition
|
||||||
//
|
//
|
||||||
template <typename Derived, typename ScalarType, int NumRows, int NumCols>
|
template <typename Derived>
|
||||||
class HouseholderQR {
|
class HouseholderQR {
|
||||||
public:
|
public:
|
||||||
typedef MatrixBase<Derived, ScalarType, NumRows, NumCols> MatrixType;
|
typedef typename Derived::value_type value_type;
|
||||||
typedef typename MatrixType::value_type value_type;
|
typedef MatrixBase<Derived, value_type, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> MatrixType;
|
||||||
|
|
||||||
HouseholderQR() :
|
HouseholderQR() :
|
||||||
mIsFactorized(false)
|
mIsFactorized(false)
|
||||||
|
|
@ -1383,10 +1805,10 @@ public:
|
||||||
private:
|
private:
|
||||||
typedef Matrix<value_type> VectorXd;
|
typedef Matrix<value_type> VectorXd;
|
||||||
typedef Matrix<value_type> MatrixXXd;
|
typedef Matrix<value_type> MatrixXXd;
|
||||||
typedef Matrix<ScalarType, NumRows, 1> ColumnVector;
|
typedef Matrix<value_type, Derived::RowsAtCompileTime, 1> ColumnVector;
|
||||||
|
|
||||||
bool mIsFactorized;
|
bool mIsFactorized;
|
||||||
Matrix<ScalarType, NumRows, NumRows> mQ;
|
Matrix<value_type, Derived::RowsAtCompileTime, Derived::RowsAtCompileTime> mQ;
|
||||||
Derived mR;
|
Derived mR;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
@ -1452,7 +1874,7 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
if (fabs(mR(i,i)) < std::numeric_limits<value_type>::epsilon() * 10) {
|
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();
|
abort();
|
||||||
}
|
}
|
||||||
x[i] = z / mR(i,i);
|
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 {
|
class ColPivHouseholderQR {
|
||||||
public:
|
public:
|
||||||
typedef MatrixBase<Derived, ScalarType, NumRows, NumCols> MatrixType;
|
typedef typename Derived::value_type value_type;
|
||||||
typedef typename MatrixType::value_type value_type;
|
typedef MatrixBase<Derived, value_type, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> MatrixType;
|
||||||
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
typedef Matrix<value_type> VectorXd;
|
typedef Matrix<value_type> VectorXd;
|
||||||
typedef Matrix<value_type> MatrixXXd;
|
typedef Matrix<value_type> MatrixXXd;
|
||||||
typedef Matrix<ScalarType, NumRows, 1> ColumnVector;
|
typedef Matrix<value_type, Derived::RowsAtCompileTime, 1> ColumnVector;
|
||||||
|
|
||||||
bool mIsFactorized;
|
bool mIsFactorized;
|
||||||
Matrix<ScalarType, NumRows, NumRows> mQ;
|
Matrix<value_type, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> mQ;
|
||||||
Derived mR;
|
Derived mR;
|
||||||
|
|
||||||
unsigned int *mPermutations;
|
unsigned int *mPermutations;
|
||||||
|
|
@ -1638,7 +2060,7 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
if (fabs(mR(i,i)) < std::numeric_limits<value_type>::epsilon() * 10) {
|
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();
|
abort();
|
||||||
}
|
}
|
||||||
x[mPermutations[i]] = z / mR(i,i);
|
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