338 lines
11 KiB
C++
338 lines
11 KiB
C++
#include "matrix3d.hpp"
|
|
#include <cmath>
|
|
|
|
namespace engine::math {
|
|
|
|
void matrix3d::setFloatArray(const float32 m[3][3]) {
|
|
this->_m[0][0] = m[0][0];
|
|
this->_m[0][1] = m[0][1];
|
|
this->_m[0][2] = m[0][2];
|
|
|
|
this->_m[1][0] = m[1][0];
|
|
this->_m[1][1] = m[1][1];
|
|
this->_m[1][2] = m[1][2];
|
|
this->_m[2][0] = m[2][0];
|
|
this->_m[2][1] = m[2][1];
|
|
this->_m[2][2] = m[2][2];
|
|
};
|
|
|
|
void matrix3d::setAll(float32 v) {
|
|
this->_m[0][0] = v;
|
|
this->_m[0][1] = v;
|
|
this->_m[0][2] = v;
|
|
|
|
this->_m[1][0] = v;
|
|
this->_m[1][1] = v;
|
|
this->_m[1][2] = v;
|
|
|
|
this->_m[2][0] = v;
|
|
this->_m[2][1] = v;
|
|
this->_m[2][2] = v;
|
|
};
|
|
|
|
vector3d matrix3d::operator[](int i) {
|
|
float32 *row = this->_m[i];
|
|
return vector3d(*row, *(row + 1), *(row + 2));
|
|
};
|
|
|
|
const vector3d matrix3d::operator[](int i) const {
|
|
return vector3d(this->_m[i][0], this->_m[i][1], this->_m[i][2]);
|
|
};
|
|
|
|
float32 matrix3d::operator()(int row, int column) {
|
|
return this->_m[row][column];
|
|
};
|
|
|
|
float32 matrix3d::operator()(int row, int column) const {
|
|
return this->_m[row][column];
|
|
};
|
|
|
|
// Default constructor all entries are 0
|
|
matrix3d::matrix3d() { this->setAll(0); };
|
|
|
|
// Default destructor just does nothing
|
|
matrix3d::~matrix3d(){};
|
|
|
|
// Constructor based on other matrix3d instance
|
|
matrix3d::matrix3d(const matrix3d &m){};
|
|
|
|
matrix3d::matrix3d(float32 a00, float32 a01, float32 a02, float32 a10,
|
|
float32 a11, float32 a12, float32 a20, float32 a21,
|
|
float32 a22) {
|
|
this->_m[0][0] = a00;
|
|
this->_m[0][1] = a01;
|
|
this->_m[0][2] = a02;
|
|
|
|
this->_m[1][0] = a10;
|
|
this->_m[1][1] = a11;
|
|
this->_m[1][2] = a12;
|
|
|
|
this->_m[2][0] = a20;
|
|
this->_m[2][1] = a21;
|
|
this->_m[2][2] = a22;
|
|
};
|
|
// Constructor that sets matrix entries based in a two dimentional float32 3x3
|
|
// array
|
|
matrix3d::matrix3d(float32 m[3][3]) { this->setFloatArray(m); };
|
|
|
|
// matrix3 Constructor with 3 vector3d each representing a row
|
|
matrix3d::matrix3d(const vector3d r1, const vector3d r2, const vector3d r3) {
|
|
this->_m[0][0] = r1[0];
|
|
this->_m[0][1] = r1[1];
|
|
this->_m[0][2] = r1[2];
|
|
|
|
this->_m[1][0] = r2[0];
|
|
this->_m[1][1] = r2[1];
|
|
this->_m[1][2] = r2[2];
|
|
|
|
this->_m[2][0] = r3[0];
|
|
this->_m[2][1] = r3[1];
|
|
this->_m[2][2] = r3[2];
|
|
};
|
|
|
|
std::string matrix3d::to_string() const {
|
|
std::string str = "\n";
|
|
str += "[" + std::to_string(this->_m[0][0]) + ", " +
|
|
std::to_string(this->_m[0][1]) + ", " +
|
|
std::to_string(this->_m[0][2]) + "]\n";
|
|
str += "[" + std::to_string(this->_m[1][0]) + ", " +
|
|
std::to_string(this->_m[1][1]) + ", " +
|
|
std::to_string(this->_m[1][2]) + "]\n";
|
|
str += "[" + std::to_string(this->_m[2][0]) + ", " +
|
|
std::to_string(this->_m[2][1]) + ", " +
|
|
std::to_string(this->_m[2][2]) + "]\n";
|
|
return str;
|
|
}
|
|
|
|
// Overload of << operator. Usefull to combine this with ostream stream
|
|
std::ostream &operator<<(std::ostream &stream, matrix3d const &m) {
|
|
std::string m_str = m.to_string();
|
|
stream << m.to_string();
|
|
return stream;
|
|
};
|
|
|
|
// Just builds an id matrix
|
|
matrix3d matrix3d::id() {
|
|
float32 id[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
|
|
return matrix3d(id);
|
|
};
|
|
|
|
matrix3d matrix3d::trans(const matrix3d &matrix) {
|
|
return matrix3d(
|
|
matrix(0, 0), matrix(1, 0), matrix(2, 0), // First column into first row
|
|
matrix(0, 1), matrix(1, 1), matrix(2, 1), // Second column into second row
|
|
matrix(0, 2), matrix(1, 2), matrix(2, 2) // Third column into third row
|
|
);
|
|
};
|
|
|
|
bool matrix3d::equals(const matrix3d &first, const matrix3d &second) {
|
|
return first(0, 0) == second(0, 0) && first(0, 1) == second(0, 1) &&
|
|
first(0, 2) == second(0, 2) && first(1, 0) == second(1, 0) &&
|
|
first(1, 1) == second(1, 1) && first(1, 2) == second(1, 2) &&
|
|
first(2, 0) == second(2, 0) && first(2, 1) == second(2, 1) &&
|
|
first(2, 2) == second(2, 2);
|
|
}
|
|
|
|
float32 matrix3d::det(const matrix3d &m) {
|
|
return m(0, 0) * DET(m(1, 1), m(1, 2), m(2, 1), m(2, 2)) -
|
|
m(0, 1) * DET(m(1, 0), m(1, 2), m(2, 0), m(2, 2)) +
|
|
m(0, 2) * DET(m(1, 0), m(1, 1), m(2, 0), m(2, 1));
|
|
};
|
|
|
|
matrix3d matrix3d::inv(const matrix3d &m) {
|
|
// Rows as vectors
|
|
const vector3d &a = m[0];
|
|
const vector3d &b = m[1];
|
|
const vector3d &c = m[2];
|
|
|
|
// Cross product of rows
|
|
vector3d r0 = vector3d::cross(b, c);
|
|
vector3d r1 = vector3d::cross(c, a);
|
|
vector3d r2 = vector3d::cross(a, b);
|
|
|
|
// determinant inverse
|
|
float32 idet = 1.0f / vector3d::dot(r2, c);
|
|
|
|
return matrix3d(INVDET(r0, idet), INVDET(r1, idet), INVDET(r2, idet));
|
|
};
|
|
|
|
matrix3d matrix3d::rotX(float32 angle) {
|
|
float32 _cos = cos(angle);
|
|
float32 _sin = sin(angle);
|
|
return matrix3d(1, 0, 0, 0, _cos, -_sin, 0, _sin, _cos);
|
|
};
|
|
|
|
matrix3d matrix3d::rotY(float32 angle) {
|
|
float32 _cos = cos(angle);
|
|
float32 _sin = sin(angle);
|
|
|
|
return matrix3d(_cos, 0, _sin, 0, 1, 0, -_sin, 0, _cos);
|
|
};
|
|
|
|
matrix3d matrix3d::rotZ(float32 angle) {
|
|
float32 _cos = cos(angle);
|
|
float32 _sin = sin(angle);
|
|
return matrix3d(_cos, -_sin, 0, _sin, _cos, 0, 0, 0, 1);
|
|
};
|
|
|
|
matrix3d matrix3d::rot(float32 angle, const vector3d &axis) {
|
|
float32 _cos = cos(angle);
|
|
float32 _sin = sin(angle);
|
|
float32 d = 1 - _cos;
|
|
|
|
float32 x = axis[0] * d;
|
|
float32 y = axis[1] * d;
|
|
float32 z = axis[2] * d;
|
|
float32 ax_y = x * axis[1];
|
|
float32 ax_z = x * axis[2];
|
|
float32 ay_z = y * axis[2];
|
|
|
|
return matrix3d(
|
|
_cos + x * axis[0], ax_y - _sin * axis[2], ax_z + _sin * axis[1],
|
|
ax_y + _sin * axis[2], _cos + y * axis[1], ay_z - _sin * axis[0],
|
|
ax_z - _sin * axis[1], ay_z + _sin * axis[0], _cos + z * axis[2]);
|
|
};
|
|
|
|
matrix3d matrix3d::scale(float32 scalex, float32 scaley, float32 scalez) {
|
|
return matrix3d(scalex, 0, 0, 0, scaley, 0, 0, 0, scalez);
|
|
};
|
|
|
|
matrix3d matrix3d::scale(const vector3d &v) {
|
|
return matrix3d(v.x, 0, 0, 0, v.y, 0, 0, 0, v.z);
|
|
}
|
|
|
|
matrix3d matrix3d::scale(float32 scale, const vector3d &vector) {
|
|
scale -= 1;
|
|
float32 x = vector[0] * scale;
|
|
float32 y = vector[1] * scale;
|
|
float32 z = vector[2] * scale;
|
|
float vx_vy = x * vector[1];
|
|
float vx_vz = x * vector[2];
|
|
float vy_vz = y * vector[2];
|
|
return matrix3d(x * vector[0] + 1, vx_vy, vx_vz, vx_vy, y * vector[1] + 1,
|
|
vx_vz, vx_vz, vy_vz, z * vector[2] + 1);
|
|
};
|
|
|
|
matrix3d matrix3d::skew(float32 angle, const vector3d v1, const vector3d &v2) {
|
|
float32 t = tan(angle);
|
|
float32 x = v1[0];
|
|
float32 y = v1[1];
|
|
float32 z = v1[2];
|
|
|
|
return matrix3d();
|
|
};
|
|
|
|
matrix3d matrix3d::reflect(const vector3d &v) {
|
|
float32 x = v.x * -2.0;
|
|
float32 y = v.y * -2.0;
|
|
float32 z = v.z * -2.0;
|
|
|
|
float32 vx_vy = x * v.y;
|
|
float32 vx_vz = x * v.z;
|
|
float32 vy_vz = y * v.z;
|
|
|
|
return matrix3d(x * v.x + 1, vx_vy, vx_vz, vx_vy, y * v.y + 1, vy_vz, vx_vz,
|
|
vy_vz, z * v.z + 1);
|
|
};
|
|
|
|
matrix3d matrix3d::involution(const vector3d &v) {
|
|
float32 x = v.x * 2.0;
|
|
float32 y = v.y * 2.0;
|
|
float32 z = v.z * 2.0;
|
|
|
|
float32 vx_vy = x * v.y;
|
|
float32 vx_vz = x * v.z;
|
|
float32 vy_vz = y * v.z;
|
|
|
|
return matrix3d(x * v.x - 1, vx_vy, vx_vz, vx_vy, y * v.y - 1, vy_vz, vx_vz,
|
|
vy_vz, z * v.z - 1);
|
|
};
|
|
|
|
// Overload of + operator. This will add 2 matrix3d instances
|
|
matrix3d operator+(const matrix3d &left, const matrix3d &right) {
|
|
MATRIX(add, 3);
|
|
add[0][0] = left[0][0] + right[0][0];
|
|
add[0][1] = left[0][1] + right[0][1];
|
|
add[0][2] = left[0][2] + right[0][2];
|
|
|
|
add[1][0] = left[1][0] + right[1][0];
|
|
add[1][1] = left[1][1] + right[1][1];
|
|
add[1][2] = left[1][2] + right[1][2];
|
|
|
|
add[2][0] = left[2][0] + right[2][0];
|
|
add[2][1] = left[2][1] + right[2][1];
|
|
add[2][2] = left[2][2] + right[2][2];
|
|
|
|
return matrix3d(add);
|
|
};
|
|
|
|
matrix3d operator-(const matrix3d &left, const matrix3d &right) {
|
|
MATRIX(m_temp, 3);
|
|
m_temp[0][0] = left[0][0] - right[0][0];
|
|
m_temp[0][1] = left[0][1] - right[0][1];
|
|
m_temp[0][2] = left[0][2] - right[0][2];
|
|
|
|
m_temp[1][0] = left[1][0] - right[1][0];
|
|
m_temp[1][1] = left[1][1] - right[1][1];
|
|
m_temp[1][2] = left[1][2] - right[1][2];
|
|
|
|
m_temp[2][0] = left[2][0] - right[2][0];
|
|
m_temp[2][1] = left[2][1] - right[2][1];
|
|
m_temp[2][2] = left[2][2] - right[2][2];
|
|
|
|
return matrix3d(m_temp);
|
|
};
|
|
|
|
// Multiplication of two 3d matrices
|
|
matrix3d operator*(const matrix3d &a, const matrix3d &b) {
|
|
float32 m00 = a(0, 0) * b(0, 0) + a(0, 1) * b(1, 0) +
|
|
a(0, 2) * b(2, 0); // a first row times b first column
|
|
float32 m01 = a(0, 0) * b(0, 1) + a(0, 1) * b(1, 1) +
|
|
a(0, 2) * b(2, 1); // a first row times b second column
|
|
float32 m02 = a(0, 0) * b(0, 2) + a(0, 1) * b(1, 2) +
|
|
a(0, 2) * b(2, 2); // a first row times b third column
|
|
float32 m10 = a(1, 0) * b(0, 0) + a(1, 1) * b(1, 0) +
|
|
a(1, 2) * b(2, 0); // a second row times b first column
|
|
float32 m11 = a(1, 0) * b(0, 1) + a(1, 1) * b(1, 1) +
|
|
a(1, 2) * b(2, 1); // a second row times b second column
|
|
float32 m12 = a(1, 0) * b(0, 2) + a(1, 1) * b(1, 2) +
|
|
a(1, 2) * b(2, 2); // a second row times b third column
|
|
float32 m20 = a(2, 0) * b(0, 0) + a(2, 1) * b(1, 0) +
|
|
a(2, 2) * b(2, 0); // a third row times b first column
|
|
float32 m21 = a(2, 0) * b(0, 1) + a(2, 1) * b(1, 1) +
|
|
a(2, 2) * b(2, 1); // a third column times b second column
|
|
float32 m22 = a(2, 0) * b(0, 2) + a(2, 1) * b(1, 2) +
|
|
a(2, 2) * b(2, 2); // a third column times b third column
|
|
return matrix3d(m00, m01, m02, m10, m11, m12, m20, m21, m22);
|
|
};
|
|
|
|
matrix3d operator*(const matrix3d &matrix, const float32 scalar) {
|
|
return matrix3d(
|
|
matrix(0, 0) * scalar, matrix(0, 1) * scalar, matrix(0, 2) * scalar,
|
|
matrix(1, 0) * scalar, matrix(1, 1) * scalar, matrix(1, 2) * scalar,
|
|
matrix(2, 0) * scalar, matrix(2, 1) * scalar, matrix(2, 2) * scalar);
|
|
};
|
|
|
|
matrix3d operator*(const float scalar, const matrix3d &matrix) {
|
|
return matrix * scalar;
|
|
}
|
|
|
|
vector3d operator*(const matrix3d &matrix, const vector3d &vector) {
|
|
|
|
return vector3d(matrix(0, 0) * vector[0] + matrix(0, 1) * vector[1] +
|
|
matrix(0, 2) * vector[2], // First row times vector
|
|
matrix(1, 0) * vector[0] + matrix(1, 1) * vector[1] +
|
|
matrix(1, 2) * vector[2], // Second row times vector
|
|
matrix(2, 0) * vector[0] + matrix(2, 1) * vector[1] +
|
|
matrix(2, 2) * vector[2]); // Third row times vector
|
|
};
|
|
|
|
vector3d operator*(const vector3d &vector, const matrix3d &matrix) {
|
|
return vector3d(vector[0] * matrix(0, 0) + vector[1] * matrix(1, 0) +
|
|
vector[2] * matrix(2, 0), // vector times first column
|
|
vector[0] * matrix(0, 1) + vector[1] * matrix(1, 1) +
|
|
vector[2] * matrix(2, 1), // vector times second column
|
|
vector[0] * matrix(0, 2) + vector[1] * matrix(1, 2) +
|
|
vector[2] * matrix(2, 2)); // vector times third column
|
|
}
|
|
}; // namespace engine::math
|