// Schedule jobs to build Jacobians from the contacts stored in the simulation context
internal static SimulationJobHandles ScheduleBuildJacobiansJobs(ref PhysicsWorld world, float timeStep, float3 gravity,
int numSolverIterations, JobHandle inputDeps, ref NativeList <DispatchPairSequencer.DispatchPair> dispatchPairs,
ref DispatchPairSequencer.SolverSchedulerInfo solverSchedulerInfo,
ref NativeStream contacts, ref NativeStream jacobians, int threadCountHint = 0)
{
SimulationJobHandles returnHandles = default;
if (threadCountHint <= 0)
{
returnHandles.FinalExecutionHandle = new BuildJacobiansJob
{
ContactsReader = contacts.AsReader(),
JacobiansWriter = jacobians.AsWriter(),
TimeStep = timeStep,
Gravity = gravity,
NumSolverIterations = numSolverIterations,
World = world,
DispatchPairs = dispatchPairs.AsDeferredJobArray(),
SolverSchedulerInfo = solverSchedulerInfo
}.Schedule(inputDeps);
}
else
{
var buildJob = new BuildJacobiansJob
{
ContactsReader = contacts.AsReader(),
JacobiansWriter = jacobians.AsWriter(),
TimeStep = timeStep,
InvTimeStep = timeStep > 0.0f ? 1.0f / timeStep : 0.0f,
GravityAcceleration = math.length(gravity),
NumSolverIterations = numSolverIterations,
World = world,
DispatchPairs = dispatchPairs.AsDeferredJobArray(),
SolverSchedulerInfo = solverSchedulerInfo
};
JobHandle handle = buildJob.ScheduleUnsafeIndex0(solverSchedulerInfo.NumWorkItems, 1, inputDeps);
returnHandles.FinalDisposeHandle = JobHandle.CombineDependencies(
dispatchPairs.Dispose(handle),
contacts.Dispose(handle));
returnHandles.FinalExecutionHandle = handle;
}
return(returnHandles);
}
// Schedules a set of jobs to iterate the provided dispatch pairs and create contacts based on them.
internal static SimulationJobHandles ScheduleCreateContactsJobs(ref PhysicsWorld world, float timeStep,
ref NativeStream contacts, ref NativeStream jacobians, ref NativeList <DispatchPairSequencer.DispatchPair> dispatchPairs,
JobHandle inputDeps, ref DispatchPairSequencer.SolverSchedulerInfo solverSchedulerInfo, int threadCountHint = 0)
{
SimulationJobHandles returnHandles = default;
if (threadCountHint <= 0)
{
contacts = new NativeStream(1, Allocator.TempJob);
jacobians = new NativeStream(1, Allocator.TempJob);
returnHandles.FinalExecutionHandle = new CreateContactsJob
{
World = world,
TimeStep = timeStep,
DispatchPairs = dispatchPairs.AsDeferredJobArray(),
SolverSchedulerInfo = solverSchedulerInfo,
ContactsWriter = contacts.AsWriter()
}.Schedule(inputDeps);
}
else
{
var numWorkItems = solverSchedulerInfo.NumWorkItems;
var contactsHandle = NativeStream.ScheduleConstruct(out contacts, numWorkItems, inputDeps, Allocator.TempJob);
var jacobiansHandle = NativeStream.ScheduleConstruct(out jacobians, numWorkItems, inputDeps, Allocator.TempJob);
var processHandle = new CreateContactsJob
{
World = world,
TimeStep = timeStep,
DispatchPairs = dispatchPairs.AsDeferredJobArray(),
SolverSchedulerInfo = solverSchedulerInfo,
ContactsWriter = contacts.AsWriter()
}.ScheduleUnsafeIndex0(numWorkItems, 1, JobHandle.CombineDependencies(contactsHandle, jacobiansHandle));
returnHandles.FinalExecutionHandle = processHandle;
}
return(returnHandles);
}
// Schedule a set of jobs which will write all overlapping body pairs to the given steam,
// where at least one of the bodies is dynamic. The results are unsorted.
public SimulationJobHandles ScheduleFindOverlapsJobs(out NativeStream dynamicVsDynamicPairsStream, out NativeStream staticVsDynamicPairsStream,
JobHandle inputDeps, int threadCountHint = 0)
{
SimulationJobHandles returnHandles = default;
if (threadCountHint <= 0)
{
dynamicVsDynamicPairsStream = new NativeStream(1, Allocator.TempJob);
staticVsDynamicPairsStream = new NativeStream(1, Allocator.TempJob);
returnHandles.FinalExecutionHandle = new FindOverlapsJob
{
Broadphase = this,
DynamicVsDynamicPairsWriter = dynamicVsDynamicPairsStream.AsWriter(),
StaticVsDynamicPairsWriter = staticVsDynamicPairsStream.AsWriter()
}.Schedule(inputDeps);
return(returnHandles);
}
var dynamicVsDynamicNodePairIndices = new NativeList <int2>(Allocator.TempJob);
var staticVsDynamicNodePairIndices = new NativeList <int2>(Allocator.TempJob);
JobHandle allocateDeps = new AllocateDynamicVsStaticNodePairs
{
dynamicVsDynamicNodePairIndices = dynamicVsDynamicNodePairIndices,
staticVsDynamicNodePairIndices = staticVsDynamicNodePairIndices,
dynamicBranchCount = m_DynamicTree.BranchCount,
staticBranchCount = m_StaticTree.BranchCount
}.Schedule(inputDeps);
// Build pairs of branch node indices
JobHandle dynamicVsDynamicPairs = new DynamicVsDynamicBuildBranchNodePairsJob
{
Ranges = m_DynamicTree.Ranges,
NumBranches = m_DynamicTree.BranchCount,
NodePairIndices = dynamicVsDynamicNodePairIndices.AsDeferredJobArray()
}.Schedule(allocateDeps);
JobHandle staticVsDynamicPairs = new StaticVsDynamicBuildBranchNodePairsJob
{
DynamicRanges = m_DynamicTree.Ranges,
StaticRanges = m_StaticTree.Ranges,
NumStaticBranches = m_StaticTree.BranchCount,
NumDynamicBranches = m_DynamicTree.BranchCount,
NodePairIndices = staticVsDynamicNodePairIndices.AsDeferredJobArray()
}.Schedule(allocateDeps);
//@TODO: We only need a dependency on allocateDeps, but the safety system doesn't understand that we can not change length list in DynamicVsDynamicBuildBranchNodePairsJob & StaticVsDynamicBuildBranchNodePairsJob
// if this is a performance issue we can use [NativeDisableContainerSafetyRestriction] on DynamicVsDynamicBuildBranchNodePairsJob & StaticVsDynamicBuildBranchNodePairsJob
JobHandle dynamicConstruct = NativeStream.ScheduleConstruct(out dynamicVsDynamicPairsStream, dynamicVsDynamicNodePairIndices, dynamicVsDynamicPairs, Allocator.TempJob);
JobHandle staticConstruct = NativeStream.ScheduleConstruct(out staticVsDynamicPairsStream, staticVsDynamicNodePairIndices, staticVsDynamicPairs, Allocator.TempJob);
// Write all overlaps to the stream (also deallocates nodePairIndices)
JobHandle dynamicVsDynamicHandle = new DynamicVsDynamicFindOverlappingPairsJob
{
DynamicTree = m_DynamicTree,
PairWriter = dynamicVsDynamicPairsStream.AsWriter(),
NodePairIndices = dynamicVsDynamicNodePairIndices.AsDeferredJobArray()
}.Schedule(dynamicVsDynamicNodePairIndices, 1, JobHandle.CombineDependencies(dynamicVsDynamicPairs, dynamicConstruct));
// Write all overlaps to the stream (also deallocates nodePairIndices)
JobHandle staticVsDynamicHandle = new StaticVsDynamicFindOverlappingPairsJob
{
StaticTree = m_StaticTree,
DynamicTree = m_DynamicTree,
PairWriter = staticVsDynamicPairsStream.AsWriter(),
NodePairIndices = staticVsDynamicNodePairIndices.AsDeferredJobArray()
}.Schedule(staticVsDynamicNodePairIndices, 1, JobHandle.CombineDependencies(staticVsDynamicPairs, staticConstruct));
// Dispose node pair lists
var disposeOverlapPairs0 = NativeListUtilityTemp.DisposeHotFix(ref dynamicVsDynamicNodePairIndices, dynamicVsDynamicHandle);
var disposeOverlapPairs1 = NativeListUtilityTemp.DisposeHotFix(ref staticVsDynamicNodePairIndices, staticVsDynamicHandle);
returnHandles.FinalDisposeHandle = JobHandle.CombineDependencies(disposeOverlapPairs0, disposeOverlapPairs1);
returnHandles.FinalExecutionHandle = JobHandle.CombineDependencies(dynamicVsDynamicHandle, staticVsDynamicHandle);
return(returnHandles);
}
public void ScheduleStepJobs(SimulationStepInput input, JobHandle inputDeps)
{
m_StepHandles = ScheduleStepJobs(input, null, inputDeps, input.ThreadCountHint);
}
// Schedule all the jobs for the simulation step.
// Enqueued callbacks can choose to inject additional jobs at defined sync points.
// threadCountHint defines which simulation type will be called:
// - threadCountHint > 0 will result in default multithreaded simulation
// - threadCountHint <=0 will result in a very small number of jobs (1 per physics step phase) that are scheduled sequentially
// Behavior doesn't change regardless of the threadCountHint provided.
public unsafe SimulationJobHandles ScheduleStepJobs(SimulationStepInput input, SimulationCallbacks callbacksIn, JobHandle inputDeps, int threadCountHint = 0)
{
if (input.TimeStep < 0)
{
throw new ArgumentOutOfRangeException();
}
if (input.NumSolverIterations <= 0)
{
throw new ArgumentOutOfRangeException();
}
bool singleThreadedSim = (threadCountHint <= 0);
// Dispose and reallocate input velocity buffer, if dynamic body count has increased.
// Dispose previous collision and trigger event data streams.
// New event streams are reallocated later when the work item count is known.
JobHandle handle = SimulationContext.ScheduleReset(ref input.World, inputDeps, false);
SimulationContext.TimeStep = input.TimeStep;
StepContext = new StepContext();
if (input.World.NumDynamicBodies == 0)
{
// No need to do anything, since nothing can move
m_StepHandles = new SimulationJobHandles(handle);
return(m_StepHandles);
}
SimulationCallbacks callbacks = callbacksIn ?? new SimulationCallbacks();
// Find all body pairs that overlap in the broadphase
var handles = input.World.CollisionWorld.ScheduleFindOverlapsJobs(
out NativeStream dynamicVsDynamicBodyPairs, out NativeStream dynamicVsStaticBodyPairs, handle, threadCountHint);
handle = handles.FinalExecutionHandle;
var disposeHandle1 = handles.FinalDisposeHandle;
var postOverlapsHandle = handle;
// Sort all overlapping and jointed body pairs into phases
handles = m_Scheduler.ScheduleCreatePhasedDispatchPairsJob(
ref input.World, ref dynamicVsDynamicBodyPairs, ref dynamicVsStaticBodyPairs, handle,
ref StepContext.PhasedDispatchPairs, out StepContext.SolverSchedulerInfo, threadCountHint);
handle = handles.FinalExecutionHandle;
var disposeHandle2 = handles.FinalDisposeHandle;
// Apply gravity and copy input velocities at this point (in parallel with the scheduler, but before the callbacks)
var applyGravityAndCopyInputVelocitiesHandle = Solver.ScheduleApplyGravityAndCopyInputVelocitiesJob(
ref input.World.DynamicsWorld, SimulationContext.InputVelocities, input.TimeStep * input.Gravity, singleThreadedSim ? handle : postOverlapsHandle, threadCountHint);
handle = JobHandle.CombineDependencies(handle, applyGravityAndCopyInputVelocitiesHandle);
handle = callbacks.Execute(SimulationCallbacks.Phase.PostCreateDispatchPairs, this, ref input.World, handle);
// Create contact points & joint Jacobians
handles = NarrowPhase.ScheduleCreateContactsJobs(ref input.World, input.TimeStep,
ref StepContext.Contacts, ref StepContext.Jacobians, ref StepContext.PhasedDispatchPairs, handle,
ref StepContext.SolverSchedulerInfo, threadCountHint);
handle = handles.FinalExecutionHandle;
var disposeHandle3 = handles.FinalDisposeHandle;
handle = callbacks.Execute(SimulationCallbacks.Phase.PostCreateContacts, this, ref input.World, handle);
// Create contact Jacobians
handles = Solver.ScheduleBuildJacobiansJobs(ref input.World, input.TimeStep, input.Gravity, input.NumSolverIterations,
handle, ref StepContext.PhasedDispatchPairs, ref StepContext.SolverSchedulerInfo,
ref StepContext.Contacts, ref StepContext.Jacobians, threadCountHint);
handle = handles.FinalExecutionHandle;
var disposeHandle4 = handles.FinalDisposeHandle;
handle = callbacks.Execute(SimulationCallbacks.Phase.PostCreateContactJacobians, this, ref input.World, handle);
// Solve all Jacobians
handles = Solver.ScheduleSolveJacobiansJobs(ref input.World.DynamicsWorld, input.TimeStep, input.NumSolverIterations,
ref StepContext.Jacobians, ref SimulationContext.CollisionEventDataStream, ref SimulationContext.TriggerEventDataStream,
ref StepContext.SolverSchedulerInfo, handle, threadCountHint);
handle = handles.FinalExecutionHandle;
var disposeHandle5 = handles.FinalDisposeHandle;
handle = callbacks.Execute(SimulationCallbacks.Phase.PostSolveJacobians, this, ref input.World, handle);
// Integrate motions
handle = Integrator.ScheduleIntegrateJobs(ref input.World.DynamicsWorld, input.TimeStep, handle, threadCountHint);
// Synchronize the collision world
if (input.SynchronizeCollisionWorld)
{
handle = input.World.CollisionWorld.ScheduleUpdateDynamicTree(ref input.World, input.TimeStep, input.Gravity, handle, threadCountHint); // TODO: timeStep = 0?
}
// Return the final simulation handle
m_StepHandles.FinalExecutionHandle = handle;
// Different dispose logic for single threaded simulation compared to "standard" threading (multi threaded)
if (singleThreadedSim)
{
handle = dynamicVsDynamicBodyPairs.Dispose(handle);
handle = dynamicVsStaticBodyPairs.Dispose(handle);
handle = StepContext.PhasedDispatchPairs.Dispose(handle);
handle = StepContext.Contacts.Dispose(handle);
handle = StepContext.Jacobians.Dispose(handle);
handle = StepContext.SolverSchedulerInfo.ScheduleDisposeJob(handle);
m_StepHandles.FinalDisposeHandle = handle;
}
else
{
// Return the final handle, which includes disposing temporary arrays
JobHandle *deps = stackalloc JobHandle[5]
{
disposeHandle1,
disposeHandle2,
disposeHandle3,
disposeHandle4,
disposeHandle5
};
m_StepHandles.FinalDisposeHandle = JobHandleUnsafeUtility.CombineDependencies(deps, 5);
}
return(m_StepHandles);
}
// Schedule jobs to solve the Jacobians stored in the simulation context
internal static unsafe SimulationJobHandles ScheduleSolveJacobiansJobs(ref DynamicsWorld dynamicsWorld, float timestep, int numIterations,
ref NativeStream jacobians, ref NativeStream collisionEvents, ref NativeStream triggerEvents,
ref DispatchPairSequencer.SolverSchedulerInfo solverSchedulerInfo, JobHandle inputDeps, int threadCountHint = 0)
{
SimulationJobHandles returnHandles = default;
if (threadCountHint <= 0)
{
collisionEvents = new NativeStream(1, Allocator.Persistent);
triggerEvents = new NativeStream(1, Allocator.Persistent);
returnHandles.FinalExecutionHandle = new SolverJob
{
CollisionEventsWriter = collisionEvents.AsWriter(),
JacobiansReader = jacobians.AsReader(),
NumIterations = numIterations,
Timestep = timestep,
TriggerEventsWriter = triggerEvents.AsWriter(),
MotionVelocities = dynamicsWorld.MotionVelocities,
Phases = solverSchedulerInfo.PhaseInfo
}.Schedule(inputDeps);
return(returnHandles);
}
JobHandle handle;
int numPhases = solverSchedulerInfo.NumPhases;
// Use persistent allocator to allow these to live until the start of next step
{
NativeArray <int> workItemList = solverSchedulerInfo.NumWorkItems;
//TODO: Change this to Allocator.TempJob when https://github.com/Unity-Technologies/Unity.Physics/issues/7 is resolved
JobHandle collisionEventStreamHandle = NativeStream.ScheduleConstruct(out collisionEvents, workItemList, inputDeps, Allocator.Persistent);
JobHandle triggerEventStreamHandle = NativeStream.ScheduleConstruct(out triggerEvents, workItemList, inputDeps, Allocator.Persistent);
handle = JobHandle.CombineDependencies(collisionEventStreamHandle, triggerEventStreamHandle);
float invNumIterations = math.rcp(numIterations);
var phaseInfoPtrs = (DispatchPairSequencer.SolverSchedulerInfo.SolvePhaseInfo *)NativeArrayUnsafeUtility.GetUnsafeBufferPointerWithoutChecks(solverSchedulerInfo.PhaseInfo);
for (int solverIterationId = 0; solverIterationId < numIterations; solverIterationId++)
{
bool lastIteration = solverIterationId == numIterations - 1;
for (int phaseId = 0; phaseId < numPhases; phaseId++)
{
var job = new SolverJob
{
JacobiansReader = jacobians.AsReader(),
PhaseIndex = phaseId,
Phases = solverSchedulerInfo.PhaseInfo,
MotionVelocities = dynamicsWorld.MotionVelocities,
StepInput = new StepInput
{
InvNumSolverIterations = invNumIterations,
IsLastIteration = lastIteration,
Timestep = timestep,
InvTimestep = timestep > 0.0f ? 1.0f / timestep : 0.0f
}
};
// Only initialize event writers for last solver iteration jobs
if (lastIteration)
{
job.CollisionEventsWriter = collisionEvents.AsWriter();
job.TriggerEventsWriter = triggerEvents.AsWriter();
}
// NOTE: The last phase must be executed on a single job since it
// int.MaxValue can't be used as batchSize since 19.1 overflows in that case...
bool isLastPhase = phaseId == numPhases - 1;
int batchSize = isLastPhase ? (int.MaxValue / 2) : 1;
int *numWorkItems = &(phaseInfoPtrs[phaseId].NumWorkItems);
handle = job.Schedule(numWorkItems, batchSize, handle);
}
}
}
// Dispose processed data
returnHandles.FinalDisposeHandle = JobHandle.CombineDependencies(
jacobians.Dispose(handle),
solverSchedulerInfo.ScheduleDisposeJob(handle));
returnHandles.FinalExecutionHandle = handle;
return(returnHandles);
}
