Working on single header simple math version
Martin Felis
parent
9588ee0674
commit
09db06fbfd
|
|
@ -376,18 +376,6 @@ configure_file(src/glfw3.pc.in src/glfw3.pc @ONLY)
|
|||
#--------------------------------------------------------------------
|
||||
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
|
||||
# The library is installed by src/CMakeLists.txt
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
#pragma once
|
||||
|
||||
#include "SimpleMath/SimpleMath.h"
|
||||
#include "math_types.h"
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <unordered_map>
|
||||
|
|
@ -134,11 +134,11 @@ bool SerializedUint16 (Serializer &serializer, const std::string &key, uint16_t&
|
|||
}
|
||||
|
||||
template <typename Serializer>
|
||||
bool SerializeVec3 (Serializer &serializer, const std::string &key, SimpleMath::Vector3f& value) {
|
||||
return serializer.SerializeData(key, reinterpret_cast<char*>(&value), sizeof(SimpleMath::Vector3f));
|
||||
bool SerializeVec3 (Serializer &serializer, const std::string &key, Vector3f& value) {
|
||||
return serializer.SerializeData(key, reinterpret_cast<char*>(&value), sizeof(Vector3f));
|
||||
}
|
||||
|
||||
template <typename Serializer>
|
||||
bool SerializeVec4 (Serializer &serializer, const std::string &key, SimpleMath::Vector4f& value) {
|
||||
return serializer.SerializeData(key, reinterpret_cast<char*>(&value), sizeof(SimpleMath::Vector4f));
|
||||
bool SerializeVec4 (Serializer &serializer, const std::string &key, Vector4f& value) {
|
||||
return serializer.SerializeData(key, reinterpret_cast<char*>(&value), sizeof(Vector4f));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -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 */
|
||||
|
|
@ -1,194 +0,0 @@
|
|||
/**
|
||||
* This is a highly inefficient math library. It was conceived by Martin
|
||||
* Felis <martin.felis@iwr.uni-heidelberg.de> while he was compiling code
|
||||
* that uses a highly efficient math library.
|
||||
*
|
||||
* It is intended to be used as a fast compiling substitute for the
|
||||
* blazingly fast Eigen3 library and tries to mimic its API to a certain
|
||||
* extend.
|
||||
*
|
||||
* Feel free to use it wherever you like. However, no guarantees are given
|
||||
* that this code does what it says it would.
|
||||
*/
|
||||
|
||||
#ifndef SIMPLEMATHBLOCK_H
|
||||
#define SIMPLEMATHBLOCK_H
|
||||
|
||||
#include <cstdlib>
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <assert.h>
|
||||
|
||||
#include "compileassert.h"
|
||||
|
||||
// #include "SimpleMathQR.h"
|
||||
|
||||
/** \brief Namespace for a highly inefficient math library
|
||||
*
|
||||
*/
|
||||
namespace SimpleMath {
|
||||
|
||||
/** \brief Namespace for fixed size elements
|
||||
*/
|
||||
// forward declaration
|
||||
template <typename val_type, unsigned int nrows, unsigned int ncols>
|
||||
class Matrix;
|
||||
|
||||
template <typename matrix_type, typename val_type>
|
||||
class Block {
|
||||
public:
|
||||
typedef val_type value_type;
|
||||
|
||||
Block() :
|
||||
mParentRows(0),
|
||||
mParentCols(0),
|
||||
mParentRowStart(0),
|
||||
mParentColStart(0)
|
||||
{ }
|
||||
Block (const matrix_type &matrix, const unsigned int row_start, const unsigned int col_start, const unsigned int row_count, const unsigned int col_count) :
|
||||
mParentRows (matrix.rows()),
|
||||
mParentCols (matrix.cols()),
|
||||
mParentRowStart (row_start),
|
||||
mParentColStart (col_start),
|
||||
mRowCount (row_count),
|
||||
mColCount (col_count),
|
||||
mTransposed (false) {
|
||||
assert (mParentRows >= mParentRowStart + mRowCount);
|
||||
assert (mParentCols >= mParentColStart + mColCount);
|
||||
|
||||
// without the following line we could not create blocks from const
|
||||
// matrices
|
||||
mParentMatrix = const_cast<matrix_type*>(&matrix);
|
||||
}
|
||||
|
||||
// copy data from the other block into this
|
||||
Block& operator=(const Block &other) {
|
||||
if (this != &other) {
|
||||
if (mRowCount != other.rows() || mColCount != other.cols()) {
|
||||
std::cerr << "Error: cannot assign blocks of different size (left is " << mRowCount << "x" << mColCount << " right is " << other.rows() << "x" << other.cols() << ")!" << std::endl;
|
||||
abort();
|
||||
}
|
||||
|
||||
value_type* temp_values = new value_type [mRowCount * mColCount];
|
||||
|
||||
for (unsigned int i = 0; i < mRowCount; i++) {
|
||||
for (unsigned int j = 0; j < mColCount; j++) {
|
||||
temp_values[i * mColCount + j] = static_cast<value_type>(other(i,j));
|
||||
}
|
||||
}
|
||||
|
||||
for (unsigned int i = 0; i < mRowCount; i++) {
|
||||
for (unsigned int j = 0; j < mColCount; j++) {
|
||||
(*this)(i,j) = temp_values[i * mColCount + j];
|
||||
}
|
||||
}
|
||||
|
||||
delete[] temp_values;
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
template <typename other_matrix_type>
|
||||
// copy data from the other block into this
|
||||
Block& operator=(const other_matrix_type &other) {
|
||||
if (mRowCount != other.rows() || mColCount != other.cols()) {
|
||||
std::cerr << "Error: cannot assign blocks of different size (left is " << mRowCount << "x" << mColCount << " right is " << other.rows() << "x" << other.cols() << ")!" << std::endl;
|
||||
abort();
|
||||
}
|
||||
|
||||
value_type *temp_values = new value_type[mRowCount * mColCount];
|
||||
|
||||
for (unsigned int i = 0; i < mRowCount; i++) {
|
||||
for (unsigned int j = 0; j < mColCount; j++) {
|
||||
temp_values[i * mColCount + j] = static_cast<value_type>(other(i,j));
|
||||
}
|
||||
}
|
||||
|
||||
for (unsigned int i = 0; i < mRowCount; i++) {
|
||||
for (unsigned int j = 0; j < mColCount; j++) {
|
||||
(*this)(i,j) = temp_values[i * mColCount + j];
|
||||
}
|
||||
}
|
||||
|
||||
delete[] temp_values;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
unsigned int rows() const {
|
||||
if (!mTransposed)
|
||||
return mRowCount;
|
||||
|
||||
return mColCount;
|
||||
}
|
||||
unsigned int cols() const {
|
||||
if (!mTransposed)
|
||||
return mColCount;
|
||||
|
||||
return mRowCount;
|
||||
}
|
||||
const val_type& operator() (const unsigned int i, const unsigned int j) const {
|
||||
|
||||
if (!mTransposed) {
|
||||
assert (i < mRowCount);
|
||||
assert (j < mColCount);
|
||||
return (*mParentMatrix) (i + mParentRowStart, j + mParentColStart);
|
||||
}
|
||||
|
||||
return (*mParentMatrix) (j + mParentRowStart, i + mParentColStart);
|
||||
}
|
||||
|
||||
val_type& operator() (const unsigned int i, const unsigned int j) {
|
||||
if (!mTransposed) {
|
||||
assert (i < mRowCount);
|
||||
assert (j < mColCount);
|
||||
return (*mParentMatrix) (i + mParentRowStart, j + mParentColStart);
|
||||
}
|
||||
|
||||
assert (j < mRowCount);
|
||||
assert (i < mColCount);
|
||||
return (*mParentMatrix) (j + mParentRowStart, i + mParentColStart);
|
||||
}
|
||||
|
||||
Block transpose() const {
|
||||
Block result (*this);
|
||||
result.mTransposed = mTransposed ^ true;
|
||||
return result;
|
||||
}
|
||||
|
||||
private:
|
||||
matrix_type *mParentMatrix;
|
||||
const unsigned int mParentRows;
|
||||
const unsigned int mParentCols;
|
||||
const unsigned int mParentRowStart;
|
||||
const unsigned int mParentColStart;
|
||||
const unsigned int mRowCount;
|
||||
const unsigned int mColCount;
|
||||
bool mTransposed;
|
||||
};
|
||||
|
||||
template <typename matrix_type, typename val_type>
|
||||
inline std::ostream& operator<<(std::ostream& output, const Block<matrix_type, val_type> &block) {
|
||||
unsigned int i,j;
|
||||
for (i = 0; i < block.rows(); i++) {
|
||||
output << "[ ";
|
||||
for (j = 0; j < block.cols(); j++) {
|
||||
output << block(i,j);
|
||||
|
||||
if (j < block.cols() - 1)
|
||||
output << ", ";
|
||||
}
|
||||
output << " ]";
|
||||
|
||||
if (block.rows() > 1 && i < block.rows() - 1)
|
||||
output << std::endl;
|
||||
}
|
||||
|
||||
return output;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
#endif /* SIMPLEMATHBLOCK_H */
|
||||
|
|
@ -1,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 */
|
||||
|
|
@ -1,94 +0,0 @@
|
|||
#ifndef _SIMPLE_MATH_CHOLESKY_H
|
||||
#define _SIMPLE_MATH_CHOLESKY_H
|
||||
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
|
||||
#include "SimpleMathDynamic.h"
|
||||
|
||||
namespace SimpleMath {
|
||||
|
||||
template <typename matrix_type>
|
||||
class LLT {
|
||||
public:
|
||||
typedef typename matrix_type::value_type value_type;
|
||||
|
||||
private:
|
||||
LLT () {}
|
||||
|
||||
typedef Dynamic::Matrix<value_type> MatrixXXd;
|
||||
typedef Dynamic::Matrix<value_type> VectorXd;
|
||||
|
||||
bool mIsFactorized;
|
||||
MatrixXXd mL;
|
||||
|
||||
public:
|
||||
LLT (const matrix_type &matrix) :
|
||||
mIsFactorized(false),
|
||||
mL(matrix) {
|
||||
compute();
|
||||
}
|
||||
LLT compute() {
|
||||
for (int i = 0; i < mL.rows(); i++) {
|
||||
for (int j = 0; j < mL.rows(); j++) {
|
||||
if (j > i) {
|
||||
mL(i,j) = 0.;
|
||||
continue;
|
||||
}
|
||||
double s = mL(i,j);
|
||||
for (int k = 0; k < j; k++) {
|
||||
s = s - mL(i,k) * mL(j,k);
|
||||
}
|
||||
if (i > j) {
|
||||
mL(i,j) = s / mL(j,j);
|
||||
} else if (s > 0.) {
|
||||
mL (i,i) = sqrt (s);
|
||||
} else {
|
||||
std::cerr << "Error computing Cholesky decomposition: matrix not symmetric positive definite!" << std::endl;
|
||||
assert (false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
mIsFactorized = true;
|
||||
|
||||
return *this;
|
||||
}
|
||||
Dynamic::Matrix<value_type> solve (
|
||||
const Dynamic::Matrix<value_type> &rhs
|
||||
) const {
|
||||
assert (mIsFactorized);
|
||||
|
||||
VectorXd y (mL.rows());
|
||||
for (unsigned int i = 0; i < mL.rows(); i++) {
|
||||
double temp = rhs[i];
|
||||
|
||||
for (unsigned int j = 0; j < i; j++) {
|
||||
temp = temp - mL(i,j) * y[j];
|
||||
}
|
||||
|
||||
y[i] = temp / mL(i,i);
|
||||
}
|
||||
|
||||
VectorXd x (mL.rows());
|
||||
for (int i = mL.rows() - 1; i >= 0; i--) {
|
||||
double temp = y[i];
|
||||
|
||||
for (unsigned int j = i + 1; j < mL.rows(); j++) {
|
||||
temp = temp - mL(j, i) * x[j];
|
||||
}
|
||||
|
||||
x[i] = temp / mL(i,i);
|
||||
}
|
||||
|
||||
return x;
|
||||
}
|
||||
Dynamic::Matrix<value_type> matrixL() const {
|
||||
return mL;
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
/* _SIMPLE_MATH_CHOLESKY_H */
|
||||
#endif
|
||||
|
|
@ -1,69 +0,0 @@
|
|||
#ifndef _SIMPLE_MATH_COMMA_INITIALIZER_H
|
||||
#define _SIMPLE_MATH_COMMA_INITIALIZER_H
|
||||
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
|
||||
#include "SimpleMathFixed.h"
|
||||
#include "SimpleMathDynamic.h"
|
||||
|
||||
namespace SimpleMath {
|
||||
|
||||
template <typename matrix_type>
|
||||
class CommaInitializer {
|
||||
public:
|
||||
typedef typename matrix_type::value_type value_type;
|
||||
|
||||
CommaInitializer(matrix_type &matrix, const value_type &value) :
|
||||
mElementWasAdded (false) {
|
||||
assert (matrix.cols() > 0 && matrix.rows() > 0);
|
||||
mParentMatrix = &matrix;
|
||||
mRowIndex = 0;
|
||||
mColIndex = 0;
|
||||
(*mParentMatrix)(mRowIndex, mColIndex) = value;
|
||||
}
|
||||
CommaInitializer(matrix_type &matrix, unsigned int row_index, unsigned int col_index) :
|
||||
mRowIndex (row_index),
|
||||
mColIndex (col_index),
|
||||
mElementWasAdded (false) {
|
||||
assert (matrix.cols() > 0 && matrix.rows() > 0);
|
||||
mParentMatrix = &matrix;
|
||||
mRowIndex = row_index;
|
||||
mColIndex = col_index;
|
||||
}
|
||||
~CommaInitializer() {
|
||||
if (!mElementWasAdded
|
||||
&& (mColIndex + 1 < mParentMatrix->cols() || mRowIndex + 1 < mParentMatrix->rows())) {
|
||||
std::cerr << "Error: too few elements passed to CommaInitializer! Expected " << mParentMatrix->size() << " but was given " << mRowIndex * mParentMatrix->cols() + mColIndex + 1 << std::endl;
|
||||
abort();
|
||||
}
|
||||
}
|
||||
CommaInitializer<matrix_type> operator, (const value_type &value) {
|
||||
mColIndex++;
|
||||
if (mColIndex >= mParentMatrix->cols()) {
|
||||
mRowIndex++;
|
||||
mColIndex = 0;
|
||||
}
|
||||
if (mRowIndex == mParentMatrix->rows() && mColIndex == 0 ) {
|
||||
std::cerr << "Error: too many elements passed to CommaInitializer! Expected " << mParentMatrix->size() << " but was given " << mRowIndex * mParentMatrix->cols() + mColIndex + 1 << std::endl;
|
||||
abort();
|
||||
}
|
||||
(*mParentMatrix)(mRowIndex, mColIndex) = value;
|
||||
mElementWasAdded = true;
|
||||
|
||||
return CommaInitializer (*mParentMatrix, mRowIndex, mColIndex);
|
||||
}
|
||||
|
||||
private:
|
||||
CommaInitializer() {}
|
||||
|
||||
matrix_type *mParentMatrix;
|
||||
unsigned int mRowIndex;
|
||||
unsigned int mColIndex;
|
||||
bool mElementWasAdded;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
/* _SIMPLE_MATH_COMMA_INITIALIZER_H */
|
||||
#endif
|
||||
|
|
@ -1,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 */
|
||||
|
|
@ -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 */
|
||||
|
|
@ -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 */
|
||||
|
|
@ -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 */
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -1,22 +0,0 @@
|
|||
#ifndef SIMPLEMATHMAP_H
|
||||
#define SIMPLEMATHMAP_H
|
||||
|
||||
#include "compileassert.h"
|
||||
|
||||
namespace SimpleMath {
|
||||
|
||||
/** \brief \brief Wraps a varying size matrix type around existing data
|
||||
*
|
||||
* \warning If you create a matrix using the map function and then assign
|
||||
* a bigger matrix you invalidate the original memory!
|
||||
*
|
||||
*/
|
||||
template < typename MatrixType >
|
||||
MatrixType Map (typename MatrixType::value_type *data, unsigned int rows, unsigned int cols) {
|
||||
return MatrixType (rows, cols, data);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// SIMPLEMATHMAP_H
|
||||
#endif
|
||||
|
|
@ -1,138 +0,0 @@
|
|||
/**
|
||||
* This is a highly inefficient math library. It was conceived by Martin
|
||||
* Felis <martin.felis@iwr.uni-heidelberg.de> while he was compiling code
|
||||
* that uses a highly efficient math library.
|
||||
*
|
||||
* It is intended to be used as a fast compiling substitute for the
|
||||
* blazingly fast Eigen3 library and tries to mimic its API to a certain
|
||||
* extend.
|
||||
*
|
||||
* Feel free to use it wherever you like. However, no guarantees are given
|
||||
* that this code does what it says it would.
|
||||
*/
|
||||
|
||||
#ifndef SIMPLEMATHMIXED_H
|
||||
#define SIMPLEMATHMIXED_H
|
||||
|
||||
#include <sstream>
|
||||
#include <cstdlib>
|
||||
#include <assert.h>
|
||||
#include <iostream>
|
||||
|
||||
#include "compileassert.h"
|
||||
|
||||
/** \brief Namespace for a highly inefficient math library
|
||||
*
|
||||
*/
|
||||
namespace SimpleMath {
|
||||
|
||||
// conversion Dynamic->Fixed
|
||||
template <typename val_type, unsigned int nrows, unsigned int ncols>
|
||||
inline Fixed::Matrix<val_type, nrows, ncols>::Matrix(const Dynamic::Matrix<val_type> &dynamic_matrix) {
|
||||
if (dynamic_matrix.cols() != ncols
|
||||
|| dynamic_matrix.rows() != nrows) {
|
||||
std::cerr << "Error: cannot assign a dynamic sized matrix of size " << dynamic_matrix.rows() << "x" << dynamic_matrix.cols() << " to a fixed size matrix of size " << nrows << "x" << ncols << "!" << std::endl;
|
||||
abort();
|
||||
}
|
||||
|
||||
for (unsigned int i = 0; i < nrows * ncols; i++) {
|
||||
mData[i] = dynamic_matrix[i];
|
||||
}
|
||||
}
|
||||
|
||||
template <typename val_type, unsigned int nrows, unsigned int ncols>
|
||||
inline Fixed::Matrix<val_type, nrows, ncols>& Fixed::Matrix<val_type, nrows, ncols>::operator=(const Dynamic::Matrix<val_type> &dynamic_matrix) {
|
||||
if (dynamic_matrix.cols() != ncols
|
||||
|| dynamic_matrix.rows() != nrows) {
|
||||
std::cerr << "Error: cannot assign a dynamic sized matrix of size " << dynamic_matrix.rows() << "x" << dynamic_matrix.cols() << " to a fixed size matrix of size " << nrows << "x" << ncols << "!" << std::endl;
|
||||
abort();
|
||||
}
|
||||
|
||||
for (unsigned int i = 0; i < nrows * ncols; i++) {
|
||||
mData[i] = dynamic_matrix[i];
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
// multiplication
|
||||
template <typename val_type, unsigned int nrows, unsigned int ncols>
|
||||
inline Dynamic::Matrix<val_type> operator*(
|
||||
const Fixed::Matrix<val_type, nrows, ncols> &matrix_a,
|
||||
const Dynamic::Matrix<val_type> &matrix_b) {
|
||||
assert (matrix_a.cols() == matrix_b.rows());
|
||||
|
||||
Dynamic::Matrix<val_type> result (nrows, matrix_b.cols());
|
||||
|
||||
result.setZero();
|
||||
|
||||
unsigned int i,j, k;
|
||||
for (i = 0; i < nrows; i++) {
|
||||
for (j = 0; j < matrix_b.cols(); j++) {
|
||||
for (k = 0; k < matrix_b.rows(); k++) {
|
||||
result(i,j) += matrix_a(i,k) * matrix_b(k,j);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
template <typename val_type, unsigned int nrows, unsigned int ncols>
|
||||
inline Dynamic::Matrix<val_type> operator*(
|
||||
const Dynamic::Matrix<val_type> &matrix_a,
|
||||
const Fixed::Matrix<val_type, nrows, ncols> &matrix_b) {
|
||||
assert (matrix_a.cols() == matrix_b.rows());
|
||||
|
||||
Dynamic::Matrix<val_type> result (matrix_a.rows(), ncols);
|
||||
|
||||
result.setZero();
|
||||
|
||||
unsigned int i,j, k;
|
||||
for (i = 0; i < matrix_a.rows(); i++) {
|
||||
for (j = 0; j < matrix_b.cols(); j++) {
|
||||
for (k = 0; k < matrix_b.rows(); k++) {
|
||||
result(i,j) += matrix_a(i,k) * matrix_b(k,j);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// equality
|
||||
template <typename val_type, unsigned int nrows, unsigned int ncols>
|
||||
inline bool operator==(
|
||||
const Fixed::Matrix<val_type, nrows, ncols> &matrix_a,
|
||||
const Dynamic::Matrix<val_type> &matrix_b) {
|
||||
assert (nrows == matrix_a.rows());
|
||||
assert (ncols == matrix_a.cols());
|
||||
|
||||
unsigned int i;
|
||||
for (i = 0; i < matrix_a.size(); i++) {
|
||||
if (matrix_a[i] != matrix_b[i])
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
template <typename val_type, unsigned int nrows, unsigned int ncols>
|
||||
inline bool operator==(
|
||||
const Dynamic::Matrix<val_type> &matrix_b,
|
||||
const Fixed::Matrix<val_type, nrows, ncols> &matrix_a) {
|
||||
assert (nrows == matrix_a.rows());
|
||||
assert (ncols == matrix_a.cols());
|
||||
|
||||
unsigned int i;
|
||||
for (i = 0; i < matrix_a.size(); i++) {
|
||||
if (matrix_a[i] != matrix_b[i])
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif /* SIMPLEMATHMIXED_H */
|
||||
|
|
@ -1,324 +0,0 @@
|
|||
#ifndef _SIMPLE_MATH_QR_H
|
||||
#define _SIMPLE_MATH_QR_H
|
||||
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
|
||||
#include "SimpleMathFixed.h"
|
||||
#include "SimpleMathDynamic.h"
|
||||
#include "SimpleMathBlock.h"
|
||||
|
||||
namespace SimpleMath {
|
||||
|
||||
template <typename matrix_type>
|
||||
class HouseholderQR {
|
||||
public:
|
||||
typedef typename matrix_type::value_type value_type;
|
||||
HouseholderQR() :
|
||||
mIsFactorized(false)
|
||||
{}
|
||||
|
||||
private:
|
||||
typedef Dynamic::Matrix<value_type> MatrixXXd;
|
||||
typedef Dynamic::Matrix<value_type> VectorXd;
|
||||
|
||||
bool mIsFactorized;
|
||||
MatrixXXd mQ;
|
||||
MatrixXXd mR;
|
||||
|
||||
public:
|
||||
HouseholderQR(const matrix_type &matrix) :
|
||||
mIsFactorized(false),
|
||||
mQ(matrix.rows(), matrix.rows())
|
||||
{
|
||||
compute(matrix);
|
||||
}
|
||||
HouseholderQR compute(const matrix_type &matrix) {
|
||||
mR = matrix;
|
||||
mQ = Dynamic::Matrix<value_type>::Identity (mR.rows(), mR.rows());
|
||||
|
||||
for (unsigned int i = 0; i < mR.cols(); i++) {
|
||||
unsigned int block_rows = mR.rows() - i;
|
||||
unsigned int block_cols = mR.cols() - i;
|
||||
|
||||
MatrixXXd current_block = mR.block(i,i, block_rows, block_cols);
|
||||
VectorXd column = current_block.block(0, 0, block_rows, 1);
|
||||
|
||||
value_type alpha = - column.norm();
|
||||
if (current_block(0,0) < 0) {
|
||||
alpha = - alpha;
|
||||
}
|
||||
|
||||
VectorXd v = current_block.block(0, 0, block_rows, 1);
|
||||
v[0] = v[0] - alpha;
|
||||
|
||||
MatrixXXd Q (MatrixXXd::Identity(mR.rows(), mR.rows()));
|
||||
Q.block(i, i, block_rows, block_rows) = MatrixXXd (Q.block(i, i, block_rows, block_rows))
|
||||
- MatrixXXd(v * v.transpose() / (v.squaredNorm() * 0.5));
|
||||
|
||||
mR = Q * mR;
|
||||
|
||||
// Normalize so that we have positive diagonal elements
|
||||
if (mR(i,i) < 0) {
|
||||
mR.block(i,i,block_rows, block_cols) = MatrixXXd(mR.block(i,i,block_rows, block_cols)) * -1.;
|
||||
Q.block(i,i,block_rows, block_rows) = MatrixXXd(Q.block(i,i,block_rows, block_rows)) * -1.;
|
||||
}
|
||||
|
||||
mQ = mQ * Q;
|
||||
}
|
||||
|
||||
mIsFactorized = true;
|
||||
|
||||
return *this;
|
||||
}
|
||||
Dynamic::Matrix<value_type> solve (
|
||||
const Dynamic::Matrix<value_type> &rhs
|
||||
) const {
|
||||
assert (mIsFactorized);
|
||||
|
||||
VectorXd y = mQ.transpose() * rhs;
|
||||
VectorXd x = VectorXd::Zero(mR.cols());
|
||||
|
||||
for (int i = mR.cols() - 1; i >= 0; --i) {
|
||||
value_type z = y[i];
|
||||
|
||||
for (unsigned int j = i + 1; j < mR.cols(); j++) {
|
||||
z = z - x[j] * mR(i,j);
|
||||
}
|
||||
|
||||
if (fabs(mR(i,i)) < std::numeric_limits<value_type>::epsilon() * 10) {
|
||||
std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero!" << std::endl;
|
||||
abort();
|
||||
}
|
||||
x[i] = z / mR(i,i);
|
||||
}
|
||||
|
||||
return x;
|
||||
}
|
||||
Dynamic::Matrix<value_type> inverse() const {
|
||||
assert (mIsFactorized);
|
||||
|
||||
VectorXd rhs_temp = VectorXd::Zero(mQ.cols());
|
||||
MatrixXXd result (mQ.cols(), mQ.cols());
|
||||
|
||||
for (unsigned int i = 0; i < mQ.cols(); i++) {
|
||||
rhs_temp[i] = 1.;
|
||||
|
||||
result.block(0, i, mQ.cols(), 1) = solve(rhs_temp);
|
||||
|
||||
rhs_temp[i] = 0.;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
Dynamic::Matrix<value_type> householderQ () const {
|
||||
return mQ;
|
||||
}
|
||||
Dynamic::Matrix<value_type> matrixR () const {
|
||||
return mR;
|
||||
}
|
||||
};
|
||||
|
||||
template <typename matrix_type>
|
||||
class ColPivHouseholderQR {
|
||||
public:
|
||||
typedef typename matrix_type::value_type value_type;
|
||||
private:
|
||||
typedef Dynamic::Matrix<value_type> MatrixXXd;
|
||||
typedef Dynamic::Matrix<value_type> VectorXd;
|
||||
|
||||
bool mIsFactorized;
|
||||
MatrixXXd mQ;
|
||||
MatrixXXd mR;
|
||||
unsigned int *mPermutations;
|
||||
value_type mThreshold;
|
||||
unsigned int mRank;
|
||||
|
||||
public:
|
||||
ColPivHouseholderQR():
|
||||
mIsFactorized(false) {
|
||||
mPermutations = new unsigned int[1];
|
||||
}
|
||||
|
||||
ColPivHouseholderQR (const ColPivHouseholderQR& other) {
|
||||
mIsFactorized = other.mIsFactorized;
|
||||
mQ = other.mQ;
|
||||
mR = other.mR;
|
||||
mPermutations = new unsigned int[mQ.cols()];
|
||||
mThreshold = other.mThreshold;
|
||||
mRank = other.mRank;
|
||||
}
|
||||
|
||||
ColPivHouseholderQR& operator= (const ColPivHouseholderQR& other) {
|
||||
if (this != &other) {
|
||||
mIsFactorized = other.mIsFactorized;
|
||||
mQ = other.mQ;
|
||||
mR = other.mR;
|
||||
delete[] mPermutations;
|
||||
mPermutations = new unsigned int[mQ.cols()];
|
||||
mThreshold = other.mThreshold;
|
||||
mRank = other.mRank;
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
ColPivHouseholderQR(const matrix_type &matrix) :
|
||||
mIsFactorized(false),
|
||||
mQ(matrix.rows(), matrix.rows()),
|
||||
mThreshold (std::numeric_limits<value_type>::epsilon() * matrix.cols()) {
|
||||
mPermutations = new unsigned int [matrix.cols()];
|
||||
for (unsigned int i = 0; i < matrix.cols(); i++) {
|
||||
mPermutations[i] = i;
|
||||
}
|
||||
compute(matrix);
|
||||
}
|
||||
~ColPivHouseholderQR() {
|
||||
delete[] mPermutations;
|
||||
}
|
||||
|
||||
ColPivHouseholderQR& setThreshold (const value_type& threshold) {
|
||||
mThreshold = threshold;
|
||||
|
||||
return *this;
|
||||
}
|
||||
ColPivHouseholderQR& compute(const matrix_type &matrix) {
|
||||
mR = matrix;
|
||||
mQ = Dynamic::Matrix<value_type>::Identity (mR.rows(), mR.rows());
|
||||
|
||||
for (unsigned int i = 0; i < mR.cols(); i++) {
|
||||
unsigned int block_rows = mR.rows() - i;
|
||||
unsigned int block_cols = mR.cols() - i;
|
||||
|
||||
// find and swap the column with the highest norm
|
||||
unsigned int col_index_norm_max = i;
|
||||
value_type col_norm_max = VectorXd(mR.block(i,i, block_rows, 1)).squaredNorm();
|
||||
|
||||
for (unsigned int j = i + 1; j < mR.cols(); j++) {
|
||||
VectorXd column = mR.block(i, j, block_rows, 1);
|
||||
|
||||
if (column.squaredNorm() > col_norm_max) {
|
||||
col_index_norm_max = j;
|
||||
col_norm_max = column.squaredNorm();
|
||||
}
|
||||
}
|
||||
|
||||
if (col_norm_max < mThreshold) {
|
||||
// if all entries of the column is close to zero, we bail out
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
if (col_index_norm_max != i) {
|
||||
VectorXd temp_col = mR.block(0, i, mR.rows(), 1);
|
||||
mR.block(0,i,mR.rows(),1) = mR.block(0, col_index_norm_max, mR.rows(), 1);
|
||||
mR.block(0, col_index_norm_max, mR.rows(), 1) = temp_col;
|
||||
|
||||
unsigned int temp_index = mPermutations[i];
|
||||
mPermutations[i] = mPermutations[col_index_norm_max];
|
||||
mPermutations[col_index_norm_max] = temp_index;
|
||||
}
|
||||
|
||||
MatrixXXd current_block = mR.block(i,i, block_rows, block_cols);
|
||||
VectorXd column = current_block.block(0, 0, block_rows, 1);
|
||||
|
||||
value_type alpha = - column.norm();
|
||||
if (current_block(0,0) < 0) {
|
||||
alpha = - alpha;
|
||||
}
|
||||
|
||||
VectorXd v = current_block.block(0, 0, block_rows, 1);
|
||||
v[0] = v[0] - alpha;
|
||||
|
||||
MatrixXXd Q (MatrixXXd::Identity(mR.rows(), mR.rows()));
|
||||
|
||||
Q.block(i, i, block_rows, block_rows) = MatrixXXd (Q.block(i, i, block_rows, block_rows))
|
||||
- MatrixXXd(v * v.transpose() / (v.squaredNorm() * static_cast<value_type>(0.5)));
|
||||
|
||||
mR = Q * mR;
|
||||
|
||||
// Normalize so that we have positive diagonal elements
|
||||
if (mR(i,i) < 0) {
|
||||
mR.block(i,i,block_rows, block_cols) = MatrixXXd(mR.block(i,i,block_rows, block_cols)) * -1.;
|
||||
Q.block(i,i,block_rows, block_rows) = MatrixXXd(Q.block(i,i,block_rows, block_rows)) * -1.;
|
||||
}
|
||||
|
||||
mQ = mQ * Q;
|
||||
}
|
||||
|
||||
mIsFactorized = true;
|
||||
|
||||
return *this;
|
||||
}
|
||||
Dynamic::Matrix<value_type> solve (
|
||||
const Dynamic::Matrix<value_type> &rhs
|
||||
) const {
|
||||
assert (mIsFactorized);
|
||||
|
||||
VectorXd y = mQ.transpose() * rhs;
|
||||
VectorXd x = VectorXd::Zero(mR.cols());
|
||||
|
||||
for (int i = mR.cols() - 1; i >= 0; --i) {
|
||||
value_type z = y[i];
|
||||
|
||||
for (unsigned int j = i + 1; j < mR.cols(); j++) {
|
||||
z = z - x[mPermutations[j]] * mR(i,j);
|
||||
}
|
||||
|
||||
if (fabs(mR(i,i)) < std::numeric_limits<value_type>::epsilon() * 10) {
|
||||
std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero!" << std::endl;
|
||||
abort();
|
||||
}
|
||||
x[mPermutations[i]] = z / mR(i,i);
|
||||
}
|
||||
|
||||
return x;
|
||||
}
|
||||
Dynamic::Matrix<value_type> inverse() const {
|
||||
assert (mIsFactorized);
|
||||
|
||||
VectorXd rhs_temp = VectorXd::Zero(mQ.cols());
|
||||
MatrixXXd result (mQ.cols(), mQ.cols());
|
||||
|
||||
for (unsigned int i = 0; i < mQ.cols(); i++) {
|
||||
rhs_temp[i] = 1.;
|
||||
|
||||
result.block(0, i, mQ.cols(), 1) = solve(rhs_temp);
|
||||
|
||||
rhs_temp[i] = 0.;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
Dynamic::Matrix<value_type> householderQ () const {
|
||||
return mQ;
|
||||
}
|
||||
Dynamic::Matrix<value_type> matrixR () const {
|
||||
return mR;
|
||||
}
|
||||
Dynamic::Matrix<value_type> matrixP () const {
|
||||
MatrixXXd P = MatrixXXd::Identity(mR.cols(), mR.cols());
|
||||
MatrixXXd identity = MatrixXXd::Identity(mR.cols(), mR.cols());
|
||||
for (unsigned int i = 0; i < mR.cols(); i++) {
|
||||
P.block(0,i,mR.cols(),1) = identity.block(0,mPermutations[i], mR.cols(), 1);
|
||||
}
|
||||
return P;
|
||||
}
|
||||
|
||||
unsigned int rank() const {
|
||||
value_type abs_threshold = fabs(mR(0,0)) * mThreshold;
|
||||
|
||||
for (unsigned int i = 1; i < mR.cols(); i++) {
|
||||
if (fabs(mR(i,i)) < abs_threshold)
|
||||
return i;
|
||||
}
|
||||
|
||||
return mR.cols();
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
/* _SIMPLE_MATH_QR_H */
|
||||
#endif
|
||||
|
|
@ -1,39 +0,0 @@
|
|||
#ifndef _COMPILE_ASSERT_H
|
||||
#define _COMPILE_ASSERT_H
|
||||
|
||||
/*
|
||||
* This is a simple compile time assertion tool taken from:
|
||||
* http://blogs.msdn.com/b/abhinaba/archive/2008/10/27/c-c-compile-time-asserts.aspx
|
||||
* written by Abhinaba Basu!
|
||||
*
|
||||
* Thanks!
|
||||
*/
|
||||
|
||||
#ifdef __cplusplus
|
||||
|
||||
#define JOIN( X, Y ) JOIN2(X,Y)
|
||||
#define JOIN2( X, Y ) X##Y
|
||||
|
||||
namespace static_assert_compat
|
||||
{
|
||||
template <bool> struct STATIC_ASSERT_FAILURE;
|
||||
template <> struct STATIC_ASSERT_FAILURE<true> { enum { value = 1 }; };
|
||||
|
||||
template<int x> struct static_assert_test{};
|
||||
}
|
||||
|
||||
#define COMPILE_ASSERT(x) \
|
||||
typedef ::static_assert_compat::static_assert_test<\
|
||||
sizeof(::static_assert_compat::STATIC_ASSERT_FAILURE< (bool)( x ) >)>\
|
||||
JOIN(_static_assert_typedef, __LINE__)
|
||||
|
||||
#else // __cplusplus
|
||||
|
||||
#define COMPILE_ASSERT(x) extern int __dummy[(int)x]
|
||||
|
||||
#endif // __cplusplus
|
||||
|
||||
#define VERIFY_EXPLICIT_CAST(from, to) COMPILE_ASSERT(sizeof(from) == sizeof(to))
|
||||
|
||||
// _COMPILE_ASSERT_H_
|
||||
#endif
|
||||
|
|
@ -2,27 +2,25 @@
|
|||
#define MESHUP_CONFIG_H
|
||||
|
||||
#include "SimpleMath/SimpleMath.h"
|
||||
#include "SimpleMath/SimpleMathGL.h"
|
||||
#include "SimpleMath/SimpleMathMap.h"
|
||||
|
||||
typedef SimpleMath::Fixed::Matrix<float, 2, 1> Vector2f;
|
||||
typedef SimpleMath::Fixed::Matrix<float, 2, 2> Matrix22f;
|
||||
typedef SimpleMath::Fixed<float, 2, 1> Vector2f;
|
||||
typedef SimpleMath::Fixed<float, 2, 2> Matrix22f;
|
||||
|
||||
typedef SimpleMath::Fixed::Matrix<float, 3, 1> Vector3f;
|
||||
typedef SimpleMath::Fixed::Matrix<float, 3, 3> Matrix33f;
|
||||
typedef SimpleMath::Fixed<float, 3, 1> Vector3f;
|
||||
typedef SimpleMath::Fixed<float, 3, 3> Matrix33f;
|
||||
|
||||
typedef SimpleMath::Fixed::Matrix<float, 4, 1> Vector4f;
|
||||
typedef SimpleMath::Fixed::Matrix<float, 4, 4> Matrix44f;
|
||||
typedef SimpleMath::Fixed<float, 4, 1> Vector4f;
|
||||
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::Matrix<float> MatrixNNf;
|
||||
typedef SimpleMath::Dynamic<float> VectorNf;
|
||||
typedef SimpleMath::Dynamic<float> MatrixNNf;
|
||||
|
||||
typedef SimpleMath::Fixed::Matrix<double, 3, 1> Vector3d;
|
||||
typedef SimpleMath::Fixed::Matrix<double, 3, 3> Matrix33d;
|
||||
typedef SimpleMath::Fixed<double, 3, 1> Vector3d;
|
||||
typedef SimpleMath::Fixed<double, 3, 3> Matrix33d;
|
||||
|
||||
typedef SimpleMath::Dynamic::Matrix<double> VectorNd;
|
||||
typedef SimpleMath::Dynamic::Matrix<double> MatrixNNd;
|
||||
typedef SimpleMath::Dynamic<double> VectorNd;
|
||||
typedef SimpleMath::Dynamic<double> MatrixNNd;
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@
|
|||
#include "imgui/imgui.h"
|
||||
#include "imgui_dock.h"
|
||||
|
||||
using namespace SimpleMath::GL;
|
||||
using namespace SimpleMath;
|
||||
|
||||
struct Renderer;
|
||||
|
||||
|
|
@ -180,10 +180,10 @@ extern "C" {
|
|||
|
||||
const struct module_api MODULE_API = {
|
||||
.init = module_init,
|
||||
.finalize = module_finalize,
|
||||
.reload = module_reload,
|
||||
.step = module_step,
|
||||
.unload = module_unload,
|
||||
.finalize = module_finalize
|
||||
.step = module_step
|
||||
};
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -13,7 +13,6 @@
|
|||
#include "imgui_dock.h"
|
||||
|
||||
using namespace SimpleMath;
|
||||
using namespace SimpleMath::GL;
|
||||
|
||||
typedef tinygltf::TinyGLTF GLTFLoader;
|
||||
|
||||
|
|
|
|||
|
|
@ -46,10 +46,10 @@ void handle_mouse (struct module_state *state) {
|
|||
view_dir = (poi - eye).normalized();
|
||||
Vector3f right = camera_rot_inv.block<1,3>(0,0).transpose();
|
||||
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,
|
||||
right[0], right[1], right[2]);
|
||||
Matrix33f rot_matrix_x = SimpleMath::GL::RotateMat33(
|
||||
Matrix33f rot_matrix_x = SimpleMath::RotateMat33(
|
||||
gGuiInputState->mousedX * 0.4f,
|
||||
0.f, 1.f, 0.f);
|
||||
poi = eye + rot_matrix_x * rot_matrix_y * view_dir;
|
||||
|
|
@ -169,9 +169,9 @@ extern "C" {
|
|||
|
||||
const struct module_api MODULE_API = {
|
||||
.init = module_init,
|
||||
.finalize = module_finalize,
|
||||
.reload = module_reload,
|
||||
.step = module_step,
|
||||
.unload = module_unload,
|
||||
.finalize = module_finalize
|
||||
.step = module_step
|
||||
};
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue