SimSweep.h

Go to the documentation of this file.

// Copyright Epic Games, Inc. All Rights Reserved.
#pragma once
 
#include "Chaos/Collision/CollisionFilter.h"
#include "Chaos/GeometryQueries.h"
#include "Chaos/ISpatialAcceleration.h"
#include "Chaos/ParticleHandle.h"
 
namespace Chaos
{
    namespace Private
    {
        class FSimSweepParticleHit
        {
        public:
            FSimSweepParticleHit()
                : HitParticle(nullptr)
            {
            }
 
            void Init()
            {
                HitParticle = nullptr;
            }
 
            bool IsHit() const
            {
                return HitParticle != nullptr;
            }
 
            // The particle that we hit, or null
            const FGeometryParticleHandle* HitParticle;
 
            // The shape that we hit (or uninitialized)
            const FPerShapeData* HitShape;
 
            // The geometry that we hit (or uninitialized)
            const FImplicitObject* HitGeometry;
 
            // The contact position (or uninitialized)
            FVec3 HitPosition;
 
            // The contact normal for HitDistance >= 0, or MTD direction for HitDistance < 0 (or uninitialized)
            FVec3 HitNormal;
 
            // The normal of the face we hit, if HitFaceIndex is set (or uninitialized)
            FVec3 HitFaceNormal;
 
            // The distance along the sweep at the hit, or the seperation if negative (i.e., -Penetration) (or uninitialized)
            FReal HitDistance;
 
            // Time of impact [0,1] (or uninitialized)
            FReal HitTOI;
 
            // The face index of the shape we hit, or INDEX_NONE (or uninitialized)
            int32 HitFaceIndex;
        };
 
        class FSimOverlapParticleShape
        {
        public:
            FSimOverlapParticleShape()
            {
            }
 
            // The particle that we hit, or null
            const FGeometryParticleHandle* HitParticle;
 
            // The shape that we hit (or uninitialized)
            const FPerShapeData* HitShape;
 
            // The world-space transform of the geometry that was hit
            FRigidTransform3 ShapeWorldTransform;
        };
 
        template<typename TVisitor>
        class TSimSweepSQVisitor : public ISpatialVisitor<FAccelerationStructureHandle, FReal>
        {
        public:
            using FVisitorData = TSpatialVisitorData<FAccelerationStructureHandle>;
 
            TSimSweepSQVisitor(const TVisitor& InVisitor)
                : Visitor(InVisitor)
            {
            }
 
            virtual bool Overlap(const FVisitorData& Instance) override final
            {
                return CallVisitor(Instance);
            }
 
            virtual bool Sweep(const FVisitorData& Instance, FQueryFastData& CurData) override final
            {
                return CallVisitor(Instance);
            }
 
            virtual bool Raycast(const FVisitorData& Instance, FQueryFastData& CurData) override final
            {
                return CallVisitor(Instance);
            }
        private:
            bool CallVisitor(const FVisitorData& Instance)
            {
                FGeometryParticleHandle* Particle = Instance.Payload.GetGeometryParticleHandle_PhysicsThread();
                Visitor(Particle);
                return true;
            }
 
            const TVisitor& Visitor;
        };
 
        template<typename TParticleFilter, typename TShapeFilter, typename TOverlapCollector>
        void SimOverlapBounds(
            ISpatialAcceleration<FAccelerationStructureHandle, FReal, 3>* SpatialAcceleration,
            const FAABB3& QueryBounds,
            TParticleFilter& ParticleFilter,
            TShapeFilter& ShapeFilter,
            TOverlapCollector& OverlapCollector)
        {
            FImplicitBox3 QueryBox(QueryBounds.Min(), QueryBounds.Max());
 
            // Functor: Process a particle encountered by the overla through the spatial acceleration
            const auto& ProcessParticle =
                [&QueryBox, &ParticleFilter, &ShapeFilter, &OverlapCollector]
                (const FGeometryParticleHandle* OtherParticle)
                -> void
            {
                // Check the particle filter to see if we should process this particle pair
                if (ParticleFilter(OtherParticle))
                {
                    // The particle transform
                    const FRigidTransform3 OtherWorldTransform = OtherParticle->GetTransformXR();
                    for (const TUniquePtr<FPerShapeData>& OtherShape : OtherParticle->ShapesArray())
                    {
                        const FImplicitObject* OtherImplicit = OtherShape->GetLeafGeometry();
 
                        // Check the filter to see if we want to  process this shape pair
                        const bool bOverlapShape = ShapeFilter(OtherShape.Get(), OtherImplicit);
 
                        if (bOverlapShape)
                        {
                            const FRigidTransform3 OtherShapeWorldTransform = FRigidTransform3(OtherShape->GetLeafRelativeTransform()) * OtherWorldTransform;
 
                            const bool bOverlapHit = OverlapQuery(
                                *OtherImplicit,
                                OtherShapeWorldTransform,
                                QueryBox,
                                FRigidTransform3::Identity,
                                0/*Thickness*/,
                                nullptr);
 
                            // If we have a hit, pass it to the collector
                            if (bOverlapHit)
                            {
                                FSimOverlapParticleShape Overlap;
                                Overlap.HitParticle = OtherParticle;
                                Overlap.HitShape = OtherShape.Get();
                                Overlap.ShapeWorldTransform = OtherShapeWorldTransform;
 
                                OverlapCollector(Overlap);
                            }
                        }
                    }
                }
            };
 
            // Generate the set of objects that overlap the bounds
            TSimSweepSQVisitor SpatialAcclerationVisitor(ProcessParticle);
            SpatialAcceleration->Overlap(QueryBounds, SpatialAcclerationVisitor);
        }
 
