2770 lines
118 KiB
C++
2770 lines
118 KiB
C++
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
|
|
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
|
|
// SPDX-License-Identifier: MIT
|
|
|
|
#include <Jolt/Jolt.h>
|
|
|
|
#include <Jolt/Physics/PhysicsSystem.h>
|
|
#include <Jolt/Physics/PhysicsSettings.h>
|
|
#include <Jolt/Physics/PhysicsUpdateContext.h>
|
|
#include <Jolt/Physics/PhysicsStepListener.h>
|
|
#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h>
|
|
#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h>
|
|
#include <Jolt/Physics/Collision/CollisionDispatch.h>
|
|
#include <Jolt/Physics/Collision/AABoxCast.h>
|
|
#include <Jolt/Physics/Collision/ShapeCast.h>
|
|
#include <Jolt/Physics/Collision/CollideShape.h>
|
|
#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
|
|
#include <Jolt/Physics/Collision/CastResult.h>
|
|
#include <Jolt/Physics/Collision/CollideConvexVsTriangles.h>
|
|
#include <Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h>
|
|
#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
|
|
#include <Jolt/Physics/Collision/SimShapeFilterWrapper.h>
|
|
#include <Jolt/Physics/Collision/InternalEdgeRemovingCollector.h>
|
|
#include <Jolt/Physics/Constraints/CalculateSolverSteps.h>
|
|
#include <Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h>
|
|
#include <Jolt/Physics/DeterminismLog.h>
|
|
#include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
|
|
#include <Jolt/Physics/SoftBody/SoftBodyShape.h>
|
|
#include <Jolt/Geometry/RayAABox.h>
|
|
#include <Jolt/Geometry/ClosestPoint.h>
|
|
#include <Jolt/Core/JobSystem.h>
|
|
#include <Jolt/Core/TempAllocator.h>
|
|
#include <Jolt/Core/QuickSort.h>
|
|
#include <Jolt/Core/ScopeExit.h>
|
|
#ifdef JPH_DEBUG_RENDERER
|
|
#include <Jolt/Renderer/DebugRenderer.h>
|
|
#endif // JPH_DEBUG_RENDERER
|
|
|
|
JPH_NAMESPACE_BEGIN
|
|
|
|
#ifdef JPH_DEBUG_RENDERER
|
|
bool PhysicsSystem::sDrawMotionQualityLinearCast = false;
|
|
#endif // JPH_DEBUG_RENDERER
|
|
|
|
//#define BROAD_PHASE BroadPhaseBruteForce
|
|
#define BROAD_PHASE BroadPhaseQuadTree
|
|
|
|
static const Color cColorUpdateBroadPhaseFinalize = Color::sGetDistinctColor(1);
|
|
static const Color cColorUpdateBroadPhasePrepare = Color::sGetDistinctColor(2);
|
|
static const Color cColorFindCollisions = Color::sGetDistinctColor(3);
|
|
static const Color cColorApplyGravity = Color::sGetDistinctColor(4);
|
|
static const Color cColorSetupVelocityConstraints = Color::sGetDistinctColor(5);
|
|
static const Color cColorBuildIslandsFromConstraints = Color::sGetDistinctColor(6);
|
|
static const Color cColorDetermineActiveConstraints = Color::sGetDistinctColor(7);
|
|
static const Color cColorFinalizeIslands = Color::sGetDistinctColor(8);
|
|
static const Color cColorContactRemovedCallbacks = Color::sGetDistinctColor(9);
|
|
static const Color cColorBodySetIslandIndex = Color::sGetDistinctColor(10);
|
|
static const Color cColorStartNextStep = Color::sGetDistinctColor(11);
|
|
static const Color cColorSolveVelocityConstraints = Color::sGetDistinctColor(12);
|
|
static const Color cColorPreIntegrateVelocity = Color::sGetDistinctColor(13);
|
|
static const Color cColorIntegrateVelocity = Color::sGetDistinctColor(14);
|
|
static const Color cColorPostIntegrateVelocity = Color::sGetDistinctColor(15);
|
|
static const Color cColorResolveCCDContacts = Color::sGetDistinctColor(16);
|
|
static const Color cColorSolvePositionConstraints = Color::sGetDistinctColor(17);
|
|
static const Color cColorFindCCDContacts = Color::sGetDistinctColor(18);
|
|
static const Color cColorStepListeners = Color::sGetDistinctColor(19);
|
|
static const Color cColorSoftBodyPrepare = Color::sGetDistinctColor(20);
|
|
static const Color cColorSoftBodyCollide = Color::sGetDistinctColor(21);
|
|
static const Color cColorSoftBodySimulate = Color::sGetDistinctColor(22);
|
|
static const Color cColorSoftBodyFinalize = Color::sGetDistinctColor(23);
|
|
|
|
PhysicsSystem::~PhysicsSystem()
|
|
{
|
|
// Remove broadphase
|
|
delete mBroadPhase;
|
|
}
|
|
|
|
void PhysicsSystem::Init(uint inMaxBodies, uint inNumBodyMutexes, uint inMaxBodyPairs, uint inMaxContactConstraints, const BroadPhaseLayerInterface &inBroadPhaseLayerInterface, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter)
|
|
{
|
|
// Clamp max bodies
|
|
uint max_bodies = min(inMaxBodies, cMaxBodiesLimit);
|
|
JPH_ASSERT(max_bodies == inMaxBodies, "Cannot support this many bodies!");
|
|
|
|
mObjectVsBroadPhaseLayerFilter = &inObjectVsBroadPhaseLayerFilter;
|
|
mObjectLayerPairFilter = &inObjectLayerPairFilter;
|
|
|
|
// Initialize body manager
|
|
mBodyManager.Init(max_bodies, inNumBodyMutexes, inBroadPhaseLayerInterface);
|
|
|
|
// Create broadphase
|
|
mBroadPhase = new BROAD_PHASE();
|
|
mBroadPhase->Init(&mBodyManager, inBroadPhaseLayerInterface);
|
|
|
|
// Init contact constraint manager
|
|
mContactManager.Init(inMaxBodyPairs, inMaxContactConstraints);
|
|
|
|
// Init islands builder
|
|
mIslandBuilder.Init(max_bodies);
|
|
|
|
// Initialize body interface
|
|
mBodyInterfaceLocking.Init(mBodyLockInterfaceLocking, mBodyManager, *mBroadPhase);
|
|
mBodyInterfaceNoLock.Init(mBodyLockInterfaceNoLock, mBodyManager, *mBroadPhase);
|
|
|
|
// Initialize narrow phase query
|
|
mNarrowPhaseQueryLocking.Init(mBodyLockInterfaceLocking, *mBroadPhase);
|
|
mNarrowPhaseQueryNoLock.Init(mBodyLockInterfaceNoLock, *mBroadPhase);
|
|
}
|
|
|
|
void PhysicsSystem::OptimizeBroadPhase()
|
|
{
|
|
mBroadPhase->Optimize();
|
|
}
|
|
|
|
void PhysicsSystem::AddStepListener(PhysicsStepListener *inListener)
|
|
{
|
|
lock_guard lock(mStepListenersMutex);
|
|
|
|
JPH_ASSERT(std::find(mStepListeners.begin(), mStepListeners.end(), inListener) == mStepListeners.end());
|
|
mStepListeners.push_back(inListener);
|
|
}
|
|
|
|
void PhysicsSystem::RemoveStepListener(PhysicsStepListener *inListener)
|
|
{
|
|
lock_guard lock(mStepListenersMutex);
|
|
|
|
StepListeners::iterator i = std::find(mStepListeners.begin(), mStepListeners.end(), inListener);
|
|
JPH_ASSERT(i != mStepListeners.end());
|
|
*i = mStepListeners.back();
|
|
mStepListeners.pop_back();
|
|
}
|
|
|
|
EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionSteps, TempAllocator *inTempAllocator, JobSystem *inJobSystem)
|
|
{
|
|
JPH_PROFILE_FUNCTION();
|
|
|
|
JPH_DET_LOG("PhysicsSystem::Update: dt: " << inDeltaTime << " steps: " << inCollisionSteps);
|
|
|
|
JPH_ASSERT(inCollisionSteps > 0);
|
|
JPH_ASSERT(inDeltaTime >= 0.0f);
|
|
|
|
// Sync point for the broadphase. This will allow it to do clean up operations without having any mutexes locked yet.
|
|
mBroadPhase->FrameSync();
|
|
|
|
// If there are no active bodies (and no step listener to wake them up) or there's no time delta
|
|
uint32 num_active_rigid_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody);
|
|
uint32 num_active_soft_bodies = mBodyManager.GetNumActiveBodies(EBodyType::SoftBody);
|
|
if ((num_active_rigid_bodies == 0 && num_active_soft_bodies == 0 && mStepListeners.empty()) || inDeltaTime <= 0.0f)
|
|
{
|
|
mBodyManager.LockAllBodies();
|
|
|
|
// Update broadphase
|
|
mBroadPhase->LockModifications();
|
|
BroadPhase::UpdateState update_state = mBroadPhase->UpdatePrepare();
|
|
mBroadPhase->UpdateFinalize(update_state);
|
|
mBroadPhase->UnlockModifications();
|
|
|
|
// If time has passed, call contact removal callbacks from contacts that existed in the previous update
|
|
if (inDeltaTime > 0.0f)
|
|
mContactManager.FinalizeContactCacheAndCallContactPointRemovedCallbacks(0, 0);
|
|
|
|
mBodyManager.UnlockAllBodies();
|
|
return EPhysicsUpdateError::None;
|
|
}
|
|
|
|
// Calculate ratio between current and previous frame delta time to scale initial constraint forces
|
|
float step_delta_time = inDeltaTime / inCollisionSteps;
|
|
float warm_start_impulse_ratio = mPhysicsSettings.mConstraintWarmStart && mPreviousStepDeltaTime > 0.0f? step_delta_time / mPreviousStepDeltaTime : 0.0f;
|
|
mPreviousStepDeltaTime = step_delta_time;
|
|
|
|
// Create the context used for passing information between jobs
|
|
PhysicsUpdateContext context(*inTempAllocator);
|
|
context.mPhysicsSystem = this;
|
|
context.mJobSystem = inJobSystem;
|
|
context.mBarrier = inJobSystem->CreateBarrier();
|
|
context.mIslandBuilder = &mIslandBuilder;
|
|
context.mStepDeltaTime = step_delta_time;
|
|
context.mWarmStartImpulseRatio = warm_start_impulse_ratio;
|
|
context.mSteps.resize(inCollisionSteps);
|
|
|
|
// Allocate space for body pairs
|
|
JPH_ASSERT(context.mBodyPairs == nullptr);
|
|
context.mBodyPairs = static_cast<BodyPair *>(inTempAllocator->Allocate(sizeof(BodyPair) * mPhysicsSettings.mMaxInFlightBodyPairs));
|
|
|
|
// Lock all bodies for write so that we can freely touch them
|
|
mStepListenersMutex.lock();
|
|
mBodyManager.LockAllBodies();
|
|
mBroadPhase->LockModifications();
|
|
|
|
// Get max number of concurrent jobs
|
|
int max_concurrency = context.GetMaxConcurrency();
|
|
|
|
// Calculate how many step listener jobs we spawn
|
|
int num_step_listener_jobs = mStepListeners.empty()? 0 : max(1, min((int)mStepListeners.size() / mPhysicsSettings.mStepListenersBatchSize / mPhysicsSettings.mStepListenerBatchesPerJob, max_concurrency));
|
|
|
|
// Number of gravity jobs depends on the amount of active bodies.
|
|
// Launch max 1 job per batch of active bodies
|
|
// Leave 1 thread for update broadphase prepare and 1 for determine active constraints
|
|
int num_apply_gravity_jobs = max(1, min(((int)num_active_rigid_bodies + cApplyGravityBatchSize - 1) / cApplyGravityBatchSize, max_concurrency - 2));
|
|
|
|
// Number of determine active constraints jobs to run depends on number of constraints.
|
|
// Leave 1 thread for update broadphase prepare and 1 for apply gravity
|
|
int num_determine_active_constraints_jobs = max(1, min(((int)mConstraintManager.GetNumConstraints() + cDetermineActiveConstraintsBatchSize - 1) / cDetermineActiveConstraintsBatchSize, max_concurrency - 2));
|
|
|
|
// Number of setup velocity constraints jobs to run depends on number of constraints.
|
|
int num_setup_velocity_constraints_jobs = max(1, min(((int)mConstraintManager.GetNumConstraints() + cSetupVelocityConstraintsBatchSize - 1) / cSetupVelocityConstraintsBatchSize, max_concurrency));
|
|
|
|
// Number of find collisions jobs to run depends on number of active bodies.
|
|
// Note that when we have more than 1 thread, we always spawn at least 2 find collisions jobs so that the first job can wait for build islands from constraints
|
|
// (which may activate additional bodies that need to be processed) while the second job can start processing collision work.
|
|
int num_find_collisions_jobs = max(max_concurrency == 1? 1 : 2, min(((int)num_active_rigid_bodies + cActiveBodiesBatchSize - 1) / cActiveBodiesBatchSize, max_concurrency));
|
|
|
|
// Number of integrate velocity jobs depends on number of active bodies.
|
|
int num_integrate_velocity_jobs = max(1, min(((int)num_active_rigid_bodies + cIntegrateVelocityBatchSize - 1) / cIntegrateVelocityBatchSize, max_concurrency));
|
|
|
|
{
|
|
JPH_PROFILE("Build Jobs");
|
|
|
|
// Iterate over collision steps
|
|
for (int step_idx = 0; step_idx < inCollisionSteps; ++step_idx)
|
|
{
|
|
bool is_first_step = step_idx == 0;
|
|
bool is_last_step = step_idx == inCollisionSteps - 1;
|
|
|
|
PhysicsUpdateContext::Step &step = context.mSteps[step_idx];
|
|
step.mContext = &context;
|
|
step.mIsFirst = is_first_step;
|
|
step.mIsLast = is_last_step;
|
|
|
|
// Create job to do broadphase finalization
|
|
// This job must finish before integrating velocities. Until then the positions will not be updated neither will bodies be added / removed.
|
|
step.mUpdateBroadphaseFinalize = inJobSystem->CreateJob("UpdateBroadPhaseFinalize", cColorUpdateBroadPhaseFinalize, [&context, &step]()
|
|
{
|
|
// Validate that all find collision jobs have stopped
|
|
JPH_ASSERT(step.mActiveFindCollisionJobs.load(memory_order_relaxed) == 0);
|
|
|
|
// Finalize the broadphase update
|
|
context.mPhysicsSystem->mBroadPhase->UpdateFinalize(step.mBroadPhaseUpdateState);
|
|
|
|
// Signal that it is done
|
|
step.mPreIntegrateVelocity.RemoveDependency();
|
|
}, num_find_collisions_jobs + 2); // depends on: find collisions, broadphase prepare update, finish building jobs
|
|
|
|
// The immediate jobs below are only immediate for the first step, the all finished job will kick them for the next step
|
|
int previous_step_dependency_count = is_first_step? 0 : 1;
|
|
|
|
// Start job immediately: Start the prepare broadphase
|
|
// Must be done under body lock protection since the order is body locks then broadphase mutex
|
|
// If this is turned around the RemoveBody call will hang since it locks in that order
|
|
step.mBroadPhasePrepare = inJobSystem->CreateJob("UpdateBroadPhasePrepare", cColorUpdateBroadPhasePrepare, [&context, &step]()
|
|
{
|
|
// Prepare the broadphase update
|
|
step.mBroadPhaseUpdateState = context.mPhysicsSystem->mBroadPhase->UpdatePrepare();
|
|
|
|
// Now the finalize can run (if other dependencies are met too)
|
|
step.mUpdateBroadphaseFinalize.RemoveDependency();
|
|
}, previous_step_dependency_count);
|
|
|
|
// This job will find all collisions
|
|
step.mBodyPairQueues.resize(max_concurrency);
|
|
step.mMaxBodyPairsPerQueue = mPhysicsSettings.mMaxInFlightBodyPairs / max_concurrency;
|
|
step.mActiveFindCollisionJobs.store(~PhysicsUpdateContext::JobMask(0) >> (sizeof(PhysicsUpdateContext::JobMask) * 8 - num_find_collisions_jobs), memory_order_release);
|
|
step.mFindCollisions.resize(num_find_collisions_jobs);
|
|
for (int i = 0; i < num_find_collisions_jobs; ++i)
|
|
{
|
|
// Build islands from constraints may activate additional bodies, so the first job will wait for this to finish in order to not miss any active bodies
|
|
int num_dep_build_islands_from_constraints = i == 0? 1 : 0;
|
|
step.mFindCollisions[i] = inJobSystem->CreateJob("FindCollisions", cColorFindCollisions, [&step, i]()
|
|
{
|
|
step.mContext->mPhysicsSystem->JobFindCollisions(&step, i);
|
|
}, num_apply_gravity_jobs + num_determine_active_constraints_jobs + 1 + num_dep_build_islands_from_constraints); // depends on: apply gravity, determine active constraints, finish building jobs, build islands from constraints
|
|
}
|
|
|
|
if (is_first_step)
|
|
{
|
|
#ifdef JPH_ENABLE_ASSERTS
|
|
// Don't allow write operations to the active bodies list
|
|
mBodyManager.SetActiveBodiesLocked(true);
|
|
#endif
|
|
|
|
// Store the number of active bodies at the start of the step
|
|
step.mNumActiveBodiesAtStepStart = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody);
|
|
|
|
// Lock all constraints
|
|
mConstraintManager.LockAllConstraints();
|
|
|
|
// Allocate memory for storing the active constraints
|
|
JPH_ASSERT(context.mActiveConstraints == nullptr);
|
|
context.mActiveConstraints = static_cast<Constraint **>(inTempAllocator->Allocate(mConstraintManager.GetNumConstraints() * sizeof(Constraint *)));
|
|
|
|
// Prepare contact buffer
|
|
mContactManager.PrepareConstraintBuffer(&context);
|
|
|
|
// Setup island builder
|
|
mIslandBuilder.PrepareContactConstraints(mContactManager.GetMaxConstraints(), context.mTempAllocator);
|
|
}
|
|
|
|
// This job applies gravity to all active bodies
|
|
step.mApplyGravity.resize(num_apply_gravity_jobs);
|
|
for (int i = 0; i < num_apply_gravity_jobs; ++i)
|
|
step.mApplyGravity[i] = inJobSystem->CreateJob("ApplyGravity", cColorApplyGravity, [&context, &step]()
|
|
{
|
|
context.mPhysicsSystem->JobApplyGravity(&context, &step);
|
|
|
|
JobHandle::sRemoveDependencies(step.mFindCollisions);
|
|
}, num_step_listener_jobs > 0? num_step_listener_jobs : previous_step_dependency_count); // depends on: step listeners (or previous step if no step listeners)
|
|
|
|
// This job will setup velocity constraints for non-collision constraints
|
|
step.mSetupVelocityConstraints.resize(num_setup_velocity_constraints_jobs);
|
|
for (int i = 0; i < num_setup_velocity_constraints_jobs; ++i)
|
|
step.mSetupVelocityConstraints[i] = inJobSystem->CreateJob("SetupVelocityConstraints", cColorSetupVelocityConstraints, [&context, &step]()
|
|
{
|
|
context.mPhysicsSystem->JobSetupVelocityConstraints(context.mStepDeltaTime, &step);
|
|
|
|
JobHandle::sRemoveDependencies(step.mSolveVelocityConstraints);
|
|
}, num_determine_active_constraints_jobs + 1); // depends on: determine active constraints, finish building jobs
|
|
|
|
// This job will build islands from constraints
|
|
step.mBuildIslandsFromConstraints = inJobSystem->CreateJob("BuildIslandsFromConstraints", cColorBuildIslandsFromConstraints, [&context, &step]()
|
|
{
|
|
context.mPhysicsSystem->JobBuildIslandsFromConstraints(&context, &step);
|
|
|
|
step.mFindCollisions[0].RemoveDependency(); // The first collisions job cannot start running until we've finished building islands and activated all bodies
|
|
step.mFinalizeIslands.RemoveDependency();
|
|
}, num_determine_active_constraints_jobs + 1); // depends on: determine active constraints, finish building jobs
|
|
|
|
// This job determines active constraints
|
|
step.mDetermineActiveConstraints.resize(num_determine_active_constraints_jobs);
|
|
for (int i = 0; i < num_determine_active_constraints_jobs; ++i)
|
|
step.mDetermineActiveConstraints[i] = inJobSystem->CreateJob("DetermineActiveConstraints", cColorDetermineActiveConstraints, [&context, &step]()
|
|
{
|
|
context.mPhysicsSystem->JobDetermineActiveConstraints(&step);
|
|
|
|
step.mBuildIslandsFromConstraints.RemoveDependency();
|
|
|
|
// Kick these jobs last as they will use up all CPU cores leaving no space for the previous job, we prefer setup velocity constraints to finish first so we kick it first
|
|
JobHandle::sRemoveDependencies(step.mSetupVelocityConstraints);
|
|
JobHandle::sRemoveDependencies(step.mFindCollisions);
|
|
}, num_step_listener_jobs > 0? num_step_listener_jobs : previous_step_dependency_count); // depends on: step listeners (or previous step if no step listeners)
|
|
|
|
// This job calls the step listeners
|
|
step.mStepListeners.resize(num_step_listener_jobs);
|
|
for (int i = 0; i < num_step_listener_jobs; ++i)
|
|
step.mStepListeners[i] = inJobSystem->CreateJob("StepListeners", cColorStepListeners, [&context, &step]()
|
|
{
|
|
// Call the step listeners
|
|
context.mPhysicsSystem->JobStepListeners(&step);
|
|
|
|
// Kick apply gravity and determine active constraint jobs
|
|
JobHandle::sRemoveDependencies(step.mApplyGravity);
|
|
JobHandle::sRemoveDependencies(step.mDetermineActiveConstraints);
|
|
}, previous_step_dependency_count);
|
|
|
|
// Unblock the previous step
|
|
if (!is_first_step)
|
|
context.mSteps[step_idx - 1].mStartNextStep.RemoveDependency();
|
|
|
|
// This job will finalize the simulation islands
|
|
step.mFinalizeIslands = inJobSystem->CreateJob("FinalizeIslands", cColorFinalizeIslands, [&context, &step]()
|
|
{
|
|
// Validate that all find collision jobs have stopped
|
|
JPH_ASSERT(step.mActiveFindCollisionJobs.load(memory_order_relaxed) == 0);
|
|
|
|
context.mPhysicsSystem->JobFinalizeIslands(&context);
|
|
|
|
JobHandle::sRemoveDependencies(step.mSolveVelocityConstraints);
|
|
step.mBodySetIslandIndex.RemoveDependency();
|
|
}, num_find_collisions_jobs + 2); // depends on: find collisions, build islands from constraints, finish building jobs
|
|
|
|
// Unblock previous job
|
|
// Note: technically we could release find collisions here but we don't want to because that could make them run before 'setup velocity constraints' which means that job won't have a thread left
|
|
step.mBuildIslandsFromConstraints.RemoveDependency();
|
|
|
|
// This job will call the contact removed callbacks
|
|
step.mContactRemovedCallbacks = inJobSystem->CreateJob("ContactRemovedCallbacks", cColorContactRemovedCallbacks, [&context, &step]()
|
|
{
|
|
context.mPhysicsSystem->JobContactRemovedCallbacks(&step);
|
|
|
|
if (step.mStartNextStep.IsValid())
|
|
step.mStartNextStep.RemoveDependency();
|
|
}, 1); // depends on the find ccd contacts
|
|
|
|
// This job will set the island index on each body (only used for debug drawing purposes)
|
|
// It will also delete any bodies that have been destroyed in the last frame
|
|
step.mBodySetIslandIndex = inJobSystem->CreateJob("BodySetIslandIndex", cColorBodySetIslandIndex, [&context, &step]()
|
|
{
|
|
context.mPhysicsSystem->JobBodySetIslandIndex();
|
|
|
|
JobHandle::sRemoveDependencies(step.mSolvePositionConstraints);
|
|
}, 2); // depends on: finalize islands, finish building jobs
|
|
|
|
// Job to start the next collision step
|
|
if (!is_last_step)
|
|
{
|
|
PhysicsUpdateContext::Step *next_step = &context.mSteps[step_idx + 1];
|
|
step.mStartNextStep = inJobSystem->CreateJob("StartNextStep", cColorStartNextStep, [this, next_step]()
|
|
{
|
|
#ifdef JPH_DEBUG
|
|
// Validate that the cached bounds are correct
|
|
mBodyManager.ValidateActiveBodyBounds();
|
|
#endif // JPH_DEBUG
|
|
|
|
// Store the number of active bodies at the start of the step
|
|
next_step->mNumActiveBodiesAtStepStart = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody);
|
|
|
|
// Clear the large island splitter
|
|
TempAllocator *temp_allocator = next_step->mContext->mTempAllocator;
|
|
mLargeIslandSplitter.Reset(temp_allocator);
|
|
|
|
// Clear the island builder
|
|
mIslandBuilder.ResetIslands(temp_allocator);
|
|
|
|
// Setup island builder
|
|
mIslandBuilder.PrepareContactConstraints(mContactManager.GetMaxConstraints(), temp_allocator);
|
|
|
|
// Restart the contact manager
|
|
mContactManager.RecycleConstraintBuffer();
|
|
|
|
// Kick the jobs of the next step (in the same order as the first step)
|
|
next_step->mBroadPhasePrepare.RemoveDependency();
|
|
if (next_step->mStepListeners.empty())
|
|
{
|
|
// Kick the gravity and active constraints jobs immediately
|
|
JobHandle::sRemoveDependencies(next_step->mApplyGravity);
|
|
JobHandle::sRemoveDependencies(next_step->mDetermineActiveConstraints);
|
|
}
|
|
else
|
|
{
|
|
// Kick the step listeners job first
|
|
JobHandle::sRemoveDependencies(next_step->mStepListeners);
|
|
}
|
|
}, 3); // depends on: update soft bodies, contact removed callbacks, finish building the previous step
|
|
}
|
|
|
|
// This job will solve the velocity constraints
|
|
step.mSolveVelocityConstraints.resize(max_concurrency);
|
|
for (int i = 0; i < max_concurrency; ++i)
|
|
step.mSolveVelocityConstraints[i] = inJobSystem->CreateJob("SolveVelocityConstraints", cColorSolveVelocityConstraints, [&context, &step]()
|
|
{
|
|
context.mPhysicsSystem->JobSolveVelocityConstraints(&context, &step);
|
|
|
|
step.mPreIntegrateVelocity.RemoveDependency();
|
|
}, num_setup_velocity_constraints_jobs + 2); // depends on: finalize islands, setup velocity constraints, finish building jobs.
|
|
|
|
// We prefer setup velocity constraints to finish first so we kick it first
|
|
JobHandle::sRemoveDependencies(step.mSetupVelocityConstraints);
|
|
JobHandle::sRemoveDependencies(step.mFindCollisions);
|
|
|
|
// Finalize islands is a dependency on find collisions so it can go last
|
|
step.mFinalizeIslands.RemoveDependency();
|
|
|
|
// This job will prepare the position update of all active bodies
|
|
step.mPreIntegrateVelocity = inJobSystem->CreateJob("PreIntegrateVelocity", cColorPreIntegrateVelocity, [&context, &step]()
|
|
{
|
|
context.mPhysicsSystem->JobPreIntegrateVelocity(&context, &step);
|
|
|
|
JobHandle::sRemoveDependencies(step.mIntegrateVelocity);
|
|
}, 2 + max_concurrency); // depends on: broadphase update finalize, solve velocity constraints, finish building jobs.
|
|
|
|
// Unblock previous jobs
|
|
step.mUpdateBroadphaseFinalize.RemoveDependency();
|
|
JobHandle::sRemoveDependencies(step.mSolveVelocityConstraints);
|
|
|
|
// This job will update the positions of all active bodies
|
|
step.mIntegrateVelocity.resize(num_integrate_velocity_jobs);
|
|
for (int i = 0; i < num_integrate_velocity_jobs; ++i)
|
|
step.mIntegrateVelocity[i] = inJobSystem->CreateJob("IntegrateVelocity", cColorIntegrateVelocity, [&context, &step]()
|
|
{
|
|
context.mPhysicsSystem->JobIntegrateVelocity(&context, &step);
|
|
|
|
step.mPostIntegrateVelocity.RemoveDependency();
|
|
}, 2); // depends on: pre integrate velocity, finish building jobs.
|
|
|
|
// Unblock previous job
|
|
step.mPreIntegrateVelocity.RemoveDependency();
|
|
|
|
// This job will finish the position update of all active bodies
|
|
step.mPostIntegrateVelocity = inJobSystem->CreateJob("PostIntegrateVelocity", cColorPostIntegrateVelocity, [&context, &step]()
|
|
{
|
|
context.mPhysicsSystem->JobPostIntegrateVelocity(&context, &step);
|
|
|
|
step.mResolveCCDContacts.RemoveDependency();
|
|
}, num_integrate_velocity_jobs + 1); // depends on: integrate velocity, finish building jobs
|
|
|
|
// Unblock previous jobs
|
|
JobHandle::sRemoveDependencies(step.mIntegrateVelocity);
|
|
|
|
// This job will update the positions and velocities for all bodies that need continuous collision detection
|
|
step.mResolveCCDContacts = inJobSystem->CreateJob("ResolveCCDContacts", cColorResolveCCDContacts, [&context, &step]()
|
|
{
|
|
context.mPhysicsSystem->JobResolveCCDContacts(&context, &step);
|
|
|
|
JobHandle::sRemoveDependencies(step.mSolvePositionConstraints);
|
|
}, 2); // depends on: integrate velocities, detect ccd contacts (added dynamically), finish building jobs.
|
|
|
|
// Unblock previous job
|
|
step.mPostIntegrateVelocity.RemoveDependency();
|
|
|
|
// Fixes up drift in positions and updates the broadphase with new body positions
|
|
step.mSolvePositionConstraints.resize(max_concurrency);
|
|
for (int i = 0; i < max_concurrency; ++i)
|
|
step.mSolvePositionConstraints[i] = inJobSystem->CreateJob("SolvePositionConstraints", cColorSolvePositionConstraints, [&context, &step]()
|
|
{
|
|
context.mPhysicsSystem->JobSolvePositionConstraints(&context, &step);
|
|
|
|
// Kick the next step
|
|
if (step.mSoftBodyPrepare.IsValid())
|
|
step.mSoftBodyPrepare.RemoveDependency();
|
|
}, 3); // depends on: resolve ccd contacts, body set island index, finish building jobs.
|
|
|
|
// Unblock previous jobs.
|
|
step.mResolveCCDContacts.RemoveDependency();
|
|
step.mBodySetIslandIndex.RemoveDependency();
|
|
|
|
// The soft body prepare job will create other jobs if needed
|
|
step.mSoftBodyPrepare = inJobSystem->CreateJob("SoftBodyPrepare", cColorSoftBodyPrepare, [&context, &step]()
|
|
{
|
|
context.mPhysicsSystem->JobSoftBodyPrepare(&context, &step);
|
|
}, max_concurrency); // depends on: solve position constraints.
|
|
|
|
// Unblock previous jobs
|
|
JobHandle::sRemoveDependencies(step.mSolvePositionConstraints);
|
|
}
|
|
}
|
|
|
|
// Build the list of jobs to wait for
|
|
JobSystem::Barrier *barrier = context.mBarrier;
|
|
{
|
|
JPH_PROFILE("Build job barrier");
|
|
|
|
StaticArray<JobHandle, cMaxPhysicsJobs> handles;
|
|
for (const PhysicsUpdateContext::Step &step : context.mSteps)
|
|
{
|
|
if (step.mBroadPhasePrepare.IsValid())
|
|
handles.push_back(step.mBroadPhasePrepare);
|
|
for (const JobHandle &h : step.mStepListeners)
|
|
handles.push_back(h);
|
|
for (const JobHandle &h : step.mDetermineActiveConstraints)
|
|
handles.push_back(h);
|
|
for (const JobHandle &h : step.mApplyGravity)
|
|
handles.push_back(h);
|
|
for (const JobHandle &h : step.mFindCollisions)
|
|
handles.push_back(h);
|
|
if (step.mUpdateBroadphaseFinalize.IsValid())
|
|
handles.push_back(step.mUpdateBroadphaseFinalize);
|
|
for (const JobHandle &h : step.mSetupVelocityConstraints)
|
|
handles.push_back(h);
|
|
handles.push_back(step.mBuildIslandsFromConstraints);
|
|
handles.push_back(step.mFinalizeIslands);
|
|
handles.push_back(step.mBodySetIslandIndex);
|
|
for (const JobHandle &h : step.mSolveVelocityConstraints)
|
|
handles.push_back(h);
|
|
handles.push_back(step.mPreIntegrateVelocity);
|
|
for (const JobHandle &h : step.mIntegrateVelocity)
|
|
handles.push_back(h);
|
|
handles.push_back(step.mPostIntegrateVelocity);
|
|
handles.push_back(step.mResolveCCDContacts);
|
|
for (const JobHandle &h : step.mSolvePositionConstraints)
|
|
handles.push_back(h);
|
|
handles.push_back(step.mContactRemovedCallbacks);
|
|
if (step.mSoftBodyPrepare.IsValid())
|
|
handles.push_back(step.mSoftBodyPrepare);
|
|
if (step.mStartNextStep.IsValid())
|
|
handles.push_back(step.mStartNextStep);
|
|
}
|
|
barrier->AddJobs(handles.data(), handles.size());
|
|
}
|
|
|
|
// Wait until all jobs finish
|
|
// Note we don't just wait for the last job. If we would and another job
|
|
// would be scheduled in between there is the possibility of a deadlock.
|
|
// The other job could try to e.g. add/remove a body which would try to
|
|
// lock a body mutex while this thread has already locked the mutex
|
|
inJobSystem->WaitForJobs(barrier);
|
|
|
|
// We're done with the barrier for this update
|
|
inJobSystem->DestroyBarrier(barrier);
|
|
|
|
#ifdef JPH_DEBUG
|
|
// Validate that the cached bounds are correct
|
|
mBodyManager.ValidateActiveBodyBounds();
|
|
#endif // JPH_DEBUG
|
|
|
|
// Clear the large island splitter
|
|
mLargeIslandSplitter.Reset(inTempAllocator);
|
|
|
|
// Clear the island builder
|
|
mIslandBuilder.ResetIslands(inTempAllocator);
|
|
|
|
// Clear the contact manager
|
|
mContactManager.FinishConstraintBuffer();
|
|
|
|
// Free active constraints
|
|
inTempAllocator->Free(context.mActiveConstraints, mConstraintManager.GetNumConstraints() * sizeof(Constraint *));
|
|
context.mActiveConstraints = nullptr;
|
|
|
|
// Free body pairs
|
|
inTempAllocator->Free(context.mBodyPairs, sizeof(BodyPair) * mPhysicsSettings.mMaxInFlightBodyPairs);
|
|
context.mBodyPairs = nullptr;
|
|
|
|
// Unlock the broadphase
|
|
mBroadPhase->UnlockModifications();
|
|
|
|
// Unlock all constraints
|
|
mConstraintManager.UnlockAllConstraints();
|
|
|
|
#ifdef JPH_ENABLE_ASSERTS
|
|
// Allow write operations to the active bodies list
|
|
mBodyManager.SetActiveBodiesLocked(false);
|
|
#endif
|
|
|
|
// Unlock all bodies
|
|
mBodyManager.UnlockAllBodies();
|
|
|
|
// Unlock step listeners
|
|
mStepListenersMutex.unlock();
|
|
|
|
// Return any errors
|
|
EPhysicsUpdateError errors = static_cast<EPhysicsUpdateError>(context.mErrors.load(memory_order_acquire));
|
|
JPH_ASSERT(errors == EPhysicsUpdateError::None, "An error occurred during the physics update, see EPhysicsUpdateError for more information");
|
|
return errors;
|
|
}
|
|
|
|
void PhysicsSystem::JobStepListeners(PhysicsUpdateContext::Step *ioStep)
|
|
{
|
|
#ifdef JPH_ENABLE_ASSERTS
|
|
// Read positions (broadphase updates concurrently so we can't write), read/write velocities
|
|
BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::Read);
|
|
|
|
// Can activate bodies only (we cache the amount of active bodies at the beginning of the step in mNumActiveBodiesAtStepStart so we cannot deactivate here)
|
|
BodyManager::GrantActiveBodiesAccess grant_active(true, false);
|
|
#endif
|
|
|
|
PhysicsStepListenerContext context;
|
|
context.mDeltaTime = ioStep->mContext->mStepDeltaTime;
|
|
context.mIsFirstStep = ioStep->mIsFirst;
|
|
context.mIsLastStep = ioStep->mIsLast;
|
|
context.mPhysicsSystem = this;
|
|
|
|
uint32 batch_size = mPhysicsSettings.mStepListenersBatchSize;
|
|
for (;;)
|
|
{
|
|
// Get the start of a new batch
|
|
uint32 batch = ioStep->mStepListenerReadIdx.fetch_add(batch_size);
|
|
if (batch >= mStepListeners.size())
|
|
break;
|
|
|
|
// Call the listeners
|
|
for (uint32 i = batch, i_end = min((uint32)mStepListeners.size(), batch + batch_size); i < i_end; ++i)
|
|
mStepListeners[i]->OnStep(context);
|
|
}
|
|
}
|
|
|
|
void PhysicsSystem::JobDetermineActiveConstraints(PhysicsUpdateContext::Step *ioStep) const
|
|
{
|
|
#ifdef JPH_ENABLE_ASSERTS
|
|
// No body access
|
|
BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::None);
|
|
#endif
|
|
|
|
uint32 num_constraints = mConstraintManager.GetNumConstraints();
|
|
uint32 num_active_constraints;
|
|
Constraint **active_constraints = (Constraint **)JPH_STACK_ALLOC(cDetermineActiveConstraintsBatchSize * sizeof(Constraint *));
|
|
|
|
for (;;)
|
|
{
|
|
// Atomically fetch a batch of constraints
|
|
uint32 constraint_idx = ioStep->mDetermineActiveConstraintReadIdx.fetch_add(cDetermineActiveConstraintsBatchSize);
|
|
if (constraint_idx >= num_constraints)
|
|
break;
|
|
|
|
// Calculate the end of the batch
|
|
uint32 constraint_idx_end = min(num_constraints, constraint_idx + cDetermineActiveConstraintsBatchSize);
|
|
|
|
// Store the active constraints at the start of the step (bodies get activated during the step which in turn may activate constraints leading to an inconsistent shapshot)
|
|
mConstraintManager.GetActiveConstraints(constraint_idx, constraint_idx_end, active_constraints, num_active_constraints);
|
|
|
|
// Copy the block of active constraints to the global list of active constraints
|
|
if (num_active_constraints > 0)
|
|
{
|
|
uint32 active_constraint_idx = ioStep->mNumActiveConstraints.fetch_add(num_active_constraints);
|
|
memcpy(ioStep->mContext->mActiveConstraints + active_constraint_idx, active_constraints, num_active_constraints * sizeof(Constraint *));
|
|
}
|
|
}
|
|
}
|
|
|
|
void PhysicsSystem::JobApplyGravity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep)
|
|
{
|
|
#ifdef JPH_ENABLE_ASSERTS
|
|
// We update velocities and need the rotation to do so
|
|
BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::Read);
|
|
#endif
|
|
|
|
// Get list of active bodies that we had at the start of the physics update.
|
|
// Any body that is activated as part of the simulation step does not receive gravity this frame.
|
|
// Note that bodies may be activated during this job but not deactivated, this means that only elements
|
|
// will be added to the array. Since the array is made to not reallocate, this is a safe operation.
|
|
const BodyID *active_bodies = mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody);
|
|
uint32 num_active_bodies_at_step_start = ioStep->mNumActiveBodiesAtStepStart;
|
|
|
|
// Fetch delta time once outside the loop
|
|
float delta_time = ioContext->mStepDeltaTime;
|
|
|
|
// Update velocities from forces
|
|
for (;;)
|
|
{
|
|
// Atomically fetch a batch of bodies
|
|
uint32 active_body_idx = ioStep->mApplyGravityReadIdx.fetch_add(cApplyGravityBatchSize);
|
|
if (active_body_idx >= num_active_bodies_at_step_start)
|
|
break;
|
|
|
|
// Calculate the end of the batch
|
|
uint32 active_body_idx_end = min(num_active_bodies_at_step_start, active_body_idx + cApplyGravityBatchSize);
|
|
|
|
// Process the batch
|
|
while (active_body_idx < active_body_idx_end)
|
|
{
|
|
Body &body = mBodyManager.GetBody(active_bodies[active_body_idx]);
|
|
if (body.IsDynamic())
|
|
{
|
|
MotionProperties *mp = body.GetMotionProperties();
|
|
Quat rotation = body.GetRotation();
|
|
|
|
if (body.GetApplyGyroscopicForce())
|
|
mp->ApplyGyroscopicForceInternal(rotation, delta_time);
|
|
|
|
mp->ApplyForceTorqueAndDragInternal(rotation, mGravity, delta_time);
|
|
}
|
|
active_body_idx++;
|
|
}
|
|
}
|
|
}
|
|
|
|
void PhysicsSystem::JobSetupVelocityConstraints(float inDeltaTime, PhysicsUpdateContext::Step *ioStep) const
|
|
{
|
|
#ifdef JPH_ENABLE_ASSERTS
|
|
// We only read positions
|
|
BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::Read);
|
|
#endif
|
|
|
|
uint32 num_constraints = ioStep->mNumActiveConstraints;
|
|
|
|
for (;;)
|
|
{
|
|
// Atomically fetch a batch of constraints
|
|
uint32 constraint_idx = ioStep->mSetupVelocityConstraintsReadIdx.fetch_add(cSetupVelocityConstraintsBatchSize);
|
|
if (constraint_idx >= num_constraints)
|
|
break;
|
|
|
|
ConstraintManager::sSetupVelocityConstraints(ioStep->mContext->mActiveConstraints + constraint_idx, min<uint32>(cSetupVelocityConstraintsBatchSize, num_constraints - constraint_idx), inDeltaTime);
|
|
}
|
|
}
|
|
|
|
void PhysicsSystem::JobBuildIslandsFromConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep)
|
|
{
|
|
#ifdef JPH_ENABLE_ASSERTS
|
|
// We read constraints and positions
|
|
BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::Read);
|
|
|
|
// Can only activate bodies
|
|
BodyManager::GrantActiveBodiesAccess grant_active(true, false);
|
|
#endif
|
|
|
|
// Prepare the island builder
|
|
mIslandBuilder.PrepareNonContactConstraints(ioStep->mNumActiveConstraints, ioContext->mTempAllocator);
|
|
|
|
// Build the islands
|
|
ConstraintManager::sBuildIslands(ioStep->mContext->mActiveConstraints, ioStep->mNumActiveConstraints, mIslandBuilder, mBodyManager);
|
|
}
|
|
|
|
void PhysicsSystem::TrySpawnJobFindCollisions(PhysicsUpdateContext::Step *ioStep) const
|
|
{
|
|
// Get how many jobs we can spawn and check if we can spawn more
|
|
uint max_jobs = ioStep->mBodyPairQueues.size();
|
|
if (CountBits(ioStep->mActiveFindCollisionJobs.load(memory_order_relaxed)) >= max_jobs)
|
|
return;
|
|
|
|
// Count how many body pairs we have waiting
|
|
uint32 num_body_pairs = 0;
|
|
for (const PhysicsUpdateContext::BodyPairQueue &queue : ioStep->mBodyPairQueues)
|
|
num_body_pairs += queue.mWriteIdx - queue.mReadIdx;
|
|
|
|
// Count how many active bodies we have waiting
|
|
uint32 num_active_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody) - ioStep->mActiveBodyReadIdx;
|
|
|
|
// Calculate how many jobs we would like
|
|
uint desired_num_jobs = min((num_body_pairs + cNarrowPhaseBatchSize - 1) / cNarrowPhaseBatchSize + (num_active_bodies + cActiveBodiesBatchSize - 1) / cActiveBodiesBatchSize, max_jobs);
|
|
|
|
for (;;)
|
|
{
|
|
// Get the bit mask of active jobs and see if we can spawn more
|
|
PhysicsUpdateContext::JobMask current_active_jobs = ioStep->mActiveFindCollisionJobs.load(memory_order_relaxed);
|
|
uint job_index = CountTrailingZeros(~current_active_jobs);
|
|
if (job_index >= desired_num_jobs)
|
|
break;
|
|
|
|
// Try to claim the job index
|
|
PhysicsUpdateContext::JobMask job_mask = PhysicsUpdateContext::JobMask(1) << job_index;
|
|
PhysicsUpdateContext::JobMask prev_value = ioStep->mActiveFindCollisionJobs.fetch_or(job_mask, memory_order_acquire);
|
|
if ((prev_value & job_mask) == 0)
|
|
{
|
|
// Add dependencies from the find collisions job to the next jobs
|
|
ioStep->mUpdateBroadphaseFinalize.AddDependency();
|
|
ioStep->mFinalizeIslands.AddDependency();
|
|
|
|
// Start the job
|
|
JobHandle job = ioStep->mContext->mJobSystem->CreateJob("FindCollisions", cColorFindCollisions, [step = ioStep, job_index]()
|
|
{
|
|
step->mContext->mPhysicsSystem->JobFindCollisions(step, job_index);
|
|
});
|
|
|
|
// Add the job to the job barrier so the main updating thread can execute the job too
|
|
ioStep->mContext->mBarrier->AddJob(job);
|
|
|
|
// Spawn only 1 extra job at a time
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
static void sFinalizeContactAllocator(PhysicsUpdateContext::Step &ioStep, const ContactConstraintManager::ContactAllocator &inAllocator)
|
|
{
|
|
// Atomically accumulate the number of found manifolds and body pairs
|
|
ioStep.mNumBodyPairs.fetch_add(inAllocator.mNumBodyPairs, memory_order_relaxed);
|
|
ioStep.mNumManifolds.fetch_add(inAllocator.mNumManifolds, memory_order_relaxed);
|
|
|
|
// Combine update errors
|
|
ioStep.mContext->mErrors.fetch_or((uint32)inAllocator.mErrors, memory_order_relaxed);
|
|
}
|
|
|
|
// Disable TSAN for this function. It detects a false positive race condition on mBodyPairs.
|
|
// We have written mBodyPairs before doing mWriteIdx++ and we check mWriteIdx before reading mBodyPairs, so this should be safe.
|
|
JPH_TSAN_NO_SANITIZE
|
|
void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int inJobIndex)
|
|
{
|
|
#ifdef JPH_ENABLE_ASSERTS
|
|
// We read positions and read velocities (for elastic collisions)
|
|
BodyAccess::Grant grant(BodyAccess::EAccess::Read, BodyAccess::EAccess::Read);
|
|
|
|
// Can only activate bodies
|
|
BodyManager::GrantActiveBodiesAccess grant_active(true, false);
|
|
#endif
|
|
|
|
// Allocation context for allocating new contact points
|
|
ContactAllocator contact_allocator(mContactManager.GetContactAllocator());
|
|
|
|
// Determine initial queue to read pairs from if no broadphase work can be done
|
|
// (always start looking at results from the next job)
|
|
int read_queue_idx = (inJobIndex + 1) % ioStep->mBodyPairQueues.size();
|
|
|
|
// Allocate space to temporarily store a batch of active bodies
|
|
BodyID *active_bodies = (BodyID *)JPH_STACK_ALLOC(cActiveBodiesBatchSize * sizeof(BodyID));
|
|
|
|
for (;;)
|
|
{
|
|
// Check if there are active bodies to be processed
|
|
uint32 active_bodies_read_idx = ioStep->mActiveBodyReadIdx;
|
|
uint32 num_active_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody);
|
|
if (active_bodies_read_idx < num_active_bodies)
|
|
{
|
|
// Take a batch of active bodies
|
|
uint32 active_bodies_read_idx_end = min(num_active_bodies, active_bodies_read_idx + cActiveBodiesBatchSize);
|
|
if (ioStep->mActiveBodyReadIdx.compare_exchange_strong(active_bodies_read_idx, active_bodies_read_idx_end))
|
|
{
|
|
// Callback when a new body pair is found
|
|
class MyBodyPairCallback : public BodyPairCollector
|
|
{
|
|
public:
|
|
// Constructor
|
|
MyBodyPairCallback(PhysicsUpdateContext::Step *inStep, ContactAllocator &ioContactAllocator, int inJobIndex) :
|
|
mStep(inStep),
|
|
mContactAllocator(ioContactAllocator),
|
|
mJobIndex(inJobIndex)
|
|
{
|
|
}
|
|
|
|
// Callback function when a body pair is found
|
|
virtual void AddHit(const BodyPair &inPair) override
|
|
{
|
|
// Check if we have space in our write queue
|
|
PhysicsUpdateContext::BodyPairQueue &queue = mStep->mBodyPairQueues[mJobIndex];
|
|
uint32 body_pairs_in_queue = queue.mWriteIdx - queue.mReadIdx;
|
|
if (body_pairs_in_queue >= mStep->mMaxBodyPairsPerQueue)
|
|
{
|
|
// Buffer full, process the pair now
|
|
mStep->mContext->mPhysicsSystem->ProcessBodyPair(mContactAllocator, inPair);
|
|
}
|
|
else
|
|
{
|
|
// Store the pair in our own queue
|
|
mStep->mContext->mBodyPairs[mJobIndex * mStep->mMaxBodyPairsPerQueue + queue.mWriteIdx % mStep->mMaxBodyPairsPerQueue] = inPair;
|
|
++queue.mWriteIdx;
|
|
}
|
|
}
|
|
|
|
private:
|
|
PhysicsUpdateContext::Step * mStep;
|
|
ContactAllocator & mContactAllocator;
|
|
int mJobIndex;
|
|
};
|
|
MyBodyPairCallback add_pair(ioStep, contact_allocator, inJobIndex);
|
|
|
|
// Copy active bodies to temporary array, broadphase will reorder them
|
|
uint32 batch_size = active_bodies_read_idx_end - active_bodies_read_idx;
|
|
memcpy(active_bodies, mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody) + active_bodies_read_idx, batch_size * sizeof(BodyID));
|
|
|
|
// Find pairs in the broadphase
|
|
mBroadPhase->FindCollidingPairs(active_bodies, batch_size, mPhysicsSettings.mSpeculativeContactDistance, *mObjectVsBroadPhaseLayerFilter, *mObjectLayerPairFilter, add_pair);
|
|
|
|
// Check if we have enough pairs in the buffer to start a new job
|
|
const PhysicsUpdateContext::BodyPairQueue &queue = ioStep->mBodyPairQueues[inJobIndex];
|
|
uint32 body_pairs_in_queue = queue.mWriteIdx - queue.mReadIdx;
|
|
if (body_pairs_in_queue >= cNarrowPhaseBatchSize)
|
|
TrySpawnJobFindCollisions(ioStep);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Lockless loop to get the next body pair from the pairs buffer
|
|
const PhysicsUpdateContext *context = ioStep->mContext;
|
|
int first_read_queue_idx = read_queue_idx;
|
|
for (;;)
|
|
{
|
|
PhysicsUpdateContext::BodyPairQueue &queue = ioStep->mBodyPairQueues[read_queue_idx];
|
|
|
|
// Get the next pair to process
|
|
uint32 pair_idx = queue.mReadIdx;
|
|
|
|
// If the pair hasn't been written yet
|
|
if (pair_idx >= queue.mWriteIdx)
|
|
{
|
|
// Go to the next queue
|
|
read_queue_idx = (read_queue_idx + 1) % ioStep->mBodyPairQueues.size();
|
|
|
|
// If we're back at the first queue, we've looked at all of them and found nothing
|
|
if (read_queue_idx == first_read_queue_idx)
|
|
{
|
|
// Collect information from the contact allocator and accumulate it in the step.
|
|
sFinalizeContactAllocator(*ioStep, contact_allocator);
|
|
|
|
// Mark this job as inactive
|
|
ioStep->mActiveFindCollisionJobs.fetch_and(~PhysicsUpdateContext::JobMask(1 << inJobIndex), memory_order_release);
|
|
|
|
// Trigger the next jobs
|
|
ioStep->mUpdateBroadphaseFinalize.RemoveDependency();
|
|
ioStep->mFinalizeIslands.RemoveDependency();
|
|
return;
|
|
}
|
|
|
|
// Try again reading from the next queue
|
|
continue;
|
|
}
|
|
|
|
// Copy the body pair out of the buffer
|
|
const BodyPair bp = context->mBodyPairs[read_queue_idx * ioStep->mMaxBodyPairsPerQueue + pair_idx % ioStep->mMaxBodyPairsPerQueue];
|
|
|
|
// Mark this pair as taken
|
|
if (queue.mReadIdx.compare_exchange_strong(pair_idx, pair_idx + 1))
|
|
{
|
|
// Process the actual body pair
|
|
ProcessBodyPair(contact_allocator, bp);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void PhysicsSystem::sDefaultSimCollideBodyVsBody(const Body &inBody1, const Body &inBody2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, CollideShapeSettings &ioCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
|
|
{
|
|
SubShapeIDCreator part1, part2;
|
|
|
|
if (inBody1.GetEnhancedInternalEdgeRemovalWithBody(inBody2))
|
|
{
|
|
// Collide with enhanced internal edge removal
|
|
ioCollideShapeSettings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll;
|
|
InternalEdgeRemovingCollector::sCollideShapeVsShape(inBody1.GetShape(), inBody2.GetShape(), Vec3::sOne(), Vec3::sOne(), inCenterOfMassTransform1, inCenterOfMassTransform2, part1, part2, ioCollideShapeSettings, ioCollector, inShapeFilter);
|
|
}
|
|
else
|
|
{
|
|
// Regular collide
|
|
CollisionDispatch::sCollideShapeVsShape(inBody1.GetShape(), inBody2.GetShape(), Vec3::sOne(), Vec3::sOne(), inCenterOfMassTransform1, inCenterOfMassTransform2, part1, part2, ioCollideShapeSettings, ioCollector, inShapeFilter);
|
|
}
|
|
}
|
|
|
|
void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const BodyPair &inBodyPair)
|
|
{
|
|
JPH_PROFILE_FUNCTION();
|
|
|
|
// Fetch body pair
|
|
Body *body1 = &mBodyManager.GetBody(inBodyPair.mBodyA);
|
|
Body *body2 = &mBodyManager.GetBody(inBodyPair.mBodyB);
|
|
JPH_ASSERT(body1->IsActive());
|
|
|
|
JPH_DET_LOG("ProcessBodyPair: id1: " << inBodyPair.mBodyA << " id2: " << inBodyPair.mBodyB << " p1: " << body1->GetCenterOfMassPosition() << " p2: " << body2->GetCenterOfMassPosition() << " r1: " << body1->GetRotation() << " r2: " << body2->GetRotation());
|
|
|
|
// Check for soft bodies
|
|
if (body2->IsSoftBody())
|
|
{
|
|
// If the 2nd body is a soft body and not active, we activate it now
|
|
if (!body2->IsActive())
|
|
mBodyManager.ActivateBodies(&inBodyPair.mBodyB, 1);
|
|
|
|
// Soft body processing is done later in the pipeline
|
|
return;
|
|
}
|
|
|
|
// Ensure that body1 has the higher motion type (i.e. dynamic trumps kinematic), this ensures that we do the collision detection in the space of a moving body,
|
|
// which avoids accuracy problems when testing a very large static object against a small dynamic object
|
|
// Ensure that body1 id < body2 id when motion types are the same.
|
|
if (body1->GetMotionType() < body2->GetMotionType()
|
|
|| (body1->GetMotionType() == body2->GetMotionType() && inBodyPair.mBodyB < inBodyPair.mBodyA))
|
|
std::swap(body1, body2);
|
|
|
|
// Check if the contact points from the previous frame are reusable and if so copy them
|
|
bool pair_handled = false, constraint_created = false;
|
|
if (mPhysicsSettings.mUseBodyPairContactCache && !(body1->IsCollisionCacheInvalid() || body2->IsCollisionCacheInvalid()))
|
|
mContactManager.GetContactsFromCache(ioContactAllocator, *body1, *body2, pair_handled, constraint_created);
|
|
|
|
// If the cache hasn't handled this body pair do actual collision detection
|
|
if (!pair_handled)
|
|
{
|
|
// Create entry in the cache for this body pair
|
|
// Needs to happen irrespective if we found a collision or not (we want to remember that no collision was found too)
|
|
ContactConstraintManager::BodyPairHandle body_pair_handle = mContactManager.AddBodyPair(ioContactAllocator, *body1, *body2);
|
|
if (body_pair_handle == nullptr)
|
|
return; // Out of cache space
|
|
|
|
// Create the query settings
|
|
CollideShapeSettings settings;
|
|
settings.mCollectFacesMode = ECollectFacesMode::CollectFaces;
|
|
settings.mActiveEdgeMode = mPhysicsSettings.mCheckActiveEdges? EActiveEdgeMode::CollideOnlyWithActive : EActiveEdgeMode::CollideWithAll;
|
|
settings.mMaxSeparationDistance = body1->IsSensor() || body2->IsSensor()? 0.0f : mPhysicsSettings.mSpeculativeContactDistance;
|
|
settings.mActiveEdgeMovementDirection = body1->GetLinearVelocity() - body2->GetLinearVelocity();
|
|
|
|
// Create shape filter
|
|
SimShapeFilterWrapperUnion shape_filter_union(mSimShapeFilter, body1);
|
|
SimShapeFilterWrapper &shape_filter = shape_filter_union.GetSimShapeFilterWrapper();
|
|
shape_filter.SetBody2(body2);
|
|
|
|
// Get transforms relative to body1
|
|
RVec3 offset = body1->GetCenterOfMassPosition();
|
|
Mat44 transform1 = Mat44::sRotation(body1->GetRotation());
|
|
Mat44 transform2 = body2->GetCenterOfMassTransform().PostTranslated(-offset).ToMat44();
|
|
|
|
if (mPhysicsSettings.mUseManifoldReduction // Check global flag
|
|
&& body1->GetUseManifoldReductionWithBody(*body2)) // Check body flag
|
|
{
|
|
// Version WITH contact manifold reduction
|
|
|
|
class MyManifold : public ContactManifold
|
|
{
|
|
public:
|
|
Vec3 mFirstWorldSpaceNormal;
|
|
};
|
|
|
|
// A temporary structure that allows us to keep track of the all manifolds between this body pair
|
|
using Manifolds = StaticArray<MyManifold, 32>;
|
|
|
|
// Create collector
|
|
class ReductionCollideShapeCollector : public CollideShapeCollector
|
|
{
|
|
public:
|
|
ReductionCollideShapeCollector(PhysicsSystem *inSystem, const Body *inBody1, const Body *inBody2) :
|
|
mSystem(inSystem),
|
|
mBody1(inBody1),
|
|
mBody2(inBody2)
|
|
{
|
|
}
|
|
|
|
virtual void AddHit(const CollideShapeResult &inResult) override
|
|
{
|
|
// The first body should be the one with the highest motion type
|
|
JPH_ASSERT(mBody1->GetMotionType() >= mBody2->GetMotionType());
|
|
JPH_ASSERT(!ShouldEarlyOut());
|
|
|
|
// Test if we want to accept this hit
|
|
if (mValidateBodyPair)
|
|
{
|
|
switch (mSystem->mContactManager.ValidateContactPoint(*mBody1, *mBody2, mBody1->GetCenterOfMassPosition(), inResult))
|
|
{
|
|
case ValidateResult::AcceptContact:
|
|
// We're just accepting this one, nothing to do
|
|
break;
|
|
|
|
case ValidateResult::AcceptAllContactsForThisBodyPair:
|
|
// Accept and stop calling the validate callback
|
|
mValidateBodyPair = false;
|
|
break;
|
|
|
|
case ValidateResult::RejectContact:
|
|
// Skip this contact
|
|
return;
|
|
|
|
case ValidateResult::RejectAllContactsForThisBodyPair:
|
|
// Skip this and early out
|
|
ForceEarlyOut();
|
|
return;
|
|
}
|
|
}
|
|
|
|
// Calculate normal
|
|
Vec3 world_space_normal = inResult.mPenetrationAxis.Normalized();
|
|
|
|
// Check if we can add it to an existing manifold
|
|
Manifolds::iterator manifold;
|
|
float contact_normal_cos_max_delta_rot = mSystem->mPhysicsSettings.mContactNormalCosMaxDeltaRotation;
|
|
for (manifold = mManifolds.begin(); manifold != mManifolds.end(); ++manifold)
|
|
if (world_space_normal.Dot(manifold->mFirstWorldSpaceNormal) >= contact_normal_cos_max_delta_rot)
|
|
{
|
|
// Update average normal
|
|
manifold->mWorldSpaceNormal += world_space_normal;
|
|
manifold->mPenetrationDepth = max(manifold->mPenetrationDepth, inResult.mPenetrationDepth);
|
|
break;
|
|
}
|
|
if (manifold == mManifolds.end())
|
|
{
|
|
// Check if array is full
|
|
if (mManifolds.size() == mManifolds.capacity())
|
|
{
|
|
// Full, find manifold with least amount of penetration
|
|
manifold = mManifolds.begin();
|
|
for (Manifolds::iterator m = mManifolds.begin() + 1; m < mManifolds.end(); ++m)
|
|
if (m->mPenetrationDepth < manifold->mPenetrationDepth)
|
|
manifold = m;
|
|
|
|
// If this contacts penetration is smaller than the smallest manifold, we skip this contact
|
|
if (inResult.mPenetrationDepth < manifold->mPenetrationDepth)
|
|
return;
|
|
|
|
// Replace the manifold
|
|
*manifold = { { mBody1->GetCenterOfMassPosition(), world_space_normal, inResult.mPenetrationDepth, inResult.mSubShapeID1, inResult.mSubShapeID2, { }, { } }, world_space_normal };
|
|
}
|
|
else
|
|
{
|
|
// Not full, create new manifold
|
|
mManifolds.push_back({ { mBody1->GetCenterOfMassPosition(), world_space_normal, inResult.mPenetrationDepth, inResult.mSubShapeID1, inResult.mSubShapeID2, { }, { } }, world_space_normal });
|
|
manifold = mManifolds.end() - 1;
|
|
}
|
|
}
|
|
|
|
// Determine contact points
|
|
const PhysicsSettings &settings = mSystem->mPhysicsSettings;
|
|
ManifoldBetweenTwoFaces(inResult.mContactPointOn1, inResult.mContactPointOn2, inResult.mPenetrationAxis, settings.mSpeculativeContactDistance + settings.mManifoldTolerance, inResult.mShape1Face, inResult.mShape2Face, manifold->mRelativeContactPointsOn1, manifold->mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, mBody1->GetCenterOfMassPosition()));
|
|
|
|
// Prune if we have more than 32 points (this means we could run out of space in the next iteration)
|
|
if (manifold->mRelativeContactPointsOn1.size() > 32)
|
|
PruneContactPoints(manifold->mFirstWorldSpaceNormal, manifold->mRelativeContactPointsOn1, manifold->mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold->mBaseOffset));
|
|
}
|
|
|
|
PhysicsSystem * mSystem;
|
|
const Body * mBody1;
|
|
const Body * mBody2;
|
|
bool mValidateBodyPair = true;
|
|
Manifolds mManifolds;
|
|
};
|
|
ReductionCollideShapeCollector collector(this, body1, body2);
|
|
|
|
// Perform collision detection between the two shapes
|
|
mSimCollideBodyVsBody(*body1, *body2, transform1, transform2, settings, collector, shape_filter);
|
|
|
|
// Add the contacts
|
|
for (ContactManifold &manifold : collector.mManifolds)
|
|
{
|
|
// Normalize the normal (is a sum of all normals from merged manifolds)
|
|
manifold.mWorldSpaceNormal = manifold.mWorldSpaceNormal.Normalized();
|
|
|
|
// If we still have too many points, prune them now
|
|
if (manifold.mRelativeContactPointsOn1.size() > 4)
|
|
PruneContactPoints(manifold.mWorldSpaceNormal, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset));
|
|
|
|
// Actually add the contact points to the manager
|
|
constraint_created |= mContactManager.AddContactConstraint(ioContactAllocator, body_pair_handle, *body1, *body2, manifold);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Version WITHOUT contact manifold reduction
|
|
|
|
// Create collector
|
|
class NonReductionCollideShapeCollector : public CollideShapeCollector
|
|
{
|
|
public:
|
|
NonReductionCollideShapeCollector(PhysicsSystem *inSystem, ContactAllocator &ioContactAllocator, Body *inBody1, Body *inBody2, const ContactConstraintManager::BodyPairHandle &inPairHandle) :
|
|
mSystem(inSystem),
|
|
mContactAllocator(ioContactAllocator),
|
|
mBody1(inBody1),
|
|
mBody2(inBody2),
|
|
mBodyPairHandle(inPairHandle)
|
|
{
|
|
}
|
|
|
|
virtual void AddHit(const CollideShapeResult &inResult) override
|
|
{
|
|
// The first body should be the one with the highest motion type
|
|
JPH_ASSERT(mBody1->GetMotionType() >= mBody2->GetMotionType());
|
|
JPH_ASSERT(!ShouldEarlyOut());
|
|
|
|
// Test if we want to accept this hit
|
|
if (mValidateBodyPair)
|
|
{
|
|
switch (mSystem->mContactManager.ValidateContactPoint(*mBody1, *mBody2, mBody1->GetCenterOfMassPosition(), inResult))
|
|
{
|
|
case ValidateResult::AcceptContact:
|
|
// We're just accepting this one, nothing to do
|
|
break;
|
|
|
|
case ValidateResult::AcceptAllContactsForThisBodyPair:
|
|
// Accept and stop calling the validate callback
|
|
mValidateBodyPair = false;
|
|
break;
|
|
|
|
case ValidateResult::RejectContact:
|
|
// Skip this contact
|
|
return;
|
|
|
|
case ValidateResult::RejectAllContactsForThisBodyPair:
|
|
// Skip this and early out
|
|
ForceEarlyOut();
|
|
return;
|
|
}
|
|
}
|
|
|
|
// Determine contact points
|
|
ContactManifold manifold;
|
|
manifold.mBaseOffset = mBody1->GetCenterOfMassPosition();
|
|
const PhysicsSettings &settings = mSystem->mPhysicsSettings;
|
|
ManifoldBetweenTwoFaces(inResult.mContactPointOn1, inResult.mContactPointOn2, inResult.mPenetrationAxis, settings.mSpeculativeContactDistance + settings.mManifoldTolerance, inResult.mShape1Face, inResult.mShape2Face, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset));
|
|
|
|
// Calculate normal
|
|
manifold.mWorldSpaceNormal = inResult.mPenetrationAxis.Normalized();
|
|
|
|
// Store penetration depth
|
|
manifold.mPenetrationDepth = inResult.mPenetrationDepth;
|
|
|
|
// Prune if we have more than 4 points
|
|
if (manifold.mRelativeContactPointsOn1.size() > 4)
|
|
PruneContactPoints(manifold.mWorldSpaceNormal, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset));
|
|
|
|
// Set other properties
|
|
manifold.mSubShapeID1 = inResult.mSubShapeID1;
|
|
manifold.mSubShapeID2 = inResult.mSubShapeID2;
|
|
|
|
// Actually add the contact points to the manager
|
|
mConstraintCreated |= mSystem->mContactManager.AddContactConstraint(mContactAllocator, mBodyPairHandle, *mBody1, *mBody2, manifold);
|
|
}
|
|
|
|
PhysicsSystem * mSystem;
|
|
ContactAllocator & mContactAllocator;
|
|
Body * mBody1;
|
|
Body * mBody2;
|
|
ContactConstraintManager::BodyPairHandle mBodyPairHandle;
|
|
bool mValidateBodyPair = true;
|
|
bool mConstraintCreated = false;
|
|
};
|
|
NonReductionCollideShapeCollector collector(this, ioContactAllocator, body1, body2, body_pair_handle);
|
|
|
|
// Perform collision detection between the two shapes
|
|
mSimCollideBodyVsBody(*body1, *body2, transform1, transform2, settings, collector, shape_filter);
|
|
|
|
constraint_created = collector.mConstraintCreated;
|
|
}
|
|
}
|
|
|
|
// If a contact constraint was created, we need to do some extra work
|
|
if (constraint_created)
|
|
{
|
|
// Wake up sleeping bodies
|
|
BodyID body_ids[2];
|
|
int num_bodies = 0;
|
|
if (body1->IsDynamic() && !body1->IsActive())
|
|
body_ids[num_bodies++] = body1->GetID();
|
|
if (body2->IsDynamic() && !body2->IsActive())
|
|
body_ids[num_bodies++] = body2->GetID();
|
|
if (num_bodies > 0)
|
|
mBodyManager.ActivateBodies(body_ids, num_bodies);
|
|
|
|
// Link the two bodies
|
|
mIslandBuilder.LinkBodies(body1->GetIndexInActiveBodiesInternal(), body2->GetIndexInActiveBodiesInternal());
|
|
}
|
|
}
|
|
|
|
void PhysicsSystem::JobFinalizeIslands(PhysicsUpdateContext *ioContext)
|
|
{
|
|
#ifdef JPH_ENABLE_ASSERTS
|
|
// We only touch island data
|
|
BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::None);
|
|
#endif
|
|
|
|
// Finish collecting the islands, at this point the active body list doesn't change so it's safe to access
|
|
mIslandBuilder.Finalize(mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody), mBodyManager.GetNumActiveBodies(EBodyType::RigidBody), mContactManager.GetNumConstraints(), ioContext->mTempAllocator);
|
|
|
|
// Prepare the large island splitter
|
|
if (mPhysicsSettings.mUseLargeIslandSplitter)
|
|
mLargeIslandSplitter.Prepare(mIslandBuilder, mBodyManager.GetNumActiveBodies(EBodyType::RigidBody), ioContext->mTempAllocator);
|
|
}
|
|
|
|
void PhysicsSystem::JobBodySetIslandIndex()
|
|
{
|
|
#ifdef JPH_ENABLE_ASSERTS
|
|
// We only touch island data
|
|
BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::None);
|
|
#endif
|
|
|
|
// Loop through the result and tag all bodies with an island index
|
|
for (uint32 island_idx = 0, n = mIslandBuilder.GetNumIslands(); island_idx < n; ++island_idx)
|
|
{
|
|
BodyID *body_start, *body_end;
|
|
mIslandBuilder.GetBodiesInIsland(island_idx, body_start, body_end);
|
|
for (const BodyID *body = body_start; body < body_end; ++body)
|
|
mBodyManager.GetBody(*body).GetMotionProperties()->SetIslandIndexInternal(island_idx);
|
|
}
|
|
}
|
|
|
|
JPH_SUPPRESS_WARNING_PUSH
|
|
JPH_CLANG_SUPPRESS_WARNING("-Wundefined-func-template") // ConstraintManager::sWarmStartVelocityConstraints / ContactConstraintManager::WarmStartVelocityConstraints is instantiated in the cpp file
|
|
|
|
void PhysicsSystem::JobSolveVelocityConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep)
|
|
{
|
|
#ifdef JPH_ENABLE_ASSERTS
|
|
// We update velocities and need to read positions to do so
|
|
BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::Read);
|
|
#endif
|
|
|
|
float delta_time = ioContext->mStepDeltaTime;
|
|
Constraint **active_constraints = ioContext->mActiveConstraints;
|
|
|
|
// Only the first step to correct for the delta time difference in the previous update
|
|
float warm_start_impulse_ratio = ioStep->mIsFirst? ioContext->mWarmStartImpulseRatio : 1.0f;
|
|
|
|
bool check_islands = true, check_split_islands = mPhysicsSettings.mUseLargeIslandSplitter;
|
|
for (;;)
|
|
{
|
|
// First try to get work from large islands
|
|
if (check_split_islands)
|
|
{
|
|
bool first_iteration;
|
|
uint split_island_index;
|
|
uint32 *constraints_begin, *constraints_end, *contacts_begin, *contacts_end;
|
|
switch (mLargeIslandSplitter.FetchNextBatch(split_island_index, constraints_begin, constraints_end, contacts_begin, contacts_end, first_iteration))
|
|
{
|
|
case LargeIslandSplitter::EStatus::BatchRetrieved:
|
|
{
|
|
if (first_iteration)
|
|
{
|
|
// Iteration 0 is used to warm start the batch (we added 1 to the number of iterations in LargeIslandSplitter::SplitIsland)
|
|
DummyCalculateSolverSteps dummy;
|
|
ConstraintManager::sWarmStartVelocityConstraints(active_constraints, constraints_begin, constraints_end, warm_start_impulse_ratio, dummy);
|
|
mContactManager.WarmStartVelocityConstraints(contacts_begin, contacts_end, warm_start_impulse_ratio, dummy);
|
|
}
|
|
else
|
|
{
|
|
// Solve velocity constraints
|
|
ConstraintManager::sSolveVelocityConstraints(active_constraints, constraints_begin, constraints_end, delta_time);
|
|
mContactManager.SolveVelocityConstraints(contacts_begin, contacts_end);
|
|
}
|
|
|
|
// Mark the batch as processed
|
|
bool last_iteration, final_batch;
|
|
mLargeIslandSplitter.MarkBatchProcessed(split_island_index, constraints_begin, constraints_end, contacts_begin, contacts_end, last_iteration, final_batch);
|
|
|
|
// Save back the lambdas in the contact cache for the warm start of the next physics update
|
|
if (last_iteration)
|
|
mContactManager.StoreAppliedImpulses(contacts_begin, contacts_end);
|
|
|
|
// We processed work, loop again
|
|
continue;
|
|
}
|
|
|
|
case LargeIslandSplitter::EStatus::WaitingForBatch:
|
|
break;
|
|
|
|
case LargeIslandSplitter::EStatus::AllBatchesDone:
|
|
check_split_islands = false;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// If that didn't succeed try to process an island
|
|
if (check_islands)
|
|
{
|
|
// Next island
|
|
uint32 island_idx = ioStep->mSolveVelocityConstraintsNextIsland++;
|
|
if (island_idx >= mIslandBuilder.GetNumIslands())
|
|
{
|
|
// We processed all islands, stop checking islands
|
|
check_islands = false;
|
|
continue;
|
|
}
|
|
|
|
JPH_PROFILE("Island");
|
|
|
|
// Get iterators for this island
|
|
uint32 *constraints_begin, *constraints_end, *contacts_begin, *contacts_end;
|
|
bool has_constraints = mIslandBuilder.GetConstraintsInIsland(island_idx, constraints_begin, constraints_end);
|
|
bool has_contacts = mIslandBuilder.GetContactsInIsland(island_idx, contacts_begin, contacts_end);
|
|
|
|
// If we don't have any contacts or constraints, we know that none of the following islands have any contacts or constraints
|
|
// (because they're sorted by most constraints first). This means we're done.
|
|
if (!has_contacts && !has_constraints)
|
|
{
|
|
#ifdef JPH_ENABLE_ASSERTS
|
|
// Validate our assumption that the next islands don't have any constraints or contacts
|
|
for (; island_idx < mIslandBuilder.GetNumIslands(); ++island_idx)
|
|
{
|
|
JPH_ASSERT(!mIslandBuilder.GetConstraintsInIsland(island_idx, constraints_begin, constraints_end));
|
|
JPH_ASSERT(!mIslandBuilder.GetContactsInIsland(island_idx, contacts_begin, contacts_end));
|
|
}
|
|
#endif // JPH_ENABLE_ASSERTS
|
|
|
|
check_islands = false;
|
|
continue;
|
|
}
|
|
|
|
// Sorting is costly but needed for a deterministic simulation, allow the user to turn this off
|
|
if (mPhysicsSettings.mDeterministicSimulation)
|
|
{
|
|
// Sort constraints to give a deterministic simulation
|
|
ConstraintManager::sSortConstraints(active_constraints, constraints_begin, constraints_end);
|
|
|
|
// Sort contacts to give a deterministic simulation
|
|
mContactManager.SortContacts(contacts_begin, contacts_end);
|
|
}
|
|
|
|
// Split up large islands
|
|
CalculateSolverSteps steps_calculator(mPhysicsSettings);
|
|
if (mPhysicsSettings.mUseLargeIslandSplitter
|
|
&& mLargeIslandSplitter.SplitIsland(island_idx, mIslandBuilder, mBodyManager, mContactManager, active_constraints, steps_calculator))
|
|
continue; // Loop again to try to fetch the newly split island
|
|
|
|
// We didn't create a split, just run the solver now for this entire island. Begin by warm starting.
|
|
ConstraintManager::sWarmStartVelocityConstraints(active_constraints, constraints_begin, constraints_end, warm_start_impulse_ratio, steps_calculator);
|
|
mContactManager.WarmStartVelocityConstraints(contacts_begin, contacts_end, warm_start_impulse_ratio, steps_calculator);
|
|
steps_calculator.Finalize();
|
|
|
|
// Store the number of position steps for later
|
|
mIslandBuilder.SetNumPositionSteps(island_idx, steps_calculator.GetNumPositionSteps());
|
|
|
|
// Solve velocity constraints
|
|
for (uint velocity_step = 0; velocity_step < steps_calculator.GetNumVelocitySteps(); ++velocity_step)
|
|
{
|
|
bool applied_impulse = ConstraintManager::sSolveVelocityConstraints(active_constraints, constraints_begin, constraints_end, delta_time);
|
|
applied_impulse |= mContactManager.SolveVelocityConstraints(contacts_begin, contacts_end);
|
|
if (!applied_impulse)
|
|
break;
|
|
}
|
|
|
|
// Save back the lambdas in the contact cache for the warm start of the next physics update
|
|
mContactManager.StoreAppliedImpulses(contacts_begin, contacts_end);
|
|
|
|
// We processed work, loop again
|
|
continue;
|
|
}
|
|
|
|
if (check_islands)
|
|
{
|
|
// If there are islands, we don't need to wait and can pick up new work
|
|
continue;
|
|
}
|
|
else if (check_split_islands)
|
|
{
|
|
// If there are split islands, but we didn't do any work, give up a time slice
|
|
std::this_thread::yield();
|
|
}
|
|
else
|
|
{
|
|
// No more work
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
JPH_SUPPRESS_WARNING_POP
|
|
|
|
void PhysicsSystem::JobPreIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep)
|
|
{
|
|
// Reserve enough space for all bodies that may need a cast
|
|
TempAllocator *temp_allocator = ioContext->mTempAllocator;
|
|
JPH_ASSERT(ioStep->mCCDBodies == nullptr);
|
|
ioStep->mCCDBodiesCapacity = mBodyManager.GetNumActiveCCDBodies();
|
|
ioStep->mCCDBodies = (CCDBody *)temp_allocator->Allocate(ioStep->mCCDBodiesCapacity * sizeof(CCDBody));
|
|
|
|
// Initialize the mapping table between active body and CCD body
|
|
JPH_ASSERT(ioStep->mActiveBodyToCCDBody == nullptr);
|
|
ioStep->mNumActiveBodyToCCDBody = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody);
|
|
ioStep->mActiveBodyToCCDBody = (int *)temp_allocator->Allocate(ioStep->mNumActiveBodyToCCDBody * sizeof(int));
|
|
|
|
// Prepare the split island builder for solving the position constraints
|
|
mLargeIslandSplitter.PrepareForSolvePositions();
|
|
}
|
|
|
|
void PhysicsSystem::JobIntegrateVelocity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep)
|
|
{
|
|
#ifdef JPH_ENABLE_ASSERTS
|
|
// We update positions and need velocity to do so, we also clamp velocities so need to write to them
|
|
BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::ReadWrite);
|
|
#endif
|
|
|
|
float delta_time = ioContext->mStepDeltaTime;
|
|
const BodyID *active_bodies = mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody);
|
|
uint32 num_active_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody);
|
|
uint32 num_active_bodies_after_find_collisions = ioStep->mActiveBodyReadIdx;
|
|
|
|
// We can move bodies that are not part of an island. In this case we need to notify the broadphase of the movement.
|
|
static constexpr int cBodiesBatch = 64;
|
|
BodyID *bodies_to_update_bounds = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID));
|
|
int num_bodies_to_update_bounds = 0;
|
|
|
|
for (;;)
|
|
{
|
|
// Atomically fetch a batch of bodies
|
|
uint32 active_body_idx = ioStep->mIntegrateVelocityReadIdx.fetch_add(cIntegrateVelocityBatchSize);
|
|
if (active_body_idx >= num_active_bodies)
|
|
break;
|
|
|
|
// Calculate the end of the batch
|
|
uint32 active_body_idx_end = min(num_active_bodies, active_body_idx + cIntegrateVelocityBatchSize);
|
|
|
|
// Process the batch
|
|
while (active_body_idx < active_body_idx_end)
|
|
{
|
|
// Update the positions using an Symplectic Euler step (which integrates using the updated velocity v1' rather
|
|
// than the original velocity v1):
|
|
// x1' = x1 + h * v1'
|
|
// At this point the active bodies list does not change, so it is safe to access the array.
|
|
BodyID body_id = active_bodies[active_body_idx];
|
|
Body &body = mBodyManager.GetBody(body_id);
|
|
MotionProperties *mp = body.GetMotionProperties();
|
|
|
|
JPH_DET_LOG("JobIntegrateVelocity: id: " << body_id << " v: " << body.GetLinearVelocity() << " w: " << body.GetAngularVelocity());
|
|
|
|
// Clamp velocities (not for kinematic bodies)
|
|
if (body.IsDynamic())
|
|
{
|
|
mp->ClampLinearVelocity();
|
|
mp->ClampAngularVelocity();
|
|
}
|
|
|
|
// Update the rotation of the body according to the angular velocity
|
|
// For motion type discrete we need to do this anyway, for motion type linear cast we have multiple choices
|
|
// 1. Rotate the body first and then sweep
|
|
// 2. First sweep and then rotate the body at the end
|
|
// 3. Pick some in between rotation (e.g. half way), then sweep and finally rotate the remainder
|
|
// (1) has some clear advantages as when a long thin body hits a surface away from the center of mass, this will result in a large angular velocity and a limited reduction in linear velocity.
|
|
// When simulation the rotation first before doing the translation, the body will be able to rotate away from the contact point allowing the center of mass to approach the surface. When using
|
|
// approach (2) in this case what will happen is that we will immediately detect the same collision again (the body has not rotated and the body was already colliding at the end of the previous
|
|
// time step) resulting in a lot of stolen time and the body appearing to be frozen in an unnatural pose (like it is glued at an angle to the surface). (2) obviously has some negative side effects
|
|
// too as simulating the rotation first may cause it to tunnel through a small object that the linear cast might have otherwise detected. In any case a linear cast is not good for detecting
|
|
// tunneling due to angular rotation, so we don't care about that too much (you'd need a full cast to take angular effects into account).
|
|
body.AddRotationStep(body.GetAngularVelocity() * delta_time);
|
|
|
|
// Get delta position
|
|
Vec3 delta_pos = body.GetLinearVelocity() * delta_time;
|
|
|
|
// If the position should be updated (or if it is delayed because of CCD)
|
|
bool update_position = true;
|
|
|
|
switch (mp->GetMotionQuality())
|
|
{
|
|
case EMotionQuality::Discrete:
|
|
// No additional collision checking to be done
|
|
break;
|
|
|
|
case EMotionQuality::LinearCast:
|
|
if (body.IsDynamic() // Kinematic bodies cannot be stopped
|
|
&& !body.IsSensor()) // We don't support CCD sensors
|
|
{
|
|
// Determine inner radius (the smallest sphere that fits into the shape)
|
|
float inner_radius = body.GetShape()->GetInnerRadius();
|
|
JPH_ASSERT(inner_radius > 0.0f, "The shape has no inner radius, this makes the shape unsuitable for the linear cast motion quality as we cannot move it without risking tunneling.");
|
|
|
|
// Measure translation in this step and check if it above the threshold to perform a linear cast
|
|
float linear_cast_threshold_sq = Square(mPhysicsSettings.mLinearCastThreshold * inner_radius);
|
|
if (delta_pos.LengthSq() > linear_cast_threshold_sq)
|
|
{
|
|
// This body needs a cast
|
|
uint32 ccd_body_idx = ioStep->mNumCCDBodies++;
|
|
JPH_ASSERT(active_body_idx < ioStep->mNumActiveBodyToCCDBody);
|
|
ioStep->mActiveBodyToCCDBody[active_body_idx] = ccd_body_idx;
|
|
new (&ioStep->mCCDBodies[ccd_body_idx]) CCDBody(body_id, delta_pos, linear_cast_threshold_sq, min(mPhysicsSettings.mPenetrationSlop, mPhysicsSettings.mLinearCastMaxPenetration * inner_radius));
|
|
|
|
update_position = false;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
if (update_position)
|
|
{
|
|
// Move the body now
|
|
body.AddPositionStep(delta_pos);
|
|
|
|
// If the body was activated due to an earlier CCD step it will have an index in the active
|
|
// body list that it higher than the highest one we processed during FindCollisions
|
|
// which means it hasn't been assigned an island and will not be updated by an island
|
|
// this means that we need to update its bounds manually
|
|
if (mp->GetIndexInActiveBodiesInternal() >= num_active_bodies_after_find_collisions)
|
|
{
|
|
body.CalculateWorldSpaceBoundsInternal();
|
|
bodies_to_update_bounds[num_bodies_to_update_bounds++] = body.GetID();
|
|
if (num_bodies_to_update_bounds == cBodiesBatch)
|
|
{
|
|
// Buffer full, flush now
|
|
mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false);
|
|
num_bodies_to_update_bounds = 0;
|
|
}
|
|
}
|
|
|
|
// We did not create a CCD body
|
|
ioStep->mActiveBodyToCCDBody[active_body_idx] = -1;
|
|
}
|
|
|
|
active_body_idx++;
|
|
}
|
|
}
|
|
|
|
// Notify change bounds on requested bodies
|
|
if (num_bodies_to_update_bounds > 0)
|
|
mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false);
|
|
}
|
|
|
|
void PhysicsSystem::JobPostIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) const
|
|
{
|
|
// Validate that our reservations were correct
|
|
JPH_ASSERT(ioStep->mNumCCDBodies <= mBodyManager.GetNumActiveCCDBodies());
|
|
|
|
if (ioStep->mNumCCDBodies == 0)
|
|
{
|
|
// No continuous collision detection jobs -> kick the next job ourselves
|
|
ioStep->mContactRemovedCallbacks.RemoveDependency();
|
|
}
|
|
else
|
|
{
|
|
// Run the continuous collision detection jobs
|
|
int num_continuous_collision_jobs = min(int(ioStep->mNumCCDBodies + cNumCCDBodiesPerJob - 1) / cNumCCDBodiesPerJob, ioContext->GetMaxConcurrency());
|
|
ioStep->mResolveCCDContacts.AddDependency(num_continuous_collision_jobs);
|
|
ioStep->mContactRemovedCallbacks.AddDependency(num_continuous_collision_jobs - 1); // Already had 1 dependency
|
|
for (int i = 0; i < num_continuous_collision_jobs; ++i)
|
|
{
|
|
JobHandle job = ioContext->mJobSystem->CreateJob("FindCCDContacts", cColorFindCCDContacts, [ioContext, ioStep]()
|
|
{
|
|
ioContext->mPhysicsSystem->JobFindCCDContacts(ioContext, ioStep);
|
|
|
|
ioStep->mResolveCCDContacts.RemoveDependency();
|
|
ioStep->mContactRemovedCallbacks.RemoveDependency();
|
|
});
|
|
ioContext->mBarrier->AddJob(job);
|
|
}
|
|
}
|
|
}
|
|
|
|
// Helper function to calculate the motion of a body during this CCD step
|
|
inline static Vec3 sCalculateBodyMotion(const Body &inBody, float inDeltaTime)
|
|
{
|
|
// If the body is linear casting, the body has not yet moved so we need to calculate its motion
|
|
if (inBody.IsDynamic() && inBody.GetMotionProperties()->GetMotionQuality() == EMotionQuality::LinearCast)
|
|
return inDeltaTime * inBody.GetLinearVelocity();
|
|
|
|
// Body has already moved, so we don't need to correct for anything
|
|
return Vec3::sZero();
|
|
}
|
|
|
|
// Helper function that finds the CCD body corresponding to a body (if it exists)
|
|
inline static PhysicsUpdateContext::Step::CCDBody *sGetCCDBody(const Body &inBody, PhysicsUpdateContext::Step *inStep)
|
|
{
|
|
// Only rigid bodies can have a CCD body
|
|
if (!inBody.IsRigidBody())
|
|
return nullptr;
|
|
|
|
// If the body has no motion properties it cannot have a CCD body
|
|
const MotionProperties *motion_properties = inBody.GetMotionPropertiesUnchecked();
|
|
if (motion_properties == nullptr)
|
|
return nullptr;
|
|
|
|
// If it is not active it cannot have a CCD body
|
|
uint32 active_index = motion_properties->GetIndexInActiveBodiesInternal();
|
|
if (active_index == Body::cInactiveIndex)
|
|
return nullptr;
|
|
|
|
// Check if the active body has a corresponding CCD body
|
|
JPH_ASSERT(active_index < inStep->mNumActiveBodyToCCDBody); // Ensure that the body has a mapping to CCD body
|
|
int ccd_index = inStep->mActiveBodyToCCDBody[active_index];
|
|
if (ccd_index < 0)
|
|
return nullptr;
|
|
|
|
PhysicsUpdateContext::Step::CCDBody *ccd_body = &inStep->mCCDBodies[ccd_index];
|
|
JPH_ASSERT(ccd_body->mBodyID1 == inBody.GetID(), "We found the wrong CCD body!");
|
|
return ccd_body;
|
|
}
|
|
|
|
void PhysicsSystem::JobFindCCDContacts(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep)
|
|
{
|
|
#ifdef JPH_ENABLE_ASSERTS
|
|
// We only read positions, but the validate callback may read body positions and velocities
|
|
BodyAccess::Grant grant(BodyAccess::EAccess::Read, BodyAccess::EAccess::Read);
|
|
#endif
|
|
|
|
// Allocation context for allocating new contact points
|
|
ContactAllocator contact_allocator(mContactManager.GetContactAllocator());
|
|
|
|
// Settings
|
|
ShapeCastSettings settings;
|
|
settings.mUseShrunkenShapeAndConvexRadius = true;
|
|
settings.mBackFaceModeTriangles = EBackFaceMode::IgnoreBackFaces;
|
|
settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces;
|
|
settings.mReturnDeepestPoint = true;
|
|
settings.mCollectFacesMode = ECollectFacesMode::CollectFaces;
|
|
settings.mActiveEdgeMode = mPhysicsSettings.mCheckActiveEdges? EActiveEdgeMode::CollideOnlyWithActive : EActiveEdgeMode::CollideWithAll;
|
|
|
|
for (;;)
|
|
{
|
|
// Fetch the next body to cast
|
|
uint32 idx = ioStep->mNextCCDBody++;
|
|
if (idx >= ioStep->mNumCCDBodies)
|
|
break;
|
|
CCDBody &ccd_body = ioStep->mCCDBodies[idx];
|
|
const Body &body = mBodyManager.GetBody(ccd_body.mBodyID1);
|
|
|
|
// Filter out layers
|
|
DefaultBroadPhaseLayerFilter broadphase_layer_filter = GetDefaultBroadPhaseLayerFilter(body.GetObjectLayer());
|
|
DefaultObjectLayerFilter object_layer_filter = GetDefaultLayerFilter(body.GetObjectLayer());
|
|
|
|
#ifdef JPH_DEBUG_RENDERER
|
|
// Draw start and end shape of cast
|
|
if (sDrawMotionQualityLinearCast)
|
|
{
|
|
RMat44 com = body.GetCenterOfMassTransform();
|
|
body.GetShape()->Draw(DebugRenderer::sInstance, com, Vec3::sOne(), Color::sGreen, false, true);
|
|
DebugRenderer::sInstance->DrawArrow(com.GetTranslation(), com.GetTranslation() + ccd_body.mDeltaPosition, Color::sGreen, 0.1f);
|
|
body.GetShape()->Draw(DebugRenderer::sInstance, com.PostTranslated(ccd_body.mDeltaPosition), Vec3::sOne(), Color::sRed, false, true);
|
|
}
|
|
#endif // JPH_DEBUG_RENDERER
|
|
|
|
// Create a collector that will find the maximum distance allowed to travel while not penetrating more than 'max penetration'
|
|
class CCDNarrowPhaseCollector : public CastShapeCollector
|
|
{
|
|
public:
|
|
CCDNarrowPhaseCollector(const BodyManager &inBodyManager, ContactConstraintManager &inContactConstraintManager, CCDBody &inCCDBody, ShapeCastResult &inResult, float inDeltaTime) :
|
|
mBodyManager(inBodyManager),
|
|
mContactConstraintManager(inContactConstraintManager),
|
|
mCCDBody(inCCDBody),
|
|
mResult(inResult),
|
|
mDeltaTime(inDeltaTime)
|
|
{
|
|
}
|
|
|
|
virtual void AddHit(const ShapeCastResult &inResult) override
|
|
{
|
|
JPH_PROFILE_FUNCTION();
|
|
|
|
// Check if this is a possible earlier hit than the one before
|
|
float fraction = inResult.mFraction;
|
|
if (fraction < mCCDBody.mFractionPlusSlop)
|
|
{
|
|
// Normalize normal
|
|
Vec3 normal = inResult.mPenetrationAxis.Normalized();
|
|
|
|
// Calculate how much we can add to the fraction to penetrate the collision point by mMaxPenetration.
|
|
// Note that the normal is pointing towards body 2!
|
|
// Let the extra distance that we can travel along delta_pos be 'dist': mMaxPenetration / dist = cos(angle between normal and delta_pos) = normal . delta_pos / |delta_pos|
|
|
// <=> dist = mMaxPenetration * |delta_pos| / normal . delta_pos
|
|
// Converting to a faction: delta_fraction = dist / |delta_pos| = mLinearCastTreshold / normal . delta_pos
|
|
float denominator = normal.Dot(mCCDBody.mDeltaPosition);
|
|
if (denominator > mCCDBody.mMaxPenetration) // Avoid dividing by zero, if extra hit fraction > 1 there's also no point in continuing
|
|
{
|
|
float fraction_plus_slop = fraction + mCCDBody.mMaxPenetration / denominator;
|
|
if (fraction_plus_slop < mCCDBody.mFractionPlusSlop)
|
|
{
|
|
const Body &body2 = mBodyManager.GetBody(inResult.mBodyID2);
|
|
|
|
// Check if we've already accepted all hits from this body
|
|
if (mValidateBodyPair)
|
|
{
|
|
// Validate the contact result
|
|
const Body &body1 = mBodyManager.GetBody(mCCDBody.mBodyID1);
|
|
ValidateResult validate_result = mContactConstraintManager.ValidateContactPoint(body1, body2, body1.GetCenterOfMassPosition(), inResult); // Note that the center of mass of body 1 is the start of the sweep and is used as base offset below
|
|
switch (validate_result)
|
|
{
|
|
case ValidateResult::AcceptContact:
|
|
// Just continue
|
|
break;
|
|
|
|
case ValidateResult::AcceptAllContactsForThisBodyPair:
|
|
// Accept this and all following contacts from this body
|
|
mValidateBodyPair = false;
|
|
break;
|
|
|
|
case ValidateResult::RejectContact:
|
|
return;
|
|
|
|
case ValidateResult::RejectAllContactsForThisBodyPair:
|
|
// Reject this and all following contacts from this body
|
|
mRejectAll = true;
|
|
ForceEarlyOut();
|
|
return;
|
|
}
|
|
}
|
|
|
|
// This is the earliest hit so far, store it
|
|
mCCDBody.mContactNormal = normal;
|
|
mCCDBody.mBodyID2 = inResult.mBodyID2;
|
|
mCCDBody.mSubShapeID2 = inResult.mSubShapeID2;
|
|
mCCDBody.mFraction = fraction;
|
|
mCCDBody.mFractionPlusSlop = fraction_plus_slop;
|
|
mResult = inResult;
|
|
|
|
// Result was assuming body 2 is not moving, but it is, so we need to correct for it
|
|
Vec3 movement2 = fraction * sCalculateBodyMotion(body2, mDeltaTime);
|
|
if (!movement2.IsNearZero())
|
|
{
|
|
mResult.mContactPointOn1 += movement2;
|
|
mResult.mContactPointOn2 += movement2;
|
|
for (Vec3 &v : mResult.mShape1Face)
|
|
v += movement2;
|
|
for (Vec3 &v : mResult.mShape2Face)
|
|
v += movement2;
|
|
}
|
|
|
|
// Update early out fraction
|
|
UpdateEarlyOutFraction(fraction_plus_slop);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
bool mValidateBodyPair; ///< If we still have to call the ValidateContactPoint for this body pair
|
|
bool mRejectAll; ///< Reject all further contacts between this body pair
|
|
|
|
private:
|
|
const BodyManager & mBodyManager;
|
|
ContactConstraintManager & mContactConstraintManager;
|
|
CCDBody & mCCDBody;
|
|
ShapeCastResult & mResult;
|
|
float mDeltaTime;
|
|
BodyID mAcceptedBodyID;
|
|
};
|
|
|
|
// Narrowphase collector
|
|
ShapeCastResult cast_shape_result;
|
|
CCDNarrowPhaseCollector np_collector(mBodyManager, mContactManager, ccd_body, cast_shape_result, ioContext->mStepDeltaTime);
|
|
|
|
// This collector wraps the narrowphase collector and collects the closest hit
|
|
class CCDBroadPhaseCollector : public CastShapeBodyCollector
|
|
{
|
|
public:
|
|
CCDBroadPhaseCollector(const CCDBody &inCCDBody, const Body &inBody1, const RShapeCast &inShapeCast, ShapeCastSettings &inShapeCastSettings, SimShapeFilterWrapper &inShapeFilter, CCDNarrowPhaseCollector &ioCollector, const BodyManager &inBodyManager, PhysicsUpdateContext::Step *inStep, float inDeltaTime) :
|
|
mCCDBody(inCCDBody),
|
|
mBody1(inBody1),
|
|
mBody1Extent(inShapeCast.mShapeWorldBounds.GetExtent()),
|
|
mShapeCast(inShapeCast),
|
|
mShapeCastSettings(inShapeCastSettings),
|
|
mShapeFilter(inShapeFilter),
|
|
mCollector(ioCollector),
|
|
mBodyManager(inBodyManager),
|
|
mStep(inStep),
|
|
mDeltaTime(inDeltaTime)
|
|
{
|
|
}
|
|
|
|
virtual void AddHit(const BroadPhaseCastResult &inResult) override
|
|
{
|
|
JPH_PROFILE_FUNCTION();
|
|
|
|
JPH_ASSERT(inResult.mFraction <= GetEarlyOutFraction(), "This hit should not have been passed on to the collector");
|
|
|
|
// Test if we're colliding with ourselves
|
|
if (mBody1.GetID() == inResult.mBodyID)
|
|
return;
|
|
|
|
// Avoid treating duplicates, if both bodies are doing CCD then only consider collision if body ID < other body ID
|
|
const Body &body2 = mBodyManager.GetBody(inResult.mBodyID);
|
|
const CCDBody *ccd_body2 = sGetCCDBody(body2, mStep);
|
|
if (ccd_body2 != nullptr && mCCDBody.mBodyID1 > ccd_body2->mBodyID1)
|
|
return;
|
|
|
|
// Test group filter
|
|
if (!mBody1.GetCollisionGroup().CanCollide(body2.GetCollisionGroup()))
|
|
return;
|
|
|
|
// TODO: For now we ignore sensors
|
|
if (body2.IsSensor())
|
|
return;
|
|
|
|
// Get relative movement of these two bodies
|
|
Vec3 direction = mShapeCast.mDirection - sCalculateBodyMotion(body2, mDeltaTime);
|
|
|
|
// Test if the remaining movement is less than our movement threshold
|
|
if (direction.LengthSq() < mCCDBody.mLinearCastThresholdSq)
|
|
return;
|
|
|
|
// Get the bounds of 2, widen it by the extent of 1 and test a ray to see if it hits earlier than the current early out fraction
|
|
AABox bounds = body2.GetWorldSpaceBounds();
|
|
bounds.mMin -= mBody1Extent;
|
|
bounds.mMax += mBody1Extent;
|
|
float hit_fraction = RayAABox(Vec3(mShapeCast.mCenterOfMassStart.GetTranslation()), RayInvDirection(direction), bounds.mMin, bounds.mMax);
|
|
if (hit_fraction > GetPositiveEarlyOutFraction()) // If early out fraction <= 0, we have the possibility of finding a deeper hit so we need to clamp the early out fraction
|
|
return;
|
|
|
|
// Reset collector (this is a new body pair)
|
|
mCollector.ResetEarlyOutFraction(GetEarlyOutFraction());
|
|
mCollector.mValidateBodyPair = true;
|
|
mCollector.mRejectAll = false;
|
|
|
|
// Set body ID on shape filter
|
|
mShapeFilter.SetBody2(&body2);
|
|
|
|
// Provide direction as hint for the active edges algorithm
|
|
mShapeCastSettings.mActiveEdgeMovementDirection = direction;
|
|
|
|
// Do narrow phase collision check
|
|
RShapeCast relative_cast(mShapeCast.mShape, mShapeCast.mScale, mShapeCast.mCenterOfMassStart, direction, mShapeCast.mShapeWorldBounds);
|
|
body2.GetTransformedShape().CastShape(relative_cast, mShapeCastSettings, mShapeCast.mCenterOfMassStart.GetTranslation(), mCollector, mShapeFilter);
|
|
|
|
// Update early out fraction based on narrow phase collector
|
|
if (!mCollector.mRejectAll)
|
|
UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
|
|
}
|
|
|
|
const CCDBody & mCCDBody;
|
|
const Body & mBody1;
|
|
Vec3 mBody1Extent;
|
|
RShapeCast mShapeCast;
|
|
ShapeCastSettings & mShapeCastSettings;
|
|
SimShapeFilterWrapper & mShapeFilter;
|
|
CCDNarrowPhaseCollector & mCollector;
|
|
const BodyManager & mBodyManager;
|
|
PhysicsUpdateContext::Step *mStep;
|
|
float mDeltaTime;
|
|
};
|
|
|
|
// Create shape filter
|
|
SimShapeFilterWrapperUnion shape_filter_union(mSimShapeFilter, &body);
|
|
SimShapeFilterWrapper &shape_filter = shape_filter_union.GetSimShapeFilterWrapper();
|
|
|
|
// Check if we collide with any other body. Note that we use the non-locking interface as we know the broadphase cannot be modified at this point.
|
|
RShapeCast shape_cast(body.GetShape(), Vec3::sOne(), body.GetCenterOfMassTransform(), ccd_body.mDeltaPosition);
|
|
CCDBroadPhaseCollector bp_collector(ccd_body, body, shape_cast, settings, shape_filter, np_collector, mBodyManager, ioStep, ioContext->mStepDeltaTime);
|
|
mBroadPhase->CastAABoxNoLock({ shape_cast.mShapeWorldBounds, shape_cast.mDirection }, bp_collector, broadphase_layer_filter, object_layer_filter);
|
|
|
|
// Check if there was a hit
|
|
if (ccd_body.mFractionPlusSlop < 1.0f)
|
|
{
|
|
const Body &body2 = mBodyManager.GetBody(ccd_body.mBodyID2);
|
|
|
|
// Determine contact manifold
|
|
ContactManifold manifold;
|
|
manifold.mBaseOffset = shape_cast.mCenterOfMassStart.GetTranslation();
|
|
ManifoldBetweenTwoFaces(cast_shape_result.mContactPointOn1, cast_shape_result.mContactPointOn2, cast_shape_result.mPenetrationAxis, mPhysicsSettings.mManifoldTolerance, cast_shape_result.mShape1Face, cast_shape_result.mShape2Face, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset));
|
|
manifold.mSubShapeID1 = cast_shape_result.mSubShapeID1;
|
|
manifold.mSubShapeID2 = cast_shape_result.mSubShapeID2;
|
|
manifold.mPenetrationDepth = cast_shape_result.mPenetrationDepth;
|
|
manifold.mWorldSpaceNormal = ccd_body.mContactNormal;
|
|
|
|
// Call contact point callbacks
|
|
mContactManager.OnCCDContactAdded(contact_allocator, body, body2, manifold, ccd_body.mContactSettings);
|
|
|
|
if (ccd_body.mContactSettings.mIsSensor)
|
|
{
|
|
// If this is a sensor, we don't want to solve the contact
|
|
ccd_body.mFractionPlusSlop = 1.0f;
|
|
ccd_body.mBodyID2 = BodyID();
|
|
}
|
|
else
|
|
{
|
|
// Calculate the average position from the manifold (this will result in the same impulse applied as when we apply impulses to all contact points)
|
|
if (manifold.mRelativeContactPointsOn2.size() > 1)
|
|
{
|
|
Vec3 average_contact_point = Vec3::sZero();
|
|
for (const Vec3 &v : manifold.mRelativeContactPointsOn2)
|
|
average_contact_point += v;
|
|
average_contact_point /= (float)manifold.mRelativeContactPointsOn2.size();
|
|
ccd_body.mContactPointOn2 = manifold.mBaseOffset + average_contact_point;
|
|
}
|
|
else
|
|
ccd_body.mContactPointOn2 = manifold.mBaseOffset + cast_shape_result.mContactPointOn2;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Collect information from the contact allocator and accumulate it in the step.
|
|
sFinalizeContactAllocator(*ioStep, contact_allocator);
|
|
}
|
|
|
|
void PhysicsSystem::JobResolveCCDContacts(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep)
|
|
{
|
|
#ifdef JPH_ENABLE_ASSERTS
|
|
// Read/write body access
|
|
BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::ReadWrite);
|
|
|
|
// We activate bodies that we collide with
|
|
BodyManager::GrantActiveBodiesAccess grant_active(true, false);
|
|
#endif
|
|
|
|
uint32 num_active_bodies_after_find_collisions = ioStep->mActiveBodyReadIdx;
|
|
TempAllocator *temp_allocator = ioContext->mTempAllocator;
|
|
|
|
// Check if there's anything to do
|
|
uint num_ccd_bodies = ioStep->mNumCCDBodies;
|
|
if (num_ccd_bodies > 0)
|
|
{
|
|
// Sort on fraction so that we process earliest collisions first
|
|
// This is needed to make the simulation deterministic and also to be able to stop contact processing
|
|
// between body pairs if an earlier hit was found involving the body by another CCD body
|
|
// (if it's body ID < this CCD body's body ID - see filtering logic in CCDBroadPhaseCollector)
|
|
CCDBody **sorted_ccd_bodies = (CCDBody **)temp_allocator->Allocate(num_ccd_bodies * sizeof(CCDBody *));
|
|
JPH_SCOPE_EXIT([temp_allocator, sorted_ccd_bodies, num_ccd_bodies]{ temp_allocator->Free(sorted_ccd_bodies, num_ccd_bodies * sizeof(CCDBody *)); });
|
|
{
|
|
JPH_PROFILE("Sort");
|
|
|
|
// We don't want to copy the entire struct (it's quite big), so we create a pointer array first
|
|
CCDBody *src_ccd_bodies = ioStep->mCCDBodies;
|
|
CCDBody **dst_ccd_bodies = sorted_ccd_bodies;
|
|
CCDBody **dst_ccd_bodies_end = dst_ccd_bodies + num_ccd_bodies;
|
|
while (dst_ccd_bodies < dst_ccd_bodies_end)
|
|
*(dst_ccd_bodies++) = src_ccd_bodies++;
|
|
|
|
// Which we then sort
|
|
QuickSort(sorted_ccd_bodies, sorted_ccd_bodies + num_ccd_bodies, [](const CCDBody *inBody1, const CCDBody *inBody2)
|
|
{
|
|
if (inBody1->mFractionPlusSlop != inBody2->mFractionPlusSlop)
|
|
return inBody1->mFractionPlusSlop < inBody2->mFractionPlusSlop;
|
|
|
|
return inBody1->mBodyID1 < inBody2->mBodyID1;
|
|
});
|
|
}
|
|
|
|
// We can collide with bodies that are not active, we track them here so we can activate them in one go at the end.
|
|
// This is also needed because we can't modify the active body array while we iterate it.
|
|
static constexpr int cBodiesBatch = 64;
|
|
BodyID *bodies_to_activate = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID));
|
|
int num_bodies_to_activate = 0;
|
|
|
|
// We can move bodies that are not part of an island. In this case we need to notify the broadphase of the movement.
|
|
BodyID *bodies_to_update_bounds = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID));
|
|
int num_bodies_to_update_bounds = 0;
|
|
|
|
for (uint i = 0; i < num_ccd_bodies; ++i)
|
|
{
|
|
const CCDBody *ccd_body = sorted_ccd_bodies[i];
|
|
Body &body1 = mBodyManager.GetBody(ccd_body->mBodyID1);
|
|
MotionProperties *body_mp = body1.GetMotionProperties();
|
|
|
|
// If there was a hit
|
|
if (!ccd_body->mBodyID2.IsInvalid())
|
|
{
|
|
Body &body2 = mBodyManager.GetBody(ccd_body->mBodyID2);
|
|
|
|
// Determine if the other body has a CCD body
|
|
CCDBody *ccd_body2 = sGetCCDBody(body2, ioStep);
|
|
if (ccd_body2 != nullptr)
|
|
{
|
|
JPH_ASSERT(ccd_body2->mBodyID2 != ccd_body->mBodyID1, "If we collided with another body, that other body should have ignored collisions with us!");
|
|
|
|
// Check if the other body found a hit that is further away
|
|
if (ccd_body2->mFraction > ccd_body->mFraction)
|
|
{
|
|
// Reset the colliding body of the other CCD body. The other body will shorten its distance traveled and will not do any collision response (we'll do that).
|
|
// This means that at this point we have triggered a contact point add/persist for our further hit by accident for the other body.
|
|
// We accept this as calling the contact point callbacks here would require persisting the manifolds up to this point and doing the callbacks single threaded.
|
|
ccd_body2->mBodyID2 = BodyID();
|
|
ccd_body2->mFractionPlusSlop = ccd_body->mFraction;
|
|
}
|
|
}
|
|
|
|
// If the other body moved less than us before hitting something, we're not colliding with it so we again have triggered contact point add/persist callbacks by accident.
|
|
// We'll just move to the collision position anyway (as that's the last position we know is good), but we won't do any collision response.
|
|
if (ccd_body2 == nullptr || ccd_body2->mFraction >= ccd_body->mFraction)
|
|
{
|
|
const ContactSettings &contact_settings = ccd_body->mContactSettings;
|
|
|
|
// Calculate contact point velocity for body 1
|
|
Vec3 r1_plus_u = Vec3(ccd_body->mContactPointOn2 - (body1.GetCenterOfMassPosition() + ccd_body->mFraction * ccd_body->mDeltaPosition));
|
|
Vec3 v1 = body1.GetPointVelocityCOM(r1_plus_u);
|
|
|
|
// Calculate inverse mass for body 1
|
|
float inv_m1 = contact_settings.mInvMassScale1 * body_mp->GetInverseMass();
|
|
|
|
if (body2.IsRigidBody())
|
|
{
|
|
// Calculate contact point velocity for body 2
|
|
Vec3 r2 = Vec3(ccd_body->mContactPointOn2 - body2.GetCenterOfMassPosition());
|
|
Vec3 v2 = body2.GetPointVelocityCOM(r2);
|
|
|
|
// Calculate relative contact velocity
|
|
Vec3 relative_velocity = v2 - v1;
|
|
float normal_velocity = relative_velocity.Dot(ccd_body->mContactNormal);
|
|
|
|
// Calculate velocity bias due to restitution
|
|
float normal_velocity_bias;
|
|
if (contact_settings.mCombinedRestitution > 0.0f && normal_velocity < -mPhysicsSettings.mMinVelocityForRestitution)
|
|
normal_velocity_bias = contact_settings.mCombinedRestitution * normal_velocity;
|
|
else
|
|
normal_velocity_bias = 0.0f;
|
|
|
|
// Get inverse mass of body 2
|
|
float inv_m2 = body2.GetMotionPropertiesUnchecked() != nullptr? contact_settings.mInvMassScale2 * body2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f;
|
|
|
|
// Solve contact constraint
|
|
AxisConstraintPart contact_constraint;
|
|
contact_constraint.CalculateConstraintPropertiesWithMassOverride(body1, inv_m1, contact_settings.mInvInertiaScale1, r1_plus_u, body2, inv_m2, contact_settings.mInvInertiaScale2, r2, ccd_body->mContactNormal, normal_velocity_bias);
|
|
contact_constraint.SolveVelocityConstraintWithMassOverride(body1, inv_m1, body2, inv_m2, ccd_body->mContactNormal, -FLT_MAX, FLT_MAX);
|
|
|
|
// Apply friction
|
|
if (contact_settings.mCombinedFriction > 0.0f)
|
|
{
|
|
// Calculate friction direction by removing normal velocity from the relative velocity
|
|
Vec3 friction_direction = relative_velocity - normal_velocity * ccd_body->mContactNormal;
|
|
float friction_direction_len_sq = friction_direction.LengthSq();
|
|
if (friction_direction_len_sq > 1.0e-12f)
|
|
{
|
|
// Normalize friction direction
|
|
friction_direction /= sqrt(friction_direction_len_sq);
|
|
|
|
// Calculate max friction impulse
|
|
float max_lambda_f = contact_settings.mCombinedFriction * contact_constraint.GetTotalLambda();
|
|
|
|
AxisConstraintPart friction;
|
|
friction.CalculateConstraintPropertiesWithMassOverride(body1, inv_m1, contact_settings.mInvInertiaScale1, r1_plus_u, body2, inv_m2, contact_settings.mInvInertiaScale2, r2, friction_direction);
|
|
friction.SolveVelocityConstraintWithMassOverride(body1, inv_m1, body2, inv_m2, friction_direction, -max_lambda_f, max_lambda_f);
|
|
}
|
|
}
|
|
|
|
// Clamp velocity of body 2
|
|
if (body2.IsDynamic())
|
|
{
|
|
MotionProperties *body2_mp = body2.GetMotionProperties();
|
|
body2_mp->ClampLinearVelocity();
|
|
body2_mp->ClampAngularVelocity();
|
|
}
|
|
}
|
|
else
|
|
{
|
|
SoftBodyMotionProperties *soft_mp = static_cast<SoftBodyMotionProperties *>(body2.GetMotionProperties());
|
|
const SoftBodyShape *soft_shape = static_cast<const SoftBodyShape *>(body2.GetShape());
|
|
|
|
// Convert the sub shape ID of the soft body to a face
|
|
uint32 face_idx = soft_shape->GetFaceIndex(ccd_body->mSubShapeID2);
|
|
const SoftBodyMotionProperties::Face &face = soft_mp->GetFace(face_idx);
|
|
|
|
// Get vertices of the face
|
|
SoftBodyMotionProperties::Vertex &vtx0 = soft_mp->GetVertex(face.mVertex[0]);
|
|
SoftBodyMotionProperties::Vertex &vtx1 = soft_mp->GetVertex(face.mVertex[1]);
|
|
SoftBodyMotionProperties::Vertex &vtx2 = soft_mp->GetVertex(face.mVertex[2]);
|
|
|
|
// Inverse mass of the face
|
|
float vtx0_mass = vtx0.mInvMass > 0.0f? 1.0f / vtx0.mInvMass : 1.0e10f;
|
|
float vtx1_mass = vtx1.mInvMass > 0.0f? 1.0f / vtx1.mInvMass : 1.0e10f;
|
|
float vtx2_mass = vtx2.mInvMass > 0.0f? 1.0f / vtx2.mInvMass : 1.0e10f;
|
|
float inv_m2 = 1.0f / (vtx0_mass + vtx1_mass + vtx2_mass);
|
|
|
|
// Calculate barycentric coordinates of the contact point on the soft body's face
|
|
float u, v, w;
|
|
RMat44 inv_body2_transform = body2.GetInverseCenterOfMassTransform();
|
|
Vec3 local_contact = Vec3(inv_body2_transform * ccd_body->mContactPointOn2);
|
|
ClosestPoint::GetBaryCentricCoordinates(vtx0.mPosition - local_contact, vtx1.mPosition - local_contact, vtx2.mPosition - local_contact, u, v, w);
|
|
|
|
// Calculate contact point velocity for the face
|
|
Vec3 v2 = inv_body2_transform.Multiply3x3Transposed(u * vtx0.mVelocity + v * vtx1.mVelocity + w * vtx2.mVelocity);
|
|
float normal_velocity = (v2 - v1).Dot(ccd_body->mContactNormal);
|
|
|
|
// Calculate velocity bias due to restitution
|
|
float normal_velocity_bias;
|
|
if (contact_settings.mCombinedRestitution > 0.0f && normal_velocity < -mPhysicsSettings.mMinVelocityForRestitution)
|
|
normal_velocity_bias = contact_settings.mCombinedRestitution * normal_velocity;
|
|
else
|
|
normal_velocity_bias = 0.0f;
|
|
|
|
// Calculate resulting velocity change (the math here is similar to AxisConstraintPart but without an inertia term for body 2 as we treat it as a point mass)
|
|
Vec3 r1_plus_u_x_n = r1_plus_u.Cross(ccd_body->mContactNormal);
|
|
Vec3 invi1_r1_plus_u_x_n = contact_settings.mInvInertiaScale1 * body1.GetInverseInertia().Multiply3x3(r1_plus_u_x_n);
|
|
float jv = r1_plus_u_x_n.Dot(body_mp->GetAngularVelocity()) - normal_velocity - normal_velocity_bias;
|
|
float inv_effective_mass = inv_m1 + inv_m2 + invi1_r1_plus_u_x_n.Dot(r1_plus_u_x_n);
|
|
float lambda = jv / inv_effective_mass;
|
|
body_mp->SubLinearVelocityStep((lambda * inv_m1) * ccd_body->mContactNormal);
|
|
body_mp->SubAngularVelocityStep(lambda * invi1_r1_plus_u_x_n);
|
|
Vec3 delta_v2 = inv_body2_transform.Multiply3x3(lambda * ccd_body->mContactNormal);
|
|
vtx0.mVelocity += delta_v2 * vtx0.mInvMass;
|
|
vtx1.mVelocity += delta_v2 * vtx1.mInvMass;
|
|
vtx2.mVelocity += delta_v2 * vtx2.mInvMass;
|
|
}
|
|
|
|
// Clamp velocity of body 1
|
|
body_mp->ClampLinearVelocity();
|
|
body_mp->ClampAngularVelocity();
|
|
|
|
// Activate the 2nd body if it is not already active
|
|
if (body2.IsDynamic() && !body2.IsActive())
|
|
{
|
|
bodies_to_activate[num_bodies_to_activate++] = ccd_body->mBodyID2;
|
|
if (num_bodies_to_activate == cBodiesBatch)
|
|
{
|
|
// Batch is full, activate now
|
|
mBodyManager.ActivateBodies(bodies_to_activate, num_bodies_to_activate);
|
|
num_bodies_to_activate = 0;
|
|
}
|
|
}
|
|
|
|
#ifdef JPH_DEBUG_RENDERER
|
|
if (sDrawMotionQualityLinearCast)
|
|
{
|
|
// Draw the collision location
|
|
RMat44 collision_transform = body1.GetCenterOfMassTransform().PostTranslated(ccd_body->mFraction * ccd_body->mDeltaPosition);
|
|
body1.GetShape()->Draw(DebugRenderer::sInstance, collision_transform, Vec3::sOne(), Color::sYellow, false, true);
|
|
|
|
// Draw the collision location + slop
|
|
RMat44 collision_transform_plus_slop = body1.GetCenterOfMassTransform().PostTranslated(ccd_body->mFractionPlusSlop * ccd_body->mDeltaPosition);
|
|
body1.GetShape()->Draw(DebugRenderer::sInstance, collision_transform_plus_slop, Vec3::sOne(), Color::sOrange, false, true);
|
|
|
|
// Draw contact normal
|
|
DebugRenderer::sInstance->DrawArrow(ccd_body->mContactPointOn2, ccd_body->mContactPointOn2 - ccd_body->mContactNormal, Color::sYellow, 0.1f);
|
|
|
|
// Draw post contact velocity
|
|
DebugRenderer::sInstance->DrawArrow(collision_transform.GetTranslation(), collision_transform.GetTranslation() + body1.GetLinearVelocity(), Color::sOrange, 0.1f);
|
|
DebugRenderer::sInstance->DrawArrow(collision_transform.GetTranslation(), collision_transform.GetTranslation() + body1.GetAngularVelocity(), Color::sPurple, 0.1f);
|
|
}
|
|
#endif // JPH_DEBUG_RENDERER
|
|
}
|
|
}
|
|
|
|
// Update body position
|
|
body1.AddPositionStep(ccd_body->mDeltaPosition * ccd_body->mFractionPlusSlop);
|
|
|
|
// If the body was activated due to an earlier CCD step it will have an index in the active
|
|
// body list that it higher than the highest one we processed during FindCollisions
|
|
// which means it hasn't been assigned an island and will not be updated by an island
|
|
// this means that we need to update its bounds manually
|
|
if (body_mp->GetIndexInActiveBodiesInternal() >= num_active_bodies_after_find_collisions)
|
|
{
|
|
body1.CalculateWorldSpaceBoundsInternal();
|
|
bodies_to_update_bounds[num_bodies_to_update_bounds++] = body1.GetID();
|
|
if (num_bodies_to_update_bounds == cBodiesBatch)
|
|
{
|
|
// Buffer full, flush now
|
|
mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false);
|
|
num_bodies_to_update_bounds = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Activate the requested bodies
|
|
if (num_bodies_to_activate > 0)
|
|
mBodyManager.ActivateBodies(bodies_to_activate, num_bodies_to_activate);
|
|
|
|
// Notify change bounds on requested bodies
|
|
if (num_bodies_to_update_bounds > 0)
|
|
mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false);
|
|
}
|
|
|
|
// Ensure we free the CCD bodies array now, will not call the destructor!
|
|
temp_allocator->Free(ioStep->mActiveBodyToCCDBody, ioStep->mNumActiveBodyToCCDBody * sizeof(int));
|
|
ioStep->mActiveBodyToCCDBody = nullptr;
|
|
ioStep->mNumActiveBodyToCCDBody = 0;
|
|
temp_allocator->Free(ioStep->mCCDBodies, ioStep->mCCDBodiesCapacity * sizeof(CCDBody));
|
|
ioStep->mCCDBodies = nullptr;
|
|
ioStep->mCCDBodiesCapacity = 0;
|
|
}
|
|
|
|
void PhysicsSystem::JobContactRemovedCallbacks(const PhysicsUpdateContext::Step *ioStep)
|
|
{
|
|
#ifdef JPH_ENABLE_ASSERTS
|
|
// We don't touch any bodies
|
|
BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::None);
|
|
#endif
|
|
|
|
// Reset the Body::EFlags::InvalidateContactCache flag for all bodies
|
|
mBodyManager.ValidateContactCacheForAllBodies();
|
|
|
|
// Finalize the contact cache (this swaps the read and write versions of the contact cache)
|
|
// Trigger all contact removed callbacks by looking at last step contact points that have not been flagged as reused
|
|
mContactManager.FinalizeContactCacheAndCallContactPointRemovedCallbacks(ioStep->mNumBodyPairs, ioStep->mNumManifolds);
|
|
}
|
|
|
|
class PhysicsSystem::BodiesToSleep : public NonCopyable
|
|
{
|
|
public:
|
|
static constexpr int cBodiesToSleepSize = 512;
|
|
static constexpr int cMaxBodiesToPutInBuffer = 128;
|
|
|
|
inline BodiesToSleep(BodyManager &inBodyManager, BodyID *inBodiesToSleepBuffer) : mBodyManager(inBodyManager), mBodiesToSleepBuffer(inBodiesToSleepBuffer), mBodiesToSleepCur(inBodiesToSleepBuffer) { }
|
|
|
|
inline ~BodiesToSleep()
|
|
{
|
|
// Flush the bodies to sleep buffer
|
|
int num_bodies_in_buffer = int(mBodiesToSleepCur - mBodiesToSleepBuffer);
|
|
if (num_bodies_in_buffer > 0)
|
|
mBodyManager.DeactivateBodies(mBodiesToSleepBuffer, num_bodies_in_buffer);
|
|
}
|
|
|
|
inline void PutToSleep(const BodyID *inBegin, const BodyID *inEnd)
|
|
{
|
|
int num_bodies_to_sleep = int(inEnd - inBegin);
|
|
if (num_bodies_to_sleep > cMaxBodiesToPutInBuffer)
|
|
{
|
|
// Too many bodies, deactivate immediately
|
|
mBodyManager.DeactivateBodies(inBegin, num_bodies_to_sleep);
|
|
}
|
|
else
|
|
{
|
|
// Check if there's enough space in the bodies to sleep buffer
|
|
int num_bodies_in_buffer = int(mBodiesToSleepCur - mBodiesToSleepBuffer);
|
|
if (num_bodies_in_buffer + num_bodies_to_sleep > cBodiesToSleepSize)
|
|
{
|
|
// Flush the bodies to sleep buffer
|
|
mBodyManager.DeactivateBodies(mBodiesToSleepBuffer, num_bodies_in_buffer);
|
|
mBodiesToSleepCur = mBodiesToSleepBuffer;
|
|
}
|
|
|
|
// Copy the bodies in the buffer
|
|
memcpy(mBodiesToSleepCur, inBegin, num_bodies_to_sleep * sizeof(BodyID));
|
|
mBodiesToSleepCur += num_bodies_to_sleep;
|
|
}
|
|
}
|
|
|
|
private:
|
|
BodyManager & mBodyManager;
|
|
BodyID * mBodiesToSleepBuffer;
|
|
BodyID * mBodiesToSleepCur;
|
|
};
|
|
|
|
void PhysicsSystem::CheckSleepAndUpdateBounds(uint32 inIslandIndex, const PhysicsUpdateContext *ioContext, const PhysicsUpdateContext::Step *ioStep, BodiesToSleep &ioBodiesToSleep)
|
|
{
|
|
// Get the bodies that belong to this island
|
|
BodyID *bodies_begin, *bodies_end;
|
|
mIslandBuilder.GetBodiesInIsland(inIslandIndex, bodies_begin, bodies_end);
|
|
|
|
// Only check sleeping in the last step
|
|
// Also resets force and torque used during the apply gravity phase
|
|
if (ioStep->mIsLast)
|
|
{
|
|
JPH_PROFILE("Check Sleeping");
|
|
|
|
static_assert(int(ECanSleep::CannotSleep) == 0 && int(ECanSleep::CanSleep) == 1, "Loop below makes this assumption");
|
|
int all_can_sleep = mPhysicsSettings.mAllowSleeping? int(ECanSleep::CanSleep) : int(ECanSleep::CannotSleep);
|
|
|
|
float time_before_sleep = mPhysicsSettings.mTimeBeforeSleep;
|
|
float max_movement = mPhysicsSettings.mPointVelocitySleepThreshold * time_before_sleep;
|
|
|
|
for (const BodyID *body_id = bodies_begin; body_id < bodies_end; ++body_id)
|
|
{
|
|
Body &body = mBodyManager.GetBody(*body_id);
|
|
|
|
// Update bounding box
|
|
body.CalculateWorldSpaceBoundsInternal();
|
|
|
|
// Update sleeping
|
|
all_can_sleep &= int(body.UpdateSleepStateInternal(ioContext->mStepDeltaTime, max_movement, time_before_sleep));
|
|
|
|
// Reset force and torque
|
|
MotionProperties *mp = body.GetMotionProperties();
|
|
mp->ResetForce();
|
|
mp->ResetTorque();
|
|
}
|
|
|
|
// If all bodies indicate they can sleep we can deactivate them
|
|
if (all_can_sleep == int(ECanSleep::CanSleep))
|
|
ioBodiesToSleep.PutToSleep(bodies_begin, bodies_end);
|
|
}
|
|
else
|
|
{
|
|
JPH_PROFILE("Update Bounds");
|
|
|
|
// Update bounding box only for all other steps
|
|
for (const BodyID *body_id = bodies_begin; body_id < bodies_end; ++body_id)
|
|
{
|
|
Body &body = mBodyManager.GetBody(*body_id);
|
|
body.CalculateWorldSpaceBoundsInternal();
|
|
}
|
|
}
|
|
|
|
// Notify broadphase of changed objects (find ccd contacts can do linear casts in the next step, so we need to do this every step)
|
|
// Note: Shuffles the BodyID's around!!!
|
|
mBroadPhase->NotifyBodiesAABBChanged(bodies_begin, int(bodies_end - bodies_begin), false);
|
|
}
|
|
|
|
void PhysicsSystem::JobSolvePositionConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep)
|
|
{
|
|
#ifdef JPH_ENABLE_ASSERTS
|
|
// We fix up position errors
|
|
BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::ReadWrite);
|
|
|
|
// Can only deactivate bodies
|
|
BodyManager::GrantActiveBodiesAccess grant_active(false, true);
|
|
#endif
|
|
|
|
float delta_time = ioContext->mStepDeltaTime;
|
|
float baumgarte = mPhysicsSettings.mBaumgarte;
|
|
Constraint **active_constraints = ioContext->mActiveConstraints;
|
|
|
|
// Keep a buffer of bodies that need to go to sleep in order to not constantly lock the active bodies mutex and create contention between all solving threads
|
|
BodiesToSleep bodies_to_sleep(mBodyManager, (BodyID *)JPH_STACK_ALLOC(BodiesToSleep::cBodiesToSleepSize * sizeof(BodyID)));
|
|
|
|
bool check_islands = true, check_split_islands = mPhysicsSettings.mUseLargeIslandSplitter;
|
|
for (;;)
|
|
{
|
|
// First try to get work from large islands
|
|
if (check_split_islands)
|
|
{
|
|
bool first_iteration;
|
|
uint split_island_index;
|
|
uint32 *constraints_begin, *constraints_end, *contacts_begin, *contacts_end;
|
|
switch (mLargeIslandSplitter.FetchNextBatch(split_island_index, constraints_begin, constraints_end, contacts_begin, contacts_end, first_iteration))
|
|
{
|
|
case LargeIslandSplitter::EStatus::BatchRetrieved:
|
|
// Solve the batch
|
|
ConstraintManager::sSolvePositionConstraints(active_constraints, constraints_begin, constraints_end, delta_time, baumgarte);
|
|
mContactManager.SolvePositionConstraints(contacts_begin, contacts_end);
|
|
|
|
// Mark the batch as processed
|
|
bool last_iteration, final_batch;
|
|
mLargeIslandSplitter.MarkBatchProcessed(split_island_index, constraints_begin, constraints_end, contacts_begin, contacts_end, last_iteration, final_batch);
|
|
|
|
// The final batch will update all bounds and check sleeping
|
|
if (final_batch)
|
|
CheckSleepAndUpdateBounds(mLargeIslandSplitter.GetIslandIndex(split_island_index), ioContext, ioStep, bodies_to_sleep);
|
|
|
|
// We processed work, loop again
|
|
continue;
|
|
|
|
case LargeIslandSplitter::EStatus::WaitingForBatch:
|
|
break;
|
|
|
|
case LargeIslandSplitter::EStatus::AllBatchesDone:
|
|
check_split_islands = false;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// If that didn't succeed try to process an island
|
|
if (check_islands)
|
|
{
|
|
// Next island
|
|
uint32 island_idx = ioStep->mSolvePositionConstraintsNextIsland++;
|
|
if (island_idx >= mIslandBuilder.GetNumIslands())
|
|
{
|
|
// We processed all islands, stop checking islands
|
|
check_islands = false;
|
|
continue;
|
|
}
|
|
|
|
JPH_PROFILE("Island");
|
|
|
|
// Get iterators for this island
|
|
uint32 *constraints_begin, *constraints_end, *contacts_begin, *contacts_end;
|
|
mIslandBuilder.GetConstraintsInIsland(island_idx, constraints_begin, constraints_end);
|
|
mIslandBuilder.GetContactsInIsland(island_idx, contacts_begin, contacts_end);
|
|
|
|
// If this island is a large island, it will be picked up as a batch and we don't need to do anything here
|
|
uint num_items = uint(constraints_end - constraints_begin) + uint(contacts_end - contacts_begin);
|
|
if (mPhysicsSettings.mUseLargeIslandSplitter
|
|
&& num_items >= LargeIslandSplitter::cLargeIslandTreshold)
|
|
continue;
|
|
|
|
// Check if this island needs solving
|
|
if (num_items > 0)
|
|
{
|
|
// Iterate
|
|
uint num_position_steps = mIslandBuilder.GetNumPositionSteps(island_idx);
|
|
for (uint position_step = 0; position_step < num_position_steps; ++position_step)
|
|
{
|
|
bool applied_impulse = ConstraintManager::sSolvePositionConstraints(active_constraints, constraints_begin, constraints_end, delta_time, baumgarte);
|
|
applied_impulse |= mContactManager.SolvePositionConstraints(contacts_begin, contacts_end);
|
|
if (!applied_impulse)
|
|
break;
|
|
}
|
|
}
|
|
|
|
// After solving we will update all bounds and check sleeping
|
|
CheckSleepAndUpdateBounds(island_idx, ioContext, ioStep, bodies_to_sleep);
|
|
|
|
// We processed work, loop again
|
|
continue;
|
|
}
|
|
|
|
if (check_islands)
|
|
{
|
|
// If there are islands, we don't need to wait and can pick up new work
|
|
continue;
|
|
}
|
|
else if (check_split_islands)
|
|
{
|
|
// If there are split islands, but we didn't do any work, give up a time slice
|
|
std::this_thread::yield();
|
|
}
|
|
else
|
|
{
|
|
// No more work
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
void PhysicsSystem::JobSoftBodyPrepare(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep)
|
|
{
|
|
JPH_PROFILE_FUNCTION();
|
|
|
|
{
|
|
#ifdef JPH_ENABLE_ASSERTS
|
|
// Reading soft body positions
|
|
BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::Read);
|
|
#endif
|
|
|
|
// Get the active soft bodies
|
|
BodyIDVector active_bodies;
|
|
mBodyManager.GetActiveBodies(EBodyType::SoftBody, active_bodies);
|
|
|
|
// Quit if there are no active soft bodies
|
|
if (active_bodies.empty())
|
|
{
|
|
// Kick the next step
|
|
if (ioStep->mStartNextStep.IsValid())
|
|
ioStep->mStartNextStep.RemoveDependency();
|
|
return;
|
|
}
|
|
|
|
// Sort to get a deterministic update order
|
|
QuickSort(active_bodies.begin(), active_bodies.end());
|
|
|
|
// Allocate soft body contexts
|
|
ioContext->mNumSoftBodies = (uint)active_bodies.size();
|
|
ioContext->mSoftBodyUpdateContexts = (SoftBodyUpdateContext *)ioContext->mTempAllocator->Allocate(ioContext->mNumSoftBodies * sizeof(SoftBodyUpdateContext));
|
|
|
|
// Initialize soft body contexts
|
|
for (SoftBodyUpdateContext *sb_ctx = ioContext->mSoftBodyUpdateContexts, *sb_ctx_end = ioContext->mSoftBodyUpdateContexts + ioContext->mNumSoftBodies; sb_ctx < sb_ctx_end; ++sb_ctx)
|
|
{
|
|
new (sb_ctx) SoftBodyUpdateContext;
|
|
Body &body = mBodyManager.GetBody(active_bodies[sb_ctx - ioContext->mSoftBodyUpdateContexts]);
|
|
SoftBodyMotionProperties *mp = static_cast<SoftBodyMotionProperties *>(body.GetMotionProperties());
|
|
mp->InitializeUpdateContext(ioContext->mStepDeltaTime, body, *this, *sb_ctx);
|
|
}
|
|
}
|
|
|
|
// We're ready to collide the first soft body
|
|
ioContext->mSoftBodyToCollide.store(0, memory_order_release);
|
|
|
|
// Determine number of jobs to spawn
|
|
int num_soft_body_jobs = ioContext->GetMaxConcurrency();
|
|
|
|
// Create finalize job
|
|
ioStep->mSoftBodyFinalize = ioContext->mJobSystem->CreateJob("SoftBodyFinalize", cColorSoftBodyFinalize, [ioContext, ioStep]()
|
|
{
|
|
ioContext->mPhysicsSystem->JobSoftBodyFinalize(ioContext);
|
|
|
|
// Kick the next step
|
|
if (ioStep->mStartNextStep.IsValid())
|
|
ioStep->mStartNextStep.RemoveDependency();
|
|
}, num_soft_body_jobs); // depends on: soft body simulate
|
|
ioContext->mBarrier->AddJob(ioStep->mSoftBodyFinalize);
|
|
|
|
// Create simulate jobs
|
|
ioStep->mSoftBodySimulate.resize(num_soft_body_jobs);
|
|
for (int i = 0; i < num_soft_body_jobs; ++i)
|
|
ioStep->mSoftBodySimulate[i] = ioContext->mJobSystem->CreateJob("SoftBodySimulate", cColorSoftBodySimulate, [ioStep, i]()
|
|
{
|
|
ioStep->mContext->mPhysicsSystem->JobSoftBodySimulate(ioStep->mContext, i);
|
|
|
|
ioStep->mSoftBodyFinalize.RemoveDependency();
|
|
}, num_soft_body_jobs); // depends on: soft body collide
|
|
ioContext->mBarrier->AddJobs(ioStep->mSoftBodySimulate.data(), ioStep->mSoftBodySimulate.size());
|
|
|
|
// Create collision jobs
|
|
ioStep->mSoftBodyCollide.resize(num_soft_body_jobs);
|
|
for (int i = 0; i < num_soft_body_jobs; ++i)
|
|
ioStep->mSoftBodyCollide[i] = ioContext->mJobSystem->CreateJob("SoftBodyCollide", cColorSoftBodyCollide, [ioContext, ioStep]()
|
|
{
|
|
ioContext->mPhysicsSystem->JobSoftBodyCollide(ioContext);
|
|
|
|
for (const JobHandle &h : ioStep->mSoftBodySimulate)
|
|
h.RemoveDependency();
|
|
}); // depends on: nothing
|
|
ioContext->mBarrier->AddJobs(ioStep->mSoftBodyCollide.data(), ioStep->mSoftBodyCollide.size());
|
|
}
|
|
|
|
void PhysicsSystem::JobSoftBodyCollide(PhysicsUpdateContext *ioContext) const
|
|
{
|
|
#ifdef JPH_ENABLE_ASSERTS
|
|
// Reading rigid body positions and velocities
|
|
BodyAccess::Grant grant(BodyAccess::EAccess::Read, BodyAccess::EAccess::Read);
|
|
#endif
|
|
|
|
for (;;)
|
|
{
|
|
// Fetch the next soft body
|
|
uint sb_idx = ioContext->mSoftBodyToCollide.fetch_add(1, std::memory_order_acquire);
|
|
if (sb_idx >= ioContext->mNumSoftBodies)
|
|
break;
|
|
|
|
// Do a broadphase check
|
|
SoftBodyUpdateContext &sb_ctx = ioContext->mSoftBodyUpdateContexts[sb_idx];
|
|
sb_ctx.mMotionProperties->DetermineCollidingShapes(sb_ctx, *this, GetBodyLockInterfaceNoLock());
|
|
}
|
|
}
|
|
|
|
void PhysicsSystem::JobSoftBodySimulate(PhysicsUpdateContext *ioContext, uint inThreadIndex) const
|
|
{
|
|
#ifdef JPH_ENABLE_ASSERTS
|
|
// Updating velocities of soft bodies, allow the contact listener to read the soft body state
|
|
BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::Read);
|
|
#endif
|
|
|
|
// Calculate at which body we start to distribute the workload across the threads
|
|
uint num_soft_bodies = ioContext->mNumSoftBodies;
|
|
uint start_idx = inThreadIndex * num_soft_bodies / ioContext->GetMaxConcurrency();
|
|
|
|
// Keep running partial updates until everything has been updated
|
|
uint status;
|
|
do
|
|
{
|
|
// Reset status
|
|
status = 0;
|
|
|
|
// Update all soft bodies
|
|
for (uint i = 0; i < num_soft_bodies; ++i)
|
|
{
|
|
// Fetch the soft body context
|
|
SoftBodyUpdateContext &sb_ctx = ioContext->mSoftBodyUpdateContexts[(start_idx + i) % num_soft_bodies];
|
|
|
|
// To avoid trashing the cache too much, we prefer to stick to one soft body until we cannot progress it any further
|
|
uint sb_status;
|
|
do
|
|
{
|
|
sb_status = (uint)sb_ctx.mMotionProperties->ParallelUpdate(sb_ctx, mPhysicsSettings);
|
|
status |= sb_status;
|
|
} while (sb_status == (uint)SoftBodyMotionProperties::EStatus::DidWork);
|
|
}
|
|
|
|
// If we didn't perform any work, yield the thread so that something else can run
|
|
if (!(status & (uint)SoftBodyMotionProperties::EStatus::DidWork))
|
|
std::this_thread::yield();
|
|
}
|
|
while (status != (uint)SoftBodyMotionProperties::EStatus::Done);
|
|
}
|
|
|
|
void PhysicsSystem::JobSoftBodyFinalize(PhysicsUpdateContext *ioContext)
|
|
{
|
|
#ifdef JPH_ENABLE_ASSERTS
|
|
// Updating rigid body velocities and soft body positions / velocities
|
|
BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::ReadWrite);
|
|
|
|
// Can activate and deactivate bodies
|
|
BodyManager::GrantActiveBodiesAccess grant_active(true, true);
|
|
#endif
|
|
|
|
static constexpr int cBodiesBatch = 64;
|
|
BodyID *bodies_to_update_bounds = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID));
|
|
int num_bodies_to_update_bounds = 0;
|
|
BodyID *bodies_to_put_to_sleep = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID));
|
|
int num_bodies_to_put_to_sleep = 0;
|
|
|
|
for (SoftBodyUpdateContext *sb_ctx = ioContext->mSoftBodyUpdateContexts, *sb_ctx_end = ioContext->mSoftBodyUpdateContexts + ioContext->mNumSoftBodies; sb_ctx < sb_ctx_end; ++sb_ctx)
|
|
{
|
|
// Apply the rigid body velocity deltas
|
|
sb_ctx->mMotionProperties->UpdateRigidBodyVelocities(*sb_ctx, GetBodyInterfaceNoLock());
|
|
|
|
// Update the position
|
|
sb_ctx->mBody->SetPositionAndRotationInternal(sb_ctx->mBody->GetPosition() + sb_ctx->mDeltaPosition, sb_ctx->mBody->GetRotation(), false);
|
|
|
|
BodyID id = sb_ctx->mBody->GetID();
|
|
bodies_to_update_bounds[num_bodies_to_update_bounds++] = id;
|
|
if (num_bodies_to_update_bounds == cBodiesBatch)
|
|
{
|
|
// Buffer full, flush now
|
|
mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false);
|
|
num_bodies_to_update_bounds = 0;
|
|
}
|
|
|
|
if (sb_ctx->mCanSleep == ECanSleep::CanSleep)
|
|
{
|
|
// This body should go to sleep
|
|
bodies_to_put_to_sleep[num_bodies_to_put_to_sleep++] = id;
|
|
if (num_bodies_to_put_to_sleep == cBodiesBatch)
|
|
{
|
|
mBodyManager.DeactivateBodies(bodies_to_put_to_sleep, num_bodies_to_put_to_sleep);
|
|
num_bodies_to_put_to_sleep = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Notify change bounds on requested bodies
|
|
if (num_bodies_to_update_bounds > 0)
|
|
mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false);
|
|
|
|
// Notify bodies to go to sleep
|
|
if (num_bodies_to_put_to_sleep > 0)
|
|
mBodyManager.DeactivateBodies(bodies_to_put_to_sleep, num_bodies_to_put_to_sleep);
|
|
|
|
// Free soft body contexts
|
|
ioContext->mTempAllocator->Free(ioContext->mSoftBodyUpdateContexts, ioContext->mNumSoftBodies * sizeof(SoftBodyUpdateContext));
|
|
}
|
|
|
|
void PhysicsSystem::SaveState(StateRecorder &inStream, EStateRecorderState inState, const StateRecorderFilter *inFilter) const
|
|
{
|
|
JPH_PROFILE_FUNCTION();
|
|
|
|
inStream.Write(inState);
|
|
|
|
if (uint8(inState) & uint8(EStateRecorderState::Global))
|
|
{
|
|
inStream.Write(mPreviousStepDeltaTime);
|
|
inStream.Write(mGravity);
|
|
}
|
|
|
|
if (uint8(inState) & uint8(EStateRecorderState::Bodies))
|
|
mBodyManager.SaveState(inStream, inFilter);
|
|
|
|
if (uint8(inState) & uint8(EStateRecorderState::Contacts))
|
|
mContactManager.SaveState(inStream, inFilter);
|
|
|
|
if (uint8(inState) & uint8(EStateRecorderState::Constraints))
|
|
mConstraintManager.SaveState(inStream, inFilter);
|
|
}
|
|
|
|
bool PhysicsSystem::RestoreState(StateRecorder &inStream, const StateRecorderFilter *inFilter)
|
|
{
|
|
JPH_PROFILE_FUNCTION();
|
|
|
|
EStateRecorderState state = EStateRecorderState::All; // Set this value for validation. If a partial state is saved, validation will not work anyway.
|
|
inStream.Read(state);
|
|
|
|
if (uint8(state) & uint8(EStateRecorderState::Global))
|
|
{
|
|
inStream.Read(mPreviousStepDeltaTime);
|
|
inStream.Read(mGravity);
|
|
}
|
|
|
|
if (uint8(state) & uint8(EStateRecorderState::Bodies))
|
|
{
|
|
if (!mBodyManager.RestoreState(inStream))
|
|
return false;
|
|
|
|
// Update bounding boxes for all bodies in the broadphase
|
|
if (inStream.IsLastPart())
|
|
{
|
|
Array<BodyID> bodies;
|
|
for (const Body *b : mBodyManager.GetBodies())
|
|
if (BodyManager::sIsValidBodyPointer(b) && b->IsInBroadPhase())
|
|
bodies.push_back(b->GetID());
|
|
if (!bodies.empty())
|
|
mBroadPhase->NotifyBodiesAABBChanged(&bodies[0], (int)bodies.size());
|
|
}
|
|
}
|
|
|
|
if (uint8(state) & uint8(EStateRecorderState::Contacts))
|
|
{
|
|
if (!mContactManager.RestoreState(inStream, inFilter))
|
|
return false;
|
|
}
|
|
|
|
if (uint8(state) & uint8(EStateRecorderState::Constraints))
|
|
{
|
|
if (!mConstraintManager.RestoreState(inStream))
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
void PhysicsSystem::SaveBodyState(const Body &inBody, StateRecorder &inStream) const
|
|
{
|
|
mBodyManager.SaveBodyState(inBody, inStream);
|
|
}
|
|
|
|
void PhysicsSystem::RestoreBodyState(Body &ioBody, StateRecorder &inStream)
|
|
{
|
|
mBodyManager.RestoreBodyState(ioBody, inStream);
|
|
|
|
BodyID id = ioBody.GetID();
|
|
mBroadPhase->NotifyBodiesAABBChanged(&id, 1);
|
|
}
|
|
|
|
JPH_NAMESPACE_END
|