From 4058e6a72edc6e11630b3ec6b67d97e2b9324473 Mon Sep 17 00:00:00 2001 From: Jorrit Rouwe Date: Sun, 22 Sep 2024 22:36:23 +0200 Subject: [PATCH] SoftBodyManifold now returns sensor contacts separately (#1276) Before this change, there was a limit of a single colliding body per soft body vertex. If the closest body happened to be a sensor this effectively disabled the collision with the world and caused artifacts. We can now also detect multiple sensor contacts per soft body and they are returned through a new interface SoftBodyManifold::GetSensorContactBodyID. * Shape::CollideSoftBodyVertices is no longer using SoftBodyVertex but CollideSoftBodyVertexIterator which can write to other things than a SoftBodyVertex * Removed delta time and displacement due to gravity from Shape::CollideSoftBodyVertices * Added soft body vs sensor test that doesn't have a limit of 1 sensor per soft body Fixes #1016 --- Jolt/Core/StridedPtr.h | 63 ++++ Jolt/Jolt.cmake | 2 + Jolt/Jolt.natvis | 3 + .../Collision/CollideSoftBodyVertexIterator.h | 110 ++++++ .../CollideSoftBodyVerticesVsTriangles.h | 28 +- Jolt/Physics/Collision/Shape/BoxShape.cpp | 24 +- Jolt/Physics/Collision/Shape/BoxShape.h | 2 +- Jolt/Physics/Collision/Shape/CapsuleShape.cpp | 24 +- Jolt/Physics/Collision/Shape/CapsuleShape.h | 2 +- .../Physics/Collision/Shape/CompoundShape.cpp | 4 +- Jolt/Physics/Collision/Shape/CompoundShape.h | 2 +- .../Collision/Shape/ConvexHullShape.cpp | 17 +- .../Physics/Collision/Shape/ConvexHullShape.h | 2 +- .../Physics/Collision/Shape/CylinderShape.cpp | 20 +- Jolt/Physics/Collision/Shape/CylinderShape.h | 2 +- Jolt/Physics/Collision/Shape/EmptyShape.h | 2 +- .../Collision/Shape/HeightFieldShape.cpp | 10 +- .../Collision/Shape/HeightFieldShape.h | 2 +- Jolt/Physics/Collision/Shape/MeshShape.cpp | 10 +- Jolt/Physics/Collision/Shape/MeshShape.h | 2 +- .../Shape/OffsetCenterOfMassShape.cpp | 4 +- .../Collision/Shape/OffsetCenterOfMassShape.h | 2 +- Jolt/Physics/Collision/Shape/PlaneShape.cpp | 18 +- Jolt/Physics/Collision/Shape/PlaneShape.h | 2 +- .../Shape/RotatedTranslatedShape.cpp | 4 +- .../Collision/Shape/RotatedTranslatedShape.h | 2 +- Jolt/Physics/Collision/Shape/ScaledShape.cpp | 4 +- Jolt/Physics/Collision/Shape/ScaledShape.h | 2 +- Jolt/Physics/Collision/Shape/Shape.h | 12 +- Jolt/Physics/Collision/Shape/SphereShape.cpp | 17 +- Jolt/Physics/Collision/Shape/SphereShape.h | 2 +- .../Collision/Shape/TaperedCapsuleShape.cpp | 17 +- .../Collision/Shape/TaperedCapsuleShape.h | 2 +- .../Collision/Shape/TaperedCylinderShape.cpp | 20 +- .../Collision/Shape/TaperedCylinderShape.h | 2 +- .../Physics/Collision/Shape/TriangleShape.cpp | 10 +- Jolt/Physics/Collision/Shape/TriangleShape.h | 2 +- Jolt/Physics/SoftBody/SoftBodyManifold.h | 17 +- .../SoftBody/SoftBodyMotionProperties.cpp | 323 +++++++++++------- .../SoftBody/SoftBodyMotionProperties.h | 24 +- Jolt/Physics/SoftBody/SoftBodyShape.cpp | 2 +- Jolt/Physics/SoftBody/SoftBodyShape.h | 2 +- Jolt/Physics/SoftBody/SoftBodyUpdateContext.h | 5 +- Jolt/Physics/SoftBody/SoftBodyVertex.h | 16 +- Samples/Samples.cmake | 2 + Samples/SamplesApp.cpp | 44 +-- Samples/Tests/SoftBody/SoftBodySensorTest.cpp | 69 ++++ Samples/Tests/SoftBody/SoftBodySensorTest.h | 21 ++ 48 files changed, 665 insertions(+), 313 deletions(-) create mode 100644 Jolt/Core/StridedPtr.h create mode 100644 Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h create mode 100644 Samples/Tests/SoftBody/SoftBodySensorTest.cpp create mode 100644 Samples/Tests/SoftBody/SoftBodySensorTest.h diff --git a/Jolt/Core/StridedPtr.h b/Jolt/Core/StridedPtr.h new file mode 100644 index 000000000..1cf97fa67 --- /dev/null +++ b/Jolt/Core/StridedPtr.h @@ -0,0 +1,63 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// A strided pointer behaves exactly like a normal pointer except that the +/// elements that the pointer points to can be part of a larger structure. +/// The stride gives the number of bytes from one element to the next. +template +class JPH_EXPORT StridedPtr +{ +public: + using value_type = T; + + /// Constructors + StridedPtr() = default; + StridedPtr(const StridedPtr &inRHS) = default; + StridedPtr(T *inPtr, int inStride = sizeof(T)) : mPtr(const_cast(reinterpret_cast(inPtr))), mStride(inStride) { } + + /// Assignment + inline StridedPtr & operator = (const StridedPtr &inRHS) = default; + + /// Incrementing / decrementing + inline StridedPtr & operator ++ () { mPtr += mStride; return *this; } + inline StridedPtr & operator -- () { mPtr -= mStride; return *this; } + inline StridedPtr operator ++ (int) { StridedPtr old_ptr(*this); mPtr += mStride; return old_ptr; } + inline StridedPtr operator -- (int) { StridedPtr old_ptr(*this); mPtr -= mStride; return old_ptr; } + inline StridedPtr operator + (int inOffset) const { StridedPtr new_ptr(*this); new_ptr.mPtr += inOffset * mStride; return new_ptr; } + inline StridedPtr operator - (int inOffset) const { StridedPtr new_ptr(*this); new_ptr.mPtr -= inOffset * mStride; return new_ptr; } + inline void operator += (int inOffset) { mPtr += inOffset * mStride; } + inline void operator -= (int inOffset) { mPtr -= inOffset * mStride; } + + /// Distance between two pointers in elements + inline int operator - (const StridedPtr &inRHS) const { JPH_ASSERT(inRHS.mStride == mStride); return (mPtr - inRHS.mPtr) / mStride; } + + /// Comparison operators + inline bool operator == (const StridedPtr &inRHS) const { return mPtr == inRHS.mPtr; } + inline bool operator != (const StridedPtr &inRHS) const { return mPtr != inRHS.mPtr; } + inline bool operator <= (const StridedPtr &inRHS) const { return mPtr <= inRHS.mPtr; } + inline bool operator >= (const StridedPtr &inRHS) const { return mPtr >= inRHS.mPtr; } + inline bool operator < (const StridedPtr &inRHS) const { return mPtr < inRHS.mPtr; } + inline bool operator > (const StridedPtr &inRHS) const { return mPtr > inRHS.mPtr; } + + /// Access value + inline T & operator * () const { return *reinterpret_cast(mPtr); } + inline T * operator -> () const { return reinterpret_cast(mPtr); } + inline T & operator [] (int inOffset) const { uint8 *ptr = mPtr + inOffset * mStride; return *reinterpret_cast(ptr); } + + /// Explicit conversion + inline T * GetPtr() const { return reinterpret_cast(mPtr); } + + /// Get stride in bytes + inline int GetStride() const { return mStride; } + +private: + uint8 * mPtr = nullptr; /// Pointer to element + int mStride = 0; /// Stride (number of bytes) between elements +}; + +JPH_NAMESPACE_END diff --git a/Jolt/Jolt.cmake b/Jolt/Jolt.cmake index c451d8bf3..f514496e0 100644 --- a/Jolt/Jolt.cmake +++ b/Jolt/Jolt.cmake @@ -68,6 +68,7 @@ set(JOLT_PHYSICS_SRC_FILES ${JOLT_PHYSICS_ROOT}/Core/StreamOut.h ${JOLT_PHYSICS_ROOT}/Core/StreamUtils.h ${JOLT_PHYSICS_ROOT}/Core/StreamWrapper.h + ${JOLT_PHYSICS_ROOT}/Core/StridedPtr.h ${JOLT_PHYSICS_ROOT}/Core/StringTools.cpp ${JOLT_PHYSICS_ROOT}/Core/StringTools.h ${JOLT_PHYSICS_ROOT}/Core/TempAllocator.h @@ -197,6 +198,7 @@ set(JOLT_PHYSICS_SRC_FILES ${JOLT_PHYSICS_ROOT}/Physics/Collision/CollideConvexVsTriangles.h ${JOLT_PHYSICS_ROOT}/Physics/Collision/CollidePointResult.h ${JOLT_PHYSICS_ROOT}/Physics/Collision/CollideShape.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/CollideSoftBodyVertexIterator.h ${JOLT_PHYSICS_ROOT}/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h ${JOLT_PHYSICS_ROOT}/Physics/Collision/CollideSphereVsTriangles.cpp ${JOLT_PHYSICS_ROOT}/Physics/Collision/CollideSphereVsTriangles.h diff --git a/Jolt/Jolt.natvis b/Jolt/Jolt.natvis index b4e9ddbd0..117ff9033 100644 --- a/Jolt/Jolt.natvis +++ b/Jolt/Jolt.natvis @@ -94,4 +94,7 @@ + + {(value_type *)mPtr}, stride={mStride} + diff --git a/Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h b/Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h new file mode 100644 index 000000000..e976aa9de --- /dev/null +++ b/Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h @@ -0,0 +1,110 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that allows iterating over the vertices of a soft body. +/// It tracks the largest penetration and allows storing the resulting collision in a different structure than the soft body vertex itself. +class CollideSoftBodyVertexIterator +{ +public: + /// Default constructor + CollideSoftBodyVertexIterator() = default; + CollideSoftBodyVertexIterator(const CollideSoftBodyVertexIterator &) = default; + + /// Construct using (strided) pointers + CollideSoftBodyVertexIterator(const StridedPtr &inPosition, const StridedPtr &inInvMass, const StridedPtr &inCollisionPlane, const StridedPtr &inLargestPenetration, const StridedPtr &inCollidingShapeIndex) : + mPosition(inPosition), + mInvMass(inInvMass), + mCollisionPlane(inCollisionPlane), + mLargestPenetration(inLargestPenetration), + mCollidingShapeIndex(inCollidingShapeIndex) + { + } + + /// Construct using a soft body vertex + explicit CollideSoftBodyVertexIterator(SoftBodyVertex *inVertices) : + mPosition(&inVertices->mPosition, sizeof(SoftBodyVertex)), + mInvMass(&inVertices->mInvMass, sizeof(SoftBodyVertex)), + mCollisionPlane(&inVertices->mCollisionPlane, sizeof(SoftBodyVertex)), + mLargestPenetration(&inVertices->mLargestPenetration, sizeof(SoftBodyVertex)), + mCollidingShapeIndex(&inVertices->mCollidingShapeIndex, sizeof(SoftBodyVertex)) + { + } + + /// Default assignment + CollideSoftBodyVertexIterator & operator = (const CollideSoftBodyVertexIterator &) = default; + + /// Equality operator. + /// Note: Only used to determine end iterator, so we only compare position. + bool operator != (const CollideSoftBodyVertexIterator &inRHS) const + { + return mPosition != inRHS.mPosition; + } + + /// Next vertex + CollideSoftBodyVertexIterator & operator ++ () + { + ++mPosition; + ++mInvMass; + ++mCollisionPlane; + ++mLargestPenetration; + ++mCollidingShapeIndex; + return *this; + } + + /// Add an offset + /// Note: Only used to determine end iterator, so we only set position. + CollideSoftBodyVertexIterator operator + (int inOffset) const + { + return CollideSoftBodyVertexIterator(mPosition + inOffset, StridedPtr(), StridedPtr(), StridedPtr(), StridedPtr()); + } + + /// Get the position of the current vertex + Vec3 GetPosition() const + { + return *mPosition; + } + + /// Get the inverse mass of the current vertex + float GetInvMass() const + { + return *mInvMass; + } + + /// Update penetration of the current vertex + /// @return Returns true if the vertex has the largest penetration so far, this means you need to follow up by calling SetCollision + bool UpdatePenetration(float inLargestPenetration) const + { + float &penetration = *mLargestPenetration; + if (penetration >= inLargestPenetration) + return false; + penetration = inLargestPenetration; + return true; + } + + /// Update the collision of the current vertex + void SetCollision(const Plane &inCollisionPlane, int inCollidingShapeIndex) const + { + *mCollisionPlane = inCollisionPlane; + *mCollidingShapeIndex = inCollidingShapeIndex; + } + +private: + /// Input data + StridedPtr mPosition; + StridedPtr mInvMass; + + /// Output data + StridedPtr mCollisionPlane; + StridedPtr mLargestPenetration; + StridedPtr mCollidingShapeIndex; +}; + +JPH_NAMESPACE_END diff --git a/Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h b/Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h index 4385e15cd..c91becb64 100644 --- a/Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h +++ b/Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h @@ -4,7 +4,7 @@ #pragma once -#include +#include #include JPH_NAMESPACE_BEGIN @@ -20,9 +20,9 @@ class JPH_EXPORT CollideSoftBodyVerticesVsTriangles { } - JPH_INLINE void StartVertex(const SoftBodyVertex &inVertex) + JPH_INLINE void StartVertex(const CollideSoftBodyVertexIterator &inVertex) { - mLocalPosition = mInvTransform * inVertex.mPosition; + mLocalPosition = mInvTransform * inVertex.GetPosition(); mClosestDistanceSq = FLT_MAX; } @@ -43,7 +43,7 @@ class JPH_EXPORT CollideSoftBodyVerticesVsTriangles } } - JPH_INLINE void FinishVertex(SoftBodyVertex &ioVertex, int inCollidingShapeIndex) const + JPH_INLINE void FinishVertex(const CollideSoftBodyVertexIterator &ioVertex, int inCollidingShapeIndex) const { if (mClosestDistanceSq < FLT_MAX) { @@ -57,29 +57,21 @@ class JPH_EXPORT CollideSoftBodyVerticesVsTriangles { // Closest is interior to the triangle, use plane as collision plane but don't allow more than 0.1 m penetration // because otherwise a triangle half a level a way will have a huge penetration if it is back facing - float penetration = min(triangle_normal.Dot(v0 - ioVertex.mPosition), 0.1f); - if (penetration > ioVertex.mLargestPenetration) - { - ioVertex.mLargestPenetration = penetration; - ioVertex.mCollisionPlane = Plane::sFromPointAndNormal(v0, triangle_normal); - ioVertex.mCollidingShapeIndex = inCollidingShapeIndex; - } + float penetration = min(triangle_normal.Dot(v0 - ioVertex.GetPosition()), 0.1f); + if (ioVertex.UpdatePenetration(penetration)) + ioVertex.SetCollision(Plane::sFromPointAndNormal(v0, triangle_normal), inCollidingShapeIndex); } else { // Closest point is on an edge or vertex, use closest point as collision plane Vec3 closest_point = mTransform * (mLocalPosition + mClosestPoint); - Vec3 normal = ioVertex.mPosition - closest_point; + Vec3 normal = ioVertex.GetPosition() - closest_point; if (normal.Dot(triangle_normal) > 0.0f) // Ignore back facing edges { float normal_length = normal.Length(); float penetration = -normal_length; - if (penetration > ioVertex.mLargestPenetration) - { - ioVertex.mLargestPenetration = penetration; - ioVertex.mCollisionPlane = Plane::sFromPointAndNormal(closest_point, normal_length > 0.0f? normal / normal_length : triangle_normal); - ioVertex.mCollidingShapeIndex = inCollidingShapeIndex; - } + if (ioVertex.UpdatePenetration(penetration)) + ioVertex.SetCollision(Plane::sFromPointAndNormal(closest_point, normal_length > 0.0f? normal / normal_length : triangle_normal), inCollidingShapeIndex); } } } diff --git a/Jolt/Physics/Collision/Shape/BoxShape.cpp b/Jolt/Physics/Collision/Shape/BoxShape.cpp index 4b3548f44..dc7ea4828 100644 --- a/Jolt/Physics/Collision/Shape/BoxShape.cpp +++ b/Jolt/Physics/Collision/Shape/BoxShape.cpp @@ -11,7 +11,7 @@ #include #include #include -#include +#include #include #include #include @@ -225,16 +225,16 @@ void BoxShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShape ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); } -void BoxShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +void BoxShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const { Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation(); Vec3 half_extent = inScale.Abs() * mHalfExtent; - for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v) - if (v->mInvMass > 0.0f) + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) { // Convert to local space - Vec3 local_pos = inverse_transform * v->mPosition; + Vec3 local_pos = inverse_transform * v.GetPosition(); // Clamp point to inside box Vec3 clamped_point = Vec3::sMax(Vec3::sMin(local_pos, half_extent), -half_extent); @@ -246,18 +246,15 @@ void BoxShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg Vec3 delta = half_extent - local_pos.Abs(); int index = delta.GetLowestComponentIndex(); float penetration = delta[index]; - if (penetration > v->mLargestPenetration) + if (v.UpdatePenetration(penetration)) { - v->mLargestPenetration = penetration; - // Calculate contact point and normal Vec3 possible_normals[] = { Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ() }; Vec3 normal = local_pos.GetSign() * possible_normals[index]; Vec3 point = normal * half_extent; // Store collision - v->mCollisionPlane = Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform); - v->mCollidingShapeIndex = inCollidingShapeIndex; + v.SetCollision(Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); } } else @@ -268,15 +265,12 @@ void BoxShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg // Penetration will be negative since we're not penetrating float penetration = -normal_length; - if (penetration > v->mLargestPenetration) + if (v.UpdatePenetration(penetration)) { normal /= normal_length; - v->mLargestPenetration = penetration; - // Store collision - v->mCollisionPlane = Plane::sFromPointAndNormal(clamped_point, normal).GetTransformed(inCenterOfMassTransform); - v->mCollidingShapeIndex = inCollidingShapeIndex; + v.SetCollision(Plane::sFromPointAndNormal(clamped_point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); } } } diff --git a/Jolt/Physics/Collision/Shape/BoxShape.h b/Jolt/Physics/Collision/Shape/BoxShape.h index 36d33db66..748222201 100644 --- a/Jolt/Physics/Collision/Shape/BoxShape.h +++ b/Jolt/Physics/Collision/Shape/BoxShape.h @@ -77,7 +77,7 @@ class JPH_EXPORT BoxShape final : public ConvexShape virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; // See: Shape::CollideSoftBodyVertices - virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; // See Shape::GetTrianglesStart virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; diff --git a/Jolt/Physics/Collision/Shape/CapsuleShape.cpp b/Jolt/Physics/Collision/Shape/CapsuleShape.cpp index d00552c45..48a6cba3e 100644 --- a/Jolt/Physics/Collision/Shape/CapsuleShape.cpp +++ b/Jolt/Physics/Collision/Shape/CapsuleShape.cpp @@ -12,7 +12,7 @@ #include #include #include -#include +#include #include #include #include @@ -321,7 +321,7 @@ void CapsuleShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubS ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); } -void CapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +void CapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const { JPH_ASSERT(IsValidScale(inScale)); @@ -332,11 +332,11 @@ void CapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec float half_height_of_cylinder = scale * mHalfHeightOfCylinder; float radius = scale * mRadius; - for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v) - if (v->mInvMass > 0.0f) + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) { // Calculate penetration - Vec3 local_pos = inverse_transform * v->mPosition; + Vec3 local_pos = inverse_transform * v.GetPosition(); if (abs(local_pos.GetY()) <= half_height_of_cylinder) { // Near cylinder @@ -344,17 +344,14 @@ void CapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec normal.SetY(0.0f); float normal_length = normal.Length(); float penetration = radius - normal_length; - if (penetration > v->mLargestPenetration) + if (v.UpdatePenetration(penetration)) { - v->mLargestPenetration = penetration; - // Calculate contact point and normal normal = normal_length > 0.0f? normal / normal_length : Vec3::sAxisX(); Vec3 point = radius * normal; // Store collision - v->mCollisionPlane = Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform); - v->mCollidingShapeIndex = inCollidingShapeIndex; + v.SetCollision(Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); } } else @@ -364,17 +361,14 @@ void CapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec Vec3 delta = local_pos - center; float distance = delta.Length(); float penetration = radius - distance; - if (penetration > v->mLargestPenetration) + if (v.UpdatePenetration(penetration)) { - v->mLargestPenetration = penetration; - // Calculate contact point and normal Vec3 normal = delta / distance; Vec3 point = center + radius * normal; // Store collision - v->mCollisionPlane = Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform); - v->mCollidingShapeIndex = inCollidingShapeIndex; + v.SetCollision(Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); } } } diff --git a/Jolt/Physics/Collision/Shape/CapsuleShape.h b/Jolt/Physics/Collision/Shape/CapsuleShape.h index a187553a8..5a1f88407 100644 --- a/Jolt/Physics/Collision/Shape/CapsuleShape.h +++ b/Jolt/Physics/Collision/Shape/CapsuleShape.h @@ -86,7 +86,7 @@ class JPH_EXPORT CapsuleShape final : public ConvexShape virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; // See: Shape::CollideSoftBodyVertices - virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; // See Shape::GetTrianglesStart virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; diff --git a/Jolt/Physics/Collision/Shape/CompoundShape.cpp b/Jolt/Physics/Collision/Shape/CompoundShape.cpp index 68656b477..f5fe04ecf 100644 --- a/Jolt/Physics/Collision/Shape/CompoundShape.cpp +++ b/Jolt/Physics/Collision/Shape/CompoundShape.cpp @@ -262,12 +262,12 @@ void CompoundShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg i } #endif // JPH_DEBUG_RENDERER -void CompoundShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +void CompoundShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const { for (const SubShape &shape : mSubShapes) { Mat44 transform = shape.GetLocalTransformNoScale(inScale); - shape.mShape->CollideSoftBodyVertices(inCenterOfMassTransform * transform, shape.TransformScale(inScale), ioVertices, inNumVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex); + shape.mShape->CollideSoftBodyVertices(inCenterOfMassTransform * transform, shape.TransformScale(inScale), inVertices, inNumVertices, inCollidingShapeIndex); } } diff --git a/Jolt/Physics/Collision/Shape/CompoundShape.h b/Jolt/Physics/Collision/Shape/CompoundShape.h index e07e9aec6..23a9ab459 100644 --- a/Jolt/Physics/Collision/Shape/CompoundShape.h +++ b/Jolt/Physics/Collision/Shape/CompoundShape.h @@ -109,7 +109,7 @@ class JPH_EXPORT CompoundShape : public Shape #endif // JPH_DEBUG_RENDERER // See: Shape::CollideSoftBodyVertices - virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; // See Shape::TransformShape virtual void TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override; diff --git a/Jolt/Physics/Collision/Shape/ConvexHullShape.cpp b/Jolt/Physics/Collision/Shape/ConvexHullShape.cpp index 7d26880a5..b0faf7d89 100644 --- a/Jolt/Physics/Collision/Shape/ConvexHullShape.cpp +++ b/Jolt/Physics/Collision/Shape/ConvexHullShape.cpp @@ -11,7 +11,7 @@ #include #include #include -#include +#include #include #include #include @@ -1057,7 +1057,7 @@ void ConvexHullShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inS ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); } -void ConvexHullShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +void ConvexHullShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const { Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation(); @@ -1065,10 +1065,10 @@ void ConvexHullShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, bool is_not_scaled = ScaleHelpers::IsNotScaled(inScale); float scale_flip = ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f; - for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v) - if (v->mInvMass > 0.0f) + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) { - Vec3 local_pos = inverse_transform * v->mPosition; + Vec3 local_pos = inverse_transform * v.GetPosition(); // Find most facing plane float max_distance = -FLT_MAX; @@ -1150,17 +1150,14 @@ void ConvexHullShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, penetration = -penetration; else normal = -normal; - if (penetration > v->mLargestPenetration) + if (v.UpdatePenetration(penetration)) { - v->mLargestPenetration = penetration; - // Calculate contact plane normal = normal_length > 0.0f? normal / normal_length : max_plane_normal; Plane plane = Plane::sFromPointAndNormal(closest_point, normal); // Store collision - v->mCollisionPlane = plane.GetTransformed(inCenterOfMassTransform); - v->mCollidingShapeIndex = inCollidingShapeIndex; + v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); } } } diff --git a/Jolt/Physics/Collision/Shape/ConvexHullShape.h b/Jolt/Physics/Collision/Shape/ConvexHullShape.h index 8837a82d8..8207764f1 100644 --- a/Jolt/Physics/Collision/Shape/ConvexHullShape.h +++ b/Jolt/Physics/Collision/Shape/ConvexHullShape.h @@ -90,7 +90,7 @@ class JPH_EXPORT ConvexHullShape final : public ConvexShape virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; // See: Shape::CollideSoftBodyVertices - virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; // See Shape::GetTrianglesStart virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; diff --git a/Jolt/Physics/Collision/Shape/CylinderShape.cpp b/Jolt/Physics/Collision/Shape/CylinderShape.cpp index 7ed80b02b..06fc5ec71 100644 --- a/Jolt/Physics/Collision/Shape/CylinderShape.cpp +++ b/Jolt/Physics/Collision/Shape/CylinderShape.cpp @@ -11,7 +11,7 @@ #include #include #include -#include +#include #include #include #include @@ -300,7 +300,7 @@ void CylinderShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSub ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); } -void CylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +void CylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const { JPH_ASSERT(IsValidScale(inScale)); @@ -311,10 +311,10 @@ void CylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Ve float half_height = abs_scale.GetY() * mHalfHeight; float radius = abs_scale.GetX() * mRadius; - for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v) - if (v->mInvMass > 0.0f) + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) { - Vec3 local_pos = inverse_transform * v->mPosition; + Vec3 local_pos = inverse_transform * v.GetPosition(); // Calculate penetration into side surface Vec3 side_normal = local_pos; @@ -348,14 +348,8 @@ void CylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Ve // Calculate penetration Plane plane = Plane::sFromPointAndNormal(point, normal); float penetration = -plane.SignedDistance(local_pos); - if (penetration > v->mLargestPenetration) - { - v->mLargestPenetration = penetration; - - // Store collision - v->mCollisionPlane = plane.GetTransformed(inCenterOfMassTransform); - v->mCollidingShapeIndex = inCollidingShapeIndex; - } + if (v.UpdatePenetration(penetration)) + v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); } } diff --git a/Jolt/Physics/Collision/Shape/CylinderShape.h b/Jolt/Physics/Collision/Shape/CylinderShape.h index def0f9c50..126524228 100644 --- a/Jolt/Physics/Collision/Shape/CylinderShape.h +++ b/Jolt/Physics/Collision/Shape/CylinderShape.h @@ -81,7 +81,7 @@ class JPH_EXPORT CylinderShape final : public ConvexShape virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; // See: Shape::CollideSoftBodyVertices - virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; // See Shape::GetTrianglesStart virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; diff --git a/Jolt/Physics/Collision/Shape/EmptyShape.h b/Jolt/Physics/Collision/Shape/EmptyShape.h index a1d5b7792..4b2e47c0f 100644 --- a/Jolt/Physics/Collision/Shape/EmptyShape.h +++ b/Jolt/Physics/Collision/Shape/EmptyShape.h @@ -50,7 +50,7 @@ class JPH_EXPORT EmptyShape final : public Shape virtual bool CastRay([[maybe_unused]] const RayCast &inRay, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator, [[maybe_unused]] RayCastResult &ioHit) const override { return false; } virtual void CastRay([[maybe_unused]] const RayCast &inRay, [[maybe_unused]] const RayCastSettings &inRayCastSettings, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator, [[maybe_unused]] CastRayCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter = { }) const override { /* Do nothing */ } virtual void CollidePoint([[maybe_unused]] Vec3Arg inPoint, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator, [[maybe_unused]] CollidePointCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter = { }) const override { /* Do nothing */ } - virtual void CollideSoftBodyVertices([[maybe_unused]] Mat44Arg inCenterOfMassTransform, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] SoftBodyVertex *ioVertices, [[maybe_unused]] uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, [[maybe_unused]] int inCollidingShapeIndex) const override { /* Do nothing */ } + virtual void CollideSoftBodyVertices([[maybe_unused]] Mat44Arg inCenterOfMassTransform, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] const CollideSoftBodyVertexIterator &inVertices, [[maybe_unused]] uint inNumVertices, [[maybe_unused]] int inCollidingShapeIndex) const override { /* Do nothing */ } virtual void GetTrianglesStart([[maybe_unused]] GetTrianglesContext &ioContext, [[maybe_unused]] const AABox &inBox, [[maybe_unused]] Vec3Arg inPositionCOM, [[maybe_unused]] QuatArg inRotation, [[maybe_unused]] Vec3Arg inScale) const override { /* Do nothing */ } virtual int GetTrianglesNext([[maybe_unused]] GetTrianglesContext &ioContext, [[maybe_unused]] int inMaxTrianglesRequested, [[maybe_unused]] Float3 *outTriangleVertices, [[maybe_unused]] const PhysicsMaterial **outMaterials = nullptr) const override { return 0; } Stats GetStats() const override { return { sizeof(*this), 0 }; } diff --git a/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp b/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp index 37b1b0ccd..ed1d2e3c2 100644 --- a/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp +++ b/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp @@ -2201,7 +2201,7 @@ void HeightFieldShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &in // A height field doesn't have volume, so we can't test insideness } -void HeightFieldShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +void HeightFieldShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const { JPH_PROFILE_FUNCTION(); @@ -2241,12 +2241,12 @@ void HeightFieldShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Visitor visitor(inCenterOfMassTransform, inScale); - for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v) - if (v->mInvMass > 0.0f) + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) { - visitor.StartVertex(*v); + visitor.StartVertex(v); WalkHeightField(visitor); - visitor.FinishVertex(*v, inCollidingShapeIndex); + visitor.FinishVertex(v, inCollidingShapeIndex); } } diff --git a/Jolt/Physics/Collision/Shape/HeightFieldShape.h b/Jolt/Physics/Collision/Shape/HeightFieldShape.h index 572b3470d..54a53ab24 100644 --- a/Jolt/Physics/Collision/Shape/HeightFieldShape.h +++ b/Jolt/Physics/Collision/Shape/HeightFieldShape.h @@ -174,7 +174,7 @@ class JPH_EXPORT HeightFieldShape final : public Shape virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; // See: Shape::CollideSoftBodyVertices - virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; // See Shape::GetTrianglesStart virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; diff --git a/Jolt/Physics/Collision/Shape/MeshShape.cpp b/Jolt/Physics/Collision/Shape/MeshShape.cpp index c2a8e557f..ffb2ce0df 100644 --- a/Jolt/Physics/Collision/Shape/MeshShape.cpp +++ b/Jolt/Physics/Collision/Shape/MeshShape.cpp @@ -793,7 +793,7 @@ void MeshShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShap sCollidePointUsingRayCast(*this, inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter); } -void MeshShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +void MeshShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const { JPH_PROFILE_FUNCTION(); @@ -830,12 +830,12 @@ void MeshShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Ar Visitor visitor(inCenterOfMassTransform, inScale); - for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v) - if (v->mInvMass > 0.0f) + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) { - visitor.StartVertex(*v); + visitor.StartVertex(v); WalkTreePerTriangle(SubShapeIDCreator(), visitor); - visitor.FinishVertex(*v, inCollidingShapeIndex); + visitor.FinishVertex(v, inCollidingShapeIndex); } } diff --git a/Jolt/Physics/Collision/Shape/MeshShape.h b/Jolt/Physics/Collision/Shape/MeshShape.h index 7df68c287..c0d878a7e 100644 --- a/Jolt/Physics/Collision/Shape/MeshShape.h +++ b/Jolt/Physics/Collision/Shape/MeshShape.h @@ -125,7 +125,7 @@ class JPH_EXPORT MeshShape final : public Shape virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; // See: Shape::CollideSoftBodyVertices - virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; // See Shape::GetTrianglesStart virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; diff --git a/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp b/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp index 942eed0d9..8996b46c4 100644 --- a/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp +++ b/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp @@ -127,9 +127,9 @@ void OffsetCenterOfMassShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCrea mInnerShape->CollidePoint(inPoint + mOffset, inSubShapeIDCreator, ioCollector, inShapeFilter); } -void OffsetCenterOfMassShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +void OffsetCenterOfMassShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const { - mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, ioVertices, inNumVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex); + mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inVertices, inNumVertices, inCollidingShapeIndex); } void OffsetCenterOfMassShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const diff --git a/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h b/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h index 1774c5504..537232d05 100644 --- a/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h +++ b/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h @@ -97,7 +97,7 @@ class JPH_EXPORT OffsetCenterOfMassShape final : public DecoratedShape virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; // See: Shape::CollideSoftBodyVertices - virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; // See Shape::CollectTransformedShapes virtual void CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override; diff --git a/Jolt/Physics/Collision/Shape/PlaneShape.cpp b/Jolt/Physics/Collision/Shape/PlaneShape.cpp index d36f33c45..1ebc25ab3 100644 --- a/Jolt/Physics/Collision/Shape/PlaneShape.cpp +++ b/Jolt/Physics/Collision/Shape/PlaneShape.cpp @@ -14,7 +14,7 @@ #include #include #include -#include +#include #include #include #include @@ -246,24 +246,20 @@ void PlaneShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubSha ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); } -void PlaneShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +void PlaneShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const { JPH_PROFILE_FUNCTION(); // Convert plane to world space Plane plane = mPlane.Scaled(inScale).GetTransformed(inCenterOfMassTransform); - for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v) - if (v->mInvMass > 0.0f) + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) { // Calculate penetration - float penetration = -plane.SignedDistance(v->mPosition); - if (penetration > v->mLargestPenetration) - { - v->mLargestPenetration = penetration; - v->mCollisionPlane = plane; - v->mCollidingShapeIndex = inCollidingShapeIndex; - } + float penetration = -plane.SignedDistance(v.GetPosition()); + if (v.UpdatePenetration(penetration)) + v.SetCollision(plane, inCollidingShapeIndex); } } diff --git a/Jolt/Physics/Collision/Shape/PlaneShape.h b/Jolt/Physics/Collision/Shape/PlaneShape.h index 6c36ae65b..b7920f2a9 100644 --- a/Jolt/Physics/Collision/Shape/PlaneShape.h +++ b/Jolt/Physics/Collision/Shape/PlaneShape.h @@ -92,7 +92,7 @@ class JPH_EXPORT PlaneShape final : public Shape virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; // See: Shape::CollideSoftBodyVertices - virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; // See Shape::GetTrianglesStart virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; diff --git a/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp b/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp index d031b4166..66b8bea1e 100644 --- a/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp +++ b/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp @@ -161,9 +161,9 @@ void RotatedTranslatedShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreat mInnerShape->CollidePoint(transform * inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter); } -void RotatedTranslatedShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +void RotatedTranslatedShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const { - mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform * Mat44::sRotation(mRotation), inScale, ioVertices, inNumVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex); + mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform * Mat44::sRotation(mRotation), inScale, inVertices, inNumVertices, inCollidingShapeIndex); } void RotatedTranslatedShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const diff --git a/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h b/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h index a27851b0f..a0c9ce1e1 100644 --- a/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h +++ b/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h @@ -98,7 +98,7 @@ class JPH_EXPORT RotatedTranslatedShape final : public DecoratedShape virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; // See: Shape::CollideSoftBodyVertices - virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; // See Shape::CollectTransformedShapes virtual void CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override; diff --git a/Jolt/Physics/Collision/Shape/ScaledShape.cpp b/Jolt/Physics/Collision/Shape/ScaledShape.cpp index 9393f4cb0..151181389 100644 --- a/Jolt/Physics/Collision/Shape/ScaledShape.cpp +++ b/Jolt/Physics/Collision/Shape/ScaledShape.cpp @@ -139,9 +139,9 @@ void ScaledShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubSh mInnerShape->CollidePoint(inv_scale * inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter); } -void ScaledShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +void ScaledShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const { - mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform, inScale * mScale, ioVertices, inNumVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex); + mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform, inScale * mScale, inVertices, inNumVertices, inCollidingShapeIndex); } void ScaledShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const diff --git a/Jolt/Physics/Collision/Shape/ScaledShape.h b/Jolt/Physics/Collision/Shape/ScaledShape.h index 9b0fc5d9c..393e8105d 100644 --- a/Jolt/Physics/Collision/Shape/ScaledShape.h +++ b/Jolt/Physics/Collision/Shape/ScaledShape.h @@ -95,7 +95,7 @@ class JPH_EXPORT ScaledShape final : public DecoratedShape virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; // See: Shape::CollideSoftBodyVertices - virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; // See Shape::CollectTransformedShapes virtual void CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override; diff --git a/Jolt/Physics/Collision/Shape/Shape.h b/Jolt/Physics/Collision/Shape/Shape.h index 0b8539c83..38f6f888b 100644 --- a/Jolt/Physics/Collision/Shape/Shape.h +++ b/Jolt/Physics/Collision/Shape/Shape.h @@ -33,7 +33,7 @@ class SubShapeID; class PhysicsMaterial; class TransformedShape; class Plane; -class SoftBodyVertex; +class CollideSoftBodyVertexIterator; class Shape; class StreamOut; class StreamIn; @@ -323,12 +323,10 @@ class JPH_EXPORT Shape : public RefTarget, public NonCopyable /// Collides all vertices of a soft body with this shape and updates SoftBodyVertex::mCollisionPlane, SoftBodyVertex::mCollidingShapeIndex and SoftBodyVertex::mLargestPenetration if a collision with more penetration was found. /// @param inCenterOfMassTransform Center of mass transform for this shape relative to the vertices. /// @param inScale Scale in local space of the shape (scales relative to its center of mass) - /// @param ioVertices The vertices of the soft body - /// @param inNumVertices The number of vertices in ioVertices - /// @param inDeltaTime Delta time of this time step (can be used to extrapolate the position using the velocity of the particle) - /// @param inDisplacementDueToGravity Displacement due to gravity during this time step - /// @param inCollidingShapeIndex Value to store in SoftBodyVertex::mCollidingShapeIndex when a collision was found - virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const = 0; + /// @param inVertices The vertices of the soft body + /// @param inNumVertices The number of vertices in inVertices + /// @param inCollidingShapeIndex Value to store in CollideSoftBodyVertexIterator::mCollidingShapeIndex when a collision was found + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const = 0; /// Collect the leaf transformed shapes of all leaf shapes of this shape. /// inBox is the world space axis aligned box which leaf shapes should collide with. diff --git a/Jolt/Physics/Collision/Shape/SphereShape.cpp b/Jolt/Physics/Collision/Shape/SphereShape.cpp index 9213919f3..b44dd77a1 100644 --- a/Jolt/Physics/Collision/Shape/SphereShape.cpp +++ b/Jolt/Physics/Collision/Shape/SphereShape.cpp @@ -11,7 +11,7 @@ #include #include #include -#include +#include #include #include #include @@ -276,29 +276,26 @@ void SphereShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubSh ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); } -void SphereShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +void SphereShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const { Vec3 center = inCenterOfMassTransform.GetTranslation(); float radius = GetScaledRadius(inScale); - for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v) - if (v->mInvMass > 0.0f) + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) { // Calculate penetration - Vec3 delta = v->mPosition - center; + Vec3 delta = v.GetPosition() - center; float distance = delta.Length(); float penetration = radius - distance; - if (penetration > v->mLargestPenetration) + if (v.UpdatePenetration(penetration)) { - v->mLargestPenetration = penetration; - // Calculate contact point and normal Vec3 normal = distance > 0.0f? delta / distance : Vec3::sAxisY(); Vec3 point = center + radius * normal; // Store collision - v->mCollisionPlane = Plane::sFromPointAndNormal(point, normal); - v->mCollidingShapeIndex = inCollidingShapeIndex; + v.SetCollision(Plane::sFromPointAndNormal(point, normal), inCollidingShapeIndex); } } } diff --git a/Jolt/Physics/Collision/Shape/SphereShape.h b/Jolt/Physics/Collision/Shape/SphereShape.h index c84df100c..dcec723bb 100644 --- a/Jolt/Physics/Collision/Shape/SphereShape.h +++ b/Jolt/Physics/Collision/Shape/SphereShape.h @@ -81,7 +81,7 @@ class JPH_EXPORT SphereShape final : public ConvexShape virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; // See: Shape::CollideSoftBodyVertices - virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; // See Shape::GetTrianglesStart virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; diff --git a/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp b/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp index e540fdad0..a3a9e020d 100644 --- a/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp +++ b/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp @@ -9,7 +9,7 @@ #include #include #include -#include +#include #include #include #include @@ -301,7 +301,7 @@ AABox TaperedCapsuleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, return AABox(p1, p2); } -void TaperedCapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +void TaperedCapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const { JPH_ASSERT(IsValidScale(inScale)); @@ -317,10 +317,10 @@ void TaperedCapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransfo float scaled_top_radius = scale_xz * mTopRadius; float scaled_bottom_radius = scale_xz * mBottomRadius; - for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v) - if (v->mInvMass > 0.0f) + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) { - Vec3 local_pos = scale_y_flip * (inverse_transform * v->mPosition); + Vec3 local_pos = scale_y_flip * (inverse_transform * v.GetPosition()); Vec3 position, normal; @@ -359,16 +359,13 @@ void TaperedCapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransfo Plane plane = Plane::sFromPointAndNormal(position, normal); float penetration = -plane.SignedDistance(local_pos); - if (penetration > v->mLargestPenetration) + if (v.UpdatePenetration(penetration)) { - v->mLargestPenetration = penetration; - // Need to flip the normal's y if capsule is flipped (this corresponds to flipping both the point and the normal around y) plane.SetNormal(scale_y_flip * plane.GetNormal()); // Store collision - v->mCollisionPlane = plane.GetTransformed(inCenterOfMassTransform); - v->mCollidingShapeIndex = inCollidingShapeIndex; + v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); } } } diff --git a/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h b/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h index f0a6fd8f9..e7e070fec 100644 --- a/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h +++ b/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h @@ -81,7 +81,7 @@ class JPH_EXPORT TaperedCapsuleShape final : public ConvexShape virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; // See: Shape::CollideSoftBodyVertices - virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; #ifdef JPH_DEBUG_RENDERER // See Shape::Draw diff --git a/Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp b/Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp index a6ffb5ff8..1df2a8b0b 100644 --- a/Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp +++ b/Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp @@ -9,7 +9,7 @@ #include #include #include -#include +#include #include #include #include @@ -383,7 +383,7 @@ void TaperedCylinderShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); } -void TaperedCylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +void TaperedCylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const { JPH_ASSERT(IsValidScale(inScale)); @@ -395,10 +395,10 @@ void TaperedCylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransf Vec3 top_3d(0, top, 0); Vec3 bottom_3d(0, bottom, 0); - for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v) - if (v->mInvMass > 0.0f) + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) { - Vec3 local_pos = inverse_transform * v->mPosition; + Vec3 local_pos = inverse_transform * v.GetPosition(); // Calculate penetration into side surface Vec3 normal_xz = sCalculateSideNormalXZ(local_pos); @@ -500,14 +500,8 @@ void TaperedCylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransf // Calculate penetration Plane plane = Plane::sFromPointAndNormal(point, normal); float penetration = -plane.SignedDistance(local_pos); - if (penetration > v->mLargestPenetration) - { - v->mLargestPenetration = penetration; - - // Store collision - v->mCollisionPlane = plane.GetTransformed(inCenterOfMassTransform); - v->mCollidingShapeIndex = inCollidingShapeIndex; - } + if (v.UpdatePenetration(penetration)) + v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex); } } diff --git a/Jolt/Physics/Collision/Shape/TaperedCylinderShape.h b/Jolt/Physics/Collision/Shape/TaperedCylinderShape.h index 41bdf310a..d19113398 100644 --- a/Jolt/Physics/Collision/Shape/TaperedCylinderShape.h +++ b/Jolt/Physics/Collision/Shape/TaperedCylinderShape.h @@ -76,7 +76,7 @@ class JPH_EXPORT TaperedCylinderShape final : public ConvexShape virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; // See: Shape::CollideSoftBodyVertices - virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; // See Shape::GetTrianglesStart virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; diff --git a/Jolt/Physics/Collision/Shape/TriangleShape.cpp b/Jolt/Physics/Collision/Shape/TriangleShape.cpp index 894e8a309..17f09f112 100644 --- a/Jolt/Physics/Collision/Shape/TriangleShape.cpp +++ b/Jolt/Physics/Collision/Shape/TriangleShape.cpp @@ -269,16 +269,16 @@ void TriangleShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSub // Can't be inside a triangle } -void TriangleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +void TriangleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const { CollideSoftBodyVerticesVsTriangles collider(inCenterOfMassTransform, inScale); - for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v) - if (v->mInvMass > 0.0f) + for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v) + if (v.GetInvMass() > 0.0f) { - collider.StartVertex(*v); + collider.StartVertex(v); collider.ProcessTriangle(mV1, mV2, mV3); - collider.FinishVertex(*v, inCollidingShapeIndex); + collider.FinishVertex(v, inCollidingShapeIndex); } } diff --git a/Jolt/Physics/Collision/Shape/TriangleShape.h b/Jolt/Physics/Collision/Shape/TriangleShape.h index cd15f48c8..8a1c0cf08 100644 --- a/Jolt/Physics/Collision/Shape/TriangleShape.h +++ b/Jolt/Physics/Collision/Shape/TriangleShape.h @@ -90,7 +90,7 @@ class JPH_EXPORT TriangleShape final : public ConvexShape virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; // See: Shape::CollideSoftBodyVertices - virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; // See Shape::GetTrianglesStart virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; diff --git a/Jolt/Physics/SoftBody/SoftBodyManifold.h b/Jolt/Physics/SoftBody/SoftBodyManifold.h index ea3a65b3b..de21ec50d 100644 --- a/Jolt/Physics/SoftBody/SoftBodyManifold.h +++ b/Jolt/Physics/SoftBody/SoftBodyManifold.h @@ -39,6 +39,18 @@ class SoftBodyManifold return inVertex.mHasContact? mCollidingShapes[inVertex.mCollidingShapeIndex].mBodyID : BodyID(); } + /// Get the number of sensors that are in contact with the soft body + JPH_INLINE uint GetNumSensorContacts() const + { + return (uint)mCollidingSensors.size(); + } + + /// Get the i-th sensor that is in contact with the soft body + JPH_INLINE BodyID GetSensorContactBodyID(uint inIndex) const + { + return mCollidingSensors[inIndex].mBodyID; + } + private: /// Allow SoftBodyMotionProperties to construct us friend class SoftBodyMotionProperties; @@ -46,14 +58,17 @@ class SoftBodyManifold /// Constructor explicit SoftBodyManifold(const SoftBodyMotionProperties *inMotionProperties) : mVertices(inMotionProperties->mVertices), - mCollidingShapes(inMotionProperties->mCollidingShapes) + mCollidingShapes(inMotionProperties->mCollidingShapes), + mCollidingSensors(inMotionProperties->mCollidingSensors) { } using CollidingShape = SoftBodyMotionProperties::CollidingShape; + using CollidingSensor = SoftBodyMotionProperties::CollidingSensor; const Array & mVertices; const Array & mCollidingShapes; + const Array & mCollidingSensors; }; JPH_NAMESPACE_END diff --git a/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp b/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp index e609b64e7..28c3c4abf 100644 --- a/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp +++ b/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -71,9 +72,7 @@ void SoftBodyMotionProperties::Initialize(const SoftBodyCreationSettings &inSett Vertex &out_vertex = mVertices[v]; out_vertex.mPreviousPosition = out_vertex.mPosition = rotation * Vec3(in_vertex.mPosition); out_vertex.mVelocity = rotation.Multiply3x3(Vec3(in_vertex.mVelocity)); - out_vertex.mCollidingShapeIndex = -1; - out_vertex.mHasContact = false; - out_vertex.mLargestPenetration = -FLT_MAX; + out_vertex.ResetCollision(); out_vertex.mInvMass = in_vertex.mInvMass; mLocalBounds.Encapsulate(out_vertex.mPosition); } @@ -105,15 +104,19 @@ void SoftBodyMotionProperties::DetermineCollidingShapes(const SoftBodyUpdateCont { JPH_PROFILE_FUNCTION(); + // Reset flag prior to collision detection + mNeedContactCallback = false; + struct Collector : public CollideShapeBodyCollector { - Collector(const SoftBodyUpdateContext &inContext, const PhysicsSystem &inSystem, const BodyLockInterface &inBodyLockInterface, Array &ioHits) : + Collector(const SoftBodyUpdateContext &inContext, const PhysicsSystem &inSystem, const BodyLockInterface &inBodyLockInterface, Array &ioHits, Array &ioSensors) : mContext(inContext), mInverseTransform(inContext.mCenterOfMassTransform.InversedRotationTranslation()), mBodyLockInterface(inBodyLockInterface), mCombineFriction(inSystem.GetCombineFriction()), mCombineRestitution(inSystem.GetCombineRestitution()), - mHits(ioHits) + mHits(ioHits), + mSensors(ioSensors) { } @@ -149,25 +152,36 @@ void SoftBodyMotionProperties::DetermineCollidingShapes(const SoftBodyUpdateCont return; } - CollidingShape cs; - cs.mCenterOfMassTransform = (mInverseTransform * body.GetCenterOfMassTransform()).ToMat44(); - cs.mShape = body.GetShape(); - cs.mBodyID = inResult; - cs.mMotionType = body.GetMotionType(); - cs.mIsSensor = settings.mIsSensor; - cs.mUpdateVelocities = false; - cs.mFriction = mCombineFriction(soft_body, SubShapeID(), body, SubShapeID()); - cs.mRestitution = mCombineRestitution(soft_body, SubShapeID(), body, SubShapeID()); - cs.mSoftBodyInvMassScale = settings.mInvMassScale1; - if (cs.mMotionType == EMotionType::Dynamic) + Mat44 com = (mInverseTransform * body.GetCenterOfMassTransform()).ToMat44(); + if (settings.mIsSensor) + { + CollidingSensor cs; + cs.mCenterOfMassTransform = com; + cs.mShape = body.GetShape(); + cs.mBodyID = inResult; + mSensors.push_back(cs); + } + else { - const MotionProperties *mp = body.GetMotionProperties(); - cs.mInvMass = settings.mInvMassScale2 * mp->GetInverseMass(); - cs.mInvInertia = settings.mInvInertiaScale2 * mp->GetInverseInertiaForRotation(cs.mCenterOfMassTransform.GetRotation()); - cs.mOriginalLinearVelocity = cs.mLinearVelocity = mInverseTransform.Multiply3x3(mp->GetLinearVelocity()); - cs.mOriginalAngularVelocity = cs.mAngularVelocity = mInverseTransform.Multiply3x3(mp->GetAngularVelocity()); + CollidingShape cs; + cs.mCenterOfMassTransform = com; + cs.mShape = body.GetShape(); + cs.mBodyID = inResult; + cs.mMotionType = body.GetMotionType(); + cs.mUpdateVelocities = false; + cs.mFriction = mCombineFriction(soft_body, SubShapeID(), body, SubShapeID()); + cs.mRestitution = mCombineRestitution(soft_body, SubShapeID(), body, SubShapeID()); + cs.mSoftBodyInvMassScale = settings.mInvMassScale1; + if (cs.mMotionType == EMotionType::Dynamic) + { + const MotionProperties *mp = body.GetMotionProperties(); + cs.mInvMass = settings.mInvMassScale2 * mp->GetInverseMass(); + cs.mInvInertia = settings.mInvInertiaScale2 * mp->GetInverseInertiaForRotation(cs.mCenterOfMassTransform.GetRotation()); + cs.mOriginalLinearVelocity = cs.mLinearVelocity = mInverseTransform.Multiply3x3(mp->GetLinearVelocity()); + cs.mOriginalAngularVelocity = cs.mAngularVelocity = mInverseTransform.Multiply3x3(mp->GetAngularVelocity()); + } + mHits.push_back(cs); } - mHits.push_back(cs); } } } @@ -179,9 +193,10 @@ void SoftBodyMotionProperties::DetermineCollidingShapes(const SoftBodyUpdateCont ContactConstraintManager::CombineFunction mCombineFriction; ContactConstraintManager::CombineFunction mCombineRestitution; Array & mHits; + Array & mSensors; }; - Collector collector(inContext, inSystem, inBodyLockInterface, mCollidingShapes); + Collector collector(inContext, inSystem, inBodyLockInterface, mCollidingShapes, mCollidingSensors); AABox bounds = mLocalBounds; bounds.Encapsulate(mLocalPredictedBounds); bounds = bounds.Transformed(inContext.mCenterOfMassTransform); @@ -192,13 +207,36 @@ void SoftBodyMotionProperties::DetermineCollidingShapes(const SoftBodyUpdateCont inSystem.GetBroadPhaseQuery().CollideAABox(bounds, collector, broadphase_layer_filter, object_layer_filter); } -void SoftBodyMotionProperties::DetermineCollisionPlanes(const SoftBodyUpdateContext &inContext, uint inVertexStart, uint inNumVertices) +void SoftBodyMotionProperties::DetermineCollisionPlanes(uint inVertexStart, uint inNumVertices) { JPH_PROFILE_FUNCTION(); // Generate collision planes for (const CollidingShape &cs : mCollidingShapes) - cs.mShape->CollideSoftBodyVertices(cs.mCenterOfMassTransform, Vec3::sReplicate(1.0f), mVertices.data() + inVertexStart, inNumVertices, inContext.mDeltaTime, inContext.mDisplacementDueToGravity, int(&cs - mCollidingShapes.data())); + cs.mShape->CollideSoftBodyVertices(cs.mCenterOfMassTransform, Vec3::sReplicate(1.0f), CollideSoftBodyVertexIterator(mVertices.data() + inVertexStart), inNumVertices, int(&cs - mCollidingShapes.data())); +} + +void SoftBodyMotionProperties::DetermineSensorCollisions(CollidingSensor &ioSensor) +{ + JPH_PROFILE_FUNCTION(); + + Plane collision_plane; + float largest_penetration = -FLT_MAX; + int colliding_shape_idx = -1; + + // Collide sensor against all vertices + CollideSoftBodyVertexIterator vertex_iterator( + StridedPtr(&mVertices[0].mPosition, sizeof(SoftBodyVertex)), // The position and mass come from the soft body vertex + StridedPtr(&mVertices[0].mInvMass, sizeof(SoftBodyVertex)), + StridedPtr(&collision_plane, 0), // We want all vertices to result in a single collision so we pass stride 0 + StridedPtr(&largest_penetration, 0), + StridedPtr(&colliding_shape_idx, 0)); + ioSensor.mShape->CollideSoftBodyVertices(ioSensor.mCenterOfMassTransform, Vec3::sReplicate(1.0f), vertex_iterator, uint(mVertices.size()), 0); + ioSensor.mHasContact = largest_penetration > 0.0f; + + // We need a contact callback if one of the sensors collided + if (ioSensor.mHasContact) + mNeedContactCallback = true; } void SoftBodyMotionProperties::ApplyPressure(const SoftBodyUpdateContext &inContext) @@ -548,99 +586,97 @@ void SoftBodyMotionProperties::ApplyCollisionConstraintsAndUpdateVelocities(cons { // Remember that there was a collision v.mHasContact = true; - mHasContact = true; - // Sensors should not have a collision response + // We need a contact callback if one of the vertices collided + mNeedContactCallback = true; + + // Note that we already calculated the velocity, so this does not affect the velocity (next iteration starts by setting previous position to current position) CollidingShape &cs = mCollidingShapes[v.mCollidingShapeIndex]; - if (!cs.mIsSensor) + Vec3 contact_normal = v.mCollisionPlane.GetNormal(); + v.mPosition += contact_normal * projected_distance; + + // Apply friction as described in Detailed Rigid Body Simulation with Extended Position Based Dynamics - Matthias Muller et al. + // See section 3.6: + // Inverse mass: w1 = 1 / m1, w2 = 1 / m2 + (r2 x n)^T I^-1 (r2 x n) = 0 for a static object + // r2 are the contact point relative to the center of mass of body 2 + // Lagrange multiplier for contact: lambda = -c / (w1 + w2) + // Where c is the constraint equation (the distance to the plane, negative because penetrating) + // Contact normal force: fn = lambda / dt^2 + // Delta velocity due to friction dv = -vt / |vt| * min(dt * friction * fn * (w1 + w2), |vt|) = -vt * min(-friction * c / (|vt| * dt), 1) + // Note that I think there is an error in the paper, I added a mass term, see: https://github.com/matthias-research/pages/issues/29 + // Relative velocity: vr = v1 - v2 - omega2 x r2 + // Normal velocity: vn = vr . contact_normal + // Tangential velocity: vt = vr - contact_normal * vn + // Impulse: p = dv / (w1 + w2) + // Changes in particle velocities: + // v1 = v1 + p / m1 + // v2 = v2 - p / m2 (no change when colliding with a static body) + // w2 = w2 - I^-1 (r2 x p) (no change when colliding with a static body) + if (cs.mMotionType == EMotionType::Dynamic) { - // Note that we already calculated the velocity, so this does not affect the velocity (next iteration starts by setting previous position to current position) - Vec3 contact_normal = v.mCollisionPlane.GetNormal(); - v.mPosition += contact_normal * projected_distance; - - // Apply friction as described in Detailed Rigid Body Simulation with Extended Position Based Dynamics - Matthias Muller et al. - // See section 3.6: - // Inverse mass: w1 = 1 / m1, w2 = 1 / m2 + (r2 x n)^T I^-1 (r2 x n) = 0 for a static object - // r2 are the contact point relative to the center of mass of body 2 - // Lagrange multiplier for contact: lambda = -c / (w1 + w2) - // Where c is the constraint equation (the distance to the plane, negative because penetrating) - // Contact normal force: fn = lambda / dt^2 - // Delta velocity due to friction dv = -vt / |vt| * min(dt * friction * fn * (w1 + w2), |vt|) = -vt * min(-friction * c / (|vt| * dt), 1) - // Note that I think there is an error in the paper, I added a mass term, see: https://github.com/matthias-research/pages/issues/29 - // Relative velocity: vr = v1 - v2 - omega2 x r2 - // Normal velocity: vn = vr . contact_normal - // Tangential velocity: vt = vr - contact_normal * vn - // Impulse: p = dv / (w1 + w2) - // Changes in particle velocities: - // v1 = v1 + p / m1 - // v2 = v2 - p / m2 (no change when colliding with a static body) - // w2 = w2 - I^-1 (r2 x p) (no change when colliding with a static body) - if (cs.mMotionType == EMotionType::Dynamic) - { - // Calculate normal and tangential velocity (equation 30) - Vec3 r2 = v.mPosition - cs.mCenterOfMassTransform.GetTranslation(); - Vec3 v2 = cs.GetPointVelocity(r2); - Vec3 relative_velocity = v.mVelocity - v2; - Vec3 v_normal = contact_normal * contact_normal.Dot(relative_velocity); - Vec3 v_tangential = relative_velocity - v_normal; - float v_tangential_length = v_tangential.Length(); - - // Calculate resulting inverse mass of vertex - float vertex_inv_mass = cs.mSoftBodyInvMassScale * v.mInvMass; - - // Calculate inverse effective mass - Vec3 r2_cross_n = r2.Cross(contact_normal); - float w2 = cs.mInvMass + r2_cross_n.Dot(cs.mInvInertia * r2_cross_n); - float w1_plus_w2 = vertex_inv_mass + w2; - if (w1_plus_w2 > 0.0f) - { - // Calculate delta relative velocity due to friction (modified equation 31) - Vec3 dv; - if (v_tangential_length > 0.0f) - dv = v_tangential * min(cs.mFriction * projected_distance / (v_tangential_length * dt), 1.0f); - else - dv = Vec3::sZero(); - - // Calculate delta relative velocity due to restitution (equation 35) - dv += v_normal; - float prev_v_normal = (prev_v - v2).Dot(contact_normal); - if (prev_v_normal < restitution_treshold) - dv += cs.mRestitution * prev_v_normal * contact_normal; - - // Calculate impulse - Vec3 p = dv / w1_plus_w2; - - // Apply impulse to particle - v.mVelocity -= p * vertex_inv_mass; - - // Apply impulse to rigid body - cs.mLinearVelocity += p * cs.mInvMass; - cs.mAngularVelocity += cs.mInvInertia * r2.Cross(p); - - // Mark that the velocities of the body we hit need to be updated - cs.mUpdateVelocities = true; - } - } - else if (cs.mSoftBodyInvMassScale > 0.0f) + // Calculate normal and tangential velocity (equation 30) + Vec3 r2 = v.mPosition - cs.mCenterOfMassTransform.GetTranslation(); + Vec3 v2 = cs.GetPointVelocity(r2); + Vec3 relative_velocity = v.mVelocity - v2; + Vec3 v_normal = contact_normal * contact_normal.Dot(relative_velocity); + Vec3 v_tangential = relative_velocity - v_normal; + float v_tangential_length = v_tangential.Length(); + + // Calculate resulting inverse mass of vertex + float vertex_inv_mass = cs.mSoftBodyInvMassScale * v.mInvMass; + + // Calculate inverse effective mass + Vec3 r2_cross_n = r2.Cross(contact_normal); + float w2 = cs.mInvMass + r2_cross_n.Dot(cs.mInvInertia * r2_cross_n); + float w1_plus_w2 = vertex_inv_mass + w2; + if (w1_plus_w2 > 0.0f) { - // Body is not movable, equations are simpler - - // Calculate normal and tangential velocity (equation 30) - Vec3 v_normal = contact_normal * contact_normal.Dot(v.mVelocity); - Vec3 v_tangential = v.mVelocity - v_normal; - float v_tangential_length = v_tangential.Length(); - - // Apply friction (modified equation 31) + // Calculate delta relative velocity due to friction (modified equation 31) + Vec3 dv; if (v_tangential_length > 0.0f) - v.mVelocity -= v_tangential * min(cs.mFriction * projected_distance / (v_tangential_length * dt), 1.0f); + dv = v_tangential * min(cs.mFriction * projected_distance / (v_tangential_length * dt), 1.0f); + else + dv = Vec3::sZero(); - // Apply restitution (equation 35) - v.mVelocity -= v_normal; - float prev_v_normal = prev_v.Dot(contact_normal); + // Calculate delta relative velocity due to restitution (equation 35) + dv += v_normal; + float prev_v_normal = (prev_v - v2).Dot(contact_normal); if (prev_v_normal < restitution_treshold) - v.mVelocity -= cs.mRestitution * prev_v_normal * contact_normal; + dv += cs.mRestitution * prev_v_normal * contact_normal; + + // Calculate impulse + Vec3 p = dv / w1_plus_w2; + + // Apply impulse to particle + v.mVelocity -= p * vertex_inv_mass; + + // Apply impulse to rigid body + cs.mLinearVelocity += p * cs.mInvMass; + cs.mAngularVelocity += cs.mInvInertia * r2.Cross(p); + + // Mark that the velocities of the body we hit need to be updated + cs.mUpdateVelocities = true; } } + else if (cs.mSoftBodyInvMassScale > 0.0f) + { + // Body is not movable, equations are simpler + + // Calculate normal and tangential velocity (equation 30) + Vec3 v_normal = contact_normal * contact_normal.Dot(v.mVelocity); + Vec3 v_tangential = v.mVelocity - v_normal; + float v_tangential_length = v_tangential.Length(); + + // Apply friction (modified equation 31) + if (v_tangential_length > 0.0f) + v.mVelocity -= v_tangential * min(cs.mFriction * projected_distance / (v_tangential_length * dt), 1.0f); + + // Apply restitution (equation 35) + v.mVelocity -= v_normal; + float prev_v_normal = prev_v.Dot(contact_normal); + if (prev_v_normal < restitution_treshold) + v.mVelocity -= cs.mRestitution * prev_v_normal * contact_normal; + } } } } @@ -651,8 +687,18 @@ void SoftBodyMotionProperties::UpdateSoftBodyState(SoftBodyUpdateContext &ioCont JPH_PROFILE_FUNCTION(); // Contact callback - if (mHasContact && ioContext.mContactListener != nullptr) + if (mNeedContactCallback && ioContext.mContactListener != nullptr) + { + // Remove non-colliding sensors from the list + for (int i = int(mCollidingSensors.size()) - 1; i >= 0; --i) + if (!mCollidingSensors[i].mHasContact) + { + mCollidingSensors[i] = std::move(mCollidingSensors.back()); + mCollidingSensors.pop_back(); + } + ioContext.mContactListener->OnSoftBodyContactAdded(*ioContext.mBody, SoftBodyManifold(this)); + } // Loop through vertices once more to update the global state float dt = ioContext.mDeltaTime; @@ -660,7 +706,6 @@ void SoftBodyMotionProperties::UpdateSoftBodyState(SoftBodyUpdateContext &ioCont float max_v_sq = 0.0f; Vec3 linear_velocity = Vec3::sZero(), angular_velocity = Vec3::sZero(); mLocalPredictedBounds = mLocalBounds = { }; - mHasContact = false; for (Vertex &v : mVertices) { // Calculate max square velocity @@ -682,9 +727,7 @@ void SoftBodyMotionProperties::UpdateSoftBodyState(SoftBodyUpdateContext &ioCont mLocalPredictedBounds.Encapsulate(v.mPosition + v.mVelocity * dt + ioContext.mDisplacementDueToGravity); // Reset collision data for the next iteration - v.mCollidingShapeIndex = -1; - v.mHasContact = false; - v.mLargestPenetration = -FLT_MAX; + v.ResetCollision(); } // Calculate linear/angular velocity of the body by averaging all vertices and bringing the value to world space @@ -742,8 +785,9 @@ void SoftBodyMotionProperties::UpdateRigidBodyVelocities(const SoftBodyUpdateCon if (cs.mUpdateVelocities) inBodyInterface.AddLinearAndAngularVelocity(cs.mBodyID, inContext.mCenterOfMassTransform.Multiply3x3(cs.mLinearVelocity - cs.mOriginalLinearVelocity), inContext.mCenterOfMassTransform.Multiply3x3(cs.mAngularVelocity - cs.mOriginalAngularVelocity)); - // Clear colliding shapes to avoid hanging on to references to shapes + // Clear colliding shapes/sensors to avoid hanging on to references to shapes mCollidingShapes.clear(); + mCollidingSensors.clear(); } void SoftBodyMotionProperties::InitializeUpdateContext(float inDeltaTime, Body &inSoftBody, const PhysicsSystem &inSystem, SoftBodyUpdateContext &ioContext) @@ -777,6 +821,15 @@ void SoftBodyMotionProperties::StartNextIteration(const SoftBodyUpdateContext &i IntegratePositions(ioContext); } +void SoftBodyMotionProperties::StartFirstIteration(SoftBodyUpdateContext &ioContext) +{ + // Start the first iteration + JPH_IF_ENABLE_ASSERTS(uint iteration =) ioContext.mNextIteration.fetch_add(1, memory_order_relaxed); + JPH_ASSERT(iteration == 0); + StartNextIteration(ioContext); + ioContext.mState.store(SoftBodyUpdateContext::EState::ApplyConstraints, memory_order_release); +} + SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineCollisionPlanes(SoftBodyUpdateContext &ioContext) { // Do a relaxed read first to see if there is any work to do (this prevents us from doing expensive atomic operations and also prevents us from continuously incrementing the counter and overflowing it) @@ -789,19 +842,44 @@ SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineCol { // Process collision planes uint num_vertices_to_process = min(SoftBodyUpdateContext::cVertexCollisionBatch, num_vertices - next_vertex); - DetermineCollisionPlanes(ioContext, next_vertex, num_vertices_to_process); + DetermineCollisionPlanes(next_vertex, num_vertices_to_process); uint vertices_processed = ioContext.mNumCollisionVerticesProcessed.fetch_add(SoftBodyUpdateContext::cVertexCollisionBatch, memory_order_release) + num_vertices_to_process; if (vertices_processed >= num_vertices) { - // Start the first iteration - JPH_IF_ENABLE_ASSERTS(uint iteration =) ioContext.mNextIteration.fetch_add(1, memory_order_relaxed); - JPH_ASSERT(iteration == 0); - StartNextIteration(ioContext); - ioContext.mState.store(SoftBodyUpdateContext::EState::ApplyConstraints, memory_order_release); + // Determine next state + if (mCollidingSensors.empty()) + StartFirstIteration(ioContext); + else + ioContext.mState.store(SoftBodyUpdateContext::EState::DetermineSensorCollisions, memory_order_release); } return EStatus::DidWork; } } + + return EStatus::NoWork; +} + +SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineSensorCollisions(SoftBodyUpdateContext &ioContext) +{ + // Do a relaxed read to see if there are more sensors to process + uint num_sensors = (uint)mCollidingSensors.size(); + if (ioContext.mNextSensorIndex.load(memory_order_relaxed) < num_sensors) + { + // Fetch next sensor to process + uint sensor_index = ioContext.mNextSensorIndex.fetch_add(1, memory_order_acquire); + if (sensor_index < num_sensors) + { + // Process this sensor + DetermineSensorCollisions(mCollidingSensors[sensor_index]); + + // Determine next state + uint sensors_processed = ioContext.mNumSensorsProcessed.fetch_add(1, memory_order_release) + 1; + if (sensors_processed >= num_sensors) + StartFirstIteration(ioContext); + return EStatus::DidWork; + } + } + return EStatus::NoWork; } @@ -895,6 +973,9 @@ SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelUpdate(SoftB case SoftBodyUpdateContext::EState::DetermineCollisionPlanes: return ParallelDetermineCollisionPlanes(ioContext); + case SoftBodyUpdateContext::EState::DetermineSensorCollisions: + return ParallelDetermineSensorCollisions(ioContext); + case SoftBodyUpdateContext::EState::ApplyConstraints: return ParallelApplyConstraints(ioContext, inPhysicsSettings); diff --git a/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h b/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h index ded13445c..53d2bc872 100644 --- a/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h +++ b/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h @@ -175,7 +175,6 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties RefConst mShape; ///< Shape of the body we hit BodyID mBodyID; ///< Body ID of the body we hit EMotionType mMotionType; ///< Motion type of the body we hit - bool mIsSensor; ///< If the contact should be treated as a sensor vs body contact (no collision response) float mInvMass; ///< Inverse mass of the body we hit float mFriction; ///< Combined friction of the two bodies float mRestitution; ///< Combined restitution of the two bodies @@ -188,6 +187,15 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties Vec3 mOriginalAngularVelocity; ///< Angular velocity of the body in local space to the soft body at start }; + // Collect information about the colliding sensors + struct CollidingSensor + { + Mat44 mCenterOfMassTransform; ///< Transform of the body relative to the soft body + RefConst mShape; ///< Shape of the body we hit + BodyID mBodyID; ///< Body ID of the body we hit + bool mHasContact; ///< If the sensor collided with the soft body + }; + // Information about the state of all skinned vertices struct SkinState { @@ -197,7 +205,10 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties }; /// Do a narrow phase check and determine the closest feature that we can collide with - void DetermineCollisionPlanes(const SoftBodyUpdateContext &inContext, uint inVertexStart, uint inNumVertices); + void DetermineCollisionPlanes(uint inVertexStart, uint inNumVertices); + + /// Do a narrow phase check between a single sensor and the soft body + void DetermineSensorCollisions(CollidingSensor &ioSensor); /// Apply pressure force and update the vertex velocities void ApplyPressure(const SoftBodyUpdateContext &inContext); @@ -226,12 +237,18 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties /// Update the state of the soft body (position, velocity, bounds) void UpdateSoftBodyState(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings); + /// Start the first solver iteration + void StartFirstIteration(SoftBodyUpdateContext &ioContext); + /// Executes tasks that need to run on the start of an iteration (i.e. the stuff that can't run in parallel) void StartNextIteration(const SoftBodyUpdateContext &ioContext); /// Helper function for ParallelUpdate that works on batches of collision planes EStatus ParallelDetermineCollisionPlanes(SoftBodyUpdateContext &ioContext); + /// Helper function for ParallelUpdate that works on sensor collisions + EStatus ParallelDetermineSensorCollisions(SoftBodyUpdateContext &ioContext); + /// Helper function for ParallelUpdate that works on batches of constraints EStatus ParallelApplyConstraints(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings); @@ -252,6 +269,7 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties RefConst mSettings; ///< Configuration of the particles and constraints Array mVertices; ///< Current state of all vertices in the simulation Array mCollidingShapes; ///< List of colliding shapes retrieved during the last update + Array mCollidingSensors; ///< List of colliding sensors retrieved during the last update Array mSkinState; ///< List of skinned positions (1-on-1 with mVertices but only those that are used by the skinning constraints are filled in) AABox mLocalBounds; ///< Bounding box of all vertices AABox mLocalPredictedBounds; ///< Predicted bounding box for all vertices using extrapolation of velocity by last step delta time @@ -259,7 +277,7 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties float mPressure; ///< n * R * T, amount of substance * ideal gas constant * absolute temperature, see https://en.wikipedia.org/wiki/Pressure float mSkinnedMaxDistanceMultiplier = 1.0f; ///< Multiplier applied to Skinned::mMaxDistance to allow tightening or loosening of the skin constraints bool mUpdatePosition; ///< Update the position of the body while simulating (set to false for something that is attached to the static world) - bool mHasContact = false; ///< True if the soft body has collided with anything in the last update + bool mNeedContactCallback = false; ///< True if the soft body has collided with anything in the last update bool mEnableSkinConstraints = true; ///< If skin constraints are enabled bool mSkinStatePreviousPositionValid = false; ///< True if the skinning was updated in the last update so that the previous position of the skin state is valid }; diff --git a/Jolt/Physics/SoftBody/SoftBodyShape.cpp b/Jolt/Physics/SoftBody/SoftBodyShape.cpp index 6b759710d..6692b223e 100644 --- a/Jolt/Physics/SoftBody/SoftBodyShape.cpp +++ b/Jolt/Physics/SoftBody/SoftBodyShape.cpp @@ -114,7 +114,7 @@ void SoftBodyShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSub sCollidePointUsingRayCast(*this, inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter); } -void SoftBodyShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +void SoftBodyShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const { /* Not implemented */ } diff --git a/Jolt/Physics/SoftBody/SoftBodyShape.h b/Jolt/Physics/SoftBody/SoftBodyShape.h index 605ec80a2..70605dae4 100644 --- a/Jolt/Physics/SoftBody/SoftBodyShape.h +++ b/Jolt/Physics/SoftBody/SoftBodyShape.h @@ -47,7 +47,7 @@ class JPH_EXPORT SoftBodyShape final : public Shape virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; - virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override; virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; virtual Stats GetStats() const override; diff --git a/Jolt/Physics/SoftBody/SoftBodyUpdateContext.h b/Jolt/Physics/SoftBody/SoftBodyUpdateContext.h index df8d108b0..4c6a3ea5d 100644 --- a/Jolt/Physics/SoftBody/SoftBodyUpdateContext.h +++ b/Jolt/Physics/SoftBody/SoftBodyUpdateContext.h @@ -34,6 +34,7 @@ class SoftBodyUpdateContext : public NonCopyable enum class EState { DetermineCollisionPlanes, ///< Determine collision planes for vertices in parallel + DetermineSensorCollisions, ///< Determine collisions with sensors in parallel ApplyConstraints, ///< Apply constraints in parallel Done ///< Update is finished }; @@ -41,7 +42,9 @@ class SoftBodyUpdateContext : public NonCopyable // State of the update atomic mState { EState::DetermineCollisionPlanes };///< Current state of the update atomic mNextCollisionVertex { 0 }; ///< Next vertex to process for DetermineCollisionPlanes - atomic mNumCollisionVerticesProcessed { 0 }; ///< Number of vertices processed by DetermineCollisionPlanes, used to determine if we can start simulating + atomic mNumCollisionVerticesProcessed { 0 }; ///< Number of vertices processed by DetermineCollisionPlanes, used to determine if we can go to the next step + atomic mNextSensorIndex { 0 }; ///< Next sensor to process for DetermineCollisionPlanes + atomic mNumSensorsProcessed { 0 }; ///< Number of sensors processed by DetermineSensorCollisions, used to determine if we can go to the next step atomic mNextIteration { 0 }; ///< Next simulation iteration to process atomic mNextConstraintGroup { 0 }; ///< Next constraint group to process atomic mNumConstraintGroupsProcessed { 0 }; ///< Number of groups processed, used to determine if we can go to the next iteration diff --git a/Jolt/Physics/SoftBody/SoftBodyVertex.h b/Jolt/Physics/SoftBody/SoftBodyVertex.h index 68438ef4b..72d4291e2 100644 --- a/Jolt/Physics/SoftBody/SoftBodyVertex.h +++ b/Jolt/Physics/SoftBody/SoftBodyVertex.h @@ -15,13 +15,21 @@ JPH_NAMESPACE_BEGIN class SoftBodyVertex { public: - Vec3 mPreviousPosition; ///< Position at the previous time step + /// Reset collision information to prepare for a new collision check + inline void ResetCollision() + { + mLargestPenetration = -FLT_MAX; + mCollidingShapeIndex = -1; + mHasContact = false; + } + + Vec3 mPreviousPosition; ///< Internal use only. Position at the previous time step Vec3 mPosition; ///< Position, relative to the center of mass of the soft body Vec3 mVelocity; ///< Velocity, relative to the center of mass of the soft body - Plane mCollisionPlane; ///< Nearest collision plane, relative to the center of mass of the soft body - int mCollidingShapeIndex; ///< Index in the colliding shapes list of the body we may collide with + Plane mCollisionPlane; ///< Internal use only. Nearest collision plane, relative to the center of mass of the soft body + int mCollidingShapeIndex; ///< Internal use only. Index in the colliding shapes list of the body we may collide with bool mHasContact; ///< True if the vertex has collided with anything in the last update - float mLargestPenetration; ///< Used while finding the collision plane, stores the largest penetration found so far + float mLargestPenetration; ///< Internal use only. Used while finding the collision plane, stores the largest penetration found so far float mInvMass; ///< Inverse mass (1 / mass) }; diff --git a/Samples/Samples.cmake b/Samples/Samples.cmake index 5dfb8ca38..31a07b8d6 100644 --- a/Samples/Samples.cmake +++ b/Samples/Samples.cmake @@ -177,6 +177,8 @@ set(SAMPLES_SRC_FILES ${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyShapesTest.h ${SAMPLES_ROOT}/Tests/SoftBody/SoftBodySkinnedConstraintTest.cpp ${SAMPLES_ROOT}/Tests/SoftBody/SoftBodySkinnedConstraintTest.h + ${SAMPLES_ROOT}/Tests/SoftBody/SoftBodySensorTest.cpp + ${SAMPLES_ROOT}/Tests/SoftBody/SoftBodySensorTest.h ${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyStressTest.cpp ${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyStressTest.h ${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyUpdatePositionTest.cpp diff --git a/Samples/SamplesApp.cpp b/Samples/SamplesApp.cpp index 2013b4960..cc0b0a6bd 100644 --- a/Samples/SamplesApp.cpp +++ b/Samples/SamplesApp.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -355,6 +356,7 @@ JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodyCustomUpdateTest) JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodyLRAConstraintTest) JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodyBendConstraintTest) JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodySkinnedConstraintTest) +JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodySensorTest) static TestNameAndRTTI sSoftBodyTests[] = { @@ -373,7 +375,8 @@ static TestNameAndRTTI sSoftBodyTests[] = { "Soft Body Custom Update", JPH_RTTI(SoftBodyCustomUpdateTest) }, { "Soft Body LRA Constraint", JPH_RTTI(SoftBodyLRAConstraintTest) }, { "Soft Body Bend Constraint", JPH_RTTI(SoftBodyBendConstraintTest) }, - { "Soft Body Skinned Constraint", JPH_RTTI(SoftBodySkinnedConstraintTest) } + { "Soft Body Skinned Constraint", JPH_RTTI(SoftBodySkinnedConstraintTest) }, + { "Soft Body vs Sensor", JPH_RTTI(SoftBodySensorTest) } }; JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, BroadPhaseCastRayTest) @@ -1466,31 +1469,32 @@ bool SamplesApp::CastProbe(float inProbeLength, float &outFraction, RVec3 &outPo case EProbeMode::CollideSoftBody: { - // Create a soft body vertex const float fraction = 0.2f; const float max_distance = 10.0f; - SoftBodyVertex vertex; - vertex.mInvMass = 1.0f; - vertex.mPosition = fraction * direction; - vertex.mVelocity = 10.0f * direction; - vertex.mCollidingShapeIndex = -1; - vertex.mLargestPenetration = -FLT_MAX; + + // Create a soft body vertex iterator + const float inv_mass = 1.0f; + const Vec3 position = fraction * direction; + Plane largest_penetration_collision_plane; + float largest_penetration = -FLT_MAX; + int largest_penetration_colliding_shape_idx = -1; + CollideSoftBodyVertexIterator vertex_iterator(&position, &inv_mass, &largest_penetration_collision_plane, &largest_penetration, &largest_penetration_colliding_shape_idx); // Get shapes in a large radius around the start position - AABox box(Vec3(start + vertex.mPosition), max_distance); + AABox box(Vec3(start + position), max_distance); AllHitCollisionCollector collector; mPhysicsSystem->GetNarrowPhaseQuery().CollectTransformedShapes(box, collector); // Closest point found using CollideShape, position relative to 'start' - Vec3 closest_point = vertex.mPosition; + Vec3 closest_point = position; float closest_point_penetration = 0; // Test against each shape for (const TransformedShape &ts : collector.mHits) { int colliding_shape_index = int(&ts - collector.mHits.data()); - ts.mShape->CollideSoftBodyVertices((RMat44::sTranslation(-start) * ts.GetCenterOfMassTransform()).ToMat44(), ts.GetShapeScale(), &vertex, 1, 1.0f / 60.0f, Vec3::sZero(), colliding_shape_index); - if (vertex.mCollidingShapeIndex == colliding_shape_index) + ts.mShape->CollideSoftBodyVertices((RMat44::sTranslation(-start) * ts.GetCenterOfMassTransform()).ToMat44(), ts.GetShapeScale(), vertex_iterator, 1, colliding_shape_index); + if (largest_penetration_colliding_shape_idx == colliding_shape_index) { // To draw a plane, we need a point but CollideSoftBodyVertices doesn't provide one, so we use CollideShape with a tiny sphere to get the closest point and then project that onto the plane to draw the plane SphereShape point_sphere(1.0e-6f); @@ -1498,7 +1502,7 @@ bool SamplesApp::CastProbe(float inProbeLength, float &outFraction, RVec3 &outPo CollideShapeSettings settings; settings.mMaxSeparationDistance = sqrt(3.0f) * max_distance; // Box is extended in all directions by max_distance ClosestHitCollisionCollector collide_shape_collector; - ts.CollideShape(&point_sphere, Vec3::sReplicate(1.0f), RMat44::sTranslation(start + vertex.mPosition), settings, start, collide_shape_collector); + ts.CollideShape(&point_sphere, Vec3::sReplicate(1.0f), RMat44::sTranslation(start + position), settings, start, collide_shape_collector); if (collide_shape_collector.HadHit()) { closest_point = collide_shape_collector.mHit.mContactPointOn2; @@ -1508,19 +1512,19 @@ bool SamplesApp::CastProbe(float inProbeLength, float &outFraction, RVec3 &outPo } // Draw test point - mDebugRenderer->DrawMarker(start + vertex.mPosition, Color::sYellow, 0.1f); + mDebugRenderer->DrawMarker(start + position, Color::sYellow, 0.1f); mDebugRenderer->DrawMarker(start + closest_point, Color::sRed, 0.1f); // Draw collision plane - if (vertex.mCollidingShapeIndex != -1) + if (largest_penetration_colliding_shape_idx != -1) { - RVec3 plane_point = start + vertex.mPosition - vertex.mCollisionPlane.GetNormal() * vertex.mCollisionPlane.SignedDistance(vertex.mPosition); - mDebugRenderer->DrawPlane(plane_point, vertex.mCollisionPlane.GetNormal(), Color::sGreen, 2.0f); + RVec3 plane_point = start + position - largest_penetration_collision_plane.GetNormal() * largest_penetration_collision_plane.SignedDistance(position); + mDebugRenderer->DrawPlane(plane_point, largest_penetration_collision_plane.GetNormal(), Color::sGreen, 2.0f); - if (abs(closest_point_penetration - vertex.mLargestPenetration) > 0.001f) - mDebugRenderer->DrawText3D(plane_point, StringFormat("Pen %f (exp %f)", (double)vertex.mLargestPenetration, (double)closest_point_penetration)); + if (abs(closest_point_penetration - largest_penetration) > 0.001f) + mDebugRenderer->DrawText3D(plane_point, StringFormat("Pen %f (exp %f)", (double)largest_penetration, (double)closest_point_penetration)); else - mDebugRenderer->DrawText3D(plane_point, StringFormat("Pen %f", (double)vertex.mLargestPenetration)); + mDebugRenderer->DrawText3D(plane_point, StringFormat("Pen %f", (double)largest_penetration)); } } break; diff --git a/Samples/Tests/SoftBody/SoftBodySensorTest.cpp b/Samples/Tests/SoftBody/SoftBodySensorTest.cpp new file mode 100644 index 000000000..9bcae931d --- /dev/null +++ b/Samples/Tests/SoftBody/SoftBodySensorTest.cpp @@ -0,0 +1,69 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_IMPLEMENT_RTTI_VIRTUAL(SoftBodySensorTest) +{ + JPH_ADD_BASE_CLASS(SoftBodySensorTest, Test) +} + +void SoftBodySensorTest::Initialize() +{ + // Install contact listener for soft bodies + mPhysicsSystem->SetSoftBodyContactListener(this); + + // Floor + CreateFloor(); + + // Create cloth that's fixated at the corners + SoftBodyCreationSettings cloth(SoftBodyCreator::CreateClothWithFixatedCorners(), RVec3(0, 10.0f, 0), Quat::sIdentity(), Layers::MOVING); + mBodyInterface->CreateAndAddSoftBody(cloth, EActivation::Activate); + + // Some sensors to detect the cloth + BodyCreationSettings cylinder_sensor(new TaperedCylinderShapeSettings(4.0f, 1.0f, 2.0f), RVec3(0, 6, 0), Quat::sRotation(Vec3::sAxisX(), 0.5f * JPH_PI), EMotionType::Static, Layers::SENSOR); + cylinder_sensor.mIsSensor = true; + mBodyInterface->CreateAndAddBody(cylinder_sensor, EActivation::DontActivate); + + BodyCreationSettings sphere_sensor(new SphereShape(4.0f), RVec3(4, 5, 0), Quat::sIdentity(), EMotionType::Static, Layers::SENSOR); + sphere_sensor.mIsSensor = true; + mBodyInterface->CreateAndAddBody(sphere_sensor, EActivation::DontActivate); + + // Sphere that falls on the cloth to check that we don't ignore this collision + BodyCreationSettings bcs(new SphereShape(1.0f), RVec3(0, 15, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING); + bcs.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia; + bcs.mMassPropertiesOverride.mMass = 500.0f; + mBodyInterface->CreateAndAddBody(bcs, EActivation::Activate); +} + +void SoftBodySensorTest::OnSoftBodyContactAdded(const Body &inSoftBody, const SoftBodyManifold &inManifold) +{ + // Draw the vertices that are in contact + RMat44 com = inSoftBody.GetCenterOfMassTransform(); + for (const SoftBodyVertex &v : inManifold.GetVertices()) + if (inManifold.HasContact(v)) + DebugRenderer::sInstance->DrawMarker(com * v.mPosition, Color::sGreen, 0.1f); + + // Draw the sensors that are in contact with the soft body + for (uint i = 0; i < inManifold.GetNumSensorContacts(); ++i) + { + BodyID sensor_id = inManifold.GetSensorContactBodyID(i); + BodyLockRead lock(mPhysicsSystem->GetBodyLockInterfaceNoLock(), sensor_id); // Can't lock in a callback + if (lock.SucceededAndIsInBroadPhase()) + { + AABox bounds = lock.GetBody().GetWorldSpaceBounds(); + DebugRenderer::sInstance->DrawWireBox(bounds, Color::sGreen); + } + } +} diff --git a/Samples/Tests/SoftBody/SoftBodySensorTest.h b/Samples/Tests/SoftBody/SoftBodySensorTest.h new file mode 100644 index 000000000..5d6f38556 --- /dev/null +++ b/Samples/Tests/SoftBody/SoftBodySensorTest.h @@ -0,0 +1,21 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +// This test shows interaction between a soft body and a sensor +class SoftBodySensorTest : public Test, public SoftBodyContactListener +{ +public: + JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, SoftBodySensorTest) + + // See: Test + virtual void Initialize() override; + + // See: SoftBodyContactListener + virtual void OnSoftBodyContactAdded(const Body &inSoftBody, const SoftBodyManifold &inManifold) override; +};