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

Working on single header simple math version

simple_math_single_header
Martin Felis 2018-09-28 09:17:22 +02:00
parent 9588ee0674
commit 09db06fbfd
23 changed files with 1738 additions and 4631 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
3rdparty/glfw/CMakeLists.txt vendored
View File

@ -376,18 +376,6 @@ configure_file(src/glfw3.pc.in src/glfw3.pc @ONLY)
#-------------------------------------------------------------------- #--------------------------------------------------------------------
add_subdirectory(src) add_subdirectory(src)
if (GLFW_BUILD_EXAMPLES)
add_subdirectory(examples)
endif()
if (GLFW_BUILD_TESTS)
add_subdirectory(tests)
endif()
if (DOXYGEN_FOUND AND GLFW_BUILD_DOCS)
add_subdirectory(docs)
endif()
#-------------------------------------------------------------------- #--------------------------------------------------------------------
# Install files other than the library # Install files other than the library
# The library is installed by src/CMakeLists.txt # The library is installed by src/CMakeLists.txt

10
src/Serializer.h
View File

@ -1,6 +1,6 @@
#pragma once #pragma once
#include "SimpleMath/SimpleMath.h" #include "math_types.h"
#include <iostream> #include <iostream>
#include <fstream> #include <fstream>
#include <unordered_map> #include <unordered_map>
@ -134,11 +134,11 @@ bool SerializedUint16 (Serializer &serializer, const std::string &key, uint16_t&
} }
template <typename Serializer> template <typename Serializer>
bool SerializeVec3 (Serializer &serializer, const std::string &key, SimpleMath::Vector3f& value) { bool SerializeVec3 (Serializer &serializer, const std::string &key, Vector3f& value) {
return serializer.SerializeData(key, reinterpret_cast<char*>(&value), sizeof(SimpleMath::Vector3f)); return serializer.SerializeData(key, reinterpret_cast<char*>(&value), sizeof(Vector3f));
} }
template <typename Serializer> template <typename Serializer>
bool SerializeVec4 (Serializer &serializer, const std::string &key, SimpleMath::Vector4f& value) { bool SerializeVec4 (Serializer &serializer, const std::string &key, Vector4f& value) {
return serializer.SerializeData(key, reinterpret_cast<char*>(&value), sizeof(SimpleMath::Vector4f)); return serializer.SerializeData(key, reinterpret_cast<char*>(&value), sizeof(Vector4f));
} }

10
src/SimpleMath/README
View File

@ -1,10 +0,0 @@
This is a highly inefficient math library. It was conceived by Martin
Felis <martin.felis@iwr.uni-heidelberg.de> while he was waiting for code to
compile which used a highly efficient math library.
It is intended to be used as a fast compiling substitute for the
blazingly fast Eigen3 http://eigen.tuxfamily.org/index.php?title=Main_Page
library and tries to mimic its API to a certain extent.
Feel free to use it wherever you like. However, no guarantees are given
that this code does what it says it would.

1721
src/SimpleMath/SimpleMath.h
View File

File diff suppressed because it is too large Load Diff

24
src/SimpleMath/SimpleMath.h.orig
View File

@ -1,24 +0,0 @@
#ifndef _SIMPLEMATH_H
#define _SIMPLEMATH_H
#include "SimpleMathFixed.h"
#include "SimpleMathDynamic.h"
#include "SimpleMathMixed.h"
#include "SimpleMathQR.h"
#include "SimpleMathCommaInitializer.h"
typedef SimpleMath::Fixed::Matrix<int, 3, 1> Vector3i;
typedef SimpleMath::Fixed::Matrix<double, 3, 1> Vector3d;
typedef SimpleMath::Fixed::Matrix<double, 3, 3> Matrix33d;
typedef SimpleMath::Fixed::Matrix<double, 4, 1> Vector4d;
typedef SimpleMath::Fixed::Matrix<float, 3, 1> Vector3f;
typedef SimpleMath::Fixed::Matrix<float, 4, 1> Vector4f;
typedef SimpleMath::Fixed::Matrix<float, 3, 3> Matrix33f;
typedef SimpleMath::Fixed::Matrix<float, 4, 4> Matrix44f;
typedef SimpleMath::Dynamic::Matrix<double> VectorNd;
#endif /* _SIMPLEMATH_H */

194
src/SimpleMath/SimpleMathBlock.h
View File

