ps2-ified math

This commit is contained in:
aap committed 2021-09-07 15:10:01 +02:00
1 parent c01afa98c3
commit 9076bede61
10 files changed
+297 -69

No files matched your search

+2 -2
View File
@@ -93,9 +93,9 @@ strcmpIgnoringDigits(const char *s1, const char *s2)
if(c2) s2++;
if(c1 == '\0' && c2 == '\0') return true;
#ifndef ASCII_STRCMP
if(iswdigit(c1) && iswdigit(c2))
if(isdigit(c1) && isdigit(c2))
#else
if(__ascii_iswdigit(c1) && __ascii_iswdigit(c2))
if(__ascii_isdigit(c1) && __ascii_isdigit(c2))
#endif
continue;
#ifndef ASCII_STRCMP
+4 -4
View File
@@ -38,7 +38,7 @@ CAnimBlendNode::Update(CVector &trans, CQuaternion &rot, float weight)
}
if(sequence->type & CAnimBlendSequence::KF_ROT){
rot.Slerp(kfB->rotation, kfA->rotation, theta, invSin, t);
rot *= blend;
rot.Scale(blend);
}
}
@@ -76,7 +76,7 @@ CAnimBlendNode::UpdateCompressed(CVector &trans, CQuaternion &rot, float weight)
kfA->GetRotation(&rotA);
kfB->GetRotation(&rotB);
rot.Slerp(rotB, rotA, theta, invSin, t);
rot *= blend;
rot.Scale(blend);
}
}
@@ -237,8 +237,8 @@ CAnimBlendNode::CalcDeltasCompressed(void)
kfB->GetRotation(&rotB);
float cos = DotProduct(rotA, rotB);
if(cos < 0.0f){
rotB *= -1.0f;
kfB->SetRotation(rotB);
rotB = -rotB;
kfB->SetRotation(-rotB);
}
cos = DotProduct(rotA, rotB);
if(cos > 1.0f)
+103 -5
View File
@@ -78,12 +78,13 @@ CMatrix::UpdateRW(void)
}
}
void
CMatrix&
CMatrix::operator=(CMatrix const &rhs)
{
memcpy(this, &rhs, sizeof(f));
if (m_attachment)
UpdateRW();
return *this;
}
void
@@ -411,8 +412,40 @@ CMatrix::Rotate(float x, float y, float z)
CMatrix &
CMatrix::operator*=(CMatrix const &rhs)
{
// TODO: VU0 code
#ifdef GTA_PS2
// TODO(PS2) don't trash fields
__asm__ __volatile__("\n\
.set noreorder\n\
lqc2 vf01,0x00(%0)\n\
lqc2 vf02,0x10(%0)\n\
lqc2 vf03,0x20(%0)\n\
lqc2 vf04,0x30(%0)\n\
lqc2 vf05,0x00(%1)\n\
lqc2 vf06,0x10(%1)\n\
lqc2 vf07,0x20(%1)\n\
lqc2 vf08,0x30(%1)\n\
vmulax.xyz ACC,vf01,vf05\n\
vmadday.xyz ACC,vf02,vf05\n\
vmaddz.xyz vf05,vf03,vf05\n\
vmulax.xyz ACC,vf01,vf06\n\
vmadday.xyz ACC,vf02,vf06\n\
vmaddz.xyz vf06,vf03,vf06\n\
vmulax.xyz ACC,vf01,vf07\n\
vmadday.xyz ACC,vf02,vf07\n\
vmaddz.xyz vf07,vf03,vf07\n\
vmulax.xyz ACC,vf01,vf08\n\
vmadday.xyz ACC,vf02,vf08\n\
vmaddaz.xyz ACC,vf03,vf08\n\
vmaddw.xyz vf08,vf04,vf00\n\
sqc2 vf05,0x00(%0)\n\
sqc2 vf06,0x10(%0)\n\
sqc2 vf07,0x20(%0)\n\
sqc2 vf08,0x30(%0)\n\
.set reorder\n\
": : "r" (this) , "r" (&rhs) : "memory");
#else
*this = *this * rhs;
#endif
return *this;
}
@@ -432,8 +465,39 @@ CMatrix::Reorthogonalise(void)
CMatrix
operator*(const CMatrix &m1, const CMatrix &m2)
{
// TODO: VU0 code
CMatrix out;
#ifdef GTA_PS2
// TODO(PS2) don't trash fields
__asm__ __volatile__("\n\
.set noreorder\n\
lqc2 vf01,0x00(%1)\n\
lqc2 vf02,0x10(%1)\n\
lqc2 vf03,0x20(%1)\n\
lqc2 vf04,0x30(%1)\n\
lqc2 vf05,0x00(%2)\n\
lqc2 vf06,0x10(%2)\n\
lqc2 vf07,0x20(%2)\n\
lqc2 vf08,0x30(%2)\n\
vmulax.xyz ACC,vf01,vf05\n\
vmadday.xyz ACC,vf02,vf05\n\
vmaddz.xyz vf05,vf03,vf05\n\
vmulax.xyz ACC,vf01,vf06\n\
vmadday.xyz ACC,vf02,vf06\n\
vmaddz.xyz vf06,vf03,vf06\n\
vmulax.xyz ACC,vf01,vf07\n\
vmadday.xyz ACC,vf02,vf07\n\
vmaddz.xyz vf07,vf03,vf07\n\
vmulax.xyz ACC,vf01,vf08\n\
vmadday.xyz ACC,vf02,vf08\n\
vmaddaz.xyz ACC,vf03,vf08\n\
vmaddw.xyz vf08,vf04,vf00\n\
sqc2 vf05,0x00(%0)\n\
sqc2 vf06,0x10(%0)\n\
sqc2 vf07,0x20(%0)\n\
sqc2 vf08,0x30(%0)\n\
.set reorder\n\
": : "r" (&out), "r" (&m1) , "r" (&m2) : "memory");
#else
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;
@@ -446,13 +510,47 @@ operator*(const CMatrix &m1, const CMatrix &m2)
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;
#endif
return out;
}
CMatrix &
Invert(const CMatrix &src, CMatrix &dst)
{
// TODO: VU0 code
#ifdef GTA_PS2
// TODO(PS2) don't trash fields
__asm__ __volatile__("\n\
.set noreorder\n\
lq $8,0x00(%0)\n\
lq $9,0x10(%0)\n\
lq $10,0x20(%0)\n\
lqc2 vf04,0x30(%0)\n\
vmove vf05,vf04\n\
vsub.xyz vf04,vf04,vf04\n\
vmove vf09,vf04\n\
qmfc2 $11,vf04\n\
pextlw $12,$9,$8\n\
pextuw $13,$9,$8\n\
pextlw $14,$11,$10\n\
pextuw $15,$11,$10\n\
pcpyld $8,$14,$12\n\
pcpyud $9,$12,$14\n\
pcpyld $10,$15,$13\n\
qmtc2 $8,vf06\n\
qmtc2 $9,vf07\n\
qmtc2 $10,vf08\n\
vmulax.xyz ACC,vf06,vf05\n\
vmadday.xyz ACC,vf07,vf05\n\
vmaddz.xyz vf04,vf08,vf05\n\
vsub.xyz vf04,vf09,vf04\n\
sq $8,0x00(%1)\n\
sq $9,0x10(%1)\n\
sq $10,0x20(%1)\n\
sqc2 vf04,0x30(%1)\n\
.set reorder\n\
": : "r" (&src) , "r" (&dst)
: "$8", "$9", "$10", "$11", "$12", "$13", "$14", "$15", "memory");
#else
dst.f[3][0] = dst.f[3][1] = dst.f[3][2] = 0.0f;
dst.f[0][0] = src.f[0][0];
@@ -483,7 +581,7 @@ Invert(const CMatrix &src, CMatrix &dst)
dst.f[3][0] = -dst.f[3][0];
dst.f[3][1] = -dst.f[3][1];
dst.f[3][2] = -dst.f[3][2];
#endif
return dst;
}
+6 -2
View File
@@ -1,10 +1,14 @@
#ifndef __GTA_MATRIX_H__
#define __GTA_MATRIX_H__
#ifdef GTA_PS2
class TYPEALIGN(16) CMatrix
#else
class CMatrix
#endif
{
public:
#ifdef GTA_PS2
#ifdef GTA_PS2_notyet // disabled right now because of VU0 code from III
union
{
float f[4][4];
@@ -49,7 +53,7 @@ public:
void Detach(void);
void Update(void);
void UpdateRW(void);
void operator=(CMatrix const &rhs);
CMatrix &operator=(CMatrix const &rhs);
CMatrix &operator+=(CMatrix const &rhs);
CMatrix &operator*=(CMatrix const &rhs);
+129 -10
View File
@@ -1,26 +1,133 @@
#include "common.h"
#include "Quaternion.h"
CQuaternion&
CQuaternion::operator=(CQuaternion const &rhs)
{
#ifdef GTA_PS2
__asm__ __volatile__("\n\
lq $8,(%1)\n\
sq $8,(%0)\n\
": : "r" (this), "r" (&rhs)
: "$8", "memory");
#else
x = rhs.x;
y = rhs.y;
z = rhs.z;
w = rhs.w;
#endif
return *this;
}
const CQuaternion &
CQuaternion::operator+=(CQuaternion const &right)
{
#ifdef GTA_PS2
__asm__ __volatile__("\n\
lqc2 vf01,(%0)\n\
lqc2 vf02,(%1)\n\
vadd vf01,vf01,vf02\n\
sqc2 vf01,(%0)\n\
": : "r" (this), "r" (&right)
: "memory");
#else
x += right.x;
y += right.y;
z += right.z;
w += right.w;
#endif
return *this;
}
const CQuaternion &
CQuaternion::operator-=(CQuaternion const &right)
{
#ifdef GTA_PS2
__asm__ __volatile__("\n\
lqc2 vf01,(%0)\n\
lqc2 vf02,(%1)\n\
vsub vf01,vf01,vf02\n\
sqc2 vf01,(%0)\n\
": : "r" (this), "r" (&right)
: "memory");
#else
x -= right.x;
y -= right.y;
z -= right.z;
w -= right.w;
#endif
return *this;
}
void
CQuaternion::Scale(float s)
{
#ifdef GTA_PS2
__asm__ __volatile__("\n\
lqc2 vf01,(%0)\n\
mfc1 $8,%1\n\
qmtc2 $8,vf02\n\
vmulx vf01,vf01,vf02\n\
sqc2 vf01,(%0)\n\
": : "r" (this), "f" (s)
: "$8", "memory");
#else
x *= s;
y *= s;
z *= s;
w *= s;
#endif
}
void
CQuaternion::Copy(const CQuaternion &q)
{
#ifdef GTA_PS2
__asm__ __volatile__("\n\
lq $8,(%1)\n\
sq $8,(%0)\n\
": : "r" (this), "r" (&q)
: "$8", "memory");
#else
x = q.x;
y = q.y;
z = q.z;
w = q.w;
#endif
}
CQuaternion&
CQuaternion::Invert(void){
#ifdef GTA_PS2
__asm__ __volatile__("\n\
lqc2 vf01,(%0)\n\
vsub.xyz vf01,vf00,vf01\n\
sqc2 vf01,(%0)\n\
": : "r" (this)
: "memory");
#else
x = -x;
y = -y;
z = -z;
#endif
return *this;
}
void
CQuaternion::Normalise(void)
{
float sq = MagnitudeSqr();
float sq = GetLengthSquared();
if (sq == 0.0f)
w = 1.0f;
else {
float invsqrt = RecipSqrt(sq);
x *= invsqrt;
y *= invsqrt;
z *= invsqrt;
w *= invsqrt;
}
else
Scale(RecipSqrt(sq));
}
void
CQuaternion::Slerp(const CQuaternion &q1, const CQuaternion &q2, float theta, float invSin, float t)
{
if (theta == 0.0f)
*this = q2;
Copy(q2);
else {
float w1, w2;
if (theta > PI / 2) {
@@ -31,8 +138,20 @@ 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
#ifdef GTA_PS2
__asm__ __volatile__("\n\
lqc2 vf01,(%1)\n\
lqc2 vf02,(%2)\n\
qmtc2 %4,vf03\n\
qmtc2 %3,vf04\n\
vmulax ACC,vf01,vf04\n\
vmaddx vf01,vf02,vf03\n\
sqc2 vf01,(%0)\n\
": : "r" (this), "r" (&q1), "r" (&q2), "r" (w1), "r" (w2)
: "memory");
#else
*this = w1 * q1 + w2 * q2;
#endif
}
}
+12 -38
View File
@@ -1,55 +1,29 @@
#ifndef __GTA_QUATERNION_H__
#define __GTA_QUATERNION_H__
// TODO: actually implement this
#ifdef GTA_PS2
class TYPEALIGN(16) CQuaternion
#else
class CQuaternion
#endif
{
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; }
// float Magnitude(void) const { return Sqrt(x*x + y*y + z*z + w*w); }
float GetLengthSquared(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;
}
CQuaternion &Invert(void);
void Scale(float s);
void Copy(const CQuaternion &q);
const CQuaternion &operator+=(CQuaternion const &right) {
x += right.x;
y += right.y;
z += right.z;
w += right.w;
return *this;
}
CQuaternion &operator=(CQuaternion const &rhs);
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;
}
const CQuaternion &operator+=(CQuaternion const &right);
const CQuaternion &operator-=(CQuaternion const &right);
CQuaternion operator-() const {
return CQuaternion(-x, -y, -z, -w);
+2 -2
View File
@@ -20,9 +20,9 @@ public:
}
// (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 Magnitude(void) const { return Sqrt(MagnitudeSqr()); }
float MagnitudeSqr(void) const { return x*x + y*y + z*z; }
float Magnitude2D(void) const { return Sqrt(x*x + y*y); }
float Magnitude2D(void) const { return Sqrt(MagnitudeSqr2D()); }
float MagnitudeSqr2D(void) const { return x*x + y*y; }
void Normalise(void);
+24 -6
View File
@@ -1,9 +1,24 @@
#include "common.h"
#include "VuVector.h"
// TODO: move more stuff into here
float
CVuVector::MagnitudeSqr(void) const
{
#ifdef GTA_PS2
float ret;
__asm__ __volatile__("\n\
lwc1 $f2,0(%1)\n\
lwc1 $f3,0(%1)\n\
lwc1 $f4,0(%1)\n\
mula.s $f2,$f2\n\
madda.s $f3,$f3\n\
madd.s %0,$f4,$f4\n\
": "=f"(ret) : "r" (this));
return ret;
#else
return x*x + y*y + z*z;
#endif
}
void TransformPoint(CVuVector &out, const CMatrix &mat, const CVuVector &in)
{
@@ -43,7 +58,8 @@ void TransformPoint(CVuVector &out, const CMatrix &mat, const RwV3d &in)
vmaddaz.xyz ACC, vf04,vf01\n\
vmaddw.xyz vf06,vf05,vf00\n\
sqc2 vf06,0x0(%0)\n\
": : "r" (&out) , "r" (&mat) ,"r" (&in): "memory");
": : "r" (&out) , "r" (&mat) ,"r" (&in)
: "$8", "$9", "$10", "memory");
#else
out = mat * in;
#endif
@@ -51,7 +67,8 @@ void TransformPoint(CVuVector &out, const CMatrix &mat, const RwV3d &in)
void TransformPoints(CVuVector *out, int n, const CMatrix &mat, const RwV3d *in, int stride)
{
#ifdef GTA_PS3
#ifdef GTA_PS2
if(n == 0) return; // TODO(PS2): this shouldn't happen
__asm__ __volatile__("\n\
paddub $3,%4,$0\n\
lqc2 vf02,0x0(%2)\n\
@@ -77,7 +94,8 @@ void TransformPoints(CVuVector *out, int n, const CMatrix &mat, const RwV3d *in,
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");
": : "r" (out) , "r" (n), "r" (&mat), "r" (in), "r" (stride)
: "$3", "$8", "$9", "$10", "memory");
#else
while(n--){
*out = mat * *in;
+2
View File
@@ -10,6 +10,8 @@ public:
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) {}
float MagnitudeSqr(void) const;
/*
void Normalise(void) {
float sq = MagnitudeSqr();
+13
View File
@@ -13,7 +13,20 @@ 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 fabsf(x); }
inline float Sqrt(float x) { return sqrtf(x); }
#ifdef GTA_PS2
inline float RecipSqrt(float x, float y)
{
float ret;
__asm__ __volatile__("\n\
rsqrt.s %0,%1,%2\n\
" : "=f" (ret)
: "f" (x), "f" (y)
);
return ret;
}
#else
inline float RecipSqrt(float x, float y) { return x/Sqrt(y); }
#endif
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); }