// Clone the world
public DynamicsWorld Clone()
{
DynamicsWorld clone = new DynamicsWorld
{
m_MotionDatas = new NativeArray <MotionData>(m_MotionDatas.Length, Allocator.Persistent, NativeArrayOptions.UninitializedMemory),
m_MotionVelocities = new NativeArray <MotionVelocity>(m_MotionVelocities.Length, Allocator.Persistent, NativeArrayOptions.UninitializedMemory),
m_NumMotions = m_NumMotions,
m_Joints = new NativeArray <Joint>(m_Joints.Length, Allocator.Persistent, NativeArrayOptions.UninitializedMemory),
m_NumJoints = m_NumJoints,
EntityJointIndexMap = new NativeHashMap <Entity, int>(m_Joints.Length, Allocator.Persistent),
};
clone.m_MotionDatas.CopyFrom(m_MotionDatas);
clone.m_MotionVelocities.CopyFrom(m_MotionVelocities);
clone.m_Joints.CopyFrom(m_Joints);
clone.UpdateJointIndexMap();
return(clone);
}
// Schedule a job to integrate the world's motions forward by the given time step.
internal static JobHandle ScheduleIntegrateJobs(ref DynamicsWorld world, float timeStep, JobHandle inputDeps, int threadCountHint = 0)
{
var job = new IntegrateMotionsJob
{
MotionDatas = world.MotionDatas,
MotionVelocities = world.MotionVelocities,
TimeStep = timeStep
};
if (threadCountHint <= 0)
{
return(job.Schedule(inputDeps));
}
else
{
return(job.Schedule(world.NumMotions, 64, inputDeps));
}
}
// 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);
}
// Schedule the job to apply gravity to all dynamic bodies and copy input velocities
internal static JobHandle ScheduleApplyGravityAndCopyInputVelocitiesJob(ref DynamicsWorld world, NativeSlice <Velocity> inputVelocities,
float3 gravityAcceleration, JobHandle inputDeps, int threadCountHint = 0)
{
var job = new ApplyGravityAndCopyInputVelocitiesJob
{
MotionDatas = world.MotionDatas,
MotionVelocities = world.MotionVelocities,
InputVelocities = inputVelocities,
GravityAcceleration = gravityAcceleration
};
if (threadCountHint <= 0)
{
return(job.Schedule(inputDeps));
}
else
{
return(job.Schedule(world.NumMotions, 64, inputDeps));
}
}
// 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);
}
// Schedule a job to integrate the world's motions forward by the given time step.
internal static JobHandle ScheduleIntegrateJobs(ref DynamicsWorld world, float timeStep, JobHandle inputDeps, bool multiThreaded = true)
{
if (!multiThreaded)
{
var job = new IntegrateMotionsJob
{
MotionDatas = world.MotionDatas,
MotionVelocities = world.MotionVelocities,
TimeStep = timeStep
};
return(job.Schedule(inputDeps));
}
else
{
var job = new ParallelIntegrateMotionsJob
{
MotionDatas = world.MotionDatas,
MotionVelocities = world.MotionVelocities,
TimeStep = timeStep
};
return(job.Schedule(world.NumMotions, 64, inputDeps));
}
}
// 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);
}
// Construct a world with the given number of uninitialized bodies and joints
public PhysicsWorld(int numStaticBodies, int numDynamicBodies, int numJoints)
{
CollisionWorld = new CollisionWorld(numStaticBodies, numDynamicBodies);
DynamicsWorld = new DynamicsWorld(numDynamicBodies, numJoints);
}
// 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);
}