@ -1,194 +0,0 @@
/**
* This is a highly inefficient math library. It was conceived by Martin
* Felis <martin.felis@iwr.uni-heidelberg.de> while he was compiling code
* that uses a highly efficient math library.
*
* It is intended to be used as a fast compiling substitute for the
* blazingly fast Eigen3 library and tries to mimic its API to a certain
* extend.
*
* Feel free to use it wherever you like. However, no guarantees are given
* that this code does what it says it would.
*/
#ifndef SIMPLEMATHBLOCK_H
#define SIMPLEMATHBLOCK_H
#include <cstdlib>
#include <cmath>
#include <iostream>
#include <assert.h>
#include "compileassert.h"
// #include "SimpleMathQR.h"
/** \brief Namespace for a highly inefficient math library
*
*/
namespace SimpleMath {
/** \brief Namespace for fixed size elements
*/
// forward declaration
template <typename val_type, unsigned int nrows, unsigned int ncols>
class Matrix;
template <typename matrix_type, typename val_type>
class Block {
public:
typedef val_type value_type;
Block() :
mParentRows(0),
mParentCols(0),
mParentRowStart(0),
mParentColStart(0)
{ }
Block (const matrix_type &matrix, const unsigned int row_start, const unsigned int col_start, const unsigned int row_count, const unsigned int col_count) :
mParentRows (matrix.rows()),
mParentCols (matrix.cols()),
mParentRowStart (row_start),
mParentColStart (col_start),
mRowCount (row_count),
mColCount (col_count),
mTransposed (false) {
assert (mParentRows >= mParentRowStart + mRowCount);
assert (mParentCols >= mParentColStart + mColCount);
// without the following line we could not create blocks from const
// matrices
mParentMatrix = const_cast<matrix_type*>(&matrix);
}
// copy data from the other block into this
Block& operator=(const Block &other) {
if (this != &other) {
if (mRowCount != other.rows() || mColCount != other.cols()) {
std::cerr << "Error: cannot assign blocks of different size (left is " << mRowCount << "x" << mColCount << " right is " << other.rows() << "x" << other.cols() << ")!" << std::endl;
abort();
}
value_type* temp_values = new value_type [mRowCount * mColCount];
for (unsigned int i = 0; i < mRowCount; i++) {
for (unsigned int j = 0; j < mColCount; j++) {
temp_values[i * mColCount + j] = static_cast<value_type>(other(i,j));
}
}
for (unsigned int i = 0; i < mRowCount; i++) {
for (unsigned int j = 0; j < mColCount; j++) {
(*this)(i,j) = temp_values[i * mColCount + j];
}
}
delete[] temp_values;
}
return *this;
}
template <typename other_matrix_type>
// copy data from the other block into this
Block& operator=(const other_matrix_type &other) {
if (mRowCount != other.rows() || mColCount != other.cols()) {
std::cerr << "Error: cannot assign blocks of different size (left is " << mRowCount << "x" << mColCount << " right is " << other.rows() << "x" << other.cols() << ")!" << std::endl;
abort();
}
value_type *temp_values = new value_type[mRowCount * mColCount];
for (unsigned int i = 0; i < mRowCount; i++) {
for (unsigned int j = 0; j < mColCount; j++) {
temp_values[i * mColCount + j] = static_cast<value_type>(other(i,j));
}
}
for (unsigned int i = 0; i < mRowCount; i++) {
for (unsigned int j = 0; j < mColCount; j++) {
(*this)(i,j) = temp_values[i * mColCount + j];
}
}
delete[] temp_values;
return *this;
}
unsigned int rows() const {
if (!mTransposed)
return mRowCount;
return mColCount;
}
unsigned int cols() const {
if (!mTransposed)
return mColCount;
return mRowCount;
}
const val_type& operator() (const unsigned int i, const unsigned int j) const {
if (!mTransposed) {
assert (i < mRowCount);
assert (j < mColCount);
return (*mParentMatrix) (i + mParentRowStart, j + mParentColStart);
}
return (*mParentMatrix) (j + mParentRowStart, i + mParentColStart);
}
val_type& operator() (const unsigned int i, const unsigned int j) {
if (!mTransposed) {
assert (i < mRowCount);
assert (j < mColCount);
return (*mParentMatrix) (i + mParentRowStart, j + mParentColStart);
}
assert (j < mRowCount);
assert (i < mColCount);
return (*mParentMatrix) (j + mParentRowStart, i + mParentColStart);
}
Block transpose() const {
Block result (*this);
result.mTransposed = mTransposed ^ true;
return result;
}
private:
matrix_type *mParentMatrix;
const unsigned int mParentRows;
const unsigned int mParentCols;
const unsigned int mParentRowStart;
const unsigned int mParentColStart;
const unsigned int mRowCount;
const unsigned int mColCount;
bool mTransposed;
};
template <typename matrix_type, typename val_type>
inline std::ostream& operator<<(std::ostream& output, const Block<matrix_type, val_type> &block) {
unsigned int i,j;
for (i = 0; i < block.rows(); i++) {
output << "[ ";
for (j = 0; j < block.cols(); j++) {
output << block(i,j);
if (j < block.cols() - 1)
output << ", ";
}
output << " ]";
if (block.rows() > 1 && i < block.rows() - 1)
output << std::endl;
}
return output;
}
}
#endif /* SIMPLEMATHBLOCK_H */

220
src/SimpleMath/SimpleMathBlock.h.orig
View File

@ -1,220 +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;
}
template <typename other_matrix_type>
other_matrix_type operator+(const other_matrix_type& other) {
other_matrix_type result (this);
for (unsigned int i = 0; i < mRowCount; i++) {
for (unsigned int j = 0; j < mColCount; j++) {
result += other(i,j);
}
}
return result;
}
template <typename other_matrix_type>
other_matrix_type operator-(const other_matrix_type& other) {
other_matrix_type result (this);
for (unsigned int i = 0; i < mRowCount; i++) {
for (unsigned int j = 0; j < mColCount; j++) {
result -= other(i,j);
}
}
return result;
}
unsigned int rows() const {
if (!mTransposed)
return mRowCount;
return mColCount;
}
unsigned int cols() const {
if (!mTransposed)
return mColCount;
return mRowCount;
}
const val_type& operator() (const unsigned int i, const unsigned int j) const {
if (!mTransposed) {
assert (i < mRowCount);
assert (j < mColCount);
return (*mParentMatrix) (i + mParentRowStart, j + mParentColStart);
}
return (*mParentMatrix) (j + mParentRowStart, i + mParentColStart);
}
val_type& operator() (const unsigned int i, const unsigned int j) {
if (!mTransposed) {
assert (i < mRowCount);
assert (j < mColCount);
return (*mParentMatrix) (i + mParentRowStart, j + mParentColStart);
}
assert (j < mRowCount);
assert (i < mColCount);
return (*mParentMatrix) (j + mParentRowStart, i + mParentColStart);
}
Block transpose() const {
Block result (*this);
result.mTransposed = mTransposed ^ true;
return result;
}
private:
matrix_type *mParentMatrix;
const unsigned int mParentRows;
const unsigned int mParentCols;
const unsigned int mParentRowStart;
const unsigned int mParentColStart;
const unsigned int mRowCount;
const unsigned int mColCount;
bool mTransposed;
};
template <typename matrix_type, typename val_type>
inline std::ostream& operator<<(std::ostream& output, const Block<matrix_type, val_type> &block) {
unsigned int i,j;
for (i = 0; i < block.rows(); i++) {
output << "[ ";
for (j = 0; j < block.cols(); j++) {
output << block(i,j);
if (j < block.cols() - 1)
output << ", ";
}
output << " ]";
if (block.rows() > 1 && i < block.rows() - 1)
output << std::endl;
}
return output;
}
}
#endif /* SIMPLEMATHBLOCK_H */

94
src/SimpleMath/SimpleMathCholesky.h
View File

@ -1,94 +0,0 @@
#ifndef _SIMPLE_MATH_CHOLESKY_H
#define _SIMPLE_MATH_CHOLESKY_H
#include <iostream>
#include <limits>
#include "SimpleMathDynamic.h"
namespace SimpleMath {
template <typename matrix_type>
class LLT {
public:
typedef typename matrix_type::value_type value_type;
private:
LLT () {}
typedef Dynamic::Matrix<value_type> MatrixXXd;
typedef Dynamic::Matrix<value_type> VectorXd;
bool mIsFactorized;
MatrixXXd mL;
public:
LLT (const matrix_type &matrix) :
mIsFactorized(false),
mL(matrix) {
compute();
}
LLT compute() {
for (int i = 0; i < mL.rows(); i++) {
for (int j = 0; j < mL.rows(); j++) {
if (j > i) {
mL(i,j) = 0.;
continue;
}
double s = mL(i,j);
for (int k = 0; k < j; k++) {
s = s - mL(i,k) * mL(j,k);
}
if (i > j) {
mL(i,j) = s / mL(j,j);
} else if (s > 0.) {
mL (i,i) = sqrt (s);
} else {
std::cerr << "Error computing Cholesky decomposition: matrix not symmetric positive definite!" << std::endl;
assert (false);
}
}
}
mIsFactorized = true;
return *this;
}
Dynamic::Matrix<value_type> solve (
const Dynamic::Matrix<value_type> &rhs
) const {
assert (mIsFactorized);
VectorXd y (mL.rows());
for (unsigned int i = 0; i < mL.rows(); i++) {
double temp = rhs[i];
for (unsigned int j = 0; j < i; j++) {
temp = temp - mL(i,j) * y[j];
}
y[i] = temp / mL(i,i);
}
VectorXd x (mL.rows());
for (int i = mL.rows() - 1; i >= 0; i--) {
double temp = y[i];
for (unsigned int j = i + 1; j < mL.rows(); j++) {
temp = temp - mL(j, i) * x[j];
}
x[i] = temp / mL(i,i);
}
return x;
}
Dynamic::Matrix<value_type> matrixL() const {
return mL;
}
};
}
/* _SIMPLE_MATH_CHOLESKY_H */
#endif

