summaryrefslogtreecommitdiffstats
path: root/src/math
diff options
context:
space:
mode:
Diffstat (limited to 'src/math')
-rw-r--r--src/math/Matrix.h148
-rw-r--r--src/math/Vector.h6
-rw-r--r--src/math/Vector2D.h3
-rw-r--r--src/math/math.cpp139
4 files changed, 174 insertions, 122 deletions
diff --git a/src/math/Matrix.h b/src/math/Matrix.h
index 05a6eb03..bfe85afa 100644
--- a/src/math/Matrix.h
+++ b/src/math/Matrix.h
@@ -125,6 +125,24 @@ public:
m_matrix.pos.y = 0.0f;
m_matrix.pos.z = 0.0f;
}
+ void Scale(float scale)
+ {
+ // GTA treats this as 4x4 floats
+ m_matrix.right.x *= scale;
+ m_matrix.right.y *= scale;
+ m_matrix.right.z *= scale;
+ m_matrix.up.x *= scale;
+ m_matrix.up.y *= scale;
+ m_matrix.up.z *= scale;
+ m_matrix.at.x *= scale;
+ m_matrix.at.y *= scale;
+ m_matrix.at.z *= scale;
+ m_matrix.pos.x *= scale;
+ m_matrix.pos.y *= scale;
+ m_matrix.pos.z *= scale;
+ m_matrix.flags = 0;
+ }
+
void SetRotateXOnly(float angle){
float c = Cos(angle);
@@ -192,40 +210,10 @@ public:
m_matrix.pos.y = 0.0f;
m_matrix.pos.z = 0.0f;
}
- void 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;
+ void SetRotate(float xAngle, float yAngle, float zAngle);
+ void Rotate(float x, float y, float z);
- 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 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);
- }
+ void Reorthogonalise(void);
void CopyOnlyMatrix(CMatrix *other){
m_matrix = other->m_matrix;
}
@@ -245,35 +233,13 @@ public:
}
};
-inline CMatrix&
-Invert(const CMatrix &src, CMatrix &dst)
-{
- // GTA handles this as a raw 4x4 orthonormal matrix
- // and trashes the RW flags, let's not do that
- // actual copy of librw code:
- RwMatrix *d = &dst.m_matrix;
- const RwMatrix *s = &src.m_matrix;
- d->right.x = s->right.x;
- d->right.y = s->up.x;
- d->right.z = s->at.x;
- d->up.x = s->right.y;
- d->up.y = s->up.y;
- d->up.z = s->at.y;
- d->at.x = s->right.z;
- d->at.y = s->up.z;
- d->at.z = s->at.z;
- d->pos.x = -(s->pos.x*s->right.x +
- s->pos.y*s->right.y +
- s->pos.z*s->right.z);
- d->pos.y = -(s->pos.x*s->up.x +
- s->pos.y*s->up.y +
- s->pos.z*s->up.z);
- d->pos.z = -(s->pos.x*s->at.x +
- s->pos.y*s->at.y +
- s->pos.z*s->at.z);
- d->flags = rwMATRIXTYPEORTHONORMAL;
- return dst;
-}
+
+CMatrix &Invert(const CMatrix &src, CMatrix &dst);
+CVector operator*(const CMatrix &mat, const CVector &vec);
+CMatrix operator*(const CMatrix &m1, const CMatrix &m2);
+CVector MultiplyInverse(const CMatrix &mat, const CVector &vec);
+CVector Multiply3x3(const CMatrix &mat, const CVector &vec);
+CVector Multiply3x3(const CVector &vec, const CMatrix &mat);
inline CMatrix
Invert(const CMatrix &matrix)
@@ -282,64 +248,6 @@ Invert(const CMatrix &matrix)
return Invert(matrix, inv);
}
-inline CVector
-operator*(const CMatrix &mat, const CVector &vec)
-{
- 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,
- mat.m_matrix.right.z * vec.x + mat.m_matrix.up.z * vec.y + mat.m_matrix.at.z * vec.z + mat.m_matrix.pos.z);
-}
-
-inline CMatrix
-operator*(const CMatrix &m1, const CMatrix &m2)
-{
- 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;
-}
-
-inline CVector
-MultiplyInverse(const CMatrix &mat, const CVector &vec)
-{
- CVector v(vec.x - mat.m_matrix.pos.x, vec.y - mat.m_matrix.pos.y, vec.z - mat.m_matrix.pos.z);
- return CVector(
- mat.m_matrix.right.x * v.x + mat.m_matrix.right.y * v.y + mat.m_matrix.right.z * v.z,
- mat.m_matrix.up.x * v.x + mat.m_matrix.up.y * v.y + mat.m_matrix.up.z * v.z,
- mat.m_matrix.at.x * v.x + mat.m_matrix.at.y * v.y + mat.m_matrix.at.z * v.z);
-}
-
-inline CVector
-Multiply3x3(const CMatrix &mat, const CVector &vec)
-{
- 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,
- mat.m_matrix.right.z * vec.x + mat.m_matrix.up.z * vec.y + mat.m_matrix.at.z * vec.z);
-}
-
-inline CVector
-Multiply3x3(const CVector &vec, const CMatrix &mat)
-{
- return CVector(
- mat.m_matrix.right.x * vec.x + mat.m_matrix.right.y * vec.y + mat.m_matrix.right.z * vec.z,
- mat.m_matrix.up.x * vec.x + mat.m_matrix.up.y * vec.y + mat.m_matrix.up.z * vec.z,
- mat.m_matrix.at.x * vec.x + mat.m_matrix.at.y * vec.y + mat.m_matrix.at.z * vec.z);
-}
class CCompressedMatrixNotAligned
{
diff --git a/src/math/Vector.h b/src/math/Vector.h
index f794a57f..42087339 100644
--- a/src/math/Vector.h
+++ b/src/math/Vector.h
@@ -31,7 +31,7 @@ public:
void Normalise(void) {
float sq = MagnitudeSqr();
if(sq > 0.0f){
- float invsqrt = 1.0f/Sqrt(sq); // CMaths::RecipSqrt
+ float invsqrt = RecipSqrt(sq);
x *= invsqrt;
y *= invsqrt;
z *= invsqrt;
@@ -71,6 +71,10 @@ public:
return CVector(-x, -y, -z);
}
+ const bool operator==(CVector const &right) {
+ return x == right.x && y == right.y && z == right.z;
+ }
+
bool IsZero(void) { return x == 0.0f && y == 0.0f && z == 0.0f; }
};
diff --git a/src/math/Vector2D.h b/src/math/Vector2D.h
index e6b04c14..c8835ec0 100644
--- a/src/math/Vector2D.h
+++ b/src/math/Vector2D.h
@@ -7,13 +7,14 @@ public:
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 = 1.0f/Sqrt(sq);
+ float invsqrt = RecipSqrt(sq);
x *= invsqrt;
y *= invsqrt;
}else
diff --git a/src/math/math.cpp b/src/math/math.cpp
index c1199fcc..e8b7d933 100644
--- a/src/math/math.cpp
+++ b/src/math/math.cpp
@@ -5,6 +5,145 @@
// TODO: move more stuff into here
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::Rotate(float x, float y, float z)
+{
+ // TODO? do this directly without creating another matrix
+ CMatrix rot;
+ rot.SetRotate(x, y, z);
+ *this = rot * *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&
+Invert(const CMatrix &src, CMatrix &dst)
+{
+ // GTA handles this as a raw 4x4 orthonormal matrix
+ // and trashes the RW flags, let's not do that
+ // actual copy of librw code:
+ RwMatrix *d = &dst.m_matrix;
+ const RwMatrix *s = &src.m_matrix;
+ d->right.x = s->right.x;
+ d->right.y = s->up.x;
+ d->right.z = s->at.x;
+ d->up.x = s->right.y;
+ d->up.y = s->up.y;
+ d->up.z = s->at.y;
+ d->at.x = s->right.z;
+ d->at.y = s->up.z;
+ d->at.z = s->at.z;
+ d->pos.x = -(s->pos.x*s->right.x +
+ s->pos.y*s->right.y +
+ s->pos.z*s->right.z);
+ d->pos.y = -(s->pos.x*s->up.x +
+ s->pos.y*s->up.y +
+ s->pos.z*s->up.z);
+ d->pos.z = -(s->pos.x*s->at.x +
+ s->pos.y*s->at.y +
+ s->pos.z*s->at.z);
+ d->flags = rwMATRIXTYPEORTHONORMAL;
+ return dst;
+}
+
+CVector
+operator*(const CMatrix &mat, const CVector &vec)
+{
+ 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,
+ mat.m_matrix.right.z * vec.x + mat.m_matrix.up.z * vec.y + mat.m_matrix.at.z * vec.z + mat.m_matrix.pos.z);
+}
+
+CMatrix
+operator*(const CMatrix &m1, const CMatrix &m2)
+{
+ 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;
+}
+
+CVector
+MultiplyInverse(const CMatrix &mat, const CVector &vec)
+{
+ CVector v(vec.x - mat.m_matrix.pos.x, vec.y - mat.m_matrix.pos.y, vec.z - mat.m_matrix.pos.z);
+ return CVector(
+ mat.m_matrix.right.x * v.x + mat.m_matrix.right.y * v.y + mat.m_matrix.right.z * v.z,
+ mat.m_matrix.up.x * v.x + mat.m_matrix.up.y * v.y + mat.m_matrix.up.z * v.z,
+ mat.m_matrix.at.x * v.x + mat.m_matrix.at.y * v.y + mat.m_matrix.at.z * v.z);
+}
+
+CVector
+Multiply3x3(const CMatrix &mat, const CVector &vec)
+{
+ 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,
+ mat.m_matrix.right.z * vec.x + mat.m_matrix.up.z * vec.y + mat.m_matrix.at.z * vec.z);
+}
+
+CVector
+Multiply3x3(const CVector &vec, const CMatrix &mat)
+{
+ return CVector(
+ mat.m_matrix.right.x * vec.x + mat.m_matrix.right.y * vec.y + mat.m_matrix.right.z * vec.z,
+ mat.m_matrix.up.x * vec.x + mat.m_matrix.up.y * vec.y + mat.m_matrix.up.z * vec.z,
+ mat.m_matrix.at.x * vec.x + mat.m_matrix.at.y * vec.y + mat.m_matrix.at.z * vec.z);
+}
+
+
+void
CQuaternion::Slerp(const CQuaternion &q1, const CQuaternion &q2, float theta, float invSin, float t)
{
if(theta == 0.0f)
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-24 21:58:44 +0100