mirror of
https://gitlab.com/shinovon/re3-symbian.git
synced 2026-05-23 01:57:21 +03:00
Initial commit
This commit is contained in:
Shinovon
commit
77cdaaf97e
827 changed files with 418745 additions and 0 deletions
544
src/math/Matrix.cpp
Normal file
544
src/math/Matrix.cpp
Normal file
|
|
@ -0,0 +1,544 @@
|
|||
#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)
|
||||
{
|
||||
GetRight() = m_attachment->right;
|
||||
GetForward() = m_attachment->up;
|
||||
GetUp() = m_attachment->at;
|
||||
GetPosition() = m_attachment->pos;
|
||||
}
|
||||
|
||||
void
|
||||
CMatrix::UpdateRW(void)
|
||||
{
|
||||
if (m_attachment) {
|
||||
m_attachment->right = GetRight();
|
||||
m_attachment->up = GetForward();
|
||||
m_attachment->at = GetUp();
|
||||
m_attachment->pos = GetPosition();
|
||||
RwMatrixUpdate(m_attachment);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
CMatrix::operator=(CMatrix const &rhs)
|
||||
{
|
||||
memcpy(this, &rhs, sizeof(f));
|
||||
if (m_attachment)
|
||||
UpdateRW();
|
||||
}
|
||||
|
||||
void
|
||||
CMatrix::CopyOnlyMatrix(const CMatrix &other)
|
||||
{
|
||||
memcpy(this, &other, sizeof(f));
|
||||
}
|
||||
|
||||
CMatrix &
|
||||
CMatrix::operator+=(CMatrix const &rhs)
|
||||
{
|
||||
GetRight() += rhs.GetRight();
|
||||
GetForward() += rhs.GetForward();
|
||||
GetUp() += rhs.GetUp();
|
||||
GetPosition() += rhs.GetPosition();
|
||||
return *this;
|
||||
}
|
||||
|
||||
void
|
||||
CMatrix::SetUnity(void)
|
||||
{
|
||||
rx = 1.0f;
|
||||
ry = 0.0f;
|
||||
rz = 0.0f;
|
||||
fx = 0.0f;
|
||||
fy = 1.0f;
|
||||
fz = 0.0f;
|
||||
ux = 0.0f;
|
||||
uy = 0.0f;
|
||||
uz = 1.0f;
|
||||
px = 0.0f;
|
||||
py = 0.0f;
|
||||
pz = 0.0f;
|
||||
}
|
||||
|
||||
void
|
||||
CMatrix::ResetOrientation(void)
|
||||
{
|
||||
rx = 1.0f;
|
||||
ry = 0.0f;
|
||||
rz = 0.0f;
|
||||
fx = 0.0f;
|
||||
fy = 1.0f;
|
||||
fz = 0.0f;
|
||||
ux = 0.0f;
|
||||
uy = 0.0f;
|
||||
uz = 1.0f;
|
||||
}
|
||||
|
||||
void
|
||||
CMatrix::SetScale(float s)
|
||||
{
|
||||
rx = s;
|
||||
ry = 0.0f;
|
||||
rz = 0.0f;
|
||||
|
||||
fx = 0.0f;
|
||||
fy = s;
|
||||
fz = 0.0f;
|
||||
|
||||
ux = 0.0f;
|
||||
uy = 0.0f;
|
||||
uz = s;
|
||||
|
||||
px = 0.0f;
|
||||
py = 0.0f;
|
||||
pz = 0.0f;
|
||||
}
|
||||
|
||||
void
|
||||
CMatrix::SetTranslate(float x, float y, float z)
|
||||
{
|
||||
rx = 1.0f;
|
||||
ry = 0.0f;
|
||||
rz = 0.0f;
|
||||
|
||||
fx = 0.0f;
|
||||
fy = 1.0f;
|
||||
fz = 0.0f;
|
||||
|
||||
ux = 0.0f;
|
||||
uy = 0.0f;
|
||||
uz = 1.0f;
|
||||
|
||||
px = x;
|
||||
py = y;
|
||||
pz = z;
|
||||
}
|
||||
|
||||
void
|
||||
CMatrix::SetRotateXOnly(float angle)
|
||||
{
|
||||
float c = Cos(angle);
|
||||
float s = Sin(angle);
|
||||
|
||||
rx = 1.0f;
|
||||
ry = 0.0f;
|
||||
rz = 0.0f;
|
||||
|
||||
fx = 0.0f;
|
||||
fy = c;
|
||||
fz = s;
|
||||
|
||||
ux = 0.0f;
|
||||
uy = -s;
|
||||
uz = c;
|
||||
}
|
||||
|
||||
void
|
||||
CMatrix::SetRotateYOnly(float angle)
|
||||
{
|
||||
float c = Cos(angle);
|
||||
float s = Sin(angle);
|
||||
|
||||
rx = c;
|
||||
ry = 0.0f;
|
||||
rz = -s;
|
||||
|
||||
fx = 0.0f;
|
||||
fy = 1.0f;
|
||||
fz = 0.0f;
|
||||
|
||||
ux = s;
|
||||
uy = 0.0f;
|
||||
uz = c;
|
||||
}
|
||||
|
||||
void
|
||||
CMatrix::SetRotateZOnly(float angle)
|
||||
{
|
||||
float c = Cos(angle);
|
||||
float s = Sin(angle);
|
||||
|
||||
rx = c;
|
||||
ry = s;
|
||||
rz = 0.0f;
|
||||
|
||||
fx = -s;
|
||||
fy = c;
|
||||
fz = 0.0f;
|
||||
|
||||
ux = 0.0f;
|
||||
uy = 0.0f;
|
||||
uz = 1.0f;
|
||||
}
|
||||
|
||||
void
|
||||
CMatrix::SetRotateX(float angle)
|
||||
{
|
||||
SetRotateXOnly(angle);
|
||||
px = 0.0f;
|
||||
py = 0.0f;
|
||||
pz = 0.0f;
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
CMatrix::SetRotateY(float angle)
|
||||
{
|
||||
SetRotateYOnly(angle);
|
||||
px = 0.0f;
|
||||
py = 0.0f;
|
||||
pz = 0.0f;
|
||||
}
|
||||
|
||||
void
|
||||
CMatrix::SetRotateZ(float angle)
|
||||
{
|
||||
SetRotateZOnly(angle);
|
||||
px = 0.0f;
|
||||
py = 0.0f;
|
||||
pz = 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);
|
||||
|
||||
rx = cZ * cY - (sZ * sX) * sY;
|
||||
ry = (cZ * sX) * sY + sZ * cY;
|
||||
rz = -cX * sY;
|
||||
|
||||
fx = -sZ * cX;
|
||||
fy = cZ * cX;
|
||||
fz = sX;
|
||||
|
||||
ux = (sZ * sX) * cY + cZ * sY;
|
||||
uy = sZ * sY - (cZ * sX) * cY;
|
||||
uz = cX * cY;
|
||||
|
||||
px = 0.0f;
|
||||
py = 0.0f;
|
||||
pz = 0.0f;
|
||||
}
|
||||
|
||||
void
|
||||
CMatrix::RotateX(float x)
|
||||
{
|
||||
float c = Cos(x);
|
||||
float s = Sin(x);
|
||||
|
||||
float ry = this->ry;
|
||||
float rz = this->rz;
|
||||
float uy = this->fy;
|
||||
float uz = this->fz;
|
||||
float ay = this->uy;
|
||||
float az = this->uz;
|
||||
float py = this->py;
|
||||
float pz = this->pz;
|
||||
|
||||
this->ry = c * ry - s * rz;
|
||||
this->rz = c * rz + s * ry;
|
||||
this->fy = c * uy - s * uz;
|
||||
this->fz = c * uz + s * uy;
|
||||
this->uy = c * ay - s * az;
|
||||
this->uz = c * az + s * ay;
|
||||
this->py = c * py - s * pz;
|
||||
this->pz = c * pz + s * py;
|
||||
}
|
||||
|
||||
void
|
||||
CMatrix::RotateY(float y)
|
||||
{
|
||||
float c = Cos(y);
|
||||
float s = Sin(y);
|
||||
|
||||
float rx = this->rx;
|
||||
float rz = this->rz;
|
||||
float ux = this->fx;
|
||||
float uz = this->fz;
|
||||
float ax = this->ux;
|
||||
float az = this->uz;
|
||||
float px = this->px;
|
||||
float pz = this->pz;
|
||||
|
||||
this->rx = c * rx + s * rz;
|
||||
this->rz = c * rz - s * rx;
|
||||
this->fx = c * ux + s * uz;
|
||||
this->fz = c * uz - s * ux;
|
||||
this->ux = c * ax + s * az;
|
||||
this->uz = c * az - s * ax;
|
||||
this->px = c * px + s * pz;
|
||||
this->pz = c * pz - s * px;
|
||||
}
|
||||
|
||||
void
|
||||
CMatrix::RotateZ(float z)
|
||||
{
|
||||
float c = Cos(z);
|
||||
float s = Sin(z);
|
||||
|
||||
float ry = this->ry;
|
||||
float rx = this->rx;
|
||||
float uy = this->fy;
|
||||
float ux = this->fx;
|
||||
float ay = this->uy;
|
||||
float ax = this->ux;
|
||||
float py = this->py;
|
||||
float px = this->px;
|
||||
|
||||
this->rx = c * rx - s * ry;
|
||||
this->ry = c * ry + s * rx;
|
||||
this->fx = c * ux - s * uy;
|
||||
this->fy = c * uy + s * ux;
|
||||
this->ux = c * ax - s * ay;
|
||||
this->uy = c * ay + s * ax;
|
||||
this->px = c * px - s * py;
|
||||
this->py = 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 = this->rx;
|
||||
float ry = this->ry;
|
||||
float rz = this->rz;
|
||||
float ux = this->fx;
|
||||
float uy = this->fy;
|
||||
float uz = this->fz;
|
||||
float ax = this->ux;
|
||||
float ay = this->uy;
|
||||
float az = this->uz;
|
||||
float px = this->px;
|
||||
float py = this->py;
|
||||
float pz = this->pz;
|
||||
|
||||
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;
|
||||
|
||||
this->rx = x1 * rx + y1 * ry + z1 * rz;
|
||||
this->ry = x2 * rx + y2 * ry + z2 * rz;
|
||||
this->rz = x3 * rx + y3 * ry + z3 * rz;
|
||||
this->fx = x1 * ux + y1 * uy + z1 * uz;
|
||||
this->fy = x2 * ux + y2 * uy + z2 * uz;
|
||||
this->fz = x3 * ux + y3 * uy + z3 * uz;
|
||||
this->ux = x1 * ax + y1 * ay + z1 * az;
|
||||
this->uy = x2 * ax + y2 * ay + z2 * az;
|
||||
this->uz = x3 * ax + y3 * ay + z3 * az;
|
||||
this->px = x1 * px + y1 * py + z1 * pz;
|
||||
this->py = x2 * px + y2 * py + z2 * pz;
|
||||
this->pz = 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;
|
||||
out.rx = m1.rx * m2.rx + m1.fx * m2.ry + m1.ux * m2.rz;
|
||||
out.ry = m1.ry * m2.rx + m1.fy * m2.ry + m1.uy * m2.rz;
|
||||
out.rz = m1.rz * m2.rx + m1.fz * m2.ry + m1.uz * m2.rz;
|
||||
out.fx = m1.rx * m2.fx + m1.fx * m2.fy + m1.ux * m2.fz;
|
||||
out.fy = m1.ry * m2.fx + m1.fy * m2.fy + m1.uy * m2.fz;
|
||||
out.fz = m1.rz * m2.fx + m1.fz * m2.fy + m1.uz * m2.fz;
|
||||
out.ux = m1.rx * m2.ux + m1.fx * m2.uy + m1.ux * m2.uz;
|
||||
out.uy = m1.ry * m2.ux + m1.fy * m2.uy + m1.uy * m2.uz;
|
||||
out.uz = m1.rz * m2.ux + m1.fz * m2.uy + m1.uz * m2.uz;
|
||||
out.px = m1.rx * m2.px + m1.fx * m2.py + m1.ux * m2.pz + m1.px;
|
||||
out.py = m1.ry * m2.px + m1.fy * m2.py + m1.uy * m2.pz + m1.py;
|
||||
out.pz = m1.rz * m2.px + m1.fz * m2.py + m1.uz * m2.pz + m1.pz;
|
||||
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
|
||||
dst.f[3][0] = dst.f[3][1] = dst.f[3][2] = 0.0f;
|
||||
#ifndef FIX_BUGS
|
||||
dst.f[3][3] = src.f[3][3];
|
||||
#endif
|
||||
|
||||
dst.f[0][0] = src.f[0][0];
|
||||
dst.f[0][1] = src.f[1][0];
|
||||
dst.f[0][2] = src.f[2][0];
|
||||
#ifndef FIX_BUGS
|
||||
dst.f[0][3] = src.f[3][0];
|
||||
#endif
|
||||
dst.f[1][0] = src.f[0][1];
|
||||
dst.f[1][1] = src.f[1][1];
|
||||
dst.f[1][2] = src.f[2][1];
|
||||
#ifndef FIX_BUGS
|
||||
dst.f[1][3] = src.f[3][1];
|
||||
#endif
|
||||
dst.f[2][0] = src.f[0][2];
|
||||
dst.f[2][1] = src.f[1][2];
|
||||
dst.f[2][2] = src.f[2][2];
|
||||
#ifndef FIX_BUGS
|
||||
dst.f[2][3] = src.f[3][2];
|
||||
#endif
|
||||
|
||||
dst.f[3][0] += dst.f[0][0] * src.f[3][0];
|
||||
dst.f[3][1] += dst.f[0][1] * src.f[3][0];
|
||||
dst.f[3][2] += dst.f[0][2] * src.f[3][0];
|
||||
#ifndef FIX_BUGS
|
||||
dst.f[3][3] += dst.f[0][3] * src.f[3][0];
|
||||
#endif
|
||||
|
||||
dst.f[3][0] += dst.f[1][0] * src.f[3][1];
|
||||
dst.f[3][1] += dst.f[1][1] * src.f[3][1];
|
||||
dst.f[3][2] += dst.f[1][2] * src.f[3][1];
|
||||
#ifndef FIX_BUGS
|
||||
dst.f[3][3] += dst.f[1][3] * src.f[3][1];
|
||||
#endif
|
||||
|
||||
dst.f[3][0] += dst.f[2][0] * src.f[3][2];
|
||||
dst.f[3][1] += dst.f[2][1] * src.f[3][2];
|
||||
dst.f[3][2] += dst.f[2][2] * src.f[3][2];
|
||||
#ifndef FIX_BUGS
|
||||
dst.f[3][3] += dst.f[2][3] * src.f[3][2];
|
||||
#endif
|
||||
|
||||
dst.f[3][0] = -dst.f[3][0];
|
||||
dst.f[3][1] = -dst.f[3][1];
|
||||
dst.f[3][2] = -dst.f[3][2];
|
||||
#ifndef FIX_BUGS
|
||||
dst.f[3][3] = src.f[3][3] - dst.f[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();
|
||||
}
|
||||
131
src/math/Matrix.h
Normal file
131
src/math/Matrix.h
Normal file
|
|
@ -0,0 +1,131 @@
|
|||
#pragma once
|
||||
|
||||
class CMatrix
|
||||
{
|
||||
public:
|
||||
union
|
||||
{
|
||||
float f[4][4];
|
||||
struct
|
||||
{
|
||||
float rx, ry, rz, rw;
|
||||
float fx, fy, fz, fw;
|
||||
float ux, uy, uz, uw;
|
||||
float px, py, pz, pw;
|
||||
};
|
||||
};
|
||||
|
||||
RwMatrix *m_attachment;
|
||||
bool m_hasRwMatrix; // are we the 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);
|
||||
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);
|
||||
CMatrix &operator*=(CMatrix const &rhs);
|
||||
|
||||
CVector &GetPosition(void) { return *(CVector*)&px; }
|
||||
CVector &GetRight(void) { return *(CVector*)℞ }
|
||||
CVector &GetForward(void) { return *(CVector*)&fx; }
|
||||
CVector &GetUp(void) { return *(CVector*)&ux; }
|
||||
|
||||
const CVector &GetPosition(void) const { return *(CVector*)&px; }
|
||||
const CVector &GetRight(void) const { return *(CVector*)℞ }
|
||||
const CVector &GetForward(void) const { return *(CVector*)&fx; }
|
||||
const CVector &GetUp(void) const { return *(CVector*)&ux; }
|
||||
|
||||
|
||||
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){
|
||||
px += x;
|
||||
py += y;
|
||||
pz += z;
|
||||
}
|
||||
void Translate(const CVector &trans){ Translate(trans.x, trans.y, trans.z); }
|
||||
|
||||
void SetScale(float s);
|
||||
void Scale(float scale)
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
#ifdef FIX_BUGS // BUGFIX from VC
|
||||
for (int j = 0; j < 3; j++)
|
||||
#else
|
||||
for (int j = 0; j < 4; j++)
|
||||
#endif
|
||||
f[i][j] *= scale;
|
||||
}
|
||||
|
||||
|
||||
void SetRotateXOnly(float angle);
|
||||
void SetRotateYOnly(float angle);
|
||||
void SetRotateZOnly(float angle);
|
||||
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);
|
||||
void RotateY(float y);
|
||||
void RotateZ(float z);
|
||||
|
||||
void Reorthogonalise(void);
|
||||
void CopyOnlyMatrix(const CMatrix &other);
|
||||
void SetUnity(void);
|
||||
void ResetOrientation(void);
|
||||
void SetTranslateOnly(float x, float y, float z) {
|
||||
px = x;
|
||||
py = y;
|
||||
pz = z;
|
||||
}
|
||||
void SetTranslateOnly(const CVector& pos) {
|
||||
SetTranslateOnly(pos.x, pos.y, pos.z);
|
||||
}
|
||||
void CheckIntegrity(){}
|
||||
};
|
||||
|
||||
|
||||
CMatrix &Invert(const CMatrix &src, CMatrix &dst);
|
||||
CMatrix Invert(const CMatrix &matrix);
|
||||
CMatrix operator*(const CMatrix &m1, const CMatrix &m2);
|
||||
inline CVector MultiplyInverse(const CMatrix &mat, const CVector &vec)
|
||||
{
|
||||
CVector v(vec.x - mat.px, vec.y - mat.py, vec.z - mat.pz);
|
||||
return CVector(
|
||||
mat.rx * v.x + mat.ry * v.y + mat.rz * v.z,
|
||||
mat.fx * v.x + mat.fy * v.y + mat.fz * v.z,
|
||||
mat.ux * v.x + mat.uy * v.y + mat.uz * v.z);
|
||||
}
|
||||
|
||||
|
||||
|
||||
class CCompressedMatrixNotAligned
|
||||
{
|
||||
CVector m_vecPos;
|
||||
int8 m_rightX;
|
||||
int8 m_rightY;
|
||||
int8 m_rightZ;
|
||||
int8 m_upX;
|
||||
int8 m_upY;
|
||||
int8 m_upZ;
|
||||
public:
|
||||
void CompressFromFullMatrix(CMatrix &other);
|
||||
void DecompressIntoFullMatrix(CMatrix &other);
|
||||
};
|
||||
|
||||
class CCompressedMatrix : public CCompressedMatrixNotAligned
|
||||
{
|
||||
int _alignment; // no clue what would this align to
|
||||
};
|
||||
177
src/math/Quaternion.cpp
Normal file
177
src/math/Quaternion.cpp
Normal 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);
|
||||
}
|
||||
95
src/math/Quaternion.h
Normal file
95
src/math/Quaternion.h
Normal file
|
|
@ -0,0 +1,95 @@
|
|||
#pragma once
|
||||
|
||||
// TODO: actually implement this
|
||||
class CQuaternion
|
||||
{
|
||||
public:
|
||||
float x, y, z, w;
|
||||
CQuaternion(void) {}
|
||||
CQuaternion(float x, float y, float z, float w) : x(x), y(y), z(z), w(w) {}
|
||||
|
||||
float Magnitude(void) const { return Sqrt(x*x + y*y + z*z + w*w); }
|
||||
float MagnitudeSqr(void) const { return x*x + y*y + z*z + w*w; }
|
||||
void Normalise(void);
|
||||
void Multiply(const CQuaternion &q1, const CQuaternion &q2);
|
||||
void Invert(void){ // Conjugate would have been a better name
|
||||
x = -x;
|
||||
y = -y;
|
||||
z = -z;
|
||||
}
|
||||
|
||||
const CQuaternion &operator+=(CQuaternion const &right) {
|
||||
x += right.x;
|
||||
y += right.y;
|
||||
z += right.z;
|
||||
w += right.w;
|
||||
return *this;
|
||||
}
|
||||
|
||||
const CQuaternion &operator-=(CQuaternion const &right) {
|
||||
x -= right.x;
|
||||
y -= right.y;
|
||||
z -= right.z;
|
||||
w -= right.w;
|
||||
return *this;
|
||||
}
|
||||
|
||||
const CQuaternion &operator*=(float right) {
|
||||
x *= right;
|
||||
y *= right;
|
||||
z *= right;
|
||||
w *= right;
|
||||
return *this;
|
||||
}
|
||||
|
||||
const CQuaternion &operator/=(float right) {
|
||||
x /= right;
|
||||
y /= right;
|
||||
z /= right;
|
||||
w /= right;
|
||||
return *this;
|
||||
}
|
||||
|
||||
CQuaternion operator-() const {
|
||||
return CQuaternion(-x, -y, -z, -w);
|
||||
}
|
||||
|
||||
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
|
||||
DotProduct(const CQuaternion &q1, const CQuaternion &q2)
|
||||
{
|
||||
return q1.x*q2.x + q1.y*q2.y + q1.z*q2.z + q1.w*q2.w;
|
||||
}
|
||||
|
||||
inline CQuaternion operator+(const CQuaternion &left, const CQuaternion &right)
|
||||
{
|
||||
return CQuaternion(left.x + right.x, left.y + right.y, left.z + right.z, left.w + right.w);
|
||||
}
|
||||
|
||||
inline CQuaternion operator-(const CQuaternion &left, const CQuaternion &right)
|
||||
{
|
||||
return CQuaternion(left.x - right.x, left.y - right.y, left.z - right.z, left.w - right.w);
|
||||
}
|
||||
|
||||
inline CQuaternion operator*(const CQuaternion &left, float right)
|
||||
{
|
||||
return CQuaternion(left.x * right, left.y * right, left.z * right, left.w * right);
|
||||
}
|
||||
|
||||
inline CQuaternion operator*(float left, const CQuaternion &right)
|
||||
{
|
||||
return CQuaternion(left * right.x, left * right.y, left * right.z, left * right.w);
|
||||
}
|
||||
|
||||
inline CQuaternion operator/(const CQuaternion &left, float right)
|
||||
{
|
||||
return CQuaternion(left.x / right, left.y / right, left.z / right, left.w / right);
|
||||
}
|
||||
17
src/math/Rect.cpp
Normal file
17
src/math/Rect.cpp
Normal 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;
|
||||
}
|
||||
71
src/math/Rect.h
Normal file
71
src/math/Rect.h
Normal file
|
|
@ -0,0 +1,71 @@
|
|||
#pragma once
|
||||
|
||||
class CRect
|
||||
{
|
||||
public:
|
||||
float left; // x min
|
||||
float bottom; // y max
|
||||
float right; // x max
|
||||
float top; // y min
|
||||
|
||||
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;
|
||||
if(v.y < top) top = v.y;
|
||||
if(v.y > bottom) bottom = v.y;
|
||||
}
|
||||
void ContainRect(const CRect &r){
|
||||
if(r.left < left) left = r.left;
|
||||
if(r.right > right) right = r.right;
|
||||
if(r.top < top) top = r.top;
|
||||
if(r.bottom > bottom) bottom = r.bottom;
|
||||
}
|
||||
|
||||
bool IsPointInside(const CVector2D &p){
|
||||
return p.x >= left &&
|
||||
p.x <= right &&
|
||||
p.y >= top &&
|
||||
p.y <= bottom;
|
||||
}
|
||||
bool IsPointInside(const CVector2D &p, float extraRadius){
|
||||
return p.x >= left-extraRadius &&
|
||||
p.x <= right+extraRadius &&
|
||||
p.y >= top-extraRadius &&
|
||||
p.y <= bottom+extraRadius;
|
||||
}
|
||||
|
||||
void Translate(float x, float y){
|
||||
left += x;
|
||||
right += x;
|
||||
bottom += y;
|
||||
top += y;
|
||||
}
|
||||
|
||||
void Grow(float r) {
|
||||
left -= r;
|
||||
right += r;
|
||||
top -= r;
|
||||
bottom += r;
|
||||
}
|
||||
|
||||
void Grow(float l, float r)
|
||||
{
|
||||
left -= l;
|
||||
top -= l;
|
||||
right += r;
|
||||
bottom += r;
|
||||
}
|
||||
|
||||
void Grow(float l, float r, float t, float b)
|
||||
{
|
||||
left -= l;
|
||||
top -= t;
|
||||
right += r;
|
||||
bottom += b;
|
||||
}
|
||||
|
||||
float GetWidth(void) { return right - left; }
|
||||
float GetHeight(void) { return bottom - top; }
|
||||
};
|
||||
46
src/math/Vector.cpp
Normal file
46
src/math/Vector.cpp
Normal 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.rx * vec.x + mat.fx * vec.y + mat.ux * vec.z,
|
||||
mat.ry * vec.x + mat.fy * vec.y + mat.uy * vec.z,
|
||||
mat.rz * vec.x + mat.fz * vec.y + mat.uz * vec.z);
|
||||
}
|
||||
|
||||
CVector
|
||||
Multiply3x3(const CVector &vec, const CMatrix &mat)
|
||||
{
|
||||
return CVector(mat.rx * vec.x + mat.ry * vec.y + mat.rz * vec.z,
|
||||
mat.fx * vec.x + mat.fy * vec.y + mat.fz * vec.z,
|
||||
mat.ux * vec.x + mat.uy * vec.y + mat.uz * vec.z);
|
||||
}
|
||||
|
||||
CVector
|
||||
operator*(const CMatrix &mat, const CVector &vec)
|
||||
{
|
||||
// TODO: VU0 code
|
||||
return CVector(mat.rx * vec.x + mat.fx * vec.y + mat.ux * vec.z + mat.px,
|
||||
mat.ry * vec.x + mat.fy * vec.y + mat.uy * vec.z + mat.py,
|
||||
mat.rz * vec.x + mat.fz * vec.y + mat.uz * vec.z + mat.pz);
|
||||
}
|
||||
129
src/math/Vector.h
Normal file
129
src/math/Vector.h
Normal file
|
|
@ -0,0 +1,129 @@
|
|||
#pragma once
|
||||
|
||||
class CVector : public RwV3d
|
||||
{
|
||||
public:
|
||||
CVector(void) {}
|
||||
CVector(float x, float y, float z)
|
||||
{
|
||||
this->x = x;
|
||||
this->y = y;
|
||||
this->z = z;
|
||||
}
|
||||
|
||||
CVector(const RwV3d &v)
|
||||
{
|
||||
x = v.x;
|
||||
y = v.y;
|
||||
z = v.z;
|
||||
}
|
||||
// (0,1,0) means no rotation. So get right vector and its atan
|
||||
float Heading(void) const { return Atan2(-x, y); }
|
||||
float Magnitude(void) const { return Sqrt(x*x + y*y + z*z); }
|
||||
float 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);
|
||||
|
||||
void Normalise2D(void) {
|
||||
float sq = MagnitudeSqr2D();
|
||||
float invsqrt = RecipSqrt(sq);
|
||||
x *= invsqrt;
|
||||
y *= invsqrt;
|
||||
}
|
||||
|
||||
const CVector &operator+=(CVector const &right) {
|
||||
x += right.x;
|
||||
y += right.y;
|
||||
z += right.z;
|
||||
return *this;
|
||||
}
|
||||
|
||||
const CVector &operator-=(CVector const &right) {
|
||||
x -= right.x;
|
||||
y -= right.y;
|
||||
z -= right.z;
|
||||
return *this;
|
||||
}
|
||||
|
||||
const CVector &operator*=(float right) {
|
||||
x *= right;
|
||||
y *= right;
|
||||
z *= right;
|
||||
return *this;
|
||||
}
|
||||
|
||||
const CVector &operator/=(float right) {
|
||||
x /= right;
|
||||
y /= right;
|
||||
z /= right;
|
||||
return *this;
|
||||
}
|
||||
|
||||
CVector operator-() const {
|
||||
return CVector(-x, -y, -z);
|
||||
}
|
||||
|
||||
const bool operator==(CVector const &right) {
|
||||
return x == right.x && y == right.y && z == right.z;
|
||||
}
|
||||
|
||||
const bool operator!=(CVector const &right) {
|
||||
return x != right.x || y != right.y || z != right.z;
|
||||
}
|
||||
|
||||
bool IsZero(void) const { return x == 0.0f && y == 0.0f && z == 0.0f; }
|
||||
};
|
||||
|
||||
inline CVector operator+(const CVector &left, const CVector &right)
|
||||
{
|
||||
return CVector(left.x + right.x, left.y + right.y, left.z + right.z);
|
||||
}
|
||||
|
||||
inline CVector operator-(const CVector &left, const CVector &right)
|
||||
{
|
||||
return CVector(left.x - right.x, left.y - right.y, left.z - right.z);
|
||||
}
|
||||
|
||||
inline CVector operator*(const CVector &left, float right)
|
||||
{
|
||||
return CVector(left.x * right, left.y * right, left.z * right);
|
||||
}
|
||||
|
||||
inline CVector operator*(float left, const CVector &right)
|
||||
{
|
||||
return CVector(left * right.x, left * right.y, left * right.z);
|
||||
}
|
||||
|
||||
inline CVector operator/(const CVector &left, float right)
|
||||
{
|
||||
return CVector(left.x / right, left.y / right, left.z / right);
|
||||
}
|
||||
|
||||
inline float
|
||||
DotProduct(const CVector &v1, const CVector &v2)
|
||||
{
|
||||
return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
|
||||
}
|
||||
|
||||
CVector CrossProduct(const CVector &v1, const CVector &v2);
|
||||
|
||||
inline float
|
||||
Distance(const CVector &v1, const CVector &v2)
|
||||
{
|
||||
return (v2 - v1).Magnitude();
|
||||
}
|
||||
|
||||
inline float
|
||||
Distance2D(const CVector &v1, const CVector &v2)
|
||||
{
|
||||
float x = v2.x - v1.x;
|
||||
float y = v2.y - v1.y;
|
||||
return Sqrt(x*x + y*y);
|
||||
}
|
||||
|
||||
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);
|
||||
109
src/math/Vector2D.h
Normal file
109
src/math/Vector2D.h
Normal file
|
|
@ -0,0 +1,109 @@
|
|||
#pragma once
|
||||
|
||||
class CVector2D
|
||||
{
|
||||
public:
|
||||
float x, y;
|
||||
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();
|
||||
// assert(sq != 0.0f); // just be safe here
|
||||
float invsqrt = RecipSqrt(sq);
|
||||
x *= invsqrt;
|
||||
y *= invsqrt;
|
||||
}
|
||||
|
||||
void NormaliseSafe(void) {
|
||||
float sq = MagnitudeSqr();
|
||||
if(sq > 0.0f){
|
||||
float invsqrt = RecipSqrt(sq);
|
||||
x *= invsqrt;
|
||||
y *= invsqrt;
|
||||
}else
|
||||
x = 1.0f;
|
||||
}
|
||||
|
||||
const CVector2D &operator+=(CVector2D const &right) {
|
||||
x += right.x;
|
||||
y += right.y;
|
||||
return *this;
|
||||
}
|
||||
|
||||
const CVector2D &operator-=(CVector2D const &right) {
|
||||
x -= right.x;
|
||||
y -= right.y;
|
||||
return *this;
|
||||
}
|
||||
|
||||
const CVector2D &operator*=(float right) {
|
||||
x *= right;
|
||||
y *= right;
|
||||
return *this;
|
||||
}
|
||||
|
||||
const CVector2D &operator/=(float right) {
|
||||
x /= right;
|
||||
y /= right;
|
||||
return *this;
|
||||
}
|
||||
CVector2D operator-(const CVector2D &rhs) const {
|
||||
return CVector2D(x-rhs.x, y-rhs.y);
|
||||
}
|
||||
CVector2D operator+(const CVector2D &rhs) const {
|
||||
return CVector2D(x+rhs.x, y+rhs.y);
|
||||
}
|
||||
CVector2D operator/(float t) const {
|
||||
return CVector2D(x/t, y/t);
|
||||
}
|
||||
};
|
||||
|
||||
inline float
|
||||
DotProduct2D(const CVector2D &v1, const CVector2D &v2)
|
||||
{
|
||||
return v1.x*v2.x + v1.y*v2.y;
|
||||
}
|
||||
|
||||
inline float
|
||||
CrossProduct2D(const CVector2D &v1, const CVector2D &v2)
|
||||
{
|
||||
return v1.x*v2.y - v1.y*v2.x;
|
||||
}
|
||||
|
||||
inline float
|
||||
Distance2D(const CVector2D &v, float x, float y)
|
||||
{
|
||||
return Sqrt((v.x-x)*(v.x-x) + (v.y-y)*(v.y-y));
|
||||
}
|
||||
|
||||
inline float
|
||||
DistanceSqr2D(const CVector2D &v, float x, float y)
|
||||
{
|
||||
return (v.x-x)*(v.x-x) + (v.y-y)*(v.y-y);
|
||||
}
|
||||
|
||||
inline void
|
||||
NormalizeXY(float &x, float &y)
|
||||
{
|
||||
float l = Sqrt(x*x + y*y);
|
||||
if(l != 0.0f){
|
||||
x /= l;
|
||||
y /= l;
|
||||
}else
|
||||
x = 1.0f;
|
||||
}
|
||||
|
||||
inline CVector2D operator*(const CVector2D &left, float right)
|
||||
{
|
||||
return CVector2D(left.x * right, left.y * right);
|
||||
}
|
||||
|
||||
inline CVector2D operator*(float left, const CVector2D &right)
|
||||
{
|
||||
return CVector2D(left * right.x, left * right.y);
|
||||
}
|
||||
32
src/math/VuVector.h
Normal file
32
src/math/VuVector.h
Normal file
|
|
@ -0,0 +1,32 @@
|
|||
#pragma once
|
||||
|
||||
class TYPEALIGN(16) CVuVector : public CVector
|
||||
{
|
||||
public:
|
||||
float w;
|
||||
CVuVector(void) {}
|
||||
CVuVector(float x, float y, float z) : CVector(x, y, z) {}
|
||||
CVuVector(float x, float y, float z, float w) : CVector(x, y, z), w(w) {}
|
||||
CVuVector(const CVector &v) : CVector(v.x, v.y, v.z) {}
|
||||
CVuVector(const RwV3d &v) : CVector(v) {}
|
||||
/*
|
||||
void Normalise(void) {
|
||||
float sq = MagnitudeSqr();
|
||||
// TODO: VU0 code
|
||||
if(sq > 0.0f){
|
||||
float invsqrt = RecipSqrt(sq);
|
||||
x *= invsqrt;
|
||||
y *= invsqrt;
|
||||
z *= invsqrt;
|
||||
}else
|
||||
x = 1.0f;
|
||||
}
|
||||
*/
|
||||
|
||||
// TODO: operator-
|
||||
};
|
||||
|
||||
void TransformPoint(CVuVector &out, const CMatrix &mat, const CVuVector &in);
|
||||
void TransformPoint(CVuVector &out, const CMatrix &mat, const RwV3d &in);
|
||||
void TransformPoints(CVuVector *out, int n, const CMatrix &mat, const RwV3d *in, int stride);
|
||||
void TransformPoints(CVuVector *out, int n, const CMatrix &mat, const CVuVector *in);
|
||||
118
src/math/math.cpp
Normal file
118
src/math/math.cpp
Normal file
|
|
@ -0,0 +1,118 @@
|
|||
#include "common.h"
|
||||
|
||||
#include "VuVector.h"
|
||||
|
||||
// TODO: move more stuff into here
|
||||
|
||||
|
||||
void TransformPoint(CVuVector &out, const CMatrix &mat, const CVuVector &in)
|
||||
{
|
||||
#ifdef GTA_PS2
|
||||
__asm__ __volatile__("\n\
|
||||
lqc2 vf01,0x0(%2)\n\
|
||||
lqc2 vf02,0x0(%1)\n\
|
||||
lqc2 vf03,0x10(%1)\n\
|
||||
lqc2 vf04,0x20(%1)\n\
|
||||
lqc2 vf05,0x30(%1)\n\
|
||||
vmulax.xyz ACC, vf02,vf01\n\
|
||||
vmadday.xyz ACC, vf03,vf01\n\
|
||||
vmaddaz.xyz ACC, vf04,vf01\n\
|
||||
vmaddw.xyz vf06,vf05,vf00\n\
|
||||
sqc2 vf06,0x0(%0)\n\
|
||||
": : "r" (&out) , "r" (&mat) ,"r" (&in): "memory");
|
||||
#else
|
||||
out = mat * in;
|
||||
#endif
|
||||
}
|
||||
|
||||
void TransformPoint(CVuVector &out, const CMatrix &mat, const RwV3d &in)
|
||||
{
|
||||
#ifdef GTA_PS2
|
||||
__asm__ __volatile__("\n\
|
||||
ldr $8,0x0(%2)\n\
|
||||
ldl $8,0x7(%2)\n\
|
||||
lw $9,0x8(%2)\n\
|
||||
pcpyld $10,$9,$8\n\
|
||||
qmtc2 $10,vf01\n\
|
||||
lqc2 vf02,0x0(%1)\n\
|
||||
lqc2 vf03,0x10(%1)\n\
|
||||
lqc2 vf04,0x20(%1)\n\
|
||||
lqc2 vf05,0x30(%1)\n\
|
||||
vmulax.xyz ACC, vf02,vf01\n\
|
||||
vmadday.xyz ACC, vf03,vf01\n\
|
||||
vmaddaz.xyz ACC, vf04,vf01\n\
|
||||
vmaddw.xyz vf06,vf05,vf00\n\
|
||||
sqc2 vf06,0x0(%0)\n\
|
||||
": : "r" (&out) , "r" (&mat) ,"r" (&in): "memory");
|
||||
#else
|
||||
out = mat * in;
|
||||
#endif
|
||||
}
|
||||
|
||||
void TransformPoints(CVuVector *out, int n, const CMatrix &mat, const RwV3d *in, int stride)
|
||||
{
|
||||
#ifdef GTA_PS3
|
||||
__asm__ __volatile__("\n\
|
||||
paddub $3,%4,$0\n\
|
||||
lqc2 vf02,0x0(%2)\n\
|
||||
lqc2 vf03,0x10(%2)\n\
|
||||
lqc2 vf04,0x20(%2)\n\
|
||||
lqc2 vf05,0x30(%2)\n\
|
||||
ldr $8,0x0(%3)\n\
|
||||
ldl $8,0x7(%3)\n\
|
||||
lw $9,0x8(%3)\n\
|
||||
pcpyld $10,$9,$8\n\
|
||||
qmtc2 $10,vf01\n\
|
||||
1: vmulax.xyz ACC, vf02,vf01\n\
|
||||
vmadday.xyz ACC, vf03,vf01\n\
|
||||
vmaddaz.xyz ACC, vf04,vf01\n\
|
||||
vmaddw.xyz vf06,vf05,vf00\n\
|
||||
add %3,%3,$3\n\
|
||||
ldr $8,0x0(%3)\n\
|
||||
ldl $8,0x7(%3)\n\
|
||||
lw $9,0x8(%3)\n\
|
||||
pcpyld $10,$9,$8\n\
|
||||
qmtc2 $10,vf01\n\
|
||||
addi %1,%1,-1\n\
|
||||
addiu %0,%0,0x10\n\
|
||||
sqc2 vf06,-0x10(%0)\n\
|
||||
bnez %1,1b\n\
|
||||
": : "r" (out) , "r" (n), "r" (&mat), "r" (in), "r" (stride): "memory");
|
||||
#else
|
||||
while(n--){
|
||||
*out = mat * *in;
|
||||
in = (RwV3d*)((uint8*)in + stride);
|
||||
out++;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void TransformPoints(CVuVector *out, int n, const CMatrix &mat, const CVuVector *in)
|
||||
{
|
||||
#ifdef GTA_PS2
|
||||
__asm__ __volatile__("\n\
|
||||
lqc2 vf02,0x0(%2)\n\
|
||||
lqc2 vf03,0x10(%2)\n\
|
||||
lqc2 vf04,0x20(%2)\n\
|
||||
lqc2 vf05,0x30(%2)\n\
|
||||
lqc2 vf01,0x0(%3)\n\
|
||||
nop\n\
|
||||
1: vmulax.xyz ACC, vf02,vf01\n\
|
||||
vmadday.xyz ACC, vf03,vf01\n\
|
||||
vmaddaz.xyz ACC, vf04,vf01\n\
|
||||
vmaddw.xyz vf06,vf05,vf00\n\
|
||||
lqc2 vf01,0x10(%3)\n\
|
||||
addiu %3,%3,0x10\n\
|
||||
addi %1,%1,-1\n\
|
||||
addiu %0,%0,0x10\n\
|
||||
sqc2 vf06,-0x10(%0)\n\
|
||||
bnez %1,1b\n\
|
||||
": : "r" (out) , "r" (n), "r" (&mat) ,"r" (in): "memory");
|
||||
#else
|
||||
while(n--){
|
||||
*out = mat * *in;
|
||||
in++;
|
||||
out++;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
19
src/math/maths.h
Normal file
19
src/math/maths.h
Normal file
|
|
@ -0,0 +1,19 @@
|
|||
#pragma once
|
||||
|
||||
// wrapper around float versions of functions
|
||||
// in gta they are in CMaths but that makes the code rather noisy
|
||||
|
||||
inline float Sin(float x) { return sinf(x); }
|
||||
inline float Asin(float x) { return asinf(x); }
|
||||
inline float Cos(float x) { return cosf(x); }
|
||||
inline float Acos(float x) { return acosf(x); }
|
||||
inline float Tan(float x) { return tanf(x); }
|
||||
inline float Atan(float x) { return atanf(x); }
|
||||
inline float Atan2(float y, float x) { return atan2f(y, x); }
|
||||
inline float Abs(float x) { return fabsf(x); }
|
||||
inline float Sqrt(float x) { return sqrtf(x); }
|
||||
inline float RecipSqrt(float x, float y) { return x/Sqrt(y); }
|
||||
inline float RecipSqrt(float x) { return RecipSqrt(1.0f, x); }
|
||||
inline float Pow(float x, float y) { return powf(x, y); }
|
||||
inline float Floor(float x) { return floorf(x); }
|
||||
inline float Ceil(float x) { return ceilf(x); }
|
||||
Loading…
Add table
Add a link
Reference in a new issue