ksys/phys: Add more RigidBody functions and Havok utils

data/uking_functions.csv

@@ -83105,16 +83105,16 @@ Address,Quality,Size,Name
 0x0000007100f94bbc,O,000160,_ZNK4ksys4phys9RigidBody23isEntityMotionFlag20OffEv
 0x0000007100f94c5c,O,000156,_ZN4ksys4phys9RigidBody18setColImpulseScaleEf
 0x0000007100f94cf8,O,000148,_ZNK4ksys4phys9RigidBody18getColImpulseScaleEv
-0x0000007100f94d8c,U,000228,phys::RigidBody::x_102
+0x0000007100f94d8c,O,000228,_ZN4ksys4phys9RigidBody25hasConstraintWithUserDataEv
 0x0000007100f94e70,O,000008,_ZN4ksys4phys9RigidBody4lockEv
 0x0000007100f94e78,O,000008,_ZN4ksys4phys9RigidBody6unlockEv
 0x0000007100f94e80,U,000152,phys::RigidBody::x_103
 0x0000007100f94f18,U,000404,phys::RigidBody::x_104
 0x0000007100f950ac,U,000328,phys::RigidBody::x_105
-0x0000007100f951f4,U,000232,phys::RigidBody::x_106
+0x0000007100f951f4,O,000232,_ZN4ksys4phys9RigidBody21setEntityMotionFlag40Eb
-0x0000007100f952dc,U,000168,phys::RigidBody::x_107
+0x0000007100f952dc,O,000168,_ZNK4ksys4phys9RigidBody22isEntityMotionFlag40OnEv
-0x0000007100f95384,U,000280,phys::RigidBody::m6
+0x0000007100f95384,O,000280,_ZN4ksys4phys9RigidBody27resetInertiaAndCenterOfMassEv
-0x0000007100f9549c,U,000292,phys::RigidBody::m7
+0x0000007100f9549c,O,000292,_ZN4ksys4phys9RigidBody32computeShapeVolumeMassPropertiesEPfPN4sead7Vector3IfEES6_
 0x0000007100f955c0,U,000768,phys::RigidBody::x_108
 0x0000007100f958c0,U,000416,phys::RigidBody::x_109
 0x0000007100f95a60,U,000376,phys::RigidBody::x_110
@@ -106077,7 +106077,7 @@ Address,Quality,Size,Name
 0x000000710166dec4,L,000012,hkpShapeDisplayBuilder::hkpShapeDisplayBuilderEnvironment::hkpShapeDisplayBuilderEnvironment
 0x000000710166ded0,L,000040,hkpShapeDisplayBuilder::hkpShapeDisplayBuilder
 0x000000710166def8,L,000080,hkpShapeDisplayBuilder::buildDisplayGeometries
-0x000000710166df48,U,002124,
+0x000000710166df48,L,002124,
 0x000000710166e794,U,000244,
 0x000000710166e888,U,001268,
 0x000000710166ed7c,U,000292,
@@ -106095,23 +106095,23 @@ Address,Quality,Size,Name
 0x00000071016718cc,U,001284,
 0x0000007101671dd0,U,001000,
 0x00000071016721b8,U,000092,
-0x0000007101672214,U,000084,hkpUserShapeDisplayBuilder::dtor
+0x0000007101672214,L,000084,hkpUserShapeDisplayBuilder::dtor
-0x0000007101672268,U,000080,
+0x0000007101672268,L,000080,
-0x00000071016722b8,U,000224,
+0x00000071016722b8,L,000224,
-0x0000007101672398,U,000084,
+0x0000007101672398,L,000084,
-0x00000071016723ec,U,000096,
+0x00000071016723ec,L,000096,
-0x000000710167244c,U,000152,
+0x000000710167244c,L,000152,
-0x00000071016724e4,U,000096,
+0x00000071016724e4,L,000096,
-0x0000007101672544,U,000152,
+0x0000007101672544,L,000152,
-0x00000071016725dc,U,000060,
+0x00000071016725dc,L,000060,
-0x0000007101672618,U,000876,
+0x0000007101672618,L,000876,
-0x0000007101672984,U,000104,
+0x0000007101672984,L,000104,
-0x00000071016729ec,U,001952,
+0x00000071016729ec,L,001952,
-0x000000710167318c,U,000324,
+0x000000710167318c,L,000324,
-0x00000071016732d0,U,000320,
+0x00000071016732d0,L,000320,
-0x0000007101673410,U,002464,
+0x0000007101673410,L,002464,
-0x0000007101673db0,U,000896,
+0x0000007101673db0,L,000896,
-0x0000007101674130,U,000012,hkpPhysicsData::getClass
+0x0000007101674130,L,000012,hkpPhysicsData::getClass
 0x000000710167413c,L,000020,
 0x0000007101674150,L,000012,
 0x000000710167415c,L,000176,

