Added better support for casting vector3d to/from MAtrix
This commit is contained in:
245
Matrix.cpp
245
Matrix.cpp
@@ -8,18 +8,21 @@
|
||||
#include <type_traits>
|
||||
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
Matrix<rows, columns>::Matrix(float value) {
|
||||
Matrix<rows, columns>::Matrix(float value)
|
||||
{
|
||||
this->Fill(value);
|
||||
}
|
||||
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
Matrix<rows, columns>::Matrix(const std::array<float, rows * columns> &array) {
|
||||
Matrix<rows, columns>::Matrix(const std::array<float, rows * columns> &array)
|
||||
{
|
||||
this->setMatrixToArray(array);
|
||||
}
|
||||
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
template <typename... Args>
|
||||
Matrix<rows, columns>::Matrix(Args... args) {
|
||||
Matrix<rows, columns>::Matrix(Args... args)
|
||||
{
|
||||
constexpr uint16_t arraySize{static_cast<uint16_t>(rows) *
|
||||
static_cast<uint16_t>(columns)};
|
||||
|
||||
@@ -31,9 +34,12 @@ Matrix<rows, columns>::Matrix(Args... args) {
|
||||
}
|
||||
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
Matrix<rows, columns>::Matrix(const Matrix<rows, columns> &other) {
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
|
||||
Matrix<rows, columns>::Matrix(const Matrix<rows, columns> &other)
|
||||
{
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++)
|
||||
{
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++)
|
||||
{
|
||||
this->matrix[row_idx * columns + column_idx] =
|
||||
other.Get(row_idx, column_idx);
|
||||
}
|
||||
@@ -42,15 +48,21 @@ Matrix<rows, columns>::Matrix(const Matrix<rows, columns> &other) {
|
||||
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
void Matrix<rows, columns>::setMatrixToArray(
|
||||
const std::array<float, rows * columns> &array) {
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
|
||||
const std::array<float, rows * columns> &array)
|
||||
{
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++)
|
||||
{
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++)
|
||||
{
|
||||
uint16_t array_idx =
|
||||
static_cast<uint16_t>(row_idx) * static_cast<uint16_t>(columns) +
|
||||
static_cast<uint16_t>(column_idx);
|
||||
if (array_idx < array.size()) {
|
||||
if (array_idx < array.size())
|
||||
{
|
||||
this->matrix[row_idx * columns + column_idx] = array[array_idx];
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
this->matrix[row_idx * columns + column_idx] = 0;
|
||||
}
|
||||
}
|
||||
@@ -60,9 +72,12 @@ void Matrix<rows, columns>::setMatrixToArray(
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
Matrix<rows, columns> &
|
||||
Matrix<rows, columns>::Add(const Matrix<rows, columns> &other,
|
||||
Matrix<rows, columns> &result) const {
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
|
||||
Matrix<rows, columns> &result) const
|
||||
{
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++)
|
||||
{
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++)
|
||||
{
|
||||
result[row_idx][column_idx] =
|
||||
this->Get(row_idx, column_idx) + other.Get(row_idx, column_idx);
|
||||
}
|
||||
@@ -73,9 +88,12 @@ Matrix<rows, columns>::Add(const Matrix<rows, columns> &other,
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
Matrix<rows, columns> &
|
||||
Matrix<rows, columns>::Sub(const Matrix<rows, columns> &other,
|
||||
Matrix<rows, columns> &result) const {
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
|
||||
Matrix<rows, columns> &result) const
|
||||
{
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++)
|
||||
{
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++)
|
||||
{
|
||||
result[row_idx][column_idx] =
|
||||
this->Get(row_idx, column_idx) - other.Get(row_idx, column_idx);
|
||||
}
|
||||
@@ -88,16 +106,19 @@ template <uint8_t rows, uint8_t columns>
|
||||
template <uint8_t other_columns>
|
||||
Matrix<rows, columns> &
|
||||
Matrix<rows, columns>::Mult(const Matrix<columns, other_columns> &other,
|
||||
Matrix<rows, other_columns> &result) const {
|
||||
Matrix<rows, other_columns> &result) const
|
||||
{
|
||||
// allocate some buffers for all of our dot products
|
||||
Matrix<1, columns> this_row;
|
||||
Matrix<rows, 1> other_column;
|
||||
Matrix<1, rows> other_column_t;
|
||||
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++)
|
||||
{
|
||||
// get our row
|
||||
this->GetRow(row_idx, this_row);
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++)
|
||||
{
|
||||
// get the other matrix'ss column
|
||||
other.GetColumn(column_idx, other_column);
|
||||
// transpose the other matrix's column
|
||||
@@ -114,9 +135,12 @@ Matrix<rows, columns>::Mult(const Matrix<columns, other_columns> &other,
|
||||
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
Matrix<rows, columns> &
|
||||
Matrix<rows, columns>::Mult(float scalar, Matrix<rows, columns> &result) const {
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
|
||||
Matrix<rows, columns>::Mult(float scalar, Matrix<rows, columns> &result) const
|
||||
{
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++)
|
||||
{
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++)
|
||||
{
|
||||
result[row_idx][column_idx] = this->Get(row_idx, column_idx) * scalar;
|
||||
}
|
||||
}
|
||||
@@ -126,7 +150,8 @@ Matrix<rows, columns>::Mult(float scalar, Matrix<rows, columns> &result) const {
|
||||
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
Matrix<rows, columns> &
|
||||
Matrix<rows, columns>::Invert(Matrix<rows, columns> &result) const {
|
||||
Matrix<rows, columns>::Invert(Matrix<rows, columns> &result) const
|
||||
{
|
||||
// since all matrix sizes have to be statically specified at compile time we
|
||||
// can do this
|
||||
static_assert(rows == columns,
|
||||
@@ -135,7 +160,8 @@ Matrix<rows, columns>::Invert(Matrix<rows, columns> &result) const {
|
||||
// unfortunately we can't calculate this at compile time so we'll just reurn
|
||||
// zeros
|
||||
float determinant{this->Det()};
|
||||
if (determinant == 0) {
|
||||
if (determinant == 0)
|
||||
{
|
||||
// you can't invert a matrix with a negative determinant
|
||||
result.Fill(0);
|
||||
return result;
|
||||
@@ -161,9 +187,12 @@ Matrix<rows, columns>::Invert(Matrix<rows, columns> &result) const {
|
||||
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
Matrix<columns, rows> &
|
||||
Matrix<rows, columns>::Transpose(Matrix<columns, rows> &result) const {
|
||||
for (uint8_t column_idx{0}; column_idx < rows; column_idx++) {
|
||||
for (uint8_t row_idx{0}; row_idx < columns; row_idx++) {
|
||||
Matrix<rows, columns>::Transpose(Matrix<columns, rows> &result) const
|
||||
{
|
||||
for (uint8_t column_idx{0}; column_idx < rows; column_idx++)
|
||||
{
|
||||
for (uint8_t row_idx{0}; row_idx < columns; row_idx++)
|
||||
{
|
||||
result[row_idx][column_idx] = this->Get(column_idx, row_idx);
|
||||
}
|
||||
}
|
||||
@@ -173,20 +202,26 @@ Matrix<rows, columns>::Transpose(Matrix<columns, rows> &result) const {
|
||||
|
||||
// explicitly define the determinant for a 2x2 matrix because it is definitely
|
||||
// the fastest way to calculate a 2x2 matrix determinant
|
||||
template <> float Matrix<0, 0>::Det() const { return 1e+6; }
|
||||
template <> float Matrix<1, 1>::Det() const { return this->matrix[0]; }
|
||||
template <> float Matrix<2, 2>::Det() const {
|
||||
template <>
|
||||
float Matrix<0, 0>::Det() const { return 1e+6; }
|
||||
template <>
|
||||
float Matrix<1, 1>::Det() const { return this->matrix[0]; }
|
||||
template <>
|
||||
float Matrix<2, 2>::Det() const
|
||||
{
|
||||
return this->matrix[0] * this->matrix[3] - this->matrix[1] * this->matrix[2];
|
||||
}
|
||||
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
float Matrix<rows, columns>::Det() const {
|
||||
float Matrix<rows, columns>::Det() const
|
||||
{
|
||||
static_assert(rows == columns,
|
||||
"You can't take the determinant of a non-square matrix.");
|
||||
|
||||
Matrix<rows - 1, columns - 1> MinorMatrix{};
|
||||
float determinant{0};
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++)
|
||||
{
|
||||
// for odd indices the sign is negative
|
||||
float sign = (column_idx % 2 == 0) ? 1 : -1;
|
||||
determinant += sign * this->matrix[column_idx] *
|
||||
@@ -199,9 +234,12 @@ float Matrix<rows, columns>::Det() const {
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
Matrix<rows, columns> &
|
||||
Matrix<rows, columns>::ElementMultiply(const Matrix<rows, columns> &other,
|
||||
Matrix<rows, columns> &result) const {
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
|
||||
Matrix<rows, columns> &result) const
|
||||
{
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++)
|
||||
{
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++)
|
||||
{
|
||||
result[row_idx][column_idx] =
|
||||
this->Get(row_idx, column_idx) * other.Get(row_idx, column_idx);
|
||||
}
|
||||
@@ -213,9 +251,12 @@ Matrix<rows, columns>::ElementMultiply(const Matrix<rows, columns> &other,
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
Matrix<rows, columns> &
|
||||
Matrix<rows, columns>::ElementDivide(const Matrix<rows, columns> &other,
|
||||
Matrix<rows, columns> &result) const {
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
|
||||
Matrix<rows, columns> &result) const
|
||||
{
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++)
|
||||
{
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++)
|
||||
{
|
||||
result[row_idx][column_idx] =
|
||||
this->Get(row_idx, column_idx) / other.Get(row_idx, column_idx);
|
||||
}
|
||||
@@ -226,8 +267,10 @@ Matrix<rows, columns>::ElementDivide(const Matrix<rows, columns> &other,
|
||||
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
float Matrix<rows, columns>::Get(uint8_t row_index,
|
||||
uint8_t column_index) const {
|
||||
if (row_index > rows - 1 || column_index > columns - 1) {
|
||||
uint8_t column_index) const
|
||||
{
|
||||
if (row_index > rows - 1 || column_index > columns - 1)
|
||||
{
|
||||
return 1e+10; // TODO: We should throw something here instead of failing
|
||||
// quietly
|
||||
}
|
||||
@@ -237,7 +280,8 @@ float Matrix<rows, columns>::Get(uint8_t row_index,
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
Matrix<1, columns> &
|
||||
Matrix<rows, columns>::GetRow(uint8_t row_index,
|
||||
Matrix<1, columns> &row) const {
|
||||
Matrix<1, columns> &row) const
|
||||
{
|
||||
memcpy(&(row[0]), this->matrix.begin() + row_index * columns,
|
||||
columns * sizeof(float));
|
||||
|
||||
@@ -247,8 +291,10 @@ Matrix<rows, columns>::GetRow(uint8_t row_index,
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
Matrix<rows, 1> &
|
||||
Matrix<rows, columns>::GetColumn(uint8_t column_index,
|
||||
Matrix<rows, 1> &column) const {
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
|
||||
Matrix<rows, 1> &column) const
|
||||
{
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++)
|
||||
{
|
||||
column[row_idx][0] = this->Get(row_idx, column_index);
|
||||
}
|
||||
|
||||
@@ -256,13 +302,17 @@ Matrix<rows, columns>::GetColumn(uint8_t column_index,
|
||||
}
|
||||
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
void Matrix<rows, columns>::ToString(std::string &stringBuffer) const {
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
|
||||
void Matrix<rows, columns>::ToString(std::string &stringBuffer) const
|
||||
{
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++)
|
||||
{
|
||||
stringBuffer += "|";
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++)
|
||||
{
|
||||
stringBuffer +=
|
||||
std::to_string(this->matrix[row_idx * columns + column_idx]);
|
||||
if (column_idx != columns - 1) {
|
||||
if (column_idx != columns - 1)
|
||||
{
|
||||
stringBuffer += "\t";
|
||||
}
|
||||
}
|
||||
@@ -272,8 +322,10 @@ void Matrix<rows, columns>::ToString(std::string &stringBuffer) const {
|
||||
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
std::array<float, columns> &Matrix<rows, columns>::
|
||||
operator[](uint8_t row_index) {
|
||||
if (row_index > rows - 1) {
|
||||
operator[](uint8_t row_index)
|
||||
{
|
||||
if (row_index > rows - 1)
|
||||
{
|
||||
// TODO: We should throw something here instead of failing quietly.
|
||||
row_index = 0;
|
||||
}
|
||||
@@ -285,9 +337,12 @@ operator[](uint8_t row_index) {
|
||||
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
Matrix<rows, columns> &Matrix<rows, columns>::
|
||||
operator=(const Matrix<rows, columns> &other) {
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
|
||||
operator=(const Matrix<rows, columns> &other)
|
||||
{
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++)
|
||||
{
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++)
|
||||
{
|
||||
this->matrix[row_idx * columns + column_idx] =
|
||||
other.Get(row_idx, column_idx);
|
||||
}
|
||||
@@ -298,7 +353,8 @@ operator=(const Matrix<rows, columns> &other) {
|
||||
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
Matrix<rows, columns> Matrix<rows, columns>::
|
||||
operator+(const Matrix<rows, columns> &other) const {
|
||||
operator+(const Matrix<rows, columns> &other) const
|
||||
{
|
||||
Matrix<rows, columns> buffer{};
|
||||
this->Add(other, buffer);
|
||||
return buffer;
|
||||
@@ -306,7 +362,8 @@ operator+(const Matrix<rows, columns> &other) const {
|
||||
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
Matrix<rows, columns> Matrix<rows, columns>::
|
||||
operator-(const Matrix<rows, columns> &other) const {
|
||||
operator-(const Matrix<rows, columns> &other) const
|
||||
{
|
||||
Matrix<rows, columns> buffer{};
|
||||
this->Sub(other, buffer);
|
||||
return buffer;
|
||||
@@ -314,14 +371,16 @@ operator-(const Matrix<rows, columns> &other) const {
|
||||
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
Matrix<rows, columns> Matrix<rows, columns>::
|
||||
operator*(const Matrix<rows, columns> &other) const {
|
||||
operator*(const Matrix<rows, columns> &other) const
|
||||
{
|
||||
Matrix<rows, columns> buffer{};
|
||||
this->Mult(other, buffer);
|
||||
return buffer;
|
||||
}
|
||||
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
Matrix<rows, columns> Matrix<rows, columns>::operator*(float scalar) const {
|
||||
Matrix<rows, columns> Matrix<rows, columns>::operator*(float scalar) const
|
||||
{
|
||||
Matrix<rows, columns> buffer{};
|
||||
this->Mult(scalar, buffer);
|
||||
return buffer;
|
||||
@@ -330,9 +389,11 @@ Matrix<rows, columns> Matrix<rows, columns>::operator*(float scalar) const {
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
template <uint8_t vector_size>
|
||||
float Matrix<rows, columns>::dotProduct(const Matrix<1, vector_size> &vec1,
|
||||
const Matrix<1, vector_size> &vec2) {
|
||||
const Matrix<1, vector_size> &vec2)
|
||||
{
|
||||
float sum{0};
|
||||
for (uint8_t i{0}; i < vector_size; i++) {
|
||||
for (uint8_t i{0}; i < vector_size; i++)
|
||||
{
|
||||
sum += vec1.Get(0, i) * vec2.Get(0, i);
|
||||
}
|
||||
|
||||
@@ -342,9 +403,11 @@ float Matrix<rows, columns>::dotProduct(const Matrix<1, vector_size> &vec1,
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
template <uint8_t vector_size>
|
||||
float Matrix<rows, columns>::dotProduct(const Matrix<vector_size, 1> &vec1,
|
||||
const Matrix<vector_size, 1> &vec2) {
|
||||
const Matrix<vector_size, 1> &vec2)
|
||||
{
|
||||
float sum{0};
|
||||
for (uint8_t i{0}; i < vector_size; i++) {
|
||||
for (uint8_t i{0}; i < vector_size; i++)
|
||||
{
|
||||
sum += vec1.Get(i, 0) * vec2.Get(i, 0);
|
||||
}
|
||||
|
||||
@@ -352,9 +415,12 @@ float Matrix<rows, columns>::dotProduct(const Matrix<vector_size, 1> &vec1,
|
||||
}
|
||||
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
void Matrix<rows, columns>::Fill(float value) {
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
|
||||
void Matrix<rows, columns>::Fill(float value)
|
||||
{
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++)
|
||||
{
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++)
|
||||
{
|
||||
this->matrix[row_idx * columns + column_idx] = value;
|
||||
}
|
||||
}
|
||||
@@ -362,11 +428,14 @@ void Matrix<rows, columns>::Fill(float value) {
|
||||
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
Matrix<rows, columns> &
|
||||
Matrix<rows, columns>::MatrixOfMinors(Matrix<rows, columns> &result) const {
|
||||
Matrix<rows, columns>::MatrixOfMinors(Matrix<rows, columns> &result) const
|
||||
{
|
||||
Matrix<rows - 1, columns - 1> MinorMatrix{};
|
||||
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++)
|
||||
{
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++)
|
||||
{
|
||||
this->MinorMatrix(MinorMatrix, row_idx, column_idx);
|
||||
result[row_idx][column_idx] = MinorMatrix.Det();
|
||||
}
|
||||
@@ -378,15 +447,20 @@ Matrix<rows, columns>::MatrixOfMinors(Matrix<rows, columns> &result) const {
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
Matrix<rows - 1, columns - 1> &
|
||||
Matrix<rows, columns>::MinorMatrix(Matrix<rows - 1, columns - 1> &result,
|
||||
uint8_t row_idx, uint8_t column_idx) const {
|
||||
uint8_t row_idx, uint8_t column_idx) const
|
||||
{
|
||||
std::array<float, (rows - 1) * (columns - 1)> subArray{};
|
||||
uint16_t array_idx{0};
|
||||
for (uint8_t row_iter{0}; row_iter < rows; row_iter++) {
|
||||
if (row_iter == row_idx) {
|
||||
for (uint8_t row_iter{0}; row_iter < rows; row_iter++)
|
||||
{
|
||||
if (row_iter == row_idx)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (uint8_t column_iter{0}; column_iter < columns; column_iter++) {
|
||||
if (column_iter == column_idx) {
|
||||
for (uint8_t column_iter{0}; column_iter < columns; column_iter++)
|
||||
{
|
||||
if (column_iter == column_idx)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
subArray[array_idx] = this->Get(row_iter, column_iter);
|
||||
@@ -400,9 +474,12 @@ Matrix<rows, columns>::MinorMatrix(Matrix<rows - 1, columns - 1> &result,
|
||||
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
Matrix<rows, columns> &
|
||||
Matrix<rows, columns>::adjugate(Matrix<rows, columns> &result) const {
|
||||
for (uint8_t row_iter{0}; row_iter < rows; row_iter++) {
|
||||
for (uint8_t column_iter{0}; column_iter < columns; column_iter++) {
|
||||
Matrix<rows, columns>::adjugate(Matrix<rows, columns> &result) const
|
||||
{
|
||||
for (uint8_t row_iter{0}; row_iter < rows; row_iter++)
|
||||
{
|
||||
for (uint8_t column_iter{0}; column_iter < columns; column_iter++)
|
||||
{
|
||||
float sign = ((row_iter + 1) % 2) == 0 ? -1 : 1;
|
||||
sign *= ((column_iter + 1) % 2) == 0 ? -1 : 1;
|
||||
result[column_iter][row_iter] = this->Get(row_iter, column_iter) * sign;
|
||||
@@ -414,16 +491,20 @@ Matrix<rows, columns>::adjugate(Matrix<rows, columns> &result) const {
|
||||
|
||||
template <uint8_t rows, uint8_t columns>
|
||||
Matrix<rows, columns> &
|
||||
Matrix<rows, columns>::Normalize(Matrix<rows, columns> &result) const {
|
||||
Matrix<rows, columns>::Normalize(Matrix<rows, columns> &result) const
|
||||
{
|
||||
float sum{0};
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++)
|
||||
{
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++)
|
||||
{
|
||||
float val{this->Get(row_idx, column_idx)};
|
||||
sum += val * val;
|
||||
}
|
||||
}
|
||||
|
||||
if (sum == 0) {
|
||||
if (sum == 0)
|
||||
{
|
||||
// this wouldn't do anything anyways
|
||||
result.Fill(1e+6);
|
||||
return result;
|
||||
@@ -431,8 +512,10 @@ Matrix<rows, columns>::Normalize(Matrix<rows, columns> &result) const {
|
||||
|
||||
sum = sqrt(sum);
|
||||
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
|
||||
for (uint8_t row_idx{0}; row_idx < rows; row_idx++)
|
||||
{
|
||||
for (uint8_t column_idx{0}; column_idx < columns; column_idx++)
|
||||
{
|
||||
result[row_idx][column_idx] = this->Get(row_idx, column_idx) / sum;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user