From 0ff9ead1d2fcb784e07e2ce2ae161c22e2d6cea0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?L=C3=A9o=20Lam?= Date: Sat, 19 Mar 2022 01:25:12 +0100 Subject: [PATCH] ksys/phys: Add RayHitCollector --- data/uking_functions.csv | 14 +- lib/hkStubs/CMakeLists.txt | 4 + .../Common/Base/Container/Array/hkArray.h | 28 ++ .../Common/Base/Math/Matrix/hkMatrix3f.h | 7 + .../Common/Base/Math/Matrix/hkMatrix3f.inl | 19 ++ .../Base/Memory/Router/hkMemoryRouter.h | 3 +- .../Query/CastUtil/hkpWorldRayCastOutput.h | 30 +++ .../Shape/Convex/Triangle/hkpTriangleShape.h | 160 ++++++++++++ .../Collide/StaticCompound/hkpShapeKeyTable.h | 43 +++ .../StaticCompound/hkpStaticCompoundShape.h | 247 ++++++++++++++++++ src/KingSystem/Physics/System/physRayCast.cpp | 215 +++++++++++++++ src/KingSystem/Physics/System/physRayCast.h | 8 +- 12 files changed, 769 insertions(+), 9 deletions(-) create mode 100644 lib/hkStubs/Havok/Physics2012/Collide/Query/CastUtil/hkpWorldRayCastOutput.h create mode 100644 lib/hkStubs/Havok/Physics2012/Collide/Shape/Convex/Triangle/hkpTriangleShape.h create mode 100644 lib/hkStubs/Havok/Physics2012/Internal/Collide/StaticCompound/hkpShapeKeyTable.h create mode 100644 lib/hkStubs/Havok/Physics2012/Internal/Collide/StaticCompound/hkpStaticCompoundShape.h diff --git a/data/uking_functions.csv b/data/uking_functions.csv index 9bd3cef8..7d528022 100644 --- a/data/uking_functions.csv +++ b/data/uking_functions.csv @@ -84046,12 +84046,12 @@ Address,Quality,Size,Name 0x0000007100fc4870,O,000028,_ZNK4ksys4phys7RayCast5get34EPN4sead7Vector3IfEE 0x0000007100fc488c,U,000840,phys::RaycastQuery::x_0 0x0000007100fc4bd4,U,000032,phys::RaycastQuery::x_1 -0x0000007100fc4bf4,U,000004,phys::RayHitCollector::dtor -0x0000007100fc4bf8,U,000080,phys::RayHitCollector::dtord -0x0000007100fc4c48,U,000456,phys::RayHitCollector::addRayHit -0x0000007100fc4e10,U,000080,phys::FilteredRayHitCollector::dtord -0x0000007100fc4e60,U,000476,phys::FilteredRayHitCollector::addRayHit -0x0000007100fc503c,U,000476,phys::FilteredRayHitCollector::x_0 +0x0000007100fc4bf4,O,000004,_ZN4ksys4phys15RayHitCollectorD1Ev +0x0000007100fc4bf8,O,000080,_ZN4ksys4phys15RayHitCollectorD0Ev +0x0000007100fc4c48,m,000456,_ZN4ksys4phys15RayHitCollector9addRayHitERK9hkpCdBodyRK30hkpShapeRayCastCollectorOutput +0x0000007100fc4e10,O,000080,_ZN4ksys4phys29NormalCheckingRayHitCollectorD0Ev +0x0000007100fc4e60,m,000476,_ZN4ksys4phys29NormalCheckingRayHitCollector9addRayHitERK9hkpCdBodyRK30hkpShapeRayCastCollectorOutput +0x0000007100fc503c,O,000476,_ZNK4ksys4phys29NormalCheckingRayHitCollector11checkNormalERK9hkpCdBodyRK30hkpShapeRayCastCollectorOutput 0x0000007100fc5218,O,000112,_ZNK4ksys4phys7RayCast27checkDerivedRuntimeTypeInfoEPKN4sead15RuntimeTypeInfo9InterfaceE 0x0000007100fc5288,O,000092,_ZNK4ksys4phys7RayCast18getRuntimeTypeInfoEv 0x0000007100fc52e4,U,000004,phys::RayCast::m4 @@ -84180,7 +84180,7 @@ Address,Quality,Size,Name 0x0000007100fc8bbc,O,000560,_ZN4ksys4phys9ShapeCast20registerContactPointERK14hkpRootCdPointPNS0_9RigidBodyENS1_13ClampDistanceE 0x0000007100fc8dec,O,000100,_ZN4ksys4phys31FilteredClosestCdPointCollectorC1EPNS0_9RigidBodyEPNS0_21QueryContactPointInfoE 0x0000007100fc8e50,O,000004,_ZN4ksys4phys31FilteredClosestCdPointCollectorD1Ev -0x0000007100fc8e54,m,000080,_ZN4ksys4phys31FilteredClosestCdPointCollectorD0Ev +0x0000007100fc8e54,O,000080,_ZN4ksys4phys31FilteredClosestCdPointCollectorD0Ev 0x0000007100fc8ea4,O,000040,_ZN4ksys4phys31FilteredClosestCdPointCollector5resetEv 0x0000007100fc8ecc,O,000476,_ZN4ksys4phys31FilteredClosestCdPointCollector10addCdPointERK10hkpCdPoint 0x0000007100fc90a8,O,000112,_ZNK4ksys4phys9ShapeCast27checkDerivedRuntimeTypeInfoEPKN4sead15RuntimeTypeInfo9InterfaceE 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); @@ -287,6 +289,22 @@ inline hkResult hkArrayBase::_reserveExactly(hkMemoryAllocator& alloc, int n) return HK_SUCCESS; } +template +inline void hkArrayBase::_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 +inline void hkArrayBase::_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 inline typename hkArrayBase::iterator hkArrayBase::begin() { return m_data; @@ -385,6 +403,16 @@ inline void hkArray::pushBack(const T& e) { this->_pushBack(AllocatorType().get(), e); } +template +inline void hkArray::setSize(int size) { + this->_setSize(AllocatorType().get(), size); +} + +template +inline void hkArray::setSize(int size, const T& fill) { + this->_setSize(AllocatorType().get(), size, fill); +} + template inline hkInplaceArray::hkInplaceArray(int size) : hkArray(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 @@ -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(); d->m_col2 = hkVector4f::getConstant(); } + +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(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 + +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 +#include + +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 + HK_FORCE_INLINE const hkVector4& getVertex() const; + + HK_FORCE_INLINE void setVertex(int i, const hkVector4& vertex); + template + 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 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 +inline const hkVector4& hkpTriangleShape::getVertex() const { + return (&m_vertexA)[I]; +} + +inline void hkpTriangleShape::setVertex(int i, const hkVector4& vertex) { + getVertex(i) = vertex; +} + +template +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 +#include + +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& 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 +#include +#include +#include + +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& 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& 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& 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 m_instances; + hkArray 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::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& 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); +} diff --git a/src/KingSystem/Physics/System/physRayCast.cpp b/src/KingSystem/Physics/System/physRayCast.cpp index ea25cea3..a44e1203 100644 --- a/src/KingSystem/Physics/System/physRayCast.cpp +++ b/src/KingSystem/Physics/System/physRayCast.cpp @@ -1,8 +1,70 @@ #include "KingSystem/Physics/System/physRayCast.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "KingSystem/Physics/System/physEntityContactListener.h" +#include "KingSystem/Physics/System/physGroupFilter.h" +#include "KingSystem/Physics/System/physSystem.h" +#include "KingSystem/Physics/physConversions.h" #include "KingSystem/Physics/physLayerMaskBuilder.h" namespace ksys::phys { +class RayHitCollector : public hkpRayHitCollector { +public: + HK_DECLARE_CLASS_ALLOCATOR(RayHitCollector) + + RayHitCollector(hkpWorldRayCastOutput* output, ContactLayerType layer_type, + const sead::PtrArray* ignored_groups, + GroundHit ignored_ground_hit) + : mOutput(output), mLayerType(layer_type), mIgnoredGroups(ignored_groups), + mIgnoredGroundHit(ignored_ground_hit) {} + + ~RayHitCollector() override; + + void addRayHit(const hkpCdBody& cdBody, const hkpShapeRayCastCollectorOutput& hitInfo) override; + +protected: + bool isIgnoredGroup(const hkpCdBody& cdBody, + const hkpShapeRayCastCollectorOutput& hitInfo) const; + + bool isIgnoredGroundHit(const hkpCdBody& cdBody, + const hkpShapeRayCastCollectorOutput& hitInfo) const; + + hkpWorldRayCastOutput* mOutput; + ContactLayerType mLayerType; + const sead::PtrArray* mIgnoredGroups; + GroundHit mIgnoredGroundHit; +}; + +class NormalCheckingRayHitCollector : public RayHitCollector { +public: + HK_DECLARE_CLASS_ALLOCATOR(NormalCheckingRayHitCollector) + + NormalCheckingRayHitCollector(hkpWorldRayCastOutput* output, ContactLayerType layer_type, + const sead::PtrArray* ignored_groups, + GroundHit ignored_ground_hit, const sead::Vector3f* ray_line, + RayCast::NormalCheckingMode mode) + : RayHitCollector(output, layer_type, ignored_groups, ignored_ground_hit), + mRayLine(ray_line), mMode(mode) {} + + ~NormalCheckingRayHitCollector() override; + + void addRayHit(const hkpCdBody& cdBody, const hkpShapeRayCastCollectorOutput& hitInfo) override; + + bool checkNormal(const hkpCdBody& cdBody, const hkpShapeRayCastCollectorOutput& hitInfo) const; + +private: + const sead::Vector3f* mRayLine; + RayCast::NormalCheckingMode mMode; +}; + RayCast::RayCast(SystemGroupHandler* group_handler, GroundHit ground_hit) : mGroupHandler(group_handler), mGroundHit(ground_hit) { reset(); @@ -139,4 +201,157 @@ void RayCast::get34(sead::Vector3f* out) const { *out = _34; } +RayHitCollector::~RayHitCollector() = default; + +bool RayHitCollector::isIgnoredGroup(const hkpCdBody& cdBody, + const hkpShapeRayCastCollectorOutput& hitInfo) const { + if (mIgnoredGroups == nullptr || + cdBody.getRootCollidable()->getShape()->getType() == hkcdShapeType::STATIC_COMPOUND) { + return false; + } + + auto filter_info = cdBody.getRootCollidable()->getCollisionFilterInfo(); + auto* group_filter = System::instance()->getGroupFilter(mLayerType); + auto group_idx = int(group_filter->getCollisionFilterInfoGroupHandlerIdx(filter_info)); + + if (group_idx != 0) { + for (int i = 0; i < mIgnoredGroups->size(); ++i) { + if ((*mIgnoredGroups)[i]->getIndex() == group_idx) + return true; + } + } + + return false; +} + +bool RayHitCollector::isIgnoredGroundHit(const hkpCdBody& cdBody, + const hkpShapeRayCastCollectorOutput& hitInfo) const { + if (mIgnoredGroundHit == GroundHit::Ignore || + cdBody.getRootCollidable()->getShape()->getType() == hkcdShapeType::STATIC_COMPOUND || + mLayerType != ContactLayerType::Entity) { + return false; + } + + auto filter_info = cdBody.getRootCollidable()->getCollisionFilterInfo(); + auto body_ground_hit = EntityCollisionMask(filter_info).getGroundHit(); + if (int(mIgnoredGroundHit) == body_ground_hit) + return true; + + return false; +} + +// NON_MATCHING: group_idx check should be a ccmp +void RayHitCollector::addRayHit(const hkpCdBody& cdBody, + const hkpShapeRayCastCollectorOutput& hitInfo) { + bool closer = hitInfo.m_hitFraction < mOutput->m_hitFraction; + if (!closer) + return; + + if (isIgnoredGroup(cdBody, hitInfo)) + return; + + if (isIgnoredGroundHit(cdBody, hitInfo)) + return; + + if (System::instance()->getEntityContactListenerField91() && + mLayerType == ContactLayerType::Entity && + EntityContactListener::isObjectOrGroundOrNPCOrTree(cdBody)) { + // XXX: Similar checks show up in various other collectors. Can this be refactored? + return; + } + + *static_cast(mOutput) = hitInfo; + mOutput->m_rootCollidable = cdBody.getRootCollidable(); + shapeKeysFromCdBody(mOutput->m_shapeKeys, hkpShapeRayCastOutput::MAX_HIERARCHY_DEPTH, cdBody); + m_earlyOutHitFraction = hitInfo.m_hitFraction; +} + +NormalCheckingRayHitCollector::~NormalCheckingRayHitCollector() = default; + +void NormalCheckingRayHitCollector::addRayHit(const hkpCdBody& cdBody, + const hkpShapeRayCastCollectorOutput& hitInfo) { + bool closer = hitInfo.m_hitFraction < mOutput->m_hitFraction; + if (!closer) + return; + + if (isIgnoredGroup(cdBody, hitInfo)) + return; + + if (isIgnoredGroundHit(cdBody, hitInfo)) + return; + + if (!checkNormal(cdBody, hitInfo)) + return; + + if (System::instance()->getEntityContactListenerField91() && + mLayerType == ContactLayerType::Entity && + EntityContactListener::isObjectOrGroundOrNPCOrTree(cdBody)) { + return; + } + + *static_cast(mOutput) = hitInfo; + mOutput->m_rootCollidable = cdBody.getRootCollidable(); + shapeKeysFromCdBody(mOutput->m_shapeKeys, hkpShapeRayCastOutput::MAX_HIERARCHY_DEPTH, cdBody); + m_earlyOutHitFraction = hitInfo.m_hitFraction; +} + +template +static bool checkDot(RayCast::NormalCheckingMode mode, float dot) { + if (mode == RayCast::NormalCheckingMode::_0) { + if constexpr (Invert) + return !(dot < 0); + else + return (dot < 0); + } else { + if constexpr (Invert) + return !(dot > 0); + else + return (dot > 0); + } +} + +bool NormalCheckingRayHitCollector::checkNormal( + const hkpCdBody& cdBody, const hkpShapeRayCastCollectorOutput& hitInfo) const { + if (cdBody.getShape()->getType() != hkcdShapeType::TRIANGLE) + return checkDot(mMode, mRayLine->dot(toVec3(hitInfo.m_normal))); + + auto* triangle = static_cast(cdBody.getShape()); + + auto vertexA = triangle->getVertex<0>(); + auto vertexB = triangle->getVertex<1>(); + auto vertexC = triangle->getVertex<2>(); + + hkVector4f BminusA; + BminusA.setSub(vertexB, vertexA); + + hkVector4f CminusA; + CminusA.setSub(vertexC, vertexA); + + hkVector4f triangle_line; + triangle_line.setCross(BminusA, CminusA); + triangle_line.normalize<3>(); + + const auto body_rotation = cdBody.getRootCollidable()->getTransform().getRotation(); + + hkRotationf rotation = body_rotation; + + if (cdBody.getRootCollidable()->getShape()->getType() == hkcdShapeType::STATIC_COMPOUND) { + auto* sc = + static_cast(cdBody.getRootCollidable()->getShape()); + + int instance_id; + hkpShapeKey child_key; + sc->decomposeShapeKey(cdBody.getShapeKey(), instance_id, child_key); + + hkRotationf shape_rotation; + shape_rotation.set(sc->getInstances()[instance_id].getTransform().getRotation()); + rotation.mul(shape_rotation); + } + + hkVector4f rotated_normal; + rotated_normal.setRotatedDir(rotation, hitInfo.m_normal); + + return checkDot(mMode, triangle_line.dot<3>(rotated_normal)); +} + } // namespace ksys::phys diff --git a/src/KingSystem/Physics/System/physRayCast.h b/src/KingSystem/Physics/System/physRayCast.h index d2b915e8..58cd5f5d 100644 --- a/src/KingSystem/Physics/System/physRayCast.h +++ b/src/KingSystem/Physics/System/physRayCast.h @@ -19,6 +19,12 @@ class SystemGroupHandler; class RayCast { SEAD_RTTI_BASE(RayCast) public: + enum class NormalCheckingMode { + _0 = 0, + _1 = 1, + DoNotCheck = 2, + }; + // TODO: what kind of callback is this? using Callback = sead::IDelegate1; @@ -105,7 +111,7 @@ private: void* _60; sead::SafeArray mLayerMasks{}; sead::Atomic _70; - sead::Atomic _74; + NormalCheckingMode mNormalCheckingMode; MaterialMask mMaterialMask; RigidBody* mRigidBody{}; sead::Atomic _98;