|
|
@@ -0,0 +1,208 @@
|
|
|
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
|
|
|
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
|
|
|
+// SPDX-License-Identifier: MIT
|
|
|
+
|
|
|
+#pragma once
|
|
|
+
|
|
|
+#include <Jolt/Core/QuickSort.h>
|
|
|
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
|
|
|
+
|
|
|
+//#define JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
|
|
|
+
|
|
|
+#ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
|
|
|
+#include <Jolt/Renderer/DebugRenderer.h>
|
|
|
+#endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
|
|
|
+
|
|
|
+JPH_NAMESPACE_BEGIN
|
|
|
+
|
|
|
+// Removes internal edges from collision results. Can be used to filter out 'ghost collisions'.
|
|
|
+// Based on: Contact generation for meshes - Pierre Terdiman (https://www.codercorner.com/MeshContacts.pdf)
|
|
|
+class InternalEdgeRemovingCollector : public CollideShapeCollector
|
|
|
+{
|
|
|
+ static constexpr uint cMaxDelayedResults = 16;
|
|
|
+ static constexpr uint cMaxVoidedFeatures = 128;
|
|
|
+
|
|
|
+ /// Check if a vertex is voided
|
|
|
+ inline bool IsVoided(Vec3 inV) const
|
|
|
+ {
|
|
|
+ for (const Float3 &vf : mVoidedFeatures)
|
|
|
+ if (inV.IsClose(Vec3::sLoadFloat3Unsafe(vf), 1.0e-8f))
|
|
|
+ return true;
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ /// Add all vertices of a face to the voided features
|
|
|
+ inline void VoidFeatures(const CollideShapeResult &inResult)
|
|
|
+ {
|
|
|
+ for (const Vec3 &v : inResult.mShape2Face)
|
|
|
+ if (!IsVoided(v))
|
|
|
+ {
|
|
|
+ if (mVoidedFeatures.size() == cMaxVoidedFeatures)
|
|
|
+ break;
|
|
|
+ Float3 f;
|
|
|
+ v.StoreFloat3(&f);
|
|
|
+ mVoidedFeatures.push_back(f);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ /// Call the chained collector
|
|
|
+ inline void Chain(const CollideShapeResult &inResult)
|
|
|
+ {
|
|
|
+ mChainedCollector.AddHit(inResult);
|
|
|
+ }
|
|
|
+
|
|
|
+ /// Call the chained collector and void all features of inResult
|
|
|
+ inline void ChainAndVoid(const CollideShapeResult &inResult)
|
|
|
+ {
|
|
|
+ Chain(inResult);
|
|
|
+ VoidFeatures(inResult);
|
|
|
+
|
|
|
+ #ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
|
|
|
+ DebugRenderer::sInstance->DrawWirePolygon(RMat44::sIdentity(), inResult.mShape2Face, Color::sGreen);
|
|
|
+ DebugRenderer::sInstance->DrawArrow(RVec3(inResult.mContactPointOn2), RVec3(inResult.mContactPointOn2) + inResult.mPenetrationAxis.NormalizedOr(Vec3::sZero()), Color::sGreen, 0.1f);
|
|
|
+ #endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
|
|
|
+ }
|
|
|
+
|
|
|
+public:
|
|
|
+ /// Constructor, configures a collector to be called with all the results that do not hit internal edges
|
|
|
+ explicit InternalEdgeRemovingCollector(CollideShapeCollector &inChainedCollector) :
|
|
|
+ mChainedCollector(inChainedCollector)
|
|
|
+ {
|
|
|
+ }
|
|
|
+
|
|
|
+ // See: CollideShapeCollector::AddHit
|
|
|
+ virtual void AddHit(const CollideShapeResult &inResult) override
|
|
|
+ {
|
|
|
+ // We only support welding when the shape is a triangle or has more vertices so that we can calculate a normal
|
|
|
+ if (inResult.mShape2Face.size() < 3)
|
|
|
+ return ChainAndVoid(inResult);
|
|
|
+
|
|
|
+ // Get the triangle normal of shape 2 face
|
|
|
+ Vec3 triangle_normal = (inResult.mShape2Face[1] - inResult.mShape2Face[0]).Cross(inResult.mShape2Face[2] - inResult.mShape2Face[0]);
|
|
|
+ float triangle_normal_len = triangle_normal.Length();
|
|
|
+ if (triangle_normal_len < 1e-6f)
|
|
|
+ return ChainAndVoid(inResult);
|
|
|
+
|
|
|
+ // If the triangle normal matches the contact normal within 1 degree, we can process the contact immediately
|
|
|
+ // We make the assumption here that if the contact normal and the triangle normal align that the we're dealing with a 'face contact'
|
|
|
+ Vec3 contact_normal = -inResult.mPenetrationAxis;
|
|
|
+ float contact_normal_len = inResult.mPenetrationAxis.Length();
|
|
|
+ if (triangle_normal.Dot(contact_normal) > 0.999848f * contact_normal_len * triangle_normal_len) // cos(1 degree)
|
|
|
+ return ChainAndVoid(inResult);
|
|
|
+
|
|
|
+ // Delayed processing
|
|
|
+ if (mDelayedResults.size() == cMaxDelayedResults)
|
|
|
+ return ChainAndVoid(inResult);
|
|
|
+ mDelayedResults.push_back(inResult);
|
|
|
+ }
|
|
|
+
|
|
|
+ /// After all hits have been added, call this function to process the delayed results
|
|
|
+ void Flush()
|
|
|
+ {
|
|
|
+ // Sort on biggest penetration depth first
|
|
|
+ uint sorted_indices[cMaxDelayedResults];
|
|
|
+ for (uint i = 0; i < uint(mDelayedResults.size()); ++i)
|
|
|
+ sorted_indices[i] = i;
|
|
|
+ QuickSort(sorted_indices, sorted_indices + mDelayedResults.size(), [this](uint inLHS, uint inRHS) { return mDelayedResults[inLHS].mPenetrationDepth > mDelayedResults[inRHS].mPenetrationDepth; });
|
|
|
+
|
|
|
+ // Loop over all results
|
|
|
+ for (uint i = 0; i < uint(mDelayedResults.size()); ++i)
|
|
|
+ {
|
|
|
+ const CollideShapeResult &r = mDelayedResults[sorted_indices[i]];
|
|
|
+
|
|
|
+ // Determine which vertex or which edge is the closest to the contact point
|
|
|
+ float best_dist_sq = FLT_MAX;
|
|
|
+ uint best_v1_idx = 0;
|
|
|
+ uint best_v2_idx = 0;
|
|
|
+ uint num_v = uint(r.mShape2Face.size());
|
|
|
+ uint v1_idx = num_v - 1;
|
|
|
+ Vec3 v1 = r.mShape2Face[v1_idx] - r.mContactPointOn2;
|
|
|
+ for (uint v2_idx = 0; v2_idx < num_v; ++v2_idx)
|
|
|
+ {
|
|
|
+ Vec3 v2 = r.mShape2Face[v2_idx] - r.mContactPointOn2;
|
|
|
+ Vec3 v1_v2 = v2 - v1;
|
|
|
+ float denominator = v1_v2.LengthSq();
|
|
|
+ if (denominator < Square(FLT_EPSILON))
|
|
|
+ {
|
|
|
+ // Degenerate, assume v1 is closest, v2 will be tested in a later iteration
|
|
|
+ float v1_len_sq = v1.LengthSq();
|
|
|
+ if (v1_len_sq < best_dist_sq)
|
|
|
+ {
|
|
|
+ best_dist_sq = v1_len_sq;
|
|
|
+ best_v1_idx = v1_idx;
|
|
|
+ best_v2_idx = v1_idx;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ // Taken from ClosestPoint::GetBaryCentricCoordinates
|
|
|
+ float fraction = -v1.Dot(v1_v2) / denominator;
|
|
|
+ if (fraction < 1.0e-6f)
|
|
|
+ {
|
|
|
+ // Closest lies on v1
|
|
|
+ float v1_len_sq = v1.LengthSq();
|
|
|
+ if (v1_len_sq < best_dist_sq)
|
|
|
+ {
|
|
|
+ best_dist_sq = v1_len_sq;
|
|
|
+ best_v1_idx = v1_idx;
|
|
|
+ best_v2_idx = v1_idx;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if (fraction < 1.0f - 1.0e-6f)
|
|
|
+ {
|
|
|
+ // Closest lies on the line segment v1, v2
|
|
|
+ Vec3 closest = v1 + fraction * v1_v2;
|
|
|
+ float closest_len_sq = closest.LengthSq();
|
|
|
+ if (closest_len_sq < best_dist_sq)
|
|
|
+ {
|
|
|
+ best_dist_sq = closest_len_sq;
|
|
|
+ best_v1_idx = v1_idx;
|
|
|
+ best_v2_idx = v2_idx;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ // else closest is v2, but v2 will be tested in a later iteration
|
|
|
+ }
|
|
|
+
|
|
|
+ v1_idx = v2_idx;
|
|
|
+ v1 = v2;
|
|
|
+ }
|
|
|
+
|
|
|
+ // Check if this vertex/edge is voided
|
|
|
+ bool voided = IsVoided(r.mShape2Face[best_v1_idx])
|
|
|
+ && (best_v1_idx == best_v2_idx || IsVoided(r.mShape2Face[best_v2_idx]));
|
|
|
+
|
|
|
+ #ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
|
|
|
+ Color color = voided? Color::sRed : Color::sYellow;
|
|
|
+ DebugRenderer::sInstance->DrawText3D(RVec3(r.mContactPointOn2), StringFormat("%d: %g", i, r.mPenetrationDepth), color, 0.1f);
|
|
|
+ DebugRenderer::sInstance->DrawWirePolygon(RMat44::sIdentity(), r.mShape2Face, color);
|
|
|
+ DebugRenderer::sInstance->DrawArrow(RVec3(r.mContactPointOn2), RVec3(r.mContactPointOn2) + r.mPenetrationAxis.NormalizedOr(Vec3::sZero()), color, 0.1f);
|
|
|
+ DebugRenderer::sInstance->DrawMarker(RVec3(r.mShape2Face[best_v1_idx]), IsVoided(r.mShape2Face[best_v1_idx])? Color::sRed : Color::sYellow, 0.1f);
|
|
|
+ DebugRenderer::sInstance->DrawMarker(RVec3(r.mShape2Face[best_v2_idx]), IsVoided(r.mShape2Face[best_v2_idx])? Color::sRed : Color::sYellow, 0.1f);
|
|
|
+ #endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
|
|
|
+
|
|
|
+ // No voided features, accept the contact
|
|
|
+ if (!voided)
|
|
|
+ Chain(r);
|
|
|
+
|
|
|
+ // Void the features of this face
|
|
|
+ VoidFeatures(r);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ /// Version of CollisionDispatch::sCollideShapeVsShape that removes internal edges
|
|
|
+ static void sCollideShapeVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { })
|
|
|
+ {
|
|
|
+ JPH_ASSERT(inCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces); // Won't work without collecting faces
|
|
|
+
|
|
|
+ InternalEdgeRemovingCollector wrapper(ioCollector);
|
|
|
+ CollisionDispatch::sCollideShapeVsShape(inShape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, wrapper, inShapeFilter);
|
|
|
+ wrapper.Flush();
|
|
|
+ }
|
|
|
+
|
|
|
+private:
|
|
|
+ CollideShapeCollector & mChainedCollector;
|
|
|
+ StaticArray<Float3, cMaxVoidedFeatures> mVoidedFeatures; // Read with Vec3::sLoadFloat3Unsafe so must not be the last member
|
|
|
+ StaticArray<CollideShapeResult, cMaxDelayedResults> mDelayedResults;
|
|
|
+};
|
|
|
+
|
|
|
+JPH_NAMESPACE_END
|
Jorrit Rouwe