ksys/phys: Add Phantom

data/uking_functions.csv

@@ -1897,7 +1897,7 @@ Address,Quality,Size,Name
 0x0000007100062054,U,000204,act::DummyUserTag::rtti1
 0x0000007100062120,U,000092,
 0x000000710006217c,O,000012,_ZNK4ksys4phys7UserTag7getNameEv
-0x0000007100062188,O,000012,_ZNK4ksys4phys7UserTag8getName2Ev
+0x0000007100062188,O,000012,_ZNK4ksys4phys7UserTag7getNameEPNS0_9RigidBodyE
 0x0000007100062194,U,000020,
 0x00000071000621a8,U,000052,
 0x00000071000621dc,O,000048,_ZN5uking6action14MotorcycleWaitC1ERKN4ksys3act2ai10ActionBase7InitArgE
@@ -73750,7 +73750,7 @@ Address,Quality,Size,Name
 0x0000007100d40690,O,000012,_ZNK4ksys3act14PhysicsUserTag7getNameEv
 0x0000007100d4069c,U,000064,act::PhysicsUserTag::m4
 0x0000007100d406dc,U,000008,act::PhysicsUserTag::m7
-0x0000007100d406e4,O,000028,_ZNK4ksys3act14PhysicsUserTag8getName2Ev
+0x0000007100d406e4,O,000028,_ZNK4ksys3act14PhysicsUserTag7getNameEPNS_4phys9RigidBodyE
 0x0000007100d40700,O,000204,_ZNK4ksys3act14PhysicsUserTag27checkDerivedRuntimeTypeInfoEPKN4sead15RuntimeTypeInfo9InterfaceE
 0x0000007100d407cc,O,000092,_ZNK4ksys3act14PhysicsUserTag18getRuntimeTypeInfoEv
 0x0000007100d40828,O,000360,_ZN4ksys3map12PlacementMapC1Ev
@@ -83727,22 +83727,22 @@ Address,Quality,Size,Name
 0x0000007100fb1920,O,000092,_ZNK4ksys4phys28SphereBasedClosestPointQuery18getRuntimeTypeInfoEv
 0x0000007100fb197c,O,000140,_ZNK4sead15RuntimeTypeInfo6DeriveIN4ksys4phys17ClosestPointQueryEE9isDerivedEPKNS0_9InterfaceE
 0x0000007100fb1a08,O,000140,_ZNK4sead15RuntimeTypeInfo6DeriveIN4ksys4phys25ClosestPointQueryWithInfoEE9isDerivedEPKNS0_9InterfaceE
-0x0000007100fb1a94,U,000312,
-0x0000007100fb1bcc,U,000240,
-0x0000007100fb1cbc,U,000020,
-0x0000007100fb1cd0,U,000124,
-0x0000007100fb1d4c,U,000124,
-0x0000007100fb1dc8,U,000196,
-0x0000007100fb1e8c,U,000080,
-0x0000007100fb1edc,U,000004,
-0x0000007100fb1ee0,U,000088,
-0x0000007100fb1f38,U,000228,
-0x0000007100fb201c,U,000008,
-0x0000007100fb2024,U,000004,nullsub_4240
-0x0000007100fb2028,U,000112,
-0x0000007100fb2098,U,000092,
-0x0000007100fb20f4,U,000004,nullsub_4241
-0x0000007100fb20f8,U,000004,j__ZdlPv_1003
+0x0000007100fb1a94,O,000312,_ZN4ksys4phys7Phantom4makeEPN4sead4HeapERKNS2_9BoundBox3IfEENS0_16ContactLayerTypeEPKNS0_16LayerMaskBuilderEP14hkpAabbPhantom
+0x0000007100fb1bcc,O,000240,_ZN4ksys4phys7Phantom4freeEPS1_
+0x0000007100fb1cbc,O,000020,_ZNK4ksys4phys7Phantom14isAddedToWorldEv
+0x0000007100fb1cd0,O,000124,_ZN4ksys4phys7Phantom22removePhantomFromWorldEv
+0x0000007100fb1d4c,O,000124,_ZN4ksys4phys7Phantom17addPhantomToWorldEv
+0x0000007100fb1dc8,O,000196,_ZN4ksys4phys7Phantom28updateHavokFilterInfoAndAabbEv
+0x0000007100fb1e8c,O,000080,_ZN4ksys4phys18WorldBorderPhantomC1Ev
+0x0000007100fb1edc,O,000004,_ZN4ksys4phys18WorldBorderPhantomD1Ev
+0x0000007100fb1ee0,O,000088,_ZN4ksys4phys18WorldBorderPhantomD0Ev
+0x0000007100fb1f38,O,000228,_ZN4ksys4phys18WorldBorderPhantom24addOverlappingCollidableEP13hkpCollidable
+0x0000007100fb201c,O,000008,_ZN14hkpAabbPhantom14getMotionStateEv
+0x0000007100fb2024,O,000004,_ZN10hkpPhantom27updateShapeCollectionFilterEv
+0x0000007100fb2028,O,000112,_ZNK4ksys4phys7Phantom27checkDerivedRuntimeTypeInfoEPKN4sead15RuntimeTypeInfo9InterfaceE
+0x0000007100fb2098,O,000092,_ZNK4ksys4phys7Phantom18getRuntimeTypeInfoEv
+0x0000007100fb20f4,O,000004,_ZN4ksys4phys7PhantomD2Ev
+0x0000007100fb20f8,O,000004,_ZN4ksys4phys7PhantomD0Ev
 0x0000007100fb20fc,O,000308,_ZN4ksys4phys10ContactMgrC1Ev
 0x0000007100fb2230,O,000180,_ZN4ksys4phys10ContactMgrD1Ev
 0x0000007100fb22e4,O,000036,_ZN4ksys4phys10ContactMgrD0Ev

