274 lines
8.6 KiB
C++
274 lines
8.6 KiB
C++
#include "Matrix4.h"
|
|
|
|
#include "PackStream.h"
|
|
#include "Vector3.h"
|
|
#include <cmath>
|
|
|
|
Matrix4::Matrix4(bool identity) {
|
|
b = c = d = e = g = h = 0.0;
|
|
i = j = l = m = n = o = 0.0;
|
|
a = f = k = p = (identity ? 1.0 : 0.0);
|
|
}
|
|
|
|
Matrix4::Matrix4(double a, double b, double c, double d, double e, double f, double g, double h, double i, double j,
|
|
double k, double l, double m, double n, double o, double p)
|
|
: a(a), b(b), c(c), d(d), e(e), f(f), g(g), h(h), i(i), j(j), k(k), l(l), m(m), n(n), o(o), p(p) {
|
|
}
|
|
|
|
void Matrix4::save(PackStream *stream) const {
|
|
stream->write(&a);
|
|
stream->write(&b);
|
|
stream->write(&c);
|
|
stream->write(&d);
|
|
stream->write(&e);
|
|
stream->write(&f);
|
|
stream->write(&g);
|
|
stream->write(&h);
|
|
stream->write(&i);
|
|
stream->write(&j);
|
|
stream->write(&k);
|
|
stream->write(&l);
|
|
stream->write(&m);
|
|
stream->write(&n);
|
|
stream->write(&o);
|
|
stream->write(&p);
|
|
}
|
|
|
|
void Matrix4::load(PackStream *stream) {
|
|
stream->read(&a);
|
|
stream->read(&b);
|
|
stream->read(&c);
|
|
stream->read(&d);
|
|
stream->read(&e);
|
|
stream->read(&f);
|
|
stream->read(&g);
|
|
stream->read(&h);
|
|
stream->read(&i);
|
|
stream->read(&j);
|
|
stream->read(&k);
|
|
stream->read(&l);
|
|
stream->read(&m);
|
|
stream->read(&n);
|
|
stream->read(&o);
|
|
stream->read(&p);
|
|
}
|
|
|
|
Matrix4 Matrix4::mult(const Matrix4 &other) const {
|
|
Matrix4 result;
|
|
result.a = a * other.a + b * other.e + c * other.i + d * other.m;
|
|
result.b = a * other.b + b * other.f + c * other.j + d * other.n;
|
|
result.c = a * other.c + b * other.g + c * other.k + d * other.o;
|
|
result.d = a * other.d + b * other.h + c * other.l + d * other.p;
|
|
result.e = e * other.a + f * other.e + g * other.i + h * other.m;
|
|
result.f = e * other.b + f * other.f + g * other.j + h * other.n;
|
|
result.g = e * other.c + f * other.g + g * other.k + h * other.o;
|
|
result.h = e * other.d + f * other.h + g * other.l + h * other.p;
|
|
result.i = i * other.a + j * other.e + k * other.i + l * other.m;
|
|
result.j = i * other.b + j * other.f + k * other.j + l * other.n;
|
|
result.k = i * other.c + j * other.g + k * other.k + l * other.o;
|
|
result.l = i * other.d + j * other.h + k * other.l + l * other.p;
|
|
result.m = m * other.a + n * other.e + o * other.i + p * other.m;
|
|
result.n = m * other.b + n * other.f + o * other.j + p * other.n;
|
|
result.o = m * other.c + n * other.g + o * other.k + p * other.o;
|
|
result.p = m * other.d + n * other.h + o * other.l + p * other.p;
|
|
return result;
|
|
}
|
|
|
|
Vector3 Matrix4::multPoint(const Vector3 &v) const {
|
|
Vector3 result;
|
|
result.x = a * v.x + b * v.y + c * v.z + d;
|
|
result.y = e * v.x + f * v.y + g * v.z + h;
|
|
result.z = i * v.x + j * v.y + k * v.z + l;
|
|
return result;
|
|
}
|
|
|
|
Vector3 Matrix4::transform(const Vector3 &v) const {
|
|
double w = m * v.x + n * v.y + o * v.z + p;
|
|
if (w != 0.0) {
|
|
w = 1.0 / w;
|
|
return Vector3((a * v.x + b * v.y + c * v.z + d) * w, (e * v.x + f * v.y + g * v.z + h) * w,
|
|
(i * v.x + j * v.y + k * v.z + l) * w);
|
|
} else {
|
|
return Vector3(a * v.x + b * v.y + c * v.z + d, e * v.x + f * v.y + g * v.z + h,
|
|
i * v.x + j * v.y + k * v.z + l);
|
|
}
|
|
}
|
|
|
|
Matrix4 Matrix4::transposed() const {
|
|
Matrix4 result;
|
|
result.a = a;
|
|
result.e = b;
|
|
result.i = c;
|
|
result.m = d;
|
|
result.b = e;
|
|
result.f = f;
|
|
result.j = g;
|
|
result.n = h;
|
|
result.c = i;
|
|
result.g = j;
|
|
result.k = k;
|
|
result.o = l;
|
|
result.d = m;
|
|
result.h = n;
|
|
result.l = o;
|
|
result.p = p;
|
|
return result;
|
|
}
|
|
|
|
Matrix4 Matrix4::newScale(double x, double y, double z) {
|
|
Matrix4 result;
|
|
result.a = x;
|
|
result.f = y;
|
|
result.k = z;
|
|
return result;
|
|
}
|
|
|
|
Matrix4 Matrix4::newTranslate(double x, double y, double z) {
|
|
Matrix4 result;
|
|
result.d = x;
|
|
result.h = y;
|
|
result.l = z;
|
|
return result;
|
|
}
|
|
|
|
Matrix4 Matrix4::newRotateX(double angle) {
|
|
Matrix4 result;
|
|
double si = sin(angle);
|
|
double co = cos(angle);
|
|
result.f = result.k = co;
|
|
result.g = -si;
|
|
result.j = si;
|
|
return result;
|
|
}
|
|
|
|
Matrix4 Matrix4::newRotateY(double angle) {
|
|
Matrix4 result;
|
|
double si = sin(angle);
|
|
double co = cos(angle);
|
|
result.a = result.k = co;
|
|
result.c = si;
|
|
result.i = -si;
|
|
return result;
|
|
}
|
|
|
|
Matrix4 Matrix4::newRotateZ(double angle) {
|
|
Matrix4 result;
|
|
double si = sin(angle);
|
|
double co = cos(angle);
|
|
result.a = result.f = co;
|
|
result.b = -si;
|
|
result.e = si;
|
|
return result;
|
|
}
|
|
|
|
Matrix4 Matrix4::newRotateAxis(double angle, const Vector3 &_axis) {
|
|
Matrix4 result;
|
|
double si = sin(angle);
|
|
double co = cos(angle);
|
|
double c1 = 1.0 - co;
|
|
Vector3 axis = _axis.normalize();
|
|
result.a = axis.x * axis.x * c1 + co;
|
|
result.b = axis.x * axis.y * c1 - axis.z * si;
|
|
result.c = axis.x * axis.z * c1 + axis.y * si;
|
|
result.e = axis.y * axis.x * c1 + axis.z * si;
|
|
result.f = axis.y * axis.y * c1 + co;
|
|
result.g = axis.y * axis.z * c1 - axis.x * si;
|
|
result.i = axis.x * axis.z * c1 - axis.y * si;
|
|
result.j = axis.y * axis.z * c1 + axis.x * si;
|
|
result.k = axis.z * axis.z * c1 + co;
|
|
return result;
|
|
}
|
|
|
|
Matrix4 Matrix4::newRotateEuler(double heading, double attitude, double bank) {
|
|
Matrix4 result;
|
|
double ch = cos(heading);
|
|
double sh = sin(heading);
|
|
double ca = cos(attitude);
|
|
double sa = sin(attitude);
|
|
double cb = cos(bank);
|
|
double sb = sin(bank);
|
|
result.a = ch * ca;
|
|
result.b = sh * sb - ch * sa * cb;
|
|
result.c = ch * sa * sb + sh * cb;
|
|
result.e = sa;
|
|
result.f = ca * cb;
|
|
result.g = -ca * sb;
|
|
result.i = -sh * ca;
|
|
result.j = sh * sa * cb + ch * sb;
|
|
result.k = -sh * sa * sb + ch * cb;
|
|
return result;
|
|
}
|
|
|
|
Matrix4 Matrix4::newRotateTripleAxis(const Vector3 &x, const Vector3 &y, const Vector3 &z) {
|
|
Matrix4 result;
|
|
result.a = x.x;
|
|
result.b = y.x;
|
|
result.c = z.x;
|
|
result.e = x.y;
|
|
result.f = y.y;
|
|
result.g = z.y;
|
|
result.i = x.z;
|
|
result.j = y.z;
|
|
result.k = z.z;
|
|
return result;
|
|
}
|
|
|
|
Matrix4 Matrix4::newLookAt(const Vector3 &eye, const Vector3 &at, const Vector3 &up) {
|
|
Vector3 z = at.sub(eye).normalize();
|
|
Vector3 x = up.crossProduct(z).normalize();
|
|
Vector3 y = z.crossProduct(x);
|
|
Matrix4 result = Matrix4::newRotateTripleAxis(x, y, z);
|
|
result.d = eye.x;
|
|
result.h = eye.y;
|
|
result.l = eye.z;
|
|
return result.inversed();
|
|
}
|
|
|
|
Matrix4 Matrix4::newPerspective(double fov_y, double aspect, double near, double far) {
|
|
Matrix4 result;
|
|
double fo = 1.0 / tan(fov_y / 2.0);
|
|
result.a = fo / aspect;
|
|
result.f = fo;
|
|
result.k = (far + near) / (near - far);
|
|
result.l = 2.0 * far * near / (near - far);
|
|
result.o = -1.0;
|
|
result.p = 0.0;
|
|
return result;
|
|
}
|
|
|
|
double Matrix4::getDeterminant() const {
|
|
return ((a * f - e * b) * (k * p - o * l) - (a * j - i * b) * (g * p - o * h) + (a * n - m * b) * (g * l - k * h) +
|
|
(e * j - i * f) * (c * p - o * d) - (e * n - m * f) * (c * l - k * d) + (i * n - m * j) * (c * h - g * d));
|
|
}
|
|
|
|
Matrix4 Matrix4::inversed() const {
|
|
Matrix4 result;
|
|
double det = getDeterminant();
|
|
|
|
if (fabs(det) >= 0.00001) {
|
|
det = 1.0 / det;
|
|
|
|
result.a = det * (f * (k * p - o * l) + j * (o * h - g * p) + n * (g * l - k * h));
|
|
result.e = det * (g * (i * p - m * l) + k * (m * h - e * p) + o * (e * l - i * h));
|
|
result.i = det * (h * (i * n - m * j) + l * (m * f - e * n) + p * (e * j - i * f));
|
|
result.m = det * (e * (n * k - j * o) + i * (f * o - n * g) + m * (j * g - f * k));
|
|
|
|
result.b = det * (j * (c * p - o * d) + n * (k * d - c * l) + b * (o * l - k * p));
|
|
result.f = det * (k * (a * p - m * d) + o * (i * d - a * l) + c * (m * l - i * p));
|
|
result.j = det * (l * (a * n - m * b) + p * (i * b - a * j) + d * (m * j - i * n));
|
|
result.n = det * (i * (n * c - b * o) + m * (b * k - j * c) + a * (j * o - n * k));
|
|
|
|
result.c = det * (n * (c * h - g * d) + b * (g * p - o * h) + f * (o * d - c * p));
|
|
result.g = det * (o * (a * h - e * d) + c * (e * p - m * h) + g * (m * d - a * p));
|
|
result.k = det * (p * (a * f - e * b) + d * (e * n - m * f) + h * (m * b - a * n));
|
|
result.o = det * (m * (f * c - b * g) + a * (n * g - f * o) + e * (b * o - n * c));
|
|
|
|
result.d = det * (b * (k * h - g * l) + f * (c * l - k * d) + j * (g * d - c * h));
|
|
result.h = det * (c * (i * h - e * l) + g * (a * l - i * d) + k * (e * d - a * h));
|
|
result.l = det * (d * (i * f - e * j) + h * (a * j - i * b) + l * (e * b - a * f));
|
|
result.p = det * (a * (f * k - j * g) + e * (j * c - b * k) + i * (b * g - f * c));
|
|
}
|
|
|
|
return result;
|
|
}
|