lib/hkStubs/CMakeLists.txt

@@ -59,6 +59,8 @@ add_library(hkStubs OBJECT
   Havok/Common/Base/Types/Physics/MotionState/hkMotionState.h
   Havok/Common/Base/Types/Properties/hkSimpleProperty.h
 
+  Havok/Common/GeometryUtilities/Inertia/hkInertiaTensorComputer.h
+
   Havok/Common/Serialize/Resource/hkResource.h
   Havok/Common/Serialize/Util/hkNativePackfileUtils.h
   Havok/Common/Serialize/Util/hkRootLevelContainer.h
@@ -108,6 +110,7 @@ add_library(hkStubs OBJECT
   Havok/Physics2012/Dynamics/Entity/hkpEntity.h
   Havok/Physics2012/Dynamics/Entity/hkpRigidBody.h
   Havok/Physics2012/Dynamics/Entity/hkpRigidBodyCinfo.h
+  Havok/Physics2012/Dynamics/Inertia/hkpInertiaTensorComputer.h
   Havok/Physics2012/Dynamics/Motion/hkpMotion.h
   Havok/Physics2012/Dynamics/Motion/Rigid/hkpBoxMotion.h
   Havok/Physics2012/Dynamics/Motion/Rigid/hkpFixedRigidMotion.h

lib/hkStubs/Havok/Common/Base/Math/Matrix/hkMatrix3f.h

@@ -8,33 +8,57 @@ public:
 
     HK_FORCE_INLINE hkFloat32& operator()(int row, int col);
     HK_FORCE_INLINE const hkFloat32& operator()(int row, int col) const;
+    template <int Row, int Col>
+    HK_FORCE_INLINE hkSimdFloat32 get() const;
+    template <int Row, int Col>
+    HK_FORCE_INLINE void set(hkSimdFloat32Parameter s);
 
     HK_FORCE_INLINE hkVector4f& getColumn(int i);
     HK_FORCE_INLINE const hkVector4f& getColumn(int i) const;
+    template <int I>
+    HK_FORCE_INLINE const hkVector4f& getColumn() const;
 
     HK_FORCE_INLINE void getRows(hkVector4f& r0, hkVector4f& r1, hkVector4f& r2) const;
 
+    HK_FORCE_INLINE void setZero();
+    HK_FORCE_INLINE void setIdentity();
+
     hkVector4f m_col0;
     hkVector4f m_col1;
     hkVector4f m_col2;
 };
 
-hkFloat32& hkMatrix3f::operator()(int row, int col) {
+inline hkFloat32& hkMatrix3f::operator()(int row, int col) {
     return getColumn(col)(row);
 }
 
-const hkFloat32& hkMatrix3f::operator()(int row, int col) const {
+inline const hkFloat32& hkMatrix3f::operator()(int row, int col) const {
     return getColumn(col)(row);
 }
 
-hkVector4f& hkMatrix3f::getColumn(int i) {
+template <int Row, int Col>
+inline hkSimdFloat32 hkMatrix3f::get() const {
+    return getColumn<Col>().template getComponent<Row>();
+}
+
+template <int Row, int Col>
+inline void hkMatrix3f::set(hkSimdFloat32Parameter s) {
+    getColumn<Col>().template setComponent<Row>(s);
+}
+
+inline hkVector4f& hkMatrix3f::getColumn(int i) {
     return (&m_col0)[i];
 }
 
-const hkVector4f& hkMatrix3f::getColumn(int i) const {
+inline const hkVector4f& hkMatrix3f::getColumn(int i) const {
     return (&m_col0)[i];
 }
 
+template <int I>
+inline const hkVector4f& hkMatrix3f::getColumn() 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));
@@ -47,3 +71,16 @@ inline void hkMatrix3f::getRows(hkVector4f& r0, hkVector4f& r1, hkVector4f& r2)
     r1 = c1;
     r2 = c2;
 }
+
+inline void hkMatrix3f::setZero() {
+    m_col0.setZero();
+    m_col1.setZero();
+    m_col2.setZero();
+}
+
+inline void hkMatrix3f::setIdentity() {
+    hkMatrix3f* __restrict d = this;
+    d->m_col0 = hkVector4f::getConstant<HK_QUADREAL_1000>();
+    d->m_col1 = hkVector4f::getConstant<HK_QUADREAL_0100>();
+    d->m_col2 = hkVector4f::getConstant<HK_QUADREAL_0010>();
+}

lib/hkStubs/Havok/Common/Base/Math/Matrix/hkTransformf.h

@@ -21,6 +21,7 @@ public:
 
     HK_FORCE_INLINE void set(const hkRotationf& r, hkVector4fParameter t);
     HK_FORCE_INLINE void set(hkQuaternionfParameter q, hkVector4fParameter t);
+    HK_FORCE_INLINE void setIdentity();
 
     hkRotationf m_rotation;
     hkVector4f m_translation;
@@ -54,3 +55,8 @@ inline void hkTransformf::set(const hkQuaternionf& q, const hkVector4f& t) {
     m_rotation.set(q);
     m_translation = t;
 }
+
+inline void hkTransformf::setIdentity() {
+    m_rotation.setIdentity();
+    m_translation.setZero();
+}

lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h

@@ -141,6 +141,10 @@ public:
     hkSimdFloat32 getY() const { return getComponent<1>(); }
     hkSimdFloat32 getZ() const { return getComponent<2>(); }
     hkSimdFloat32 getW() const { return getComponent<3>(); }
+    template <int I>
+    HK_FORCE_INLINE void setComponent(hkSimdFloat32Parameter val) {
+        v[I] = val;
+    }
     void setComponent(int i, hkSimdFloat32Parameter val) { v[i] = val; }
     void setX(hkSimdFloat32Parameter val) { setComponent(0, val); }
     void setY(hkSimdFloat32Parameter val) { setComponent(1, val); }

lib/hkStubs/Havok/Common/GeometryUtilities/Inertia/hkInertiaTensorComputer.h

@@ -0,0 +1,115 @@
+#pragma once
+
+#include <Havok/Common/Base/hkBase.h>
+
+struct hkGeometry;
+struct hkStridedVertices;
+
+struct hkMassProperties {
+    HK_DECLARE_CLASS_ALLOCATOR(hkMassProperties)
+    HK_DECLARE_REFLECTION()
+
+    hkMassProperties() : m_volume(0), m_mass(0) {
+        m_centerOfMass.setZero();
+        m_inertiaTensor.setZero();
+    }
+
+    explicit hkMassProperties(hkFinishLoadedObjectFlag flag) {}
+
+    void scaleToDensity(hkSimdRealParameter density);
+    void scaleToMass(hkSimdRealParameter newMass);
+
+    hkReal m_volume;
+    hkReal m_mass;
+    hkVector4 m_centerOfMass;
+    hkMatrix3 m_inertiaTensor;
+};
+
+struct hkMassElement {
+    HK_DECLARE_CLASS_ALLOCATOR(hkMassElement)
+
+    HK_FORCE_INLINE hkMassElement() { m_transform.setIdentity(); }
+
+    HK_FORCE_INLINE hkMassElement(const hkMassProperties& properties, const hkTransform& transform)
+        : m_properties(properties), m_transform(transform) {}
+
+    hkMassProperties m_properties;
+    hkTransform m_transform;
+};
+
+class hkInertiaTensorComputer {
+public:
+    static hkResult computeSphereVolumeMassProperties(hkReal radius, hkReal mass,
+                                                      hkMassProperties& result);
+
+    static hkResult computeBoxVolumeMassProperties(hkVector4Parameter halfExtents, hkReal mass,
+                                                   hkMassProperties& result);
+
+    static hkResult computeBoxVolumeMassPropertiesDiagonalized(hkVector4Parameter halfExtents,
+                                                               hkReal mass,
+                                                               hkVector4& inertiaDiagonal,
+                                                               hkReal& volume);
+
+    static hkResult computeCapsuleVolumeMassProperties(hkVector4Parameter startAxis,
+                                                       hkVector4Parameter endAxis, hkReal radius,
+                                                       hkReal mass, hkMassProperties& result);
+
+    static hkResult computeSphereSurfaceMassProperties(hkReal radius, hkReal mass,
+                                                       hkReal surfaceThickness,
+                                                       hkMassProperties& result);
+
+    static hkResult computeBoxSurfaceMassProperties(hkVector4Parameter halfExtents, hkReal mass,
+                                                    hkReal surfaceThickness,
+                                                    hkMassProperties& result);
+
+    static hkResult computeTriangleSurfaceMassProperties(hkVector4Parameter v0,
+                                                         hkVector4Parameter v1,
+                                                         hkVector4Parameter v2, hkReal mass,
+                                                         hkReal surfaceThickness,
+                                                         hkMassProperties& result);
+
+    static hkResult computeCylinderVolumeMassProperties(hkVector4Parameter startAxis,
+                                                        hkVector4Parameter endAxis, hkReal radius,
+                                                        hkReal mass, hkMassProperties& result);
+
+    static hkResult computeConvexHullMassProperties(const hkStridedVertices& vertices,
+                                                    hkReal radius, hkMassProperties& result);
+
+    using ConvexHullMassPropertiesFunction = hkResult (*)(const hkStridedVertices&, hkReal,
+                                                          hkMassProperties&);
+
+    static ConvexHullMassPropertiesFunction s_computeConvexHullMassPropertiesFunction;
+
+    static hkResult computeVertexHullVolumeMassProperties(const hkReal* vertexIn, int striding,
+                                                          int numVertices, hkReal mass,
+                                                          hkMassProperties& result);
+
+    static hkResult computeVertexCloudMassProperties(const hkReal* vertexIn, int striding,
+                                                     int numVertices, hkReal mass,
+                                                     hkMassProperties& result);
+
+    static hkResult computeGeometrySurfaceMassProperties(const hkGeometry* geom,
+                                                         hkReal surfaceThickness,
+                                                         hkBool distributeUniformly, hkReal mass,
+                                                         hkMassProperties& result);
+
+    static void computeGeometryVolumeMassProperties(const hkGeometry* geom, hkReal mass,
+                                                    hkMassProperties& result);
+
+    static hkResult computeGeometryVolumeMassPropertiesChecked(const hkGeometry* geom, hkReal mass,
+                                                               hkMassProperties& result);
+
+    static hkResult combineMassProperties(const hkArray<hkMassElement>& elements,
+                                          hkMassProperties& result);
+
+    static void simplifyInertiaTensorToOrientedParticle(hkMatrix3& inertia);
+
+    static void convertInertiaTensorToPrincipleAxis(hkMatrix3& inertia,
+                                                    hkRotation& principleAxisOut);
+
+    static void shiftInertiaToCom(hkVector4Parameter shift, hkSimdRealParameter mass,
+                                  hkMatrix3& inertia);
+
+    static void shiftInertiaFromCom(hkVector4Parameter shift, hkSimdRealParameter mass,
+                                    hkMatrix3& inertia);
+};

lib/hkStubs/Havok/Physics/Constraint/hkpConstraintInstance.h

@@ -141,3 +141,11 @@ public:
     struct hkConstraintInternal* m_internal;
     hkUint32 m_uid;
 };
+
+inline const hkpConstraintData* hkpConstraintInstance::getData() const {
+    return m_data;
+}
+
+inline hkpConstraintData* hkpConstraintInstance::getDataRw() const {
+    return m_data;
+}

lib/hkStubs/Havok/Physics2012/Dynamics/Inertia/hkpInertiaTensorComputer.h

@@ -0,0 +1,32 @@
+#pragma once
+
+#include <Havok/Common/GeometryUtilities/Inertia/hkInertiaTensorComputer.h>
+
+class hkcdShape;
+class hkpConstraintInstance;
+class hkpRigidBody;
+class hkpRigidBodyCinfo;
+class hkpShape;
+
+class hkpInertiaTensorComputer : public hkInertiaTensorComputer {
+public:
+    static void computeShapeVolumeMassProperties(const hkcdShape* shape, hkReal mass,
+                                                 hkMassProperties& result);
+
+    static void setShapeVolumeMassProperties(const hkpShape* shape, hkReal mass,
+                                             hkpRigidBodyCinfo& bodyInfo);
+
+    static void setMassProperties(const hkMassProperties& props, hkpRigidBodyCinfo& bodyInfo);
+
+    static void setAndScaleToDensity(const hkMassProperties& props, hkSimdRealParameter density,
+                                     hkpRigidBodyCinfo& bodyInfo);
+
+    static void setAndScaleToMass(const hkMassProperties& props, hkSimdRealParameter mass,
+                                  hkpRigidBodyCinfo& bodyInfo);
+
+    static void clipInertia(hkReal maxInertiaRatio, hkpRigidBodyCinfo& bodyInfo);
+
+    static void optimizeInertiasOfConstraintTree(hkpConstraintInstance* const* constraints,
+                                                 int numConstraints, hkpRigidBody* rootBody,
+                                                 hkReal inertiaFactorHint = 1.5f);
+};

src/KingSystem/Physics/RigidBody/physRigidBody.cpp

@@ -1,6 +1,9 @@
 #include "KingSystem/Physics/RigidBody/physRigidBody.h"
+#include <Havok/Physics/Constraint/Data/hkpConstraintData.h>
+#include <Havok/Physics/Constraint/hkpConstraintInstance.h>
 #include <Havok/Physics2012/Dynamics/Collide/hkpResponseModifier.h>
 #include <Havok/Physics2012/Dynamics/Entity/hkpRigidBody.h>
+#include <Havok/Physics2012/Dynamics/Inertia/hkpInertiaTensorComputer.h>
 #include <Havok/Physics2012/Dynamics/Motion/Rigid/hkpFixedRigidMotion.h>
 #include <Havok/Physics2012/Dynamics/Motion/Rigid/hkpKeyframedRigidMotion.h>
 #include <cmath>
@@ -768,6 +771,63 @@ float RigidBody::getColImpulseScale() const {
     return getEntityMotionAccessor()->getColImpulseScale();
 }
 
+bool RigidBody::hasConstraintWithUserData() {
+    auto lock = makeScopedLock(true);
+
+    for (int i = 0, n = getHkBody()->getNumConstraints(); i < n; ++i) {
+        auto* constraint = getHkBody()->getConstraint(i);
+        if (constraint->getData()->getType() != hkpConstraintData::CONSTRAINT_TYPE_CONTACT &&
+            constraint->m_userData != 0) {
+            return true;
+        }
+    }
+
+    return false;
+}
+
+void RigidBody::setEntityMotionFlag40(bool set) {
+    if (!isEntity() || isCharacterControllerType())
+        return;
+    getEntityMotionAccessor()->changeFlag(RigidBodyMotionEntity::Flag::_40, set);
+}
+
+bool RigidBody::isEntityMotionFlag40On() const {
+    if (!isEntity() || !mMotionAccessor || isCharacterControllerType())
+        return false;
+    return getEntityMotionAccessor()->hasFlag(RigidBodyMotionEntity::Flag::_40);
+}
+
+void RigidBody::resetInertiaAndCenterOfMass() {
+    float volume;
+    sead::Vector3f center_of_mass;
+    sead::Vector3f inertia;
+    computeShapeVolumeMassProperties(&volume, &center_of_mass, &inertia);
+
+    setInertiaLocal(inertia);
+    setCenterOfMassInLocal(center_of_mass);
+}
+
+void RigidBody::computeShapeVolumeMassProperties(float* volume, sead::Vector3f* center_of_mass,
+                                                 sead::Vector3f* inertia_tensor) {
+    hkMassProperties properties;
+    const auto shape = getHkBody()->getCollidable()->getShape();
+    const float mass = getMass();
+    hkpInertiaTensorComputer::computeShapeVolumeMassProperties(shape, mass, properties);
+
+    if (volume != nullptr)
+        *volume = properties.m_volume;
+
+    if (center_of_mass != nullptr)
+        storeToVec3(center_of_mass, properties.m_centerOfMass);
+
+    if (inertia_tensor != nullptr) {
+        hkVector4f diagonal{properties.m_inertiaTensor.get<0, 0>(),
+                            properties.m_inertiaTensor.get<1, 1>(),
+                            properties.m_inertiaTensor.get<2, 2>()};
+        storeToVec3(inertia_tensor, diagonal);
+    }
+}
+
 void RigidBody::resetPosition() {
     // debug logging?
     [[maybe_unused]] sead::Vector3f position = getPosition();

src/KingSystem/Physics/RigidBody/physRigidBody.h

@@ -105,8 +105,14 @@ public:
     // FIXME: types and names
     virtual float m4();
     virtual void m5();
-    virtual void m6();
+
-    virtual void m7();
+    /// Recalculate inertia, volume and center of mass based on the shape and mass of the rigid body
+    /// and update this rigid body to match the computed values.
+    virtual void resetInertiaAndCenterOfMass();
+
+    /// All three parameters may be null.
+    virtual void computeShapeVolumeMassProperties(float* volume, sead::Vector3f* center_of_mass,
+                                                  sead::Vector3f* inertia_tensor);
 
     bool initMotionAccessorForDynamicMotion(sead::Heap* heap);
     bool initMotionAccessor(const RigidBodyInstanceParam& param, sead::Heap* heap,
@@ -299,6 +305,17 @@ public:
     Type getType() const { return mType; }
     bool isCharacterControllerType() const { return mType == Type::CharacterController; }
 
+    bool hasConstraintWithUserData();
+    // 0x0000007100f94e80
+    bool x_103(int a);
+    // 0x0000007100f94f18
+    bool x_104(RigidBody* other_body, int a, int b);
+    // 0x0000007100f950ac
+    bool x_105();
+
+    void setEntityMotionFlag40(bool set);
+    bool isEntityMotionFlag40On() const;
+
     void lock();
     void lock(bool also_lock_world);
     void unlock();