ksys/phys: Add RayHitCollector

9 files changed, 540 insertions, 1 deletions

diff --git a/lib/hkStubs/CMakeLists.txt b/lib/hkStubs/CMakeLists.txt
index 6bcca644..688bad81 100644
--- a/lib/hkStubs/CMakeLists.txt
+++ b/lib/hkStubs/CMakeLists.txt
@@ -114,6 +114,7 @@ add_library(hkStubs OBJECT
   Havok/Physics2012/Collide/Query/hkpRayHitCollector.h
   Havok/Physics2012/Collide/Query/CastUtil/hkpLinearCastInput.h
   Havok/Physics2012/Collide/Query/CastUtil/hkpWorldRayCastInput.h
+  Havok/Physics2012/Collide/Query/CastUtil/hkpWorldRayCastOutput.h
   Havok/Physics2012/Collide/Query/Collector/PointCollector/hkpAllCdPointCollector.h
   Havok/Physics2012/Collide/Query/Collector/PointCollector/hkpClosestCdPointCollector.h
   Havok/Physics2012/Collide/Query/Collector/PointCollector/hkpRootCdPoint.h
@@ -133,6 +134,7 @@ add_library(hkStubs OBJECT
   Havok/Physics2012/Collide/Shape/Convex/ConvexVertices/hkpConvexVerticesShape.h
   Havok/Physics2012/Collide/Shape/Convex/Cylinder/hkpCylinderShape.h
   Havok/Physics2012/Collide/Shape/Convex/Sphere/hkpSphereShape.h
+  Havok/Physics2012/Collide/Shape/Convex/Triangle/hkpTriangleShape.h
   Havok/Physics2012/Collide/Shape/HeightField/hkpHeightFieldShape.h
   Havok/Physics2012/Collide/Shape/HeightField/hkpSphereRepShape.h
   Havok/Physics2012/Collide/Shape/HeightField/Plane/hkpPlaneShape.h
@@ -188,6 +190,8 @@ add_library(hkStubs OBJECT
   Havok/Physics2012/Dynamics/World/Util/hkpWorldConstraintUtil.h
 
   Havok/Physics2012/Internal/Collide/Mopp/Code/hkpMoppCode.h
+  Havok/Physics2012/Internal/Collide/StaticCompound/hkpShapeKeyTable.h
+  Havok/Physics2012/Internal/Collide/StaticCompound/hkpStaticCompoundShape.h
   Havok/Physics2012/Internal/Solver/Contact/hkpSimpleContactConstraintDataInfo.h
 
   Havok/Physics2012/Utilities/CharacterControl/CharacterRigidBody/hkpCharacterRigidBody.h
diff --git a/lib/hkStubs/Havok/Common/Base/Container/Array/hkArray.h b/lib/hkStubs/Havok/Common/Base/Container/Array/hkArray.h
index 3a56e09b..f9aa8ee7 100644
--- a/lib/hkStubs/Havok/Common/Base/Container/Array/hkArray.h
+++ b/lib/hkStubs/Havok/Common/Base/Container/Array/hkArray.h
@@ -122,6 +122,8 @@ public:
 
     HK_FORCE_INLINE void clearAndDeallocate();
     HK_FORCE_INLINE void pushBack(const T& e);
+    HK_FORCE_INLINE void setSize(int size);
+    HK_FORCE_INLINE void setSize(int size, const T& fill);
 
 protected:
     HK_FORCE_INLINE hkArray(const hkArray& other);
@@ -288,6 +290,22 @@ inline hkResult hkArrayBase<T>::_reserveExactly(hkMemoryAllocator& alloc, int n)
 }
 
 template <typename T>
+inline void hkArrayBase<T>::_setSize(hkMemoryAllocator& alloc, int n) {
+    _reserve(alloc, n);
+    hkArrayUtil::destruct(m_data + n, m_size - n);
+    hkArrayUtil::construct(m_data + m_size, n - m_size);
+    m_size = n;
+}
+
+template <typename T>
+inline void hkArrayBase<T>::_setSize(hkMemoryAllocator& alloc, int n, const T& fill) {
+    _reserve(alloc, n);
+    hkArrayUtil::destruct(m_data + n, m_size - n);
+    hkArrayUtil::constructWithCopy(m_data + m_size, n - m_size, fill);
+    m_size = n;
+}
+
+template <typename T>
 inline typename hkArrayBase<T>::iterator hkArrayBase<T>::begin() {
     return m_data;
 }
@@ -385,6 +403,16 @@ inline void hkArray<T, Allocator>::pushBack(const T& e) {
     this->_pushBack(AllocatorType().get(), e);
 }
 
+template <typename T, typename Allocator>
+inline void hkArray<T, Allocator>::setSize(int size) {
+    this->_setSize(AllocatorType().get(), size);
+}
+
+template <typename T, typename Allocator>
+inline void hkArray<T, Allocator>::setSize(int size, const T& fill) {
+    this->_setSize(AllocatorType().get(), size, fill);
+}
+
 template <typename T, unsigned N, typename Allocator>
 inline hkInplaceArray<T, N, Allocator>::hkInplaceArray(int size)
     : hkArray<T, Allocator>(m_storage, size, N) {}
diff --git a/lib/hkStubs/Havok/Common/Base/Math/Matrix/hkMatrix3f.h b/lib/hkStubs/Havok/Common/Base/Math/Matrix/hkMatrix3f.h
index c1d3fe82..dc5d7934 100644
--- a/lib/hkStubs/Havok/Common/Base/Math/Matrix/hkMatrix3f.h
+++ b/lib/hkStubs/Havok/Common/Base/Math/Matrix/hkMatrix3f.h
@@ -4,6 +4,9 @@ class hkMatrix3f {
 public:
     hkMatrix3f() {}  // NOLINT(modernize-use-equals-default)
 
+    HK_FORCE_INLINE hkMatrix3f(const hkMatrix3f& other);
+    HK_FORCE_INLINE hkMatrix3f& operator=(const hkMatrix3f& other);
+
     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>
@@ -21,6 +24,10 @@ public:
     HK_FORCE_INLINE void setZero();
     HK_FORCE_INLINE void setIdentity();
 
+    void setMul(const hkMatrix3f& a, const hkMatrix3f& b);
+    HK_FORCE_INLINE void _setMul(const hkMatrix3f& a, const hkMatrix3f& b);
+    void mul(const hkMatrix3f& a);
+
     hkVector4f m_col0;
     hkVector4f m_col1;
     hkVector4f m_col2;
diff --git a/lib/hkStubs/Havok/Common/Base/Math/Matrix/hkMatrix3f.inl b/lib/hkStubs/Havok/Common/Base/Math/Matrix/hkMatrix3f.inl
index b55ed805..db5a775f 100644
--- a/lib/hkStubs/Havok/Common/Base/Math/Matrix/hkMatrix3f.inl
+++ b/lib/hkStubs/Havok/Common/Base/Math/Matrix/hkMatrix3f.inl
@@ -1,3 +1,16 @@
+inline hkMatrix3f::hkMatrix3f(const hkMatrix3f& other) {
+    m_col0 = other.getColumn<0>();
+    m_col1 = other.getColumn<1>();
+    m_col2 = other.getColumn<2>();
+}
+
+inline hkMatrix3f& hkMatrix3f::operator=(const hkMatrix3f& other) {
+    m_col0 = other.getColumn<0>();
+    m_col1 = other.getColumn<1>();
+    m_col2 = other.getColumn<2>();
+    return *this;
+}
+
 inline hkFloat32& hkMatrix3f::operator()(int row, int col) {
     return getColumn(col)(row);
 }
@@ -54,3 +67,9 @@ inline void hkMatrix3f::setIdentity() {
     d->m_col1 = hkVector4f::getConstant<HK_QUADREAL_0100>();
     d->m_col2 = hkVector4f::getConstant<HK_QUADREAL_0010>();
 }
+
+inline void hkMatrix3f::_setMul(const hkMatrix3f& a, const hkMatrix3f& b) {
+    m_col0._setRotatedDir(a, b.getColumn<0>());
+    m_col1._setRotatedDir(a, b.getColumn<1>());
+    m_col2._setRotatedDir(a, b.getColumn<2>());
+}
diff --git a/lib/hkStubs/Havok/Common/Base/Memory/Router/hkMemoryRouter.h b/lib/hkStubs/Havok/Common/Base/Memory/Router/hkMemoryRouter.h
index 17766817..1d31bda6 100644
--- a/lib/hkStubs/Havok/Common/Base/Memory/Router/hkMemoryRouter.h
+++ b/lib/hkStubs/Havok/Common/Base/Memory/Router/hkMemoryRouter.h
@@ -99,7 +99,8 @@ HK_FORCE_INLINE void hkDeallocateChunk(TYPE* ptr, int numberOfObjects) {
                 p, (b->getMemorySizeAndFlags() == 0xffff) ? static_cast<int>(nbytes) :             \
                                                             b->getMemorySizeAndFlags());           \
         } else {                                                                                   \
-            hkMemoryRouter::getInstance().ALLOCATOR().blockFree(p, sizeof(CLASS_TYPE));            \
+            if (p)                                                                                 \
+                hkMemoryRouter::getInstance().ALLOCATOR().blockFree(p, sizeof(CLASS_TYPE));        \
         }                                                                                          \
     }                                                                                              \
     HK_FORCE_INLINE void* operator new(hk_size_t, void* p) { return p; }                           \
diff --git a/lib/hkStubs/Havok/Physics2012/Collide/Query/CastUtil/hkpWorldRayCastOutput.h b/lib/hkStubs/Havok/Physics2012/Collide/Query/CastUtil/hkpWorldRayCastOutput.h
new file mode 100644
index 00000000..615563a1
--- /dev/null
+++ b/lib/hkStubs/Havok/Physics2012/Collide/Query/CastUtil/hkpWorldRayCastOutput.h
@@ -0,0 +1,30 @@
+#pragma once
+
+#include <Havok/Physics2012/Collide/Shape/Query/hkpShapeRayCastOutput.h>
+
+class hkpCollidable;
+
+struct hkpWorldRayCastOutput : public hkpShapeRayCastOutput {
+    HK_DECLARE_CLASS_ALLOCATOR(hkpWorldRayCastOutput)
+
+    inline hkBool hasHit() const;
+
+    inline hkBool operator<(const hkpWorldRayCastOutput& b) const;
+
+    inline void reset();
+
+    const hkpCollidable* m_rootCollidable = nullptr;
+};
+
+inline hkBool hkpWorldRayCastOutput::hasHit() const {
+    return m_rootCollidable != nullptr;
+}
+
+inline hkBool hkpWorldRayCastOutput::operator<(const hkpWorldRayCastOutput& b) const {
+    return m_hitFraction < b.m_hitFraction;
+}
+
+inline void hkpWorldRayCastOutput::reset() {
+    m_rootCollidable = nullptr;
+    hkpShapeRayCastOutput::reset();
+}
diff --git a/lib/hkStubs/Havok/Physics2012/Collide/Shape/Convex/Triangle/hkpTriangleShape.h b/lib/hkStubs/Havok/Physics2012/Collide/Shape/Convex/Triangle/hkpTriangleShape.h
new file mode 100644
index 00000000..8a1c6724
--- /dev/null
+++ b/lib/hkStubs/Havok/Physics2012/Collide/Shape/Convex/Triangle/hkpTriangleShape.h
@@ -0,0 +1,160 @@
+#pragma once
+
+#include <Havok/Physics2012/Collide/Shape/Convex/hkpConvexShape.h>
+#include <Havok/Physics2012/Collide/Util/Welding/hkpWeldingUtility.h>
+
+class hkpTriangleShape : public hkpConvexShape {
+public:
+    HK_DECLARE_CLASS_ALLOCATOR(hkpTriangleShape)
+    HK_DECLARE_REFLECTION()
+    HKCD_DECLARE_SHAPE_TYPE(hkcdShapeType::TRIANGLE)
+
+public:
+    explicit hkpTriangleShape(hkFinishLoadedObjectFlag flag);
+
+    HK_FORCE_INLINE explicit hkpTriangleShape(
+        hkReal radius = hkConvexShapeDefaultRadius, hkUint16 weldingInfo = 0,
+        hkpWeldingUtility::WeldingType type = hkpWeldingUtility::WELDING_TYPE_NONE);
+
+    HK_FORCE_INLINE hkpTriangleShape(const hkVector4& v0, const hkVector4& v1, const hkVector4& v2,
+                                     hkReal radius = hkConvexShapeDefaultRadius);
+
+    HK_FORCE_INLINE const hkVector4* getVertices() const;
+    HK_FORCE_INLINE hkVector4& getVertex(int i);
+    HK_FORCE_INLINE const hkVector4& getVertex(int i) const;
+    template <int I>
+    HK_FORCE_INLINE const hkVector4& getVertex() const;
+
+    HK_FORCE_INLINE void setVertex(int i, const hkVector4& vertex);
+    template <int I>
+    HK_FORCE_INLINE void setVertex(hkVector4Parameter vertex);
+
+    HK_FORCE_INLINE hkUint16 getWeldingInfo() const;
+    HK_FORCE_INLINE void setWeldingInfo(hkUint16 info);
+    HK_FORCE_INLINE hkpWeldingUtility::WeldingType getWeldingType() const;
+    HK_FORCE_INLINE void setWeldingType(hkpWeldingUtility::WeldingType type);
+
+    HK_FORCE_INLINE bool isExtruded() const;
+    HK_FORCE_INLINE const hkVector4& getExtrusion() const;
+    HK_FORCE_INLINE void setExtrusion(const hkVector4& extrusion);
+
+    void getSupportingVertex(hkVector4Parameter direction,
+                             hkcdVertex& supportingVertexOut) const override;
+
+    void convertVertexIdsToVertices(const hkpVertexId* ids, int numIds,
+                                    hkcdVertex* verticesOut) const override;
+
+    int weldContactPoint(hkpVertexId* featurePoints, hkUint8& numFeaturePoints,
+                         hkVector4& contactPointWs, const hkTransform* thisObjTransform,
+                         const hkpConvexShape* collidingShape,
+                         const hkTransform* collidingTransform,
+                         hkVector4& separatingNormalInOut) const override;
+
+    void getCentre(hkVector4& centreOut) const override;
+
+    HK_FORCE_INLINE int getNumCollisionSpheres() const override;
+
+    const hkSphere* getCollisionSpheres(hkSphere* sphereBuffer) const override;
+
+    void getAabb(const hkTransform& localToWorld, hkReal tolerance, hkAabb& out) const override;
+
+    hkBool castRay(const hkpShapeRayCastInput& input,
+                   hkpShapeRayCastOutput& results) const override;
+
+    hkVector4Comparison castRayBundle(const hkpShapeRayBundleCastInput& input,
+                                      hkpShapeRayBundleCastOutput& results,
+                                      hkVector4ComparisonParameter mask) const override;
+
+    void getFirstVertex(hkVector4& v) const override;
+
+protected:
+    hkUint16 m_weldingInfo;
+    hkEnum<hkpWeldingUtility::WeldingType, hkUint8> m_weldingType;
+    hkUint8 m_isExtruded;
+
+    hkVector4 m_vertexA;
+    hkVector4 m_vertexB;
+    hkVector4 m_vertexC;
+
+    hkVector4 m_extrusion;
+};
+
+inline hkpTriangleShape::hkpTriangleShape(hkReal radius, hkUint16 weldingInfo,
+                                          hkpWeldingUtility::WeldingType type)
+    : hkpConvexShape(HKCD_SHAPE_TYPE_FROM_CLASS(hkpTriangleShape), radius) {
+    setWeldingInfo(weldingInfo);
+    setWeldingType(type);
+    m_extrusion.setZero();
+    m_isExtruded = 0;
+}
+
+inline hkpTriangleShape::hkpTriangleShape(const hkVector4& v0, const hkVector4& v1,
+                                          const hkVector4& v2, hkReal radius)
+    : hkpConvexShape(HKCD_SHAPE_TYPE_FROM_CLASS(hkpTriangleShape), radius) {
+    m_vertexA = v0;
+    m_vertexB = v1;
+    m_vertexC = v2;
+
+    setWeldingInfo(0);
+    setWeldingType(hkpWeldingUtility::WELDING_TYPE_NONE);
+    m_extrusion.setZero();
+    m_isExtruded = 0;
+}
+
+inline const hkVector4* hkpTriangleShape::getVertices() const {
+    return &m_vertexA;
+}
+
+inline hkVector4& hkpTriangleShape::getVertex(int i) {
+    return (&m_vertexA)[i];
+}
+
+inline const hkVector4& hkpTriangleShape::getVertex(int i) const {
+    return (&m_vertexA)[i];
+}
+
+template <int I>
+inline const hkVector4& hkpTriangleShape::getVertex() const {
+    return (&m_vertexA)[I];
+}
+
+inline void hkpTriangleShape::setVertex(int i, const hkVector4& vertex) {
+    getVertex(i) = vertex;
+}
+
+template <int I>
+inline void hkpTriangleShape::setVertex(const hkVector4f& vertex) {
+    (&m_vertexA)[I] = vertex;
+}
+
+inline hkUint16 hkpTriangleShape::getWeldingInfo() const {
+    return m_weldingInfo;
+}
+
+inline void hkpTriangleShape::setWeldingInfo(hkUint16 info) {
+    m_weldingInfo = info;
+}
+
+inline hkpWeldingUtility::WeldingType hkpTriangleShape::getWeldingType() const {
+    return m_weldingType;
+}
+
+inline void hkpTriangleShape::setWeldingType(hkpWeldingUtility::WeldingType type) {
+    m_weldingType = type;
+}
+
+inline bool hkpTriangleShape::isExtruded() const {
+    return m_isExtruded;
+}
+
+inline const hkVector4& hkpTriangleShape::getExtrusion() const {
+    return m_extrusion;
+}
+
+inline void hkpTriangleShape::setExtrusion(const hkVector4& extrusion) {
+    m_extrusion = extrusion;
+}
+
+inline int hkpTriangleShape::getNumCollisionSpheres() const {
+    return 3 + 3 * m_isExtruded;
+}
diff --git a/lib/hkStubs/Havok/Physics2012/Internal/Collide/StaticCompound/hkpShapeKeyTable.h b/lib/hkStubs/Havok/Physics2012/Internal/Collide/StaticCompound/hkpShapeKeyTable.h
new file mode 100644
index 00000000..aa54ddc5
--- /dev/null
+++ b/lib/hkStubs/Havok/Physics2012/Internal/Collide/StaticCompound/hkpShapeKeyTable.h
@@ -0,0 +1,43 @@
+#pragma once
+
+#include <Havok/Common/Base/hkBase.h>
+#include <Havok/Physics2012/Collide/Shape/hkpShapeBase.h>
+
+class hkpShapeKeyTable {
+public:
+    HK_DECLARE_CLASS_ALLOCATOR(hkpShapeKeyTable)
+    HK_DECLARE_REFLECTION()
+
+    enum { NUM_LISTS = 32, NUM_SLOTS_PER_BLOCK = 63 };
+
+    // A block of shape key slots.
+    // Free slots have a value of HK_INVALID_SHAPEKEY.
+    struct Block {
+        HK_DECLARE_CLASS_ALLOCATOR(Block)
+        HK_DECLARE_REFLECTION()
+
+        Block();
+        ~Block();
+
+        alignas(16) hkpShapeKey m_slots[NUM_SLOTS_PER_BLOCK];
+        Block* m_next;
+    };
+
+    enum { NUM_BYTES_PER_BLOCK = sizeof(Block) };
+
+    hkpShapeKeyTable();
+    explicit hkpShapeKeyTable(hkFinishLoadedObjectFlag flag);
+    ~hkpShapeKeyTable();
+
+    void insert(hkpShapeKey key);
+    void remove(hkpShapeKey key);
+    bool exists(hkpShapeKey key) const;
+
+    bool isEmpty() const { return (m_occupancyBitField == 0); }
+    void clear();
+    void getDistribution(hkArray<int>& counts) const;
+
+protected:
+    Block* m_lists;
+    hkUint32 m_occupancyBitField;
+};
diff --git a/lib/hkStubs/Havok/Physics2012/Internal/Collide/StaticCompound/hkpStaticCompoundShape.h b/lib/hkStubs/Havok/Physics2012/Internal/Collide/StaticCompound/hkpStaticCompoundShape.h
new file mode 100644
index 00000000..61b7b039
--- /dev/null
+++ b/lib/hkStubs/Havok/Physics2012/Internal/Collide/StaticCompound/hkpStaticCompoundShape.h
@@ -0,0 +1,247 @@
+#pragma once
+
+#include <Havok/Common/Base/hkBase.h>
+#include <Havok/Physics2012/Collide/Shape/Compound/Tree/hkpBvTreeShape.h>
+#include <Havok/Physics2012/Collide/Shape/hkpShapeContainer.h>
+#include <Havok/Physics2012/Internal/Collide/StaticCompound/hkpShapeKeyTable.h>
+
+class hkpStaticCompoundShape : public hkpBvTreeShape, public hkpShapeContainer {
+public:
+    HK_DECLARE_CLASS_ALLOCATOR(hkpStaticCompoundShape)
+    HK_DECLARE_REFLECTION()
+    HKCD_DECLARE_SHAPE_TYPE(hkcdShapeType::STATIC_COMPOUND)
+
+    struct Instance {
+    public:
+        enum {
+            FLAG_IS_LEAF = 1 << 0,
+            FLAG_HAS_TRANSFORM = 1 << 1,
+            FLAG_HAS_SCALE = 1 << 2,
+            FLAG_HAS_FLIP = 1 << 3,
+
+            FLAG_IS_DISABLED = 1 << 4,
+            FLAG_HAS_DISABLED_SMALL_KEYS = 1 << 5,
+            FLAG_HAS_DISABLED_LARGE_KEYS = 1 << 6,
+
+            FLAGS_HAS_DISABLED_KEYS = FLAG_HAS_DISABLED_SMALL_KEYS | FLAG_HAS_DISABLED_LARGE_KEYS,
+            FLAGS_ALL = (1 << 7) - 1,
+        };
+
+        enum : hkUint32 {
+            NUM_BITS_FOR_FLAGS = 7,
+            NUM_BITS_FOR_SHAPE_SIZE = 4,
+            NUM_BITS_FOR_DISABLED_SMALL_KEYS = 37
+        };
+
+    protected:
+        HK_FORCE_INLINE void setShapeSizeForSpu(hkUint32 size);
+        HK_FORCE_INLINE void setFlags(hkUint32 flags);
+
+        hkQsTransform m_transform;
+        const hkpShape* m_shape;
+
+    public:
+        HK_DECLARE_CLASS_ALLOCATOR(Instance)
+        HK_DECLARE_REFLECTION()
+
+        HK_FORCE_INLINE const hkQsTransform& getTransform() const;
+        HK_FORCE_INLINE const hkpShape* getShape() const;
+        HK_FORCE_INLINE hkUint32 getShapeSizeForSpu() const;
+        HK_FORCE_INLINE hkUint32 getFlags() const;
+
+        hkUint32 m_filterInfo;
+        hkUint32 m_childFilterInfoMask;
+        hkUlong m_userData;
+
+        friend class hkpStaticCompoundShape;
+        friend class hkpStaticCompoundShape_Internals;
+    };
+
+    enum Config {
+#ifdef HK_REAL_IS_DOUBLE
+        NUM_BYTES_FOR_TREE = 96
+#else
+        NUM_BYTES_FOR_TREE = 48
+#endif
+    };
+
+public:
+    explicit hkpStaticCompoundShape(ReferencePolicy ref = REFERENCE_POLICY_INCREMENT);
+    explicit hkpStaticCompoundShape(hkFinishLoadedObjectFlag flag);
+
+    ~hkpStaticCompoundShape() override;
+
+    int addInstance(const hkpShape* shape, const hkQsTransform& transform);
+
+    void bake();
+
+    HK_FORCE_INLINE void decomposeShapeKey(hkpShapeKey keyIn, int& instanceIdOut,
+                                           hkpShapeKey& childKeyOut) const;
+
+    HK_FORCE_INLINE hkpShapeKey composeShapeKey(int instanceId, hkpShapeKey childKey) const;
+
+    HK_FORCE_INLINE hkInt8 getNumBitsForChildShapeKey() const;
+
+    HK_FORCE_INLINE const hkArray<Instance>& getInstances() const;
+
+    HK_FORCE_INLINE void setInstanceFilterInfo(int instanceId, hkUint32 filterInfo);
+    HK_FORCE_INLINE hkUint32 getInstanceFilterInfo(int instanceId) const;
+
+    HK_FORCE_INLINE void setInstanceFilterInfoMask(int instanceId, hkUint32 filterInfoMask);
+    HK_FORCE_INLINE hkUint32 getInstanceFilterInfoMask(int instanceId) const;
+
+    HK_FORCE_INLINE void setInstanceUserData(int instanceId, hkUlong userData);
+    HK_FORCE_INLINE hkUlong getInstanceUserData(int instanceId) const;
+
+    HK_FORCE_INLINE void setInstanceExtraInfo(int instanceId, hkUint16 instanceInfo);
+    HK_FORCE_INLINE hkUint16 getInstanceExtraInfo(int instanceId) const;
+    HK_FORCE_INLINE const hkArray<hkUint16>& getInstanceExtraInfos() const;
+
+    HK_FORCE_INLINE void setInstanceEnabled(int instanceId, hkBool32 isEnabled);
+    HK_FORCE_INLINE hkBool32 isInstanceEnabled(int instanceId) const;
+
+    void setShapeKeyEnabled(hkpShapeKey key, hkBool32 isEnabled);
+    hkBool32 isShapeKeyEnabled(hkpShapeKey key) const;
+
+    void enableAllInstancesAndShapeKeys();
+
+    void castAabb(const hkpShape* shape, const hkTransform& transform, hkVector4Parameter to,
+                  hkpAabbCastCollector& collector, hkReal tolerance = 0.0f) const;
+
+    const hkpShapeContainer* getContainer() const override { return this; }
+    int calcSizeForSpu(const CalcSizeForSpuInput& input, int spuBufferSizeLeft) const override;
+    hkReal getMaximumProjection(const hkVector4& direction) const override;
+    void getAabb(const hkTransform& localToWorld, hkReal tolerance, hkAabb& out) const override;
+    hkBool castRay(const hkpShapeRayCastInput& input,
+                   hkpShapeRayCastOutput& results) const override;
+    void castRayWithCollector(const hkpShapeRayCastInput& input, const hkpCdBody& body,
+                              hkpRayHitCollector& collector) const override;
+
+    void queryAabb(const hkAabb& aabb, hkArray<hkpShapeKey>& hits) const override;
+    hkUint32 queryAabbImpl(const hkAabb& aabb, hkpShapeKey* hits, int maxNumKeys) const override;
+    void castAabbImpl(const hkAabb& from, hkVector4Parameter to,
+                      hkpAabbCastCollector& collector) const override;
+
+    int getNumChildShapes() const override;
+    hkpShapeKey getFirstKey() const override;
+    hkpShapeKey getNextKey(hkpShapeKey oldKey) const override;
+    const hkpShape* getChildShape(hkpShapeKey key, hkpShapeBuffer& buffer) const override;
+    hkUint32 getCollisionFilterInfo(hkpShapeKey key) const override;
+
+protected:
+    hkInt8 m_numBitsForChildShapeKey;
+    hkInt8 m_referencePolicy;
+    hkUint32 m_childShapeKeyMask;
+
+    hkArray<Instance> m_instances;
+    hkArray<hkUint16> m_instanceExtraInfos;
+    hkpShapeKeyTable m_disabledLargeShapeKeyTable;
+
+    alignas(16) hkUint8 m_tree[NUM_BYTES_FOR_TREE];
+
+    friend class hkpStaticCompoundShape_Internals;
+};
+
+inline const hkQsTransform& hkpStaticCompoundShape::Instance::getTransform() const {
+    return m_transform;
+}
+
+inline const hkpShape* hkpStaticCompoundShape::Instance::getShape() const {
+    return m_shape;
+}
+
+inline void hkpStaticCompoundShape::Instance::setFlags(hkUint32 flags) {
+    m_transform.m_translation.setInt24W(
+        int((m_transform.m_translation.getInt24W() & ~FLAGS_ALL) | flags));
+}
+
+inline hkUint32 hkpStaticCompoundShape::Instance::getFlags() const {
+    return m_transform.m_translation.getInt24W() & FLAGS_ALL;
+}
+
+inline void hkpStaticCompoundShape::Instance::setShapeSizeForSpu(hkUint32 size) {
+    const hkUint32 shapeSizeMask = ((1 << NUM_BITS_FOR_SHAPE_SIZE) - 1) << NUM_BITS_FOR_FLAGS;
+    hkUint32 bitfield = (m_transform.m_translation.getInt24W() & ~shapeSizeMask);
+
+    const hkUint32 storedShapeSize = size / 16;
+
+    bitfield |= storedShapeSize << NUM_BITS_FOR_FLAGS;
+    m_transform.m_translation.setInt24W(int(bitfield));
+}
+
+inline hkUint32 hkpStaticCompoundShape::Instance::getShapeSizeForSpu() const {
+    return ((m_transform.m_translation.getInt24W() >> NUM_BITS_FOR_FLAGS) &
+            ((1 << NUM_BITS_FOR_SHAPE_SIZE) - 1)) *
+           16;
+}
+
+inline void hkpStaticCompoundShape::decomposeShapeKey(hkpShapeKey keyIn, int& instanceIdOut,
+                                                      hkpShapeKey& childKeyOut) const {
+    instanceIdOut = int(keyIn >> m_numBitsForChildShapeKey);
+    childKeyOut = keyIn & m_childShapeKeyMask;
+}
+
+inline hkpShapeKey hkpStaticCompoundShape::composeShapeKey(int instanceId,
+                                                           hkpShapeKey childKey) const {
+    return (instanceId << m_numBitsForChildShapeKey) | childKey;
+}
+
+inline hkInt8 hkpStaticCompoundShape::getNumBitsForChildShapeKey() const {
+    return m_numBitsForChildShapeKey;
+}
+
+inline const hkArray<hkpStaticCompoundShape::Instance>&
+hkpStaticCompoundShape::getInstances() const {
+    return m_instances;
+}
+
+inline void hkpStaticCompoundShape::setInstanceFilterInfo(int instanceId, hkUint32 filterInfo) {
+    m_instances[instanceId].m_filterInfo = filterInfo;
+}
+
+inline hkUint32 hkpStaticCompoundShape::getInstanceFilterInfo(int instanceId) const {
+    return m_instances[instanceId].m_filterInfo;
+}
+
+inline void hkpStaticCompoundShape::setInstanceFilterInfoMask(int instanceId,
+                                                              hkUint32 filterInfoMask) {
+    m_instances[instanceId].m_childFilterInfoMask = filterInfoMask;
+}
+
+inline hkUint32 hkpStaticCompoundShape::getInstanceFilterInfoMask(int instanceId) const {
+    return m_instances[instanceId].m_childFilterInfoMask;
+}
+
+inline void hkpStaticCompoundShape::setInstanceUserData(int instanceId, hkUlong userData) {
+    m_instances[instanceId].m_userData = userData;
+}
+
+inline hkUlong hkpStaticCompoundShape::getInstanceUserData(int instanceId) const {
+    return m_instances[instanceId].m_userData;
+}
+
+inline void hkpStaticCompoundShape::setInstanceExtraInfo(int instanceId, hkUint16 instanceInfo) {
+    const int maxSize = hkMath::max2(m_instanceExtraInfos.getSize(), instanceId + 1);
+    m_instanceExtraInfos.setSize(maxSize);
+    m_instanceExtraInfos[instanceId] = instanceInfo;
+}
+
+inline hkUint16 hkpStaticCompoundShape::getInstanceExtraInfo(int instanceId) const {
+    return m_instanceExtraInfos[instanceId];
+}
+
+inline const hkArray<hkUint16>& hkpStaticCompoundShape::getInstanceExtraInfos() const {
+    return m_instanceExtraInfos;
+}
+
+inline void hkpStaticCompoundShape::setInstanceEnabled(int instanceId, hkBool32 isEnabled) {
+    Instance& instance = m_instances[instanceId];
+    hkUint32 flags = instance.getFlags();
+    flags =
+        (isEnabled ? (flags & ~Instance::FLAG_IS_DISABLED) : (flags | Instance::FLAG_IS_DISABLED));
+    instance.setFlags(flags);
+}
+
+inline hkBool32 hkpStaticCompoundShape::isInstanceEnabled(int instanceId) const {
+    return !(m_instances[instanceId].getFlags() & Instance::FLAG_IS_DISABLED);
+}