Mat4.cpp 6.62 KiB
#include "Mat4.h"
namespace util {
// Constructor
Mat4::Mat4(std::array<float, 16> arr) : arr(arr){};
Mat4::Mat4() : arr(identity().arr){};
// Operator
float Mat4::operator[](std::array<int, 2> i) const {
return arr[4 * i[0] + i[1]];
}
float& Mat4::operator[](std::array<int, 2> i) {
return arr[4 * i[0] + i[1]];
}
Mat4 Mat4::operator*(const Mat4& rhs) const {
Mat4 n;
for (int c = 0; c != 4; c++) {
for (int r = 0; r != 4; r++) {
float v = 0;
for (int k = 0; k != 4; k++) v += arr[4 * k + r] * rhs[{c, k}];
n[{c, r}] = v;
}
}
return n;
}
bool Mat4::operator==(const Mat4& rhs) const {
return arr == rhs.arr;
}
bool Mat4::operator!=(const Mat4& rhs) const {
return !(*this == rhs);
}
// Methods
Mat4 Mat4::transpose() const {
Mat4 x;
for (int c = 0; c != 4; c++) {
for (int r = 0; r != 4; r++) {
x[{c, r}] = arr[4 * r + c];
}
}
return x;
}
Vec3 Mat4::transformDir(Vec3 v) const {
Mat4 mat(*this);
const float x =
mat[{0, 0}] * v[0] + mat[{0, 1}] * v[1] + mat[{0, 2}] * v[2];
const float y =
mat[{1, 0}] * v[0] + mat[{1, 1}] * v[1] + mat[{1, 2}] * v[2];
const float z =
mat[{2, 0}] * v[0] + mat[{2, 1}] * v[1] + mat[{2, 2}] * v[2];
return Vec3(x, y, z);
}
Vec3 Mat4::transformPoint(Vec3 v) const {
Mat4 mat(*this);
float x = mat[{0, 0}] * v[0] + mat[{0, 1}] * v[1] + mat[{0, 2}] * v[2] +
mat[{0, 3}];
float y = mat[{1, 0}] * v[0] + mat[{1, 1}] * v[1] + mat[{1, 2}] * v[2] +
mat[{1, 3}];
float z = mat[{2, 0}] * v[0] + mat[{2, 1}] * v[1] + mat[{2, 2}] * v[2] +
mat[{2, 3}];
return Vec3(x, y, z);
}
Vec3 Mat4::position() const {
Mat4 mat(*this);
return Vec3(mat[{0, 3}], mat[{1, 3}], mat[{2, 3}]);
}
Mat4 Mat4::invertFull() const {
Mat4 ret;
std::array<float, 16> mat(arr);
std::array<float, 16> dst(ret.arr);
std::array<float, 12> tmp;
/* temparray for pairs */
std::array<float, 16> src;
/* array of transpose source matrix */
float det;
/* determinant */
/*
* transpose matrix
*/
for (int i = 0; i < 4; i++) {
src[i] = mat[i * 4];
src[i + 4] = mat[i * 4 + 1];
src[i + 8] = mat[i * 4 + 2];
src[i + 12] = mat[i * 4 + 3];
}
/* calculate pairs for first 8 elements (cofactors) */
tmp[0] = src[10] * src[15];
tmp[1] = src[11] * src[14];
tmp[2] = src[9] * src[15];
tmp[3] = src[11] * src[13];
tmp[4] = src[9] * src[14];
tmp[5] = src[10] * src[13];
tmp[6] = src[8] * src[15];
tmp[7] = src[11] * src[12];
tmp[8] = src[8] * src[14];
tmp[9] = src[10] * src[12];
tmp[10] = src[8] * src[13];
tmp[11] = src[9] * src[12];
/* calculate first 8 elements (cofactors) */
dst[0] = tmp[0] * src[5] + tmp[3] * src[6] + tmp[4] * src[7];
dst[0] -= tmp[1] * src[5] + tmp[2] * src[6] + tmp[5] * src[7];
dst[1] = tmp[1] * src[4] + tmp[6] * src[6] + tmp[9] * src[7];
dst[1] -= tmp[0] * src[4] + tmp[7] * src[6] + tmp[8] * src[7];
dst[2] = tmp[2] * src[4] + tmp[7] * src[5] + tmp[10] * src[7];
dst[2] -= tmp[3] * src[4] + tmp[6] * src[5] + tmp[11] * src[7];
dst[3] = tmp[5] * src[4] + tmp[8] * src[5] + tmp[11] * src[6];
dst[3] -= tmp[4] * src[4] + tmp[9] * src[5] + tmp[10] * src[6];
dst[4] = tmp[1] * src[1] + tmp[2] * src[2] + tmp[5] * src[3];
dst[4] -= tmp[0] * src[1] + tmp[3] * src[2] + tmp[4] * src[3];
dst[5] = tmp[0] * src[0] + tmp[7] * src[2] + tmp[8] * src[3];
dst[5] -= tmp[1] * src[0] + tmp[6] * src[2] + tmp[9] * src[3];
dst[6] = tmp[3] * src[0] + tmp[6] * src[1] + tmp[11] * src[3];
dst[6] -= tmp[2] * src[0] + tmp[7] * src[1] + tmp[10] * src[3];
dst[7] = tmp[4] * src[0] + tmp[9] * src[1] + tmp[10] * src[2];
dst[7] -= tmp[5] * src[0] + tmp[8] * src[1] + tmp[11] * src[2];
/* calculate pairs for second 8 elements (cofactors) */
tmp[0] = src[2] * src[7];
tmp[1] = src[3] * src[6];
tmp[2] = src[1] * src[7];
tmp[3] = src[3] * src[5];
tmp[4] = src[1] * src[6];
tmp[5] = src[2] * src[5];
tmp[6] = src[0] * src[7];
tmp[7] = src[3] * src[4];
tmp[8] = src[0] * src[6];
tmp[9] = src[2] * src[4];
tmp[10] = src[0] * src[5];
tmp[11] = src[1] * src[4];
/* calculate second 8 elements (cofactors) */
dst[8] = tmp[0] * src[13] + tmp[3] * src[14] + tmp[4] * src[15];
dst[8] -= tmp[1] * src[13] + tmp[2] * src[14] + tmp[5] * src[15];
dst[9] = tmp[1] * src[12] + tmp[6] * src[14] + tmp[9] * src[15];
dst[9] -= tmp[0] * src[12] + tmp[7] * src[14] + tmp[8] * src[15];
dst[10] = tmp[2] * src[12] + tmp[7] * src[13] + tmp[10] * src[15];
dst[10] -= tmp[3] * src[12] + tmp[6] * src[13] + tmp[11] * src[15]; dst[11] = tmp[5] * src[12] + tmp[8] * src[13] + tmp[11] * src[14];
dst[11] -= tmp[4] * src[12] + tmp[9] * src[13] + tmp[10] * src[14];
dst[12] = tmp[2] * src[10] + tmp[5] * src[11] + tmp[1] * src[9];
dst[12] -= tmp[4] * src[11] + tmp[0] * src[9] + tmp[3] * src[10];
dst[13] = tmp[8] * src[11] + tmp[0] * src[8] + tmp[7] * src[10];
dst[13] -= tmp[6] * src[10] + tmp[9] * src[11] + tmp[1] * src[8];
dst[14] = tmp[6] * src[9] + tmp[11] * src[11] + tmp[3] * src[8];
dst[14] -= tmp[10] * src[11] + tmp[2] * src[8] + tmp[7] * src[9];
dst[15] = tmp[10] * src[10] + tmp[4] * src[8] + tmp[9] * src[9];
dst[15] -= tmp[8] * src[9] + tmp[11] * src[10] + tmp[5] * src[8];
/* calculate determinant */
det = src[0] * dst[0] + src[1] * dst[1] + src[2] * dst[2] + src[3] * dst[3];
/* calculate matrix inverse */
det = 1 / det;
for (int j = 0; j < 16; j++) {
dst[j] *= det;
}
Mat4 retu(dst);
return retu;
}
// Static
Mat4 translate(Vec3 xyz) {
Mat4 matrix;
matrix[{0, 3}] = xyz[0];
matrix[{1, 3}] = xyz[1];
matrix[{2, 3}] = xyz[2];
return matrix;
}
Mat4 rotate(Vec3 axis, float angle) {
Mat4 matrix;
float rad = (angle / 180) * M_PI;
float cosa = cos(rad);
float sina = sin(rad);
float l = sqrt(axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]);
float rx = axis[0] / l;
float ry = axis[1] / l;
float rz = axis[2] / l;
float icosa = 1 - cosa;
{
matrix[{0, 0}] = (icosa * rx * rx + cosa);
matrix[{1, 0}] = (icosa * rx * ry + rz * sina);
matrix[{2, 0}] = (icosa * rx * rz - ry * sina);
}
{
matrix[{0, 1}] = (icosa * rx * ry - rz * sina);
matrix[{1, 1}] = (icosa * ry * ry + cosa);
matrix[{2, 1}] = (icosa * ry * rz + rx * sina);
}
{
matrix[{0, 2}] = (icosa * rx * rz + ry * sina);
matrix[{1, 2}] = (icosa * ry * rz - rx * sina);
matrix[{2, 2}] = (icosa * rz * rz + cosa);
}
return matrix;
}
Mat4 scale(Vec3 xyz) {
Mat4 matrix;
matrix[{0, 0}] = xyz[0];
matrix[{1, 1}] = xyz[1];
matrix[{2, 2}] = xyz[2];
return matrix;
}
Mat4 identity() {
return Mat4({1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1});
}// Out
std::ostream& operator<<(std::ostream& os, const Mat4& rhs) {
for (int c = 0; c < 4; c++) {
os << "(";
for (int r = 0; r < 4; r++) {
os << rhs[{c, r}];
if (r != 3) {
os << ",";
}
}
os << ")" << std::endl;
}
return os;
}
} // namespace util