diff --git a/data/uking_functions.csv b/data/uking_functions.csv index d7144b19..dd093e4d 100644 --- a/data/uking_functions.csv +++ b/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 -0x0000007100f952dc,U,000168,phys::RigidBody::x_107 -0x0000007100f95384,U,000280,phys::RigidBody::m6 -0x0000007100f9549c,U,000292,phys::RigidBody::m7 +0x0000007100f951f4,O,000232,_ZN4ksys4phys9RigidBody21setEntityMotionFlag40Eb +0x0000007100f952dc,O,000168,_ZNK4ksys4phys9RigidBody22isEntityMotionFlag40OnEv +0x0000007100f95384,O,000280,_ZN4ksys4phys9RigidBody27resetInertiaAndCenterOfMassEv +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 -0x0000007101672268,U,000080, -0x00000071016722b8,U,000224, -0x0000007101672398,U,000084, -0x00000071016723ec,U,000096, -0x000000710167244c,U,000152, -0x00000071016724e4,U,000096, -0x0000007101672544,U,000152, -0x00000071016725dc,U,000060, -0x0000007101672618,U,000876, -0x0000007101672984,U,000104, -0x00000071016729ec,U,001952, -0x000000710167318c,U,000324, -0x00000071016732d0,U,000320, -0x0000007101673410,U,002464, -0x0000007101673db0,U,000896, -0x0000007101674130,U,000012,hkpPhysicsData::getClass +0x0000007101672214,L,000084,hkpUserShapeDisplayBuilder::dtor +0x0000007101672268,L,000080, +0x00000071016722b8,L,000224, +0x0000007101672398,L,000084, +0x00000071016723ec,L,000096, +0x000000710167244c,L,000152, +0x00000071016724e4,L,000096, +0x0000007101672544,L,000152, +0x00000071016725dc,L,000060, +0x0000007101672618,L,000876, +0x0000007101672984,L,000104, +0x00000071016729ec,L,001952, +0x000000710167318c,L,000324, +0x00000071016732d0,L,000320, +0x0000007101673410,L,002464, +0x0000007101673db0,L,000896, +0x0000007101674130,L,000012,hkpPhysicsData::getClass 0x000000710167413c,L,000020, 0x0000007101674150,L,000012, 0x000000710167415c,L,000176, diff --git a/lib/hkStubs/CMakeLists.txt b/lib/hkStubs/CMakeLists.txt index 96567fb4..41100b10 100644 --- a/lib/hkStubs/CMakeLists.txt +++ b/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 diff --git a/lib/hkStubs/Havok/Common/Base/Math/Matrix/hkMatrix3f.h b/lib/hkStubs/Havok/Common/Base/Math/Matrix/hkMatrix3f.h index 2c758615..9445f303 100644 --- a/lib/hkStubs/Havok/Common/Base/Math/Matrix/hkMatrix3f.h +++ b/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 + HK_FORCE_INLINE hkSimdFloat32 get() const; + template + HK_FORCE_INLINE void set(hkSimdFloat32Parameter s); HK_FORCE_INLINE hkVector4f& getColumn(int i); HK_FORCE_INLINE const hkVector4f& getColumn(int i) const; + template + 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 +inline hkSimdFloat32 hkMatrix3f::get() const { + return getColumn().template getComponent(); +} + +template +inline void hkMatrix3f::set(hkSimdFloat32Parameter s) { + getColumn().template setComponent(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 +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(); + d->m_col1 = hkVector4f::getConstant(); + d->m_col2 = hkVector4f::getConstant(); +} diff --git a/lib/hkStubs/Havok/Common/Base/Math/Matrix/hkTransformf.h b/lib/hkStubs/Havok/Common/Base/Math/Matrix/hkTransformf.h index 0e4dab59..2c27cd33 100644 --- a/lib/hkStubs/Havok/Common/Base/Math/Matrix/hkTransformf.h +++ b/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(); +} diff --git a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h index 2e9b145f..62a1ae12 100644 --- a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h +++ b/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 + 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); } diff --git a/lib/hkStubs/Havok/Common/GeometryUtilities/Inertia/hkInertiaTensorComputer.h b/lib/hkStubs/Havok/Common/GeometryUtilities/Inertia/hkInertiaTensorComputer.h new file mode 100644 index 00000000..f6e2ebab --- /dev/null +++ b/lib/hkStubs/Havok/Common/GeometryUtilities/Inertia/hkInertiaTensorComputer.h @@ -0,0 +1,115 @@ +#pragma once + +#include + +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& 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); +}; diff --git a/lib/hkStubs/Havok/Physics/Constraint/hkpConstraintInstance.h b/lib/hkStubs/Havok/Physics/Constraint/hkpConstraintInstance.h index a175f9a6..f91cc7df 100644 --- a/lib/hkStubs/Havok/Physics/Constraint/hkpConstraintInstance.h +++ b/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; +} diff --git a/lib/hkStubs/Havok/Physics2012/Dynamics/Inertia/hkpInertiaTensorComputer.h b/lib/hkStubs/Havok/Physics2012/Dynamics/Inertia/hkpInertiaTensorComputer.h new file mode 100644 index 00000000..6a90f024 --- /dev/null +++ b/lib/hkStubs/Havok/Physics2012/Dynamics/Inertia/hkpInertiaTensorComputer.h @@ -0,0 +1,32 @@ +#pragma once + +#include + +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); +}; diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp index e9c984e5..4aa618ee 100644 --- a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp +++ b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp @@ -1,6 +1,9 @@ #include "KingSystem/Physics/RigidBody/physRigidBody.h" +#include +#include #include #include +#include #include #include #include @@ -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, ¢er_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(); diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.h b/src/KingSystem/Physics/RigidBody/physRigidBody.h index b6013250..a308227d 100644 --- a/src/KingSystem/Physics/RigidBody/physRigidBody.h +++ b/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();