diff options
Diffstat (limited to 'src/math/math.cpp')
| -rw-r--r-- | src/math/math.cpp | 139 |
1 files changed, 139 insertions, 0 deletions
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) |