mirror of
https://github.com/zeldaret/ss
synced 2026-07-08 13:56:15 -04:00
mVec and Egg Math (Quat, Vector3f, Matrix34f)
This commit is contained in:
elijah-thomas774
@@ -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;
|
||||
|
||||
@@ -2,19 +2,56 @@
|
||||
|
||||
#include "types.h"
|
||||
|
||||
namespace EGG
|
||||
{
|
||||
namespace EGG {
|
||||
|
||||
template <typename T>
|
||||
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<f32>::zero
|
||||
// Math<f32>::pi_half
|
||||
// Math<f32>::neg(f32)
|
||||
// Math<f32>::abs(f32)
|
||||
|
||||
// f32 impls
|
||||
// /* 8049ab60 */ Math<f32>::sqrt(f32);
|
||||
// /* 8049abb0 */ Math<f32>::sin(f32);
|
||||
|
||||
@@ -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
|
||||
|
||||
+65
-16
@@ -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<f32>::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
|
||||
|
||||
+120
-31
@@ -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<f32>::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<f32>::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
|
||||
+5
-7
@@ -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; }
|
||||
|
||||
+138
-5
@@ -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
|
||||
+18
-4
@@ -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() {}
|
||||
|
||||
@@ -0,0 +1,371 @@
|
||||
#pragma once
|
||||
|
||||
#include <egg/math/eggMatrix.h>
|
||||
#include <rvl/GX.h>
|
||||
#include <rvl/MTX.h>
|
||||
|
||||
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<f32>::sin(r.x), Math<f32>::sin(r.y), Math<f32>::sin(r.z)};
|
||||
const f32 cos[3] = {Math<f32>::cos(r.x), Math<f32>::cos(r.y), Math<f32>::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<f32>::sin(r.x), Math<f32>::sin(r.y), Math<f32>::sin(r.z)};
|
||||
const f32 cos[3] = {Math<f32>::cos(r.x), Math<f32>::cos(r.y), Math<f32>::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<f32>::sin(r.x), Math<f32>::sin(r.y), Math<f32>::sin(r.z)};
|
||||
const f32 cos[3] = {Math<f32>::cos(r.x), Math<f32>::cos(r.y), Math<f32>::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<f32>::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<f32>::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<f32>::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<f32>::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<f32>::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
|
||||
@@ -0,0 +1,235 @@
|
||||
#include <egg/math/eggQuat.h>
|
||||
|
||||
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<f32>::cos(rpy.z * 0.5f);
|
||||
const f32 cp = Math<f32>::cos(rpy.y * 0.5f);
|
||||
const f32 cr = Math<f32>::cos(rpy.x * 0.5f);
|
||||
const f32 sy = Math<f32>::sin(rpy.z * 0.5f);
|
||||
const f32 sp = Math<f32>::sin(rpy.y * 0.5f);
|
||||
const f32 sr = Math<f32>::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<f32>::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<f32>::pi_half();
|
||||
// rpy.z = Math<f32>::atan2(-t0, -a2 * t1);
|
||||
// } else {
|
||||
// rpy.x = Math<f32>::atan2(a3, a4);
|
||||
// rpy.y = Math<f32>::asin(-a2);
|
||||
// rpy.z = Math<f32>::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<f32>::cos(half_angle);
|
||||
const f32 sin = Math<f32>::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<f32>::sqrt(norm());
|
||||
if (mag > 0.0f) {
|
||||
multScalar(Math<f32>::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<f32>::acos(dot);
|
||||
const f32 sin_theta_0 = Math<f32>::sin(theta_0);
|
||||
|
||||
f32 a, b;
|
||||
if (Math<f32>::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<f32>::sin(theta_0 - theta);
|
||||
b = sin_theta_0_inv * Math<f32>::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<f32>::acos(dot);
|
||||
if (theta_0 > t2) {
|
||||
t = t2 / theta_0;
|
||||
}
|
||||
const f32 sin_theta_0 = Math<f32>::sin(theta_0);
|
||||
|
||||
f32 a, b;
|
||||
|
||||
if (Math<f32>::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<f32>::sin(theta_0 - theta);
|
||||
b = sin_theta_0_inv * Math<f32>::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<f32>::sqrt(v);
|
||||
|
||||
if (t0 <= Math<f32>::epsilon()) {
|
||||
setUnit();
|
||||
} else {
|
||||
f32 inv = Math<f32>::inv(t0);
|
||||
set(t0 * 0.5f, cross.x * inv, cross.y * inv, cross.z * inv);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace EGG
|
||||
@@ -0,0 +1,60 @@
|
||||
#include <egg/math/eggVector.h>
|
||||
|
||||
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
|
||||
Reference in New Issue
Block a user