69
src/SimpleMath/SimpleMathCommaInitializer.h
View File

@ -1,69 +0,0 @@
#ifndef _SIMPLE_MATH_COMMA_INITIALIZER_H
#define _SIMPLE_MATH_COMMA_INITIALIZER_H
#include <iostream>
#include <limits>
#include "SimpleMathFixed.h"
#include "SimpleMathDynamic.h"
namespace SimpleMath {
template <typename matrix_type>
class CommaInitializer {
public:
typedef typename matrix_type::value_type value_type;
CommaInitializer(matrix_type &matrix, const value_type &value) :
mElementWasAdded (false) {
assert (matrix.cols() > 0 && matrix.rows() > 0);
mParentMatrix = &matrix;
mRowIndex = 0;
mColIndex = 0;
(*mParentMatrix)(mRowIndex, mColIndex) = value;
}
CommaInitializer(matrix_type &matrix, unsigned int row_index, unsigned int col_index) :
mRowIndex (row_index),
mColIndex (col_index),
mElementWasAdded (false) {
assert (matrix.cols() > 0 && matrix.rows() > 0);
mParentMatrix = &matrix;
mRowIndex = row_index;
mColIndex = col_index;
}
~CommaInitializer() {
if (!mElementWasAdded
&& (mColIndex + 1 < mParentMatrix->cols() || mRowIndex + 1 < mParentMatrix->rows())) {
std::cerr << "Error: too few elements passed to CommaInitializer! Expected " << mParentMatrix->size() << " but was given " << mRowIndex * mParentMatrix->cols() + mColIndex + 1 << std::endl;
abort();
}
}
CommaInitializer<matrix_type> operator, (const value_type &value) {
mColIndex++;
if (mColIndex >= mParentMatrix->cols()) {
mRowIndex++;
mColIndex = 0;
}
if (mRowIndex == mParentMatrix->rows() && mColIndex == 0 ) {
std::cerr << "Error: too many elements passed to CommaInitializer! Expected " << mParentMatrix->size() << " but was given " << mRowIndex * mParentMatrix->cols() + mColIndex + 1 << std::endl;
abort();
}
(*mParentMatrix)(mRowIndex, mColIndex) = value;
mElementWasAdded = true;
return CommaInitializer (*mParentMatrix, mRowIndex, mColIndex);
}
private:
CommaInitializer() {}
matrix_type *mParentMatrix;
unsigned int mRowIndex;
unsigned int mColIndex;
bool mElementWasAdded;
};
}
/* _SIMPLE_MATH_COMMA_INITIALIZER_H */
#endif

667
src/SimpleMath/SimpleMathDynamic.h
View File

@ -1,667 +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 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 */

619
src/SimpleMath/SimpleMathDynamic.h.orig
View File

@ -1,619 +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>
explicit 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;
}
static matrix_type Zero() {
matrix_type result;
result.setZero();
return result;
}
static matrix_type Zero(int rows, int cols = 1) {
matrix_type result (rows, cols);
result.setZero();
return result;
}
static matrix_type Constant (int rows, val_type value) {
matrix_type result (rows, 1);
unsigned int i;
for (i = 0; i < result.size(); i++)
result[i] = value;
return result;
}
static matrix_type Constant (int rows, int cols, val_type value) {
matrix_type result (rows, cols);
unsigned int i;
for (i = 0; i < result.size(); i++)
result[i] = value;
return result;
}
static matrix_type Identity (int rows, int cols = 1) {
assert (rows == cols);
matrix_type result (rows, cols);
result.identity();
return result;
}
void identity() {
assert (nrows == ncols);
setZero();
for (unsigned int i = 0; i < ncols; i++)
mData[i * ncols + i] = 1.;
}
void random() {
for (unsigned int i = 0; i < nrows * ncols; i++)
mData[i] = static_cast<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);
}
// 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 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 */

885
src/SimpleMath/SimpleMathFixed.h
View File

