227 lines
6.9 KiB
C++
227 lines
6.9 KiB
C++
#pragma once
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
#include "vector3.h"
|
|
|
|
template <typename T>
|
|
|
|
class matrix4x4 {
|
|
private:
|
|
T cell[16];
|
|
public:
|
|
matrix4x4(void) { }
|
|
matrix4x4(T val) {
|
|
cell[0] = cell[1] = cell[2] = cell[3] = cell[4] = cell[5] = cell[6] =
|
|
cell[7] = cell[8] = cell[9] = cell[10] = cell[11] = cell[12] = cell[13] =
|
|
cell[14] = cell[15] = val;
|
|
}
|
|
|
|
/* Row-major 4x3 matrix. */
|
|
void LoadFromOdeMatrix(const T* r) {
|
|
cell[ 0] = r[ 0];cell[ 1] = r[ 4]; cell[ 2] = r[ 8];cell[ 3] = 0;
|
|
cell[ 4] = r[ 1];cell[ 5] = r[ 5]; cell[ 6] = r[ 9];cell[ 7] = 0;
|
|
cell[ 8] = r[ 2];cell[ 9] = r[ 6]; cell[10] = r[10];cell[11] = 0;
|
|
cell[12] = 0; cell[13] = 0; cell[14] = 0; cell[15] = 1;
|
|
}
|
|
|
|
/* Row-major 3x3 matrix. */
|
|
void LoadFrom3x3Matrix(const T* r) {
|
|
cell[ 0] = r[ 0]; cell[ 4] = r[ 1]; cell[ 8] = r[ 2]; cell[12] = 0;
|
|
cell[ 1] = r[ 3]; cell[ 5] = r[ 4]; cell[ 9] = r[ 5]; cell[13] = 0;
|
|
cell[ 2] = r[ 6]; cell[ 6] = r[ 7]; cell[10] = r[ 8]; cell[14] = 0;
|
|
cell[ 3] = 0; cell[ 7] = 0; cell[11] = 0; cell[15] = 1;
|
|
}
|
|
|
|
/* Row-major. */
|
|
void SaveTo3x3Matrix(T* r) {
|
|
r[0] = cell[0]; r[1] = cell[4]; r[2] = cell[8];
|
|
r[3] = cell[1]; r[4] = cell[5]; r[5] = cell[9];
|
|
r[6] = cell[2]; r[7] = cell[6]; r[8] = cell[10];
|
|
}
|
|
|
|
void SaveToOdeMatrix(T r[12]) {
|
|
r[0] = cell[0]; r[1] = cell[4]; r[ 2] = cell[ 8]; r[ 3] = 0;
|
|
r[4] = cell[1]; r[5] = cell[5]; r[ 6] = cell[ 9]; r[ 7] = 0;
|
|
r[8] = cell[2]; r[9] = cell[6]; r[10] = cell[10]; r[11] = 0;
|
|
}
|
|
|
|
static matrix4x4 Identity(void) {
|
|
matrix4x4 m = matrix4x4(0);
|
|
m.cell[0] = m.cell[5] = m.cell[10] = m.cell[15] = 1.0f;
|
|
return m;
|
|
}
|
|
|
|
static matrix4x4 ScaleMatrix(T x, T y, T z) {
|
|
matrix4x4 m;
|
|
m[ 0] = x; m[ 1] = m[ 2] = m[ 3] = 0;
|
|
m[ 5] = y; m[ 4] = m[ 6] = m[ 7] = 0;
|
|
m[10] = z; m[ 8] = m[ 9] = m[11] = 0;
|
|
m[12] = m[13] = m[14] = 0; m[15] = 1;
|
|
return m;
|
|
}
|
|
|
|
void RotateZ(T radians) { *this = (*this) * RotateZMatrix(radians); }
|
|
void RotateY(T radians) { *this = (*this) * RotateYMatrix(radians); }
|
|
void rotateX(T radians) { *this = (*this) * RotateXMatrix(radians); }
|
|
|
|
static matrix4x4 RotateXMatrix(T radians) {
|
|
matrix4x4 m;
|
|
T cos_r = cosf(radians);
|
|
T sin_r = sinf(radians);
|
|
m[ 0] = 1.0f;
|
|
m[ 1] = 0;
|
|
m[ 2] = 0;
|
|
m[ 3] = 0;
|
|
|
|
m[ 4] = 0;
|
|
m[ 5] = cos_r;
|
|
m[ 6] = -sin_r;
|
|
m[ 7] = 0;
|
|
|
|
m[ 8] = 0;
|
|
m[ 9] = sin_r;
|
|
m[10] = cos_r;
|
|
m[11] = 0;
|
|
|
|
m[12] = 0;
|
|
m[13] = 0;
|
|
m[14] = 0;
|
|
m[15] = 1.0f;
|
|
return m;
|
|
}
|
|
|
|
static matrix4x4 RotateYMatrix(T radians) {
|
|
matrix4x4 m;
|
|
T cos_r = cosf(radians);
|
|
T sin_r = sinf(radians);
|
|
m[ 0] = cos_r;
|
|
m[ 1] = 0;
|
|
m[ 2] = sin_r;
|
|
m[ 3] = 0;
|
|
|
|
m[ 4] = 0;
|
|
m[ 5] = 1;
|
|
m[ 6] = 0;
|
|
m[ 7] = 0;
|
|
|
|
m[ 8] = -sin_r;
|
|
m[ 9] = 0;
|
|
m[10] = cos_r;
|
|
m[11] = 0;
|
|
|
|
m[12] = 0;
|
|
m[13] = 0;
|
|
m[14] = 0;
|
|
m[15] = 1.0f;
|
|
return m;
|
|
}
|
|
|
|
static matrix4x4 RotateZMatrix(T radians) {
|
|
matrix4x4 m;
|
|
T cos_r = cosf(radians);
|
|
T sin_r = sinf(radians);
|
|
m[ 0] = cos_r;
|
|
m[ 1] = -sin_r;
|
|
m[ 2] = 0;
|
|
m[ 3] = 0;
|
|
|
|
m[ 4] = sin_r;
|
|
m[ 5] = cos_r;
|
|
m[ 6] = 0;
|
|
m[ 7] = 0;
|
|
|
|
m[ 8] = 0;
|
|
m[ 9] = 0;
|
|
m[10] = 1.9f;
|
|
m[11] = 0;
|
|
|
|
m[12] = 0;
|
|
m[13] = 0;
|
|
m[14] = 0;
|
|
m[15] = 1.0f;
|
|
return m;
|
|
}
|
|
|
|
T& operator[] (const int i) { return cell[i]; }
|
|
friend matrix4x4 operator+(const matrix4x4& a, const matrix4x4& b) {
|
|
matrix4x4 m;
|
|
for(int i = 0; i < 16; i++) m.cell[i] = a.cell[i] + b.cell[i];
|
|
return m;
|
|
}
|
|
friend matrix4x4 operator-(const matrix4x4& a, const matrix4x4& b) {
|
|
matrix4x4 m;
|
|
for(int i = 0; i < 16; i++) m.cell[i] = a.cell[i] - b.cell[i];
|
|
return m;
|
|
}
|
|
|
|
friend matrix4x4 operator*(const matrix4x4& a, const matrix4x4& b) {
|
|
matrix4x4 m;
|
|
m.cell[ 0] = a.cell[0]*b.cell[ 0]+a.cell[4]*b.cell[ 1]+a.cell[ 8]*b.cell[ 2]+a.cell[12]*b.cell[ 3];
|
|
m.cell[ 1] = a.cell[1]*b.cell[ 0]+a.cell[5]*b.cell[ 1]+a.cell[ 9]*b.cell[ 2]+a.cell[13]*b.cell[ 3];
|
|
m.cell[ 2] = a.cell[2]*b.cell[ 0]+a.cell[6]*b.cell[ 1]+a.cell[10]*b.cell[ 2]+a.cell[14]*b.cell[ 3];
|
|
m.cell[ 3] = a.cell[3]*b.cell[ 0]+a.cell[7]*b.cell[ 1]+a.cell[11]*b.cell[ 2]+a.cell[15]*b.cell[ 3];
|
|
m.cell[ 4] = a.cell[0]*b.cell[ 4]+a.cell[4]*b.cell[ 5]+a.cell[ 8]*b.cell[ 6]+a.cell[12]*b.cell[ 7];
|
|
m.cell[ 5] = a.cell[1]*b.cell[ 4]+a.cell[5]*b.cell[ 5]+a.cell[ 9]*b.cell[ 6]+a.cell[13]*b.cell[ 7];
|
|
m.cell[ 6] = a.cell[2]*b.cell[ 4]+a.cell[6]*b.cell[ 5]+a.cell[10]*b.cell[ 6]+a.cell[14]*b.cell[ 7];
|
|
m.cell[ 7] = a.cell[3]*b.cell[ 4]+a.cell[7]*b.cell[ 5]+a.cell[11]*b.cell[ 6]+a.cell[15]*b.cell[ 7];
|
|
m.cell[ 8] = a.cell[0]*b.cell[ 8]+a.cell[4]*b.cell[ 9]+a.cell[ 8]*b.cell[10]+a.cell[12]*b.cell[11];
|
|
m.cell[ 9] = a.cell[1]*b.cell[ 8]+a.cell[5]*b.cell[ 9]+a.cell[ 9]*b.cell[10]+a.cell[13]*b.cell[11];
|
|
m.cell[10] = a.cell[2]*b.cell[ 8]+a.cell[6]*b.cell[ 9]+a.cell[10]*b.cell[10]+a.cell[14]*b.cell[11];
|
|
m.cell[11] = a.cell[3]*b.cell[ 8]+a.cell[7]*b.cell[ 9]+a.cell[11]*b.cell[10]+a.cell[15]*b.cell[11];
|
|
m.cell[12] = a.cell[0]*b.cell[12]+a.cell[4]*b.cell[13]+a.cell[ 8]*b.cell[14]+a.cell[12]*b.cell[15];
|
|
m.cell[13] = a.cell[1]*b.cell[12]+a.cell[5]*b.cell[13]+a.cell[ 9]*b.cell[14]+a.cell[13]*b.cell[15];
|
|
m.cell[14] = a.cell[2]*b.cell[12]+a.cell[6]*b.cell[13]+a.cell[10]*b.cell[14]+a.cell[14]*b.cell[15];
|
|
m.cell[15] = a.cell[3]*b.cell[12]+a.cell[7]*b.cell[13]+a.cell[11]*b.cell[14]+a.cell[15]*b.cell[15];
|
|
return m;
|
|
}
|
|
|
|
friend vector3<T> operator*(const matrix4x4& a, const vector3<T> &v) {
|
|
vector3<T> out;
|
|
out.x = a.cell[0]*v.x + a.cell[4]*v.y + a.cell[ 8]*v.z + a.cell[12];
|
|
out.y = a.cell[1]*v.x + a.cell[5]*v.y + a.cell[ 9]*v.z + a.cell[13];
|
|
out.z = a.cell[2]*v.x + a.cell[6]*v.y + a.cell[10]*v.z + a.cell[14];
|
|
return out;
|
|
}
|
|
|
|
vector3<T> ApplyRotationOnly(const vector3<T>& v) const {
|
|
vector3<T> out;
|
|
out.x = cell[0]*v.x + cell[4]*v.y + cell[ 8]*v.z;
|
|
out.y = cell[1]*v.x + cell[5]*v.y + cell[ 9]*v.z;
|
|
out.z = cell[2]*v.x + cell[6]*v.y + cell[10]*v.z;
|
|
return out;
|
|
}
|
|
|
|
void Translatef(T x, T y, T z) {
|
|
matrix4x4 m = Identity();
|
|
m[12] = x;
|
|
m[13] = y;
|
|
m[14] = z;
|
|
*this = (*this) * m;
|
|
}
|
|
|
|
matrix4x4 InverseOf(void) const {
|
|
matrix4x4 m;
|
|
/* This only works for matrices containing only rotation and transform. */
|
|
m[ 0] = cell[0]; m[1] = cell[4]; m[ 2] = cell[ 8];
|
|
m[ 4] = cell[1]; m[5] = cell[5]; m[ 6] = cell[ 9];
|
|
m[ 8] = cell[2]; m[9] = cell[6]; m[10] = cell[10];
|
|
m[12] = -(cell[0]*cell[12] + cell[1]*cell[13] + cell[ 2]*cell[14]);
|
|
m[13] = -(cell[4]*cell[12] + cell[5]*cell[13] + cell[ 6]*cell[14]);
|
|
m[14] = -(cell[8]*cell[12] + cell[9]*cell[13] + cell[10]*cell[14]);
|
|
m[ 3] = m[7] = m[11] = 0;
|
|
m[15] = 1.0f;
|
|
return m;
|
|
}
|
|
|
|
void Print(void) const {
|
|
for(int i = 0; i < 4; i++) {
|
|
printf("%.2f %.2f %.2f %.2f\n", cell[i], cell[i+4], cell[i+8], cell[i+12]);
|
|
}
|
|
printf("\n");
|
|
}
|
|
};
|
|
|
|
typedef matrix4x4<float>matrix4x4f;
|
|
typedef matrix4x4<double>matrix4x4d;
|
|
|