From 9f61c9e6ac6b1ac5692cf6352d2ebbd47a31a686 Mon Sep 17 00:00:00 2001 From: claude-bot Date: Mon, 13 Jul 2026 12:27:07 +0000 Subject: Import Cai1Hsu/re3 @ miami (reVC / GTA:VC decompilation) Snapshot import (no upstream history) into git.ancap.in.ua/claude, per @lzcnt. Source: https://github.com/Cai1Hsu/re3 branch miami. --- src/math/Matrix.cpp | 531 ++++++++++++++++++++++++++++++++++++++++++++++++ src/math/Matrix.h | 170 ++++++++++++++++ src/math/Quaternion.cpp | 177 ++++++++++++++++ src/math/Quaternion.h | 95 +++++++++ src/math/Rect.cpp | 17 ++ src/math/Rect.h | 71 +++++++ src/math/Vector.cpp | 46 +++++ src/math/Vector.h | 129 ++++++++++++ src/math/Vector2D.h | 104 ++++++++++ src/math/VuVector.h | 30 +++ src/math/math.cpp | 118 +++++++++++ src/math/maths.h | 19 ++ 12 files changed, 1507 insertions(+) create mode 100644 src/math/Matrix.cpp create mode 100644 src/math/Matrix.h create mode 100644 src/math/Quaternion.cpp create mode 100644 src/math/Quaternion.h create mode 100644 src/math/Rect.cpp create mode 100644 src/math/Rect.h create mode 100644 src/math/Vector.cpp create mode 100644 src/math/Vector.h create mode 100644 src/math/Vector2D.h create mode 100644 src/math/VuVector.h create mode 100644 src/math/math.cpp create mode 100644 src/math/maths.h (limited to 'src/math') diff --git a/src/math/Matrix.cpp b/src/math/Matrix.cpp new file mode 100644 index 0000000..c0d909c --- /dev/null +++ b/src/math/Matrix.cpp @@ -0,0 +1,531 @@ +#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) +{ + GetRight() = m_attachment->right; + GetForward() = m_attachment->up; + GetUp() = m_attachment->at; + GetPosition() = m_attachment->pos; +} + +void +CMatrix::UpdateRW(void) +{ + if (m_attachment) { + m_attachment->right = GetRight(); + m_attachment->up = GetForward(); + m_attachment->at = GetUp(); + m_attachment->pos = GetPosition(); + RwMatrixUpdate(m_attachment); + } +} + +void +CMatrix::operator=(CMatrix const &rhs) +{ + memcpy(this, &rhs, sizeof(f)); + if (m_attachment) + UpdateRW(); +} + +void +CMatrix::CopyOnlyMatrix(const CMatrix &other) +{ + memcpy(this, &other, sizeof(f)); +} + +CMatrix & +CMatrix::operator+=(CMatrix const &rhs) +{ + GetRight() += rhs.GetRight(); + GetForward() += rhs.GetForward(); + GetUp() += rhs.GetUp(); + GetPosition() += rhs.GetPosition(); + return *this; +} + +void +CMatrix::SetUnity(void) +{ + rx = 1.0f; + ry = 0.0f; + rz = 0.0f; + fx = 0.0f; + fy = 1.0f; + fz = 0.0f; + ux = 0.0f; + uy = 0.0f; + uz = 1.0f; + px = 0.0f; + py = 0.0f; + pz = 0.0f; +} + +void +CMatrix::ResetOrientation(void) +{ + rx = 1.0f; + ry = 0.0f; + rz = 0.0f; + fx = 0.0f; + fy = 1.0f; + fz = 0.0f; + ux = 0.0f; + uy = 0.0f; + uz = 1.0f; +} + +void +CMatrix::SetScale(float s) +{ + rx = s; + ry = 0.0f; + rz = 0.0f; + + fx = 0.0f; + fy = s; + fz = 0.0f; + + ux = 0.0f; + uy = 0.0f; + uz = s; + + px = 0.0f; + py = 0.0f; + pz = 0.0f; +} + +void +CMatrix::SetTranslate(float x, float y, float z) +{ + rx = 1.0f; + ry = 0.0f; + rz = 0.0f; + + fx = 0.0f; + fy = 1.0f; + fz = 0.0f; + + ux = 0.0f; + uy = 0.0f; + uz = 1.0f; + + px = x; + py = y; + pz = z; +} + +void +CMatrix::SetRotateXOnly(float angle) +{ + float c = Cos(angle); + float s = Sin(angle); + + rx = 1.0f; + ry = 0.0f; + rz = 0.0f; + + fx = 0.0f; + fy = c; + fz = s; + + ux = 0.0f; + uy = -s; + uz = c; +} + +void +CMatrix::SetRotateYOnly(float angle) +{ + float c = Cos(angle); + float s = Sin(angle); + + rx = c; + ry = 0.0f; + rz = -s; + + fx = 0.0f; + fy = 1.0f; + fz = 0.0f; + + ux = s; + uy = 0.0f; + uz = c; +} + +void +CMatrix::SetRotateZOnly(float angle) +{ + float c = Cos(angle); + float s = Sin(angle); + + rx = c; + ry = s; + rz = 0.0f; + + fx = -s; + fy = c; + fz = 0.0f; + + ux = 0.0f; + uy = 0.0f; + uz = 1.0f; +} + +void +CMatrix::SetRotateX(float angle) +{ + SetRotateXOnly(angle); + px = 0.0f; + py = 0.0f; + pz = 0.0f; +} + + +void +CMatrix::SetRotateY(float angle) +{ + SetRotateYOnly(angle); + px = 0.0f; + py = 0.0f; + pz = 0.0f; +} + +void +CMatrix::SetRotateZ(float angle) +{ + SetRotateZOnly(angle); + px = 0.0f; + py = 0.0f; + pz = 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); + + rx = cZ * cY - (sZ * sX) * sY; + ry = (cZ * sX) * sY + sZ * cY; + rz = -cX * sY; + + fx = -sZ * cX; + fy = cZ * cX; + fz = sX; + + ux = (sZ * sX) * cY + cZ * sY; + uy = sZ * sY - (cZ * sX) * cY; + uz = cX * cY; + + px = 0.0f; + py = 0.0f; + pz = 0.0f; +} + +void +CMatrix::RotateX(float x) +{ + float c = Cos(x); + float s = Sin(x); + + float ry = this->ry; + float rz = this->rz; + float uy = this->fy; + float uz = this->fz; + float ay = this->uy; + float az = this->uz; + float py = this->py; + float pz = this->pz; + + this->ry = c * ry - s * rz; + this->rz = c * rz + s * ry; + this->fy = c * uy - s * uz; + this->fz = c * uz + s * uy; + this->uy = c * ay - s * az; + this->uz = c * az + s * ay; + this->py = c * py - s * pz; + this->pz = c * pz + s * py; +} + +void +CMatrix::RotateY(float y) +{ + float c = Cos(y); + float s = Sin(y); + + float rx = this->rx; + float rz = this->rz; + float ux = this->fx; + float uz = this->fz; + float ax = this->ux; + float az = this->uz; + float px = this->px; + float pz = this->pz; + + this->rx = c * rx + s * rz; + this->rz = c * rz - s * rx; + this->fx = c * ux + s * uz; + this->fz = c * uz - s * ux; + this->ux = c * ax + s * az; + this->uz = c * az - s * ax; + this->px = c * px + s * pz; + this->pz = c * pz - s * px; +} + +void +CMatrix::RotateZ(float z) +{ + float c = Cos(z); + float s = Sin(z); + + float ry = this->ry; + float rx = this->rx; + float uy = this->fy; + float ux = this->fx; + float ay = this->uy; + float ax = this->ux; + float py = this->py; + float px = this->px; + + this->rx = c * rx - s * ry; + this->ry = c * ry + s * rx; + this->fx = c * ux - s * uy; + this->fy = c * uy + s * ux; + this->ux = c * ax - s * ay; + this->uy = c * ay + s * ax; + this->px = c * px - s * py; + this->py = 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 = this->rx; + float ry = this->ry; + float rz = this->rz; + float ux = this->fx; + float uy = this->fy; + float uz = this->fz; + float ax = this->ux; + float ay = this->uy; + float az = this->uz; + float px = this->px; + float py = this->py; + float pz = this->pz; + + 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; + + this->rx = x1 * rx + y1 * ry + z1 * rz; + this->ry = x2 * rx + y2 * ry + z2 * rz; + this->rz = x3 * rx + y3 * ry + z3 * rz; + this->fx = x1 * ux + y1 * uy + z1 * uz; + this->fy = x2 * ux + y2 * uy + z2 * uz; + this->fz = x3 * ux + y3 * uy + z3 * uz; + this->ux = x1 * ax + y1 * ay + z1 * az; + this->uy = x2 * ax + y2 * ay + z2 * az; + this->uz = x3 * ax + y3 * ay + z3 * az; + this->px = x1 * px + y1 * py + z1 * pz; + this->py = x2 * px + y2 * py + z2 * pz; + this->pz = 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; + out.rx = m1.rx * m2.rx + m1.fx * m2.ry + m1.ux * m2.rz; + out.ry = m1.ry * m2.rx + m1.fy * m2.ry + m1.uy * m2.rz; + out.rz = m1.rz * m2.rx + m1.fz * m2.ry + m1.uz * m2.rz; + out.fx = m1.rx * m2.fx + m1.fx * m2.fy + m1.ux * m2.fz; + out.fy = m1.ry * m2.fx + m1.fy * m2.fy + m1.uy * m2.fz; + out.fz = m1.rz * m2.fx + m1.fz * m2.fy + m1.uz * m2.fz; + out.ux = m1.rx * m2.ux + m1.fx * m2.uy + m1.ux * m2.uz; + out.uy = m1.ry * m2.ux + m1.fy * m2.uy + m1.uy * m2.uz; + out.uz = m1.rz * m2.ux + m1.fz * m2.uy + m1.uz * m2.uz; + out.px = m1.rx * m2.px + m1.fx * m2.py + m1.ux * m2.pz + m1.px; + out.py = m1.ry * m2.px + m1.fy * m2.py + m1.uy * m2.pz + m1.py; + out.pz = m1.rz * m2.px + m1.fz * m2.py + m1.uz * m2.pz + m1.pz; + return out; +} + +CMatrix & +Invert(const CMatrix &src, CMatrix &dst) +{ + // TODO: VU0 code + dst.f[3][0] = dst.f[3][1] = dst.f[3][2] = 0.0f; + + dst.f[0][0] = src.f[0][0]; + dst.f[0][1] = src.f[1][0]; + dst.f[0][2] = src.f[2][0]; + + dst.f[1][0] = src.f[0][1]; + dst.f[1][1] = src.f[1][1]; + dst.f[1][2] = src.f[2][1]; + + dst.f[2][0] = src.f[0][2]; + dst.f[2][1] = src.f[1][2]; + dst.f[2][2] = src.f[2][2]; + + + dst.f[3][0] += dst.f[0][0] * src.f[3][0]; + dst.f[3][1] += dst.f[0][1] * src.f[3][0]; + dst.f[3][2] += dst.f[0][2] * src.f[3][0]; + + dst.f[3][0] += dst.f[1][0] * src.f[3][1]; + dst.f[3][1] += dst.f[1][1] * src.f[3][1]; + dst.f[3][2] += dst.f[1][2] * src.f[3][1]; + + dst.f[3][0] += dst.f[2][0] * src.f[3][2]; + dst.f[3][1] += dst.f[2][1] * src.f[3][2]; + dst.f[3][2] += dst.f[2][2] * src.f[3][2]; + + dst.f[3][0] = -dst.f[3][0]; + dst.f[3][1] = -dst.f[3][1]; + dst.f[3][2] = -dst.f[3][2]; + + return dst; +} + +void +CMatrix::CopyToRwMatrix(RwMatrix* matrix) +{ + matrix->right = GetRight(); + matrix->up = GetForward(); + matrix->at = GetUp(); + matrix->pos = GetPosition(); + RwMatrixUpdate(matrix); +} + +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 diff --git a/src/math/Matrix.h b/src/math/Matrix.h new file mode 100644 index 0000000..6da4c76 --- /dev/null +++ b/src/math/Matrix.h @@ -0,0 +1,170 @@ +#pragma once + +class CMatrix +{ +public: +#ifdef GTA_PS2 + union + { + float f[4][4]; + struct + { + float rx, ry, rz; + RwMatrix *m_attachment; + float fx, fy, fz; + bool m_hasRwMatrix; // are we the owner? + float ux, uy, uz, uw; + float px, py, pz, pw; + }; + }; +#else + union + { + float f[4][4]; + struct + { + float rx, ry, rz, rw; + float fx, fy, fz, fw; + float ux, uy, uz, uw; + float px, py, pz, pw; + }; + }; + + RwMatrix *m_attachment; + bool m_hasRwMatrix; // are we the owner? +#endif + + CMatrix(void); + CMatrix(CMatrix const &m); + CMatrix(RwMatrix *matrix, bool owner = false); + CMatrix(float scale){ + m_attachment = nil; + m_hasRwMatrix = false; + SetScale(scale); + } + ~CMatrix(void); + void Attach(RwMatrix *matrix, bool owner = false); + void AttachRW(RwMatrix *matrix, bool owner = false); + void Detach(void); + void Update(void); + void UpdateRW(void); + void operator=(CMatrix const &rhs); + CMatrix &operator+=(CMatrix const &rhs); + CMatrix &operator*=(CMatrix const &rhs); + + CVector &GetPosition(void) { return *(CVector*)&px; } + CVector &GetRight(void) { return *(CVector*)℞ } + CVector &GetForward(void) { return *(CVector*)&fx; } + CVector &GetUp(void) { return *(CVector*)&ux; } + + const CVector &GetPosition(void) const { return *(CVector*)&px; } + const CVector &GetRight(void) const { return *(CVector*)℞ } + const CVector &GetForward(void) const { return *(CVector*)&fx; } + const CVector &GetUp(void) const { return *(CVector*)&ux; } + + + void SetTranslate(float x, float y, float z); + void SetTranslate(const CVector &trans){ SetTranslate(trans.x, trans.y, trans.z); } + void Translate(float x, float y, float z){ + px += x; + py += y; + pz += z; + } + void Translate(const CVector &trans){ Translate(trans.x, trans.y, trans.z); } + + void SetScale(float s); + void Scale(float scale) + { + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + f[i][j] *= scale; + } + void Scale(float sx, float sy, float sz) + { + for (int i = 0; i < 3; i++){ + f[i][0] *= sx; + f[i][1] *= sy; + f[i][2] *= sz; + } + } + + + void SetRotateXOnly(float angle); + void SetRotateYOnly(float angle); + void SetRotateZOnly(float angle); + void SetRotateZOnlyScaled(float angle, float scale) { + float c = Cos(angle); + float s = Sin(angle); + + rx = c * scale; + ry = s * scale; + rz = 0.0f; + + fx = -s * scale; + fy = c * scale; + fz = 0.0f; + + ux = 0.0f; + uy = 0.0f; + uz = scale; + } + void SetRotateX(float angle); + void SetRotateY(float angle); + void SetRotateZ(float angle); + void SetRotate(float xAngle, float yAngle, float zAngle); + void Rotate(float x, float y, float z); + void RotateX(float x); + void RotateY(float y); + void RotateZ(float z); + + void Reorthogonalise(void); + void CopyOnlyMatrix(const CMatrix &other); + void SetUnity(void); + void ResetOrientation(void); + + void CopyToRwMatrix(RwMatrix* matrix); + + void SetTranslateOnly(float x, float y, float z) { + px = x; + py = y; + pz = z; + } + void SetTranslateOnly(const CVector& pos) { + SetTranslateOnly(pos.x, pos.y, pos.z); + } + void CheckIntegrity(){} +}; + + +CMatrix &Invert(const CMatrix &src, CMatrix &dst); +CMatrix Invert(const CMatrix &matrix); +CMatrix operator*(const CMatrix &m1, const CMatrix &m2); +inline CVector MultiplyInverse(const CMatrix &mat, const CVector &vec) +{ + CVector v(vec.x - mat.px, vec.y - mat.py, vec.z - mat.pz); + return CVector( + mat.rx * v.x + mat.ry * v.y + mat.rz * v.z, + mat.fx * v.x + mat.fy * v.y + mat.fz * v.z, + mat.ux * v.x + mat.uy * v.y + mat.uz * v.z); +} + + + +class CCompressedMatrixNotAligned +{ + CVector m_vecPos; + int8 m_rightX; + int8 m_rightY; + int8 m_rightZ; + int8 m_upX; + int8 m_upY; + int8 m_upZ; +public: + void CompressFromFullMatrix(CMatrix &other); + void DecompressIntoFullMatrix(CMatrix &other); +}; + +class CCompressedMatrix : public CCompressedMatrixNotAligned +{ + int _alignment; // no clue what would this align to +}; \ No newline at end of file diff --git a/src/math/Quaternion.cpp b/src/math/Quaternion.cpp new file mode 100644 index 0000000..b0e782e --- /dev/null +++ b/src/math/Quaternion.cpp @@ -0,0 +1,177 @@ +#include "common.h" +#include "Quaternion.h" + +void +CQuaternion::Normalise(void) +{ + float sq = MagnitudeSqr(); + if (sq == 0.0f) + w = 1.0f; + else { + float invsqrt = RecipSqrt(sq); + x *= invsqrt; + y *= invsqrt; + z *= invsqrt; + w *= invsqrt; + } +} + +void +CQuaternion::Slerp(const CQuaternion &q1, const CQuaternion &q2, float theta, float invSin, float t) +{ + if (theta == 0.0f) + *this = q2; + else { + float w1, w2; + if (theta > PI / 2) { + theta = PI - theta; + w1 = Sin((1.0f - t) * theta) * invSin; + w2 = -Sin(t * theta) * invSin; + } else { + w1 = Sin((1.0f - t) * theta) * invSin; + w2 = Sin(t * theta) * invSin; + } + // TODO: VU0 code + *this = w1 * q1 + w2 * q2; + } +} + +void +CQuaternion::Multiply(const CQuaternion &q1, const CQuaternion &q2) +{ + x = (q2.z * q1.y) - (q1.z * q2.y) + (q1.x * q2.w) + (q2.x * q1.w); + y = (q2.x * q1.z) - (q1.x * q2.z) + (q1.y * q2.w) + (q2.y * q1.w); + z = (q2.y * q1.x) - (q1.y * q2.x) + (q1.z * q2.w) + (q2.z * q1.w); + w = (q2.w * q1.w) - (q2.x * q1.x) - (q2.y * q1.y) - (q2.z * q1.z); +} + +void +CQuaternion::Get(RwV3d *axis, float *angle) +{ + *angle = Acos(w); + float s = Sin(*angle); + + axis->x = x * (1.0f / s); + axis->y = y * (1.0f / s); + axis->z = z * (1.0f / s); +} + +void +CQuaternion::Set(RwV3d *axis, float angle) +{ + float halfCos = Cos(angle * 0.5f); + float halfSin = Sin(angle * 0.5f); + x = axis->x * halfSin; + y = axis->y * halfSin; + z = axis->z * halfSin; + w = halfCos; +} + +void +CQuaternion::Get(RwMatrix *matrix) +{ + float x2 = x + x; + float y2 = y + y; + float z2 = z + z; + + float x_2x = x * x2; + float x_2y = x * y2; + float x_2z = x * z2; + float y_2y = y * y2; + float y_2z = y * z2; + float z_2z = z * z2; + float w_2x = w * x2; + float w_2y = w * y2; + float w_2z = w * z2; + + matrix->right.x = 1.0f - (y_2y + z_2z); + matrix->up.x = x_2y - w_2z; + matrix->at.x = x_2z + w_2y; + matrix->right.y = x_2y + w_2z; + matrix->up.y = 1.0f - (x_2x + z_2z); + matrix->at.y = y_2z - w_2x; + matrix->right.z = x_2z - w_2y; + matrix->up.z = y_2z + w_2x; + matrix->at.z = 1.0f - (x_2x + y_2y); +} + +void +CQuaternion::Set(const RwMatrix &matrix) +{ + float f, s, m; + + f = matrix.up.y + matrix.right.x + matrix.at.z; + if (f >= 0.0f) { + s = Sqrt(f + 1.0f); + w = 0.5f * s; + m = 0.5f / s; + x = (matrix.up.z - matrix.at.y) * m; + y = (matrix.at.x - matrix.right.z) * m; + z = (matrix.right.y - matrix.up.x) * m; + return; + } + + f = matrix.right.x - matrix.up.y - matrix.at.z; + if (f >= 0.0f) { + s = Sqrt(f + 1.0f); + x = 0.5f * s; + m = 0.5f / s; + y = (matrix.up.x + matrix.right.y) * m; + z = (matrix.at.x + matrix.right.z) * m; + w = (matrix.up.z - matrix.at.y) * m; + return; + } + + f = matrix.up.y - matrix.right.x - matrix.at.z; + if (f >= 0.0f) { + s = Sqrt(f + 1.0f); + y = 0.5f * s; + m = 0.5f / s; + w = (matrix.at.x - matrix.right.z) * m; + x = (matrix.up.x - matrix.right.y) * m; + z = (matrix.at.y + matrix.up.z) * m; + return; + } + + f = matrix.at.z - (matrix.up.y + matrix.right.x); + s = Sqrt(f + 1.0f); + z = 0.5f * s; + m = 0.5f / s; + w = (matrix.right.y - matrix.up.x) * m; + x = (matrix.at.x + matrix.right.z) * m; + y = (matrix.at.y + matrix.up.z) * m; +} + +void +CQuaternion::Get(float *f1, float *f2, float *f3) +{ + RwMatrix matrix; + + Get(&matrix); + *f3 = Atan2(matrix.right.y, matrix.up.y); + if (*f3 < 0.0f) + *f3 += TWOPI; + float s = Sin(*f3); + float c = Cos(*f3); + *f1 = Atan2(-matrix.at.y, s * matrix.right.y + c * matrix.up.y); + if (*f1 < 0.0f) + *f1 += TWOPI; + *f2 = Atan2(-(matrix.right.z * c - matrix.up.z * s), matrix.right.x * c - matrix.up.x * s); + if (*f2 < 0.0f) + *f2 += TWOPI; +} + +void +CQuaternion::Set(float f1, float f2, float f3) +{ + float c1 = Cos(f1 * 0.5f); + float c2 = Cos(f2 * 0.5f); + float c3 = Cos(f3 * 0.5f); + float s1 = Sin(f1 * 0.5f); + float s2 = Sin(f2 * 0.5f); + float s3 = Sin(f3 * 0.5f); + x = ((c2 * c1) * s3) - ((s2 * s1) * c3); + y = ((s1 * c2) * c3) + ((s2 * c1) * s3); + z = ((s2 * c1) * c3) - ((s1 * c2) * s3); + w = ((c2 * c1) * c3) + ((s2 * s1) * s3); +} \ No newline at end of file diff --git a/src/math/Quaternion.h b/src/math/Quaternion.h new file mode 100644 index 0000000..47c94f7 --- /dev/null +++ b/src/math/Quaternion.h @@ -0,0 +1,95 @@ +#pragma once + +// TODO: actually implement this +class CQuaternion +{ +public: + float x, y, z, w; + CQuaternion(void) {} + CQuaternion(float x, float y, float z, float w) : x(x), y(y), z(z), w(w) {} + + float Magnitude(void) const { return Sqrt(x*x + y*y + z*z + w*w); } + float MagnitudeSqr(void) const { return x*x + y*y + z*z + w*w; } + void Normalise(void); + void Multiply(const CQuaternion &q1, const CQuaternion &q2); + void Invert(void){ // Conjugate would have been a better name + x = -x; + y = -y; + z = -z; + } + + const CQuaternion &operator+=(CQuaternion const &right) { + x += right.x; + y += right.y; + z += right.z; + w += right.w; + return *this; + } + + const CQuaternion &operator-=(CQuaternion const &right) { + x -= right.x; + y -= right.y; + z -= right.z; + w -= right.w; + return *this; + } + + const CQuaternion &operator*=(float right) { + x *= right; + y *= right; + z *= right; + w *= right; + return *this; + } + + const CQuaternion &operator/=(float right) { + x /= right; + y /= right; + z /= right; + w /= right; + return *this; + } + + CQuaternion operator-() const { + return CQuaternion(-x, -y, -z, -w); + } + + void Slerp(const CQuaternion &q1, const CQuaternion &q2, float theta, float invSin, float t); + void Get(RwV3d *axis, float *angle); + void Set(RwV3d *axis, float angle); + void Get(RwMatrix *matrix); + void Set(const RwMatrix &matrix); + void Set(float f1, float f2, float f3); + void Get(float *f1, float *f2, float *f3); +}; + +inline float +DotProduct(const CQuaternion &q1, const CQuaternion &q2) +{ + return q1.x*q2.x + q1.y*q2.y + q1.z*q2.z + q1.w*q2.w; +} + +inline CQuaternion operator+(const CQuaternion &left, const CQuaternion &right) +{ + return CQuaternion(left.x + right.x, left.y + right.y, left.z + right.z, left.w + right.w); +} + +inline CQuaternion operator-(const CQuaternion &left, const CQuaternion &right) +{ + return CQuaternion(left.x - right.x, left.y - right.y, left.z - right.z, left.w - right.w); +} + +inline CQuaternion operator*(const CQuaternion &left, float right) +{ + return CQuaternion(left.x * right, left.y * right, left.z * right, left.w * right); +} + +inline CQuaternion operator*(float left, const CQuaternion &right) +{ + return CQuaternion(left * right.x, left * right.y, left * right.z, left * right.w); +} + +inline CQuaternion operator/(const CQuaternion &left, float right) +{ + return CQuaternion(left.x / right, left.y / right, left.z / right, left.w / right); +} diff --git a/src/math/Rect.cpp b/src/math/Rect.cpp new file mode 100644 index 0000000..de6320a --- /dev/null +++ b/src/math/Rect.cpp @@ -0,0 +1,17 @@ +#include "common.h" + +CRect::CRect(void) +{ + left = 1000000.0f; + top = 1000000.0f; + right = -1000000.0f; + bottom = -1000000.0f; +} + +CRect::CRect(float l, float t, float r, float b) +{ + left = l; + top = t; + right = r; + bottom = b; +} \ No newline at end of file diff --git a/src/math/Rect.h b/src/math/Rect.h new file mode 100644 index 0000000..e9b2589 --- /dev/null +++ b/src/math/Rect.h @@ -0,0 +1,71 @@ +#pragma once + +class CRect +{ +public: + float left; // x min + float bottom; // y max + float right; // x max + float top; // y min + + CRect(void); + CRect(float l, float t, float r, float b); + void ContainPoint(CVector const &v){ + if(v.x < left) left = v.x; + if(v.x > right) right = v.x; + if(v.y < top) top = v.y; + if(v.y > bottom) bottom = v.y; + } + void ContainRect(const CRect &r){ + if(r.left < left) left = r.left; + if(r.right > right) right = r.right; + if(r.top < top) top = r.top; + if(r.bottom > bottom) bottom = r.bottom; + } + + bool IsPointInside(const CVector2D &p){ + return p.x >= left && + p.x <= right && + p.y >= top && + p.y <= bottom; + } + bool IsPointInside(const CVector2D &p, float extraRadius){ + return p.x >= left-extraRadius && + p.x <= right+extraRadius && + p.y >= top-extraRadius && + p.y <= bottom+extraRadius; + } + + void Translate(float x, float y){ + left += x; + right += x; + bottom += y; + top += y; + } + + void Grow(float r) { + left -= r; + right += r; + top -= r; + bottom += r; + } + + void Grow(float l, float r) + { + left -= l; + top -= l; + right += r; + bottom += r; + } + + void Grow(float l, float r, float t, float b) + { + left -= l; + top -= t; + right += r; + bottom += b; + } + + float GetWidth(void) { return right - left; } + float GetHeight(void) { return bottom - top; } +}; diff --git a/src/math/Vector.cpp b/src/math/Vector.cpp new file mode 100644 index 0000000..ee76e55 --- /dev/null +++ b/src/math/Vector.cpp @@ -0,0 +1,46 @@ +#include "common.h" + +void +CVector::Normalise(void) +{ + float sq = MagnitudeSqr(); + if (sq > 0.0f) { + float invsqrt = RecipSqrt(sq); + x *= invsqrt; + y *= invsqrt; + z *= invsqrt; + } else + x = 1.0f; +} + +CVector +CrossProduct(const CVector &v1, const CVector &v2) +{ + return CVector(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x); +} + +CVector +Multiply3x3(const CMatrix &mat, const CVector &vec) +{ + // TODO: VU0 code + return CVector(mat.rx * vec.x + mat.fx * vec.y + mat.ux * vec.z, + mat.ry * vec.x + mat.fy * vec.y + mat.uy * vec.z, + mat.rz * vec.x + mat.fz * vec.y + mat.uz * vec.z); +} + +CVector +Multiply3x3(const CVector &vec, const CMatrix &mat) +{ + return CVector(mat.rx * vec.x + mat.ry * vec.y + mat.rz * vec.z, + mat.fx * vec.x + mat.fy * vec.y + mat.fz * vec.z, + mat.ux * vec.x + mat.uy * vec.y + mat.uz * vec.z); +} + +CVector +operator*(const CMatrix &mat, const CVector &vec) +{ + // TODO: VU0 code + return CVector(mat.rx * vec.x + mat.fx * vec.y + mat.ux * vec.z + mat.px, + mat.ry * vec.x + mat.fy * vec.y + mat.uy * vec.z + mat.py, + mat.rz * vec.x + mat.fz * vec.y + mat.uz * vec.z + mat.pz); +} diff --git a/src/math/Vector.h b/src/math/Vector.h new file mode 100644 index 0000000..0212845 --- /dev/null +++ b/src/math/Vector.h @@ -0,0 +1,129 @@ +#pragma once + +class CVector : public RwV3d +{ +public: + CVector(void) {} + CVector(float x, float y, float z) + { + this->x = x; + this->y = y; + this->z = z; + } + + CVector(const RwV3d &v) + { + x = v.x; + y = v.y; + z = v.z; + } + // (0,1,0) means no rotation. So get right vector and its atan + float Heading(void) const { return Atan2(-x, y); } + float Magnitude(void) const { return Sqrt(x*x + y*y + z*z); } + float MagnitudeSqr(void) const { return x*x + y*y + z*z; } + float Magnitude2D(void) const { return Sqrt(x*x + y*y); } + float MagnitudeSqr2D(void) const { return x*x + y*y; } + void Normalise(void); + + void Normalise2D(void) { + float sq = MagnitudeSqr2D(); + float invsqrt = RecipSqrt(sq); + x *= invsqrt; + y *= invsqrt; + } + + const CVector &operator+=(CVector const &right) { + x += right.x; + y += right.y; + z += right.z; + return *this; + } + + const CVector &operator-=(CVector const &right) { + x -= right.x; + y -= right.y; + z -= right.z; + return *this; + } + + const CVector &operator*=(float right) { + x *= right; + y *= right; + z *= right; + return *this; + } + + const CVector &operator/=(float right) { + x /= right; + y /= right; + z /= right; + return *this; + } + + CVector operator-() const { + return CVector(-x, -y, -z); + } + + const bool operator==(CVector const &right) const { + return x == right.x && y == right.y && z == right.z; + } + + const bool operator!=(CVector const &right) const { + return x != right.x || y != right.y || z != right.z; + } + + bool IsZero(void) const { return x == 0.0f && y == 0.0f && z == 0.0f; } +}; + +inline CVector operator+(const CVector &left, const CVector &right) +{ + return CVector(left.x + right.x, left.y + right.y, left.z + right.z); +} + +inline CVector operator-(const CVector &left, const CVector &right) +{ + return CVector(left.x - right.x, left.y - right.y, left.z - right.z); +} + +inline CVector operator*(const CVector &left, float right) +{ + return CVector(left.x * right, left.y * right, left.z * right); +} + +inline CVector operator*(float left, const CVector &right) +{ + return CVector(left * right.x, left * right.y, left * right.z); +} + +inline CVector operator/(const CVector &left, float right) +{ + return CVector(left.x / right, left.y / right, left.z / right); +} + +inline float +DotProduct(const CVector &v1, const CVector &v2) +{ + return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z; +} + +CVector CrossProduct(const CVector &v1, const CVector &v2); + +inline float +Distance(const CVector &v1, const CVector &v2) +{ + return (v2 - v1).Magnitude(); +} + +inline float +Distance2D(const CVector &v1, const CVector &v2) +{ + float x = v2.x - v1.x; + float y = v2.y - v1.y; + return Sqrt(x*x + y*y); +} + +class CMatrix; + +CVector Multiply3x3(const CMatrix &mat, const CVector &vec); +CVector Multiply3x3(const CVector &vec, const CMatrix &mat); +CVector operator*(const CMatrix &mat, const CVector &vec); \ No newline at end of file diff --git a/src/math/Vector2D.h b/src/math/Vector2D.h new file mode 100644 index 0000000..deabd0b --- /dev/null +++ b/src/math/Vector2D.h @@ -0,0 +1,104 @@ +#pragma once + +class CVector2D +{ +public: + float x, y; + CVector2D(void) {} + CVector2D(float x, float y) : x(x), y(y) {} + CVector2D(const CVector &v) : x(v.x), y(v.y) {} + float Heading(void) const { return Atan2(-x, y); } + float Magnitude(void) const { return Sqrt(x*x + y*y); } + float MagnitudeSqr(void) const { return x*x + y*y; } + + void Normalise(void) { + float sq = MagnitudeSqr(); + if(sq > 0.0f){ + float invsqrt = RecipSqrt(sq); + x *= invsqrt; + y *= invsqrt; + }else + x = 1.0f; + } + + const CVector2D &operator+=(CVector2D const &right) { + x += right.x; + y += right.y; + return *this; + } + + const CVector2D &operator-=(CVector2D const &right) { + x -= right.x; + y -= right.y; + return *this; + } + + const CVector2D &operator*=(float right) { + x *= right; + y *= right; + return *this; + } + + const CVector2D &operator/=(float right) { + x /= right; + y /= right; + return *this; + } + CVector2D operator-(const CVector2D &rhs) const { + return CVector2D(x-rhs.x, y-rhs.y); + } + CVector2D operator+(const CVector2D &rhs) const { + return CVector2D(x+rhs.x, y+rhs.y); + } + CVector2D operator/(float t) const { + return CVector2D(x/t, y/t); + } + CVector2D operator-() const { + return CVector2D(-x, -y); + } +}; + +inline float +DotProduct2D(const CVector2D &v1, const CVector2D &v2) +{ + return v1.x*v2.x + v1.y*v2.y; +} + +inline float +CrossProduct2D(const CVector2D &v1, const CVector2D &v2) +{ + return v1.x*v2.y - v1.y*v2.x; +} + +inline float +Distance2D(const CVector2D &v, float x, float y) +{ + return Sqrt((v.x-x)*(v.x-x) + (v.y-y)*(v.y-y)); +} + +inline float +DistanceSqr2D(const CVector2D &v, float x, float y) +{ + return (v.x-x)*(v.x-x) + (v.y-y)*(v.y-y); +} + +inline void +NormalizeXY(float &x, float &y) +{ + float l = Sqrt(x*x + y*y); + if(l != 0.0f){ + x /= l; + y /= l; + }else + x = 1.0f; +} + +inline CVector2D operator*(const CVector2D &left, float right) +{ + return CVector2D(left.x * right, left.y * right); +} + +inline CVector2D operator*(float left, const CVector2D &right) +{ + return CVector2D(left * right.x, left * right.y); +} diff --git a/src/math/VuVector.h b/src/math/VuVector.h new file mode 100644 index 0000000..30d62cf --- /dev/null +++ b/src/math/VuVector.h @@ -0,0 +1,30 @@ +#pragma once + +class TYPEALIGN(16) CVuVector : public CVector +{ +public: + float w; + CVuVector(void) {} + CVuVector(float x, float y, float z) : CVector(x, y, z) {} + CVuVector(float x, float y, float z, float w) : CVector(x, y, z), w(w) {} + CVuVector(const CVector &v) : CVector(v.x, v.y, v.z) {} + CVuVector(const RwV3d &v) : CVector(v) {} +/* + void Normalise(void) { + float sq = MagnitudeSqr(); + // TODO: VU0 code + if(sq > 0.0f){ + float invsqrt = RecipSqrt(sq); + x *= invsqrt; + y *= invsqrt; + z *= invsqrt; + }else + x = 1.0f; + } +*/ +}; + +void TransformPoint(CVuVector &out, const CMatrix &mat, const CVuVector &in); +void TransformPoint(CVuVector &out, const CMatrix &mat, const RwV3d &in); +void TransformPoints(CVuVector *out, int n, const CMatrix &mat, const RwV3d *in, int stride); +void TransformPoints(CVuVector *out, int n, const CMatrix &mat, const CVuVector *in); diff --git a/src/math/math.cpp b/src/math/math.cpp new file mode 100644 index 0000000..8cb56da --- /dev/null +++ b/src/math/math.cpp @@ -0,0 +1,118 @@ +#include "common.h" + +#include "VuVector.h" + +// TODO: move more stuff into here + + +void TransformPoint(CVuVector &out, const CMatrix &mat, const CVuVector &in) +{ +#ifdef GTA_PS2 + __asm__ __volatile__("\n\ + lqc2 vf01,0x0(%2)\n\ + lqc2 vf02,0x0(%1)\n\ + lqc2 vf03,0x10(%1)\n\ + lqc2 vf04,0x20(%1)\n\ + lqc2 vf05,0x30(%1)\n\ + vmulax.xyz ACC, vf02,vf01\n\ + vmadday.xyz ACC, vf03,vf01\n\ + vmaddaz.xyz ACC, vf04,vf01\n\ + vmaddw.xyz vf06,vf05,vf00\n\ + sqc2 vf06,0x0(%0)\n\ + ": : "r" (&out) , "r" (&mat) ,"r" (&in): "memory"); +#else + out = mat * in; +#endif +} + +void TransformPoint(CVuVector &out, const CMatrix &mat, const RwV3d &in) +{ +#ifdef GTA_PS2 + __asm__ __volatile__("\n\ + ldr $8,0x0(%2)\n\ + ldl $8,0x7(%2)\n\ + lw $9,0x8(%2)\n\ + pcpyld $10,$9,$8\n\ + qmtc2 $10,vf01\n\ + lqc2 vf02,0x0(%1)\n\ + lqc2 vf03,0x10(%1)\n\ + lqc2 vf04,0x20(%1)\n\ + lqc2 vf05,0x30(%1)\n\ + vmulax.xyz ACC, vf02,vf01\n\ + vmadday.xyz ACC, vf03,vf01\n\ + vmaddaz.xyz ACC, vf04,vf01\n\ + vmaddw.xyz vf06,vf05,vf00\n\ + sqc2 vf06,0x0(%0)\n\ + ": : "r" (&out) , "r" (&mat) ,"r" (&in): "memory"); +#else + out = mat * in; +#endif +} + +void TransformPoints(CVuVector *out, int n, const CMatrix &mat, const RwV3d *in, int stride) +{ +#ifdef GTA_PS3 + __asm__ __volatile__("\n\ + paddub $3,%4,$0\n\ + lqc2 vf02,0x0(%2)\n\ + lqc2 vf03,0x10(%2)\n\ + lqc2 vf04,0x20(%2)\n\ + lqc2 vf05,0x30(%2)\n\ + ldr $8,0x0(%3)\n\ + ldl $8,0x7(%3)\n\ + lw $9,0x8(%3)\n\ + pcpyld $10,$9,$8\n\ + qmtc2 $10,vf01\n\ + 1: vmulax.xyz ACC, vf02,vf01\n\ + vmadday.xyz ACC, vf03,vf01\n\ + vmaddaz.xyz ACC, vf04,vf01\n\ + vmaddw.xyz vf06,vf05,vf00\n\ + add %3,%3,$3\n\ + ldr $8,0x0(%3)\n\ + ldl $8,0x7(%3)\n\ + lw $9,0x8(%3)\n\ + pcpyld $10,$9,$8\n\ + qmtc2 $10,vf01\n\ + addi %1,%1,-1\n\ + addiu %0,%0,0x10\n\ + sqc2 vf06,-0x10(%0)\n\ + bnez %1,1b\n\ + ": : "r" (out) , "r" (n), "r" (&mat), "r" (in), "r" (stride): "memory"); +#else + while(n--){ + *out = mat * *in; + in = (RwV3d*)((uint8*)in + stride); + out++; + } +#endif +} + +void TransformPoints(CVuVector *out, int n, const CMatrix &mat, const CVuVector *in) +{ +#ifdef GTA_PS2 + __asm__ __volatile__("\n\ + lqc2 vf02,0x0(%2)\n\ + lqc2 vf03,0x10(%2)\n\ + lqc2 vf04,0x20(%2)\n\ + lqc2 vf05,0x30(%2)\n\ + lqc2 vf01,0x0(%3)\n\ + nop\n\ + 1: vmulax.xyz ACC, vf02,vf01\n\ + vmadday.xyz ACC, vf03,vf01\n\ + vmaddaz.xyz ACC, vf04,vf01\n\ + vmaddw.xyz vf06,vf05,vf00\n\ + lqc2 vf01,0x10(%3)\n\ + addiu %3,%3,0x10\n\ + addi %1,%1,-1\n\ + addiu %0,%0,0x10\n\ + sqc2 vf06,-0x10(%0)\n\ + bnez %1,1b\n\ + ": : "r" (out) , "r" (n), "r" (&mat) ,"r" (in): "memory"); +#else + while(n--){ + *out = mat * *in; + in++; + out++; + } +#endif +} diff --git a/src/math/maths.h b/src/math/maths.h new file mode 100644 index 0000000..8d68bf6 --- /dev/null +++ b/src/math/maths.h @@ -0,0 +1,19 @@ +#pragma once + +// wrapper around float versions of functions +// in gta they are in CMaths but that makes the code rather noisy + +inline float Sin(float x) { return sinf(x); } +inline float Asin(float x) { return asinf(x); } +inline float Cos(float x) { return cosf(x); } +inline float Acos(float x) { return acosf(x); } +inline float Tan(float x) { return tanf(x); } +inline float Atan(float x) { return atanf(x); } +inline float Atan2(float y, float x) { return atan2f(y, x); } +inline float Abs(float x) { return fabs(x); } +inline float Sqrt(float x) { return sqrtf(x); } +inline float RecipSqrt(float x, float y) { return x/Sqrt(y); } +inline float RecipSqrt(float x) { return RecipSqrt(1.0f, x); } +inline float Pow(float x, float y) { return powf(x, y); } +inline float Floor(float x) { return floorf(x); } +inline float Ceil(float x) { return ceilf(x); } -- cgit v1.2.3