@ -1,885 +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
) {
assert (nrows == 2);
assert (ncols == 1);
mData[0] = v00;
mData[1] = v01;
}
void set(
const val_type &v00, const val_type &v01
) {
COMPILE_ASSERT (nrows * ncols == 2);
mData[0] = v00;
mData[1] = v01;
}
Matrix (
const val_type &v00, const val_type &v01, const val_type &v02
) {
assert (nrows == 3);
assert (ncols == 1);
mData[0] = v00;
mData[1] = v01;
mData[2] = v02;
}
void set(
const val_type &v00, const val_type &v01, const val_type &v02
) {
COMPILE_ASSERT (nrows * ncols == 3);
mData[0] = v00;
mData[1] = v01;
mData[2] = v02;
}
Matrix (
const val_type &v00, const val_type &v01, const val_type &v02,
const val_type &v10, const val_type &v11, const val_type &v12,
const val_type &v20, const val_type &v21, const val_type &v22
) {
COMPILE_ASSERT (nrows == 3);
COMPILE_ASSERT (ncols == 3);
mData[0] = v00;
mData[1] = v01;
mData[2] = v02;
mData[1 * 3 + 0] = v10;
mData[1 * 3 + 1] = v11;
mData[1 * 3 + 2] = v12;
mData[2 * 3 + 0] = v20;
mData[2 * 3 + 1] = v21;
mData[2 * 3 + 2] = v22;
}
void set(
const val_type v00, const val_type v01, const val_type v02,
const val_type v10, const val_type v11, const val_type v12,
const val_type v20, const val_type v21, const val_type v22
) {
COMPILE_ASSERT (nrows == 3);
COMPILE_ASSERT (ncols == 3);
mData[0] = v00;
mData[1] = v01;
mData[2] = v02;
mData[1 * 3 + 0] = v10;
mData[1 * 3 + 1] = v11;
mData[1 * 3 + 2] = v12;
mData[2 * 3 + 0] = v20;
mData[2 * 3 + 1] = v21;
mData[2 * 3 + 2] = v22;
}
Matrix (
const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03
) {
assert (nrows == 4);
assert (ncols == 1);
mData[0] = v00;
mData[1] = v01;
mData[2] = v02;
mData[3] = v03;
}
void set(
const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03
) {
COMPILE_ASSERT (nrows * ncols == 4);
mData[0] = v00;
mData[1] = v01;
mData[2] = v02;
mData[3] = v03;
}
Matrix (
const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03,
const val_type &v10, const val_type &v11, const val_type &v12, const val_type &v13,
const val_type &v20, const val_type &v21, const val_type &v22, const val_type &v23,
const val_type &v30, const val_type &v31, const val_type &v32, const val_type &v33
) {
COMPILE_ASSERT (nrows == 4);
COMPILE_ASSERT (ncols == 4);
mData[0] = v00;
mData[1] = v01;
mData[2] = v02;
mData[3] = v03;
mData[1 * 4 + 0] = v10;
mData[1 * 4 + 1] = v11;
mData[1 * 4 + 2] = v12;
mData[1 * 4 + 3] = v13;
mData[2 * 4 + 0] = v20;
mData[2 * 4 + 1] = v21;
mData[2 * 4 + 2] = v22;
mData[2 * 4 + 3] = v23;
mData[3 * 4 + 0] = v30;
mData[3 * 4 + 1] = v31;
mData[3 * 4 + 2] = v32;
mData[3 * 4 + 3] = v33;
}
void set(
const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03,
const val_type &v10, const val_type &v11, const val_type &v12, const val_type &v13,
const val_type &v20, const val_type &v21, const val_type &v22, const val_type &v23,
const val_type &v30, const val_type &v31, const val_type &v32, const val_type &v33
) {
COMPILE_ASSERT (nrows == 4);
COMPILE_ASSERT (ncols == 4);
mData[0] = v00;
mData[1] = v01;
mData[2] = v02;
mData[3] = v03;
mData[1 * 4 + 0] = v10;
mData[1 * 4 + 1] = v11;
mData[1 * 4 + 2] = v12;
mData[1 * 4 + 3] = v13;
mData[2 * 4 + 0] = v20;
mData[2 * 4 + 1] = v21;
mData[2 * 4 + 2] = v22;
mData[2 * 4 + 3] = v23;
mData[3 * 4 + 0] = v30;
mData[3 * 4 + 1] = v31;
mData[3 * 4 + 2] = v32;
mData[3 * 4 + 3] = v33;
}
Matrix (
const val_type &v00, const val_type &v01, const val_type &v02,
const val_type &v03, const val_type &v04, const val_type &v05
) {
COMPILE_ASSERT (nrows == 6);
COMPILE_ASSERT (ncols == 1);
mData[0] = v00;
mData[1] = v01;
mData[2] = v02;
mData[3] = v03;
mData[4] = v04;
mData[5] = v05;
}
void set(
const val_type &v00, const val_type &v01, const val_type &v02,
const val_type &v03, const val_type &v04, const val_type &v05
) {
COMPILE_ASSERT (nrows * ncols == 6);
mData[0] = v00;
mData[1] = v01;
mData[2] = v02;
mData[3] = v03;
mData[4] = v04;
mData[5] = v05;
}
Matrix (
const val_type &v00, const val_type &v01, const val_type &v02,
const val_type &v03, const val_type &v04, const val_type &v05,
const val_type &v10, const val_type &v11, const val_type &v12,
const val_type &v13, const val_type &v14, const val_type &v15,
const val_type &v20, const val_type &v21, const val_type &v22,
const val_type &v23, const val_type &v24, const val_type &v25,
const val_type &v30, const val_type &v31, const val_type &v32,
const val_type &v33, const val_type &v34, const val_type &v35,
const val_type &v40, const val_type &v41, const val_type &v42,
const val_type &v43, const val_type &v44, const val_type &v45,
const val_type &v50, const val_type &v51, const val_type &v52,
const val_type &v53, const val_type &v54, const val_type &v55
) {
COMPILE_ASSERT (nrows == 6);
COMPILE_ASSERT (ncols == 6);
mData[0] = v00;
mData[1] = v01;
mData[2] = v02;
mData[3] = v03;
mData[4] = v04;
mData[5] = v05;
mData[6 + 0] = v10;
mData[6 + 1] = v11;
mData[6 + 2] = v12;
mData[6 + 3] = v13;
mData[6 + 4] = v14;
mData[6 + 5] = v15;
mData[12 + 0] = v20;
mData[12 + 1] = v21;
mData[12 + 2] = v22;
mData[12 + 3] = v23;
mData[12 + 4] = v24;
mData[12 + 5] = v25;
mData[18 + 0] = v30;
mData[18 + 1] = v31;
mData[18 + 2] = v32;
mData[18 + 3] = v33;
mData[18 + 4] = v34;
mData[18 + 5] = v35;
mData[24 + 0] = v40;
mData[24 + 1] = v41;
mData[24 + 2] = v42;
mData[24 + 3] = v43;
mData[24 + 4] = v44;
mData[24 + 5] = v45;
mData[30 + 0] = v50;
mData[30 + 1] = v51;
mData[30 + 2] = v52;
mData[30 + 3] = v53;
mData[30 + 4] = v54;
mData[30 + 5] = v55;
};
void set(
const val_type v00, const val_type v01, const val_type v02,
const val_type v03, const val_type v04, const val_type v05,
const val_type v10, const val_type v11, const val_type v12,
const val_type v13, const val_type v14, const val_type v15,
const val_type v20, const val_type v21, const val_type v22,
const val_type v23, const val_type v24, const val_type v25,
const val_type v30, const val_type v31, const val_type v32,
const val_type v33, const val_type v34, const val_type v35,
const val_type v40, const val_type v41, const val_type v42,
const val_type v43, const val_type v44, const val_type v45,
const val_type v50, const val_type v51, const val_type v52,
const val_type v53, const val_type v54, const val_type v55
) {
COMPILE_ASSERT (nrows == 6);
COMPILE_ASSERT (ncols == 6);
mData[0] = v00;
mData[1] = v01;
mData[2] = v02;
mData[3] = v03;
mData[4] = v04;
mData[5] = v05;
mData[6 + 0] = v10;
mData[6 + 1] = v11;
mData[6 + 2] = v12;
mData[6 + 3] = v13;
mData[6 + 4] = v14;
mData[6 + 5] = v15;
mData[12 + 0] = v20;
mData[12 + 1] = v21;
mData[12 + 2] = v22;
mData[12 + 3] = v23;
mData[12 + 4] = v24;
mData[12 + 5] = v25;
mData[18 + 0] = v30;
mData[18 + 1] = v31;
mData[18 + 2] = v32;
mData[18 + 3] = v33;
mData[18 + 4] = v34;
mData[18 + 5] = v35;
mData[24 + 0] = v40;
mData[24 + 1] = v41;
mData[24 + 2] = v42;
mData[24 + 3] = v43;
mData[24 + 4] = v44;
mData[24 + 5] = v45;
mData[30 + 0] = v50;
mData[30 + 1] = v51;
mData[30 + 2] = v52;
mData[30 + 3] = v53;
mData[30 + 4] = v54;
mData[30 + 5] = v55;
}
// comparison
bool operator==(const Matrix &matrix) const {
for (unsigned int i = 0; i < nrows * ncols; i++) {
if (mData[i] != matrix.mData[i])
return false;
}
return true;
}
bool operator!=(const Matrix &matrix) const {
for (unsigned int i = 0; i < nrows * ncols; i++) {
if (mData[i] != matrix.mData[i])
return true;
}
return false;
}
// access operators
const val_type& operator[](const unsigned int &index) const {
assert (index >= 0 && index < nrows * ncols);
return mData[index];
};
val_type& operator[](const unsigned int &index) {
assert (index >= 0 && index < nrows * ncols);
return mData[index];
}
const val_type& operator()(const unsigned int &row, const unsigned int &col) const {
assert (row >= 0 && row < nrows && col >= 0 && col < ncols);
return mData[row*ncols + col];
};
val_type& operator()(const unsigned int &row, const unsigned int &col) {
assert (row >= 0 && row < nrows && col >= 0 && col < ncols);
return mData[row*ncols + col];
};
void zero() {
for (unsigned int i = 0; i < ncols * nrows; i++)
mData[i] = 0.;
}
void setZero() {
zero();
}
val_type norm() const {
return sqrt(this->squaredNorm());
}
matrix_type normalize() {
val_type length = this->norm();
for (unsigned int i = 0; i < ncols * nrows; i++)
mData[i] /= length;
return *this;
}
matrix_type normalized() const {
return matrix_type (*this) / this->norm();
}
Matrix<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;
}
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;
}
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 */

