From a8a28c151273e5d1a33707b61304e27762eb6378 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 # Conflicts: # src/control/CarCtrl.cpp # src/math/Matrix.h # src/math/Vector2D.h # src/math/math.cpp # src/render/Skidmarks.cpp --- src/math/Matrix.cpp | 529 ++++++++++++++++++++++++++++++++++++++++++++++++ src/math/Matrix.h | 257 +++-------------------- src/math/Quaternion.cpp | 177 ++++++++++++++++ src/math/Quaternion.h | 18 +- src/math/Rect.cpp | 17 ++ src/math/Rect.h | 14 +- src/math/Vector.cpp | 46 +++++ src/math/Vector.h | 36 +--- src/math/Vector2D.h | 2 +- src/math/math.cpp | 220 -------------------- 10 files changed, 814 insertions(+), 502 deletions(-) create mode 100644 src/math/Matrix.cpp create mode 100644 src/math/Quaternion.cpp create mode 100644 src/math/Rect.cpp create mode 100644 src/math/Vector.cpp (limited to 'src/math') diff --git a/src/math/Matrix.cpp b/src/math/Matrix.cpp new file mode 100644 index 00000000..80e287d3 --- /dev/null +++ b/src/math/Matrix.cpp @@ -0,0 +1,529 @@ +#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 + 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; + + dst_fm[0][0] = scr_fm[0][0]; + dst_fm[0][1] = scr_fm[1][0]; + dst_fm[0][2] = scr_fm[2][0]; + + dst_fm[1][0] = scr_fm[0][1]; + dst_fm[1][1] = scr_fm[1][1]; + dst_fm[1][2] = scr_fm[2][1]; + + dst_fm[2][0] = scr_fm[0][2]; + dst_fm[2][1] = scr_fm[1][2]; + dst_fm[2][2] = scr_fm[2][2]; + + + 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]; + + 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]; + + 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]; + + dst_fm[3][0] = -dst_fm[3][0]; + dst_fm[3][1] = -dst_fm[3][1]; + dst_fm[3][2] = -dst_fm[3][2]; + + 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 diff --git a/src/math/Matrix.h b/src/math/Matrix.h index 73870fe5..bf811af7 100644 --- a/src/math/Matrix.h +++ b/src/math/Matrix.h @@ -7,28 +7,22 @@ public: RwMatrix *m_attachment; bool m_hasRwMatrix; // are we the owner? - CMatrix(void){ - m_attachment = nil; - m_hasRwMatrix = false; - } - CMatrix(CMatrix const &m){ - m_attachment = nil; - m_hasRwMatrix = false; - *this = m; - } - CMatrix(RwMatrix *matrix, bool owner = false){ - m_attachment = nil; - Attach(matrix, owner); - } + 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){ - if(m_hasRwMatrix && m_attachment) - RwMatrixDestroy(m_attachment); - } + ~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); #ifdef RWCORE_H operator RwMatrix (void) const { return m_matrix; @@ -38,59 +32,7 @@ public: return &m_matrix; } #endif - void Attach(RwMatrix *matrix, bool owner = false){ -#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 AttachRW(RwMatrix *matrix, bool owner = false){ - if(m_hasRwMatrix && m_attachment) - RwMatrixDestroy(m_attachment); - m_attachment = matrix; - m_hasRwMatrix = owner; - UpdateRW(); - } - void Detach(void){ - if(m_hasRwMatrix && m_attachment) - RwMatrixDestroy(m_attachment); - m_attachment = nil; - } - void Update(void){ - m_matrix = *m_attachment; - } - void UpdateRW(void){ - if(m_attachment){ - *m_attachment = m_matrix; - RwMatrixUpdate(m_attachment); - } - } - void operator=(CMatrix const &rhs){ - m_matrix = rhs.m_matrix; - if(m_attachment) - UpdateRW(); - } - 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; - } - CMatrix& operator*=(CMatrix const &rhs); + CMatrix &operator*=(CMatrix const &rhs); const CVector &GetPosition(void) const { return *(CVector*)&m_matrix.pos; } CVector& GetPosition(void) { return *(CVector*)&m_matrix.pos; } @@ -98,23 +40,7 @@ public: CVector &GetForward(void) { return *(CVector*)&m_matrix.up; } CVector &GetUp(void) { return *(CVector*)&m_matrix.at; } - void 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 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){ m_matrix.pos.x += x; @@ -123,23 +49,7 @@ public: } void Translate(const CVector &trans){ Translate(trans.x, trans.y, trans.z); } - void 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 SetScale(float s); void Scale(float scale) { float *pFloatMatrix = (float*)&m_matrix; @@ -158,66 +68,9 @@ public: } - void 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 SetRotateX(float angle){ - SetRotateXOnly(angle); - m_matrix.pos.x = 0.0f; - m_matrix.pos.y = 0.0f; - m_matrix.pos.z = 0.0f; - } - void 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 SetRotateY(float angle){ - SetRotateYOnly(angle); - m_matrix.pos.x = 0.0f; - m_matrix.pos.y = 0.0f; - m_matrix.pos.z = 0.0f; - } - void 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 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); @@ -234,12 +87,9 @@ public: m_matrix.at.y = 0.0f; m_matrix.at.z = scale; } - void SetRotateZ(float angle){ - SetRotateZOnly(angle); - m_matrix.pos.x = 0.0f; - m_matrix.pos.y = 0.0f; - m_matrix.pos.z = 0.0f; - } + 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); @@ -247,9 +97,9 @@ public: void RotateZ(float z); void Reorthogonalise(void); - void CopyOnlyMatrix(CMatrix *other){ - m_matrix = other->m_matrix; - } + void CopyOnlyMatrix(CMatrix *other); + void SetUnity(void); + void ResetOrientation(void); void CopyRwMatrix(RwMatrix *matrix){ m_matrix = *matrix; } @@ -259,31 +109,6 @@ public: RwMatrixUpdate(matrix); } - void 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 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 SetTranslateOnly(float x, float y, float z) { m_matrix.pos.x = x; m_matrix.pos.y = y; @@ -292,11 +117,12 @@ public: void SetTranslateOnly(const CVector& pos) { SetTranslateOnly(pos.x, pos.y, pos.z); } + void CheckIntegrity(){} }; CMatrix &Invert(const CMatrix &src, CMatrix &dst); -CVector operator*(const CMatrix &mat, const CVector &vec); +CMatrix Invert(const CMatrix &matrix); CMatrix operator*(const CMatrix &m1, const CMatrix &m2); inline CVector MultiplyInverse(const CMatrix &mat, const CVector &vec) { @@ -307,15 +133,6 @@ inline CVector MultiplyInverse(const CMatrix &mat, const CVector &vec) mat.m_matrix.at.x * v.x + mat.m_matrix.at.y * v.y + mat.m_matrix.at.z * v.z); } -const CVector Multiply3x3(const CMatrix &mat, const CVector &vec); -const CVector Multiply3x3(const CVector &vec, const CMatrix &mat); - -inline CMatrix -Invert(const CMatrix &matrix) -{ - CMatrix inv; - return Invert(matrix, inv); -} class CCompressedMatrixNotAligned @@ -328,28 +145,8 @@ class CCompressedMatrixNotAligned int8 m_upY; int8 m_upZ; public: - void 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 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(); - } + void CompressFromFullMatrix(CMatrix &other); + void DecompressIntoFullMatrix(CMatrix &other); }; class CCompressedMatrix : public CCompressedMatrixNotAligned diff --git a/src/math/Quaternion.cpp b/src/math/Quaternion.cpp new file mode 100644 index 00000000..b0e782e2 --- /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 index dac49362..a5a34626 100644 --- a/src/math/Quaternion.h +++ b/src/math/Quaternion.h @@ -10,18 +10,8 @@ public: 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) { - float sq = MagnitudeSqr(); - if(sq == 0.0f) - w = 1.0f; - else{ - float invsqrt = RecipSqrt(sq); - x *= invsqrt; - y *= invsqrt; - z *= invsqrt; - w *= invsqrt; - } - } + void Normalise(void); + void Multiply(const CQuaternion &q1, const CQuaternion &q2); const CQuaternion &operator+=(CQuaternion const &right) { x += right.x; @@ -60,8 +50,12 @@ public: } 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 diff --git a/src/math/Rect.cpp b/src/math/Rect.cpp new file mode 100644 index 00000000..de6320ad --- /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 index 326bb479..fa8d8de4 100644 --- a/src/math/Rect.h +++ b/src/math/Rect.h @@ -8,18 +8,8 @@ public: float right; // x max float top; // y min - CRect(void){ - left = 1000000.0f; - top = 1000000.0f; - right = -1000000.0f; - bottom = -1000000.0f; - } - CRect(float l, float t, float r, float b){ - left = l; - top = t; - right = r; - bottom = b; - } + 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; diff --git a/src/math/Vector.cpp b/src/math/Vector.cpp new file mode 100644 index 00000000..42e1828e --- /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.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); +} + +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, + 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); +} diff --git a/src/math/Vector.h b/src/math/Vector.h index 5918a5d1..7ee01149 100644 --- a/src/math/Vector.h +++ b/src/math/Vector.h @@ -24,24 +24,7 @@ public: 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) { - float sq = MagnitudeSqr(); - if(sq > 0.0f){ - float invsqrt = RecipSqrt(sq); - x *= invsqrt; - y *= invsqrt; - z *= invsqrt; - }else - x = 1.0f; - } - - void Normalise(float norm) { - float sq = MagnitudeSqr(); - float invsqrt = RecipSqrt(norm, sq); - x *= invsqrt; - y *= invsqrt; - z *= invsqrt; - } + void Normalise(void); void Normalise2D(void) { float sq = MagnitudeSqr2D(); @@ -124,17 +107,16 @@ DotProduct(const CVector &v1, const CVector &v2) return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z; } -inline const 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 CrossProduct(const CVector &v1, const CVector &v2); inline float Distance(const CVector &v1, const CVector &v2) { return (v2 - v1).Magnitude(); -} \ No newline at end of file +} + +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 index 7bfccae6..2c4e57b5 100644 --- a/src/math/Vector2D.h +++ b/src/math/Vector2D.h @@ -18,7 +18,7 @@ public: x *= invsqrt; y *= invsqrt; }else - y = 1.0f; + x = 1.0f; } const CVector2D &operator+=(CVector2D const &right) { diff --git a/src/math/math.cpp b/src/math/math.cpp index d231b147..8cb56dab 100644 --- a/src/math/math.cpp +++ b/src/math/math.cpp @@ -1,6 +1,5 @@ #include "common.h" -#include "Quaternion.h" #include "VuVector.h" // TODO: move more stuff into here @@ -117,222 +116,3 @@ void TransformPoints(CVuVector *out, int n, const CMatrix &mat, const CVuVector } #endif } - - -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::RotateX(float x) -{ - Rotate(x, 0.0f, 0.0f); -} - -void -CMatrix::RotateY(float y) -{ - Rotate(0.0f, y, 0.0f); -} - -void -CMatrix::RotateZ(float z) -{ - Rotate(0.0f, 0.0f, z); -} - -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) -{ - // 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: - 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) -{ - // 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, - 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) -{ - // 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& -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, - mat.m_matrix.right.z * vec.x + mat.m_matrix.up.z * vec.y + mat.m_matrix.at.z * vec.z); -} - -const 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) - *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::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); -} -- cgit v1.2.3