ksys/phys: Add more RigidBody functions and Havok utils

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();