Move a bunch of math to cpp files + small fixes

This commit is contained in:
Sergeanur 2020-09-14 20:48:49 +03:00
parent 93d77f340d
commit 38ec1bd50d
14 changed files with 857 additions and 529 deletions

View File

@ -1080,11 +1080,11 @@ void CCarCtrl::SlowCarDownForCarsSectorList(CPtrList& lst, CVehicle* pVehicle, f
void CCarCtrl::SlowCarDownForOtherCar(CEntity* pOtherEntity, CVehicle* pVehicle, float* pSpeed, float curSpeed) void CCarCtrl::SlowCarDownForOtherCar(CEntity* pOtherEntity, CVehicle* pVehicle, float* pSpeed, float curSpeed)
{ {
CVector forwardA = pVehicle->GetForward(); CVector forwardA = pVehicle->GetForward();
((CVector2D)forwardA).Normalise(); ((CVector2D)forwardA).NormaliseSafe();
if (DotProduct2D(pOtherEntity->GetPosition() - pVehicle->GetPosition(), forwardA) < 0.0f) if (DotProduct2D(pOtherEntity->GetPosition() - pVehicle->GetPosition(), forwardA) < 0.0f)
return; return;
CVector forwardB = pOtherEntity->GetForward(); CVector forwardB = pOtherEntity->GetForward();
((CVector2D)forwardB).Normalise(); ((CVector2D)forwardB).NormaliseSafe();
forwardA.z = forwardB.z = 0.0f; forwardA.z = forwardB.z = 0.0f;
CVehicle* pOtherVehicle = (CVehicle*)pOtherEntity; CVehicle* pOtherVehicle = (CVehicle*)pOtherEntity;
/* why is the argument CEntity if it's always CVehicle anyway and is casted? */ /* why is the argument CEntity if it's always CVehicle anyway and is casted? */
@ -1337,7 +1337,7 @@ void CCarCtrl::WeaveForOtherCar(CEntity* pOtherEntity, CVehicle* pVehicle, float
pVehicle->GetModelInfo()->GetColModel()->boundingSphere.radius < distance) pVehicle->GetModelInfo()->GetColModel()->boundingSphere.radius < distance)
return; return;
CVector2D forward = pVehicle->GetForward(); CVector2D forward = pVehicle->GetForward();
forward.Normalise(); forward.NormaliseSafe();
float forwardAngle = CGeneral::GetATanOfXY(forward.x, forward.y); float forwardAngle = CGeneral::GetATanOfXY(forward.x, forward.y);
float angleDiff = angleBetweenVehicles - forwardAngle; float angleDiff = angleBetweenVehicles - forwardAngle;
float lenProjection = ABS(pOtherCar->GetColModel()->boundingBox.max.y * sin(angleDiff)); float lenProjection = ABS(pOtherCar->GetColModel()->boundingBox.max.y * sin(angleDiff));
@ -2276,7 +2276,7 @@ float CCarCtrl::FindMaxSteerAngle(CVehicle* pVehicle)
void CCarCtrl::SteerAICarWithPhysicsFollowPath(CVehicle* pVehicle, float* pSwerve, float* pAccel, float* pBrake, bool* pHandbrake) void CCarCtrl::SteerAICarWithPhysicsFollowPath(CVehicle* pVehicle, float* pSwerve, float* pAccel, float* pBrake, bool* pHandbrake)
{ {
CVector2D forward = pVehicle->GetForward(); CVector2D forward = pVehicle->GetForward();
forward.Normalise(); forward.NormaliseSafe();
CCarPathLink* pCurrentLink = &ThePaths.m_carPathLinks[pVehicle->AutoPilot.m_nCurrentPathNodeInfo]; CCarPathLink* pCurrentLink = &ThePaths.m_carPathLinks[pVehicle->AutoPilot.m_nCurrentPathNodeInfo];
CCarPathLink* pNextLink = &ThePaths.m_carPathLinks[pVehicle->AutoPilot.m_nNextPathNodeInfo]; CCarPathLink* pNextLink = &ThePaths.m_carPathLinks[pVehicle->AutoPilot.m_nNextPathNodeInfo];
CVector2D currentPathLinkForward(pCurrentLink->GetDirX() * pVehicle->AutoPilot.m_nCurrentDirection, CVector2D currentPathLinkForward(pCurrentLink->GetDirX() * pVehicle->AutoPilot.m_nCurrentDirection,
@ -2410,7 +2410,7 @@ void CCarCtrl::SteerAICarWithPhysicsHeadingForTarget(CVehicle* pVehicle, CPhysic
{ {
*pHandbrake = false; *pHandbrake = false;
CVector2D forward = pVehicle->GetForward(); CVector2D forward = pVehicle->GetForward();
forward.Normalise(); forward.NormaliseSafe();
float angleToTarget = CGeneral::GetATanOfXY(targetX - pVehicle->GetPosition().x, targetY - pVehicle->GetPosition().y); float angleToTarget = CGeneral::GetATanOfXY(targetX - pVehicle->GetPosition().x, targetY - pVehicle->GetPosition().y);
float angleForward = CGeneral::GetATanOfXY(forward.x, forward.y); float angleForward = CGeneral::GetATanOfXY(forward.x, forward.y);
if (pVehicle->AutoPilot.m_nDrivingStyle == DRIVINGSTYLE_AVOID_CARS) if (pVehicle->AutoPilot.m_nDrivingStyle == DRIVINGSTYLE_AVOID_CARS)
@ -2497,7 +2497,7 @@ void CCarCtrl::SteerAICarWithPhysicsTryingToBlockTarget_Stop(CVehicle* pVehicle,
void CCarCtrl::SteerAIBoatWithPhysicsHeadingForTarget(CBoat* pBoat, float targetX, float targetY, float* pSwerve, float* pAccel, float* pBrake) void CCarCtrl::SteerAIBoatWithPhysicsHeadingForTarget(CBoat* pBoat, float targetX, float targetY, float* pSwerve, float* pAccel, float* pBrake)
{ {
CVector2D forward(pBoat->GetForward()); CVector2D forward(pBoat->GetForward());
forward.Normalise(); forward.NormaliseSafe();
CVector2D distanceToTarget = CVector2D(targetX, targetY) - pBoat->GetPosition(); CVector2D distanceToTarget = CVector2D(targetX, targetY) - pBoat->GetPosition();
float angleToTarget = CGeneral::GetATanOfXY(distanceToTarget.x, distanceToTarget.y); float angleToTarget = CGeneral::GetATanOfXY(distanceToTarget.x, distanceToTarget.y);
float angleForward = CGeneral::GetATanOfXY(forward.x, forward.y); float angleForward = CGeneral::GetATanOfXY(forward.x, forward.y);
@ -2733,7 +2733,7 @@ bool CCarCtrl::GenerateOneEmergencyServicesCar(uint32 mi, CVector vecPos)
pVehicle->AutoPilot.m_nTempAction = TEMPACT_NONE; pVehicle->AutoPilot.m_nTempAction = TEMPACT_NONE;
pVehicle->AutoPilot.m_nDrivingStyle = DRIVINGSTYLE_AVOID_CARS; pVehicle->AutoPilot.m_nDrivingStyle = DRIVINGSTYLE_AVOID_CARS;
CVector2D direction = vecPos - spawnPos; CVector2D direction = vecPos - spawnPos;
direction.Normalise(); direction.NormaliseSafe();
pVehicle->GetForward() = CVector(direction.x, direction.y, 0.0f); pVehicle->GetForward() = CVector(direction.x, direction.y, 0.0f);
pVehicle->GetRight() = CVector(direction.y, -direction.x, 0.0f); pVehicle->GetRight() = CVector(direction.y, -direction.x, 0.0f);
pVehicle->GetUp() = CVector(0.0f, 0.0f, 1.0f); pVehicle->GetUp() = CVector(0.0f, 0.0f, 1.0f);

552
src/math/Matrix.cpp Normal file
View File

@ -0,0 +1,552 @@
#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
// GTA handles this as a raw 4x4 orthonormal matrix
// and trashes the RW flags, let's not do that
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;
#ifndef FIX_BUGS
dst_fm[3][3] = scr_fm[3][3];
#endif
dst_fm[0][0] = scr_fm[0][0];
dst_fm[0][1] = scr_fm[1][0];
dst_fm[0][2] = scr_fm[2][0];
#ifndef FIX_BUGS
dst_fm[0][3] = scr_fm[3][0];
#endif
dst_fm[1][0] = scr_fm[0][1];
dst_fm[1][1] = scr_fm[1][1];
dst_fm[1][2] = scr_fm[2][1];
#ifndef FIX_BUGS
dst_fm[1][3] = scr_fm[3][1];
#endif
dst_fm[2][0] = scr_fm[0][2];
dst_fm[2][1] = scr_fm[1][2];
dst_fm[2][2] = scr_fm[2][2];
#ifndef FIX_BUGS
dst_fm[2][3] = scr_fm[3][2];
#endif
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];
#ifndef FIX_BUGS
dst_fm[3][3] += dst_fm[0][3] * scr_fm[3][0];
#endif
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];
#ifndef FIX_BUGS
dst_fm[3][3] += dst_fm[1][3] * scr_fm[3][1];
#endif
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];
#ifndef FIX_BUGS
dst_fm[3][3] += dst_fm[2][3] * scr_fm[3][2];
#endif
dst_fm[3][0] = -dst_fm[3][0];
dst_fm[3][1] = -dst_fm[3][1];
dst_fm[3][2] = -dst_fm[3][2];
#ifndef FIX_BUGS
dst_fm[3][3] = scr_fm[3][3] - dst_fm[3][3];
#endif
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();
}

View File

@ -7,80 +7,22 @@ public:
RwMatrix *m_attachment; RwMatrix *m_attachment;
bool m_hasRwMatrix; // are we the owner? bool m_hasRwMatrix; // are we the owner?
CMatrix(void){ CMatrix(void);
m_attachment = nil; CMatrix(CMatrix const &m);
m_hasRwMatrix = false; CMatrix(RwMatrix *matrix, bool owner = 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(float scale){ CMatrix(float scale){
m_attachment = nil; m_attachment = nil;
m_hasRwMatrix = false; m_hasRwMatrix = false;
SetScale(scale); SetScale(scale);
} }
~CMatrix(void){ ~CMatrix(void);
if(m_hasRwMatrix && m_attachment) void Attach(RwMatrix *matrix, bool owner = false);
RwMatrixDestroy(m_attachment); void AttachRW(RwMatrix *matrix, bool owner = false);
} void Detach(void);
void Attach(RwMatrix *matrix, bool owner = false){ void Update(void);
#ifdef FIX_BUGS void UpdateRW(void);
if(m_attachment && m_hasRwMatrix) void operator=(CMatrix const &rhs);
#else CMatrix &operator+=(CMatrix const &rhs);
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);
CVector &GetPosition(void){ return *(CVector*)&m_matrix.pos; } CVector &GetPosition(void){ return *(CVector*)&m_matrix.pos; }
@ -88,23 +30,7 @@ public:
CVector &GetForward(void) { return *(CVector*)&m_matrix.up; } CVector &GetForward(void) { return *(CVector*)&m_matrix.up; }
CVector &GetUp(void) { return *(CVector*)&m_matrix.at; } CVector &GetUp(void) { return *(CVector*)&m_matrix.at; }
void SetTranslate(float x, float y, float z){ 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(const CVector &trans){ SetTranslate(trans.x, trans.y, trans.z); } void SetTranslate(const CVector &trans){ SetTranslate(trans.x, trans.y, trans.z); }
void Translate(float x, float y, float z){ void Translate(float x, float y, float z){
m_matrix.pos.x += x; m_matrix.pos.x += x;
@ -113,23 +39,7 @@ public:
} }
void Translate(const CVector &trans){ Translate(trans.x, trans.y, trans.z); } void Translate(const CVector &trans){ Translate(trans.x, trans.y, trans.z); }
void SetScale(float s){ 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 Scale(float scale) void Scale(float scale)
{ {
float *pFloatMatrix = (float*)&m_matrix; float *pFloatMatrix = (float*)&m_matrix;
@ -143,66 +53,9 @@ public:
} }
void SetRotateXOnly(float angle){ void SetRotateXOnly(float angle);
float c = Cos(angle); void SetRotateYOnly(float angle);
float s = Sin(angle); void SetRotateZOnly(float 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 SetRotateZOnlyScaled(float angle, float scale) { void SetRotateZOnlyScaled(float angle, float scale) {
float c = Cos(angle); float c = Cos(angle);
float s = Sin(angle); float s = Sin(angle);
@ -219,12 +72,9 @@ public:
m_matrix.at.y = 0.0f; m_matrix.at.y = 0.0f;
m_matrix.at.z = scale; m_matrix.at.z = scale;
} }
void SetRotateZ(float angle){ void SetRotateX(float angle);
SetRotateZOnly(angle); void SetRotateY(float angle);
m_matrix.pos.x = 0.0f; void SetRotateZ(float angle);
m_matrix.pos.y = 0.0f;
m_matrix.pos.z = 0.0f;
}
void SetRotate(float xAngle, float yAngle, float zAngle); void SetRotate(float xAngle, float yAngle, float zAngle);
void Rotate(float x, float y, float z); void Rotate(float x, float y, float z);
void RotateX(float x); void RotateX(float x);
@ -232,34 +82,9 @@ public:
void RotateZ(float z); void RotateZ(float z);
void Reorthogonalise(void); void Reorthogonalise(void);
void CopyOnlyMatrix(CMatrix *other){ void CopyOnlyMatrix(CMatrix *other);
m_matrix = other->m_matrix; void SetUnity(void);
} void ResetOrientation(void);
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) { void SetTranslateOnly(float x, float y, float z) {
m_matrix.pos.x = x; m_matrix.pos.x = x;
m_matrix.pos.y = y; m_matrix.pos.y = y;
@ -268,11 +93,12 @@ public:
void SetTranslateOnly(const CVector& pos) { void SetTranslateOnly(const CVector& pos) {
SetTranslateOnly(pos.x, pos.y, pos.z); SetTranslateOnly(pos.x, pos.y, pos.z);
} }
void CheckIntegrity(){}
}; };
CMatrix &Invert(const CMatrix &src, CMatrix &dst); 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); CMatrix operator*(const CMatrix &m1, const CMatrix &m2);
inline CVector MultiplyInverse(const CMatrix &mat, const CVector &vec) inline CVector MultiplyInverse(const CMatrix &mat, const CVector &vec)
{ {
@ -283,15 +109,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); 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 class CCompressedMatrixNotAligned
@ -304,28 +121,8 @@ class CCompressedMatrixNotAligned
int8 m_upY; int8 m_upY;
int8 m_upZ; int8 m_upZ;
public: public:
void CompressFromFullMatrix(CMatrix &other) void CompressFromFullMatrix(CMatrix &other);
{ void DecompressIntoFullMatrix(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();
}
}; };
class CCompressedMatrix : public CCompressedMatrixNotAligned class CCompressedMatrix : public CCompressedMatrixNotAligned

177
src/math/Quaternion.cpp Normal file
View File

@ -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);
}

View File

@ -10,18 +10,8 @@ public:
float Magnitude(void) const { return Sqrt(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 MagnitudeSqr(void) const { return x*x + y*y + z*z + w*w; }
void Normalise(void) { void Normalise(void);
float sq = MagnitudeSqr(); void Multiply(const CQuaternion &q1, const CQuaternion &q2);
if(sq == 0.0f)
w = 1.0f;
else{
float invsqrt = RecipSqrt(sq);
x *= invsqrt;
y *= invsqrt;
z *= invsqrt;
w *= invsqrt;
}
}
const CQuaternion &operator+=(CQuaternion const &right) { const CQuaternion &operator+=(CQuaternion const &right) {
x += right.x; x += right.x;
@ -60,8 +50,12 @@ public:
} }
void Slerp(const CQuaternion &q1, const CQuaternion &q2, float theta, float invSin, float t); 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 Set(RwV3d *axis, float angle);
void Get(RwMatrix *matrix); 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 inline float

17
src/math/Rect.cpp Normal file
View File

@ -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;
}

View File

@ -8,18 +8,8 @@ public:
float right; // x max float right; // x max
float top; // y min float top; // y min
CRect(void){ CRect(void);
left = 1000000.0f; CRect(float l, float t, float r, float b);
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;
}
void ContainPoint(CVector const &v){ void ContainPoint(CVector const &v){
if(v.x < left) left = v.x; if(v.x < left) left = v.x;
if(v.x > right) right = v.x; if(v.x > right) right = v.x;

46
src/math/Vector.cpp Normal file
View File

@ -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);
}

View File

@ -24,24 +24,7 @@ public:
float MagnitudeSqr(void) const { return x*x + y*y + z*z; } 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(x*x + y*y); }
float MagnitudeSqr2D(void) const { return x*x + y*y; } float MagnitudeSqr2D(void) const { return x*x + y*y; }
void Normalise(void) { 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 Normalise2D(void) { void Normalise2D(void) {
float sq = MagnitudeSqr2D(); 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; return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
} }
inline const CVector CVector CrossProduct(const CVector &v1, const CVector &v2);
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);
}
inline float inline float
Distance(const CVector &v1, const CVector &v2) Distance(const CVector &v1, const CVector &v2)
{ {
return (v2 - v1).Magnitude(); return (v2 - v1).Magnitude();
} }
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);

View File

@ -11,7 +11,13 @@ public:
float Magnitude(void) const { return Sqrt(x*x + y*y); } float Magnitude(void) const { return Sqrt(x*x + y*y); }
float MagnitudeSqr(void) const { return x*x + y*y; } float MagnitudeSqr(void) const { return x*x + y*y; }
void Normalise(void); void Normalise(void) {
float sq = MagnitudeSqr();
// assert(sq != 0.0f); // just be safe here
float invsqrt = RecipSqrt(sq);
x *= invsqrt;
y *= invsqrt;
}
void NormaliseSafe(void) { void NormaliseSafe(void) {
float sq = MagnitudeSqr(); float sq = MagnitudeSqr();
@ -20,7 +26,7 @@ public:
x *= invsqrt; x *= invsqrt;
y *= invsqrt; y *= invsqrt;
}else }else
y = 1.0f; x = 1.0f;
} }
const CVector2D &operator+=(CVector2D const &right) { const CVector2D &operator+=(CVector2D const &right) {

View File

@ -1,6 +1,5 @@
#include "common.h" #include "common.h"
#include "Quaternion.h"
#include "VuVector.h" #include "VuVector.h"
// TODO: move more stuff into here // TODO: move more stuff into here
@ -117,235 +116,3 @@ void TransformPoints(CVuVector *out, int n, const CMatrix &mat, const CVuVector
} }
#endif #endif
} }
void
CVector2D::Normalise(void)
{
float sq = MagnitudeSqr();
assert(sq != 0.0f); // just be safe here
//if(sq > 0.0f){
float invsqrt = RecipSqrt(sq);
x *= invsqrt;
y *= invsqrt;
//}else
// x = 1.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::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);
}

View File

@ -700,7 +700,7 @@ CGlass::WindowRespondsToExplosion(CEntity *entity, CVector point)
if ( fDistToGlass < 10.0f ) if ( fDistToGlass < 10.0f )
{ {
distToGlass.Normalise(0.3f); distToGlass *= (0.3f / fDistToGlass); // normalise
WindowRespondsToCollision(object, 10000.0f, distToGlass, object->GetPosition(), true); WindowRespondsToCollision(object, 10000.0f, distToGlass, object->GetPosition(), true);
} }
else else

View File

@ -222,10 +222,11 @@ CSkidmarks::RegisterOne(uintptr id, CVector pos, float fwdX, float fwdY, bool *i
aSkidmarks[i].m_pos[aSkidmarks[i].m_last] = pos; aSkidmarks[i].m_pos[aSkidmarks[i].m_last] = pos;
CVector2D dist = aSkidmarks[i].m_pos[aSkidmarks[i].m_last] - aSkidmarks[i].m_pos[aSkidmarks[i].m_last-1]; CVector2D right(aSkidmarks[i].m_pos[aSkidmarks[i].m_last].y - aSkidmarks[i].m_pos[aSkidmarks[i].m_last - 1].y,
dist.NormaliseSafe(); aSkidmarks[i].m_pos[aSkidmarks[i].m_last - 1].x - aSkidmarks[i].m_pos[aSkidmarks[i].m_last].x);
right.NormaliseSafe();
fwd.NormaliseSafe(); fwd.NormaliseSafe();
CVector2D right(dist.y, -dist.x);
float turn = DotProduct2D(fwd, right); float turn = DotProduct2D(fwd, right);
turn = Abs(turn) + 1.0f; turn = Abs(turn) + 1.0f;
aSkidmarks[i].m_side[aSkidmarks[i].m_last] = CVector(right.x, right.y, 0.0f) * turn * 0.125f; aSkidmarks[i].m_side[aSkidmarks[i].m_last] = CVector(right.x, right.y, 0.0f) * turn * 0.125f;

View File

@ -140,8 +140,7 @@ void CWaterCannon::Render(void)
if ( !bInit ) if ( !bInit )
{ {
CVector cp = CrossProduct(m_avecPos[pointB] - m_avecPos[pointA], TheCamera.GetForward()); CVector cp = CrossProduct(m_avecPos[pointB] - m_avecPos[pointA], TheCamera.GetForward());
cp.Normalise(0.05f); norm = cp * (0.05f / cp.Magnitude());
norm = cp;
bInit = true; bInit = true;
} }