// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) // SPDX-FileCopyrightText: 2021 Jorrit Rouwe // SPDX-License-Identifier: MIT #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #ifdef JPH_DEBUG_RENDERER #include #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) { JPH_ASSERT(inMaxBodies <= BodyID::cMaxBodyIndex, "Cannot support this many bodies"); mObjectVsBroadPhaseLayerFilter = &inObjectVsBroadPhaseLayerFilter; mObjectLayerPairFilter = &inObjectLayerPairFilter; // Initialize body manager mBodyManager.Init(inMaxBodies, 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(inMaxBodies); // 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 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) || inDeltaTime <= 0.0f) { mBodyManager.LockAllBodies(); // Update broadphase mBroadPhase->LockModifications(); BroadPhase::UpdateState update_state = mBroadPhase->UpdatePrepare(); mBroadPhase->UpdateFinalize(update_state); mBroadPhase->UnlockModifications(); // Call contact removal callbacks from contacts that existed in the previous update 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(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(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 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(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(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(); 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; BodyID *active_bodies = (BodyID *)JPH_STACK_ALLOC(batch_size * sizeof(BodyID)); 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::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 // If we want enhanced active edge detection for this body pair bool enhanced_active_edges = body1->GetEnhancedInternalEdgeRemovalWithBody(*body2); // Create the query settings CollideShapeSettings settings; settings.mCollectFacesMode = ECollectFacesMode::CollectFaces; settings.mActiveEdgeMode = mPhysicsSettings.mCheckActiveEdges && !enhanced_active_edges? 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; // 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, Square(settings.mSpeculativeContactDistance) + settings.mManifoldToleranceSq, 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 SubShapeIDCreator part1, part2; auto f = enhanced_active_edges? InternalEdgeRemovingCollector::sCollideShapeVsShape : CollisionDispatch::sCollideShapeVsShape; f(body1->GetShape(), body2->GetShape(), Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), transform1, transform2, part1, part2, 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, Square(settings.mSpeculativeContactDistance) + settings.mManifoldToleranceSq, 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 SubShapeIDCreator part1, part2; auto f = enhanced_active_edges? InternalEdgeRemovingCollector::sCollideShapeVsShape : CollisionDispatch::sCollideShapeVsShape; f(body1->GetShape(), body2->GetShape(), Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), transform1, transform2, part1, part2, 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::sReplicate(1.0f), 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::sReplicate(1.0f), 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::sReplicate(1.0f), 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.mManifoldToleranceSq, 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(body2.GetMotionProperties()); const SoftBodyShape *soft_shape = static_cast(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::sReplicate(1.0f), 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::sReplicate(1.0f), 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(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 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