825
src/SimpleMath/SimpleMathFixed.h.orig
View File

@ -1,825 +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 "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 PartialPivLU;
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& 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;
}
static matrix_type Zero() {
matrix_type result;
result.setZero();
return result;
}
static matrix_type Zero(int ignore_me) {
matrix_type result;
result.setZero();
return result;
}
static matrix_type Zero(int ignore_me, int ignore_me_too) {
matrix_type result;
result.setZero();
return result;
}
static matrix_type Identity() {
matrix_type result;
result.identity();
return result;
}
static matrix_type Identity(int ignore_me, int ignore_me_too) {
matrix_type result;
result.identity();
return result;
}
void identity() {
COMPILE_ASSERT (nrows == ncols);
setZero();
for (unsigned int i = 0; i < ncols; i++)
mData[i * ncols + i] = 1.;
}
void random() {
for (unsigned int i = 0; i < nrows * ncols; i++)
mData[i] = static_cast<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);
}
// 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;
}
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 PartialPivLU<matrix_type> partialPivLU() const {
return PartialPivLU<matrix_type>(*this);
}
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 */

346
src/SimpleMath/SimpleMathGL.h
View File

@ -1,346 +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

107
src/SimpleMath/SimpleMathLU.h
View File

@ -1,107 +0,0 @@
#ifndef _SIMPLE_MATH_LU_H
#define _SIMPLE_MATH_LU_H
#include <iostream>
#include <limits>
#include "SimpleMathFixed.h"
#include "SimpleMathDynamic.h"
#include "SimpleMathBlock.h"
namespace SimpleMath {
template <typename matrix_type>
class PartialPivLU {
public:
typedef typename matrix_type::value_type value_type;
private:
PartialPivLU() {}
typedef Dynamic::Matrix<value_type> MatrixXXd;
typedef Dynamic::Matrix<value_type> VectorXd;
bool mIsFactorized;
unsigned int *mPermutations;
MatrixXXd matrixLU;
public:
PartialPivLU(const matrix_type &matrix) :
mIsFactorized(false),
matrixLU(matrix) {
// We can only solve quadratic systems
assert (matrixLU.rows() == matrixLU.cols());
mPermutations = new unsigned int [matrix.cols()];
for (unsigned int i = 0; i < matrix.cols(); i++) {
mPermutations[i] = i;
}
compute();
}
PartialPivLU compute() {
unsigned int n = matrixLU.rows();
unsigned int pi;
unsigned int i,j,k;
for (j = 0; j < n; j++) {
double pv = fabs (matrixLU(j,mPermutations[j]));
// LOG << "j = " << j << " pv = " << pv << std::endl;
// find the pivot
for (k = j; k < n; k++) {
double pt = fabs (matrixLU(j,mPermutations[k]));
if (pt > pv) {
pv = pt;
pi = k;
unsigned int p_swap = mPermutations[j];
mPermutations[j] = mPermutations[pi];
mPermutations[pi] = p_swap;
// LOG << "swap " << j << " with " << pi << std::endl;
// LOG << "j = " << j << " pv = " << pv << std::endl;
}
}
for (i = j + 1; i < n; i++) {
if (fabs(A(j,pivot[j])) <= std::numeric_limits<double>::epsilon()) {
std::cerr << "Error: pivoting failed for matrix A = " << std::endl;
std::cerr << "A = " << std::endl << A << std::endl;
std::cerr << "b = " << b << std::endl;
}
// assert (fabs(A(j,pivot[j])) > std::numeric_limits<double>::epsilon());
double d = A(i,pivot[j])/A(j,pivot[j]);
}
for (k = j; k < n; k++) {
A(i,pivot[k]) -= A(j,pivot[k]) * d;
}
}
mIsFactorized = true;
return *this;
}
Dynamic::Matrix<value_type> solve (
const Dynamic::Matrix<value_type> &rhs
) const {
assert (mIsFactorized);
// temporary result vector which contains the pivoted result
VectorXd px = rhs;
for (int i = mR.cols() - 1; i >= 0; --i) {
for (j = i + 1; j < n; j++) {
px[i] += A(i, pivot[j]) * px[j];
}
px[i] = (b[i] - px[i]) / A (i, pivot[i]);
}
return x;
}
};
}
/* _SIMPLE_MATH_LU_H */
#endif

22
src/SimpleMath/SimpleMathMap.h
View File

@ -1,22 +0,0 @@
#ifndef SIMPLEMATHMAP_H
#define SIMPLEMATHMAP_H
#include "compileassert.h"
namespace SimpleMath {
/** \brief \brief Wraps a varying size matrix type around existing data
*
* \warning If you create a matrix using the map function and then assign
* a bigger matrix you invalidate the original memory!
*
*/
template < typename MatrixType >
MatrixType Map (typename MatrixType::value_type *data, unsigned int rows, unsigned int cols) {
return MatrixType (rows, cols, data);
}
}
// SIMPLEMATHMAP_H
#endif

138
src/SimpleMath/SimpleMathMixed.h
View File

