mirror of https://github.com/zeldaret/botw.git
ksys/phys: Add RigidBodyAccessor
This commit is contained in:
Léo Lam
parent
3162c0c85c
commit
6ef3bb9327
|
|
@ -83519,19 +83519,19 @@ Address,Quality,Size,Name
|
|||
0x0000007100fa9c4c,U,000068,
|
||||
0x0000007100fa9c90,U,000288,
|
||||
0x0000007100fa9db0,U,000092,
|
||||
0x0000007100fa9e0c,U,000024,
|
||||
0x0000007100fa9e24,U,000004,nullsub_4237
|
||||
0x0000007100fa9e28,U,000004,j__ZdlPv_1001
|
||||
0x0000007100fa9e2c,U,000044,
|
||||
0x0000007100fa9e58,U,000032,
|
||||
0x0000007100fa9e78,U,000016,
|
||||
0x0000007100fa9e88,U,000112,physicsXXXGetMtx
|
||||
0x0000007100fa9ef8,U,000032,physicsXXXGetVelocity
|
||||
0x0000007100fa9f18,U,000032,
|
||||
0x0000007100fa9f38,U,000116,
|
||||
0x0000007100fa9fac,U,000120,
|
||||
0x0000007100faa024,U,000020,
|
||||
0x0000007100faa038,U,000064,
|
||||
0x0000007100fa9e0c,O,000024,_ZN4ksys4phys17RigidBodyAccessorC1EP12hkpRigidBody
|
||||
0x0000007100fa9e24,O,000004,_ZN4ksys4phys17RigidBodyAccessorD1Ev
|
||||
0x0000007100fa9e28,O,000004,_ZN4ksys4phys17RigidBodyAccessorD0Ev
|
||||
0x0000007100fa9e2c,O,000044,_ZNK4ksys4phys17RigidBodyAccessor13getMotionTypeEv
|
||||
0x0000007100fa9e58,O,000032,_ZNK4ksys4phys17RigidBodyAccessor11getPositionEPN4sead7Vector3IfEE
|
||||
0x0000007100fa9e78,O,000016,_ZNK4ksys4phys17RigidBodyAccessor11getRotationEPN4sead4QuatIfEE
|
||||
0x0000007100fa9e88,O,000112,_ZNK4ksys4phys17RigidBodyAccessor12getTransformEPN4sead8Matrix34IfEE
|
||||
0x0000007100fa9ef8,O,000032,_ZNK4ksys4phys17RigidBodyAccessor17getLinearVelocityEPN4sead7Vector3IfEE
|
||||
0x0000007100fa9f18,O,000032,_ZNK4ksys4phys17RigidBodyAccessor18getAngularVelocityEPN4sead7Vector3IfEE
|
||||
0x0000007100fa9f38,O,000116,_ZNK4ksys4phys17RigidBodyAccessor22isVelocityGreaterEqualEf
|
||||
0x0000007100fa9fac,O,000120,_ZNK4ksys4phys17RigidBodyAccessor16getPointVelocityEPN4sead7Vector3IfEERKS4_
|
||||
0x0000007100faa024,O,000020,_ZNK4ksys4phys17RigidBodyAccessor13getTimeFactorEv
|
||||
0x0000007100faa038,O,000064,_ZNK4ksys4phys17RigidBodyAccessor20getDeltaCenterOfMassEPN4sead7Vector3IfEES5_
|
||||
0x0000007100faa078,U,000564,
|
||||
0x0000007100faa2ac,U,000224,
|
||||
0x0000007100faa3bc,U,000364,
|
||||
|
|
|
|||
|
Can't render this file because it is too large.
|
|
|
@ -6,7 +6,44 @@ class hkMatrix3f {
|
|||
public:
|
||||
hkMatrix3f() {} // NOLINT(modernize-use-equals-default)
|
||||
|
||||
HK_FORCE_INLINE hkFloat32& operator()(int row, int col);
|
||||
HK_FORCE_INLINE const hkFloat32& operator()(int row, int col) const;
|
||||
|
||||
HK_FORCE_INLINE hkVector4f& getColumn(int i);
|
||||
HK_FORCE_INLINE const hkVector4f& getColumn(int i) const;
|
||||
|
||||
HK_FORCE_INLINE void getRows(hkVector4f& r0, hkVector4f& r1, hkVector4f& r2) const;
|
||||
|
||||
hkVector4f m_col0;
|
||||
hkVector4f m_col1;
|
||||
hkVector4f m_col2;
|
||||
};
|
||||
|
||||
hkFloat32& hkMatrix3f::operator()(int row, int col) {
|
||||
return getColumn(col)(row);
|
||||
}
|
||||
|
||||
const hkFloat32& hkMatrix3f::operator()(int row, int col) const {
|
||||
return getColumn(col)(row);
|
||||
}
|
||||
|
||||
hkVector4f& hkMatrix3f::getColumn(int i) {
|
||||
return (&m_col0)[i];
|
||||
}
|
||||
|
||||
const hkVector4f& hkMatrix3f::getColumn(int i) const {
|
||||
return (&m_col0)[i];
|
||||
}
|
||||
|
||||
inline void hkMatrix3f::getRows(hkVector4f& r0, hkVector4f& r1, hkVector4f& r2) const {
|
||||
hkVector4f c0;
|
||||
c0.set(m_col0(0), m_col1(0), m_col2(0));
|
||||
hkVector4f c1;
|
||||
c1.set(m_col0(1), m_col1(1), m_col2(1));
|
||||
hkVector4f c2;
|
||||
c2.set(m_col0(2), m_col1(2), m_col2(2));
|
||||
|
||||
r0 = c0;
|
||||
r1 = c1;
|
||||
r2 = c2;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -5,6 +5,16 @@
|
|||
class hkQuaternionf {
|
||||
public:
|
||||
hkQuaternionf() {} // NOLINT(modernize-use-equals-default)
|
||||
HK_FORCE_INLINE hkQuaternionf(hkFloat32 ix, hkFloat32 iy, hkFloat32 iz, hkFloat32 r);
|
||||
HK_FORCE_INLINE void set(hkFloat32 ix, hkFloat32 iy, hkFloat32 iz, hkFloat32 r);
|
||||
|
||||
hkVector4f m_vec;
|
||||
};
|
||||
|
||||
inline hkQuaternionf::hkQuaternionf(hkFloat32 ix, hkFloat32 iy, hkFloat32 iz, hkFloat32 r) {
|
||||
set(ix, iy, iz, r);
|
||||
}
|
||||
|
||||
inline void hkQuaternionf::set(hkFloat32 ix, hkFloat32 iy, hkFloat32 iz, hkFloat32 r) {
|
||||
m_vec.set(ix, iy, iz, r);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -23,6 +23,7 @@ public:
|
|||
HK_FORCE_INLINE hkVector4f() {}
|
||||
HK_FORCE_INLINE hkVector4f(hkFloat32 x, hkFloat32 y, hkFloat32 z, hkFloat32 w = 0);
|
||||
HK_FORCE_INLINE hkVector4f(const hkVector4f& other);
|
||||
HK_FORCE_INLINE hkVector4f& operator=(hkVector4fParameter) = default;
|
||||
|
||||
// ========== Vector initialization
|
||||
|
||||
|
|
@ -75,8 +76,23 @@ public:
|
|||
|
||||
// ========== Comparisons
|
||||
|
||||
HK_FORCE_INLINE hkVector4fComparison less(hkVector4fParameter a) const;
|
||||
HK_FORCE_INLINE hkVector4fComparison lessEqual(hkVector4fParameter a) const;
|
||||
HK_FORCE_INLINE hkVector4fComparison greater(hkVector4fParameter a) const;
|
||||
HK_FORCE_INLINE hkVector4fComparison greaterEqual(hkVector4fParameter a) const;
|
||||
HK_FORCE_INLINE hkVector4fComparison equal(hkVector4fParameter a) const;
|
||||
HK_FORCE_INLINE hkVector4fComparison notEqual(hkVector4fParameter a) const;
|
||||
HK_FORCE_INLINE hkVector4fComparison lessZero() const;
|
||||
HK_FORCE_INLINE hkVector4fComparison lessEqualZero() const;
|
||||
HK_FORCE_INLINE hkVector4fComparison greaterZero() const;
|
||||
HK_FORCE_INLINE hkVector4fComparison greaterEqualZero() const;
|
||||
HK_FORCE_INLINE hkVector4fComparison equalZero() const;
|
||||
HK_FORCE_INLINE hkVector4fComparison notEqualZero() const;
|
||||
|
||||
// ========== Sign, comparisons, clamping
|
||||
|
||||
void setAbs(hkVector4fParameter a);
|
||||
|
||||
// ========== Matrix operations (out-of-line)
|
||||
|
||||
void setRotatedDir(const hkMatrix3f& a, hkVector4fParameter b);
|
||||
|
|
@ -93,8 +109,21 @@ public:
|
|||
|
||||
// ========== Component access
|
||||
|
||||
hkSimdFloat32 operator()(int i) const { return v[i]; }
|
||||
hkSimdFloat32 operator[](int i) const { return v[i]; }
|
||||
hkFloat32& operator()(int i) { return reinterpret_cast<float*>(&v)[i]; }
|
||||
hkFloat32& operator[](int i) { return reinterpret_cast<float*>(&v)[i]; }
|
||||
const hkFloat32& operator()(int i) const { return reinterpret_cast<const float*>(&v)[i]; }
|
||||
const hkFloat32& operator[](int i) const { return reinterpret_cast<const float*>(&v)[i]; }
|
||||
|
||||
hkSimdFloat32 getComponent(int i) const { return v[i]; }
|
||||
hkSimdFloat32 getX() const { return getComponent(0); }
|
||||
hkSimdFloat32 getY() const { return getComponent(1); }
|
||||
hkSimdFloat32 getZ() const { return getComponent(2); }
|
||||
hkSimdFloat32 getW() const { return getComponent(3); }
|
||||
void setComponent(int i, hkSimdFloat32Parameter val) { v[i] = val; }
|
||||
void setX(hkSimdFloat32Parameter val) { setComponent(0, val); }
|
||||
void setY(hkSimdFloat32Parameter val) { setComponent(1, val); }
|
||||
void setZ(hkSimdFloat32Parameter val) { setComponent(2, val); }
|
||||
void setW(hkSimdFloat32Parameter val) { setComponent(3, val); }
|
||||
|
||||
// ========== Load/store
|
||||
|
||||
|
|
|
|||
|
|
@ -63,7 +63,7 @@ inline void hkVector4f::mul(hkSimdFloat32Parameter a) {
|
|||
|
||||
inline void hkVector4f::setMul(hkVector4fParameter a, hkSimdFloat32Parameter r) {
|
||||
#ifdef HK_VECTOR4F_AARCH64_NEON
|
||||
v = vmulq_n_f32(v, r);
|
||||
v = vmulq_n_f32(a.v, r);
|
||||
#else
|
||||
v *= r.val();
|
||||
#endif
|
||||
|
|
@ -75,7 +75,7 @@ inline void hkVector4f::setMul(hkSimdFloat32Parameter r, hkVector4fParameter a)
|
|||
|
||||
inline void hkVector4f::setAdd(hkVector4fParameter a, hkSimdFloat32Parameter b) {
|
||||
#ifdef HK_VECTOR4F_AARCH64_NEON
|
||||
v = vaddq_f32(v, vdupq_n_f32(b));
|
||||
v = vaddq_f32(a.v, vdupq_n_f32(b));
|
||||
#else
|
||||
v += b.val();
|
||||
#endif
|
||||
|
|
@ -83,7 +83,7 @@ inline void hkVector4f::setAdd(hkVector4fParameter a, hkSimdFloat32Parameter b)
|
|||
|
||||
inline void hkVector4f::setSub(hkVector4fParameter a, hkSimdFloat32Parameter b) {
|
||||
#ifdef HK_VECTOR4F_AARCH64_NEON
|
||||
v = vsubq_f32(v, vdupq_n_f32(b));
|
||||
v = vsubq_f32(a.v, vdupq_n_f32(b));
|
||||
#else
|
||||
v -= b.val();
|
||||
#endif
|
||||
|
|
@ -167,12 +167,31 @@ inline void hkVector4f::setCross(hkVector4fParameter a, hkVector4fParameter b) {
|
|||
// x = a[1] * b[2] - b[1] * a[2]
|
||||
// y = a[2] * b[0] - b[2] * a[0]
|
||||
// ---- ---- ---- ----
|
||||
// a bb b aa
|
||||
|
||||
// 1 2 3 4
|
||||
#ifdef HK_VECTOR4F_AARCH64_NEON
|
||||
// Shuffle a and b to get the column vectors 2 and 4
|
||||
const auto a_yzwx = vextq_f32(a.v, a.v, 1);
|
||||
const auto b_yzwx = vextq_f32(b.v, b.v, 1);
|
||||
const auto a_yz = vget_low_f32(a_yzwx);
|
||||
const auto b_yz = vget_low_f32(b_yzwx);
|
||||
const auto a_wx = vget_high_f32(a_yzwx);
|
||||
const auto b_wx = vget_high_f32(b_yzwx);
|
||||
const auto a_xw = vrev64_f32(a_wx);
|
||||
const auto b_xw = vrev64_f32(b_wx);
|
||||
const auto a_yzxw = vcombine_f32(a_yz, a_xw);
|
||||
const auto b_yzxw = vcombine_f32(b_yz, b_xw);
|
||||
const auto cross = (a.v * b_yzxw) - (a_yzxw * b.v);
|
||||
// Shuffle `cross` back to the correct order (zxyw -> xyzw)
|
||||
const auto cross_yzwx = vextq_f32(cross, cross, 1);
|
||||
const auto c_xy = vget_low_f32(cross);
|
||||
const auto c_yz = vget_low_f32(cross_yzwx);
|
||||
v = vcombine_f32(c_yz, c_xy);
|
||||
#else
|
||||
auto cross0 = a.v * __builtin_shufflevector(b.v, b.v, 1, 2, 0, 3);
|
||||
auto cross1 = b.v * __builtin_shufflevector(a.v, a.v, 1, 2, 0, 3);
|
||||
auto diff = cross0 - cross1;
|
||||
v = __builtin_shufflevector(diff, diff, 1, 2, 0, 3);
|
||||
#endif
|
||||
}
|
||||
|
||||
inline void hkVector4f::setInterpolate(hkVector4fParameter v0, hkVector4fParameter v1,
|
||||
|
|
@ -183,6 +202,63 @@ inline void hkVector4f::setInterpolate(hkVector4fParameter v0, hkVector4fParamet
|
|||
setAddMul(v0, diff, t);
|
||||
}
|
||||
|
||||
inline hkVector4fComparison hkVector4f::less(hkVector4fParameter a) const {
|
||||
return hkVector4fComparison::convert(v < a.v);
|
||||
}
|
||||
|
||||
inline hkVector4fComparison hkVector4f::lessEqual(hkVector4fParameter a) const {
|
||||
return hkVector4fComparison::convert(v <= a.v);
|
||||
}
|
||||
|
||||
inline hkVector4fComparison hkVector4f::greater(hkVector4fParameter a) const {
|
||||
return hkVector4fComparison::convert(v > a.v);
|
||||
}
|
||||
|
||||
inline hkVector4fComparison hkVector4f::greaterEqual(hkVector4fParameter a) const {
|
||||
return hkVector4fComparison::convert(v >= a.v);
|
||||
}
|
||||
|
||||
inline hkVector4fComparison hkVector4f::equal(hkVector4fParameter a) const {
|
||||
return hkVector4fComparison::convert(v == a.v);
|
||||
}
|
||||
|
||||
inline hkVector4fComparison hkVector4f::notEqual(hkVector4fParameter a) const {
|
||||
return hkVector4fComparison::convert(v != a.v);
|
||||
}
|
||||
|
||||
inline hkVector4fComparison hkVector4f::lessZero() const {
|
||||
return hkVector4fComparison::convert(v < m128());
|
||||
}
|
||||
|
||||
inline hkVector4fComparison hkVector4f::lessEqualZero() const {
|
||||
return hkVector4fComparison::convert(v <= m128());
|
||||
}
|
||||
|
||||
inline hkVector4fComparison hkVector4f::greaterZero() const {
|
||||
return hkVector4fComparison::convert(v > m128());
|
||||
}
|
||||
|
||||
inline hkVector4fComparison hkVector4f::greaterEqualZero() const {
|
||||
return hkVector4fComparison::convert(v >= m128());
|
||||
}
|
||||
|
||||
inline hkVector4fComparison hkVector4f::equalZero() const {
|
||||
return hkVector4fComparison::convert(v == m128());
|
||||
}
|
||||
|
||||
inline hkVector4fComparison hkVector4f::notEqualZero() const {
|
||||
return hkVector4fComparison::convert(v != m128());
|
||||
}
|
||||
|
||||
inline void hkVector4f::setAbs(hkVector4fParameter a) {
|
||||
#ifdef HK_VECTOR4F_AARCH64_NEON
|
||||
v = vabsq_f32(a.v);
|
||||
#else
|
||||
for (int i = 0; i < 4; ++i)
|
||||
v[i] = std::abs(a[i]);
|
||||
#endif
|
||||
}
|
||||
|
||||
inline void hkVector4f::_setRotatedDir(const hkMatrix3f& a, hkVector4fParameter b) {
|
||||
#ifdef HK_VECTOR4F_AARCH64_NEON
|
||||
auto col0 = vmulq_laneq_f32(a.m_col0.v, v, 0);
|
||||
|
|
|
|||
|
|
@ -15,6 +15,33 @@ class hkVector4fComparison {
|
|||
public:
|
||||
HK_DECLARE_CLASS_ALLOCATOR(hkVector4fComparison)
|
||||
|
||||
enum Mask {
|
||||
INDEX_W = 3,
|
||||
INDEX_Z = 2,
|
||||
INDEX_Y = 1,
|
||||
INDEX_X = 0,
|
||||
|
||||
MASK_NONE = 0x0, // 0000
|
||||
MASK_W = (1 << INDEX_W), // 0001
|
||||
MASK_Z = (1 << INDEX_Z), // 0010
|
||||
MASK_ZW = (MASK_Z | MASK_W), // 0011
|
||||
|
||||
MASK_Y = (1 << INDEX_Y), // 0100
|
||||
MASK_YW = (MASK_Y | MASK_W), // 0101
|
||||
MASK_YZ = (MASK_Y | MASK_Z), // 0110
|
||||
MASK_YZW = (MASK_YZ | MASK_W), // 0111
|
||||
|
||||
MASK_X = (1 << INDEX_X), // 1000
|
||||
MASK_XW = (MASK_X | MASK_W), // 1001
|
||||
MASK_XZ = (MASK_X | MASK_Z), // 1010
|
||||
MASK_XZW = (MASK_XZ | MASK_W), // 1011
|
||||
|
||||
MASK_XY = (MASK_X | MASK_Y), // 1100
|
||||
MASK_XYW = (MASK_XY | MASK_W), // 1101
|
||||
MASK_XYZ = (MASK_XY | MASK_Z), // 1110
|
||||
MASK_XYZW = (MASK_XY | MASK_ZW) // 1111
|
||||
};
|
||||
|
||||
HK_FORCE_INLINE static hkVector4fComparison convert(const m128& x) { return {x}; }
|
||||
|
||||
HK_FORCE_INLINE void setAnd(hkVector4fComparisonParameter a, hkVector4fComparisonParameter b);
|
||||
|
|
@ -28,6 +55,14 @@ public:
|
|||
hkVector4fComparisonParameter trueValue,
|
||||
hkVector4fComparisonParameter falseValue);
|
||||
|
||||
template <Mask m = Mask::MASK_XYZW>
|
||||
HK_FORCE_INLINE hkBool32 allAreSet() const;
|
||||
HK_FORCE_INLINE hkBool32 allAreSet(Mask m) const;
|
||||
|
||||
template <Mask M = Mask::MASK_XYZW>
|
||||
HK_FORCE_INLINE hkBool32 anyIsSet() const;
|
||||
HK_FORCE_INLINE hkBool32 anyIsSet(Mask m) const;
|
||||
|
||||
m128u m_mask;
|
||||
};
|
||||
|
||||
|
|
@ -64,3 +99,25 @@ inline void hkVector4fComparison::setSelect(hkVector4fComparisonParameter comp,
|
|||
m_mask = (comp.m_mask & trueValue.m_mask) | (comp.m_mask & ~falseValue.m_mask);
|
||||
#endif
|
||||
}
|
||||
|
||||
template <hkVector4fComparison::Mask m>
|
||||
inline hkBool32 hkVector4fComparison::allAreSet() const {
|
||||
return ((m & MASK_X) == 0 || m_mask[0] != 0) && ((m & MASK_Y) == 0 || m_mask[1] != 0) &&
|
||||
((m & MASK_Z) == 0 || m_mask[2] != 0) && ((m & MASK_W) == 0 || m_mask[3] != 0);
|
||||
}
|
||||
|
||||
inline hkBool32 hkVector4fComparison::allAreSet(hkVector4fComparison::Mask m) const {
|
||||
return ((m & MASK_X) == 0 || m_mask[0] != 0) && ((m & MASK_Y) == 0 || m_mask[1] != 0) &&
|
||||
((m & MASK_Z) == 0 || m_mask[2] != 0) && ((m & MASK_W) == 0 || m_mask[3] != 0);
|
||||
}
|
||||
|
||||
template <hkVector4fComparison::Mask m>
|
||||
inline hkBool32 hkVector4fComparison::anyIsSet() const {
|
||||
return ((m & MASK_X) != 0 && m_mask[0] != 0) || ((m & MASK_Y) != 0 && m_mask[1] != 0) ||
|
||||
((m & MASK_Z) != 0 && m_mask[2] != 0) || ((m & MASK_W) != 0 && m_mask[3] != 0);
|
||||
}
|
||||
|
||||
inline hkBool32 hkVector4fComparison::anyIsSet(Mask m) const {
|
||||
return ((m & MASK_X) != 0 && m_mask[0] != 0) || ((m & MASK_Y) != 0 && m_mask[1] != 0) ||
|
||||
((m & MASK_Z) != 0 && m_mask[2] != 0) || ((m & MASK_W) != 0 && m_mask[3] != 0);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -177,7 +177,7 @@ inline const hkVector4& hkpMotion::getLinearVelocity() const {
|
|||
}
|
||||
|
||||
inline const hkVector4& hkpMotion::getAngularVelocity() const {
|
||||
return m_linearVelocity;
|
||||
return m_angularVelocity;
|
||||
}
|
||||
|
||||
inline void hkpMotion::getPointVelocity(const hkVector4& p, hkVector4& vecOut) const {
|
||||
|
|
|
|||
2
lib/sead
2
lib/sead
|
|
@ -1 +1 @@
|
|||
Subproject commit ec7fdc84424b4dd54fe29642650eba0068f637b7
|
||||
Subproject commit 09be2c3398c6fb732505f20a55c7d4c22f6a26c1
|
||||
|
|
@ -19,6 +19,8 @@ target_sources(uking PRIVATE
|
|||
RigidBody/physMotionAccessor.h
|
||||
RigidBody/physRigidBody.cpp
|
||||
RigidBody/physRigidBody.h
|
||||
RigidBody/physRigidBodyAccessor.cpp
|
||||
RigidBody/physRigidBodyAccessor.h
|
||||
RigidBody/physRigidBodyFactory.cpp
|
||||
RigidBody/physRigidBodyFactory.h
|
||||
RigidBody/physRigidBodyParam.cpp
|
||||
|
|
@ -92,4 +94,6 @@ target_sources(uking PRIVATE
|
|||
System/physSystemData.h
|
||||
System/physUserTag.cpp
|
||||
System/physUserTag.h
|
||||
|
||||
physConversions.h
|
||||
)
|
||||
|
|
|
|||
|
|
@ -2,6 +2,7 @@
|
|||
#include <Havok/Physics2012/Collide/Shape/Convex/Capsule/hkpCapsuleShape.h>
|
||||
#include <heap/seadHeap.h>
|
||||
#include <math/seadMathCalcCommon.h>
|
||||
#include "KingSystem/Physics/physConversions.h"
|
||||
|
||||
namespace ksys::phys {
|
||||
|
||||
|
|
@ -98,12 +99,12 @@ void CapsuleBody::sub_7100FABE80(sead::Vector3f* veca, sead::Vector3f* vecb,
|
|||
const hkTransformf& rb_vec) {
|
||||
if (veca != nullptr) {
|
||||
hkVector4 tmp;
|
||||
tmp.setTransformedPos(rb_vec, hkVector4(vertex_a.x, vertex_a.y, vertex_a.z));
|
||||
tmp.setTransformedPos(rb_vec, toHkVec4(vertex_a));
|
||||
tmp.store<3>(veca->e.data());
|
||||
}
|
||||
if (vecb != nullptr) {
|
||||
hkVector4 tmp;
|
||||
tmp.setTransformedPos(rb_vec, hkVector4(vertex_b.x, vertex_b.y, vertex_b.z));
|
||||
tmp.setTransformedPos(rb_vec, toHkVec4(vertex_b));
|
||||
tmp.store<3>(vecb->e.data());
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -7,7 +7,7 @@ namespace ksys::phys {
|
|||
|
||||
RigidBody::RigidBody(u32 a, u32 mass_scaling, hkpRigidBody* hk_body, const sead::SafeString& name,
|
||||
sead::Heap* heap, bool a7)
|
||||
: mCS(heap), mHkBody(hk_body), mHkBodyMgr(hk_body), _b4(a) {
|
||||
: mCS(heap), mHkBody(hk_body), mRigidBodyAccessor(hk_body), _b4(a) {
|
||||
if (!name.isEmpty()) {
|
||||
mHkBody->setName(name.cstr());
|
||||
}
|
||||
|
|
@ -69,7 +69,7 @@ MotionType RigidBody::getMotionInfo() const {
|
|||
return MotionType::Keyframed;
|
||||
if (mMotionFlags.isOn(MotionFlag::Fixed))
|
||||
return MotionType::Fixed;
|
||||
return mHkBodyMgr.getMotionInfo();
|
||||
return mRigidBodyAccessor.getMotionType();
|
||||
}
|
||||
|
||||
void RigidBody::setContactMask(u32 value) {
|
||||
|
|
|
|||
|
|
@ -6,6 +6,7 @@
|
|||
#include <prim/seadTypedBitFlag.h>
|
||||
#include <thread/seadAtomic.h>
|
||||
#include <thread/seadCriticalSection.h>
|
||||
#include "KingSystem/Physics/RigidBody/physRigidBodyAccessor.h"
|
||||
#include "KingSystem/Physics/RigidBody/physRigidBodyParam.h"
|
||||
#include "KingSystem/Physics/System/physDefines.h"
|
||||
#include "KingSystem/Utils/Types.h"
|
||||
|
|
@ -25,14 +26,6 @@ public:
|
|||
class RigidBody : public sead::IDisposer, public RigidBase {
|
||||
SEAD_RTTI_BASE(RigidBody)
|
||||
public:
|
||||
struct HkBodyMgr {
|
||||
explicit HkBodyMgr(hkpRigidBody* body);
|
||||
virtual ~HkBodyMgr();
|
||||
MotionType getMotionInfo() const;
|
||||
|
||||
void* p;
|
||||
};
|
||||
|
||||
enum class Flag1 {
|
||||
MassScaling = 1 << 0,
|
||||
_2 = 1 << 1,
|
||||
|
|
@ -107,7 +100,7 @@ private:
|
|||
void* _88 = nullptr;
|
||||
void* _90 = nullptr;
|
||||
u16 _98 = 0;
|
||||
HkBodyMgr mHkBodyMgr;
|
||||
RigidBodyAccessor mRigidBodyAccessor;
|
||||
f32 _b0 = 1.0f;
|
||||
u32 _b4 = 0;
|
||||
MotionAccessor* mMotionAccessor = nullptr;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,99 @@
|
|||
#include "KingSystem/Physics/RigidBody/physRigidBodyAccessor.h"
|
||||
#include <Havok/Physics2012/Dynamics/Entity/hkpRigidBody.h>
|
||||
#include "KingSystem/Physics/physConversions.h"
|
||||
|
||||
namespace ksys::phys {
|
||||
|
||||
RigidBodyAccessor::RigidBodyAccessor(hkpRigidBody* body) : mBody(body) {}
|
||||
|
||||
RigidBodyAccessor::~RigidBodyAccessor() = default;
|
||||
|
||||
MotionType RigidBodyAccessor::getMotionType() const {
|
||||
switch (getBody()->getMotionType()) {
|
||||
case hkpMotion::MOTION_INVALID:
|
||||
break;
|
||||
case hkpMotion::MOTION_DYNAMIC:
|
||||
case hkpMotion::MOTION_SPHERE_INERTIA:
|
||||
case hkpMotion::MOTION_BOX_INERTIA:
|
||||
return MotionType::Dynamic;
|
||||
case hkpMotion::MOTION_KEYFRAMED:
|
||||
return MotionType::Keyframed;
|
||||
case hkpMotion::MOTION_FIXED:
|
||||
return MotionType::Fixed;
|
||||
case hkpMotion::MOTION_THIN_BOX_INERTIA:
|
||||
break;
|
||||
case hkpMotion::MOTION_CHARACTER:
|
||||
return MotionType::Dynamic;
|
||||
case hkpMotion::MOTION_MAX_ID:
|
||||
break;
|
||||
}
|
||||
return MotionType::Invalid;
|
||||
}
|
||||
|
||||
void RigidBodyAccessor::getPosition(sead::Vector3f* pos) const {
|
||||
toVec3(pos, getBody()->getPosition());
|
||||
}
|
||||
|
||||
void RigidBodyAccessor::getRotation(sead::Quatf* rot) const {
|
||||
toQuat(rot, getBody()->getRotation());
|
||||
}
|
||||
|
||||
void RigidBodyAccessor::getTransform(sead::Matrix34f* mtx) const {
|
||||
toMtx34(mtx, getBody()->getTransform());
|
||||
}
|
||||
|
||||
void RigidBodyAccessor::getLinearVelocity(sead::Vector3f* vel) const {
|
||||
toVec3(vel, getBody()->getLinearVelocity());
|
||||
}
|
||||
|
||||
void RigidBodyAccessor::getAngularVelocity(sead::Vector3f* vel) const {
|
||||
toVec3(vel, getBody()->getAngularVelocity());
|
||||
}
|
||||
|
||||
bool RigidBodyAccessor::isVelocityGreaterEqual(float vel) const {
|
||||
hkVector4f v;
|
||||
v.setAll(vel);
|
||||
|
||||
hkVector4f linvel;
|
||||
linvel.setAbs(getBody()->getLinearVelocity());
|
||||
|
||||
hkVector4f angvel;
|
||||
angvel.setAbs(getBody()->getAngularVelocity());
|
||||
|
||||
return v.greaterEqual(linvel).allAreSet<hkVector4fComparison::MASK_XYZ>() &&
|
||||
v.greaterEqual(angvel).allAreSet<hkVector4fComparison::MASK_XYZ>();
|
||||
}
|
||||
|
||||
void RigidBodyAccessor::getPointVelocity(sead::Vector3f* vel, const sead::Vector3f& point) const {
|
||||
const auto p = toHkVec4(point);
|
||||
hkVector4f out;
|
||||
getBody()->getPointVelocity(p, out);
|
||||
toVec3(vel, out);
|
||||
}
|
||||
|
||||
float RigidBodyAccessor::getTimeFactor() const {
|
||||
return getBody()->getTimeFactor();
|
||||
}
|
||||
|
||||
void RigidBodyAccessor::getDeltaCenterOfMass(sead::Vector3f* out_delta_pos,
|
||||
sead::Vector3f* out_delta_angle) const {
|
||||
const hkMotionState* state = getBody()->getMotion()->getMotionState();
|
||||
const auto center1 = state->getSweptTransform().m_centerOfMass1;
|
||||
const auto center0 = state->getSweptTransform().m_centerOfMass0;
|
||||
const auto delta_angle = state->m_deltaAngle;
|
||||
|
||||
if (out_delta_pos != nullptr) {
|
||||
hkVector4f value;
|
||||
value.setSub(center1, center0);
|
||||
value.mul(center1.getW()); // W is the time step
|
||||
value.store<3>(out_delta_pos->e.data());
|
||||
}
|
||||
|
||||
if (out_delta_angle != nullptr) {
|
||||
hkVector4f value;
|
||||
value.setMul(delta_angle, center1.getW());
|
||||
value.store<3>(out_delta_angle->e.data());
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace ksys::phys
|
||||
|
|
@ -0,0 +1,44 @@
|
|||
#pragma once
|
||||
|
||||
#include <math/seadMatrix.h>
|
||||
#include <math/seadQuat.h>
|
||||
#include <math/seadVector.h>
|
||||
#include "KingSystem/Physics/System/physDefines.h"
|
||||
|
||||
class hkpRigidBody;
|
||||
|
||||
namespace ksys::phys {
|
||||
|
||||
class RigidBodyAccessor {
|
||||
public:
|
||||
explicit RigidBodyAccessor(hkpRigidBody* body);
|
||||
virtual ~RigidBodyAccessor();
|
||||
|
||||
hkpRigidBody* getBody() const { return mBody; }
|
||||
|
||||
MotionType getMotionType() const;
|
||||
|
||||
void getPosition(sead::Vector3f* pos) const;
|
||||
void getRotation(sead::Quatf* rot) const;
|
||||
void getTransform(sead::Matrix34f* mtx) const;
|
||||
|
||||
void getLinearVelocity(sead::Vector3f* vel) const;
|
||||
void getAngularVelocity(sead::Vector3f* vel) const;
|
||||
|
||||
/// Whether `vel` >= abs(c) for each component c of the linear velocity and angular velocity.
|
||||
// XXX: does this function even make sense? We're comparing `vel` against two quantities
|
||||
// that have different units!
|
||||
bool isVelocityGreaterEqual(float vel) const;
|
||||
|
||||
void getPointVelocity(sead::Vector3f* vel, const sead::Vector3f& point) const;
|
||||
|
||||
float getTimeFactor() const;
|
||||
|
||||
/// Both parameters are allowed to be null.
|
||||
void getDeltaCenterOfMass(sead::Vector3f* out_delta_pos, sead::Vector3f* out_delta_angle) const;
|
||||
|
||||
private:
|
||||
hkpRigidBody* mBody;
|
||||
};
|
||||
|
||||
} // namespace ksys::phys
|
||||
|
|
@ -171,6 +171,7 @@ enum class MotionType {
|
|||
Fixed = 1,
|
||||
Keyframed = 2,
|
||||
Unknown = 3,
|
||||
Invalid = -1,
|
||||
};
|
||||
|
||||
union ReceiverMask {
|
||||
|
|
|
|||
|
|
@ -0,0 +1,86 @@
|
|||
#pragma once
|
||||
|
||||
#include <Havok/Common/Base/hkBase.h>
|
||||
#include <math/seadMatrix.h>
|
||||
#include <math/seadQuat.h>
|
||||
#include <math/seadVector.h>
|
||||
|
||||
#ifdef __aarch64__
|
||||
#include <arm_neon.h>
|
||||
#endif
|
||||
|
||||
namespace ksys::phys {
|
||||
|
||||
inline void toVec3(sead::Vector3f* out, const hkVector4f& vec) {
|
||||
out->x = vec.getX();
|
||||
out->y = vec.getY();
|
||||
out->z = vec.getZ();
|
||||
}
|
||||
|
||||
[[nodiscard]] inline sead::Vector3f toVec3(const hkVector4f& vec) {
|
||||
return {vec.getX(), vec.getY(), vec.getZ()};
|
||||
}
|
||||
|
||||
inline void toHkVec4(hkVector4f* out, const sead::Vector3f& vec) {
|
||||
out->set(vec.x, vec.y, vec.z);
|
||||
}
|
||||
|
||||
[[nodiscard]] inline hkVector4f toHkVec4(const sead::Vector3f& vec) {
|
||||
return {vec.x, vec.y, vec.z};
|
||||
}
|
||||
|
||||
inline void toQuat(sead::Quatf* out, const hkQuaternionf& quat) {
|
||||
out->set(quat.m_vec.getW(), quat.m_vec.getX(), quat.m_vec.getY(), quat.m_vec.getZ());
|
||||
}
|
||||
|
||||
[[nodiscard]] inline sead::Quatf toQuat(const hkQuaternionf& quat) {
|
||||
return {quat.m_vec.getW(), quat.m_vec.getX(), quat.m_vec.getY(), quat.m_vec.getZ()};
|
||||
}
|
||||
|
||||
inline void toHkQuat(hkQuaternionf* out, const sead::Quatf& quat) {
|
||||
out->set(quat.x, quat.y, quat.x, quat.w);
|
||||
}
|
||||
|
||||
[[nodiscard]] inline hkQuaternionf toHkQuat(const sead::Quatf& quat) {
|
||||
return {quat.x, quat.y, quat.x, quat.w};
|
||||
}
|
||||
|
||||
inline void toMtx34(sead::Matrix34f* out, const hkTransformf& transform) {
|
||||
const hkRotationf& rotate = transform.getRotation();
|
||||
const hkVector4f& translate = transform.getTranslation();
|
||||
|
||||
hkVector4f row0, row1, row2;
|
||||
|
||||
#ifdef __aarch64__
|
||||
// XXX: this leads to really poor codegen (compared to using getRows, which
|
||||
// is optimised into Neon zip/transpose instructions). Is Nintendo to blame
|
||||
// for this bad usage of Neon intrinsics, or did Havok mess up their Neon getRows?
|
||||
|
||||
row0.v = vld1q_lane_f32(&rotate(0, 0), row0.v, 0);
|
||||
row1.v = vld1q_lane_f32(&rotate(1, 0), row1.v, 0);
|
||||
row2.v = vld1q_lane_f32(&rotate(2, 0), row2.v, 0);
|
||||
|
||||
row0.v = vld1q_lane_f32(&rotate(0, 1), row0.v, 1);
|
||||
row1.v = vld1q_lane_f32(&rotate(1, 1), row1.v, 1);
|
||||
row2.v = vld1q_lane_f32(&rotate(2, 1), row2.v, 1);
|
||||
|
||||
row0.v = vld1q_lane_f32(&rotate(0, 2), row0.v, 2);
|
||||
row1.v = vld1q_lane_f32(&rotate(1, 2), row1.v, 2);
|
||||
row2.v = vld1q_lane_f32(&rotate(2, 2), row2.v, 2);
|
||||
|
||||
row0.v = vld1q_lane_f32(&translate(0), row0.v, 3);
|
||||
row1.v = vld1q_lane_f32(&translate(1), row1.v, 3);
|
||||
row2.v = vld1q_lane_f32(&translate(2), row2.v, 3);
|
||||
#else
|
||||
rotate.getRows(row0, row1, row2);
|
||||
row0[3] = translate[0];
|
||||
row1[3] = translate[1];
|
||||
row2[3] = translate[2];
|
||||
#endif
|
||||
|
||||
row0.store<4>(out->m[0]);
|
||||
row1.store<4>(out->m[1]);
|
||||
row2.store<4>(out->m[2]);
|
||||
}
|
||||
|
||||
} // namespace ksys::phys
|
||||
Loading…
Reference in New Issue