        template<typename TShapeFilter, typename THitCollector>
        void SimSweepParticlePair(
            const FGeometryParticleHandle* SweptParticle,
            const FGeometryParticleHandle* OtherParticle,
            const FVec3& StartPos,
            const FRotation3& Rot,
            const FVec3& Dir,
            const FReal Length,
            TShapeFilter& ShapeFilter,
            THitCollector& HitCollector)
        {
            // The particle transforms
            const FRigidTransform3 SweptWorldTransform = FRigidTransform3(StartPos, Rot);
            const FRigidTransform3 OtherWorldTransform = OtherParticle->GetTransformXR();
 
            // Storage for the next hit result in the sweep (we only need one at a time)
            FSimSweepParticleHit Hit;
            Hit.HitParticle = OtherParticle;
 
            // Visit all shape pairs and run a sweep (if they pass a filter check)
            for (const TUniquePtr<FPerShapeData>& SweptShape : SweptParticle->ShapesArray())
            {
                const FImplicitObject* SweptImplicit = SweptShape->GetLeafGeometry();
                const FRigidTransform3 SweptShapeWorldTransform = FRigidTransform3(SweptShape->GetLeafRelativeTransform()) * SweptWorldTransform;
 
                for (const TUniquePtr<FPerShapeData>& OtherShape : OtherParticle->ShapesArray())
                {
                    const FImplicitObject* OtherImplicit = OtherShape->GetLeafGeometry();
 
                    // Check the filter to see if we want to  process this shape pair
                    const bool bSweepShape = ShapeFilter(SweptShape.Get(), SweptImplicit, OtherShape.Get(), OtherImplicit);
 
                    if (bSweepShape)
                    {
                        const FRigidTransform3 OtherShapeWorldTransform = FRigidTransform3(OtherShape->GetLeafRelativeTransform()) * OtherWorldTransform;
 
                        const bool bSweepHit = SweepQuery(
                            *OtherImplicit,
                            OtherShapeWorldTransform,
                            *SweptImplicit,
                            SweptShapeWorldTransform,
                            Dir,
                            Length,
                            Hit.HitDistance,
                            Hit.HitPosition,
                            Hit.HitNormal,
                            Hit.HitFaceIndex,
                            Hit.HitFaceNormal,
                            0/*Thickness*/,
                            true/*bComputeMTD*/);
 
                        // If we have a hit, pass it to the collector
                        if (bSweepHit)
                        {
                            // Fill in the hit data that wasn't filled in by SweepQuery
                            Hit.HitTOI = (Hit.HitDistance > 0) ? (Hit.HitDistance / Length) : 0;
 
                            HitCollector(Dir, Length, Hit);
                        }
                    }
                }
            }
        }
 
        template<typename TParticleFilter, typename TShapeFilter, typename THitCollector>
        void SimSweepParticle(
            ISpatialAcceleration<FAccelerationStructureHandle, FReal, 3>* SpatialAcceleration,
            const FGeometryParticleHandle* SweptParticle,
            const FVec3& StartPos, 
            const FRotation3& Rot,
            const FVec3& Dir,
            const FReal Length,
            TParticleFilter& ParticleFilter, 
            TShapeFilter& ShapeFilter,
            THitCollector& HitCollector)
        {
            // Cannot sweep infinite particles (they are all static anyway)
            if (!SweptParticle->HasBounds())
            {
                return;
            }
 
            // @todo(chaos): do we need to generate a hit for no sweep?
            if (Length < UE_SMALL_NUMBER)
            {
                return;
            }
 
            // Functor: Process a particle encountered by the sweep through the spatial acceleration
            // by sweeping all shapes of the swept particle against all shapes of the other particle.
            const auto& ProcessParticlePair = 
                [SweptParticle, &StartPos, &Dir, &Length, &Rot, &ParticleFilter, &ShapeFilter, &HitCollector]
                (const FGeometryParticleHandle* OtherParticle)
                -> void
                {
                    // We cannot hit ourself
                    if (OtherParticle != SweptParticle)
                    {
                        // Check the particle filter to see if we should process this particle pair
                        if (ParticleFilter(SweptParticle, OtherParticle))
                        {
                            // Do the sweep
                            SimSweepParticlePair(SweptParticle, OtherParticle, StartPos, Rot, Dir, Length, ShapeFilter, HitCollector);
                        }
                    }
                };
 
            // Calculate the ray start/end and extents for the spatial acceleration sweep
            // (the sweep through the spatial acceleration requires a ray centered on the bounds)
            const FAABB3 TransformedParticleBounds = SweptParticle->GetGeometry()->CalculateTransformedBounds(FRigidTransform3(StartPos, Rot));
            const FVec3 BoundsExtents = FReal(0.5) * TransformedParticleBounds.Extents();
            const FVec3 BoundsStartPos = TransformedParticleBounds.Center();
 
            // Sweep our bounds through the spatial acceleration structure and then sweep the shapes of our particle
            // against those of any particles that our bounds sweep encounters.
            TSimSweepSQVisitor SpatialAcclerationVisitor(ProcessParticlePair);
            SpatialAcceleration->Sweep(BoundsStartPos, Dir, Length, BoundsExtents, SpatialAcclerationVisitor);
        }
 
 
        class FSimSweepParticleFilterBroadPhase
        {
        public:
            FSimSweepParticleFilterBroadPhase(FIgnoreCollisionManager* InIgnoreCollisionManager)
                : IgnoreCollisionManager(InIgnoreCollisionManager)
            {
            }
 
