diff --git a/include/d/a/d_a_base.h b/include/d/a/d_a_base.h index 4593d186..94ed6bee 100644 --- a/include/d/a/d_a_base.h +++ b/include/d/a/d_a_base.h @@ -86,7 +86,9 @@ public: } void copyPosition() { - pos_copy = position; + pos_copy.x = position.x; + pos_copy.y = position.y; + pos_copy.z = position.z; } void copyRotation() { rot_copy = rotation; diff --git a/include/egg/math/eggMath.h b/include/egg/math/eggMath.h index e0580140..1a5a40c6 100644 --- a/include/egg/math/eggMath.h +++ b/include/egg/math/eggMath.h @@ -2,19 +2,56 @@ #include "types.h" -namespace EGG -{ +namespace EGG { template class Math { public: + static T maxNumber() { + // TODO: Generalize to other classes + // This is low priority since it will always be a float + return FLT_MAX; + } + + static T pi() { + return 3.14159265f; + } + + static T pi_half() { + return pi() / 2.0f; + } + + static T epsilon() { + return 1.192092896e-07f; + } + + static T inv(T t) { + return 1 / t; + } + + static T abs(T t) { + return t > 0 ? t : -t; + } + static T sqrt(T); static T sin(T); static T cos(T); + static T tan(T); + static T asin(T); static T acos(T); - static T atan2(T); + static T atan2(T, T); + static T log10(T); + + static T gcd(T, T); + static T lcm(T, T); }; +// There is +// Math::zero +// Math::pi_half +// Math::neg(f32) +// Math::abs(f32) + // f32 impls // /* 8049ab60 */ Math::sqrt(f32); // /* 8049abb0 */ Math::sin(f32); diff --git a/include/egg/math/eggMatrix.h b/include/egg/math/eggMatrix.h index 49be2360..4c38c30a 100644 --- a/include/egg/math/eggMatrix.h +++ b/include/egg/math/eggMatrix.h @@ -1,36 +1,94 @@ #pragma once -#include "types.h" -#include "egg/math/eggVector.h" #include "egg/math/eggQuat.h" +#include "egg/math/eggVector.h" +#include "types.h" -namespace EGG -{ +namespace EGG { -// Maybe inherits -class Matrix34f { -public: - f32 mData[3][4]; -public: - Matrix34f() {}; - f32& operator()(int i, int j) {return mData[i][j]; } - /* 8049ac70 */ bool InverseTo(Matrix34f* dest) const; - /* 8049ac80 */ bool InverseTransposeTo(Matrix34f* dest) const; +struct Matrix34f { + Matrix34f() {} + /* 8049acd0 */ + Matrix34f(f32 xx, f32 xy, f32 xz, f32 xw, f32 yx, f32 yy, f32 yz, f32 yw, f32 zx, f32 zy, + f32 zz, f32 zw); + + ////////////////////////////////////////////////////////////////////////////// + f32 operator()(int i, int j) const { + return m[i][j]; + } + f32 &operator()(int i, int j) { + return m[i][j]; + } + f32 operator()(int i) const { + return arr[i]; + } + f32 &operator()(int i) { + return arr[i]; + } + + /* 8049ac70 */ void inverseTo(Matrix34f &to) const; + /* 8049ac80 */ void inverseTransposeTo(Matrix34f &to) const; /* 8049ac90 */ void makeIdentity(); - /* 8049acd0 */ Matrix34f( \ - f32 fxx, f32 fxy, f32 fxz, f32 fxw, \ - f32 fyx, f32 fyy, f32 fyz, f32 fyw, \ - f32 fzx, f32 fzy, f32 fzz, f32 fzw); - /* 8049ad20 */ void makeQT(const Quatf&, const Vector3f&); - /* 8049ade0 */ void makeQ(const Quatf&); // these could be swapped - /* 8049ae90 */ void fromQuat(const Quatf& from); // these could be swapped - /* 8049af40 */ void toQuat(Quatf& dest); - /* 8049b250 */ void slerpTo(const Matrix34f&, Matrix34f&, f32) const; - /* 8049b2d0 */ void setAxisRotation(const Vector3f&, f32); + /* */ void makeSRT(const Vector3f &s, const Vector3f &r, const Vector3f &t); + /* */ void makeRT(const Vector3f &r, const Vector3f &t); + /* */ void makeR(const Vector3f &r); + /* */ void makeST(const Vector3f &s, const Vector3f &t); + /* */ void makeSQT(const Vector3f &s, const Quatf &q, const Vector3f &t); + /* */ void makeSQ(const Vector3f &, const Quatf &); + /* 8049ad20 */ void makeQT(const Quatf &, const Vector3f &); + /* 8049ade0 */ void makeQ(const Quatf &); + /* */ void makeS(const Vector3f &s); + /* */ void makeT(const Vector3f &t); + /* 8049ae90 */ void fromQuat(const Quatf &q); + /* 8049af40 */ void toQuat(Quatf &q) const; + /* 8049b250 */ void slerpTo(const Matrix34f &, Matrix34f &, f32) const; + /* */ void copyTo16(f32 *pf) const; + /* 8049b2d0 */ void setAxisRotation(const Vector3f &, f32); + /* */ Vector3f multVector(const Vector3f &vec) const; /* 8049b310 */ void loadPosMtx(u32); - /* 8049b320 */ void multiplyTo(const Matrix34f&, Matrix34f& out); + /* */ void loadNrmMtx(u32 nrmMtxId); + /* 8049b320 */ void multiplyTo(const Matrix34f &m2, Matrix34f &to) const; + /* */ void dump(); + + ////////////////////////////////////////////////////////////////////////////// public: - /* 80674c00 */ static Matrix34f ident; + void concat(const Matrix34f &, Matrix34f &) const; + void copyFrom(const Matrix34f &other); + void rotate(const Vector3f &rpy) const; + void multVectorTo(const Vector3f &from, Vector3f &to) const; + void transposeTo(Matrix34f &to) const; + void setBase(int idx, const Vector3f &b) { + m[0][idx] = b.x; + m[1][idx] = b.y; + m[2][idx] = b.z; + } + void getBase(int idx, Vector3f &b) const { + b.x = m[0][idx]; + b.y = m[1][idx]; + b.z = m[2][idx]; + } + void setTranslation(const Vector3f &t) { + setBase(3, t); + } + void getTranslation(Vector3f &t) const { + getBase(3, t); + } + void makeZero() { + for (int i = 0; i < 12; i++) { + arr[i] = 0.0f; + } + } + + ////////////////////////////////////////////////////////////////////////////// +public: + union { + f32 m[3][4]; + f32 arr[12]; + }; + + ////////////////////////////////////////////////////////////////////////////// +public: + /* 80674c00 */ static const Matrix34f ident; }; } // namespace EGG diff --git a/include/egg/math/eggQuat.h b/include/egg/math/eggQuat.h index 5c8572c0..bdd18ed8 100644 --- a/include/egg/math/eggQuat.h +++ b/include/egg/math/eggQuat.h @@ -1,26 +1,75 @@ #pragma once -#include "types.h" #include "egg/math/eggVector.h" +#include "types.h" -namespace EGG -{ +namespace EGG { -class Quatf { -public: - f32 x,y,z,w; -public: +struct Quatf : Vector3f { Quatf() {} - // could also be void set(f32, f32, f32, f32); - /* 8049b390 */ Quatf(f32 fx, f32 fy, f32 fz, f32 fw); - /* 8049b3b0 */ void setAxisRotation(const Vector3f&, f32); - /* 8049b450 */ f32 norm(); + Quatf(f32 f, Vector3f v) : w(f), Vector3f(v) {} + + // ~Quatf() {} + + friend Quatf operator*(const Quatf &q, const Vector3f &vec) { + Vector3f crossed = q.cross(vec); + Vector3f scaled = vec * q.w; + Quatf ret = Quatf(-q.Vector3f::dot(vec), crossed + scaled); + return ret; + } + + // TODO: Implement + friend Quatf operator*(const Quatf &u, const Quatf &v) { + Vector3f cross = u.cross(v); + Vector3f v_mul_w = u.w * v; + Vector3f u_mul_w = v.w * v; + Vector3f added_2 = u_mul_w + (cross + v_mul_w); + Quatf out = Quatf(u.w * v.w - u.Vector3f::dot(v), added_2); + return out; + }; + + /* 8049b390 */ void set(f32 fw, f32 fx, f32 fy, f32 fz); + /* */ void set(f32 f, const Vector3f &vec); + /* */ void setRPY(const EGG::Vector3f &rpy); + /* */ Vector3f calcRPY(); + /* */ void setRPY(f32 roll, f32 pitch, f32 yaw); + /* 8049b3b0 */ void setAxisRotation(const Vector3f &, f32); + /* 8049b450 */ f32 norm(); /* 8049b480 */ void normalise(); - /* 8049b500 */ void conjugate(); - /* 8049b550 */ Vector3f rotateVector(const Vector3f&); - /* 8049b800 */ void slerpTo(const Quatf&, f32, Quatf& out) const; - /* 8049b800 */ void slerpTo(const Quatf&, f32, f32, Quatf& out) const; - /* 8049bbb0 */ void makeVectorRotation(Vector3f&, Vector3f&); + /* 8049b500 */ Quatf conjugate(); + /* */ Quatf inverse(); + /* 8049b550 */ Vector3f rotateVector(const Vector3f &); + /* */ Vector3f rotateVectorInv(const Vector3f &); + /* 8049b800 */ void slerpTo(const Quatf &, f32, Quatf &out) const; + /* 8049b800 */ void limitSlerpTo(const Quatf &, f32, f32, Quatf &out) const; + /* */ void makeVectorRotationLimit(Vector3f &, Vector3f &, f32); + /* 8049bbb0 */ void makeVectorRotation(Vector3f &, Vector3f &); + + f32 dot(const Quatf &q) const { + return q.Vector3f::dot(q) + w * q.w; + } + f32 squaredLength() const { + return w * w + x * x + y * y + z * z; + } + f32 length() const { + return Math::sqrt(squaredLength()); + } + void multScalar(f32 s) { + w *= s; + x *= s; + y *= s; + z *= s; + } + void setUnit() { + set(1.0f, 0.0f, 0.0f, 0.0f); + } + // union { + // Vector3f v; + // struct { + // f32 x, y, z; + // }; + // }; + f32 w; }; } // namespace EGG diff --git a/include/egg/math/eggVector.h b/include/egg/math/eggVector.h index bd14d0a6..e68059e6 100644 --- a/include/egg/math/eggVector.h +++ b/include/egg/math/eggVector.h @@ -1,46 +1,135 @@ #pragma once -#include "types.h" +#include "MSL_C/float.h" #include "egg/math/eggMath.h" -namespace EGG -{ +#include "nw4r/math/vec.h" +#include "types.h" -class Vector3f { -public: - f32 x,y,z; -public: - // may need change - Vector3f(const Vector3f& other) {x = other.x, y = other.y, z = other.z;} - // may need change - f32 squaredLength() { return x*x + y*y + z*z; } - f32 length() { return Math::sqrt(squaredLength()); } - /* 8049bcc0 */ void normalise(); - /* 8049bd50 */ void setLength(Vector3f& src, f32 length); - /* 8049be10 */ void setLength(f32 length); +namespace EGG { -public: - /* 80674c30 */ static Vector3f zero; - /* 80674c3c */ static Vector3f ex; - /* 80674c48 */ static Vector3f ey; - /* 80674c54 */ static Vector3f ez; +struct Vector3f : public nw4r::math::VEC3 { + Vector3f() {} // __ct__Q23EGG8Vector3fFv + Vector3f(f32 fx, f32 fy, f32 fz) : VEC3(fx, fy, fz) {} // __ct__Q23EGG8Vector3fFfff + + // ~Vector3f() {} + + f32 &operator()(int i) { // __cl__Q23EGG8Vector3fFi + return ((f32 *)this)[i]; + } + Vector3f operator*(f32 f) const { // __ml__Q23EGG8Vector3fCFf + return Vector3f(x * f, y * f, z * f); + } + friend Vector3f operator*(f32 f, const Vector3f &v) { + return v.operator*(f); + } + + Vector3f operator+(const Vector3f &v) { // __pl__Q23EGG8Vector3fCFRCQ23EGG8Vector3f + return Vector3f(x + v.x, y + v.y, z + v.z); + } + + Vector3f &operator+=(const Vector3f &v) { // __apl__Q23EGG8Vector3fFRCQ23EGG8Vector3f + x += v.x; + y += v.y; + z += v.z; + return *this; + } + + Vector3f operator-() const { // __mi__Q23EGG8Vector3fCFv + return Vector3f(-1.0f * x, -1.0f * y, -1.0f * z); + } + + Vector3f operator-(const Vector3f &v) { // __mi__Q23EGG8Vector3fCFRCQ23EGG8Vector3f + return Vector3f(x - v.x, y - v.y, z - v.z); + } + + Vector3f &operator-=(const Vector3f &v) { // __ami__Q23EGG8Vector3fFRCQ23EGG8Vector3f + x -= v.x; + y -= v.y; + z -= v.z; + return *this; + } + + Vector3f &operator*=(f32 f) { // __amu__Q23EGG8Vector3fFf + multScalar(f); + return *this; + } + + Vector3f operator/(f32 f) const { // __dv__Q23EGG8Vector3fCFf + return Vector3f(x / f, y / f, z / f); + } + + Vector3f &operator/=(f32 f) { // __adv__Q23EGG8Vector3fCFf // assumed + divScalar(f); + return *this; + } + + bool operator!=(const Vector3f &v) { // __ne__Q23EGG8Vector3fCFRCQ23EGG8Vector3f + return x != v.x || y != v.y || z != v.z; + } + + /* 8049bcc0 */ f32 normalise(); + /* 8049bd50 */ f32 setLength(const Vector3f &src, f32 len); + /* 8049be10 */ f32 setLength(f32 len); + + void multScalar(f32 f) { + x *= f; + y *= f; + z *= f; + } + void divScalar(f32 f) { + multScalar(1.0f / f); + } + f32 dot(const Vector3f &v) const { // dot__Q23EGG8Vector3fCFRCQ23EGG8Vector3f + return x * v.x + y * v.y + z * v.z; + } + Vector3f cross(const Vector3f &b) const { // cross__Q23EGG8Vector3fCFRCQ23EGG8Vector3f + Vector3f v; + f32 _x = (y * b.z) - (z * b.y); + f32 _y = (z * b.x) - (x * b.z); + f32 _z = (x * b.y) - (y * b.x); + v.set(_x, _y, _z); + return v; + } + f32 squaredLength() const { + return (x * x + y * y + z * z); + } + f32 length() const { + return Math::sqrt(squaredLength()); + } + void set(f32 fx, f32 fy, f32 fz) { + x = fx; + y = fy; + z = fz; + } + + /* 80674c30 */ static const Vector3f zero; + /* 80674c3c */ static const Vector3f ex; + /* 80674c48 */ static const Vector3f ey; + /* 80674c54 */ static const Vector3f ez; }; -struct Vector2f { - f32 x,y; +struct Vector2f : nw4r::math::VEC2 { + Vector2f(f32 fx, f32 fy) : VEC2(fx, fy) {} public: - /* 805767c0 */ static Vector2f zero; - /* 805767c8 */ static Vector2f ex; - /* 805767d0 */ static Vector2f ey; + /* 805767c0 */ static const Vector2f zero; + /* 805767c8 */ static const Vector2f ex; + /* 805767d0 */ static const Vector2f ey; }; struct Vector3s { - s16 x,y,z; + s16 x, y, z; + public: - /* 805767d8 */ static Vector3s zero; - /* 805767e0 */ static Vector3s ex; - /* 805767e8 */ static Vector3s ey; - /* 805767f0 */ static Vector3s ez; + Vector3s(s16 sx, s16 sy, s16 sz) { + x = sx; + y = sy; + z = sz; + } + /* 805767d8 */ static const Vector3s zero; + /* 805767e0 */ static const Vector3s ex; + /* 805767e8 */ static const Vector3s ey; + /* 805767f0 */ static const Vector3s ez; }; -} // namespace EGG +} // namespace EGG \ No newline at end of file diff --git a/include/m/m_vec.h b/include/m/m_vec.h index 6f016e58..f091412b 100644 --- a/include/m/m_vec.h +++ b/include/m/m_vec.h @@ -2,26 +2,24 @@ #include "types.h" #include "rvl/MTX.h" -#include "nw4r/math/vec.h" +#include "egg/math/eggVector.h" -class mVec3_c { +class mVec3_c : public EGG::Vector3f { public: - f32 x; - f32 y; - f32 z; /// @brief Constructs an empty vector. mVec3_c() {} /// @brief Constructs a vector from a float array. mVec3_c(const f32 *p) { x = p[0]; y = p[1]; z = p[2]; } - + mVec3_c(const mVec3_c& other) {x = other.x; y = other.y; z = other.z;} /// @brief Constructs a vector from three floating point values. mVec3_c(f32 fx, f32 fy, f32 fz) { x = fx; y = fy; z = fz; } void set(f32 fx, f32 fy, f32 fz) { x = fx; y = fy; z = fz; } - mVec3_c& operator=(mVec3_c& r) { x = r.x; y = r.y; z = r.z; return *this;} + mVec3_c& operator=(const mVec3_c& r) { x = r.x; y = r.y; z = r.z; return *this;} + /// @brief Constructs a new vector from an existing vector from the MTX library. mVec3_c(const Vec &v) { x = v.x; y = v.y; z = v.z; } diff --git a/include/nw4r/math/vec.h b/include/nw4r/math/vec.h index 7645dc18..1c868b29 100644 --- a/include/nw4r/math/vec.h +++ b/include/nw4r/math/vec.h @@ -2,14 +2,147 @@ #include "nw4r/nw4r_types.h" +// struct Vec { +// f32 x,y,z; +// }; + namespace nw4r { namespace math { - class VEC3 { - public: - f32 x,y,z; + // forward decl + struct _VEC2; struct VEC2; + struct _VEC3; struct VEC3; + struct _QUAT; struct QUAT; + struct _MTX33; struct MTX33; + struct _MTX34; struct MTX34; + struct _MTX44; struct MTX44; + void VEC3Add(VEC3* pOut, const VEC3* p1, const VEC3* p2); + void VEC3Sub(VEC3* pOut, const VEC3* p1, const VEC3* p2); + void VEC3Scale(VEC3* pOut, const VEC3* p, f32 scale); + f32 VEC3Len(const VEC3* p); + f32 VEC3LenSq(const VEC3* p); + f32 VEC3DistSq(const VEC3* p1, const VEC3* p2); + void VEC3Normalize(VEC3* pOut, const VEC3* p); + f32 VEC3Dot(const VEC3* p1, const VEC3* p2); + void VEC3Cross(VEC3* pOut, const VEC3* p1, const VEC3* p2); + void VEC3Lerp(VEC3* pOut, const VEC3* p1, const VEC3* p2, f32 t); + void VEC3Transform(VEC3* pOut, const MTX34* pM, const VEC3* pV); + void VEC3TransformCoord(VEC3* pOut, const MTX34* pM, const VEC3* pV); + + struct _VEC2 {f32 x, y;}; + struct VEC2 : _VEC2 { + VEC2() {} + VEC2(f32 fx, f32 fy) { x = fx; y = fy;} }; + + struct _VEC3 { f32 x,y,z; }; + struct VEC3 : public _VEC3 { + VEC3() {} + VEC3(f32 fx, f32 fy, f32 fz) { x = fx; y = fy; z = fz;} + // VEC3(const Vec& v) { x = v.x; y = v.y; z = v.z;} + VEC3(const f32* p) { x = p[0]; x = p[1]; z = p[2];} + VEC3(const _VEC3& v) { x = v.x; y = v.y; z = v.z; } + + // operator Vec*() { return (Vec*)this; } + // operator const Vec*() const { return (const Vec*)this; } + + f32 LenSq() const { + return x*x + y*y + z*z; + } + + VEC3 operator+(const VEC3& rhs) const { + VEC3 tmp; + VEC3Add(&tmp, this, &rhs); + return tmp; + } + VEC3& operator+=(const VEC3& rhs) { + VEC3Add(this, this, &rhs); + return *this; + } + VEC3 operator-() const { + return VEC3(-x, -y, -z); + } + VEC3 operator-(const VEC3& rhs) const { + VEC3 tmp; + VEC3Sub(&tmp, this, &rhs); + return tmp; + } + VEC3& operator-=(const VEC3& rhs) { + VEC3Sub(this, this, &rhs); + return *this; + } + VEC3 operator*(f32 f) const { + VEC3 tmp; + VEC3Scale(&tmp, this, f); + return tmp; + } + VEC3& operator*=(f32 f) { + VEC3Scale(this, this, f); + return *this; + } + VEC3 operator/(f32 f) const { + f32 r = 1.0f / f; // DWARF has it + return operator*(r); + } + VEC3& operator/=(f32 f) { + return operator*=(1.0f / f); + } + bool operator!=(const VEC3& rhs) { + return x != rhs.x || y != rhs.y || z != rhs.z; + } + }; + inline void VEC3Add(register VEC3* pOut, register const VEC3* p1, register const VEC3* p2) { + asm { + psq_l f2, 0x0(p1), 0, 0 + psq_l f2, 0x0(p2), 0, 0 + ps_add f0, f2, f1 + psq_l f2, 0x8(p1), 1, 0 + psq_l f2, 0x8(p2), 1, 0 + psq_st f0, 0x0(pOut), 0, 0 + ps_add f0, f2, f1 + psq_st f0, 0x8(pOut), 1, 0 + } + } + inline void VEC3Sub(register VEC3* pOut, register const VEC3* p1, register const VEC3* p2) { + asm + { + psq_l f2, 0x0(p1), 0, 0 + psq_l f2, 0x0(p2), 0, 0 + ps_sub f0, f2, f1 + psq_l f2, 0x8(p1), 1, 0 + psq_l f2, 0x8(p2), 1, 0 + psq_st f0, 0x0(pOut), 0, 0 + ps_sub f0, f2, f1 + psq_st f0, 0x8(pOut), 1, 0 + } + } + inline void VEC3Scale(register VEC3* pOut, register const VEC3* p, register f32 scale) { + asm + { + psq_l f2, 0x0(p), 0, 0 + ps_muls0 f1, f2, scale + psq_l f2, 0x8(p), 0, 1 + psq_st f1, 0x0(pOut), 0, 0 + ps_muls0 f1, f2, scale + psq_st f1, 0x8(pOut), 1, 0 + } + } + inline float VEC3Dot(register const VEC3 * p1, register const VEC3 * p2) + { + register f32 a, b, d, c, e; + asm + { + psq_l a, 0x4(p1), 0, 0 + psq_l b, 0x4(p2), 0, 0 + ps_mul a, a, b + psq_l c, 0(p1), 1, 0 + psq_l d, 0(p2), 1, 0 + ps_madd b, c, d, a + ps_sum0 e, b, a, a + } + return e; + } } // namespace math - -} // namespace nw4r + +} // namespace nw4r \ No newline at end of file diff --git a/src/d/a/d_a_base.cpp b/src/d/a/d_a_base.cpp index 61dbfefd..5b3ad743 100644 --- a/src/d/a/d_a_base.cpp +++ b/src/d/a/d_a_base.cpp @@ -158,15 +158,29 @@ int dAcBase_c::create() { void dAcBase_c::postCreate(fBase_c::MAIN_STATE_e state) { if (state == SUCCESS) { - pos_copy = position; - copyRotation(); + pos_copy = position; + rot_copy = rotation; room_id_copy = roomid; } dBase_c::postCreate(state); } -int dAcBase_c::preDelete() {} -int dAcBase_c::preExecute() {} +int dAcBase_c::preDelete() { + +} + +int dAcBase_c::preExecute() { + if (dBase_c::preExecute() == NOT_READY) { + return NOT_READY; + } + if (actor_properties & 0x10000000) { + if (actor_properties & 0x40000000) { + return NOT_READY; + } + // TODO: Add event control + } + return SUCCEEDED; +} int dAcBase_c::execute() {} int dAcBase_c::actorExecute() {} int dAcBase_c::actorExecuteInEvent() {} diff --git a/src/egg/math/eggMatrix.cpp b/src/egg/math/eggMatrix.cpp new file mode 100644 index 00000000..2281b9af --- /dev/null +++ b/src/egg/math/eggMatrix.cpp @@ -0,0 +1,371 @@ +#pragma once + +#include +#include +#include + +namespace EGG { + +void Matrix34f::inverseTo(Matrix34f &to) const { + PSMTXInverse(m, to.m); +} + +void Matrix34f::inverseTransposeTo(Matrix34f &to) const { + PSMTXInvXpose(m, to.m); +} + +void Matrix34f::makeIdentity() { + makeZero(); + m[2][2] = 1.0f; + m[1][1] = 1.0f; + m[0][0] = 1.0f; +} // namespace EGG + +Matrix34f::Matrix34f(f32 xx, f32 xy, f32 xz, f32 xw, f32 yx, f32 yy, f32 yz, f32 yw, f32 zx, f32 zy, + f32 zz, f32 zw) { + m[0][0] = xx; + m[0][1] = xy; + m[0][2] = xz; + m[0][3] = xw; + m[1][0] = yx; + m[1][1] = yy; + m[1][2] = yz; + m[1][3] = yw; + m[2][0] = zx; + m[2][1] = zy; + m[2][2] = zz; + m[2][3] = zw; +} + +void Matrix34f::makeSRT(const Vector3f &s, const Vector3f &r, const Vector3f &t) { + const f32 sin[3] = {Math::sin(r.x), Math::sin(r.y), Math::sin(r.z)}; + const f32 cos[3] = {Math::cos(r.x), Math::cos(r.y), Math::cos(r.z)}; + + const f32 c0_c2 = cos[0] * cos[2]; + const f32 s0_s1 = sin[0] * sin[1]; + const f32 c0_s2 = cos[0] * sin[2]; + + m[0][0] = s.x * (cos[1] * cos[2]); + m[1][0] = s.x * (cos[1] * sin[2]); + m[2][0] = s.x * (-sin[1]); + + m[0][1] = s.y * ((s0_s1 * cos[2]) - c0_s2); + m[1][1] = s.y * ((s0_s1 * sin[2]) + c0_c2); + m[2][1] = s.y * (sin[0] * cos[1]); + + m[0][2] = s.z * ((c0_c2 * sin[1]) + (sin[0] * sin[2])); + m[1][2] = s.z * ((c0_s2 * sin[1]) - (sin[0] * cos[2])); + m[2][2] = s.z * (cos[0] * cos[1]); + + m[0][3] = t.x; + m[1][3] = t.y; + m[2][3] = t.z; +} + +void Matrix34f::makeRT(const Vector3f &r, const Vector3f &t) { + const f32 sin[3] = {Math::sin(r.x), Math::sin(r.y), Math::sin(r.z)}; + const f32 cos[3] = {Math::cos(r.x), Math::cos(r.y), Math::cos(r.z)}; + + const f32 c0_c2 = cos[0] * cos[2]; + const f32 s0_s1 = sin[0] * sin[1]; + const f32 c0_s2 = cos[0] * sin[2]; + + m[0][0] = (cos[1] * cos[2]); + m[1][0] = (cos[1] * sin[2]); + m[2][0] = (-sin[1]); + + m[0][1] = (s0_s1 * cos[2]) - c0_s2; + m[1][1] = (s0_s1 * sin[2]) + c0_c2; + m[2][1] = (sin[0] * cos[1]); + + m[0][2] = (c0_c2 * sin[1]) + (sin[0] * sin[2]); + m[1][2] = (c0_s2 * sin[1]) - (sin[0] * cos[2]); + m[2][2] = (cos[0] * cos[1]); + + m[0][3] = t.x; + m[1][3] = t.y; + m[2][3] = t.z; +} + +void Matrix34f::makeR(const Vector3f &r) { + const f32 sin[3] = {Math::sin(r.x), Math::sin(r.y), Math::sin(r.z)}; + const f32 cos[3] = {Math::cos(r.x), Math::cos(r.y), Math::cos(r.z)}; + + const f32 c0_c2 = cos[0] * cos[2]; + const f32 s0_s1 = sin[0] * sin[1]; + const f32 c0_s2 = cos[0] * sin[2]; + + m[0][0] = (cos[1] * cos[2]); + m[1][0] = (cos[1] * sin[2]); + m[2][0] = (-sin[1]); + + m[0][1] = (s0_s1 * cos[2]) - c0_s2; + m[1][1] = (s0_s1 * sin[2]) + c0_c2; + m[2][1] = (sin[0] * cos[1]); + + m[0][2] = (c0_c2 * sin[1]) + (sin[0] * sin[2]); + m[1][2] = (c0_s2 * sin[1]) - (sin[0] * cos[2]); + m[2][2] = (cos[0] * cos[1]); + + m[0][3] = 0.0f; + m[1][3] = 0.0f; + m[2][3] = 0.0f; +} + +void Matrix34f::makeST(const Vector3f &s, const Vector3f &t) { + m[0][0] = s.x; + m[1][0] = 0.0f; + m[2][0] = 0.0f; + m[0][1] = 0.0f; + m[1][1] = s.y; + m[2][1] = 0.0f; + m[0][2] = 0.0f; + m[1][2] = 0.0f; + m[2][2] = s.z; + m[0][3] = t.x; + m[1][3] = t.y; + m[2][3] = t.z; +} + +void Matrix34f::makeSQT(const Vector3f &s, const Quatf &q, const Vector3f &t) { + f32 yy = 2.0f * q.y * q.y; + f32 zz = 2.0f * q.z * q.z; + f32 xx = 2.0f * q.x * q.x; + f32 xy = 2.0f * q.x * q.y; + f32 xz = 2.0f * q.x * q.z; + f32 yz = 2.0f * q.y * q.z; + f32 wz = 2.0f * q.w * q.z; + f32 wx = 2.0f * q.w * q.x; + f32 wy = 2.0f * q.w * q.y; + + m[0][0] = s.x * (1.0f - yy - zz); + m[0][1] = s.y * (xy - wz); + m[0][2] = s.z * (xz + wy); + + m[1][0] = s.x * (xy + wz); + m[1][1] = s.y * (1.0f - xx - zz); + m[1][2] = s.z * (yz - wx); + + m[2][0] = s.x * (xz - wy); + m[2][1] = s.y * (yz + wx); + m[2][2] = s.z * (1.0f - xx - yy); + + m[0][3] = t.x; + m[1][3] = t.y; + m[2][3] = t.z; +} + +void Matrix34f::makeQT(const Quatf &q, const Vector3f &t) { + f32 yy = 2.0f * q.y * q.y; + f32 zz = 2.0f * q.z * q.z; + f32 xx = 2.0f * q.x * q.x; + f32 xy = 2.0f * q.x * q.y; + f32 xz = 2.0f * q.x * q.z; + f32 yz = 2.0f * q.y * q.z; + f32 wz = 2.0f * q.w * q.z; + f32 wx = 2.0f * q.w * q.x; + f32 wy = 2.0f * q.w * q.y; + + m[0][0] = 1.0f - yy - zz; + m[0][1] = xy - wz; + m[0][2] = xz + wy; + + m[1][0] = xy + wz; + m[1][1] = 1.0f - xx - zz; + m[1][2] = yz - wx; + + m[2][0] = xz - wy; + m[2][1] = yz + wx; + m[2][2] = 1 - xx - yy; + + m[0][3] = t.x; + m[1][3] = t.y; + m[2][3] = t.z; +} + +void Matrix34f::makeQ(const Quatf &q) { + f32 yy = 2.0f * q.y * q.y; + f32 zz = 2.0f * q.z * q.z; + f32 xx = 2.0f * q.x * q.x; + f32 xy = 2.0f * q.x * q.y; + f32 xz = 2.0f * q.x * q.z; + f32 yz = 2.0f * q.y * q.z; + f32 wz = 2.0f * q.w * q.z; + f32 wx = 2.0f * q.w * q.x; + f32 wy = 2.0f * q.w * q.y; + + m[0][0] = 1.0f - yy - zz; + m[0][1] = xy - wz; + m[0][2] = xz + wy; + + m[1][0] = xy + wz; + m[1][1] = 1.0f - xx - zz; + m[1][2] = yz - wx; + + m[2][0] = xz - wy; + m[2][1] = yz + wx; + m[2][2] = 1.0f - xx - yy; + + m[0][3] = 0.0f; + m[1][3] = 0.0f; + m[2][3] = 0.0f; +} + +void Matrix34f::makeS(const Vector3f &s) { + m[0][0] = s.x; + m[0][1] = 0.0f; + m[0][2] = 0.0f; + m[1][0] = 0.0f; + m[1][1] = s.y; + m[1][2] = 0.0f; + m[2][0] = 0.0f; + m[2][1] = 0.0f; + m[2][2] = s.z; + m[0][3] = 0.0f; + m[1][3] = 0.0f; + m[2][3] = 0.0f; +} + +void Matrix34f::makeT(const Vector3f &t) { + m[0][0] = 1.0f; + m[0][1] = 0.0f; + m[0][2] = 0.0f; + m[1][0] = 0.0f; + m[1][1] = 1.0f; + m[1][2] = 0.0f; + m[2][0] = 0.0f; + m[2][1] = 0.0f; + m[2][2] = 1.0f; + m[0][3] = t.x; + m[1][3] = t.y; + m[2][3] = t.z; +} + +void Matrix34f::fromQuat(const Quatf &q) { + m[0][0] = (1.0f - 2.0f * q.y * q.y - 2.0f * q.z * q.z); + m[0][1] = (2.0f * q.x * q.y) - (2.0f * q.w * q.z); + m[0][2] = (2.0f * q.x * q.z) + (2.0f * q.w * q.y); + + m[1][0] = (2.0f * q.x * q.y) + (2.0f * q.w * q.z); + m[1][1] = (1.0f - 2.0f * q.x * q.x) - (2.0f * q.z * q.z); + m[1][2] = (2.0f * q.y * q.z) - (2.0f * q.w * q.x); + + m[2][0] = (2.0f * q.x * q.z) - (2.0f * q.w * q.y); + m[2][1] = (2.0f * q.y * q.z) + (2.0f * q.w * q.x); + m[2][2] = (1.0f - 2.0f * q.x * q.x) - (2.0f * q.y * q.y); + + m[2][3] = 0.0f; + m[1][3] = 0.0f; + m[0][3] = 0.0f; +} + +void Matrix34f::toQuat(Quatf &q) const { + const f32 temp0 = (m[0][0] + m[1][1] + m[2][2] + 1.0f) * 0.25f; + const f32 temp1 = temp0 - (m[1][1] + m[2][2]) * 0.5f; + const f32 temp2 = temp0 - (m[2][2] + m[0][0]) * 0.5f; + const f32 temp3 = temp0 - (m[0][0] + m[1][1]) * 0.5f; + + // The fun method + // int tempMax = temp0 > temp1 ? + // (temp0 > temp2 ? (temp0 > temp3 ? 0 : 3) : (temp2 > temp3 ? 2 : 3)) : + // (temp1 > temp2 ? temp1 > temp3 ? 1 : 3 : (temp2 > temp3 ? 2 : 3)); + + int tempMax; + if (temp0 > temp1) { + if (temp0 > temp2) { + if (temp0 > temp3) { + tempMax = 0; + } else { + tempMax = 3; + } + } else { + if (temp2 > temp3) { + tempMax = 2; + } else { + tempMax = 3; + } + } + } else if (temp1 > temp2) { + if (temp1 > temp3) { + tempMax = 1; + } else { + tempMax = 3; + } + } else if (temp2 > temp3) { + tempMax = 2; + } else { + tempMax = 3; + } + + switch (tempMax) { + case 0: + q.w = Math::sqrt(temp0); + q.x = (0.25f / q.w) * (m[2][1] - m[1][2]); + q.y = (0.25f / q.w) * (m[0][2] - m[2][0]); + q.z = (0.25f / q.w) * (m[1][0] - m[0][1]); + break; + case 1: + q.x = Math::sqrt(temp1); + q.w = (0.25f / q.x) * (m[2][1] - m[1][2]); + q.y = (0.25f / q.x) * (m[0][1] + m[1][0]); + q.z = (0.25f / q.x) * (m[0][2] + m[2][0]); + break; + case 2: + q.y = Math::sqrt(temp2); + q.w = (0.25f / q.y) * (m[0][2] - m[2][0]); + q.z = (0.25f / q.y) * (m[1][2] + m[2][1]); + q.x = (0.25f / q.y) * (m[1][0] + m[0][1]); + break; + case 3: + q.z = Math::sqrt(temp3); + q.w = (0.25f / q.z) * (m[1][0] - m[0][1]); + q.x = (0.25f / q.z) * (m[2][0] + m[0][2]); + q.y = (0.25f / q.z) * (m[2][1] + m[1][2]); + break; + default: + break; + } + + if (q.w < 0.0f) { + q.w = -q.w; + q.x = -q.x; + q.y = -q.y; + q.z = -q.z; + } + q.multScalar(Math::inv(q.length())); +} + +void Matrix34f::slerpTo(const Matrix34f &m2, Matrix34f &out, f32 t) const { + Quatf q1, q2, q3; + m2.toQuat(q1); + toQuat(q2); + q2.slerpTo(q1, t, q3); + out.makeQ(q3); +} + +void Matrix34f::setAxisRotation(const Vector3f &axis, f32 rot) { + Quatf q; + q.setAxisRotation(axis, rot); + makeQ(q); +} + +Vector3f Matrix34f::multVector(const Vector3f &vec) const { + Vector3f ret; + multVectorTo(vec, ret); + return ret; +} + +void Matrix34f::loadPosMtx(u32 posMtxId) { + GXLoadPosMtxImm(m, posMtxId); +} + +void Matrix34f::multiplyTo(const Matrix34f &m2, Matrix34f &to) const { + PSMTXConcat(m, m2.m, to.m); +} + +void Matrix34f::dump() {} + +const Matrix34f Matrix34f::ident(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, + 0.0f); + +} // namespace EGG diff --git a/src/egg/math/eggQuat.cpp b/src/egg/math/eggQuat.cpp new file mode 100644 index 00000000..066308b2 --- /dev/null +++ b/src/egg/math/eggQuat.cpp @@ -0,0 +1,235 @@ +#include + +namespace EGG { + +/* 8049b390 */ +void Quatf::set(f32 fw, f32 fx, f32 fy, f32 fz) { + w = fw; + x = fx; + y = fy; + z = fz; +} + +void Quatf::set(f32 fw, const Vector3f &vec) { + w = fw; + (Vector3f &)*this = vec; +} + +/* NOT IN SS */ +void Quatf::setRPY(const EGG::Vector3f &rpy) { + const f32 cy = Math::cos(rpy.z * 0.5f); + const f32 cp = Math::cos(rpy.y * 0.5f); + const f32 cr = Math::cos(rpy.x * 0.5f); + const f32 sy = Math::sin(rpy.z * 0.5f); + const f32 sp = Math::sin(rpy.y * 0.5f); + const f32 sr = Math::sin(rpy.x * 0.5f); + + const f32 cy_cp = cy * cp; + const f32 sy_sp = sy * sp; + const f32 cy_sp = cy * sp; + const f32 sy_cp = sy * cp; + + w = (cy_cp * cr) + (sy_sp * sr); + x = (cy_cp * sr) - (sy_sp * cr); + y = (cy_sp * cr) + (sy_cp * sr); + z = (sy_cp * cr) - (cy_sp * sr); +} + +/* NOT IN SS */ +// Vector3f Quatf::calcRPY() { +// Vector3f rpy; + +// const f32 ww = w * w; +// const f32 yy = v.y * v.y; +// const f32 xx = v.x * v.x; +// const f32 zz = v.z * v.z; + +// const f32 a0 = (ww + xx) - yy - zz; +// const f32 a1 = (v.x * v.y + w * v.z) * 2.0f; +// const f32 a2 = (v.x * v.z - w * v.y) * 2.0f; +// const f32 a3 = (v.y * v.z + w * v.x) * 2.0f; +// const f32 a4 = (ww - xx) - yy + zz; + +// const f32 a5 = Math::abs(a2); + +// if (a5 > 0.999999f) { +// f32 t0 = (v.x * v.y - w * v.z) * 2.0f; +// f32 t1 = (v.x * v.z + w * v.y) * 2.0f; + +// rpy.x = 0.0f; +// rpy.y = a2 / a5 * -Math::pi_half(); +// rpy.z = Math::atan2(-t0, -a2 * t1); +// } else { +// rpy.x = Math::atan2(a3, a4); +// rpy.y = Math::asin(-a2); +// rpy.z = Math::atan2(a1, a0); +// } + +// return rpy; +// } + +/* NOT IN SS */ +void Quatf::setRPY(f32 roll, f32 pitch, f32 yaw) { + setRPY(Vector3f(roll, pitch, yaw)); +} + +/* 8049b3b0 */ +void Quatf::setAxisRotation(const Vector3f &axis, f32 rot) { + 2.0f; // force sdata2 ordering + const f32 half_angle = rot * 0.5f; + const f32 cos = Math::cos(half_angle); + const f32 sin = Math::sin(half_angle); + set(cos, sin * axis.x, sin * axis.y, sin * axis.z); +} + +/* 8049b450 */ +f32 Quatf::norm() { + return w * w + Vector3f::dot(*this); +} + +/* 8049b480 */ +void Quatf::normalise() { + f32 mag = Math::sqrt(norm()); + if (mag > 0.0f) { + multScalar(Math::inv(mag)); + } +} + +/* 8049b500 */ +Quatf Quatf::conjugate() { + Quatf q; + q.w = w; + (Vector3f &)q = -1.0f * *this; + return q; +} + +/* 8049b550 */ +Vector3f Quatf::rotateVector(const Vector3f &vec) { + Quatf conj, mult; + conj = conjugate(); + mult = *this * vec; + mult = mult * conj; + return (mult); +} + +// /* NOT IN SS */ +// Quatf operator*(const Quatf &q, const Vector3f &v) { +// Vector3f crossed = q.v.cross(v); +// Vector3f scaled = q.w * v; +// Quatf ret = Quatf(-q.v.dot(v), crossed + scaled); +// return ret; +// } + +/* 8049b800 */ +void Quatf::slerpTo(const Quatf &q2, f32 t, Quatf &out) const { + f32 dot = x * q2.x + y * q2.y + z * q2.z + w * q2.w; + + if (dot > 1.0f) { + dot = 1.0f; + } else if (dot < -1.0f) { + dot = -1.0f; + } + + bool lt0; + if (dot < 0.0) { + dot = -dot; + lt0 = true; + } else { + lt0 = false; + } + + const f32 theta_0 = Math::acos(dot); + const f32 sin_theta_0 = Math::sin(theta_0); + + f32 a, b; + if (Math::abs(sin_theta_0) < 1e-5f) { + a = 1.0f - t; + b = t; + } else { + const f32 sin_theta_0_inv = 1.0f / sin_theta_0; + const f32 theta = t * theta_0; + a = sin_theta_0_inv * Math::sin(theta_0 - theta); + b = sin_theta_0_inv * Math::sin(theta); + } + + if (lt0) { + b = -b; + } + + out.x = a * x + b * q2.x; + out.y = a * y + b * q2.y; + out.z = a * z + b * q2.z; + out.w = a * w + b * q2.w; +} + +/* 8049b9d0 */ +void Quatf::limitSlerpTo(const Quatf &q2, f32 t, f32 t2, Quatf &out) const { + t2 *= 0.5f; + + f32 dot = x * q2.x + y * q2.y + z * q2.z + w * q2.w; + + if (dot > 1.0f) { + dot = 1.0f; + } else if (dot < -1.0f) { + dot = -1.0f; + } + + bool lt0; + if (dot < 0.0) { + dot = -dot; + lt0 = true; + } else { + lt0 = false; + } + + const f32 theta_0 = Math::acos(dot); + if (theta_0 > t2) { + t = t2 / theta_0; + } + const f32 sin_theta_0 = Math::sin(theta_0); + + f32 a, b; + + if (Math::abs(sin_theta_0) < 1e-5f) { + a = 1.0f - t; + b = t; + } else { + const f32 sin_theta_0_inv = 1.0f / sin_theta_0; + const f32 theta = t * theta_0; + a = sin_theta_0_inv * Math::sin(theta_0 - theta); + b = sin_theta_0_inv * Math::sin(theta); + } + + if (lt0) { + b = -b; + } + + out.x = a * x + b * q2.x; + out.y = a * y + b * q2.y; + out.z = a * z + b * q2.z; + out.w = a * w + b * q2.w; +} + +/* 8049bbb0 */ +void Quatf::makeVectorRotation(Vector3f &from, Vector3f &to) { + // f32 dot = from.dot(to); + + Vector3f cross = from.cross(to); + + f32 t0 = (from.dot(to) + 1.0f) * 2.0f; + + f32 v = t0; + if (v <= 0.0f) { + v = 0.0f; + } + t0 = Math::sqrt(v); + + if (t0 <= Math::epsilon()) { + setUnit(); + } else { + f32 inv = Math::inv(t0); + set(t0 * 0.5f, cross.x * inv, cross.y * inv, cross.z * inv); + } +} + +} // namespace EGG diff --git a/src/egg/math/eggVector.cpp b/src/egg/math/eggVector.cpp new file mode 100644 index 00000000..f90a7e4c --- /dev/null +++ b/src/egg/math/eggVector.cpp @@ -0,0 +1,60 @@ +#include + +namespace EGG { + +const Vector2f Vector2f::zero(0.0f, 0.0f); +const Vector2f Vector2f::ex(1.0f, 0.0f); +const Vector2f Vector2f::ey(0.0f, 1.0f); + +const Vector3f Vector3f::zero(0.0f, 0.0f, 0.0f); +const Vector3f Vector3f::ex(1.0f, 0.0f, 0.0f); +const Vector3f Vector3f::ey(0.0f, 1.0f, 0.0f); +const Vector3f Vector3f::ez(0.0f, 0.0f, 1.0f); + +const Vector3s Vector3s::zero(0, 0, 0); +const Vector3s Vector3s::ex(1, 0, 0); +const Vector3s Vector3s::ey(0, 1, 0); +const Vector3s Vector3s::ez(0, 0, 1); + +f32 Vector3f::normalise() { + f32 len = length(); + if (len > 0.0f) { + x *= 1.0f / len; + y *= 1.0f / len; + z *= 1.0f / len; + } + return len; +} + +f32 Vector3f::setLength(const Vector3f &src, f32 len) { + const f32 mag = src.length(); + + if (mag <= FLT_EPSILON) { + z = 0.0f; + y = 0.0f; + x = 0.0f; + return 0.0f; + } + + x = src.x * (len / mag); + y = src.y * (len / mag); + z = src.z * (len / mag); + + return mag; +} + +f32 Vector3f::setLength(f32 len) { + const f32 mag = length(); + + if (mag <= FLT_EPSILON) { + return 0.0f; + } + + x *= len / mag; + y *= len / mag; + z *= len / mag; + + return mag; +} + +} // namespace EGG \ No newline at end of file