// Schedule some jobs to solve the Jacobians stored in the simulation context
internal static unsafe JobHandle ScheduleSolveJacobiansJobs(ref DynamicsWorld dynamicsWorld, float timestep, int numIterations, ref Simulation.Context context, JobHandle inputDeps)
{
JobHandle handle;
int numPhases = context.SolverSchedulerInfo.NumPhases;
// Use persistent allocator to allow these to live until the start of next step
{
NativeArray <int> workItemList = context.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 context.CollisionEventStream, workItemList, inputDeps, Allocator.Persistent);
JobHandle triggerEventStreamHandle = NativeStream.ScheduleConstruct(out context.TriggerEventStream, workItemList, inputDeps, Allocator.Persistent);
handle = JobHandle.CombineDependencies(collisionEventStreamHandle, triggerEventStreamHandle);
float invNumIterations = math.rcp(numIterations);
var phaseInfoPtrs = (Scheduler.SolverSchedulerInfo.SolvePhaseInfo *)NativeArrayUnsafeUtility.GetUnsafeBufferPointerWithoutChecks(context.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
{
JacobianReader = context.Jacobians.AsReader(),
PhaseIndex = phaseId,
Phases = context.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 = context.CollisionEventStream.AsWriter();
job.TriggerEventsWriter = context.TriggerEventStream.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
context.DisposeJacobians = context.Jacobians.Dispose(handle);
context.DisposeJointJacobians = context.JointJacobians.Dispose(handle);
context.DisposeSolverSchedulerData = context.SolverSchedulerInfo.ScheduleDisposeJob(handle);
return(handle);
}
// 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.
internal JobHandle ScheduleFindOverlapsJobs(out BlockStream dynamicVsDynamicPairsStream, out BlockStream staticVsDynamicPairsStream, ref Simulation.Context context, JobHandle inputDeps)
{
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.m_BranchCount,
staticBranchCount = m_StaticTree.m_BranchCount
}.Schedule(inputDeps);
// Build pairs of branch node indices
JobHandle dynamicVsDynamicPairs = new DynamicVsDynamicBuildBranchNodePairsJob
{
Ranges = m_DynamicTree.Ranges,
NumBranches = m_DynamicTree.m_BranchCount,
NodePairIndices = dynamicVsDynamicNodePairIndices.AsDeferredJobArray()
}.Schedule(allocateDeps);
JobHandle staticVsDynamicPairs = new StaticVsDynamicBuildBranchNodePairsJob
{
DynamicRanges = m_DynamicTree.Ranges,
StaticRanges = m_StaticTree.Ranges,
NumStaticBranches = m_StaticTree.m_BranchCount,
NumDynamicBranches = m_DynamicTree.m_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 = BlockStream.ScheduleConstruct(out dynamicVsDynamicPairsStream, dynamicVsDynamicNodePairIndices, 0x0a542b34, dynamicVsDynamicPairs);
JobHandle staticConstruct = BlockStream.ScheduleConstruct(out staticVsDynamicPairsStream, staticVsDynamicNodePairIndices, 0x0a542666, staticVsDynamicPairs);
// Write all overlaps to the stream (also deallocates nodePairIndices)
JobHandle dynamicVsDynamicHandle = new DynamicVsDynamicFindOverlappingPairsJob
{
DynamicNodes = m_DynamicTree.Nodes,
BodyFilters = m_BodyFilters,
DynamicNodeFilters = m_DynamicTree.NodeFilters,
PairWriter = dynamicVsDynamicPairsStream,
NodePairIndices = dynamicVsDynamicNodePairIndices.AsDeferredJobArray()
}.Schedule(dynamicVsDynamicNodePairIndices, 1, JobHandle.CombineDependencies(dynamicVsDynamicPairs, dynamicConstruct));
// Write all overlaps to the stream (also deallocates nodePairIndices)
JobHandle staticVsDynamicHandle = new StaticVsDynamicFindOverlappingPairsJob
{
StaticNodes = m_StaticTree.Nodes,
DynamicNodes = m_DynamicTree.Nodes,
BodyFilters = m_BodyFilters,
StaticNodeFilters = m_StaticTree.NodeFilters,
DynamicNodeFilters = m_DynamicTree.NodeFilters,
PairWriter = staticVsDynamicPairsStream,
NodePairIndices = staticVsDynamicNodePairIndices.AsDeferredJobArray()
}.Schedule(staticVsDynamicNodePairIndices, 1, JobHandle.CombineDependencies(staticVsDynamicPairs, staticConstruct));
// Dispose node pair lists
context.DisposeOverlapPairs0 = NativeListUtilityTemp.DisposeHotFix(ref dynamicVsDynamicNodePairIndices, dynamicVsDynamicHandle);
context.DisposeOverlapPairs1 = NativeListUtilityTemp.DisposeHotFix(ref staticVsDynamicNodePairIndices, staticVsDynamicHandle);
return(JobHandle.CombineDependencies(dynamicVsDynamicHandle, staticVsDynamicHandle));
}
// Schedule some jobs to build Jacobians from the contacts stored in the simulation context
internal static JobHandle ScheduleBuildContactJacobiansJobs(ref DynamicsWorld world, float timeStep, float gravityAcceleration, ref Simulation.Context context, JobHandle inputDeps)
{
var buildJob = new BuildContactJacobiansJob
{
ContactReader = context.Contacts.AsReader(),
JointJacobianReader = context.JointJacobians.AsReader(),
JacobianWriter = context.Jacobians.AsWriter(),
TimeStep = timeStep,
InvTimeStep = timeStep > 0.0f ? 1.0f / timeStep : 0.0f,
GravityAcceleration = gravityAcceleration,
MotionDatas = world.MotionDatas,
MotionVelocities = world.MotionVelocities
};
JobHandle handle = buildJob.ScheduleUnsafeIndex0(context.SolverSchedulerInfo.NumWorkItems, 1, inputDeps);
context.DisposeContacts = context.Contacts.Dispose(handle);
return(handle);
}
public static JobHandle ScheduleProcessBodyPairsJobs(ref PhysicsWorld world, float timeStep, int numIterations, ref Simulation.Context context, JobHandle inputDeps)
{
//<todo.eoin.usermod Can we get rid of this max()? Needed if the user wants to add contacts themselves.
int numWorkItems = math.max(1, context.SolverSchedulerInfo.NumWorkItems);
context.Contacts = new BlockStream(numWorkItems, 0xcf97529c);
context.JointJacobians = new BlockStream(numWorkItems, 0xd3185f82);
context.Jacobians = new BlockStream(numWorkItems, 0x8d8f394d);
return(new ProcessBodyPairsJob
{
World = world,
TimeStep = timeStep,
NumIterations = numIterations,
PhasedDispatchPairs = context.PhasedDispatchPairsArray,
SolverSchedulerInfo = context.SolverSchedulerInfo,
ContactWriter = context.Contacts,
JointJacobianWriter = context.JointJacobians
}.Schedule(numWorkItems, 1, inputDeps));
}
internal static JobHandle ScheduleProcessBodyPairsJobs(ref PhysicsWorld world, float timeStep, int numIterations, ref Simulation.Context context, JobHandle inputDeps)
{
var numWorkItems = context.SolverSchedulerInfo.NumWorkItems;
var contactsHandle = NativeStream.ScheduleConstruct(out context.Contacts, numWorkItems, inputDeps, Allocator.TempJob);
var jointJacobiansHandle = NativeStream.ScheduleConstruct(out context.JointJacobians, numWorkItems, inputDeps, Allocator.TempJob);
var jacobiansHandle = NativeStream.ScheduleConstruct(out context.Jacobians, numWorkItems, inputDeps, Allocator.TempJob);
var processHandle = new ProcessBodyPairsJob
{
World = world,
TimeStep = timeStep,
NumIterations = numIterations,
PhasedDispatchPairs = context.PhasedDispatchPairs.AsDeferredJobArray(),
SolverSchedulerInfo = context.SolverSchedulerInfo,
ContactWriter = context.Contacts.AsWriter(),
JointJacobianWriter = context.JointJacobians.AsWriter(),
}.ScheduleUnsafeIndex0(numWorkItems, 1, JobHandle.CombineDependencies(contactsHandle, jointJacobiansHandle, jacobiansHandle));
context.DisposeProcessBodyPairs = NativeListUtilityTemp.DisposeHotFix(ref context.PhasedDispatchPairs, processHandle);
return(processHandle);
}
// Schedule some jobs to solve the Jacobians stored in the simulation context
public static unsafe JobHandle ScheduleSolveJacobiansJobs(ref DynamicsWorld dynamicsWorld, float timestep, float3 gravity, int numIterations, ref Simulation.Context context, JobHandle inputDeps)
{
JobHandle handle = inputDeps;
int numPhases = context.SolverSchedulerInfo.NumPhases;
// Use persistent allocator to allow these to live until the start of next step
int numWorkItems = math.max(context.SolverSchedulerInfo.NumWorkItems, 1); // Need at least one work item if user is going to add contacts
{
context.CollisionEventStream = new BlockStream(numWorkItems, 0xb17b474f, Allocator.Persistent);
context.TriggerEventStream = new BlockStream(numWorkItems, 0x43875d8f, Allocator.Persistent);
float invNumIterations = math.rcp(numIterations);
float gravityLength = math.length(gravity);
for (int solverIterationId = 0; solverIterationId < numIterations; solverIterationId++)
{
bool lastIteration = solverIterationId == numIterations - 1;
for (int phaseId = 0; phaseId < numPhases; phaseId++)
{
int numWorkItemsPerPhase = context.SolverSchedulerInfo.NumWorkItemsPerPhase(phaseId);
if (numWorkItemsPerPhase == 0)
{
continue;
}
var job = new SolverJob
{
JacobianReader = context.Jacobians,
WorkItemStartIndexOffset = context.SolverSchedulerInfo.FirstWorkItemsPerPhase(phaseId),
MotionVelocities = dynamicsWorld.MotionVelocities,
StepInput = new StepInput
{
InvNumSolverIterations = invNumIterations,
IsLastIteration = lastIteration,
Timestep = timestep,
GravityLength = gravityLength
}
};
// Only initialize event writers for last solver iteration jobs
if (lastIteration)
{
job.CollisionEventsWriter = context.CollisionEventStream;
job.TriggerEventsWriter = context.TriggerEventStream;
}
bool isLastPhase = phaseId == numPhases - 1;
int batchSize = isLastPhase ? numWorkItemsPerPhase : 1;
handle = job.Schedule(numWorkItemsPerPhase, batchSize, handle);
}
}
}
// Dispose processed data
context.DisposeJacobians = context.Jacobians.ScheduleDispose(handle);
context.DisposeJointJacobians = context.JointJacobians.ScheduleDispose(handle);
context.DisposeSolverSchedulerData = context.SolverSchedulerInfo.ScheduleDisposeJob(handle);
return(handle);
}
// Schedule some jobs to build Jacobians from the contacts stored in the simulation context
public static JobHandle ScheduleBuildContactJacobiansJobs(ref DynamicsWorld world, float timeStep, ref Simulation.Context context, JobHandle inputDeps)
{
var buildJob = new BuildContactJacobiansJob
{
ContactReader = context.Contacts,
JointJacobianReader = context.JointJacobians,
JacobianWriter = context.Jacobians,
TimeStep = timeStep,
MotionDatas = world.MotionDatas,
MotionVelocities = world.MotionVelocities
};
int numWorkItems = context.SolverSchedulerInfo.NumWorkItems;
JobHandle handle = buildJob.Schedule(numWorkItems, 1, inputDeps);
context.DisposeContacts = context.Contacts.ScheduleDispose(handle);
return(handle);
}
