diff options
Diffstat (limited to 'src/math')
| -rw-r--r-- | src/math/Matrix.h | 1 | ||||
| -rw-r--r-- | src/math/VuVector.h | 41 | ||||
| -rw-r--r-- | src/math/math.cpp | 128 |
3 files changed, 170 insertions, 0 deletions
diff --git a/src/math/Matrix.h b/src/math/Matrix.h index da20ea18..73870fe5 100644 --- a/src/math/Matrix.h +++ b/src/math/Matrix.h @@ -90,6 +90,7 @@ public: m_matrix.pos.z += rhs.m_matrix.pos.z; return *this; } + CMatrix& operator*=(CMatrix const &rhs); const CVector &GetPosition(void) const { return *(CVector*)&m_matrix.pos; } CVector& GetPosition(void) { return *(CVector*)&m_matrix.pos; } diff --git a/src/math/VuVector.h b/src/math/VuVector.h new file mode 100644 index 00000000..f90818e0 --- /dev/null +++ b/src/math/VuVector.h @@ -0,0 +1,41 @@ +#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) {} +#ifdef RWCORE_H + CVuVector(const RwV3d &v) : CVector(v.x, v.y, v.z) {} + + operator RwV3d (void) const { + RwV3d vecRw = { this->x, this->y, this->z }; + return vecRw; + } + + operator RwV3d *(void) { + return (RwV3d*)this; + } +#endif +/* + 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 index 0661dcac..d231b147 100644 --- a/src/math/math.cpp +++ b/src/math/math.cpp @@ -1,9 +1,124 @@ #include "common.h" #include "Quaternion.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 +} + + void CMatrix::SetRotate(float xAngle, float yAngle, float zAngle) { @@ -74,6 +189,7 @@ CMatrix::Reorthogonalise(void) 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 // actual copy of librw code: @@ -104,6 +220,7 @@ Invert(const CMatrix &src, CMatrix &dst) CVector operator*(const CMatrix &mat, const CVector &vec) { + // TODO: VU0 code return CVector( mat.m_matrix.right.x * vec.x + mat.m_matrix.up.x * vec.y + mat.m_matrix.at.x * vec.z + mat.m_matrix.pos.x, mat.m_matrix.right.y * vec.x + mat.m_matrix.up.y * vec.y + mat.m_matrix.at.y * vec.z + mat.m_matrix.pos.y, @@ -113,6 +230,7 @@ operator*(const CMatrix &mat, const CVector &vec) CMatrix operator*(const CMatrix &m1, const CMatrix &m2) { + // TODO: VU0 code CMatrix out; RwMatrix *dst = &out.m_matrix; const RwMatrix *src1 = &m1.m_matrix; @@ -132,9 +250,18 @@ operator*(const CMatrix &m1, const CMatrix &m2) return out; } +CMatrix& +CMatrix::operator*=(CMatrix const &rhs) +{ + // TODO: VU0 code + *this = *this * rhs; + return *this; +} + const CVector Multiply3x3(const CMatrix &mat, const CVector &vec) { + // TODO: VU0 code return CVector( mat.m_matrix.right.x * vec.x + mat.m_matrix.up.x * vec.y + mat.m_matrix.at.x * vec.z, mat.m_matrix.right.y * vec.x + mat.m_matrix.up.y * vec.y + mat.m_matrix.at.y * vec.z, @@ -166,6 +293,7 @@ CQuaternion::Slerp(const CQuaternion &q1, const CQuaternion &q2, float theta, fl w1 = Sin((1.0f - t) * theta) * invSin; w2 = Sin(t * theta) * invSin; } + // TODO: VU0 code *this = w1*q1 + w2*q2; } } |