From 38ec1bd50de234faf476daa15ea41778a860ca0b Mon Sep 17 00:00:00 2001 From: Sergeanur Date: Mon, 14 Sep 2020 20:48:49 +0300 Subject: Move a bunch of math to cpp files + small fixes --- src/math/Matrix.cpp | 552 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 552 insertions(+) create mode 100644 src/math/Matrix.cpp (limited to 'src/math/Matrix.cpp') diff --git a/src/math/Matrix.cpp b/src/math/Matrix.cpp new file mode 100644 index 00000000..3e77548d --- /dev/null +++ b/src/math/Matrix.cpp @@ -0,0 +1,552 @@ +#include "common.h" + +CMatrix::CMatrix(void) +{ + m_attachment = nil; + m_hasRwMatrix = false; +} + +CMatrix::CMatrix(CMatrix const &m) +{ + m_attachment = nil; + m_hasRwMatrix = false; + *this = m; +} + +CMatrix::CMatrix(RwMatrix *matrix, bool owner) +{ + m_attachment = nil; + Attach(matrix, owner); +} + +CMatrix::~CMatrix(void) +{ + if (m_hasRwMatrix && m_attachment) + RwMatrixDestroy(m_attachment); +} + +void +CMatrix::Attach(RwMatrix *matrix, bool owner) +{ +#ifdef FIX_BUGS + if (m_attachment && m_hasRwMatrix) +#else + if (m_hasRwMatrix && m_attachment) +#endif + RwMatrixDestroy(m_attachment); + m_attachment = matrix; + m_hasRwMatrix = owner; + Update(); +} + +void +CMatrix::AttachRW(RwMatrix *matrix, bool owner) +{ + if (m_hasRwMatrix && m_attachment) + RwMatrixDestroy(m_attachment); + m_attachment = matrix; + m_hasRwMatrix = owner; + UpdateRW(); +} + +void +CMatrix::Detach(void) +{ + if (m_hasRwMatrix && m_attachment) + RwMatrixDestroy(m_attachment); + m_attachment = nil; +} + +void +CMatrix::Update(void) +{ + m_matrix = *m_attachment; +} + +void +CMatrix::UpdateRW(void) +{ + if (m_attachment) { + *m_attachment = m_matrix; + RwMatrixUpdate(m_attachment); + } +} + +void +CMatrix::operator=(CMatrix const &rhs) +{ + m_matrix = rhs.m_matrix; + if (m_attachment) + UpdateRW(); +} + +void +CMatrix::CopyOnlyMatrix(CMatrix *other) +{ + m_matrix = other->m_matrix; +} + +CMatrix & +CMatrix::operator+=(CMatrix const &rhs) +{ + m_matrix.right.x += rhs.m_matrix.right.x; + m_matrix.up.x += rhs.m_matrix.up.x; + m_matrix.at.x += rhs.m_matrix.at.x; + m_matrix.right.y += rhs.m_matrix.right.y; + m_matrix.up.y += rhs.m_matrix.up.y; + m_matrix.at.y += rhs.m_matrix.at.y; + m_matrix.right.z += rhs.m_matrix.right.z; + m_matrix.up.z += rhs.m_matrix.up.z; + m_matrix.at.z += rhs.m_matrix.at.z; + m_matrix.pos.x += rhs.m_matrix.pos.x; + m_matrix.pos.y += rhs.m_matrix.pos.y; + m_matrix.pos.z += rhs.m_matrix.pos.z; + return *this; +} + +void +CMatrix::SetUnity(void) +{ + m_matrix.right.x = 1.0f; + m_matrix.right.y = 0.0f; + m_matrix.right.z = 0.0f; + m_matrix.up.x = 0.0f; + m_matrix.up.y = 1.0f; + m_matrix.up.z = 0.0f; + m_matrix.at.x = 0.0f; + m_matrix.at.y = 0.0f; + m_matrix.at.z = 1.0f; + m_matrix.pos.x = 0.0f; + m_matrix.pos.y = 0.0f; + m_matrix.pos.z = 0.0f; +} + +void +CMatrix::ResetOrientation(void) +{ + m_matrix.right.x = 1.0f; + m_matrix.right.y = 0.0f; + m_matrix.right.z = 0.0f; + m_matrix.up.x = 0.0f; + m_matrix.up.y = 1.0f; + m_matrix.up.z = 0.0f; + m_matrix.at.x = 0.0f; + m_matrix.at.y = 0.0f; + m_matrix.at.z = 1.0f; +} + +void +CMatrix::SetScale(float s) +{ + m_matrix.right.x = s; + m_matrix.right.y = 0.0f; + m_matrix.right.z = 0.0f; + + m_matrix.up.x = 0.0f; + m_matrix.up.y = s; + m_matrix.up.z = 0.0f; + + m_matrix.at.x = 0.0f; + m_matrix.at.y = 0.0f; + m_matrix.at.z = s; + + m_matrix.pos.x = 0.0f; + m_matrix.pos.y = 0.0f; + m_matrix.pos.z = 0.0f; +} + +void +CMatrix::SetTranslate(float x, float y, float z) +{ + m_matrix.right.x = 1.0f; + m_matrix.right.y = 0.0f; + m_matrix.right.z = 0.0f; + + m_matrix.up.x = 0.0f; + m_matrix.up.y = 1.0f; + m_matrix.up.z = 0.0f; + + m_matrix.at.x = 0.0f; + m_matrix.at.y = 0.0f; + m_matrix.at.z = 1.0f; + + m_matrix.pos.x = x; + m_matrix.pos.y = y; + m_matrix.pos.z = z; +} + +void +CMatrix::SetRotateXOnly(float angle) +{ + float c = Cos(angle); + float s = Sin(angle); + + m_matrix.right.x = 1.0f; + m_matrix.right.y = 0.0f; + m_matrix.right.z = 0.0f; + + m_matrix.up.x = 0.0f; + m_matrix.up.y = c; + m_matrix.up.z = s; + + m_matrix.at.x = 0.0f; + m_matrix.at.y = -s; + m_matrix.at.z = c; +} + +void +CMatrix::SetRotateYOnly(float angle) +{ + float c = Cos(angle); + float s = Sin(angle); + + m_matrix.right.x = c; + m_matrix.right.y = 0.0f; + m_matrix.right.z = -s; + + m_matrix.up.x = 0.0f; + m_matrix.up.y = 1.0f; + m_matrix.up.z = 0.0f; + + m_matrix.at.x = s; + m_matrix.at.y = 0.0f; + m_matrix.at.z = c; +} + +void +CMatrix::SetRotateZOnly(float angle) +{ + float c = Cos(angle); + float s = Sin(angle); + + m_matrix.right.x = c; + m_matrix.right.y = s; + m_matrix.right.z = 0.0f; + + m_matrix.up.x = -s; + m_matrix.up.y = c; + m_matrix.up.z = 0.0f; + + m_matrix.at.x = 0.0f; + m_matrix.at.y = 0.0f; + m_matrix.at.z = 1.0f; +} + +void +CMatrix::SetRotateX(float angle) +{ + SetRotateXOnly(angle); + m_matrix.pos.x = 0.0f; + m_matrix.pos.y = 0.0f; + m_matrix.pos.z = 0.0f; +} + + +void +CMatrix::SetRotateY(float angle) +{ + SetRotateYOnly(angle); + m_matrix.pos.x = 0.0f; + m_matrix.pos.y = 0.0f; + m_matrix.pos.z = 0.0f; +} + +void +CMatrix::SetRotateZ(float angle) +{ + SetRotateZOnly(angle); + m_matrix.pos.x = 0.0f; + m_matrix.pos.y = 0.0f; + m_matrix.pos.z = 0.0f; +} + +void +CMatrix::SetRotate(float xAngle, float yAngle, float zAngle) +{ + float cX = Cos(xAngle); + float sX = Sin(xAngle); + float cY = Cos(yAngle); + float sY = Sin(yAngle); + float cZ = Cos(zAngle); + float sZ = Sin(zAngle); + + m_matrix.right.x = cZ * cY - (sZ * sX) * sY; + m_matrix.right.y = (cZ * sX) * sY + sZ * cY; + m_matrix.right.z = -cX * sY; + + m_matrix.up.x = -sZ * cX; + m_matrix.up.y = cZ * cX; + m_matrix.up.z = sX; + + m_matrix.at.x = (sZ * sX) * cY + cZ * sY; + m_matrix.at.y = sZ * sY - (cZ * sX) * cY; + m_matrix.at.z = cX * cY; + + m_matrix.pos.x = 0.0f; + m_matrix.pos.y = 0.0f; + m_matrix.pos.z = 0.0f; +} + +void +CMatrix::RotateX(float x) +{ + float c = Cos(x); + float s = Sin(x); + + float ry = m_matrix.right.y; + float rz = m_matrix.right.z; + float uy = m_matrix.up.y; + float uz = m_matrix.up.z; + float ay = m_matrix.at.y; + float az = m_matrix.at.z; + float py = m_matrix.pos.y; + float pz = m_matrix.pos.z; + + m_matrix.right.y = c * ry - s * rz; + m_matrix.right.z = c * rz + s * ry; + m_matrix.up.y = c * uy - s * uz; + m_matrix.up.z = c * uz + s * uy; + m_matrix.at.y = c * ay - s * az; + m_matrix.at.z = c * az + s * ay; + m_matrix.pos.y = c * py - s * pz; + m_matrix.pos.z = c * pz + s * py; +} + +void +CMatrix::RotateY(float y) +{ + float c = Cos(y); + float s = Sin(y); + + float rx = m_matrix.right.x; + float rz = m_matrix.right.z; + float ux = m_matrix.up.x; + float uz = m_matrix.up.z; + float ax = m_matrix.at.x; + float az = m_matrix.at.z; + float px = m_matrix.pos.x; + float pz = m_matrix.pos.z; + + m_matrix.right.x = c * rx - s * rz; + m_matrix.right.z = c * rz + s * rx; + m_matrix.up.x = c * ux - s * uz; + m_matrix.up.z = c * uz + s * ux; + m_matrix.at.x = c * ax - s * az; + m_matrix.at.z = c * az + s * ax; + m_matrix.pos.x = c * px - s * pz; + m_matrix.pos.z = c * pz + s * px; +} + +void +CMatrix::RotateZ(float z) +{ + float c = Cos(z); + float s = Sin(z); + + float ry = m_matrix.right.y; + float rx = m_matrix.right.x; + float uy = m_matrix.up.y; + float ux = m_matrix.up.x; + float ay = m_matrix.at.y; + float ax = m_matrix.at.x; + float py = m_matrix.pos.y; + float px = m_matrix.pos.x; + + m_matrix.right.x = c * rx - s * ry; + m_matrix.right.y = c * ry + s * rx; + m_matrix.up.x = c * ux - s * uy; + m_matrix.up.y = c * uy + s * ux; + m_matrix.at.x = c * ax - s * ay; + m_matrix.at.y = c * ay + s * ax; + m_matrix.pos.x = c * px - s * py; + m_matrix.pos.y = c * py + s * px; + +} + +void +CMatrix::Rotate(float x, float y, float z) +{ + float cX = Cos(x); + float sX = Sin(x); + float cY = Cos(y); + float sY = Sin(y); + float cZ = Cos(z); + float sZ = Sin(z); + + float rx = m_matrix.right.x; + float ry = m_matrix.right.y; + float rz = m_matrix.right.z; + float ux = m_matrix.up.x; + float uy = m_matrix.up.y; + float uz = m_matrix.up.z; + float ax = m_matrix.at.x; + float ay = m_matrix.at.y; + float az = m_matrix.at.z; + float px = m_matrix.pos.x; + float py = m_matrix.pos.y; + float pz = m_matrix.pos.z; + + float x1 = cZ * cY - (sZ * sX) * sY; + float x2 = (cZ * sX) * sY + sZ * cY; + float x3 = -cX * sY; + float y1 = -sZ * cX; + float y2 = cZ * cX; + float y3 = sX; + float z1 = (sZ * sX) * cY + cZ * sY; + float z2 = sZ * sY - (cZ * sX) * cY; + float z3 = cX * cY; + + m_matrix.right.x = x1 * rx + y1 * ry + z1 * rz; + m_matrix.right.y = x2 * rx + y2 * ry + z2 * rz; + m_matrix.right.z = x3 * rx + y3 * ry + z3 * rz; + m_matrix.up.x = x1 * ux + y1 * uy + z1 * uz; + m_matrix.up.y = x2 * ux + y2 * uy + z2 * uz; + m_matrix.up.z = x3 * ux + y3 * uy + z3 * uz; + m_matrix.at.x = x1 * ax + y1 * ay + z1 * az; + m_matrix.at.y = x2 * ax + y2 * ay + z2 * az; + m_matrix.at.z = x3 * ax + y3 * ay + z3 * az; + m_matrix.pos.x = x1 * px + y1 * py + z1 * pz; + m_matrix.pos.y = x2 * px + y2 * py + z2 * pz; + m_matrix.pos.z = x3 * px + y3 * py + z3 * pz; +} + +CMatrix & +CMatrix::operator*=(CMatrix const &rhs) +{ + // TODO: VU0 code + *this = *this * rhs; + return *this; +} + +void +CMatrix::Reorthogonalise(void) +{ + CVector &r = GetRight(); + CVector &f = GetForward(); + CVector &u = GetUp(); + u = CrossProduct(r, f); + u.Normalise(); + r = CrossProduct(f, u); + r.Normalise(); + f = CrossProduct(u, r); +} + +CMatrix +operator*(const CMatrix &m1, const CMatrix &m2) +{ + // TODO: VU0 code + CMatrix out; + RwMatrix *dst = &out.m_matrix; + const RwMatrix *src1 = &m1.m_matrix; + const RwMatrix *src2 = &m2.m_matrix; + dst->right.x = src1->right.x * src2->right.x + src1->up.x * src2->right.y + src1->at.x * src2->right.z; + dst->right.y = src1->right.y * src2->right.x + src1->up.y * src2->right.y + src1->at.y * src2->right.z; + dst->right.z = src1->right.z * src2->right.x + src1->up.z * src2->right.y + src1->at.z * src2->right.z; + dst->up.x = src1->right.x * src2->up.x + src1->up.x * src2->up.y + src1->at.x * src2->up.z; + dst->up.y = src1->right.y * src2->up.x + src1->up.y * src2->up.y + src1->at.y * src2->up.z; + dst->up.z = src1->right.z * src2->up.x + src1->up.z * src2->up.y + src1->at.z * src2->up.z; + dst->at.x = src1->right.x * src2->at.x + src1->up.x * src2->at.y + src1->at.x * src2->at.z; + dst->at.y = src1->right.y * src2->at.x + src1->up.y * src2->at.y + src1->at.y * src2->at.z; + dst->at.z = src1->right.z * src2->at.x + src1->up.z * src2->at.y + src1->at.z * src2->at.z; + dst->pos.x = src1->right.x * src2->pos.x + src1->up.x * src2->pos.y + src1->at.x * src2->pos.z + src1->pos.x; + dst->pos.y = src1->right.y * src2->pos.x + src1->up.y * src2->pos.y + src1->at.y * src2->pos.z + src1->pos.y; + dst->pos.z = src1->right.z * src2->pos.x + src1->up.z * src2->pos.y + src1->at.z * src2->pos.z + src1->pos.z; + return out; +} + +CMatrix & +Invert(const CMatrix &src, CMatrix &dst) +{ + // TODO: VU0 code + // GTA handles this as a raw 4x4 orthonormal matrix + // and trashes the RW flags, let's not do that + float (*scr_fm)[4] = (float (*)[4])&src.m_matrix; + float (*dst_fm)[4] = (float (*)[4])&dst.m_matrix; + + dst_fm[3][0] = dst_fm[3][1] = dst_fm[3][2] = 0.0f; +#ifndef FIX_BUGS + dst_fm[3][3] = scr_fm[3][3]; +#endif + + dst_fm[0][0] = scr_fm[0][0]; + dst_fm[0][1] = scr_fm[1][0]; + dst_fm[0][2] = scr_fm[2][0]; +#ifndef FIX_BUGS + dst_fm[0][3] = scr_fm[3][0]; +#endif + dst_fm[1][0] = scr_fm[0][1]; + dst_fm[1][1] = scr_fm[1][1]; + dst_fm[1][2] = scr_fm[2][1]; +#ifndef FIX_BUGS + dst_fm[1][3] = scr_fm[3][1]; +#endif + dst_fm[2][0] = scr_fm[0][2]; + dst_fm[2][1] = scr_fm[1][2]; + dst_fm[2][2] = scr_fm[2][2]; +#ifndef FIX_BUGS + dst_fm[2][3] = scr_fm[3][2]; +#endif + + dst_fm[3][0] += dst_fm[0][0] * scr_fm[3][0]; + dst_fm[3][1] += dst_fm[0][1] * scr_fm[3][0]; + dst_fm[3][2] += dst_fm[0][2] * scr_fm[3][0]; +#ifndef FIX_BUGS + dst_fm[3][3] += dst_fm[0][3] * scr_fm[3][0]; +#endif + + dst_fm[3][0] += dst_fm[1][0] * scr_fm[3][1]; + dst_fm[3][1] += dst_fm[1][1] * scr_fm[3][1]; + dst_fm[3][2] += dst_fm[1][2] * scr_fm[3][1]; +#ifndef FIX_BUGS + dst_fm[3][3] += dst_fm[1][3] * scr_fm[3][1]; +#endif + + dst_fm[3][0] += dst_fm[2][0] * scr_fm[3][2]; + dst_fm[3][1] += dst_fm[2][1] * scr_fm[3][2]; + dst_fm[3][2] += dst_fm[2][2] * scr_fm[3][2]; +#ifndef FIX_BUGS + dst_fm[3][3] += dst_fm[2][3] * scr_fm[3][2]; +#endif + + dst_fm[3][0] = -dst_fm[3][0]; + dst_fm[3][1] = -dst_fm[3][1]; + dst_fm[3][2] = -dst_fm[3][2]; +#ifndef FIX_BUGS + dst_fm[3][3] = scr_fm[3][3] - dst_fm[3][3]; +#endif + + return dst; +} + +CMatrix +Invert(const CMatrix &matrix) +{ + CMatrix inv; + return Invert(matrix, inv); +} + +void +CCompressedMatrixNotAligned::CompressFromFullMatrix(CMatrix &other) +{ + m_rightX = 127.0f * other.GetRight().x; + m_rightY = 127.0f * other.GetRight().y; + m_rightZ = 127.0f * other.GetRight().z; + m_upX = 127.0f * other.GetForward().x; + m_upY = 127.0f * other.GetForward().y; + m_upZ = 127.0f * other.GetForward().z; + m_vecPos = other.GetPosition(); +} + +void +CCompressedMatrixNotAligned::DecompressIntoFullMatrix(CMatrix &other) +{ + other.GetRight().x = m_rightX / 127.0f; + other.GetRight().y = m_rightY / 127.0f; + other.GetRight().z = m_rightZ / 127.0f; + other.GetForward().x = m_upX / 127.0f; + other.GetForward().y = m_upY / 127.0f; + other.GetForward().z = m_upZ / 127.0f; + other.GetUp() = CrossProduct(other.GetRight(), other.GetForward()); + other.GetPosition() = m_vecPos; + other.Reorthogonalise(); +} \ No newline at end of file -- cgit v1.2.3