src/KingSystem/ActorSystem/actPhysicsUserTag.cpp

@@ -27,7 +27,7 @@ const sead::SafeString& PhysicsUserTag::getName() const {
     return mActor->getName();
 }
 
-const sead::SafeString& PhysicsUserTag::getName2() const {
+const sead::SafeString& PhysicsUserTag::getName(phys::RigidBody* rigid_body) const {
     return getName();
 }
 

src/KingSystem/ActorSystem/actPhysicsUserTag.h

@@ -26,7 +26,7 @@ public:
     void m5() override;
     const sead::SafeString& getName() const override;
     void m7(phys::RigidBody* rigid_body, int a) override;
-    const sead::SafeString& getName2() const override;
+    const sead::SafeString& getName(phys::RigidBody* rigid_body) const override;
 
 private:
     Actor* mActor = nullptr;

src/KingSystem/Physics/CMakeLists.txt

@@ -137,6 +137,8 @@ target_sources(uking PRIVATE
   System/physMaterialTable.h
   System/physParamSet.cpp
   System/physParamSet.h
+  System/physPhantom.cpp
+  System/physPhantom.h
   System/physQueryContactPointInfo.cpp
   System/physQueryContactPointInfo.h
   System/physRayCast.cpp

src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h

@@ -54,6 +54,9 @@ public:
     // 0x0000007100fa6ebc
     void removeRigidBody(ContactLayerType type, RigidBody* body);
 
+    // 0x0000007100fa728c
+    void onMaxPositionExceeded(ContactLayerType layer_type, RigidBody* body);
+
     // 0x0000007100fa7340
     void addImpulse(RigidBody* body_a, RigidBody* body_b, float impulse);
 

src/KingSystem/Physics/System/physPhantom.cpp

@@ -0,0 +1,122 @@
+#include "KingSystem/Physics/System/physPhantom.h"
+#include <Havok/Physics2012/Dynamics/World/hkpWorld.h>
+#include <math/seadVector.h>
+#include <optional>
+#include <prim/seadSafeString.h>
+#include "KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h"
+#include "KingSystem/Physics/System/physSystem.h"
+#include "KingSystem/Physics/System/physUserTag.h"
+#include "KingSystem/Physics/physConversions.h"
+#include "KingSystem/Utils/Debug.h"
+#include "KingSystem/Utils/HeapUtil.h"
+
+namespace ksys::phys {
+
+Phantom* Phantom::make(sead::Heap* heap, const sead::BoundBox3f& aabb, ContactLayerType layer_type,
+                       const LayerMaskBuilder* layer_mask_builder, hkpAabbPhantom* havok_phantom) {
+    auto* phantom = util::allocWithAlign<Phantom>(heap, 0x10, aabb, layer_type);
+
+    phantom->initHavokPhantom(havok_phantom);
+
+    if (layer_mask_builder != nullptr)
+        phantom->setLayerMask(*layer_mask_builder);
+
+    return phantom;
+}
+
+void Phantom::free(Phantom* phantom) {
+    phantom->removePhantomFromWorld();
+    phantom->mHavokPhantom->removeReference();
+    delete phantom;
+}
+
+Phantom::Phantom(const sead::BoundBox3f& aabb, ContactLayerType layer_type)
+    : mAabb(aabb), mLayerType(layer_type) {}
+
+void Phantom::initHavokPhantom(hkpAabbPhantom* havok_phantom) {
+    hkAabb aabb;
+    loadFromVec3(&aabb.m_min, mAabb.getMin());
+    loadFromVec3(&aabb.m_max, mAabb.getMax());
+
+    if (havok_phantom) {
+        mHavokPhantom = havok_phantom;
+        havok_phantom->setAabb(aabb);
+    } else {
+        mHavokPhantom = new hkpAabbPhantom(aabb);
+    }
+}
+
+void Phantom::setLayerMask(const LayerMaskBuilder& builder) {
+    mLayerMask = builder.getMasks()[int(mLayerType)].layers;
+    mDirtyLayerMask = true;
+    mHavokPhantom->getCollidableRw()->setCollisionFilterInfo(mLayerMask);
+}
+
+bool Phantom::isAddedToWorld() const {
+    return mHavokPhantom->isAddedToWorld();
+}
+
+void Phantom::addPhantomToWorld() {
+    if (isAddedToWorld())
+        return;
+
+    System::instance()->lockWorld(mLayerType);
+    System::instance()->getHavokWorld(mLayerType)->addPhantom(mHavokPhantom);
+    System::instance()->unlockWorld(mLayerType);
+}
+
+void Phantom::removePhantomFromWorld() {
+    if (!isAddedToWorld())
+        return;
+
+    System::instance()->lockWorld(mLayerType);
+    System::instance()->getHavokWorld(mLayerType)->removePhantom(mHavokPhantom);
+    System::instance()->unlockWorld(mLayerType);
+}
+
+void Phantom::updateHavokFilterInfoAndAabb() {
+    bool added = isAddedToWorld();
+    if (added)
+        System::instance()->lockWorld(mLayerType);
+
+    if (mDirtyLayerMask) {
+        mHavokPhantom->getCollidableRw()->setCollisionFilterInfo(mLayerMask);
+        mDirtyLayerMask = false;
+    }
+
+    hkAabb aabb;
+    loadFromVec3(&aabb.m_min, mAabb.getMin());
+    loadFromVec3(&aabb.m_max, mAabb.getMax());
+    mHavokPhantom->setAabb(aabb);
+
+    if (added)
+        System::instance()->unlockWorld(mLayerType);
+}
+
+WorldBorderPhantom::WorldBorderPhantom()
+    : hkpAabbPhantom(hkAabb{hkVector4f::zero(), hkVector4f(1, 1, 1, 0)}) {}
+
+WorldBorderPhantom::~WorldBorderPhantom() = default;
+
+void WorldBorderPhantom::addOverlappingCollidable(hkpCollidable* collidable) {
+    auto* body = getRigidBody(*collidable);
+    if (!body)
+        return;
+
+    auto position = sead::Vector3f::zero;
+    auto user_name = sead::SafeString::cEmptyString;
+    auto body_name = sead::SafeString::cEmptyString;
+
+    body->getPosition(&position);
+    body_name = body->getHkBodyName();
+    if (auto* tag = body->getUserTag()) {
+        user_name = tag->getName(body);
+    }
+
+    util::PrintDebugFmt("rigid body overlapped with WorldBorderPhantom: %f %f %f %s %s", position.x,
+                        position.y, position.z, user_name, body_name);
+
+    System::instance()->getRigidBodyRequestMgr()->onMaxPositionExceeded(body->getLayerType(), body);
+}
+
+}  // namespace ksys::phys

src/KingSystem/Physics/System/physPhantom.h

@@ -0,0 +1,60 @@
+#pragma once
+
+#include <Havok/Physics2012/Dynamics/Phantom/hkpAabbPhantom.h>
+#include <math/seadBoundBox.h>
+#include <math/seadVector.h>
+#include <prim/seadBitFlag.h>
+#include <prim/seadRuntimeTypeInfo.h>
+#include "KingSystem/Physics/physDefines.h"
+
+namespace ksys::phys {
+
+class LayerMaskBuilder;
+
+class Phantom {
+    SEAD_RTTI_BASE(Phantom)
+public:
+    /// @param havok_phantom If null, a hkpAabbPhantom will be automatically created.
+    static Phantom* make(sead::Heap* heap, const sead::BoundBox3f& aabb,
+                         ContactLayerType layer_type, const LayerMaskBuilder* layer_mask_builder,
+                         hkpAabbPhantom* havok_phantom = nullptr);
+    static void free(Phantom* phantom);
+
+    Phantom(const sead::BoundBox3f& aabb, ContactLayerType layer_type);
+
+    virtual ~Phantom() = default;
+
+    hkpAabbPhantom* getHavokPhantom() const { return mHavokPhantom; }
+    const sead::BoundBox3f& getAabb() const { return mAabb; }
+    ContactLayerType getLayerType() const { return mLayerType; }
+    sead::BitFlag32 getLayerMask() const { return mLayerMask; }
+
+    /// @param havok_phantom If null, a hkpAabbPhantom will be automatically created.
+    void initHavokPhantom(hkpAabbPhantom* havok_phantom = nullptr);
+    void setLayerMask(const LayerMaskBuilder& builder);
+
+    bool isAddedToWorld() const;
+    void addPhantomToWorld();
+    void removePhantomFromWorld();
+
+    /// Use this to update the collision filter info and the AABB of the Havok phantom.
+    void updateHavokFilterInfoAndAabb();
+
+private:
+    hkpAabbPhantom* mHavokPhantom{};
+    sead::BoundBox3f mAabb;
+    ContactLayerType mLayerType;
+    sead::BitFlag32 mLayerMask;
+    bool mDirtyLayerMask = false;
+};
+
+class WorldBorderPhantom : public hkpAabbPhantom {
+public:
+    HK_DECLARE_CLASS_ALLOCATOR(WorldBorderPhantom)
+
+    WorldBorderPhantom();
+    ~WorldBorderPhantom() override;
+    void addOverlappingCollidable(hkpCollidable* collidable) override;
+};
+
+}  // namespace ksys::phys

src/KingSystem/Physics/System/physUserTag.h

@@ -22,7 +22,9 @@ public:
     virtual void m5();
     virtual const sead::SafeString& getName() const { return sead::SafeString::cEmptyString; }
     virtual void m7(RigidBody* rigid_body, int a);
-    virtual const sead::SafeString& getName2() const { return sead::SafeString::cEmptyString; }
+    virtual const sead::SafeString& getName(RigidBody* rigid_body) const {
+        return sead::SafeString::cEmptyString;
+    }
     virtual ~UserTag() = default;
 };
 KSYS_CHECK_SIZE_NX150(UserTag, 0x8);

src/KingSystem/Utils/HeapUtil.h

@@ -126,8 +126,8 @@ KSYS_ALWAYS_INLINE inline sead::Heap* tryCreateDualHeap(sead::Heap* parent) {
 
 /// Allocate uninitialised, suitably aligned storage for an object of type T.
 template <typename T>
-inline void* allocStorage(sead::Heap* heap) {
-    return heap->alloc(sizeof(T), alignof(T));
+inline void* allocStorage(sead::Heap* heap, size_t alignment = alignof(T)) {
+    return heap->alloc(sizeof(T), static_cast<int>(alignment));
 }
 
 template <typename T, typename... Args>
@@ -136,4 +136,10 @@ inline T* alloc(sead::Heap* heap, Args&&... args) {
     return new (storage) T(std::forward<Args>(args)...);
 }
 
+template <typename T, typename... Args>
+inline T* allocWithAlign(sead::Heap* heap, size_t alignment, Args&&... args) {
+    void* storage = allocStorage<T>(heap, alignment);
+    return new (storage) T(std::forward<Args>(args)...);
+}
+
 }  // namespace ksys::util