1439 lines
40 KiB
C++
1439 lines
40 KiB
C++
/*! \file pimathmatrix.h
|
||
* \brief PIMathMatrix
|
||
*
|
||
* This file declare math matrix class, which performs various matrix operations
|
||
*/
|
||
/*
|
||
PIP - Platform Independent Primitives
|
||
PIMathMatrix
|
||
Ivan Pelipenko peri4ko@yandex.ru, Andrey Bychkov work.a.b@yandex.ru
|
||
|
||
This program is free software: you can redistribute it and/or modify
|
||
it under the terms of the GNU Lesser General Public License as published by
|
||
the Free Software Foundation, either version 3 of the License, or
|
||
(at your option) any later version.
|
||
|
||
This program is distributed in the hope that it will be useful,
|
||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||
GNU Lesser General Public License for more details.
|
||
|
||
You should have received a copy of the GNU Lesser General Public License
|
||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||
*/
|
||
|
||
#ifndef PIMATHMATRIX_H
|
||
#define PIMATHMATRIX_H
|
||
|
||
#include "pimathvector.h"
|
||
#include "pimathcomplex.h"
|
||
|
||
|
||
/// Matrix templated
|
||
|
||
#define PIMM_FOR(r, c) for (uint c = 0; c < Cols; ++c) { for (uint r = 0; r < Rows; ++r) {
|
||
#define PIMM_FOR_WB(r, c) for (uint c = 0; c < Cols; ++c) for (uint r = 0; r < Rows; ++r) // without brakes
|
||
#define PIMM_FOR_I(r, c) for (uint r = 0; r < Rows; ++r) { for (uint c = 0; c < Cols; ++c) {
|
||
#define PIMM_FOR_I_WB(r, c) for (uint r = 0; r < Rows; ++r) for (uint c = 0; c < Cols; ++c) // without brakes
|
||
#define PIMM_FOR_C(v) for (uint v = 0; v < Cols; ++v)
|
||
#define PIMM_FOR_R(v) for (uint v = 0; v < Rows; ++v)
|
||
|
||
#pragma pack(push, 1)
|
||
|
||
//! \brief A class that works with square matrix operations, the input data of which are columns, rows and the data type of the matrix
|
||
//! @tparam Rows rows number of matrix
|
||
//! @tparam Сols columns number of matrix
|
||
//! @tparam Type is the data type of the matrix. There are can be basic C++ language data and different classes where the arithmetic operators(=, +=, -=, *=, /=, ==, !=, +, -, *, /)
|
||
//! of the C++ language are implemented
|
||
template<uint Rows, uint Cols = Rows, typename Type = double>
|
||
class PIP_EXPORT PIMathMatrixT {
|
||
typedef PIMathMatrixT<Rows, Cols, Type> _CMatrix;
|
||
typedef PIMathMatrixT<Cols, Rows, Type> _CMatrixI;
|
||
typedef PIMathVectorT<Rows, Type> _CMCol;
|
||
typedef PIMathVectorT<Cols, Type> _CMRow;
|
||
static_assert(std::is_arithmetic<Type>::value, "Type must be arithmetic");
|
||
static_assert(Rows > 0, "Row count must be > 0");
|
||
static_assert(Cols > 0, "Column count must be > 0");
|
||
public:
|
||
/**
|
||
* @brief Constructor that calls the private resize method
|
||
*
|
||
* @return identitied matrix of type PIMathMatrixT
|
||
*/
|
||
PIMathMatrixT() { resize(Rows, Cols); }
|
||
|
||
/**
|
||
* @brief Constructor that calls the private resize method
|
||
*
|
||
* @param val is the PIVector with which the matrix is filled
|
||
* @return identitied matrix of type PIMathMatrixT
|
||
*/
|
||
PIMathMatrixT(const PIVector<Type> &val) {
|
||
resize(Rows, Cols);
|
||
int i = 0;
|
||
PIMM_FOR_I_WB(r, c) m[r][c] = val[i++];
|
||
}
|
||
|
||
/**
|
||
* @brief Сreates a matrix whose main diagonal is filled with ones and the remaining elements are zeros
|
||
*
|
||
* @return identity matrix of type PIMathMatrixT
|
||
*/
|
||
static _CMatrix identity() {
|
||
_CMatrix tm = _CMatrix();
|
||
PIMM_FOR_WB(r, c) tm.m[r][c] = (c == r ? Type(1) : Type(0));
|
||
return tm;
|
||
}
|
||
|
||
/**
|
||
* @brief Creates a matrix that is filled with elements
|
||
*
|
||
* @param v is a parameter the type and value of which is selected and later filled into the matrix
|
||
* @return filled matrix of type PIMathMatrixT
|
||
*/
|
||
static _CMatrix filled(const Type &v) {
|
||
_CMatrix tm;
|
||
PIMM_FOR_WB(r, c) tm.m[r][c] = v;
|
||
return tm;
|
||
}
|
||
|
||
/**
|
||
* @brief Rotation the matrix by an "angle". Works only with 2x2 matrix,
|
||
* else return default construction of PIMathMatrixT
|
||
*
|
||
* @param angle is the angle of rotation of the matrix
|
||
* @return rotated matrix
|
||
*/
|
||
static _CMatrix rotation(double angle) { return _CMatrix(); }
|
||
|
||
/**
|
||
* @brief Rotation of the matrix by an "angle" along the X axis. Works only with 3x3 matrix,
|
||
* else return default construction of PIMathMatrixT
|
||
*
|
||
* @param angle is the angle of rotation of the matrix along the X axis
|
||
* @return rotated matrix
|
||
*/
|
||
static _CMatrix rotationX(double angle) { return _CMatrix(); }
|
||
|
||
/**
|
||
* @brief Rotation of the matrix by an "angle" along the Y axis. Works only with 3x3 matrix,
|
||
* else return default construction of PIMathMatrixT
|
||
*
|
||
* @param angle is the angle of rotation of the matrix along the Y axis
|
||
* @return rotated matrix
|
||
*/
|
||
static _CMatrix rotationY(double angle) { return _CMatrix(); }
|
||
|
||
/**
|
||
* @brief Rotation of the matrix by an "angle" along the Z axis. Works only with 3x3 matrix,
|
||
* else return default construction of PIMathMatrixT
|
||
*
|
||
* @param angle is the angle of rotation of the matrix along the Z axis
|
||
* @return rotated matrix
|
||
*/
|
||
static _CMatrix rotationZ(double angle) { return _CMatrix(); }
|
||
|
||
/**
|
||
* @brief Scaling the matrix along the X axis by the value "factor". Works only with 3x3 and 2x2 matrix,
|
||
* else return default construction of PIMathMatrixT
|
||
*
|
||
* @param factor is the value of scaling by X axis
|
||
* @return rotated matrix
|
||
*/
|
||
static _CMatrix scaleX(double factor) { return _CMatrix(); }
|
||
|
||
/**
|
||
* @brief Scaling the matrix along the Y axis by the value "factor". Works only with 3x3 and 2x2 matrix,
|
||
* else return default construction of PIMathMatrixT
|
||
*
|
||
* @param factor is the value of scaling by Y axis
|
||
* @return rotated matrix
|
||
*/
|
||
static _CMatrix scaleY(double factor) { return _CMatrix(); }
|
||
|
||
/**
|
||
* @brief Scaling the matrix along the Z axis by the value "factor". Works only with 3x3 matrix,
|
||
* else return default construction of PIMathMatrixT
|
||
*
|
||
* @param factor is the value of scaling by Z axis
|
||
* @return rotated matrix
|
||
*/
|
||
static _CMatrix scaleZ(double factor) { return _CMatrix(); }
|
||
|
||
/**
|
||
* @brief Method which returns number of columns in matrix
|
||
*
|
||
* @return type uint shows number of columns
|
||
*/
|
||
uint cols() const { return Cols; }
|
||
|
||
/**
|
||
* @brief Method which returns number of rows in matrix
|
||
*
|
||
* @return type uint shows number of rows
|
||
*/
|
||
uint rows() const { return Rows; }
|
||
|
||
/**
|
||
* @brief Method which returns the selected column in PIMathVectorT format.
|
||
* If you enter an index out of the border of the matrix there will be "undefined behavior"
|
||
*
|
||
* @param index is the number of the selected column
|
||
* @return column in PIMathVectorT format
|
||
*/
|
||
_CMCol col(uint index) {
|
||
_CMCol tv;
|
||
PIMM_FOR_R(i) tv[i] = m[i][index];
|
||
return tv;
|
||
}
|
||
|
||
/**
|
||
* @brief Method which returns the selected row in PIMathVectorT format
|
||
* If you enter an index out of the border of the matrix there will be "undefined behavior"
|
||
*
|
||
* @param index is the number of the selected row
|
||
* @return row in PIMathVectorT format
|
||
*/
|
||
_CMRow row(uint index) {
|
||
_CMRow tv;
|
||
PIMM_FOR_C(i) tv[i] = m[index][i];
|
||
return tv;
|
||
}
|
||
|
||
/**
|
||
* @brief Set the selected column in matrix.
|
||
* If you enter an index out of the border of the matrix there will be "undefined behavior"
|
||
*
|
||
* @param index is the number of the selected column
|
||
* @param v is a vector of the type _CMCol that needs to fill the column
|
||
* @return matrix type _CMatrix
|
||
*/
|
||
_CMatrix &setCol(uint index, const _CMCol &v) {
|
||
PIMM_FOR_R(i) m[i][index] = v[i];
|
||
return *this;
|
||
}
|
||
|
||
/**
|
||
* @brief Set the selected row in matrix
|
||
* If you enter an index out of the border of the matrix there will be "undefined behavior"
|
||
*
|
||
* @param index is the number of the selected row
|
||
* @param v is a vector of the type _CMCol that needs to fill the row
|
||
* @return matrix type _CMatrix
|
||
*/
|
||
_CMatrix &setRow(uint index, const _CMRow &v) {
|
||
PIMM_FOR_C(i) m[index][i] = v[i];
|
||
return *this;
|
||
}
|
||
|
||
/**
|
||
* @brief Method which changes selected rows in a matrix.
|
||
* If you enter an index out of the border of the matrix there will be "undefined behavior"
|
||
*
|
||
* @param r0 is the number of the first selected row
|
||
* @param r1 is the number of the second selected row
|
||
* @return matrix type _CMatrix
|
||
*/
|
||
_CMatrix &swapRows(uint r0, uint r1) {
|
||
Type t;
|
||
PIMM_FOR_C(i) {
|
||
t = m[r0][i];
|
||
m[r0][i] = m[r1][i];
|
||
m[r1][i] = t;
|
||
}
|
||
return *this;
|
||
}
|
||
|
||
/**
|
||
* @brief Method which changes selected columns in a matrix.
|
||
* If you enter an index out of the border of the matrix there will be "undefined behavior"
|
||
*
|
||
* @param c0 is the number of the first selected column
|
||
* @param c1 is the number of the second selected column
|
||
* @return matrix type _CMatrix
|
||
*/
|
||
_CMatrix &swapCols(uint c0, uint c1) {
|
||
Type t;
|
||
PIMM_FOR_R(i) {
|
||
t = m[i][c0];
|
||
m[i][c0] = m[i][c1];
|
||
m[i][c1] = t;
|
||
}
|
||
return *this;
|
||
}
|
||
|
||
/**
|
||
* @brief Method which fills the matrix with selected value
|
||
*
|
||
* @param v is a parameter the type and value of which is selected and later filled into the matrix
|
||
* @return filled matrix type _CMatrix
|
||
*/
|
||
_CMatrix &fill(const Type &v) {
|
||
PIMM_FOR_WB(r, c) m[r][c] = v;
|
||
return *this;
|
||
}
|
||
|
||
/**
|
||
* @brief Method which checks if matrix is square
|
||
*
|
||
* @return true if matrix is square, else false
|
||
*/
|
||
bool isSquare() const { return cols() == rows(); }
|
||
|
||
/**
|
||
* @brief Method which checks if main diagonal of matrix consists of ones and another elements are zeros
|
||
*
|
||
* @return true if matrix is identitied, else false
|
||
*/
|
||
bool isIdentity() const {
|
||
PIMM_FOR_WB(r, c) if ((c == r) ? m[r][c] != Type(1) : m[r][c] != Type(0)) return false;
|
||
return true;
|
||
}
|
||
|
||
/**
|
||
* @brief Method which checks if every elements of matrix are zeros
|
||
*
|
||
* @return true if matrix is null, else false
|
||
*/
|
||
bool isNull() const {
|
||
PIMM_FOR_WB(r, c) if (m[r][c] != Type(0)) return false;
|
||
return true;
|
||
}
|
||
|
||
/**
|
||
* @brief Full access to elements reference by row "row" and col "col".
|
||
* If you enter an index out of the border of the matrix there will be "undefined behavior"
|
||
*
|
||
* @param row is a parameter that shows the row number of the matrix of the selected element
|
||
* @param col is a parameter that shows the column number of the matrix of the selected element
|
||
* @return reference to element of matrix by row "row" and col "col"
|
||
*/
|
||
Type &at(uint row, uint col) { return m[row][col]; }
|
||
|
||
/**
|
||
* @brief Full access to element by row "row" and col "col".
|
||
* If you enter an index out of the border of the matrix there will be "undefined behavior"
|
||
*
|
||
* @param row is a parameter that shows the row number of the matrix of the selected element
|
||
* @param col is a parameter that shows the column number of the matrix of the selected element
|
||
* @return element of matrix by row "row" and col "col"
|
||
*/
|
||
Type at(uint row, uint col) const { return m[row][col]; }
|
||
|
||
/**
|
||
* @brief Full access to the matrix row pointer. If you enter an index out of the border of the matrix there will be "undefined behavior"
|
||
*
|
||
* @param row is a row of necessary matrix
|
||
* @return matrix row pointer
|
||
*/
|
||
Type *operator[](uint row) { return m[row]; }
|
||
|
||
/**
|
||
* @brief Read-only access to the matrix row pointer. If you enter an index out of the border of the matrix there will be "undefined behavior"
|
||
*
|
||
* @param row is a row of necessary matrix
|
||
* @return matrix row pointer
|
||
*/
|
||
const Type *operator[](uint row) const { return m[row]; }
|
||
|
||
/**
|
||
* @brief Matrix assignment to matrix "sm"
|
||
*
|
||
* @param sm matrix for the assigment
|
||
* @return matrix equal with sm
|
||
*/
|
||
_CMatrix &operator=(const _CMatrix &sm) {
|
||
memcpy(m, sm.m, sizeof(Type) * Cols * Rows);
|
||
return *this;
|
||
}
|
||
|
||
/**
|
||
* @brief Compare with matrix "sm"
|
||
*
|
||
* @param sm matrix for the compare
|
||
* @return if matrices are equal true, else false
|
||
*/
|
||
bool operator==(const _CMatrix &sm) const {
|
||
PIMM_FOR_WB(r, c) if (m[r][c] != sm.m[r][c]) return false;
|
||
return true;
|
||
}
|
||
|
||
/**
|
||
* @brief Compare with matrix "sm"
|
||
*
|
||
* @param sm matrix for the compare
|
||
* @return if matrices are not equal true, else false
|
||
*/
|
||
bool operator!=(const _CMatrix &sm) const { return !(*this == sm); }
|
||
|
||
/**
|
||
* @brief Addition assignment with matrix "sm"
|
||
*
|
||
* @param sm matrix for the addition assigment
|
||
*/
|
||
void operator+=(const _CMatrix &sm) { PIMM_FOR_WB(r, c) m[r][c] += sm.m[r][c]; }
|
||
|
||
/**
|
||
* @brief Subtraction assignment with matrix "sm"
|
||
*
|
||
* @param sm matrix for the subtraction assigment
|
||
*/
|
||
void operator-=(const _CMatrix &sm) { PIMM_FOR_WB(r, c) m[r][c] -= sm.m[r][c]; }
|
||
|
||
/**
|
||
* @brief Multiplication assignment with value "v"
|
||
*
|
||
* @param v value for the multiplication assigment
|
||
*/
|
||
void operator*=(const Type &v) { PIMM_FOR_WB(r, c) m[r][c] *= v; }
|
||
|
||
/**
|
||
* @brief Division assignment with value "v"
|
||
*
|
||
* @param v value for the division assigment
|
||
*/
|
||
void operator/=(const Type &v) { PIMM_FOR_WB(r, c) m[r][c] /= v; }
|
||
|
||
/**
|
||
* @brief Matrix substraction
|
||
*
|
||
* @return the result of matrix substraction
|
||
*/
|
||
_CMatrix operator-() const {
|
||
_CMatrix tm;
|
||
PIMM_FOR_WB(r, c) tm.m[r][c] = -m[r][c];
|
||
return tm;
|
||
}
|
||
|
||
/**
|
||
* @brief Matrix addition
|
||
*
|
||
* @param sm is matrix term
|
||
* @return the result of matrix addition
|
||
*/
|
||
_CMatrix operator+(const _CMatrix &sm) const {
|
||
_CMatrix tm = _CMatrix(*this);
|
||
PIMM_FOR_WB(r, c) tm.m[r][c] += sm.m[r][c];
|
||
return tm;
|
||
}
|
||
|
||
/**
|
||
* @brief Matrix substraction
|
||
*
|
||
* @param sm is matrix subtractor
|
||
* @return the result of matrix substraction
|
||
*/
|
||
_CMatrix operator-(const _CMatrix &sm) const {
|
||
_CMatrix tm = _CMatrix(*this);
|
||
PIMM_FOR_WB(r, c) tm.m[r][c] -= sm.m[r][c];
|
||
return tm;
|
||
}
|
||
|
||
/**
|
||
* @brief Matrix multiplication
|
||
*
|
||
* @param v is value factor
|
||
* @return the result of matrix multiplication
|
||
*/
|
||
_CMatrix operator*(const Type &v) const {
|
||
_CMatrix tm = _CMatrix(*this);
|
||
PIMM_FOR_WB(r, c) tm.m[r][c] *= v;
|
||
return tm;
|
||
}
|
||
|
||
/**
|
||
* @brief Matrix division
|
||
*
|
||
* @param v is value divider
|
||
* @return the result of matrix division
|
||
*/
|
||
_CMatrix operator/(const Type &v) const {
|
||
_CMatrix tm = _CMatrix(*this);
|
||
PIMM_FOR_WB(r, c) tm.m[r][c] /= v;
|
||
return tm;
|
||
}
|
||
|
||
/**
|
||
* @brief Determinant of the matrix is calculated. Works only with square matrix, nonzero matrices and invertible matrix
|
||
*
|
||
* @param ok is a parameter with which we can find out if the method worked correctly
|
||
* @return matrix determinant
|
||
*/
|
||
Type determinant(bool *ok = 0) const {
|
||
_CMatrix m(*this);
|
||
bool k;
|
||
Type ret = Type(0);
|
||
m.toUpperTriangular(&k);
|
||
if (ok) *ok = k;
|
||
if (!k) return ret;
|
||
ret = Type(1);
|
||
for (uint c = 0; c < Cols; ++c)
|
||
for (uint r = 0; r < Rows; ++r)
|
||
if (r == c)
|
||
ret *= m[r][c];
|
||
return ret;
|
||
}
|
||
|
||
/**
|
||
* @brief Transforming matrix to upper triangular. Works only with square matrix, nonzero matrices and invertible matrix
|
||
*
|
||
* @param ok is a parameter with which we can find out if the method worked correctly
|
||
* @return copy of transformed upper triangular matrix
|
||
*/
|
||
_CMatrix &toUpperTriangular(bool *ok = 0) {
|
||
if (Cols != Rows) {
|
||
if (ok != 0) *ok = false;
|
||
return *this;
|
||
}
|
||
_CMatrix smat(*this);
|
||
bool ndet;
|
||
uint crow;
|
||
Type mul;
|
||
for (uint i = 0; i < Cols; ++i) {
|
||
ndet = true;
|
||
for (uint j = 0; j < Rows; ++j) if (smat.m[i][j] != 0) ndet = false;
|
||
if (ndet) {
|
||
if (ok != 0) *ok = false;
|
||
return *this;
|
||
}
|
||
}
|
||
for (uint i = 0; i < Cols; ++i) {
|
||
crow = i;
|
||
while (smat.m[i][i] == Type(0))
|
||
smat.swapRows(i, ++crow);
|
||
for (uint j = i + 1; j < Rows; ++j) {
|
||
mul = smat.m[i][j] / smat.m[i][i];
|
||
for (uint k = i; k < Cols; ++k) smat.m[k][j] -= mul * smat.m[k][i];
|
||
}
|
||
if (i < Cols - 1) {
|
||
if (fabs(smat.m[i + 1][i + 1]) < Type(1E-200)) {
|
||
if (ok != 0) *ok = false;
|
||
return *this;
|
||
}
|
||
}
|
||
}
|
||
if (ok != 0) *ok = true;
|
||
memcpy(m, smat.m, sizeof(Type) * Cols * Rows);
|
||
return *this;
|
||
}
|
||
|
||
/**
|
||
* @brief Matrix inversion operation. Works only with square matrix, nonzero matrices and invertible matrix
|
||
*
|
||
* @param ok is a parameter with which we can find out if the method worked correctly
|
||
* @return copy of inverted matrix
|
||
*/
|
||
_CMatrix &invert(bool *ok = 0) {
|
||
static_assert(Cols == Rows, "Only square matrix invertable");
|
||
_CMatrix mtmp = _CMatrix::identity(), smat(*this);
|
||
bool ndet;
|
||
uint crow;
|
||
Type mul, iddiv;
|
||
for (uint i = 0; i < Cols; ++i) {
|
||
ndet = true;
|
||
for (uint j = 0; j < Rows; ++j) if (smat.m[i][j] != 0) ndet = false;
|
||
if (ndet) {
|
||
if (ok != 0) *ok = false;
|
||
return *this;
|
||
}
|
||
}
|
||
for (uint i = 0; i < Cols; ++i) {
|
||
crow = i;
|
||
while (smat.m[i][i] == Type(0)) {
|
||
++crow;
|
||
smat.swapRows(i, crow);
|
||
mtmp.swapRows(i, crow);
|
||
}
|
||
for (uint j = i + 1; j < Rows; ++j) {
|
||
mul = smat.m[i][j] / smat.m[i][i];
|
||
for (uint k = i; k < Cols; ++k) smat.m[k][j] -= mul * smat.m[k][i];
|
||
for (uint k = 0; k < Cols; ++k) mtmp.m[k][j] -= mul * mtmp.m[k][i];
|
||
}
|
||
if (i < Cols - 1) {
|
||
if (fabs(smat.m[i + 1][i + 1]) < Type(1E-200)) {
|
||
if (ok != 0) *ok = false;
|
||
return *this;
|
||
}
|
||
}
|
||
iddiv = smat.m[i][i];
|
||
for (uint j = i; j < Cols; ++j) smat.m[j][i] /= iddiv;
|
||
for (uint j = 0; j < Cols; ++j) mtmp.m[j][i] /= iddiv;
|
||
}
|
||
for (uint i = Cols - 1; i > 0; --i) {
|
||
for (uint j = 0; j < i; ++j) {
|
||
mul = smat.m[i][j];
|
||
smat.m[i][j] -= mul;
|
||
for (uint k = 0; k < Cols; ++k) mtmp.m[k][j] -= mtmp.m[k][i] * mul;
|
||
}
|
||
}
|
||
if (ok != 0) *ok = true;
|
||
memcpy(m, mtmp.m, sizeof(Type) * Cols * Rows);
|
||
return *this;
|
||
}
|
||
|
||
/**
|
||
* @brief Matrix inversion operation. Works only with square matrix, nonzero matrices and invertible matrix
|
||
*
|
||
* @param ok is a parameter with which we can find out if the method worked correctly
|
||
* @return inverted matrix
|
||
*/
|
||
_CMatrix inverted(bool *ok = 0) const {
|
||
_CMatrix tm(*this);
|
||
tm.invert(ok);
|
||
return tm;
|
||
}
|
||
|
||
/**
|
||
* @brief Matrix transposition operation. Works only with square matrix, nonzero matrices and invertible matrix
|
||
*
|
||
* @return transposed matrix
|
||
*/
|
||
_CMatrixI transposed() const {
|
||
_CMatrixI tm;
|
||
PIMM_FOR_WB(r, c) tm[c][r] = m[r][c];
|
||
return tm;
|
||
}
|
||
|
||
private:
|
||
void resize(uint rows_, uint cols_, const Type &new_value = Type()) {
|
||
r_ = rows_;
|
||
c_ = cols_;
|
||
PIMM_FOR_WB(r, c) m[r][c] = new_value;
|
||
}
|
||
|
||
int c_, r_;
|
||
Type m[Rows][Cols];
|
||
|
||
};
|
||
|
||
#pragma pack(pop)
|
||
|
||
|
||
template<>
|
||
inline PIMathMatrixT<2u, 2u> PIMathMatrixT<2u, 2u>::rotation(double angle) {
|
||
double c = cos(angle), s = sin(angle);
|
||
PIMathMatrixT<2u, 2u> tm;
|
||
tm[0][0] = tm[1][1] = c;
|
||
tm[0][1] = -s;
|
||
tm[1][0] = s;
|
||
return tm;
|
||
}
|
||
|
||
template<>
|
||
inline PIMathMatrixT<2u, 2u> PIMathMatrixT<2u, 2u>::scaleX(double factor) {
|
||
PIMathMatrixT<2u, 2u> tm;
|
||
tm[0][0] = factor;
|
||
tm[1][1] = 1.;
|
||
return tm;
|
||
}
|
||
|
||
template<>
|
||
inline PIMathMatrixT<2u, 2u> PIMathMatrixT<2u, 2u>::scaleY(double factor) {
|
||
PIMathMatrixT<2u, 2u> tm;
|
||
tm[0][0] = 1.;
|
||
tm[1][1] = factor;
|
||
return tm;
|
||
}
|
||
|
||
template<>
|
||
inline PIMathMatrixT<3u, 3u> PIMathMatrixT<3u, 3u>::rotationX(double angle) {
|
||
double c = cos(angle), s = sin(angle);
|
||
PIMathMatrixT<3u, 3u> tm;
|
||
tm[0][0] = 1.;
|
||
tm[1][1] = tm[2][2] = c;
|
||
tm[2][1] = s;
|
||
tm[1][2] = -s;
|
||
return tm;
|
||
}
|
||
|
||
template<>
|
||
inline PIMathMatrixT<3u, 3u> PIMathMatrixT<3u, 3u>::rotationY(double angle) {
|
||
double c = cos(angle), s = sin(angle);
|
||
PIMathMatrixT<3u, 3u> tm;
|
||
tm[1][1] = 1.;
|
||
tm[0][0] = tm[2][2] = c;
|
||
tm[2][0] = -s;
|
||
tm[0][2] = s;
|
||
return tm;
|
||
}
|
||
|
||
template<>
|
||
inline PIMathMatrixT<3u, 3u> PIMathMatrixT<3u, 3u>::rotationZ(double angle) {
|
||
double c = cos(angle), s = sin(angle);
|
||
PIMathMatrixT<3u, 3u> tm;
|
||
tm[2][2] = 1.;
|
||
tm[0][0] = tm[1][1] = c;
|
||
tm[1][0] = s;
|
||
tm[0][1] = -s;
|
||
return tm;
|
||
}
|
||
|
||
template<>
|
||
inline PIMathMatrixT<3u, 3u> PIMathMatrixT<3u, 3u>::scaleX(double factor) {
|
||
PIMathMatrixT<3u, 3u> tm;
|
||
tm[1][1] = tm[2][2] = 1.;
|
||
tm[0][0] = factor;
|
||
return tm;
|
||
}
|
||
|
||
template<>
|
||
inline PIMathMatrixT<3u, 3u> PIMathMatrixT<3u, 3u>::scaleY(double factor) {
|
||
PIMathMatrixT<3u, 3u> tm;
|
||
tm[0][0] = tm[2][2] = 1.;
|
||
tm[1][1] = factor;
|
||
return tm;
|
||
}
|
||
|
||
template<>
|
||
inline PIMathMatrixT<3u, 3u> PIMathMatrixT<3u, 3u>::scaleZ(double factor) {
|
||
PIMathMatrixT<3u, 3u> tm;
|
||
tm[0][0] = tm[1][1] = 1.;
|
||
tm[2][2] = factor;
|
||
return tm;
|
||
}
|
||
|
||
#ifdef PIP_STD_IOSTREAM
|
||
template<uint Rows, uint Cols, typename Type>
|
||
inline std::ostream & operator <<(std::ostream & s, const PIMathMatrixT<Rows, Cols, Type> & m) {s << "{"; PIMM_FOR_I(r, c) s << m[r][c]; if (c < Cols - 1 || r < Rows - 1) s << ", ";} if (r < Rows - 1) s << std::endl << " ";} s << "}"; return s;}
|
||
#endif
|
||
|
||
/**
|
||
* @brief Add matrix "m" at the end of matrix and return reference to matrix
|
||
*
|
||
* @param s PICout type
|
||
* @param m PIMathMatrixT type
|
||
* @return bitwise left PICout
|
||
*/
|
||
template<uint Rows, uint Cols, typename Type>
|
||
inline PICout operator<<(PICout s, const PIMathMatrixT<Rows, Cols, Type> &m) {
|
||
s << "{";
|
||
PIMM_FOR_I(r, c) s << m[r][c];
|
||
if (c < Cols - 1 || r < Rows - 1) s << ", "; }
|
||
if (r < Rows - 1) s << PICoutManipulators::NewLine << " "; }
|
||
s << "}";
|
||
return s;
|
||
}
|
||
|
||
/**
|
||
* @brief Multiplying matrices by each other. If you enter an index out of the border of the matrix there will be "undefined behavior"
|
||
*
|
||
* @param fm first matrix multiplier
|
||
* @param sm second matrix multiplier
|
||
* @return matrix that is the result of multiplication
|
||
*/
|
||
template<uint CR, uint Rows0, uint Cols1, typename Type>
|
||
inline PIMathMatrixT<Rows0, Cols1, Type> operator*(const PIMathMatrixT<Rows0, CR, Type> &fm,
|
||
const PIMathMatrixT<CR, Cols1, Type> &sm) {
|
||
PIMathMatrixT<Rows0, Cols1, Type> tm;
|
||
Type t;
|
||
for (uint j = 0; j < Rows0; ++j) {
|
||
for (uint i = 0; i < Cols1; ++i) {
|
||
t = Type(0);
|
||
for (uint k = 0; k < CR; ++k)
|
||
t += fm[j][k] * sm[k][i];
|
||
tm[j][i] = t;
|
||
}
|
||
}
|
||
return tm;
|
||
}
|
||
|
||
/**
|
||
* @brief Multiplying matrix and vector. If you enter an index out of the border of the matrix there will be "undefined behavior"
|
||
*
|
||
* @param fm first matrix multiplier
|
||
* @param sv second vector multiplier
|
||
* @return vector that is the result of multiplication
|
||
*/
|
||
template<uint Cols, uint Rows, typename Type>
|
||
inline PIMathVectorT<Rows, Type> operator*(const PIMathMatrixT<Rows, Cols, Type> &fm,
|
||
const PIMathVectorT<Cols, Type> &sv) {
|
||
PIMathVectorT<Rows, Type> tv;
|
||
Type t;
|
||
for (uint j = 0; j < Rows; ++j) {
|
||
t = Type(0);
|
||
for (uint i = 0; i < Cols; ++i)
|
||
t += fm[j][i] * sv[i];
|
||
tv[j] = t;
|
||
}
|
||
return tv;
|
||
}
|
||
|
||
/**
|
||
* @brief Multiplying vector and matrix. If you enter an index out of the border of the matrix there will be "undefined behavior"
|
||
*
|
||
* @param sv first vector multiplier
|
||
* @param fm second matrix multiplier
|
||
* @return vector that is the result of multiplication
|
||
*/
|
||
template<uint Cols, uint Rows, typename Type>
|
||
inline PIMathVectorT<Cols, Type> operator*(const PIMathVectorT<Rows, Type> &sv,
|
||
const PIMathMatrixT<Rows, Cols, Type> &fm) {
|
||
PIMathVectorT<Cols, Type> tv;
|
||
Type t;
|
||
for (uint j = 0; j < Cols; ++j) {
|
||
t = Type(0);
|
||
for (uint i = 0; i < Rows; ++i)
|
||
t += fm[i][j] * sv[i];
|
||
tv[j] = t;
|
||
}
|
||
return tv;
|
||
}
|
||
|
||
/**
|
||
* @brief Multiplying value of type Type and matrix
|
||
*
|
||
* @param x first multiplier of type Type
|
||
* @param fm second matrix multiplier
|
||
* @return matrix that is the result of multiplication
|
||
*/
|
||
template<uint Cols, uint Rows, typename Type>
|
||
inline PIMathMatrixT<Rows, Cols, Type> operator*(const Type &x, const PIMathMatrixT<Rows, Cols, Type> &v) {
|
||
return v * x;
|
||
}
|
||
|
||
|
||
typedef PIMathMatrixT<2u, 2u, int> PIMathMatrixT22i;
|
||
typedef PIMathMatrixT<3u, 3u, int> PIMathMatrixT33i;
|
||
typedef PIMathMatrixT<4u, 4u, int> PIMathMatrixT44i;
|
||
typedef PIMathMatrixT<2u, 2u, double> PIMathMatrixT22d;
|
||
typedef PIMathMatrixT<3u, 3u, double> PIMathMatrixT33d;
|
||
typedef PIMathMatrixT<4u, 4u, double> PIMathMatrixT44d;
|
||
|
||
|
||
template<typename Type>
|
||
class PIMathMatrix;
|
||
|
||
#undef PIMM_FOR
|
||
#undef PIMM_FOR_WB
|
||
#undef PIMM_FOR_I
|
||
#undef PIMM_FOR_I_WB
|
||
#undef PIMM_FOR_C
|
||
#undef PIMM_FOR_R
|
||
|
||
|
||
|
||
|
||
|
||
/// Matrix
|
||
|
||
#define PIMM_FOR(c, r) for (uint c = 0; c < _V2D::cols_; ++c) for (uint r = 0; r < _V2D::rows_; ++r)
|
||
#define PIMM_FOR_I(c, r) for (uint r = 0; r < _V2D::rows_; ++r) for (uint c = 0; c < _V2D::cols_; ++c)
|
||
#define PIMM_FOR_A(v) for (uint v = 0; v < _V2D::mat.size(); ++v)
|
||
#define PIMM_FOR_C(v) for (uint v = 0; v < _V2D::cols_; ++v)
|
||
#define PIMM_FOR_R(v) for (uint v = 0; v < _V2D::rows_; ++v)
|
||
|
||
//! \brief A class that works with matrix operations, the input data of which is the data type of the matrix
|
||
//! @tparam There are can be basic C++ language data and different classes where the arithmetic operators(=, +=, -=, *=, /=, ==, !=, +, -, *, /)
|
||
//! of the C++ language are implemented
|
||
template<typename Type>
|
||
class PIP_EXPORT PIMathMatrix : public PIVector2D<Type> {
|
||
typedef PIVector2D<Type> _V2D;
|
||
typedef PIMathMatrix<Type> _CMatrix;
|
||
typedef PIMathVector<Type> _CMCol;
|
||
public:
|
||
/**
|
||
* @brief Constructor of class PIMathMatrix, which creates a matrix
|
||
*
|
||
* @param cols is number of matrix column uint type
|
||
* @param rows is number of matrix row uint type
|
||
* @param f is type of matrix elements
|
||
*/
|
||
PIMathMatrix(const uint cols = 0, const uint rows = 0, const Type &f = Type()) { _V2D::resize(rows, cols, f); }
|
||
|
||
/**
|
||
* @brief Constructor of class PIMathMatrix, which creates a matrix
|
||
*
|
||
* @param cols is number of matrix column uint type
|
||
* @param rows is number of matrix row uint type
|
||
* @param val is PIVector<Type> of matrix elements
|
||
*/
|
||
PIMathMatrix(const uint cols, const uint rows, const PIVector<Type> &val) {
|
||
_V2D::resize(rows, cols);
|
||
int i = 0;
|
||
PIMM_FOR_I(c, r) _V2D::element(r, c) = val[i++];
|
||
}
|
||
|
||
/**
|
||
* @brief Constructor of class PIMathMatrix, which creates a matrix
|
||
*
|
||
* @param val is PIVector<Type> of PIVector, which creates matrix
|
||
*/
|
||
PIMathMatrix(const PIVector<PIVector<Type> > &val) {
|
||
if (!val.isEmpty()) {
|
||
_V2D::resize(val.size(), val[0].size());
|
||
PIMM_FOR_I(c, r) _V2D::element(r, c) = val[r][c];
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief Constructor of class PIMathMatrix, which creates a matrix
|
||
*
|
||
* @param val is PIVector2D<Type>, which creates matrix
|
||
*/
|
||
PIMathMatrix(const PIVector2D<Type> &val) {
|
||
if (!val.isEmpty()) {
|
||
_V2D::resize(val.rows(), val.cols());
|
||
PIMM_FOR_I(c, r) _V2D::element(r, c) = val.element(r, c);
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief Creates a matrix whose main diagonal is filled with ones and the remaining elements are zeros
|
||
*
|
||
* @param cols is number of matrix column uint type
|
||
* @param rows is number of matrix row uint type
|
||
* @return identity matrix of type PIMathMatrix
|
||
*/
|
||
static _CMatrix identity(const uint cols, const uint rows) {
|
||
_CMatrix tm(cols, rows);
|
||
for (uint r = 0; r < rows; ++r) for (uint c = 0; c < cols; ++c) tm.element(r, c) = (c == r ? Type(1) : Type(0));
|
||
return tm;
|
||
}
|
||
|
||
static _CMatrix zero(const uint cols, const uint rows) {
|
||
_V2D::resize(rows, cols);
|
||
fill(Type(0));
|
||
}
|
||
|
||
/**
|
||
* @brief Creates a row matrix of every element that is equal to every element of the vector
|
||
*
|
||
* @param val is the vector type PIMathVector
|
||
* @return row matrix of every element that is equal to every element of the vector
|
||
*/
|
||
static _CMatrix matrixRow(const PIMathVector<Type> &val) { return _CMatrix(val.size(), 1, val.toVector()); }
|
||
|
||
/**
|
||
* @brief Creates a column matrix of every element that is equal to every element of the vector
|
||
*
|
||
* @param val is the vector type PIMathVector
|
||
* @return column matrix of every element that is equal to every element of the vector
|
||
*/
|
||
static _CMatrix matrixCol(const PIMathVector<Type> &val) { return _CMatrix(1, val.size(), val.toVector()); }
|
||
|
||
/**
|
||
* @brief Set the selected column in matrix. If there are more elements of the vector than elements in the column of the matrix
|
||
* or index larger than the number of columns otherwise there will be "undefined behavior"
|
||
*
|
||
* @param index is the number of the selected column
|
||
* @param v is a vector of the type _CMCol that needs to fill the column
|
||
* @return matrix type _CMatrix
|
||
*/
|
||
_CMatrix &setCol(uint index, const _CMCol &v) {
|
||
PIMM_FOR_R(i) _V2D::element(i, index) = v[i];
|
||
return *this;
|
||
}
|
||
|
||
/**
|
||
* @brief Set the selected row in matrix. If there are more elements of the vector than elements in the row of the matrix,
|
||
* or index larger than the number of rows otherwise there will be "undefined behavior"
|
||
* @param index is the number of the selected row
|
||
* @param v is a vector of the type _CMCol that needs to fill the row
|
||
* @return matrix type _CMatrix
|
||
*/
|
||
_CMatrix &setRow(uint index, const _CMCol &v) {
|
||
PIMM_FOR_C(i) _V2D::element(index, i) = v[i];
|
||
return *this;
|
||
}
|
||
|
||
/**
|
||
* @brief Method which replace selected columns in a matrix. You cannot use an index larger than the number of columns,
|
||
* otherwise there will be "undefined behavior"
|
||
*
|
||
* @param r0 is the number of the first selected row
|
||
* @param r1 is the number of the second selected row
|
||
* @return matrix type _CMatrix
|
||
*/
|
||
_CMatrix &swapCols(uint r0, uint r1) {
|
||
PIMM_FOR_C(i) { piSwap(_V2D::element(i, r0), _V2D::element(i, r1)); }
|
||
return *this;
|
||
}
|
||
|
||
/**
|
||
* @brief Method which replace selected rows in a matrix. You cannot use an index larger than the number of rows,
|
||
* otherwise there will be "undefined behavior"
|
||
*
|
||
* @param c0 is the number of the first selected row
|
||
* @param c1 is the number of the second selected row
|
||
* @return matrix type _CMatrix
|
||
*/
|
||
_CMatrix &swapRows(uint c0, uint c1) {
|
||
PIMM_FOR_R(i) { piSwap(_V2D::element(c0, i), _V2D::element(c1, i)); }
|
||
return *this;
|
||
}
|
||
|
||
/**
|
||
* @brief Method which fills the matrix with selected value
|
||
*
|
||
* @param v is a parameter the type and value of which is selected and later filled into the matrix
|
||
* @return filled matrix type _CMatrix
|
||
*/
|
||
_CMatrix &fill(const Type &v) {
|
||
PIMM_FOR_A(i) _V2D::mat[i] = v;
|
||
return *this;
|
||
}
|
||
|
||
/**
|
||
* @brief Method which checks if matrix is square
|
||
*
|
||
* @return true if matrix is square, else false
|
||
*/
|
||
bool isSquare() const { return _V2D::cols_ == _V2D::rows_; }
|
||
|
||
/**
|
||
* @brief Method which checks if main diagonal of matrix consists of ones and another elements are zeros
|
||
*
|
||
* @return true if matrix is identity, else false
|
||
*/
|
||
bool isIdentity() const {
|
||
PIMM_FOR(c, r) if ((c == r) ? _V2D::element(r, c) != Type(1) : _V2D::element(r, c) != Type(0))return false;
|
||
return true;
|
||
}
|
||
|
||
/**
|
||
* @brief Method which checks if every elements of matrix are zeros
|
||
*
|
||
* @return true if matrix elements equal to zero, else false
|
||
*/
|
||
bool isNull() const {
|
||
PIMM_FOR_A(i) if (_V2D::mat[i] != Type(0)) return false;
|
||
return true;
|
||
}
|
||
|
||
/**
|
||
* @brief Method which checks if matrix is empty
|
||
*
|
||
* @return true if matrix is valid, else false
|
||
*/
|
||
bool isValid() const { return !PIVector2D<Type>::isEmpty(); }
|
||
|
||
/**
|
||
* @brief Addition assignment with matrix "sm"
|
||
*
|
||
* @param sm matrix for the addition assigment
|
||
*/
|
||
void operator+=(const _CMatrix &sm) {
|
||
assert(_V2D::rows() == sm.rows());
|
||
assert(_V2D::cols() == sm.cols());
|
||
PIMM_FOR_A(i) _V2D::mat[i] += sm.mat[i];
|
||
}
|
||
|
||
/**
|
||
* @brief Subtraction assignment with matrix "sm"
|
||
*
|
||
* @param sm matrix for the subtraction assigment
|
||
*/
|
||
void operator-=(const _CMatrix &sm) {
|
||
assert(_V2D::rows() == sm.rows());
|
||
assert(_V2D::cols() == sm.cols());
|
||
PIMM_FOR_A(i) _V2D::mat[i] -= sm.mat[i];
|
||
}
|
||
|
||
/**
|
||
* @brief Multiplication assignment with value "v"
|
||
*
|
||
* @param v value for the multiplication assigment
|
||
*/
|
||
void operator*=(const Type &v) { PIMM_FOR_A(i) _V2D::mat[i] *= v; }
|
||
|
||
/**
|
||
* @brief Division assignment with value "v"
|
||
*
|
||
* @param v value for the division assigment
|
||
*/
|
||
void operator/=(const Type &v) { PIMM_FOR_A(i) _V2D::mat[i] /= v; }
|
||
|
||
/**
|
||
* @brief Matrix substraction
|
||
*
|
||
* @return the result of matrix substraction
|
||
*/
|
||
_CMatrix operator-() const {
|
||
_CMatrix tm(*this);
|
||
PIMM_FOR_A(i) tm.mat[i] = -_V2D::mat[i];
|
||
return tm;
|
||
}
|
||
|
||
/**
|
||
* @brief Matrix addition
|
||
*
|
||
* @param sm is matrix term
|
||
* @return the result of matrix addition
|
||
*/
|
||
_CMatrix operator+(const _CMatrix &sm) const {
|
||
_CMatrix tm(*this);
|
||
assert(tm.rows() == sm.rows());
|
||
assert(tm.cols() == sm.cols());
|
||
PIMM_FOR_A(i) tm.mat[i] += sm.mat[i];
|
||
return tm;
|
||
}
|
||
|
||
/**
|
||
* @brief Matrix subtraction
|
||
*
|
||
* @param sm is matrix subtractor
|
||
* @return the result of matrix subtraction
|
||
*/
|
||
_CMatrix operator-(const _CMatrix &sm) const {
|
||
_CMatrix tm(*this);
|
||
assert(tm.rows() == sm.rows());
|
||
assert(tm.cols() == sm.cols());
|
||
PIMM_FOR_A(i) tm.mat[i] -= sm.mat[i];
|
||
return tm;
|
||
}
|
||
|
||
/**
|
||
* @brief Matrix multiplication
|
||
*
|
||
* @param v is value factor
|
||
* @return the result of matrix multiplication
|
||
*/
|
||
_CMatrix operator*(const Type &v) const {
|
||
_CMatrix tm(*this);
|
||
PIMM_FOR_A(i) tm.mat[i] *= v;
|
||
return tm;
|
||
}
|
||
|
||
/**
|
||
* @brief Matrix division
|
||
*
|
||
* @param v is value divider
|
||
* @return the result of matrix division
|
||
*/
|
||
_CMatrix operator/(const Type &v) const {
|
||
_CMatrix tm(*this);
|
||
PIMM_FOR_A(i) tm.mat[i] /= v;
|
||
return tm;
|
||
}
|
||
|
||
/**
|
||
* @brief Determinant of the self matrix is calculated. Works only with square matrix, nonzero matrices and invertible matrix
|
||
*
|
||
* @param ok is a parameter with which we can find out if the method worked correctly
|
||
* @return matrix determinant
|
||
*/
|
||
Type determinant(bool *ok = 0) const {
|
||
_CMatrix m(*this);
|
||
bool k;
|
||
Type ret = Type(0);
|
||
m.toUpperTriangular(&k);
|
||
if (ok) *ok = k;
|
||
if (!k) return ret;
|
||
ret = Type(1);
|
||
for (uint c = 0; c < _V2D::cols_; ++c)
|
||
for (uint r = 0; r < _V2D::rows_; ++r)
|
||
if (r == c)
|
||
ret *= m.element(r, c);
|
||
return ret;
|
||
}
|
||
|
||
/**
|
||
* @brief Trace of the matrix is calculated. Works only with square matrix, nonzero matrices and invertible matrix
|
||
*
|
||
* @param ok is a parameter with which we can find out if the method worked correctly
|
||
* @return matrix trace
|
||
*/
|
||
Type trace(bool *ok = 0) const {
|
||
Type ret = Type(0);
|
||
if (!isSquare()) {
|
||
if (ok != 0) *ok = false;
|
||
return ret;
|
||
}
|
||
for (uint i = 0; i < _V2D::cols_; ++i) {
|
||
ret += _V2D::element(i, i);
|
||
}
|
||
if (ok != 0) *ok = true;
|
||
return ret;
|
||
}
|
||
|
||
/**
|
||
* @brief Transforming matrix to upper triangular. Works only with square matrix, nonzero matrices and invertible matrix
|
||
*
|
||
* @param ok is a parameter with which we can find out if the method worked correctly
|
||
* @return copy of transformed upper triangular matrix
|
||
*/
|
||
_CMatrix &toUpperTriangular(bool *ok = 0) {
|
||
if (!isSquare()) {
|
||
if (ok != 0) *ok = false;
|
||
return *this;
|
||
}
|
||
_CMatrix smat(*this);
|
||
bool ndet;
|
||
uint crow;
|
||
Type mul;
|
||
for (uint i = 0; i < _V2D::cols_; ++i) {
|
||
ndet = true;
|
||
for (uint j = 0; j < _V2D::rows_; ++j) if (smat.element(i, j) != 0) ndet = false;
|
||
if (ndet) {
|
||
if (ok != 0) *ok = false;
|
||
return *this;
|
||
}
|
||
}
|
||
for (uint i = 0; i < _V2D::cols_; ++i) {
|
||
crow = i;
|
||
while (smat.element(i, i) == Type(0))
|
||
smat.swapRows(i, ++crow);
|
||
for (uint j = i + 1; j < _V2D::rows_; ++j) {
|
||
mul = smat.element(i, j) / smat.element(i, i);
|
||
for (uint k = i; k < _V2D::cols_; ++k) smat.element(k, j) -= mul * smat.element(k, i);
|
||
}
|
||
if (i < _V2D::cols_ - 1) {
|
||
if (PIMathFloatNullCompare(smat.element(i + 1, i + 1))) {
|
||
if (ok != 0) *ok = false;
|
||
return *this;
|
||
}
|
||
}
|
||
}
|
||
if (ok != 0) *ok = true;
|
||
_V2D::mat.swap(smat.mat);
|
||
return *this;
|
||
}
|
||
|
||
/**
|
||
* @brief Matrix inversion operation. Works only with square matrix, nonzero matrices and invertible matrix
|
||
*
|
||
* @param ok is a parameter with which we can find out if the method worked correctly
|
||
* @param sv is a vector multiplier
|
||
* @return copy of inverted matrix
|
||
*/
|
||
_CMatrix &invert(bool *ok = 0, _CMCol *sv = 0) {
|
||
if (!isSquare()) {
|
||
if (ok != 0) *ok = false;
|
||
return *this;
|
||
}
|
||
_CMatrix mtmp = _CMatrix::identity(_V2D::cols_, _V2D::rows_), smat(*this);
|
||
bool ndet;
|
||
uint crow;
|
||
Type mul, iddiv;
|
||
for (uint i = 0; i < _V2D::cols_; ++i) {
|
||
ndet = true;
|
||
for (uint j = 0; j < _V2D::rows_; ++j) if (smat.element(i, j) != Type(0)) ndet = false;
|
||
if (ndet) {
|
||
if (ok != 0) *ok = false;
|
||
return *this;
|
||
}
|
||
}
|
||
for (uint i = 0; i < _V2D::cols_; ++i) {
|
||
crow = i;
|
||
while (smat.element(i, i) == Type(0)) {
|
||
++crow;
|
||
smat.swapRows(i, crow);
|
||
mtmp.swapRows(i, crow);
|
||
if (sv != 0) sv->swap(i, crow);
|
||
}
|
||
for (uint j = i + 1; j < _V2D::rows_; ++j) {
|
||
mul = smat.element(i, j) / smat.element(i, i);
|
||
for (uint k = i; k < _V2D::cols_; ++k) smat.element(k, j) -= mul * smat.element(k, i);
|
||
for (uint k = 0; k < _V2D::cols_; ++k) mtmp.element(k, j) -= mul * mtmp.element(k, i);
|
||
if (sv != 0) (*sv)[j] -= mul * (*sv)[i];
|
||
}
|
||
if (i < _V2D::cols_ - 1) {
|
||
if (PIMathFloatNullCompare(smat.element(i + 1, i + 1))) {
|
||
if (ok != 0) *ok = false;
|
||
return *this;
|
||
}
|
||
}
|
||
iddiv = smat.element(i, i);
|
||
for (uint j = i; j < _V2D::cols_; ++j) smat.element(j, i) /= iddiv;
|
||
for (uint j = 0; j < _V2D::cols_; ++j) mtmp.element(j, i) /= iddiv;
|
||
if (sv != 0) (*sv)[i] /= iddiv;
|
||
}
|
||
for (uint i = _V2D::cols_ - 1; i > 0; --i) {
|
||
for (uint j = 0; j < i; ++j) {
|
||
mul = smat.element(i, j);
|
||
smat.element(i, j) -= mul;
|
||
for (uint k = 0; k < _V2D::cols_; ++k) mtmp.element(k, j) -= mul * mtmp.element(k, i);
|
||
if (sv != 0) (*sv)[j] -= mul * (*sv)[i];
|
||
}
|
||
}
|
||
if (ok != 0) *ok = true;
|
||
PIVector2D<Type>::swap(mtmp);
|
||
return *this;
|
||
}
|
||
|
||
/**
|
||
* @brief Matrix inversion operation. Works only with square matrix, nonzero matrices and invertible matrix
|
||
*
|
||
* @param ok is a parameter with which we can find out if the method worked correctly
|
||
* @return inverted matrix
|
||
*/
|
||
_CMatrix inverted(bool *ok = 0) const {
|
||
_CMatrix tm(*this);
|
||
tm.invert(ok);
|
||
return tm;
|
||
}
|
||
|
||
/**
|
||
* @brief Matrix transposition operation
|
||
*
|
||
* @return transposed matrix
|
||
*/
|
||
_CMatrix transposed() const {
|
||
_CMatrix tm(_V2D::rows_, _V2D::cols_);
|
||
PIMM_FOR(c, r) tm.element(c, r) = _V2D::element(r, c);
|
||
return tm;
|
||
}
|
||
};
|
||
|
||
|
||
#ifdef PIP_STD_IOSTREAM
|
||
template<typename Type>
|
||
inline std::ostream & operator <<(std::ostream & s, const PIMathMatrix<Type> & m) {s << "{"; for (uint r = 0; r < m.rows(); ++r) { for (uint c = 0; c < m.cols(); ++c) { s << m.element(r, c); if (c < m.cols() - 1 || r < m.rows() - 1) s << ", ";} if (r < m.rows() - 1) s << std::endl << " ";} s << "}"; return s;}
|
||
#endif
|
||
|
||
/**
|
||
* @brief Inline operator for outputting the matrix to the console
|
||
*
|
||
* @param s PICout type
|
||
* @param the matrix type PIMathMatrix that we print to the console
|
||
* @return PIMathMatrix printed to the console
|
||
*/
|
||
template<typename Type>
|
||
inline PICout operator<<(PICout s, const PIMathMatrix<Type> &m) {
|
||
s << "Matrix{";
|
||
for (uint r = 0; r < m.rows(); ++r) {
|
||
for (uint c = 0; c < m.cols(); ++c) {
|
||
s << m.element(r, c);
|
||
if (c < m.cols() - 1 || r < m.rows() - 1) s << ", ";
|
||
}
|
||
if (r < m.rows() - 1) s << PICoutManipulators::NewLine << " ";
|
||
}
|
||
s << "}";
|
||
return s;
|
||
}
|
||
|
||
/**
|
||
* @brief Inline operator for serializing a matrix into a PIByteArray
|
||
*
|
||
* @param s PIByteArray type
|
||
* @param v PIMathMatrix type
|
||
* @return PIBiteArray serialized PIMathMatrix
|
||
*/
|
||
template<typename Type>
|
||
inline PIByteArray &operator<<(PIByteArray &s, const PIMathMatrix<Type> &v) {
|
||
s << (const PIVector2D<Type> &) v;
|
||
return s;
|
||
}
|
||
|
||
/**
|
||
* @brief Inline operator to deserialize matrix from PIByteArray
|
||
*
|
||
* @param s PIByteArray type
|
||
* @param v PIMathMatrix type
|
||
* @return PIMathMatrix deserialized from PIByteArray
|
||
*/
|
||
template<typename Type>
|
||
inline PIByteArray &operator>>(PIByteArray &s, PIMathMatrix<Type> &v) {
|
||
s >> (PIVector2D<Type> &) v;
|
||
return s;
|
||
}
|
||
|
||
|
||
/**
|
||
* @brief Multiplying matrices by each other. If you enter an index out of the border of the matrix there will be "undefined behavior"
|
||
*
|
||
* @param fm first matrix multiplier
|
||
* @param sm second matrix multiplier
|
||
* @return matrix that is the result of multiplication
|
||
*/
|
||
template<typename Type>
|
||
inline PIMathMatrix<Type> operator*(const PIMathMatrix<Type> &fm,
|
||
const PIMathMatrix<Type> &sm) {
|
||
uint cr = fm.cols(), rows0 = fm.rows(), cols1 = sm.cols();
|
||
PIMathMatrix<Type> tm(cols1, rows0);
|
||
if (fm.cols() != sm.rows()) return tm;
|
||
Type t;
|
||
for (uint j = 0; j < rows0; ++j) {
|
||
for (uint i = 0; i < cols1; ++i) {
|
||
t = Type(0);
|
||
for (uint k = 0; k < cr; ++k)
|
||
t += fm.element(j, k) * sm.element(k, i);
|
||
tm.element(j, i) = t;
|
||
}
|
||
}
|
||
return tm;
|
||
}
|
||
|
||
/**
|
||
* @brief Multiplying matrix and vector. If you enter an index out of the border of the matrix there will be "undefined behavior"
|
||
*
|
||
* @param fm first matrix multiplier
|
||
* @param sv second vector multiplier
|
||
* @return vector that is the result of multiplication
|
||
*/
|
||
template<typename Type>
|
||
inline PIMathVector<Type> operator*(const PIMathMatrix<Type> &fm,
|
||
const PIMathVector<Type> &sv) {
|
||
uint c = fm.cols(), r = fm.rows();
|
||
PIMathVector<Type> tv(r);
|
||
if (c != sv.size()) return tv;
|
||
Type t;
|
||
for (uint j = 0; j < r; ++j) {
|
||
t = Type(0);
|
||
for (uint i = 0; i < c; ++i)
|
||
t += fm.element(j, i) * sv[i];
|
||
tv[j] = t;
|
||
}
|
||
return tv;
|
||
}
|
||
|
||
/**
|
||
* @brief Multiplying vector and matrix. If you enter an index out of the border of the matrix there will be "undefined behavior"
|
||
*
|
||
* @param sv first vector multiplier
|
||
* @param fm second matrix multiplier
|
||
* @return vector that is the result of multiplication
|
||
*/
|
||
template<typename Type>
|
||
inline PIMathVector<Type> operator*(const PIMathVector<Type> &sv,
|
||
const PIMathMatrix<Type> &fm) {
|
||
uint c = fm.cols(), r = fm.rows();
|
||
PIMathVector<Type> tv(c);
|
||
Type t;
|
||
for (uint j = 0; j < c; ++j) {
|
||
t = Type(0);
|
||
for (uint i = 0; i < r; ++i)
|
||
t += fm.element(i, j) * sv[i];
|
||
tv[j] = t;
|
||
}
|
||
return tv;
|
||
}
|
||
|
||
/**
|
||
* @brief Multiplying value of type Type and matrix
|
||
*
|
||
* @param x first multiplier of type Type
|
||
* @param fm second matrix multiplier
|
||
* @return matrix that is the result of multiplication
|
||
*/
|
||
template<typename Type>
|
||
inline PIMathMatrix<Type> operator*(const Type &x, const PIMathMatrix<Type> &v) {
|
||
return v * x;
|
||
}
|
||
|
||
typedef PIMathMatrix<int> PIMathMatrixi;
|
||
typedef PIMathMatrix<double> PIMathMatrixd;
|
||
|
||
/**
|
||
* @brief Searching hermitian matrix
|
||
*
|
||
* @param m conjugate transpose matrix
|
||
* @return result of the hermitian
|
||
*/
|
||
template<typename T>
|
||
PIMathMatrix<complex<T> > hermitian(const PIMathMatrix<complex<T> > &m) {
|
||
PIMathMatrix<complex<T> > ret(m);
|
||
for (uint r = 0; r < ret.rows(); ++r)
|
||
for (uint c = 0; c < ret.cols(); ++c)
|
||
ret.element(r, c).imag(-(ret.element(r, c).imag()));
|
||
return ret.transposed();
|
||
}
|
||
|
||
#undef PIMM_FOR
|
||
#undef PIMM_FOR_I
|
||
#undef PIMM_FOR_A
|
||
#undef PIMM_FOR_C
|
||
#undef PIMM_FOR_R
|
||
|
||
#endif // PIMATHMATRIX_H
|