            bool operator()(const FGeometryParticleHandle* SweptParticle, const FGeometryParticleHandle* OtherParticle)
            {
                return ParticlePairBroadPhaseFilter(SweptParticle, OtherParticle, IgnoreCollisionManager);
            }
 
        private:
            FIgnoreCollisionManager* IgnoreCollisionManager;
        };
 
        class FSimSweepShapeFilterNarrowPhase
        {
        public:
            bool operator()(const FPerShapeData* SweptShape, const FImplicitObject* SweptImplicit, const FPerShapeData* OtherShape, const FImplicitObject* OtherImplicit)
            {
                const EImplicitObjectType SweptImplicitType = SweptImplicit ? GetInnerType(SweptImplicit->GetType()) : ImplicitObjectType::Unknown;
                const EImplicitObjectType OtherImplicitType = OtherImplicit ? GetInnerType(OtherImplicit->GetType()) : ImplicitObjectType::Unknown;
 
                return ShapePairNarrowPhaseFilter(SweptImplicitType, SweptShape, OtherImplicitType, OtherShape);
            }
        };
 
        class FSimSweepCollectorFirstHit
        {
        public:
            FSimSweepCollectorFirstHit(const FReal InHitDistanceEqualTolerance, FSimSweepParticleHit& OutFirstHit)
                : FirstHit(OutFirstHit)
                , HitDistanceEqualTolerance(InHitDistanceEqualTolerance)
            {
                Init();
            }
 
            bool operator()(const FVec3& Dir, const FReal Length, const FSimSweepParticleHit& InHit)
            {
                ProcessHit(Dir, Length, InHit);
                return true;
            }
 
            void Init()
            {
                FirstHit.Init();
            }
 
            bool IsHit() const
            {
                return FirstHit.IsHit();
            }
 
            const FSimSweepParticleHit& GetFirstHit() const
            {
                check(IsHit());
                return FirstHit;
            }
 
        private:
            void ProcessHit(const FVec3& Dir, const FReal Length, const FSimSweepParticleHit& InHit)
            {
                if (!FirstHit.IsHit() || (InHit.HitDistance < FirstHit.HitDistance + HitDistanceEqualTolerance))
                {
                    // If the distance is very close to the existing hit, keep the one with the most opposing normal.
                    if (FirstHit.IsHit() && FMath::IsNearlyEqual(InHit.HitDistance, FirstHit.HitDistance))
                    {
                        const FReal ExistingDotNormal = FVec3::DotProduct(Dir, FirstHit.HitNormal);
                        const FReal NewDotNormal = FVec3::DotProduct(Dir, InHit.HitNormal);
                        if (NewDotNormal < ExistingDotNormal)
                        {
                            FirstHit = InHit;
                        }
                        return;
                    }
 
                    // If this is a closer hit we will take it unless we are already moving out of contact.
                    // NOTE: we will only ever get an outward pointing normal for initial overlaps (HitDistance <= 0).
                    if ((InHit.HitDistance > 0) || (FVec3::DotProduct(InHit.HitNormal, Dir) < 0))
                    {
                        FirstHit = InHit;
                        return;
                    }
                }
            }
 
            FSimSweepParticleHit& FirstHit;
            FReal HitDistanceEqualTolerance;
        };
 
        extern bool SimSweepParticleFirstHit(
            ISpatialAcceleration<FAccelerationStructureHandle, FReal, 3>* SpatialAcceleration,
            FIgnoreCollisionManager* IgnoreCollisionManager,
            const FGeometryParticleHandle* SweptParticle,
            const FVec3& StartPos, 
            const FRotation3& Rot,
            const FVec3& Dir,
            const FReal Length,
            FSimSweepParticleHit& OutHit,
            const FReal InHitDistanceEqualTolerance = UE_KINDA_SMALL_NUMBER);
 
        extern bool SimOverlapBoundsAll(
            ISpatialAcceleration<FAccelerationStructureHandle, FReal, 3>* SpatialAcceleration,
            const FAABB3& QueryBounds,
            TArray<FSimOverlapParticleShape>& Overlaps);
    }
 
}