diff --git a/src/SimpleMath/SimpleMath.h b/src/SimpleMath/SimpleMath.h index 3ce7e39..31c09c8 100644 --- a/src/SimpleMath/SimpleMath.h +++ b/src/SimpleMath/SimpleMath.h @@ -45,272 +45,473 @@ class ColPivHouseholderQR; // template struct MatrixBase { - typedef MatrixBase MatrixType; - typedef ScalarType value_type; - - template - Derived& operator=(const MatrixBase& other) { - if (static_cast(this) != static_cast(&other)) { - for (size_t i = 0; i < other.rows(); i++) { - for (size_t j = 0; j < other.cols(); j++) { - this->operator()(i,j) = other(i,j); - } - } - } + typedef MatrixBase MatrixType; + typedef ScalarType value_type; - return *this; - } - - // - // operators with scalars - // - Derived operator*(const double& scalar) const { - Derived result (rows(), cols()); - - unsigned int i,j; - for (i = 0; i < rows(); i++) { - for (j = 0; j < cols(); j++) { - result (i,j) = operator()(i,j) * static_cast(scalar); - } - } - return result; - } - - Derived operator*(const float& scalar) const { - Derived result (rows(), cols()); - - unsigned int i,j; - for (i = 0; i < rows(); i++) { - for (j = 0; j < cols(); j++) { - result (i,j) = operator()(i,j) * static_cast(scalar); - } - } - return result; - } - - // - // operators with other matrices - // - template - bool operator==(const OtherDerived& other) const { - unsigned int i,j; - for (i = 0; i < rows(); i++) { - for (j = 0; j < cols(); j++) { - if (this->operator()(i,j) != other(i,j)) - return false; - } - } - - return true; - } - - CommaInitializer operator<< (const value_type& value) { - return CommaInitializer (*(static_cast(this)), value); - } - - template - Derived operator+(const OtherDerived& other) const { - Derived result (*(static_cast(this))); - result += other; - return result; - } - - template - Derived operator-(const OtherDerived& other) { - Derived result (*(static_cast(this))); - result -= other; - return result; - } - - template - Derived operator-(const OtherDerived& other) const { - Derived result (*(static_cast(this))); - result -= other; - return result; - } - - template - Matrix - operator*(const MatrixBase &other) const { - Matrix result(Matrix::Zero(rows(), other.cols())); - - unsigned int i, j, k; - for (i = 0; i < rows(); i++) { - for (j = 0; j < other.cols(); j++) { - for (k = 0; k < other.rows(); k++) { - result(i, j) += operator()(i, k) * other(k, j); - } - } + Derived& operator=(const Derived& other) { + if (static_cast(this) != static_cast(&other)) { + for (size_t i = 0; i < other.rows(); i++) { + for (size_t j = 0; j < other.cols(); j++) { + this->operator()(i,j) = other(i,j); } - - return result; + } } - template - Derived operator*=(const OtherDerived& other) { - Derived copy (*static_cast(this)); -// Derived result (Derived::Zero(rows(), other.cols())); + return *this; + } - unsigned int i,j,k; - for (i = 0; i < rows(); i++) { - for (j = 0; j < other.cols(); j++) { - for (k = 0; k < other.rows(); k++) { - operator()(i,j) += copy.operator()(i,k) * other(k,j); - } - } - } - return *this; - } + template + Derived& operator=(const MatrixBase& other) { + if (static_cast(this) != static_cast(&other)) { + for (size_t i = 0; i < other.rows(); i++) { + for (size_t j = 0; j < other.cols(); j++) { + this->operator()(i,j) = other(i,j); + } + } + } - Derived operator*=(const ScalarType& s) { - unsigned int i,j; - for (i = 0; i < rows(); i++) { - for (j = 0; j < cols(); j++) { - operator()(i,j) *= s; - } - } + return *this; + } - return *this; - } + // + // operators with scalars + // + Matrix operator*(const double& scalar) const { + Matrix result (rows(), cols()); - void set(const ScalarType& v0) { - static_assert(cols() * rows() == 1, "Invalid matrix size"); - data()[0] = v0; - } + unsigned int i,j; + for (i = 0; i < rows(); i++) { + for (j = 0; j < cols(); j++) { + result (i,j) = operator()(i,j) * static_cast(scalar); + } + } + return result; + } - void set( - const ScalarType& v0, - const ScalarType& v1, - const ScalarType& v2 - ) { - assert(cols() * rows() == 3); + Matrix operator*(const float& scalar) const { + Matrix result (rows(), cols()); - data()[0] = v0; - data()[1] = v1; - data()[2] = v2; - } + unsigned int i,j; + for (i = 0; i < rows(); i++) { + for (j = 0; j < cols(); j++) { + result (i,j) = operator()(i,j) * static_cast(scalar); + } + } + return result; + } - void set( - const ScalarType& v0, - const ScalarType& v1, - const ScalarType& v2, - const ScalarType& v3 - ) { - assert(cols() * rows() == 4); + // + // operators with other matrices + // + template + bool operator==(const MatrixBase& other) const { + unsigned int i,j; + for (i = 0; i < rows(); i++) { + for (j = 0; j < cols(); j++) { + if (this->operator()(i,j) != other(i,j)) + return false; + } + } + + return true; + } - data()[0] = v0; - data()[1] = v1; - data()[2] = v2; - data()[3] = v3; - } + template + bool operator!=(const MatrixBase& other) const { + return !(operator==(other)); + } - size_t rows() const { - return static_cast(this)->rows(); - } - size_t cols() const { - return static_cast(this)->cols(); - } + CommaInitializer operator<< (const value_type& value) { + return CommaInitializer (*(static_cast(this)), value); + } + + template + Derived operator+(const OtherDerived& other) const { + Derived result (*(static_cast(this))); + result += other; + return result; + } - const ScalarType& operator()(const size_t& i, const size_t& j) const { - return static_cast(this)->operator()(i,j); - } - ScalarType& operator()(const size_t& i, const size_t& j) { - return static_cast(this)->operator()(i,j); - } + template + Derived operator-(const OtherDerived& other) { + Derived result (*(static_cast(this))); + result -= other; + return result; + } - const ScalarType& operator[](const size_t& i) const { + template + Derived operator-(const OtherDerived& other) const { + Derived result (*(static_cast(this))); + result -= other; + return result; + } + + template + Matrix + operator*(const MatrixBase &other) const { + Matrix result(Matrix::Zero(rows(), other.cols())); + + unsigned int i, j, k; + for (i = 0; i < rows(); i++) { + for (j = 0; j < other.cols(); j++) { + for (k = 0; k < other.rows(); k++) { + result(i, j) += operator()(i, k) * other(k, j); + } + } + } + + return result; + } + + template + Derived operator*=(const OtherDerived &other) { + Derived copy (*static_cast(this)); + this->setZero(); + + unsigned int i, j, k; + for (i = 0; i < rows(); i++) { + for (j = 0; j < other.cols(); j++) { + for (k = 0; k < other.rows(); k++) { + this->operator()(i, j) += copy.operator()(i, k) * other(k, j); + } + } + } + return *this; + } + + Matrix operator-() const { + Matrix copy (*static_cast(this)); + for (int i = 0; i < rows(); i++) { + for (int j = 0; j < cols(); j++) { + copy(i,j) *= static_cast(-1.); + } + } + return copy; + } + + Derived operator*=(const ScalarType& s) { + unsigned int i,j; + for (i = 0; i < rows(); i++) { + for (j = 0; j < cols(); j++) { + operator()(i,j) *= s; + } + } + + return *this; + } + + void resize(unsigned int nrows, unsigned int ncols = 1) { + static_assert(Rows == Dynamic, "Resize of fixed size matrices not allowed."); + + // Resize the this matrix (so far only possible for subclasses of the + // Matrix class) + Matrix* this_matrix = static_cast*>(this); + this_matrix->mStorage.resize(nrows, ncols); + } + + void conservativeResize(unsigned int nrows, unsigned int ncols = 1) { + static_assert(Rows == Dynamic, "Resize of fixed size matrices not allowed."); + + + Derived copy(*this); + + unsigned int arows = std::min(nrows, (unsigned int) rows()); + unsigned int acols = std::min(ncols, (unsigned int) cols()); + + resize(nrows, ncols); + setZero(); + + // TODO: set entries to zero within the loop + for (unsigned int i = 0; i < arows; i++) { + for (unsigned int j = 0; j < acols; j++) { + this->operator()(i, j) = copy(i,j); + } + } + } + + void setZero() { + int nrows = rows(); + int ncols = cols(); + for (int i = 0; i < nrows; i++) { + for (int j = 0; j < ncols; j++) { + operator()(i,j) = static_cast(0.0); + } + } + } + + void set(const ScalarType& v0) { + static_assert(cols() * rows() == 1, "Invalid matrix size"); + data()[0] = v0; + } + + void set( + const ScalarType& v0, + const ScalarType& v1, + const ScalarType& v2 + ) { + assert(cols() * rows() == 3); + + data()[0] = v0; + data()[1] = v1; + data()[2] = v2; + } + + void set( + const ScalarType& v0, + const ScalarType& v1, + const ScalarType& v2, + const ScalarType& v3 + ) { + assert(cols() * rows() == 4); + + data()[0] = v0; + data()[1] = v1; + data()[2] = v2; + data()[3] = v3; + } + + void set( + const ScalarType& v0, + const ScalarType& v1, + const ScalarType& v2, + const ScalarType& v3, + const ScalarType& v4, + const ScalarType& v5 + ) { + assert(cols() * rows() == 6); + + data()[0] = v0; + data()[1] = v1; + data()[2] = v2; + data()[3] = v3; + data()[4] = v4; + data()[5] = v5; + } + + void set( + const ScalarType& v00, + const ScalarType& v01, + const ScalarType& v02, + const ScalarType& v03, + const ScalarType& v04, + const ScalarType& v05, + + const ScalarType& v10, + const ScalarType& v11, + const ScalarType& v12, + const ScalarType& v13, + const ScalarType& v14, + const ScalarType& v15, + + const ScalarType& v20, + const ScalarType& v21, + const ScalarType& v22, + const ScalarType& v23, + const ScalarType& v24, + const ScalarType& v25, + + const ScalarType& v30, + const ScalarType& v31, + const ScalarType& v32, + const ScalarType& v33, + const ScalarType& v34, + const ScalarType& v35, + + const ScalarType& v40, + const ScalarType& v41, + const ScalarType& v42, + const ScalarType& v43, + const ScalarType& v44, + const ScalarType& v45, + + const ScalarType& v50, + const ScalarType& v51, + const ScalarType& v52, + const ScalarType& v53, + const ScalarType& v54, + const ScalarType& v55 + ) { + assert(cols() == 6 && rows() == 6); + + operator()(0,0) = v00; + operator()(0,1) = v01; + operator()(0,2) = v02; + operator()(0,3) = v03; + operator()(0,4) = v04; + operator()(0,5) = v05; + + operator()(1,0) = v10; + operator()(1,1) = v11; + operator()(1,2) = v12; + operator()(1,3) = v13; + operator()(1,4) = v14; + operator()(1,5) = v15; + + operator()(2,0) = v20; + operator()(2,1) = v21; + operator()(2,2) = v22; + operator()(2,3) = v23; + operator()(2,4) = v24; + operator()(2,5) = v25; + + operator()(3,0) = v30; + operator()(3,1) = v31; + operator()(3,2) = v32; + operator()(3,3) = v33; + operator()(3,4) = v34; + operator()(3,5) = v35; + + operator()(4,0) = v40; + operator()(4,1) = v41; + operator()(4,2) = v42; + operator()(4,3) = v43; + operator()(4,4) = v44; + operator()(4,5) = v45; + + operator()(5,0) = v50; + operator()(5,1) = v51; + operator()(5,2) = v52; + operator()(5,3) = v53; + operator()(5,4) = v54; + operator()(5,5) = v55; + + } + + + + size_t rows() const { + return static_cast(this)->rows(); + } + + size_t cols() const { + return static_cast(this)->cols(); + } + + size_t size() const { + return static_cast(this)->rows() * static_cast(this)->cols(); + } + + const ScalarType& operator()(const size_t& i, const size_t& j) const { + return static_cast(this)->operator()(i,j); + } + ScalarType& operator()(const size_t& i, const size_t& j) { + return static_cast(this)->operator()(i,j); + } + + const ScalarType& operator[](const size_t& i) const { assert(cols() == 1); - return static_cast(this)->operator()(i,0); - } - ScalarType& operator[](const size_t& i) { - assert(cols() == 1); - return static_cast(this)->operator()(i,0); - } + return static_cast(this)->operator()(i,0); + } + ScalarType& operator[](const size_t& i) { + assert(cols() == 1); + return static_cast(this)->operator()(i,0); + } - operator ScalarType() const { + operator ScalarType() const { #ifndef NDEBUG - std::cout << "Error trying to cast to scalar type. Dimensions are: " - << static_cast(this)->rows() << ", " - << static_cast(this)->cols() << "." - << std::endl; + if ( static_cast(this)->cols() != 1 + || static_cast(this)->rows() != 1) { + std::cout << "Error trying to cast to scalar type. Dimensions are: " + << static_cast(this)->rows() << ", " + << static_cast(this)->cols() << "." + << std::endl; + } #endif - assert ( static_cast(this)->cols() == 1 - && static_cast(this)->rows() == 1); - return static_cast(this)->operator()(0,0); - } + assert ( static_cast(this)->cols() == 1 + && static_cast(this)->rows() == 1); + return static_cast(this)->operator()(0,0); + } - // - // Numerical functions - // + // + // Numerical functions + // - // TODO: separate functions for float or ScalarType matrices - ScalarType dot(const Derived& other) const { - assert ((rows() == 1 || cols() == 1) && (other.rows() == 1 || other.cols() == 1)); + // TODO: separate functions for float or ScalarType matrices + ScalarType dot(const Derived& other) const { + assert ((rows() == 1 || cols() == 1) && (other.rows() == 1 || other.cols() == 1)); - ScalarType result = 0.0; + ScalarType result = 0.0; - size_t n = rows() * cols(); - for (size_t i = 0; i < n; ++i) { - result += operator[](i) * other[i]; - } + size_t n = rows() * cols(); + for (size_t i = 0; i < n; ++i) { + result += operator[](i) * other[i]; + } - return result; - } + return result; + } - // TODO: separate functions for float or ScalarType matrices - ScalarType squaredNorm() const { - ScalarType result = static_cast(0.0); + // TODO: separate functions for float or ScalarType matrices + ScalarType squaredNorm() const { + ScalarType result = static_cast(0.0); - size_t nr = rows(); - size_t nc = cols(); - for (size_t i = 0; i < nr; ++i) { - for (size_t j = 0; j < nc; ++j) { - result += operator()(i, j) * operator()(i, j); - } - } + size_t nr = rows(); + size_t nc = cols(); + for (size_t i = 0; i < nr; ++i) { + for (size_t j = 0; j < nc; ++j) { + result += operator()(i, j) * operator()(i, j); + } + } - return result; - } + return result; + } - // TODO: separate functions for float or ScalarType matrices - ScalarType norm() const { - return static_cast(std::sqrt(squaredNorm())); - } - - // TODO: separate functions for float or ScalarType matrices - Derived normalized() const { - Derived result (*this); + // TODO: separate functions for float or ScalarType matrices + ScalarType norm() const { + return static_cast(std::sqrt(squaredNorm())); + } + + // TODO: separate functions for float or ScalarType matrices + Derived normalized() const { + Derived result (*this); - ScalarType length = this->norm(); + ScalarType length = this->norm(); - return result / length; - } + return result / length; + } - // TODO: separate functions for float or ScalarType matrices - Derived normalize() { - ScalarType length = norm(); + // TODO: separate functions for float or ScalarType matrices + Derived normalize() { + ScalarType length = norm(); - *this *= static_cast(1.0) / length; + *this *= static_cast(1.0) / length; - return *this; - } + return *this; + } - Derived cross(const Derived& other) const { - assert(cols() * rows() == 3); - - Derived result(rows(), cols()); - result[0] = operator[](1) * other[2] - operator[](2) * other[1]; - result[1] = operator[](2) * other[0] - operator[](0) * other[2]; - result[2] = operator[](0) * other[1] - operator[](1) * other[0]; + Derived cross(const Derived& other) const { + assert(cols() * rows() == 3); + + Derived result(rows(), cols()); + result[0] = operator[](1) * other[2] - operator[](2) * other[1]; + result[1] = operator[](2) * other[0] - operator[](0) * other[2]; + result[2] = operator[](0) * other[1] - operator[](1) * other[0]; - return result; - } + return result; + } Derived inverse() const { return colPivHouseholderQr().inverse(); } + ScalarType trace() const { + assert(rows() == cols()); + + ScalarType result = static_cast(0.0); + + for (unsigned int i = 0; i < rows(); i++) { + result += operator()(i,i); + } + + return result; + } + + // TODO: implement Cholesky decompositioe + const HouseholderQR llt() const { + std::cerr << "LLT decomposition uses householder!" << std::endl; + return HouseholderQR(*this); + } + const HouseholderQR householderQr() const { return HouseholderQR(*this); } @@ -319,125 +520,142 @@ struct MatrixBase { return ColPivHouseholderQR(*this); } - ScalarType* data() { - return static_cast(this)->data(); - } + ScalarType* data() { + return static_cast(this)->data(); + } - const ScalarType* data() const { - return static_cast(this)->data(); - } + const ScalarType* data() const { + return static_cast(this)->data(); + } - // - // Special Constructors - // - static Derived Zero(int NumRows = (Rows == Dynamic) ? 1 : Rows, int NumCols = (Cols == Dynamic) ? 1 : Cols) { - Derived result (NumRows, NumCols); + // + // Special Constructors + // + static Derived Zero(int NumRows = (Rows == Dynamic) ? 1 : Rows, int NumCols = (Cols == Dynamic) ? 1 : Cols) { + Derived result (NumRows, NumCols); - for (size_t i = 0; i < NumRows; i++) { - for (size_t j = 0; j < NumCols; j++) { - result(i,j) = static_cast(0.0); - } - } + for (size_t i = 0; i < NumRows; i++) { + for (size_t j = 0; j < NumCols; j++) { + result(i,j) = static_cast(0.0); + } + } - return result; - } + return result; + } - static Derived Identity(size_t NumRows = Rows, size_t NumCols = Cols) { - Derived result (Derived::Zero(NumRows, NumCols)); + static Derived Identity(size_t NumRows = Rows, size_t NumCols = Cols) { + Derived result (Derived::Zero(NumRows, NumCols)); - for (size_t i = 0; i < NumRows; i++) { - result(i,i) = static_cast(1.0); - } + for (size_t i = 0; i < NumRows; i++) { + result(i,i) = static_cast(1.0); + } - return result; - } + return result; + } - // TODO: Random() + static Derived Constant(int NumRows, const ScalarType &value) { + Derived result (NumRows, 1); - // - // Block accessors - // - template < - int block_rows, - int block_cols - > - Block< - Derived, - ScalarType, - block_rows, - block_cols - > block(int block_row_index, int block_col_index) { - assert(block_row_index + block_rows <= rows()); - assert(block_col_index + block_cols <= cols()); - return Block(static_cast(this), block_row_index, block_col_index); - } + for (size_t i = 0; i < NumRows; i++) { + result(i,0) = value; + } - template < - int block_rows, - int block_cols - > - const Block< - Derived, - ScalarType, - block_rows, - block_cols - > block(int block_row_index, int block_col_index) const { - assert(block_row_index + block_rows <= rows()); - assert(block_col_index + block_cols <= cols()); - return Block(const_cast(static_cast(this)), block_row_index, block_col_index); - } + return result; + } - Block< - Derived, - ScalarType - > block(int block_row_index, int block_col_index, - int block_num_rows, int block_num_cols) { - assert(block_row_index + block_num_rows <= rows()); - assert(block_col_index + block_num_cols <= cols()); - return Block(static_cast(this), block_row_index, block_col_index, block_num_rows, block_num_cols); - } + static Derived Constant(int NumRows, int NumCols, const ScalarType &value) { + Derived result (NumRows, NumCols); - const Block< - Derived, - ScalarType - > block(int block_row_index, int block_col_index, - int block_num_rows, int block_num_cols) const { - assert(block_row_index + block_num_rows <= rows()); - assert(block_col_index + block_num_cols <= cols()); - return Block(static_cast(this), block_row_index, block_col_index, block_num_rows, block_num_cols); - } + for (size_t i = 0; i < NumRows; i++) { + for (size_t j = 0; j < NumCols; j++) { + result(i,j) = value; + } + } - // TODO: head, tail + return result; + } - // - // Transpose - // - Transpose transpose() { - return Transpose(static_cast(this)); - } + static Derived Random(int NumRows = (Rows == Dynamic) ? 1 : Rows, int NumCols = (Cols == Dynamic) ? 1 : Cols) { + Derived result (NumRows, NumCols); - const Transpose transpose() const { - return Transpose(static_cast(this)); - } + for (size_t i = 0; i < NumRows; i++) { + for (size_t j = 0; j < NumCols; j++) { + result(i,j) = (static_cast(rand()) / static_cast(RAND_MAX)) * 2.0 - 1.0; + } + } + + return result; + } + + + // + // Block accessors + // + template < + int block_rows, + int block_cols + > + Block< + Derived, + ScalarType, + block_rows, + block_cols + > block(int block_row_index, int block_col_index) { + assert(block_row_index + block_rows <= rows()); + assert(block_col_index + block_cols <= cols()); + return Block(static_cast(this), block_row_index, block_col_index); + } + + template < + int block_rows, + int block_cols + > + const Block< + Derived, + ScalarType, + block_rows, + block_cols + > block(int block_row_index, int block_col_index) const { + assert(block_row_index + block_rows <= rows()); + assert(block_col_index + block_cols <= cols()); + return Block(const_cast(static_cast(this)), block_row_index, block_col_index); + } + + Block< + Derived, + ScalarType + > block(int block_row_index, int block_col_index, + int block_num_rows, int block_num_cols) { + assert(block_row_index + block_num_rows <= rows()); + assert(block_col_index + block_num_cols <= cols()); + return Block(static_cast(this), block_row_index, block_col_index, block_num_rows, block_num_cols); + } + + const Block< + const Derived, + ScalarType + > block(int block_row_index, int block_col_index, + int block_num_rows, int block_num_cols) const { + assert(block_row_index + block_num_rows <= rows()); + assert(block_col_index + block_num_cols <= cols()); + return Block(static_cast(this), block_row_index, block_col_index, block_num_rows, block_num_cols); + } + + // TODO: head, tail + + // + // Transpose + // + Transpose transpose() { + return Transpose(static_cast(this)); + } + + const Transpose transpose() const { + return Transpose(static_cast(this)); + } }; - -template -inline Derived operator*(const ScalarType& scalar, const MatrixBase &matrix) { - return matrix * scalar; -} - -template -inline Derived operator*(const MatrixBase &matrix, const ScalarType& scalar) { - return matrix * scalar; -} - -template -inline Derived operator/(const MatrixBase &matrix, const ScalarType& scalar) { - return matrix * (1.0 / scalar); -} - template struct Storage; @@ -447,41 +665,41 @@ struct Storage : public Storage struct Storage { - ScalarType mData[SizeAtCompileTime]; + ScalarType mData[SizeAtCompileTime]; - Storage() {} + Storage() {} - Storage(int rows, int cols) { - resize(rows, cols); - } + Storage(int rows, int cols) { + resize(rows, cols); + } - size_t rows() const { return NumRows; } + size_t rows() const { return NumRows; } - size_t cols() const { return NumCols; } + size_t cols() const { return NumCols; } - void resize(int num_rows, int num_cols) { - // Resizing of fixed size matrices not allowed + void resize(int num_rows, int num_cols) { + // Resizing of fixed size matrices not allowed #ifndef NDEBUG - if (num_rows != NumRows || num_cols != NumCols) { - std::cout << "Error: trying to resize fixed matrix from " - << NumRows << ", " << NumCols << " to " - << num_rows << ", " << num_cols << "." << std::endl; - } + if (num_rows != NumRows || num_cols != NumCols) { + std::cout << "Error: trying to resize fixed matrix from " + << NumRows << ", " << NumCols << " to " + << num_rows << ", " << num_cols << "." << std::endl; + } #endif - assert (num_rows == NumRows && num_cols == NumCols); - } + assert (num_rows == NumRows && num_cols == NumCols); + } - ScalarType& coeff(int row_index, int col_index) { - assert (row_index >= 0 && row_index <= NumRows); - assert (col_index >= 0 && col_index <= NumCols); - return mData[row_index * NumCols + col_index]; - } + ScalarType& coeff(int row_index, int col_index) { + assert (row_index >= 0 && row_index <= NumRows); + assert (col_index >= 0 && col_index <= NumCols); + return mData[row_index * NumCols + col_index]; + } - const ScalarType& coeff(int row_index, int col_index) const { - assert (row_index >= 0 && row_index <= NumRows); - assert (col_index >= 0 && col_index <= NumCols); - return mData[row_index * NumCols + col_index]; - } + const ScalarType& coeff(int row_index, int col_index) const { + assert (row_index >= 0 && row_index <= NumRows); + assert (col_index >= 0 && col_index <= NumCols); + return mData[row_index * NumCols + col_index]; + } }; template @@ -565,275 +783,399 @@ struct Storage { template -struct Matrix : public MatrixBase, ScalarType, NumRows, NumCols>{ - enum { - RowsAtCompileTime = (NumCols == Dynamic || NumRows == Dynamic) ? -1 : NumRows, - ColsAtCompileTime = (NumCols == Dynamic || NumRows == Dynamic) ? -1 : NumCols, - SizeAtCompileTime = (NumRows != Dynamic && NumCols != Dynamic) ? NumRows * NumCols : 0 - }; +struct Matrix : public MatrixBase, ScalarType, NumRows, NumCols> { + typedef Matrix DerivedBase; - Storage mStorage; + enum { + RowsAtCompileTime = (NumCols == Dynamic || NumRows == Dynamic) ? -1 : NumRows, + ColsAtCompileTime = (NumCols == Dynamic || NumRows == Dynamic) ? -1 : NumCols, + SizeAtCompileTime = (NumRows != Dynamic && NumCols != Dynamic) ? NumRows * NumCols : 0 + }; - Matrix() : - mStorage ( - SizeAtCompileTime / ColsAtCompileTime, - SizeAtCompileTime / RowsAtCompileTime - ) {} + Storage mStorage; - explicit Matrix(int rows, int cols) : - mStorage(rows, cols) {} + Matrix() : + mStorage ( + SizeAtCompileTime / ColsAtCompileTime, + SizeAtCompileTime / RowsAtCompileTime + ) {} - explicit Matrix (size_t rows, size_t cols) : - mStorage(rows, cols) {} + explicit Matrix(int rows, int cols = 1) : + mStorage(rows, cols) {} - template - Matrix(const MatrixBase& other) { - mStorage.resize(other.rows(), other.cols()); + explicit Matrix(unsigned int rows, unsigned int cols = 1) : + mStorage(rows, cols) {} - for (size_t i = 0; i < rows(); i++) { - for (size_t j = 0; j < cols(); j++) { - this->operator()(i,j) = other(i,j); - } - } - } + explicit Matrix (size_t rows, size_t cols = 1) : + mStorage(rows, cols) {} - // - // Constructor for vectors - // + template + Matrix(const MatrixBase &other) { + mStorage.resize(other.rows(), other.cols()); - Matrix ( - const ScalarType& v0 - ) { - static_assert (NumRows * NumCols == 1, "Invalid matrix size"); + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < cols(); j++) { + this->operator()(i, j) = other(i, j); + } + } + } - operator()(0,0) = v0; + Matrix (const Matrix& other) : + mStorage(other.rows(), other.cols()){ + memcpy (data(), other.data(), sizeof (ScalarType) * rows() * cols()); + } + + + Matrix& operator=(const Matrix& other) { + if (&other != this) { + mStorage.resize(other.rows(), other.cols()); + memcpy (data(), other.data(), sizeof (ScalarType) * rows() * cols()); + } + return *this; + } + + // + // Constructor for vectors + // + Matrix ( + const ScalarType& v0 + ) { + static_assert (NumRows * NumCols == 1, "Invalid matrix size"); + + operator()(0,0) = v0; + } + + Matrix ( + const ScalarType& v0, + const ScalarType& v1 + ) { + static_assert (NumRows * NumCols == 2, "Invalid matrix size"); + + operator()(0,0) = v0; + operator()(1,0) = v1; + } + + Matrix ( + const ScalarType& v0, + const ScalarType& v1, + const ScalarType& v2 + ) { + static_assert (NumRows * NumCols == 3, "Invalid matrix size"); + + operator()(0,0) = v0; + operator()(1,0) = v1; + operator()(2,0) = v2; + } + + Matrix ( + const ScalarType& v0, + const ScalarType& v1, + const ScalarType& v2, + const ScalarType& v3 + ) { + static_assert (NumRows * NumCols == 4, "Invalid matrix size"); + + operator()(0,0) = v0; + operator()(1,0) = v1; + operator()(2,0) = v2; + operator()(3,0) = v3; + } + + Matrix ( + const ScalarType& v0, + const ScalarType& v1, + const ScalarType& v2, + const ScalarType& v3, + const ScalarType& v4, + const ScalarType& v5 + ) { + static_assert (NumRows * NumCols == 6, "Invalid matrix size"); + + operator()(0,0) = v0; + operator()(1,0) = v1; + operator()(2,0) = v2; + operator()(3,0) = v3; + operator()(4,0) = v4; + operator()(5,0) = v5; + } + + // + // Constructor for matrices + // + Matrix ( + const ScalarType& v00, + const ScalarType& v01, + const ScalarType& v02, + const ScalarType& v10, + const ScalarType& v11, + const ScalarType& v12, + const ScalarType& v20, + const ScalarType& v21, + const ScalarType& v22 + ) { + static_assert (NumRows == 3 && NumCols == 3, "Invalid matrix size"); + + operator()(0,0) = v00; + operator()(0,1) = v01; + operator()(0,2) = v02; + + operator()(1,0) = v10; + operator()(1,1) = v11; + operator()(1,2) = v12; + + operator()(2,0) = v20; + operator()(2,1) = v21; + operator()(2,2) = v22; + } + + Matrix ( + const ScalarType& v00, + const ScalarType& v01, + const ScalarType& v02, + const ScalarType& v03, + const ScalarType& v10, + const ScalarType& v11, + const ScalarType& v12, + const ScalarType& v13, + const ScalarType& v20, + const ScalarType& v21, + const ScalarType& v22, + const ScalarType& v23, + const ScalarType& v30, + const ScalarType& v31, + const ScalarType& v32, + const ScalarType& v33 + ) { + static_assert (NumRows == 4 && NumCols == 4, "Invalid matrix size"); + + operator()(0,0) = v00; + operator()(0,1) = v01; + operator()(0,2) = v02; + operator()(0,3) = v03; + + operator()(1,0) = v10; + operator()(1,1) = v11; + operator()(1,2) = v12; + operator()(1,3) = v13; + + operator()(2,0) = v20; + operator()(2,1) = v21; + operator()(2,2) = v22; + operator()(2,3) = v23; + + operator()(3,0) = v30; + operator()(3,1) = v31; + operator()(3,2) = v32; + operator()(3,3) = v33; + } + + Matrix ( + const ScalarType& v00, + const ScalarType& v01, + const ScalarType& v02, + const ScalarType& v03, + const ScalarType& v04, + const ScalarType& v05, + + const ScalarType& v10, + const ScalarType& v11, + const ScalarType& v12, + const ScalarType& v13, + const ScalarType& v14, + const ScalarType& v15, + + const ScalarType& v20, + const ScalarType& v21, + const ScalarType& v22, + const ScalarType& v23, + const ScalarType& v24, + const ScalarType& v25, + + const ScalarType& v30, + const ScalarType& v31, + const ScalarType& v32, + const ScalarType& v33, + const ScalarType& v34, + const ScalarType& v35, + + const ScalarType& v40, + const ScalarType& v41, + const ScalarType& v42, + const ScalarType& v43, + const ScalarType& v44, + const ScalarType& v45, + + const ScalarType& v50, + const ScalarType& v51, + const ScalarType& v52, + const ScalarType& v53, + const ScalarType& v54, + const ScalarType& v55 + ) { + static_assert (NumRows == 6 && NumCols == 6, "Invalid matrix size"); + + operator()(0,0) = v00; + operator()(0,1) = v01; + operator()(0,2) = v02; + operator()(0,3) = v03; + operator()(0,4) = v04; + operator()(0,5) = v05; + + operator()(1,0) = v10; + operator()(1,1) = v11; + operator()(1,2) = v12; + operator()(1,3) = v13; + operator()(1,4) = v14; + operator()(1,5) = v15; + + operator()(2,0) = v20; + operator()(2,1) = v21; + operator()(2,2) = v22; + operator()(2,3) = v23; + operator()(2,4) = v24; + operator()(2,5) = v25; + + operator()(3,0) = v30; + operator()(3,1) = v31; + operator()(3,2) = v32; + operator()(3,3) = v33; + operator()(3,4) = v34; + operator()(3,5) = v35; + + operator()(4,0) = v40; + operator()(4,1) = v41; + operator()(4,2) = v42; + operator()(4,3) = v43; + operator()(4,4) = v44; + operator()(4,5) = v45; + + operator()(5,0) = v50; + operator()(5,1) = v51; + operator()(5,2) = v52; + operator()(5,3) = v53; + operator()(5,4) = v54; + operator()(5,5) = v55; + } + + template + Matrix& operator+=(const OtherDerived& other) { + assert (rows() == other.rows() && cols() == other.cols() && "Error: matrix dimensions do not match!"); + + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < cols(); j++) { + this->operator()(i,j) += other(i,j); + } + } + return *this; } - Matrix ( - const ScalarType& v0, - const ScalarType& v1 - ) { - static_assert (NumRows * NumCols == 2, "Invalid matrix size"); + template + Matrix& operator-=(const OtherDerived& other) { + assert (rows() == other.rows() && cols() == other.cols() && "Error: matrix dimensions do not match!"); - operator()(0,0) = v0; - operator()(1,0) = v1; + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < cols(); j++) { + this->operator()(i,j) -= other(i,j); + } + } + return *this; } - Matrix ( - const ScalarType& v0, - const ScalarType& v1, - const ScalarType& v2 - ) { - static_assert (NumRows * NumCols == 3, "Invalid matrix size"); + ScalarType& operator()(const size_t& i, const size_t& j) { + return mStorage.coeff(i, j); + } - operator()(0,0) = v0; - operator()(1,0) = v1; - operator()(2,0) = v2; - } + ScalarType* data() { + return mStorage.mData; + } - Matrix ( - const ScalarType& v0, - const ScalarType& v1, - const ScalarType& v2, - const ScalarType& v3 - ) { - static_assert (NumRows * NumCols == 4, "Invalid matrix size"); + const ScalarType* data() const { + return mStorage.mData; + } - operator()(0,0) = v0; - operator()(1,0) = v1; - operator()(2,0) = v2; - operator()(3,0) = v3; - } + const ScalarType& operator()(const size_t& i, const size_t& j) const { + return mStorage.coeff(i, j); + } - // - // Constructor for matrices - // - Matrix ( - const ScalarType& v00, - const ScalarType& v01, - const ScalarType& v02, - const ScalarType& v10, - const ScalarType& v11, - const ScalarType& v12, - const ScalarType& v20, - const ScalarType& v21, - const ScalarType& v22 - ) { - static_assert (NumRows == 3 && NumCols == 3, "Invalid matrix size"); + size_t cols() const { + return mStorage.cols(); + } - operator()(0,0) = v00; - operator()(0,1) = v01; - operator()(0,2) = v02; - - operator()(1,0) = v10; - operator()(1,1) = v11; - operator()(1,2) = v12; - - operator()(2,0) = v20; - operator()(2,1) = v21; - operator()(2,2) = v22; - } - - Matrix ( - const ScalarType& v00, - const ScalarType& v01, - const ScalarType& v02, - const ScalarType& v03, - const ScalarType& v10, - const ScalarType& v11, - const ScalarType& v12, - const ScalarType& v13, - const ScalarType& v20, - const ScalarType& v21, - const ScalarType& v22, - const ScalarType& v23, - const ScalarType& v30, - const ScalarType& v31, - const ScalarType& v32, - const ScalarType& v33 - ) { - static_assert (NumRows == 4 && NumCols == 4, "Invalid matrix size"); - - operator()(0,0) = v00; - operator()(0,1) = v01; - operator()(0,2) = v02; - operator()(0,3) = v03; - - operator()(1,0) = v10; - operator()(1,1) = v11; - operator()(1,2) = v12; - operator()(1,3) = v13; - - operator()(2,0) = v20; - operator()(2,1) = v21; - operator()(2,2) = v22; - operator()(2,3) = v23; - - operator()(3,0) = v30; - operator()(3,1) = v31; - operator()(3,2) = v32; - operator()(3,3) = v33; - } - - template - Matrix& operator+=(const OtherDerived& other) { - assert (NumRows == other.rows() && NumCols == other.cols() && "Error: matrix dimensions do not match!"); - - for (size_t i = 0; i < rows(); i++) { - for (size_t j = 0; j < cols(); j++) { - this->operator()(i,j) += other(i,j); - } - } - return *this; - } - - template - Matrix& operator-=(const OtherDerived& other) { - assert (rows() == other.rows() && cols() == other.cols() && "Error: matrix dimensions do not match!"); - - for (size_t i = 0; i < rows(); i++) { - for (size_t j = 0; j < cols(); j++) { - this->operator()(i,j) -= other(i,j); - } - } - return *this; - } - - ScalarType& operator()(const size_t& i, const size_t& j) { - return mStorage.coeff(i, j); - } - - ScalarType* data() { - return mStorage.mData; - } - - const ScalarType* data() const { - return mStorage.mData; - } - - const ScalarType& operator()(const size_t& i, const size_t& j) const { - return mStorage.coeff(i, j); - } - - size_t cols() const { - return mStorage.cols(); - } - - size_t rows() const { - return mStorage.rows(); - } + size_t rows() const { + return mStorage.rows(); + } }; - + // // CommaInitializer // template struct CommaInitializer { - typedef typename Derived::value_type value_type; + typedef typename Derived::value_type value_type; - private: - CommaInitializer() {} + private: + CommaInitializer() {} - Derived *mParentMatrix; - unsigned int mRowIndex; - unsigned int mColIndex; - bool mElementWasAdded; + Derived *mParentMatrix; + unsigned int mRowIndex; + unsigned int mColIndex; + bool mElementWasAdded; - public: + public: - CommaInitializer(Derived &matrix, const value_type &value) : - mParentMatrix(&matrix), - mRowIndex(0), - mColIndex(0), - mElementWasAdded(false) - { - assert (matrix.rows() > 0 && matrix.cols() > 0); - mParentMatrix->operator()(0,0) = value; - } + CommaInitializer(Derived &matrix, const value_type &value) : + mParentMatrix(&matrix), + mRowIndex(0), + mColIndex(0), + mElementWasAdded(false) + { + assert (matrix.rows() > 0 && matrix.cols() > 0); + mParentMatrix->operator()(0,0) = value; + } - CommaInitializer(Derived &matrix, unsigned int row_index, unsigned int col_index) : - mParentMatrix(&matrix), - mRowIndex(row_index), - mColIndex(col_index), - mElementWasAdded(false) - { - assert (matrix.rows() > 0 && matrix.cols() > 0); - } + CommaInitializer(Derived &matrix, unsigned int row_index, unsigned int col_index) : + mParentMatrix(&matrix), + mRowIndex(row_index), + mColIndex(col_index), + mElementWasAdded(false) + { + assert (matrix.rows() > 0 && matrix.cols() > 0); + } - ~CommaInitializer() { - if (!mElementWasAdded - && (mColIndex + 1 < mParentMatrix->cols() - || mRowIndex + 1 < mParentMatrix->rows())) { - std::cerr - << "Error: too few elements passed to CommaInitializer Expected " - << mParentMatrix->rows() * mParentMatrix->cols() - << " but was given " - << mRowIndex * mParentMatrix->cols() + mColIndex + 1 << std::endl; - abort(); - } - } + ~CommaInitializer() { + if (!mElementWasAdded + && (mColIndex + 1 < mParentMatrix->cols() + || mRowIndex + 1 < mParentMatrix->rows())) { + std::cerr + << "Error: too few elements passed to CommaInitializer Expected " + << mParentMatrix->rows() * mParentMatrix->cols() + << " but was given " + << mRowIndex * mParentMatrix->cols() + mColIndex + 1 << std::endl; + abort(); + } + } - CommaInitializer operator, (const value_type &value) { - mColIndex++; - if (mColIndex >= mParentMatrix->cols()) { - mRowIndex++; - mColIndex = 0; - } + CommaInitializer 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->rows() * mParentMatrix->cols() - << " but was given " - << mRowIndex *mParentMatrix->cols() + mColIndex + 1 << std::endl; - abort(); - } - (*mParentMatrix)(mRowIndex, mColIndex) = value; - mElementWasAdded = true; + if (mRowIndex == mParentMatrix->rows() && mColIndex == 0) { + std::cerr + << "Error: too many elements passed to CommaInitializer!Expected " + << mParentMatrix->rows() * mParentMatrix->cols() + << " but was given " + << mRowIndex *mParentMatrix->cols() + mColIndex + 1 << std::endl; + abort(); + } + (*mParentMatrix)(mRowIndex, mColIndex) = value; + mElementWasAdded = true; - return CommaInitializer(*mParentMatrix, mRowIndex, mColIndex); - } + return CommaInitializer(*mParentMatrix, mRowIndex, mColIndex); + } }; // @@ -841,61 +1183,61 @@ struct CommaInitializer { // template struct Transpose : public MatrixBase, ScalarType, NumRows, NumCols> { - Derived* mTransposeSource; + Derived* mTransposeSource; - Transpose(Derived* transpose_source) : - mTransposeSource(transpose_source) - { } + Transpose(Derived* transpose_source) : + mTransposeSource(transpose_source) + { } - Transpose(const Transpose &other) : - mTransposeSource(other.mTransposeSource) - { } + Transpose(const Transpose &other) : + mTransposeSource(other.mTransposeSource) + { } template Matrix operator*(const MatrixBase& other) const { - Matrix result (Matrix::Zero(rows(), other.cols())); + Matrix result (Matrix::Zero(rows(), other.cols())); - unsigned int i,j,k; - unsigned int nrows = rows(); - unsigned int other_ncols = other.cols(); - unsigned int other_nrows = other.rows(); + unsigned int i,j,k; + unsigned int nrows = rows(); + unsigned int other_ncols = other.cols(); + unsigned int other_nrows = other.rows(); - for (i = 0; i < nrows; i++) { - for (j = 0; j < other_ncols; j++) { - for (k = 0; k < other_nrows; k++) { - result (i,j) += operator()(i,k) * other(k,j); - } - } - } - return result; - } + for (i = 0; i < nrows; i++) { + for (j = 0; j < other_ncols; j++) { + for (k = 0; k < other_nrows; k++) { + result (i,j) += operator()(i,k) * other(k,j); + } + } + } + return result; + } - template - Transpose& operator=(const MatrixBase& other) { - if (static_cast(this) != static_cast(&other)) { - for (size_t i = 0; i < other.rows(); i++) { - for (size_t j = 0; j < other.cols(); j++) { - this->operator()(i,j) = other(i,j); - } - } - } + template + Transpose& operator=(const MatrixBase& other) { + if (static_cast(this) != static_cast(&other)) { + for (size_t i = 0; i < other.rows(); i++) { + for (size_t j = 0; j < other.cols(); j++) { + this->operator()(i,j) = other(i,j); + } + } + } - return *this; - } + return *this; + } - size_t rows() const { - return static_cast(mTransposeSource)->cols(); - } - size_t cols() const { - return static_cast(mTransposeSource)->rows(); - } + size_t rows() const { + return static_cast(mTransposeSource)->cols(); + } + size_t cols() const { + return static_cast(mTransposeSource)->rows(); + } - const ScalarType& operator()(const size_t& i, const size_t& j) const { - return static_cast(mTransposeSource)->operator()(j, i); - } - ScalarType& operator()(const size_t& i, const size_t& j) { - return static_cast(mTransposeSource)->operator()(j, i); - } + const ScalarType& operator()(const size_t& i, const size_t& j) const { + return static_cast(mTransposeSource)->operator()(j, i); + } + ScalarType& operator()(const size_t& i, const size_t& j) { + return static_cast(mTransposeSource)->operator()(j, i); + } }; // @@ -903,115 +1245,126 @@ struct Transpose : public MatrixBase struct Block : public MatrixBase, ScalarType, NumRows, NumCols> { - typedef Block matrix_type; + typedef Block matrix_type; - Derived* mBlockSource; - int row_index; - int col_index; - int nrows; - int ncols; + Derived* mBlockSource; + int row_index; + int col_index; + int nrows; + int ncols; - Block(Derived* block_source, int row_index, int col_index) : - mBlockSource(block_source), - row_index(row_index), - col_index(col_index), - nrows(NumRows), ncols(NumCols) - { - static_assert(NumRows != -1 && NumCols != -1, "Invalid block specifications: unknown number of rows and columns!"); - } + Block(Derived* block_source, int row_index, int col_index) : + mBlockSource(block_source), + row_index(row_index), + col_index(col_index), + nrows(NumRows), ncols(NumCols) + { + static_assert(NumRows != -1 && NumCols != -1, "Invalid block specifications: unknown number of rows and columns!"); + } - Block(Derived* block_source, int row_index, int col_index, int num_rows, int num_cols) : - mBlockSource(block_source), - row_index(row_index), - col_index(col_index), - nrows (num_rows), - ncols (num_cols) - { - } + Block(Derived* block_source, int row_index, int col_index, int num_rows, int num_cols) : + mBlockSource(block_source), + row_index(row_index), + col_index(col_index), + nrows (num_rows), + ncols (num_cols) + { + } - Block(const Block &other) : - mBlockSource(other.mBlockSource), - row_index(other.row_index), - col_index(other.col_index) - { } + Block(const Block &other) : + mBlockSource(other.mBlockSource), + row_index(other.row_index), + col_index(other.col_index) + { } - Block& operator=(const Block &other) { - unsigned int i,j; - for (i = 0; i < rows(); i++) { - for (j = 0; j < cols(); j++) { - this->operator()(i,j) = other(i,j); - } - } + Block& operator=(const Block &other) { + unsigned int i,j; + for (i = 0; i < rows(); i++) { + for (j = 0; j < cols(); j++) { + this->operator()(i,j) = other(i,j); + } + } - return *this; - } + return *this; + } - template - Block& operator=(const MatrixBase& other) { - unsigned int i,j; - for (i = 0; i < rows(); i++) { - for (j = 0; j < cols(); j++) { - this->operator()(i,j) = other(i,j); - } - } + template + Block& operator=(const MatrixBase& other) { + unsigned int i,j; + for (i = 0; i < rows(); i++) { + for (j = 0; j < cols(); j++) { + this->operator()(i,j) = other(i,j); + } + } - return *this; - } + return *this; + } - template - Matrix operator*(const MatrixBase& other) const { - Matrix result (rows(), other.cols()); +// template +// Matrix& operator=(const MatrixBase& other) { +// unsigned int i,j,k; +// for (i = 0; i < rows(); i++) { +// for (j = 0; j < other.cols(); j++) { +// operator()(i,k) = other(k,j); +// } +// } +// return *this; +// } - unsigned int i,j,k; - for (i = 0; i < rows(); i++) { - for (j = 0; j < other.cols(); j++) { - for (k = 0; k < other.rows(); k++) { - result (i,j) += operator()(i,k) * other(k,j); - } - } - } - return result; - } + template + Matrix operator*(const MatrixBase& other) const { + Matrix result (rows(), other.cols()); - size_t rows() const { - return nrows; - } - size_t cols() const { - return ncols; - } + unsigned int i,j,k; + for (i = 0; i < rows(); i++) { + for (j = 0; j < other.cols(); j++) { + for (k = 0; k < other.rows(); k++) { + result (i,j) += operator()(i,k) * other(k,j); + } + } + } + return result; + } - const ScalarType& operator()(const size_t& i, const size_t& j) const { - return static_cast(mBlockSource)->operator()(row_index + i, col_index + j); - } - ScalarType& operator()(const size_t& i, const size_t& j) { - return static_cast(mBlockSource)->operator()(row_index + i,col_index + j); - } + size_t rows() const { + return nrows; + } + size_t cols() const { + return ncols; + } - template - Matrix operator+(const MatrixBase& other) const { - Matrix result (rows(), other.cols()); + const ScalarType& operator()(const size_t& i, const size_t& j) const { + return static_cast(mBlockSource)->operator()(row_index + i, col_index + j); + } + ScalarType& operator()(const size_t& i, const size_t& j) { + return static_cast(mBlockSource)->operator()(row_index + i,col_index + j); + } - unsigned int i,j,k; - for (i = 0; i < rows(); i++) { - for (j = 0; j < other.cols(); j++) { - result (i,j) = operator()(i,j) + other(i,j); - } - } - return result; - } + template + Matrix operator+(const MatrixBase& other) const { + Matrix result (rows(), other.cols()); - private: - Block() { assert(0 && "Invalid call!"); }; + unsigned int i,j,k; + for (i = 0; i < rows(); i++) { + for (j = 0; j < other.cols(); j++) { + result (i,j) = operator()(i,j) + other(i,j); + } + } + return result; + } - ScalarType* data() { - assert("invalid call"); - return NULL; - } + private: + Block() { assert(0 && "Invalid call!"); }; - const ScalarType* data() const { - assert("invalid call"); - return NULL; - } + ScalarType* data() { + assert("invalid call"); + return NULL; + } + + const ScalarType* data() const { + assert("invalid call"); + return NULL; + } }; // @@ -1030,20 +1383,20 @@ public: private: typedef Matrix VectorXd; typedef Matrix MatrixXXd; - typedef Matrix ColumnVector; + typedef Matrix ColumnVector; bool mIsFactorized; Matrix mQ; Derived mR; public: - HouseholderQR(const MatrixType &matrix) : + HouseholderQR(const Derived &matrix) : mIsFactorized(false), mQ(matrix.rows(), matrix.rows()) { compute(matrix); } - HouseholderQR compute(const MatrixType &matrix) { + HouseholderQR compute(const Derived& matrix) { mR = matrix; mQ = MatrixType::Identity (mR.rows(), mR.rows()); @@ -1081,34 +1434,34 @@ public: mIsFactorized = true; return *this; - } - ColumnVector solve ( - const ColumnVector &rhs - ) const { - assert (mIsFactorized); + } + ColumnVector solve ( + const ColumnVector &rhs + ) const { + assert (mIsFactorized); - ColumnVector y = mQ.transpose() * rhs; - ColumnVector x = ColumnVector::Zero(mR.cols()); + ColumnVector y = mQ.transpose() * rhs; + ColumnVector x = ColumnVector::Zero(mR.cols()); - int ncols = mR.cols(); - for (int i = ncols - 1; i >= 0; i--) { - value_type z = y[i]; + int ncols = mR.cols(); + for (int i = ncols - 1; i >= 0; i--) { + value_type z = y[i]; - for (unsigned int j = i + 1; j < ncols; j++) { - z = z - x[j] * mR(i,j); - } + for (unsigned int j = i + 1; j < ncols; j++) { + z = z - x[j] * mR(i,j); + } - if (fabs(mR(i,i)) < std::numeric_limits::epsilon() * 10) { - std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero!" << std::endl; - abort(); - } - x[i] = z / mR(i,i); - } + if (fabs(mR(i,i)) < std::numeric_limits::epsilon() * 10) { + std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero!" << std::endl; + abort(); + } + x[i] = z / mR(i,i); + } - assert (!std::isnan(x.squaredNorm())); + assert (!std::isnan(x.squaredNorm())); - return x; - } + return x; + } Derived inverse() const { assert (mIsFactorized); @@ -1143,7 +1496,7 @@ public: private: typedef Matrix VectorXd; typedef Matrix MatrixXXd; - typedef Matrix ColumnVector; + typedef Matrix ColumnVector; bool mIsFactorized; Matrix mQ; @@ -1201,100 +1554,100 @@ public: return *this; } - ColPivHouseholderQR& compute(const MatrixType &matrix) { - mR = matrix; - mQ = MatrixType::Identity (mR.rows(), mR.rows()); + ColPivHouseholderQR& compute(const MatrixType &matrix) { + mR = matrix; + mQ = MatrixType::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; + 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(); + // 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); + 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 (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_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; + 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; - } + 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); + 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; - } + 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; + VectorXd v = current_block.block(0, 0, block_rows, 1); + v[0] = v[0] - alpha; - MatrixXXd Q (MatrixXXd::Identity(mR.rows(), mR.rows())); + 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)) - - (v * v.transpose()) / (v.squaredNorm() * static_cast(0.5)); + Q.block(i, i, block_rows, block_rows) = MatrixXXd (Q.block(i, i, block_rows, block_rows)) + - (v * v.transpose()) / (v.squaredNorm() * static_cast(0.5)); - mR = Q * mR; + 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.; - } + // 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; - } + mQ = mQ * Q; + } - mIsFactorized = true; + mIsFactorized = true; - return *this; - } - ColumnVector solve ( - const ColumnVector &rhs - ) const { - assert (mIsFactorized); + return *this; + } + ColumnVector solve ( + const ColumnVector &rhs + ) const { + assert (mIsFactorized); - ColumnVector y = mQ.transpose() * rhs; - ColumnVector x = ColumnVector::Zero(mR.cols()); + ColumnVector y = mQ.transpose() * rhs; + ColumnVector x = ColumnVector::Zero(mR.cols()); - for (int i = mR.cols() - 1; i >= 0; --i) { - value_type z = y[i]; + 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); - } + 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::epsilon() * 10) { - std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero!" << std::endl; - abort(); - } - x[mPermutations[i]] = z / mR(i,i); - } + if (fabs(mR(i,i)) < std::numeric_limits::epsilon() * 10) { + std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero!" << std::endl; + abort(); + } + x[mPermutations[i]] = z / mR(i,i); + } - assert (!std::isnan(x.squaredNorm())); + assert (!std::isnan(x.squaredNorm())); - return x; - } + return x; + } Derived inverse() const { assert (mIsFactorized); @@ -1339,6 +1692,26 @@ public: } }; +template +inline Matrix operator*(const ScalarType& scalar, const MatrixBase &matrix) { + return matrix * scalar; +} + +template +inline Matrix operator*(const MatrixBase &matrix, const ScalarType& scalar) { + return matrix * scalar; +} + +template +inline Matrix operator/(const MatrixBase &matrix, const ScalarType& scalar) { + return matrix * (1.0 / scalar); +} + +template +inline Matrix operator/=(MatrixBase &matrix, const ScalarType& scalar) { + return matrix *= (1.0 / scalar); +} + // // OpenGL Matrices and Quaternions // @@ -1352,124 +1725,124 @@ typedef Matrix Vector4f; typedef Matrix Matrix44f; 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, + 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 * 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 + 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, + 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 * 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, + 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 - ); + 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 - ); + 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 - ); + 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 - ); + 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); + 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 - ); + 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); + 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 - ); + 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(); + 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 - ); + 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 + ); } // @@ -1478,201 +1851,201 @@ inline Matrix44f LookAt( // 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; - } + 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) - ); - } + 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 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()); + 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); + // 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 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); + 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); - } + 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, + 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*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); - } + 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 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 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 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 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 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]); - } + 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 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] - ) - ); - }; + 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, + 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*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 - ); - } + 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]); - } + 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; + 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(); + res_quat = (*this) * vec_quat; + res_quat = res_quat * conjugate(); - return Vector3f (res_quat[0], res_quat[1], res_quat[2]); - } + return Vector3f (res_quat[0], res_quat[1], res_quat[2]); + } }; } /* namespace GL */ @@ -1683,42 +2056,40 @@ class Quaternion : public Vector4f { // template inline std::ostream& operator<<(std::ostream& output, const MatrixBase &matrix) { - size_t max_width = 0; - size_t out_width = output.width(); + 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); - } - } + // 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; - } + // 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(); + for (unsigned int i = 0; i < matrix.rows(); i++) { + output.width(0); + 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; + if (j < matrix.cols() - 1) + output << " "; + } + + if (matrix.rows() > 1 && i < matrix.rows() - 1) + output << std::endl; + } + return output; } } diff --git a/src/modules/RenderModule.cc b/src/modules/RenderModule.cc index 1e5f326..5a2775b 100644 --- a/src/modules/RenderModule.cc +++ b/src/modules/RenderModule.cc @@ -284,9 +284,9 @@ void Light::UpdateSplits(const Camera& camera) { Matrix44f light_matrix = LookAt (mPosition, mPosition + mDirection, Vector3f (0.f, 1.0f, 0.0f)); Matrix44f light_matrix_inv = light_matrix.inverse(); - mShadowSplits[0] = near + length * 0.02; - mShadowSplits[1] = near + length * 0.1; - mShadowSplits[2] = near + length * 0.3; + mShadowSplits[0] = near + length * 0.05; + mShadowSplits[1] = near + length * 0.15; + mShadowSplits[2] = near + length * 0.4; mShadowSplits[3] = far; float prev_split_far = near;