Pascal Serrarens
39bed8db3e
Some checks failed
Build and Run C++ Unit Tests / build-and-test (push) Failing after 1m25s
1383 lines
38 KiB
C++
1383 lines
38 KiB
C++
#include "Matrix.h"
|
|
#if !defined(NO_STD)
|
|
#include <math.h>
|
|
#include <iostream>
|
|
|
|
#endif
|
|
|
|
namespace LinearAlgebra {
|
|
|
|
#pragma region Matrix1Of
|
|
|
|
template <typename T>
|
|
Matrix1Of<T>::Matrix1Of() {}
|
|
|
|
template <typename T>
|
|
Matrix1Of<T>::Matrix1Of(int size) : size(size), externalData(false) {
|
|
if (this->size <= 0)
|
|
data = nullptr;
|
|
else
|
|
this->data = new T[size]();
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix1Of<T>::Matrix1Of(int size, T* data) : data(data), size(size) {
|
|
this->externalData = true;
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix1Of<T>::Matrix1Of(const Matrix1Of& m) : size(m.size) {
|
|
if (this->size <= 0)
|
|
this->data = nullptr;
|
|
else {
|
|
this->data = new T[this->size];
|
|
|
|
for (unsigned int ix = 0; ix < this->size; ++ix)
|
|
this->data[ix] = m.data[ix];
|
|
}
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix1Of<T>::Matrix1Of(Matrix1Of&& m) noexcept : data(m.data), size(m.size) {
|
|
m.data = nullptr; // Leave the moved-from object in a safe state
|
|
m.size = 0;
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix1Of<T>::~Matrix1Of() {
|
|
if (!this->externalData) {
|
|
if (this->size > 0)
|
|
delete[] data;
|
|
}
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix1Of<T>& Matrix1Of<T>::operator=(const Matrix1Of& m) {
|
|
if (this != &m) {
|
|
if (this->size > 0)
|
|
delete[] this->data; // Free the current memory
|
|
|
|
this->size = m.size;
|
|
this->externalData = m.externalData;
|
|
if (this->size == 0)
|
|
this->data = nullptr;
|
|
else {
|
|
this->data = new T[this->size];
|
|
for (unsigned int ix = 0; ix < this->size; ++ix)
|
|
this->data[ix] = m.data[ix];
|
|
}
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix1Of<T>& Matrix1Of<T>::operator=(Matrix1Of&& m) noexcept {
|
|
if (this != &m) {
|
|
if (this->size > 0) {
|
|
// printf("Matrix1 Move Assignment Deallocate %d\n", this->size);
|
|
delete[] this->data; // Free the current memory
|
|
}
|
|
|
|
// Transfer ownership of resources
|
|
this->size = m.size;
|
|
this->data = m.data;
|
|
this->externalData = m.externalData;
|
|
|
|
// Leave the moved-from object in a valid state
|
|
m.size = 0;
|
|
m.data = nullptr;
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
template <typename T>
|
|
unsigned int Matrix1Of<T>::Size() const {
|
|
return this->size;
|
|
}
|
|
|
|
template <typename T>
|
|
void Matrix1Of<T>::Resize(int size) {
|
|
if (!this->externalData && this->size > 0)
|
|
delete[] this->data;
|
|
|
|
this->externalData = false;
|
|
|
|
if (size <= 0) {
|
|
this->data = nullptr;
|
|
this->size = 0;
|
|
} else {
|
|
this->data = new T[size]();
|
|
}
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix1Of<T> Matrix1Of<T>::Zero(unsigned int size) {
|
|
Matrix1Of<T> r(size);
|
|
r.Fill(0);
|
|
return r;
|
|
}
|
|
|
|
template <typename T>
|
|
void Matrix1Of<T>::Fill(T value) {
|
|
for (unsigned int ix = 0; ix < this->size; ix++)
|
|
this->data[ix] = value;
|
|
}
|
|
|
|
template <typename T>
|
|
void Matrix1Of<T>::Fill(T value, unsigned int start, unsigned int stop) {
|
|
if (start > stop || start > this->size)
|
|
return;
|
|
|
|
if (stop > this->size)
|
|
stop = this->size;
|
|
|
|
for (unsigned int ix = start; ix < stop; ix++)
|
|
this->data[ix] = value;
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix1Of<T> Matrix1Of<T>::FromVector3(Vector3Of<T> v) {
|
|
Matrix1Of<T> r(3);
|
|
r.data[0] = v.horizontal;
|
|
r.data[1] = v.vertical;
|
|
r.data[2] = v.depth;
|
|
return r;
|
|
}
|
|
|
|
template <typename T>
|
|
Vector3Of<T> Matrix1Of<T>::ToVector3() {
|
|
return Vector3Of<T>(this->data[0], this->data[1], this->data[2]);
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix1Of<float> Matrix1Of<T>::FromQuaternion(Quaternion q) {
|
|
Matrix1Of<float> r(4);
|
|
r(0) = q.x;
|
|
r(1) = q.y;
|
|
r(2) = q.z;
|
|
r(3) = q.w;
|
|
return r;
|
|
}
|
|
|
|
template <typename T>
|
|
Quaternion Matrix1Of<T>::ToQuaternion() {
|
|
float x = static_cast<float>(data[0]);
|
|
float y = static_cast<float>(data[1]);
|
|
float z = static_cast<float>(data[2]);
|
|
float w = static_cast<float>(data[3]);
|
|
return Quaternion(x, y, z, w);
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix1Of<T> Matrix1Of<T>::Slice(unsigned int start, unsigned int stop) {
|
|
if (start < 0 || stop >= this->size)
|
|
std::cerr << "Slice index out of range." << std::endl;
|
|
|
|
Matrix1Of<T> result(stop - start);
|
|
int resultIx = 0;
|
|
for (unsigned int ix = start; ix < stop; ix++)
|
|
result.data[resultIx++] = this->data[ix];
|
|
|
|
return result;
|
|
}
|
|
|
|
template <typename T>
|
|
void Matrix1Of<T>::UpdateSlice(unsigned int start, unsigned int stop, const Matrix1Of& source) const {
|
|
if (start > stop || start > this->size || start > source.size)
|
|
return;
|
|
|
|
if (stop > this->size)
|
|
stop = this->size;
|
|
if (stop > source.size)
|
|
stop = source.size;
|
|
|
|
unsigned int sourceIx = 0;
|
|
for (unsigned int ix = start; ix < stop; ix++, sourceIx++) {
|
|
this->data[ix] = source.data[sourceIx];
|
|
}
|
|
}
|
|
|
|
template <typename T>
|
|
const T& Matrix1Of<T>::operator()(int ix) const {
|
|
return data[ix];
|
|
}
|
|
template <typename T>
|
|
T& Matrix1Of<T>::operator()(int ix) {
|
|
return data[ix];
|
|
}
|
|
|
|
template class Matrix1Of<int>;
|
|
template class Matrix1Of<float>;
|
|
template class Matrix1Of<double>;
|
|
|
|
#pragma endregion Matrix1Of
|
|
|
|
/*
|
|
#pragma region Matrix1
|
|
|
|
Matrix1::Matrix1() {}
|
|
|
|
Matrix1::Matrix1(int size) : size(size), externalData(false) {
|
|
if (this->size <= 0)
|
|
data = nullptr;
|
|
else
|
|
this->data = new float[size]();
|
|
}
|
|
|
|
Matrix1::Matrix1(int size, float* data) : data(data), size(size) {
|
|
this->externalData = true;
|
|
}
|
|
|
|
Matrix1::Matrix1(const Matrix1& m) : size(m.size) {
|
|
if (this->size <= 0)
|
|
this->data = nullptr;
|
|
else {
|
|
this->data = new float[this->size];
|
|
|
|
for (int ix = 0; ix < this->size; ++ix)
|
|
this->data[ix] = m.data[ix];
|
|
}
|
|
}
|
|
|
|
Matrix1::Matrix1(Matrix1&& m) noexcept : data(m.data), size(m.size) {
|
|
m.data = nullptr; // Leave the moved-from object in a safe state
|
|
m.size = 0;
|
|
}
|
|
|
|
Matrix1::~Matrix1() {
|
|
if (!this->externalData) {
|
|
if (this->size > 0)
|
|
delete[] data;
|
|
}
|
|
}
|
|
|
|
Matrix1& Matrix1::operator=(const Matrix1& m) {
|
|
if (this != &m) {
|
|
if (this->size > 0)
|
|
delete[] this->data; // Free the current memory
|
|
|
|
this->size = m.size;
|
|
this->externalData = m.externalData;
|
|
if (this->size == 0)
|
|
this->data = nullptr;
|
|
else {
|
|
this->data = new float[this->size];
|
|
for (int ix = 0; ix < this->size; ++ix)
|
|
this->data[ix] = m.data[ix];
|
|
}
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
Matrix1& Matrix1::operator=(Matrix1&& m) noexcept {
|
|
if (this != &m) {
|
|
if (this->size > 0) {
|
|
// printf("Matrix1 Move Assignment Deallocate %d\n", this->size);
|
|
delete[] this->data; // Free the current memory
|
|
}
|
|
|
|
// Transfer ownership of resources
|
|
this->size = m.size;
|
|
this->data = m.data;
|
|
this->externalData = m.externalData;
|
|
|
|
// Leave the moved-from object in a valid state
|
|
m.size = 0;
|
|
m.data = nullptr;
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
unsigned int Matrix1::Size() const {
|
|
return this->size;
|
|
}
|
|
|
|
void Matrix1::Resize(int size) {
|
|
if (!this->externalData && this->size > 0)
|
|
delete[] this->data;
|
|
|
|
this->externalData = false;
|
|
|
|
if (size <= 0) {
|
|
this->data = nullptr;
|
|
this->size = 0;
|
|
} else {
|
|
this->data = new float[size]();
|
|
}
|
|
}
|
|
|
|
Matrix1 Matrix1::FromVector3(Vector3Float v) {
|
|
Matrix1 r(3);
|
|
r.data[0] = v.horizontal;
|
|
r.data[1] = v.vertical;
|
|
r.data[2] = v.depth;
|
|
return r;
|
|
}
|
|
|
|
Matrix1 Matrix1::Zero(unsigned int size) {
|
|
Matrix1 r(size);
|
|
r.Fill(0);
|
|
return r;
|
|
}
|
|
|
|
void Matrix1::Fill(float value) {
|
|
for (int ix = 0; ix < this->size; ix++)
|
|
this->data[ix] = value;
|
|
}
|
|
|
|
void Matrix1::Fill(float value, unsigned int start, unsigned int stop) {
|
|
if (start > stop || start > this->size)
|
|
return;
|
|
|
|
if (stop > this->size)
|
|
stop = this->size;
|
|
|
|
for (unsigned int ix = start; ix < stop; ix++)
|
|
this->data[ix] = value;
|
|
}
|
|
|
|
Vector3Float Matrix1::ToVector3() {
|
|
return Vector3Float(this->data[0], this->data[1], this->data[2]);
|
|
}
|
|
|
|
Matrix1 LinearAlgebra::Matrix1::FromQuaternion(Quaternion q) {
|
|
Matrix1 r = Matrix1(4);
|
|
float* data = r.data;
|
|
data[0] = q.x;
|
|
data[1] = q.y;
|
|
data[2] = q.z;
|
|
data[3] = q.w;
|
|
return r;
|
|
}
|
|
|
|
Quaternion LinearAlgebra::Matrix1::ToQuaternion() {
|
|
return Quaternion(this->data[0], this->data[1], this->data[2], this->data[3]);
|
|
}
|
|
|
|
Matrix1 Matrix1::Slice(int start, int stop) {
|
|
if (start < 0 || stop >= this->size)
|
|
std::cerr << "Slice index out of range." << std::endl;
|
|
|
|
Matrix1 result(stop - start);
|
|
int resultIx = 0;
|
|
for (int ix = start; ix < stop; ix++)
|
|
result.data[resultIx++] = this->data[ix];
|
|
|
|
return result;
|
|
}
|
|
|
|
void Matrix1::UpdateSlice(int start, int stop, const Matrix1& source) const {
|
|
if (start > stop || start > this->size || start > source.size)
|
|
return;
|
|
|
|
if (stop > this->size)
|
|
stop = this->size;
|
|
if (stop > source.size)
|
|
stop = source.size;
|
|
|
|
unsigned int sourceIx = 0;
|
|
for (int ix = start; ix < stop; ix++, sourceIx++) {
|
|
this->data[ix] = source.data[sourceIx];
|
|
}
|
|
}
|
|
|
|
const float& Matrix1::operator()(int ix) const {
|
|
return data[ix];
|
|
}
|
|
float& Matrix1::operator()(int ix) {
|
|
return data[ix];
|
|
}
|
|
|
|
// Matrix1
|
|
#pragma endregion
|
|
*/
|
|
|
|
/*
|
|
#pragma region Matrix2
|
|
|
|
Matrix2::Matrix2() {}
|
|
|
|
Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols), externalData(false) {
|
|
this->size = nRows * nCols;
|
|
|
|
if (this->size <= 0) {
|
|
this->data = nullptr;
|
|
return;
|
|
}
|
|
|
|
// printf("Matrix2 Allocate %d\n", this->size * sizeof(float));
|
|
this->data = new float[this->size];
|
|
}
|
|
|
|
Matrix2::Matrix2(int nRows, int nCols, float* data) : nRows(nRows), nCols(nCols), data(data) {
|
|
// printf("Matrix2 No-Allocate %d\n", this->size * sizeof(float));
|
|
this->size = nRows * nCols;
|
|
this->externalData = true;
|
|
}
|
|
|
|
Matrix2::Matrix2(const Matrix2& m) : nRows(m.nRows), nCols(m.nCols), size(m.size) {
|
|
// printf("Matrix2 Copy Allocate %d\n", this->size * sizeof(float));
|
|
if (this->size == 0)
|
|
this->data = nullptr;
|
|
else {
|
|
this->data = new float[this->size];
|
|
|
|
for (int ix = 0; ix < this->size; ++ix)
|
|
this->data[ix] = m.data[ix];
|
|
}
|
|
}
|
|
|
|
Matrix2::Matrix2(Matrix2&& m) noexcept
|
|
: nRows(m.nRows), nCols(m.nCols), size(m.size), data(m.data) {
|
|
m.nRows = 0;
|
|
m.nCols = 0;
|
|
m.size = 0;
|
|
m.data = nullptr; // Leave the moved-from object in a safe state
|
|
}
|
|
|
|
Matrix2& Matrix2::operator=(const Matrix2& m) {
|
|
if (this != &m) {
|
|
if (this->size > 0) {
|
|
// printf("Matrix2 Copy Assignment Deallocate %d\n", this->size * sizeof(float));
|
|
delete[] this->data; // Free the current memory
|
|
}
|
|
|
|
this->nRows = m.nRows;
|
|
this->nCols = m.nCols;
|
|
this->size = m.size;
|
|
this->externalData = m.externalData;
|
|
if (this->size == 0)
|
|
this->data = nullptr;
|
|
else {
|
|
this->data = new float[this->size];
|
|
for (int ix = 0; ix < this->size; ++ix)
|
|
this->data[ix] = m.data[ix];
|
|
}
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
Matrix2& Matrix2::operator=(Matrix2&& m) noexcept {
|
|
if (this != &m) {
|
|
if (this->size > 0) {
|
|
// printf("Matrix2 Move Assignment Deallocate %d\n", this->size * sizeof(float));
|
|
delete[] this->data; // Free the current memory
|
|
}
|
|
|
|
// Transfer ownership of resources
|
|
this->nRows = m.nRows;
|
|
this->nCols = m.nCols;
|
|
this->size = m.size;
|
|
this->externalData = m.externalData;
|
|
this->data = m.data;
|
|
|
|
// Leave the moved-from object in a valid state
|
|
m.size = 0;
|
|
m.data = nullptr;
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
Matrix2::~Matrix2() {
|
|
if (!this->externalData) {
|
|
if (this->size > 0) {
|
|
// printf("Matrix2 Deallocate %d\n", this->size * sizeof(float));
|
|
delete[] data;
|
|
}
|
|
}
|
|
}
|
|
|
|
unsigned int Matrix2::NRows() const {
|
|
return this->nRows;
|
|
}
|
|
|
|
unsigned int Matrix2::NCols() const {
|
|
return this->nCols;
|
|
}
|
|
|
|
Matrix2 Matrix2::Zero(int nRows, int nCols) {
|
|
Matrix2 r = Matrix2(nRows, nCols);
|
|
for (int ix = 0; ix < r.size; ix++)
|
|
r.data[ix] = 0;
|
|
return r;
|
|
}
|
|
|
|
const float& Matrix2::operator()(int row, int col) const {
|
|
return data[row * this->nCols + col];
|
|
}
|
|
float& Matrix2::operator()(int row, int col) {
|
|
return data[row * this->nCols + col];
|
|
}
|
|
|
|
void Matrix2::Fill(float value) {
|
|
for (int ix = 0; ix < this->size; ix++)
|
|
this->data[ix] = value;
|
|
}
|
|
|
|
void Matrix2::FillDiagonal(float f) {
|
|
int n = this->nRows < this->nCols ? nRows : nCols;
|
|
for (int ix = 0; ix < n; ix++)
|
|
this->data[ix * this->nCols + ix] = f;
|
|
}
|
|
|
|
Matrix2 Matrix2::Identity(int size) {
|
|
return Diagonal(1, size);
|
|
}
|
|
|
|
Matrix2 Matrix2::Identity(int nRows, int nCols) {
|
|
Matrix2 m = Matrix2::Zero(nRows, nCols);
|
|
m.FillDiagonal(1);
|
|
return m;
|
|
}
|
|
|
|
Matrix2 Matrix2::Diagonal(float f, int size) {
|
|
Matrix2 r = Matrix2::Zero(size, size);
|
|
float* data = r.data;
|
|
int valueIx = 0;
|
|
for (int ix = 0; ix < size; ix++) {
|
|
data[valueIx] = f;
|
|
valueIx += size + 1;
|
|
}
|
|
return r;
|
|
}
|
|
|
|
Matrix2 Matrix2::SkewMatrix(const Vector3Of<float>& v) {
|
|
Matrix2 r = Matrix2(3, 3);
|
|
float* data = r.data;
|
|
data[0 * 3 + 0] = 0;
|
|
data[0 * 3 + 1] = -v.depth; // result(0, 1)
|
|
data[0 * 3 + 2] = v.vertical; // result(0, 2)
|
|
data[1 * 3 + 0] = v.depth; // result(1, 0)
|
|
data[1 * 3 + 1] = 0;
|
|
data[1 * 3 + 2] = -v.horizontal; // result(1, 2)
|
|
data[2 * 3 + 0] = -v.vertical; // result(2, 0)
|
|
data[2 * 3 + 1] = v.horizontal; // result(2, 1)
|
|
data[2 * 3 + 2] = 0;
|
|
return r;
|
|
}
|
|
|
|
Matrix2 Matrix2::Transpose() const {
|
|
Matrix2 r = Matrix2(this->nCols, this->nRows);
|
|
|
|
for (int rowIx = 0; rowIx < this->nRows; rowIx++) {
|
|
for (int colIx = 0; colIx < this->nCols; colIx++)
|
|
r.data[colIx * this->nRows + rowIx] = this->data[rowIx * this->nCols + colIx];
|
|
}
|
|
return r;
|
|
}
|
|
|
|
Matrix2 LinearAlgebra::Matrix2::operator-() const {
|
|
Matrix2 r = Matrix2(this->nRows, this->nCols);
|
|
for (int ix = 0; ix < r.size; ix++)
|
|
r.data[ix] = -this->data[ix];
|
|
return r;
|
|
}
|
|
|
|
Matrix2 LinearAlgebra::Matrix2::operator+(const Matrix2& v) const {
|
|
Matrix2 r = Matrix2(this->nRows, this->nCols);
|
|
for (int ix = 0; ix < r.size; ix++)
|
|
r.data[ix] = this->data[ix] + v.data[ix];
|
|
return r;
|
|
}
|
|
|
|
Matrix2& Matrix2::operator+=(const Matrix2& v) {
|
|
for (int ix = 0; ix < this->size; ix++)
|
|
this->data[ix] += v.data[ix];
|
|
return *this;
|
|
}
|
|
|
|
Matrix2 Matrix2::operator-(const Matrix2& v) const {
|
|
Matrix2 r = Matrix2(this->nRows, this->nCols);
|
|
for (int ix = 0; ix < r.size; ix++)
|
|
r.data[ix] = this->data[ix] - v.data[ix];
|
|
return r;
|
|
}
|
|
|
|
Matrix1 operator*(const Matrix2& m, const Matrix1& v) {
|
|
Matrix1 r = Matrix1(m.nRows);
|
|
for (int rowIx = 0; rowIx < m.nRows; rowIx++) {
|
|
int mRowIx = rowIx * m.nCols;
|
|
for (int colIx = 0; colIx < m.nCols; colIx++)
|
|
r(rowIx) += m.data[mRowIx + colIx] * v(colIx);
|
|
// r(rowIx) += m(rowIx, colIx) * v(colIx);
|
|
}
|
|
return r;
|
|
}
|
|
|
|
Matrix2 Matrix2::operator*(const Matrix2& B) const {
|
|
Matrix2 r = Matrix2(this->nRows, B.nCols);
|
|
|
|
const int ACols = this->nCols;
|
|
const int BCols = B.nCols;
|
|
const int ARows = this->nRows;
|
|
// const int BRows = B.nRows;
|
|
|
|
for (int i = 0; i < ARows; ++i) {
|
|
// Pre-compute row offsets
|
|
int ARowOffset = i * ACols; // ARowOffset is constant for each row of A
|
|
int BColOffset = i * BCols; // BColOffset is constant for each row of B
|
|
for (int j = 0; j < BCols; ++j) {
|
|
float sum = 0;
|
|
// printf(" 0");
|
|
int BIndex = j;
|
|
for (int k = 0; k < ACols; ++k) {
|
|
// printf(" + %f * %f", this->data[ARowOffset + k], B.data[BIndex]);
|
|
// std::cout << " + " << this->data[ARowOffset + k] << " * "
|
|
// << B.data[BIndex];
|
|
sum += this->data[ARowOffset + k] * B.data[BIndex];
|
|
BIndex += BCols;
|
|
}
|
|
r.data[BColOffset + j] = sum;
|
|
// std::cout << " = " << sum << " ix: " << BColOffset + j << "\n";
|
|
// if (sum != std::trunc(sum)) {
|
|
// int BIndex = j;
|
|
// printf(" 0");
|
|
// for (int k = 0; k < ACols; ++k) {
|
|
// printf(" + %f * %f", this->data[ARowOffset + k], B.data[BIndex]);
|
|
// BIndex += BCols;
|
|
// }
|
|
// printf(" = %f ix: %d\n", sum, BColOffset + j);
|
|
// }
|
|
}
|
|
}
|
|
|
|
// if (ACols != BRows)
|
|
// printf("### Acols != Nrows\n");
|
|
|
|
// for (size_t i = 0; i < ARows; ++i) {
|
|
// for (size_t p = 0; p < ACols; ++p) {
|
|
// float a_ip = this->data[i * ACols + p]; // A(i,p)
|
|
// for (size_t j = 0; j < BCols; ++j) {
|
|
// r.data[i * BCols + j] += a_ip * B.data[p * BCols + j]; //
|
|
// C(i,j) += A(i,p)*B(p,j)
|
|
// }
|
|
// }
|
|
// }
|
|
|
|
return r;
|
|
}
|
|
|
|
Matrix2 Matrix2::GetRows(int from, int to) {
|
|
if (from < 0 || to >= this->nRows)
|
|
std::cerr << "Slice index out of range." << std::endl;
|
|
|
|
Matrix2 result = Matrix2(to - from, this->nCols);
|
|
int resultRowIx = 0;
|
|
for (int rowIx = from; rowIx < to; rowIx++) {
|
|
for (int colIx = 0; colIx < this->nCols; colIx++)
|
|
result.data[resultRowIx * result.nCols + colIx] = this->data[rowIx * this->nCols + colIx];
|
|
|
|
resultRowIx++;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
Vector3 Matrix2::GetRow3(int rowIx) {
|
|
int cellIx = rowIx * this->nCols;
|
|
Vector3 row(this->data[cellIx + 0], this->data[cellIx + 1], this->data[cellIx + 2]);
|
|
return row;
|
|
}
|
|
|
|
void Matrix2::SetRow(int rowIx, Matrix1 source) {
|
|
int cellIx = rowIx * this->nCols;
|
|
for (int ix = 0; ix < source.Size(); ix++)
|
|
this->data[cellIx + ix] = source(ix);
|
|
}
|
|
|
|
void Matrix2::SetRow3(int rowIx, Vector3 v) {
|
|
int cellIx = rowIx * this->nCols;
|
|
this->data[cellIx + 0] = v.x;
|
|
this->data[cellIx + 1] = v.y;
|
|
this->data[cellIx + 2] = v.z;
|
|
}
|
|
|
|
Matrix2 Matrix2::Slice(int rowStart, int rowStop, int colStart, int colStop) const {
|
|
Matrix2 r = Matrix2(rowStop - rowStart, colStop - colStart);
|
|
|
|
for (int i = rowStart; i < rowStop; i++) {
|
|
for (int j = colStart; j < colStop; j++) {
|
|
int resultRowIx = i - rowStart;
|
|
int resultColIx = j - colStart;
|
|
r.data[resultRowIx * r.nCols + resultColIx] = this->data[i * this->nCols + j];
|
|
}
|
|
}
|
|
return r;
|
|
}
|
|
|
|
void Matrix2::UpdateSlice(int rowStart, int rowStop, const Matrix2& m) const {
|
|
UpdateSlice(rowStart, rowStop, 0, this->nCols, m);
|
|
}
|
|
|
|
void Matrix2::UpdateSlice(int rowStart,
|
|
int rowStop,
|
|
int colStart,
|
|
int colStop,
|
|
const Matrix2& m) const {
|
|
int mRowDataIx = 0;
|
|
// printf("UpdateSlice ====== (%d, %d) -> (%d, %d)\n", rowStart, rowStop,
|
|
// colStart, colStop);
|
|
for (int rowIx = rowStart; rowIx < rowStop; rowIx++) {
|
|
int rRowDataIx = rowIx * this->nCols;
|
|
for (int colIx = colStart; colIx < colStop; colIx++) {
|
|
this->data[rRowDataIx + colIx] = m.data[mRowDataIx + (colIx - colStart)];
|
|
// printf("UpdateSlice (%d, %d) -> (%d, %d) = %F\n", mRowDataIx / m.nCols,
|
|
// (colIx - colStart), rowIx, colIx, m.data[mRowDataIx + (colIx - colStart)]);
|
|
}
|
|
mRowDataIx += m.nCols;
|
|
}
|
|
}
|
|
|
|
|
|
// Matrix2 Matrix2::Inverse() {
|
|
// int n = this->nRows;
|
|
// // // Create an identity matrix of the same size as the original matrix
|
|
// Matrix2 augmentedMatrix(n, 2 * n);
|
|
// for (int i = 0; i < n; i++) {
|
|
// for (int j = 0; j < n; j++) {
|
|
// int ix = i * this->nCols + j;
|
|
// augmentedMatrix.data[ix] = this->data[ix];
|
|
// augmentedMatrix.data[ix + n] = (i == j) ? 1 : 0; // identity matrix;
|
|
// }
|
|
// if (n == 3) {
|
|
// int ix2 = i * this->nCols;
|
|
// std::cerr << augmentedMatrix.data[ix2] << " " << augmentedMatrix.data[ix2+1] << " "
|
|
// << augmentedMatrix.data[ix2+2] << " " << augmentedMatrix.data[ix2+3] << " "
|
|
// << augmentedMatrix.data[ix2+4] << " " << augmentedMatrix.data[ix2+5] << "\n";
|
|
// }
|
|
// }
|
|
|
|
// // Perform Gaussian elimination
|
|
// for (int i = 0; i < n; i++) {
|
|
// // Find the pivot row
|
|
// float pivot = augmentedMatrix.data[i * augmentedMatrix.nCols + i];
|
|
// std::cerr << "pivot " << (int) i << " | " << (i * augmentedMatrix.nCols + i) << " = " << pivot
|
|
// << std::endl; if (abs(pivot) < 1e-10) // Check for singular matrix std::cerr << "Matrix is singular
|
|
// and cannot be inverted. " << (int)i << std::endl;
|
|
|
|
// // Normalize the pivot row
|
|
// for (int j = 0; j < 2 * n; j++)
|
|
// augmentedMatrix.data[i * augmentedMatrix.nCols + j] /= pivot;
|
|
|
|
// // Eliminate the column below the pivot
|
|
// for (int j = i + 1; j < n; j++) {
|
|
// float factor = augmentedMatrix.data[j * augmentedMatrix.nCols + i];
|
|
// for (int k = 0; k < 2 * n; k++)
|
|
// augmentedMatrix.data[j * augmentedMatrix.nCols + k] -=
|
|
// factor * augmentedMatrix.data[i * augmentedMatrix.nCols + k];
|
|
// }
|
|
// }
|
|
|
|
// // Back substitution
|
|
// for (int i = n - 1; i >= 0; i--) {
|
|
// // Eliminate the column above the pivot
|
|
// for (int j = i - 1; j >= 0; j--) {
|
|
// float factor = augmentedMatrix.data[j * augmentedMatrix.nCols + i];
|
|
// for (int k = 0; k < 2 * n; k++)
|
|
// augmentedMatrix.data[j * augmentedMatrix.nCols + k] -=
|
|
// factor * augmentedMatrix.data[i * augmentedMatrix.nCols + k];
|
|
// }
|
|
// }
|
|
|
|
// // Extract the inverse matrix from the augmented matrix
|
|
// Matrix2 inverse(n, n);
|
|
// for (int i = 0; i < n; i++) {
|
|
// for (int j = 0; j < n; j++)
|
|
// inverse.data[i * inverse.nCols + j] = augmentedMatrix.data[i * augmentedMatrix.nCols + j + n];
|
|
// }
|
|
// return inverse;
|
|
// }
|
|
|
|
Matrix2 Matrix2::Inverse() {
|
|
int n = this->nRows; // need to check for nRows = nCols...
|
|
Matrix2 matrix = Matrix2(*this); // clone this matrix
|
|
Matrix2 inverse = Matrix2::Identity(n);
|
|
|
|
// Forward elimination
|
|
for (int i = 0; i < n; ++i) {
|
|
float pivot = matrix(i, i);
|
|
if (abs(pivot) < 1e-10) // Check for singular matrix
|
|
std::cerr << "Matrix is singular and cannot be inverted. " << (int)i << std::endl;
|
|
|
|
for (int j = 0; j < n; ++j) {
|
|
matrix(i, j) /= pivot; // Normalize pivot row
|
|
inverse(i, j) /= pivot; // Apply the same to inverse
|
|
}
|
|
|
|
for (int j = 0; j < n; ++j) {
|
|
if (j != i) { // outside diagonal
|
|
float factor = matrix(j, i);
|
|
for (int k = 0; k < n; ++k) {
|
|
matrix(j, k) -= factor * matrix(i, k); // Eliminate column
|
|
inverse(j, k) -= factor * inverse(i, k); // Update inverse
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return inverse;
|
|
}
|
|
|
|
Matrix2 Matrix2::DeleteRows(int rowStart, int rowStop) const {
|
|
Matrix2 r = Matrix2(this->nRows - (rowStop - rowStart), this->nCols);
|
|
|
|
int resultRowIx = 0;
|
|
for (int i = 0; i < this->nRows; i++) {
|
|
if (i >= rowStart && i < rowStop)
|
|
continue;
|
|
|
|
for (int j = 0; j < this->nCols; j++)
|
|
r.data[resultRowIx * r.nCols + j] = this->data[i * this->nCols + j];
|
|
|
|
resultRowIx++;
|
|
}
|
|
return r;
|
|
}
|
|
|
|
Matrix2 Matrix2::DeleteColumns(int colStart, int colStop) const {
|
|
Matrix2 r = Matrix2(this->nRows, this->nCols - (colStop - colStart));
|
|
|
|
for (int i = 0; i < this->nRows; i++) {
|
|
int resultColIx = 0;
|
|
for (int j = 0; j < this->nCols; j++) {
|
|
if (j >= colStart && j < colStop)
|
|
continue;
|
|
|
|
r.data[i * r.nCols + resultColIx++] = this->data[i * this->nCols + j];
|
|
}
|
|
}
|
|
return r;
|
|
}
|
|
|
|
/// @brief Compute the Omega matrix of a 3D vector
|
|
/// @param v The vector
|
|
/// @return 4x4 Omega matrix
|
|
template <typename T>
|
|
Matrix2 Matrix2::Omega(const Vector3Of<T>& v) {
|
|
Matrix2 r = Matrix2::Zero(4, 4);
|
|
r.UpdateSlice(0, 3, 0, 3, -Matrix2::SkewMatrix(v));
|
|
|
|
// set last row to -v
|
|
int ix = 3 * 4;
|
|
r.data[ix++] = -v.horizontal;
|
|
r.data[ix++] = -v.vertical;
|
|
r.data[ix] = -v.depth;
|
|
|
|
// Set last column to v
|
|
ix = 3;
|
|
r.data[ix += 4] = v.horizontal;
|
|
r.data[ix += 4] = v.vertical;
|
|
r.data[ix] = v.depth;
|
|
return r;
|
|
}
|
|
|
|
// Matrix2
|
|
#pragma endregion
|
|
|
|
#pragma region Block // Submatrix2
|
|
|
|
SubMatrix2::SubMatrix2(const Matrix2& m, int startRow, int startCol, int rowCount, int colCount)
|
|
: parent(m), startRow(startRow), startCol(startCol), rowCount(rowCount), colCount(colCount) {
|
|
assert(this->startRow >= 0 && this->startCol >= 0);
|
|
assert(this->startRow + this->rowCount <= this->parent.NRows() &&
|
|
this->startCol + this->colCount <= this->parent.NCols());
|
|
}
|
|
|
|
void SubMatrix2::operator=(const Matrix2& src) {
|
|
assert(src.NRows() == this->rowCount && src.NCols() == this->colCount);
|
|
|
|
// If the block is contiguous in memory and src is contiguous, we can memcpy
|
|
// row by row. In row-major layout, each row of the block is contiguous.
|
|
for (int row = 0; row < this->rowCount; row++) {
|
|
float* dstRowPtr =
|
|
this->parent.GetData() + (this->startRow + row) * this->parent.NCols() + this->startCol;
|
|
const float* srcRowPtr = src.GetData() + row * src.NCols();
|
|
// printf("(%d, %d) %d (%d -> %d)\n", this->startRow + row, startCol,
|
|
// this->colCount, (int)srcRowPtr, (int)dstRowPtr); memcpy each row
|
|
std::memcpy(dstRowPtr, srcRowPtr, sizeof(float) * this->colCount);
|
|
}
|
|
}
|
|
|
|
void SubMatrix2::operator=(const SubMatrix2& src) {
|
|
assert(src.rowCount == this->rowCount && src.colCount == this->colCount);
|
|
|
|
// If src.parent == parent and regions overlap, we must copy safely (use
|
|
// temp row buffer).
|
|
bool alias =
|
|
(&src.parent == &this->parent) && !((src.startRow + src.rowCount <= this->startRow) ||
|
|
(this->startRow + this->rowCount <= src.startRow) ||
|
|
(src.startCol + src.colCount <= this->startCol) ||
|
|
(this->startCol + this->colCount <= src.startCol));
|
|
|
|
if (!alias) {
|
|
// safe to copy row-by-row directly
|
|
for (int row = 0; row < this->rowCount; row++) {
|
|
float* dstRowPtr =
|
|
this->parent.GetData() + (this->startRow + row) * this->parent.NCols() + this->startCol;
|
|
const float* srcRowPtr =
|
|
src.parent.GetData() + (src.startRow + row) * src.parent.NCols() + src.startCol;
|
|
std::memcpy(dstRowPtr, srcRowPtr, sizeof(float) * this->colCount);
|
|
}
|
|
} else {
|
|
// overlapping regions in same parent: copy to temporary then assign
|
|
Matrix2 tmp(this->rowCount, this->colCount);
|
|
for (int row = 0; row < this->rowCount; row++) {
|
|
const float* srcRowPtr =
|
|
src.parent.GetData() + (src.startRow + row) * src.parent.NCols() + src.startCol;
|
|
std::memcpy(tmp.GetData() + row * this->colCount, srcRowPtr, sizeof(float) * this->colCount);
|
|
}
|
|
// now copy tmp into destination
|
|
for (int row = 0; row < this->rowCount; row++) {
|
|
float* dstRowPtr =
|
|
parent.GetData() + (this->startRow + row) * this->parent.NCols() + this->startCol;
|
|
std::memcpy(dstRowPtr, tmp.GetData() + row * this->colCount, sizeof(float) * this->colCount);
|
|
}
|
|
}
|
|
}
|
|
|
|
float SubMatrix2::operator()(int row, int col) const {
|
|
assert(row >= 0 && col >= 0 && row < rowCount && col < colCount);
|
|
return parent(startRow + row, startCol + col);
|
|
}
|
|
|
|
const float& SubMatrix2::operator()(int row, int col) {
|
|
assert(row >= 0 && col >= 0 && row < rowCount && col < colCount);
|
|
return parent(startRow + row, startCol + col);
|
|
}
|
|
|
|
#pragma endregion Block
|
|
*/
|
|
|
|
#pragma region Matrix2Of
|
|
|
|
template <typename T>
|
|
Matrix2Of<T>::Matrix2Of() {}
|
|
|
|
template <typename T>
|
|
Matrix2Of<T>::Matrix2Of(unsigned int rowCount, unsigned int colCount)
|
|
: nRows(rowCount), nCols(colCount), externalData(false) {
|
|
this->size = this->nRows * this->nCols;
|
|
|
|
if (this->size <= 0) {
|
|
this->data = nullptr;
|
|
return;
|
|
}
|
|
|
|
this->data = new T[this->size];
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix2Of<T>::Matrix2Of(unsigned int rowCount, unsigned int colCount, T* data)
|
|
: nRows(rowCount), nCols(colCount), data(data), externalData(true) {
|
|
this->size = this->nRows * this->nCols;
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix2Of<T>::Matrix2Of(const Matrix2Of& m) : nRows(m.nRows), nCols(m.nCols), size(m.size) {
|
|
if (this->size == 0)
|
|
this->data = nullptr;
|
|
else {
|
|
this->data = new T[this->size];
|
|
|
|
for (unsigned int ix = 0; ix < this->size; ++ix)
|
|
this->data[ix] = m.data[ix];
|
|
}
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix2Of<T>::Matrix2Of(Matrix2Of&& m) noexcept
|
|
: nRows(m.nRows), nCols(m.nCols), size(m.size), data(m.data) {
|
|
m.nRows = 0;
|
|
m.nCols = 0;
|
|
m.size = 0;
|
|
m.data = nullptr; // Leave the moved-from object in a safe state
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix2Of<T>::~Matrix2Of() {
|
|
if (!this->externalData) {
|
|
if (this->size > 0)
|
|
delete[] data;
|
|
}
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix2Of<T>& Matrix2Of<T>::operator=(const Matrix2Of& m) {
|
|
if (this != &m) {
|
|
if (this->size > 0) {
|
|
delete[] this->data; // Free the current memory
|
|
}
|
|
|
|
this->nRows = m.nRows;
|
|
this->nCols = m.nCols;
|
|
this->size = m.size;
|
|
this->externalData = m.externalData;
|
|
if (this->size == 0)
|
|
this->data = nullptr;
|
|
else {
|
|
this->data = new T[this->size];
|
|
for (unsigned int ix = 0; ix < this->size; ++ix)
|
|
this->data[ix] = m.data[ix];
|
|
}
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix2Of<T>& Matrix2Of<T>::operator=(Matrix2Of&& m) noexcept {
|
|
if (this != &m) {
|
|
if (this->size > 0) {
|
|
delete[] this->data; // Free the current memory
|
|
}
|
|
|
|
// Transfer ownership of resources
|
|
this->nRows = m.nRows;
|
|
this->nCols = m.nCols;
|
|
this->size = m.size;
|
|
this->externalData = m.externalData;
|
|
this->data = m.data;
|
|
|
|
// Leave the moved-from object in a valid state
|
|
m.size = 0;
|
|
m.data = nullptr;
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
template <typename T>
|
|
unsigned int Matrix2Of<T>::RowCount() const {
|
|
return this->nRows;
|
|
}
|
|
template <typename T>
|
|
unsigned int Matrix2Of<T>::ColCount() const {
|
|
return this->nCols;
|
|
}
|
|
|
|
template <typename T>
|
|
const T& Matrix2Of<T>::operator()(unsigned int rowIx, unsigned int colIx) const {
|
|
return data[rowIx * this->nCols + colIx];
|
|
}
|
|
template <typename T>
|
|
T& Matrix2Of<T>::operator()(unsigned int rowIx, unsigned int colIx) {
|
|
return data[rowIx * this->nCols + colIx];
|
|
}
|
|
|
|
template <typename T>
|
|
T* Matrix2Of<T>::GetData() {
|
|
return this->data;
|
|
}
|
|
template <typename T>
|
|
T* Matrix2Of<T>::GetData() const {
|
|
return this->data;
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix2Of<T> Matrix2Of<T>::Zero(unsigned int rowCount, unsigned int colCount) {
|
|
Matrix2Of r(rowCount, colCount);
|
|
for (unsigned int ix = 0; ix < r.size; ix++)
|
|
r.data[ix] = 0;
|
|
return r;
|
|
}
|
|
|
|
template <typename T>
|
|
void Matrix2Of<T>::Fill(T value) {
|
|
for (unsigned int ix = 0; ix < this->size; ix++)
|
|
this->data[ix] = value;
|
|
}
|
|
|
|
template <typename T>
|
|
void Matrix2Of<T>::FillDiagonal(T f) {
|
|
int n = this->nRows < this->nCols ? nRows : nCols;
|
|
for (int ix = 0; ix < n; ix++)
|
|
this->data[ix * this->nCols + ix] = f;
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix2Of<T> Matrix2Of<T>::Identity(unsigned int size) {
|
|
return Diagonal(1, size);
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix2Of<T> Matrix2Of<T>::Identity(unsigned int rowCount, unsigned int colCount) {
|
|
Matrix2Of<T> m = Matrix2Of::Zero(rowCount, colCount);
|
|
m.FillDiagonal(1);
|
|
return m;
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix2Of<T> Matrix2Of<T>::Diagonal(T f, unsigned int size) {
|
|
Matrix2Of r = Matrix2Of::Zero(size, size);
|
|
T* data = r.data;
|
|
int valueIx = 0;
|
|
for (unsigned int ix = 0; ix < size; ix++) {
|
|
data[valueIx] = f;
|
|
valueIx += size + 1;
|
|
}
|
|
return r;
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix2Of<T> Matrix2Of<T>::SkewMatrix(const Vector3Of<T>& v) {
|
|
Matrix2Of r(3, 3);
|
|
T* data = r.data;
|
|
data[0 * 3 + 0] = 0;
|
|
data[0 * 3 + 1] = -v.depth; // result(0, 1)
|
|
data[0 * 3 + 2] = v.vertical; // result(0, 2)
|
|
data[1 * 3 + 0] = v.depth; // result(1, 0)
|
|
data[1 * 3 + 1] = 0;
|
|
data[1 * 3 + 2] = -v.horizontal; // result(1, 2)
|
|
data[2 * 3 + 0] = -v.vertical; // result(2, 0)
|
|
data[2 * 3 + 1] = v.horizontal; // result(2, 1)
|
|
data[2 * 3 + 2] = 0;
|
|
return r;
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix1Of<T> Matrix2Of<T>::GetRow(unsigned int rowIx) {
|
|
Matrix1Of<T> r = Matrix1Of<T>(this->nCols);
|
|
unsigned int rowStartIx = rowIx * this->nCols;
|
|
for (unsigned int colIx = 0; colIx < this->nCols; colIx++)
|
|
r.data[colIx] = this->data[rowStartIx + colIx];
|
|
return r;
|
|
}
|
|
|
|
template <typename T>
|
|
Vector3Of<T> Matrix2Of<T>::GetRow3(unsigned int rowIx) {
|
|
int cellIx = rowIx * this->nCols;
|
|
Vector3Of<T> row(this->data[cellIx + 0], this->data[cellIx + 1], this->data[cellIx + 2]);
|
|
return row;
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix2Of<T> Matrix2Of<T>::GetRows(unsigned int fromRowIx, unsigned int toRowIx) {
|
|
if (toRowIx >= this->nRows)
|
|
std::cerr << "Slice index out of range." << std::endl;
|
|
|
|
Matrix2Of result = Matrix2Of(toRowIx - fromRowIx, this->nCols);
|
|
int resultRowIx = 0;
|
|
for (unsigned int rowIx = fromRowIx; rowIx < toRowIx; rowIx++) {
|
|
for (unsigned int colIx = 0; colIx < this->nCols; colIx++)
|
|
result.data[resultRowIx * result.nCols + colIx] = this->data[rowIx * this->nCols + colIx];
|
|
|
|
resultRowIx++;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
template <typename T>
|
|
void Matrix2Of<T>::SetRow(unsigned int rowIx, Matrix1Of<T> source) {
|
|
int cellIx = rowIx * this->nCols;
|
|
for (unsigned int ix = 0; ix < source.Size(); ix++)
|
|
this->data[cellIx + ix] = source(ix);
|
|
}
|
|
|
|
template <typename T>
|
|
void Matrix2Of<T>::SetRow3(unsigned int rowIx, Vector3Of<T> v) {
|
|
int cellIx = rowIx * this->nCols;
|
|
this->data[cellIx + 0] = v.horizontal;
|
|
this->data[cellIx + 1] = v.vertical;
|
|
this->data[cellIx + 2] = v.depth;
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix2Of<T> Matrix2Of<T>::DeleteRows(unsigned int fromRowIx, unsigned int toRowIx) const {
|
|
Matrix2Of r(this->nRows - (toRowIx - fromRowIx), this->nCols);
|
|
|
|
int resultRowIx = 0;
|
|
for (unsigned int i = 0; i < this->nRows; i++) {
|
|
if (i >= fromRowIx && i < toRowIx)
|
|
continue;
|
|
|
|
for (unsigned int j = 0; j < this->nCols; j++)
|
|
r.data[resultRowIx * r.nCols + j] = this->data[i * this->nCols + j];
|
|
|
|
resultRowIx++;
|
|
}
|
|
return r;
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix2Of<T> Matrix2Of<T>::DeleteColumns(unsigned int fromColIx, unsigned int toColIx) const {
|
|
Matrix2Of r(this->nRows, this->nCols - (toColIx - fromColIx));
|
|
|
|
for (unsigned int i = 0; i < this->nRows; i++) {
|
|
int resultColIx = 0;
|
|
for (unsigned int j = 0; j < this->nCols; j++) {
|
|
if (j >= fromColIx && j < toColIx)
|
|
continue;
|
|
|
|
r.data[i * r.nCols + resultColIx++] = this->data[i * this->nCols + j];
|
|
}
|
|
}
|
|
return r;
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix2Of<T> Matrix2Of<T>::operator-() const {
|
|
Matrix2Of r(this->nRows, this->nCols);
|
|
for (unsigned int ix = 0; ix < r.size; ix++)
|
|
r.data[ix] = -this->data[ix];
|
|
return r;
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix2Of<T> Matrix2Of<T>::operator+(const Matrix2Of& v) const {
|
|
Matrix2Of r(this->nRows, this->nCols);
|
|
for (unsigned int ix = 0; ix < r.size; ix++)
|
|
r.data[ix] = this->data[ix] + v.data[ix];
|
|
return r;
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix2Of<T>& Matrix2Of<T>::operator+=(const Matrix2Of& v) {
|
|
for (unsigned int ix = 0; ix < this->size; ix++)
|
|
this->data[ix] += v.data[ix];
|
|
return *this;
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix2Of<T> Matrix2Of<T>::operator-(const Matrix2Of& v) const {
|
|
Matrix2Of r(this->nRows, this->nCols);
|
|
for (unsigned int ix = 0; ix < r.size; ix++)
|
|
r.data[ix] = this->data[ix] - v.data[ix];
|
|
return r;
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix2Of<T>& Matrix2Of<T>::operator-=(const Matrix2Of& v) {
|
|
for (unsigned int ix = 0; ix < this->size; ix++)
|
|
this->data[ix] -= v.data[ix];
|
|
return *this;
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix2Of<T> Matrix2Of<T>::operator*(const Matrix2Of& B) const {
|
|
Matrix2Of r(this->nRows, B.nCols);
|
|
|
|
const int ACols = this->nCols;
|
|
const int BCols = B.nCols;
|
|
const int ARows = this->nRows;
|
|
|
|
for (int i = 0; i < ARows; ++i) {
|
|
// Pre-compute row offsets
|
|
int ARowOffset = i * ACols; // ARowOffset is constant for each row of A
|
|
int BColOffset = i * BCols; // BColOffset is constant for each row of B
|
|
for (int j = 0; j < BCols; ++j) {
|
|
T sum = 0;
|
|
int BIndex = j;
|
|
for (int k = 0; k < ACols; ++k) {
|
|
sum += this->data[ARowOffset + k] * B.data[BIndex];
|
|
BIndex += BCols;
|
|
}
|
|
r.data[BColOffset + j] = sum;
|
|
|
|
}
|
|
}
|
|
|
|
return r;
|
|
}
|
|
|
|
|
|
template <typename T>
|
|
Matrix2Of<T> Matrix2Of<T>::Transposed() const {
|
|
Matrix2Of r(this->nCols, this->nRows);
|
|
|
|
for (unsigned int rowIx = 0; rowIx < this->nRows; rowIx++) {
|
|
for (unsigned int colIx = 0; colIx < this->nCols; colIx++)
|
|
r.data[colIx * this->nRows + rowIx] = this->data[rowIx * this->nCols + colIx];
|
|
}
|
|
return r;
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix2Of<T> Matrix2Of<T>::Inverse() {
|
|
int n = this->nRows; // need to check for nRows = nCols...
|
|
Matrix2Of matrix = Matrix2Of(*this); // clone this matrix
|
|
Matrix2Of inverse = Matrix2Of::Identity(n);
|
|
|
|
// Forward elimination
|
|
for (int i = 0; i < n; ++i) {
|
|
T pivot = matrix(i, i);
|
|
if (abs(pivot) < 1e-10) // Check for singular matrix
|
|
std::cerr << "Matrix is singular and cannot be inverted. " << (int)i << std::endl;
|
|
|
|
for (int j = 0; j < n; ++j) {
|
|
matrix(i, j) /= pivot; // Normalize pivot row
|
|
inverse(i, j) /= pivot; // Apply the same to inverse
|
|
}
|
|
|
|
for (int j = 0; j < n; ++j) {
|
|
if (j != i) { // outside diagonal
|
|
T factor = matrix(j, i);
|
|
for (int k = 0; k < n; ++k) {
|
|
matrix(j, k) -= factor * matrix(i, k); // Eliminate column
|
|
inverse(j, k) -= factor * inverse(i, k); // Update inverse
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return inverse;
|
|
}
|
|
|
|
template <typename T>
|
|
Matrix2Of<T> Matrix2Of<T>::Slice(int rowStart, int rowStop, int colStart, int colStop) const {
|
|
Matrix2Of r(rowStop - rowStart, colStop - colStart);
|
|
|
|
for (int i = rowStart; i < rowStop; i++) {
|
|
for (int j = colStart; j < colStop; j++) {
|
|
int resultRowIx = i - rowStart;
|
|
int resultColIx = j - colStart;
|
|
r.data[resultRowIx * r.nCols + resultColIx] = this->data[i * this->nCols + j];
|
|
}
|
|
}
|
|
return r;
|
|
}
|
|
|
|
template <typename T>
|
|
void Matrix2Of<T>::UpdateSlice(int rowStart, int rowStop, const Matrix2Of& m) const {
|
|
UpdateSlice(rowStart, rowStop, 0, this->nCols, m);
|
|
}
|
|
|
|
template <typename T>
|
|
void Matrix2Of<T>::UpdateSlice(int rowStart,
|
|
int rowStop,
|
|
int colStart,
|
|
int colStop,
|
|
const Matrix2Of& m) const {
|
|
int mRowDataIx = 0;
|
|
// printf("UpdateSlice ====== (%d, %d) -> (%d, %d)\n", rowStart, rowStop,
|
|
// colStart, colStop);
|
|
for (int rowIx = rowStart; rowIx < rowStop; rowIx++) {
|
|
int rRowDataIx = rowIx * this->nCols;
|
|
for (int colIx = colStart; colIx < colStop; colIx++) {
|
|
this->data[rRowDataIx + colIx] = m.data[mRowDataIx + (colIx - colStart)];
|
|
// printf("UpdateSlice (%d, %d) -> (%d, %d) = %F\n", mRowDataIx / m.nCols,
|
|
// (colIx - colStart), rowIx, colIx, m.data[mRowDataIx + (colIx - colStart)]);
|
|
}
|
|
mRowDataIx += m.nCols;
|
|
}
|
|
}
|
|
|
|
// template <typename T>
|
|
// Matrix2Of<T> Matrix2Of<T>::Omega(const Vector3Of<T>& v) {
|
|
// Matrix2Of r = Matrix2Of::Zero(4, 4);
|
|
// r.UpdateSlice(0, 3, 0, 3, -Matrix2::SkewMatrix(v));
|
|
|
|
// // set last row to -v
|
|
// int ix = 3 * 4;
|
|
// r.data[ix++] = -v.horizontal;
|
|
// r.data[ix++] = -v.vertical;
|
|
// r.data[ix] = -v.depth;
|
|
|
|
// // Set last column to v
|
|
// ix = 3;
|
|
// r.data[ix += 4] = v.horizontal;
|
|
// r.data[ix += 4] = v.vertical;
|
|
// r.data[ix] = v.depth;
|
|
// return r;
|
|
// }
|
|
|
|
#pragma endregion Matrix2Of
|
|
|
|
} // namespace LinearAlgebra
|