@ -1,138 +0,0 @@
/**
* This is a highly inefficient math library. It was conceived by Martin
* Felis <martin.felis@iwr.uni-heidelberg.de> while he was compiling code
* that uses a highly efficient math library.
*
* It is intended to be used as a fast compiling substitute for the
* blazingly fast Eigen3 library and tries to mimic its API to a certain
* extend.
*
* Feel free to use it wherever you like. However, no guarantees are given
* that this code does what it says it would.
*/
#ifndef SIMPLEMATHMIXED_H
#define SIMPLEMATHMIXED_H
#include <sstream>
#include <cstdlib>
#include <assert.h>
#include <iostream>
#include "compileassert.h"
/** \brief Namespace for a highly inefficient math library
*
*/
namespace SimpleMath {
// conversion Dynamic->Fixed
template <typename val_type, unsigned int nrows, unsigned int ncols>
inline Fixed::Matrix<val_type, nrows, ncols>::Matrix(const Dynamic::Matrix<val_type> &dynamic_matrix) {
if (dynamic_matrix.cols() != ncols
|| dynamic_matrix.rows() != nrows) {
std::cerr << "Error: cannot assign a dynamic sized matrix of size " << dynamic_matrix.rows() << "x" << dynamic_matrix.cols() << " to a fixed size matrix of size " << nrows << "x" << ncols << "!" << std::endl;
abort();
}
for (unsigned int i = 0; i < nrows * ncols; i++) {
mData[i] = dynamic_matrix[i];
}
}
template <typename val_type, unsigned int nrows, unsigned int ncols>
inline Fixed::Matrix<val_type, nrows, ncols>& Fixed::Matrix<val_type, nrows, ncols>::operator=(const Dynamic::Matrix<val_type> &dynamic_matrix) {
if (dynamic_matrix.cols() != ncols
|| dynamic_matrix.rows() != nrows) {
std::cerr << "Error: cannot assign a dynamic sized matrix of size " << dynamic_matrix.rows() << "x" << dynamic_matrix.cols() << " to a fixed size matrix of size " << nrows << "x" << ncols << "!" << std::endl;
abort();
}
for (unsigned int i = 0; i < nrows * ncols; i++) {
mData[i] = dynamic_matrix[i];
}
return *this;
}
// multiplication
template <typename val_type, unsigned int nrows, unsigned int ncols>
inline Dynamic::Matrix<val_type> operator*(
const Fixed::Matrix<val_type, nrows, ncols> &matrix_a,
const Dynamic::Matrix<val_type> &matrix_b) {
assert (matrix_a.cols() == matrix_b.rows());
Dynamic::Matrix<val_type> result (nrows, matrix_b.cols());
result.setZero();
unsigned int i,j, k;
for (i = 0; i < nrows; i++) {
for (j = 0; j < matrix_b.cols(); j++) {
for (k = 0; k < matrix_b.rows(); k++) {
result(i,j) += matrix_a(i,k) * matrix_b(k,j);
}
}
}
return result;
}
template <typename val_type, unsigned int nrows, unsigned int ncols>
inline Dynamic::Matrix<val_type> operator*(
const Dynamic::Matrix<val_type> &matrix_a,
const Fixed::Matrix<val_type, nrows, ncols> &matrix_b) {
assert (matrix_a.cols() == matrix_b.rows());
Dynamic::Matrix<val_type> result (matrix_a.rows(), ncols);
result.setZero();
unsigned int i,j, k;
for (i = 0; i < matrix_a.rows(); i++) {
for (j = 0; j < matrix_b.cols(); j++) {
for (k = 0; k < matrix_b.rows(); k++) {
result(i,j) += matrix_a(i,k) * matrix_b(k,j);
}
}
}
return result;
}
// equality
template <typename val_type, unsigned int nrows, unsigned int ncols>
inline bool operator==(
const Fixed::Matrix<val_type, nrows, ncols> &matrix_a,
const Dynamic::Matrix<val_type> &matrix_b) {
assert (nrows == matrix_a.rows());
assert (ncols == matrix_a.cols());
unsigned int i;
for (i = 0; i < matrix_a.size(); i++) {
if (matrix_a[i] != matrix_b[i])
return false;
}
return true;
}
template <typename val_type, unsigned int nrows, unsigned int ncols>
inline bool operator==(
const Dynamic::Matrix<val_type> &matrix_b,
const Fixed::Matrix<val_type, nrows, ncols> &matrix_a) {
assert (nrows == matrix_a.rows());
assert (ncols == matrix_a.cols());
unsigned int i;
for (i = 0; i < matrix_a.size(); i++) {
if (matrix_a[i] != matrix_b[i])
return false;
}
return true;
}
}
#endif /* SIMPLEMATHMIXED_H */

324
src/SimpleMath/SimpleMathQR.h
View File

@ -1,324 +0,0 @@
#ifndef _SIMPLE_MATH_QR_H
#define _SIMPLE_MATH_QR_H
#include <iostream>
#include <limits>
#include "SimpleMathFixed.h"
#include "SimpleMathDynamic.h"
#include "SimpleMathBlock.h"
namespace SimpleMath {
template <typename matrix_type>
class HouseholderQR {
public:
typedef typename matrix_type::value_type value_type;
HouseholderQR() :
mIsFactorized(false)
{}
private:
typedef Dynamic::Matrix<value_type> MatrixXXd;
typedef Dynamic::Matrix<value_type> VectorXd;
bool mIsFactorized;
MatrixXXd mQ;
MatrixXXd mR;
public:
HouseholderQR(const matrix_type &matrix) :
mIsFactorized(false),
mQ(matrix.rows(), matrix.rows())
{
compute(matrix);
}
HouseholderQR compute(const matrix_type &matrix) {
mR = matrix;
mQ = Dynamic::Matrix<value_type>::Identity (mR.rows(), mR.rows());
for (unsigned int i = 0; i < mR.cols(); i++) {
unsigned int block_rows = mR.rows() - i;
unsigned int block_cols = mR.cols() - i;
MatrixXXd current_block = mR.block(i,i, block_rows, block_cols);
VectorXd column = current_block.block(0, 0, block_rows, 1);
value_type alpha = - column.norm();
if (current_block(0,0) < 0) {
alpha = - alpha;
}
VectorXd v = current_block.block(0, 0, block_rows, 1);
v[0] = v[0] - alpha;
MatrixXXd Q (MatrixXXd::Identity(mR.rows(), mR.rows()));
Q.block(i, i, block_rows, block_rows) = MatrixXXd (Q.block(i, i, block_rows, block_rows))
- MatrixXXd(v * v.transpose() / (v.squaredNorm() * 0.5));
mR = Q * mR;
// Normalize so that we have positive diagonal elements
if (mR(i,i) < 0) {
mR.block(i,i,block_rows, block_cols) = MatrixXXd(mR.block(i,i,block_rows, block_cols)) * -1.;
Q.block(i,i,block_rows, block_rows) = MatrixXXd(Q.block(i,i,block_rows, block_rows)) * -1.;
}
mQ = mQ * Q;
}
mIsFactorized = true;
return *this;
}
Dynamic::Matrix<value_type> solve (
const Dynamic::Matrix<value_type> &rhs
) const {
assert (mIsFactorized);
VectorXd y = mQ.transpose() * rhs;
VectorXd x = VectorXd::Zero(mR.cols());
for (int i = mR.cols() - 1; i >= 0; --i) {
value_type z = y[i];
for (unsigned int j = i + 1; j < mR.cols(); j++) {
z = z - x[j] * mR(i,j);
}
if (fabs(mR(i,i)) < std::numeric_limits<value_type>::epsilon() * 10) {
std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero!" << std::endl;
abort();
}
x[i] = z / mR(i,i);
}
return x;
}
Dynamic::Matrix<value_type> inverse() const {
assert (mIsFactorized);
VectorXd rhs_temp = VectorXd::Zero(mQ.cols());
MatrixXXd result (mQ.cols(), mQ.cols());
for (unsigned int i = 0; i < mQ.cols(); i++) {
rhs_temp[i] = 1.;
result.block(0, i, mQ.cols(), 1) = solve(rhs_temp);
rhs_temp[i] = 0.;
}
return result;
}
Dynamic::Matrix<value_type> householderQ () const {
return mQ;
}
Dynamic::Matrix<value_type> matrixR () const {
return mR;
}
};
template <typename matrix_type>
class ColPivHouseholderQR {
public:
typedef typename matrix_type::value_type value_type;
private:
typedef Dynamic::Matrix<value_type> MatrixXXd;
typedef Dynamic::Matrix<value_type> VectorXd;
bool mIsFactorized;
MatrixXXd mQ;
MatrixXXd mR;
unsigned int *mPermutations;
value_type mThreshold;
unsigned int mRank;
public:
ColPivHouseholderQR():
mIsFactorized(false) {
mPermutations = new unsigned int[1];
}
ColPivHouseholderQR (const ColPivHouseholderQR& other) {
mIsFactorized = other.mIsFactorized;
mQ = other.mQ;
mR = other.mR;
mPermutations = new unsigned int[mQ.cols()];
mThreshold = other.mThreshold;
mRank = other.mRank;
}
ColPivHouseholderQR& operator= (const ColPivHouseholderQR& other) {
if (this != &other) {
mIsFactorized = other.mIsFactorized;
mQ = other.mQ;
mR = other.mR;
delete[] mPermutations;
mPermutations = new unsigned int[mQ.cols()];
mThreshold = other.mThreshold;
mRank = other.mRank;
}
return *this;
}
ColPivHouseholderQR(const matrix_type &matrix) :
mIsFactorized(false),
mQ(matrix.rows(), matrix.rows()),
mThreshold (std::numeric_limits<value_type>::epsilon() * matrix.cols()) {
mPermutations = new unsigned int [matrix.cols()];
for (unsigned int i = 0; i < matrix.cols(); i++) {
mPermutations[i] = i;
}
compute(matrix);
}
~ColPivHouseholderQR() {
delete[] mPermutations;
}
ColPivHouseholderQR& setThreshold (const value_type& threshold) {
mThreshold = threshold;
return *this;
}
ColPivHouseholderQR& compute(const matrix_type &matrix) {
mR = matrix;
mQ = Dynamic::Matrix<value_type>::Identity (mR.rows(), mR.rows());
for (unsigned int i = 0; i < mR.cols(); i++) {
unsigned int block_rows = mR.rows() - i;
unsigned int block_cols = mR.cols() - i;
// find and swap the column with the highest norm
unsigned int col_index_norm_max = i;
value_type col_norm_max = VectorXd(mR.block(i,i, block_rows, 1)).squaredNorm();
for (unsigned int j = i + 1; j < mR.cols(); j++) {
VectorXd column = mR.block(i, j, block_rows, 1);
if (column.squaredNorm() > col_norm_max) {
col_index_norm_max = j;
col_norm_max = column.squaredNorm();
}
}
if (col_norm_max < mThreshold) {
// if all entries of the column is close to zero, we bail out
break;
}
if (col_index_norm_max != i) {
VectorXd temp_col = mR.block(0, i, mR.rows(), 1);
mR.block(0,i,mR.rows(),1) = mR.block(0, col_index_norm_max, mR.rows(), 1);
mR.block(0, col_index_norm_max, mR.rows(), 1) = temp_col;
unsigned int temp_index = mPermutations[i];
mPermutations[i] = mPermutations[col_index_norm_max];
mPermutations[col_index_norm_max] = temp_index;
}
MatrixXXd current_block = mR.block(i,i, block_rows, block_cols);
VectorXd column = current_block.block(0, 0, block_rows, 1);
value_type alpha = - column.norm();
if (current_block(0,0) < 0) {
alpha = - alpha;
}
VectorXd v = current_block.block(0, 0, block_rows, 1);
v[0] = v[0] - alpha;
MatrixXXd Q (MatrixXXd::Identity(mR.rows(), mR.rows()));
Q.block(i, i, block_rows, block_rows) = MatrixXXd (Q.block(i, i, block_rows, block_rows))
- MatrixXXd(v * v.transpose() / (v.squaredNorm() * static_cast<value_type>(0.5)));
mR = Q * mR;
// Normalize so that we have positive diagonal elements
if (mR(i,i) < 0) {
mR.block(i,i,block_rows, block_cols) = MatrixXXd(mR.block(i,i,block_rows, block_cols)) * -1.;
Q.block(i,i,block_rows, block_rows) = MatrixXXd(Q.block(i,i,block_rows, block_rows)) * -1.;
}
mQ = mQ * Q;
}
mIsFactorized = true;
return *this;
}
Dynamic::Matrix<value_type> solve (
const Dynamic::Matrix<value_type> &rhs
) const {
assert (mIsFactorized);
VectorXd y = mQ.transpose() * rhs;
VectorXd x = VectorXd::Zero(mR.cols());
for (int i = mR.cols() - 1; i >= 0; --i) {
value_type z = y[i];
for (unsigned int j = i + 1; j < mR.cols(); j++) {
z = z - x[mPermutations[j]] * mR(i,j);
}
if (fabs(mR(i,i)) < std::numeric_limits<value_type>::epsilon() * 10) {
std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero!" << std::endl;
abort();
}
x[mPermutations[i]] = z / mR(i,i);
}
return x;
}
Dynamic::Matrix<value_type> inverse() const {
assert (mIsFactorized);
VectorXd rhs_temp = VectorXd::Zero(mQ.cols());
MatrixXXd result (mQ.cols(), mQ.cols());
for (unsigned int i = 0; i < mQ.cols(); i++) {
rhs_temp[i] = 1.;
result.block(0, i, mQ.cols(), 1) = solve(rhs_temp);
rhs_temp[i] = 0.;
}
return result;
}
Dynamic::Matrix<value_type> householderQ () const {
return mQ;
}
Dynamic::Matrix<value_type> matrixR () const {
return mR;
}
Dynamic::Matrix<value_type> matrixP () const {
MatrixXXd P = MatrixXXd::Identity(mR.cols(), mR.cols());
MatrixXXd identity = MatrixXXd::Identity(mR.cols(), mR.cols());
for (unsigned int i = 0; i < mR.cols(); i++) {
P.block(0,i,mR.cols(),1) = identity.block(0,mPermutations[i], mR.cols(), 1);
}
return P;
}
unsigned int rank() const {
value_type abs_threshold = fabs(mR(0,0)) * mThreshold;
for (unsigned int i = 1; i < mR.cols(); i++) {
if (fabs(mR(i,i)) < abs_threshold)
return i;
}
return mR.cols();
}
};
}
/* _SIMPLE_MATH_QR_H */
#endif

39
src/SimpleMath/compileassert.h
View File

@ -1,39 +0,0 @@
#ifndef _COMPILE_ASSERT_H
#define _COMPILE_ASSERT_H
/*
* This is a simple compile time assertion tool taken from:
* http://blogs.msdn.com/b/abhinaba/archive/2008/10/27/c-c-compile-time-asserts.aspx
* written by Abhinaba Basu!
*
* Thanks!
*/
#ifdef __cplusplus
#define JOIN( X, Y ) JOIN2(X,Y)
#define JOIN2( X, Y ) X##Y
namespace static_assert_compat
{
template <bool> struct STATIC_ASSERT_FAILURE;
template <> struct STATIC_ASSERT_FAILURE<true> { enum { value = 1 }; };
template<int x> struct static_assert_test{};
}
#define COMPILE_ASSERT(x) \
typedef ::static_assert_compat::static_assert_test<\
sizeof(::static_assert_compat::STATIC_ASSERT_FAILURE< (bool)( x ) >)>\
JOIN(_static_assert_typedef, __LINE__)
#else // __cplusplus
#define COMPILE_ASSERT(x) extern int __dummy[(int)x]
#endif // __cplusplus
#define VERIFY_EXPLICIT_CAST(from, to) COMPILE_ASSERT(sizeof(from) == sizeof(to))
// _COMPILE_ASSERT_H_
#endif

28
src/math_types.h
View File

@ -2,27 +2,25 @@
#define MESHUP_CONFIG_H #define MESHUP_CONFIG_H
#include "SimpleMath/SimpleMath.h" #include "SimpleMath/SimpleMath.h"
#include "SimpleMath/SimpleMathGL.h"
#include "SimpleMath/SimpleMathMap.h"
typedef SimpleMath::Fixed::Matrix<float, 2, 1> Vector2f; typedef SimpleMath::Fixed<float, 2, 1> Vector2f;
typedef SimpleMath::Fixed::Matrix<float, 2, 2> Matrix22f; typedef SimpleMath::Fixed<float, 2, 2> Matrix22f;
typedef SimpleMath::Fixed::Matrix<float, 3, 1> Vector3f; typedef SimpleMath::Fixed<float, 3, 1> Vector3f;
typedef SimpleMath::Fixed::Matrix<float, 3, 3> Matrix33f; typedef SimpleMath::Fixed<float, 3, 3> Matrix33f;
typedef SimpleMath::Fixed::Matrix<float, 4, 1> Vector4f; typedef SimpleMath::Fixed<float, 4, 1> Vector4f;
typedef SimpleMath::Fixed::Matrix<float, 4, 4> Matrix44f; typedef SimpleMath::Fixed<float, 4, 4> Matrix44f;
typedef SimpleMath::GL::Quaternion Quaternion; typedef SimpleMath::Quaternion Quaternion;
typedef SimpleMath::Dynamic::Matrix<float> VectorNf; typedef SimpleMath::Dynamic<float> VectorNf;
typedef SimpleMath::Dynamic::Matrix<float> MatrixNNf; typedef SimpleMath::Dynamic<float> MatrixNNf;
typedef SimpleMath::Fixed::Matrix<double, 3, 1> Vector3d; typedef SimpleMath::Fixed<double, 3, 1> Vector3d;
typedef SimpleMath::Fixed::Matrix<double, 3, 3> Matrix33d; typedef SimpleMath::Fixed<double, 3, 3> Matrix33d;
typedef SimpleMath::Dynamic::Matrix<double> VectorNd; typedef SimpleMath::Dynamic<double> VectorNd;
typedef SimpleMath::Dynamic::Matrix<double> MatrixNNd; typedef SimpleMath::Dynamic<double> MatrixNNd;
#endif #endif

6
src/modules/RenderModule.cc
View File

@ -10,7 +10,7 @@
#include "imgui/imgui.h" #include "imgui/imgui.h"
#include "imgui_dock.h" #include "imgui_dock.h"
using namespace SimpleMath::GL; using namespace SimpleMath;
struct Renderer; struct Renderer;
@ -180,10 +180,10 @@ extern "C" {
const struct module_api MODULE_API = { const struct module_api MODULE_API = {
.init = module_init, .init = module_init,
.finalize = module_finalize,
.reload = module_reload, .reload = module_reload,
.step = module_step,
.unload = module_unload, .unload = module_unload,
.finalize = module_finalize .step = module_step
}; };
} }

1
src/modules/RenderUtils.cc
View File

@ -13,7 +13,6 @@
#include "imgui_dock.h" #include "imgui_dock.h"
using namespace SimpleMath; using namespace SimpleMath;
using namespace SimpleMath::GL;
typedef tinygltf::TinyGLTF GLTFLoader; typedef tinygltf::TinyGLTF GLTFLoader;

8
src/modules/TestModule.cc
View File

@ -46,10 +46,10 @@ void handle_mouse (struct module_state *state) {
view_dir = (poi - eye).normalized(); view_dir = (poi - eye).normalized();
Vector3f right = camera_rot_inv.block<1,3>(0,0).transpose(); Vector3f right = camera_rot_inv.block<1,3>(0,0).transpose();
right = view_dir.cross (Vector3f (0.f, 1.f, 0.f)); right = view_dir.cross (Vector3f (0.f, 1.f, 0.f));
Matrix33f rot_matrix_y = SimpleMath::GL::RotateMat33( Matrix33f rot_matrix_y = SimpleMath::RotateMat33(
gGuiInputState->mousedY * 0.4f, gGuiInputState->mousedY * 0.4f,
right[0], right[1], right[2]); right[0], right[1], right[2]);
Matrix33f rot_matrix_x = SimpleMath::GL::RotateMat33( Matrix33f rot_matrix_x = SimpleMath::RotateMat33(
gGuiInputState->mousedX * 0.4f, gGuiInputState->mousedX * 0.4f,
0.f, 1.f, 0.f); 0.f, 1.f, 0.f);
poi = eye + rot_matrix_x * rot_matrix_y * view_dir; poi = eye + rot_matrix_x * rot_matrix_y * view_dir;
@ -169,9 +169,9 @@ extern "C" {
const struct module_api MODULE_API = { const struct module_api MODULE_API = {
.init = module_init, .init = module_init,
.finalize = module_finalize,
.reload = module_reload, .reload = module_reload,
.step = module_step,
.unload = module_unload, .unload = module_unload,
.finalize = module_finalize .step = module_step
}; };
} }