// Solve the Jacobian
public void Solve(ref MotionVelocity velocityA, ref MotionVelocity velocityB, sfloat timestep, sfloat invTimestep)
{
// Predict the motions' transforms at the end of the step
MTransform futureWorldFromA;
MTransform futureWorldFromB;
{
quaternion dqA = Integrator.IntegrateAngularVelocity(velocityA.AngularVelocity, timestep);
quaternion dqB = Integrator.IntegrateAngularVelocity(velocityB.AngularVelocity, timestep);
quaternion futureOrientationA = math.normalize(math.mul(WorldFromA.rot, dqA));
quaternion futureOrientationB = math.normalize(math.mul(WorldFromB.rot, dqB));
futureWorldFromA = new MTransform(futureOrientationA, WorldFromA.pos + velocityA.LinearVelocity * timestep);
futureWorldFromB = new MTransform(futureOrientationB, WorldFromB.pos + velocityB.LinearVelocity * timestep);
}
// Calculate the angulars
CalculateAngulars(PivotAinA, futureWorldFromA.Rotation, out float3 angA0, out float3 angA1, out float3 angA2);
CalculateAngulars(PivotBinB, futureWorldFromB.Rotation, out float3 angB0, out float3 angB1, out float3 angB2);
// Calculate effective mass
float3 EffectiveMassDiag, EffectiveMassOffDiag;
{
// Calculate the inverse effective mass matrix
float3 invEffectiveMassDiag = new float3(
JacobianUtilities.CalculateInvEffectiveMassDiag(angA0, velocityA.InverseInertia, velocityA.InverseMass,
angB0, velocityB.InverseInertia, velocityB.InverseMass),
JacobianUtilities.CalculateInvEffectiveMassDiag(angA1, velocityA.InverseInertia, velocityA.InverseMass,
angB1, velocityB.InverseInertia, velocityB.InverseMass),
JacobianUtilities.CalculateInvEffectiveMassDiag(angA2, velocityA.InverseInertia, velocityA.InverseMass,
angB2, velocityB.InverseInertia, velocityB.InverseMass));
float3 invEffectiveMassOffDiag = new float3(
JacobianUtilities.CalculateInvEffectiveMassOffDiag(angA0, angA1, velocityA.InverseInertia, angB0, angB1, velocityB.InverseInertia),
JacobianUtilities.CalculateInvEffectiveMassOffDiag(angA0, angA2, velocityA.InverseInertia, angB0, angB2, velocityB.InverseInertia),
JacobianUtilities.CalculateInvEffectiveMassOffDiag(angA1, angA2, velocityA.InverseInertia, angB1, angB2, velocityB.InverseInertia));
// Invert to get the effective mass matrix
JacobianUtilities.InvertSymmetricMatrix(invEffectiveMassDiag, invEffectiveMassOffDiag, out EffectiveMassDiag, out EffectiveMassOffDiag);
}
// Predict error at the end of the step and calculate the impulse to correct it
float3 impulse;
{
// Find the difference between the future distance and the limit range, then apply tau and damping
sfloat futureDistanceError = CalculateError(futureWorldFromA, futureWorldFromB, out float3 futureDirection);
sfloat solveDistanceError = JacobianUtilities.CalculateCorrection(futureDistanceError, InitialError, Tau, Damping);
// Calculate the impulse to correct the error
float3 solveError = solveDistanceError * futureDirection;
float3x3 effectiveMass = JacobianUtilities.BuildSymmetricMatrix(EffectiveMassDiag, EffectiveMassOffDiag);
impulse = math.mul(effectiveMass, solveError) * invTimestep;
}
// Apply the impulse
ApplyImpulse(impulse, angA0, angA1, angA2, ref velocityA);
ApplyImpulse(-impulse, angB0, angB1, angB2, ref velocityB);
}
// Schedule all the jobs for the simulation step.
// Enqueued callbacks can choose to inject additional jobs at defined sync points.
// multiThreaded defines which simulation type will be called:
// - true will result in default multithreaded simulation
// - false 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 multiThreaded argument provided.
public unsafe SimulationJobHandles ScheduleStepJobs(SimulationStepInput input, SimulationCallbacks callbacksIn, JobHandle inputDeps, bool multiThreaded = true)
{
SafetyChecks.CheckFiniteAndPositiveAndThrow(input.TimeStep, nameof(input.TimeStep));
SafetyChecks.CheckInRangeAndThrow(input.NumSolverIterations, new int2(1, int.MaxValue), nameof(input.NumSolverIterations));
// 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(input, 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, multiThreaded);
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, multiThreaded);
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(
input.World.DynamicsWorld.MotionVelocities, SimulationContext.InputVelocities,
input.TimeStep * input.Gravity, multiThreaded ? postOverlapsHandle : handle, multiThreaded);
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, multiThreaded);
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, multiThreaded);
handle = handles.FinalExecutionHandle;
var disposeHandle4 = handles.FinalDisposeHandle;
handle = callbacks.Execute(SimulationCallbacks.Phase.PostCreateContactJacobians, this, ref input.World, handle);
// Solve all Jacobians
Solver.StabilizationData solverStabilizationData = new Solver.StabilizationData(input, SimulationContext);
handles = Solver.ScheduleSolveJacobiansJobs(ref input.World.DynamicsWorld, input.TimeStep, input.NumSolverIterations,
ref StepContext.Jacobians, ref SimulationContext.CollisionEventDataStream, ref SimulationContext.TriggerEventDataStream,
ref StepContext.SolverSchedulerInfo, solverStabilizationData, handle, multiThreaded);
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, multiThreaded);
// Synchronize the collision world
if (input.SynchronizeCollisionWorld)
{
handle = input.World.CollisionWorld.ScheduleUpdateDynamicTree(ref input.World, input.TimeStep, input.Gravity, handle, multiThreaded); // 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 (!multiThreaded)
{
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);
}
// Steps the simulation immediately on a single thread without spawning any jobs.
public static void StepImmediate(SimulationStepInput input, ref SimulationContext simulationContext)
{
SafetyChecks.CheckFiniteAndPositiveAndThrow(input.TimeStep, nameof(input.TimeStep));
SafetyChecks.CheckInRangeAndThrow(input.NumSolverIterations, new int2(1, int.MaxValue), nameof(input.NumSolverIterations));
if (input.World.NumDynamicBodies == 0)
{
// No need to do anything, since nothing can move
return;
}
// Inform the context of the timeStep
simulationContext.TimeStep = input.TimeStep;
// Find all body pairs that overlap in the broadphase
var dynamicVsDynamicBodyPairs = new NativeStream(1, Allocator.Temp);
var dynamicVsStaticBodyPairs = new NativeStream(1, Allocator.Temp);
{
var dynamicVsDynamicBodyPairsWriter = dynamicVsDynamicBodyPairs.AsWriter();
var dynamicVsStaticBodyPairsWriter = dynamicVsStaticBodyPairs.AsWriter();
input.World.CollisionWorld.FindOverlaps(ref dynamicVsDynamicBodyPairsWriter, ref dynamicVsStaticBodyPairsWriter);
}
// Create dispatch pairs
var dispatchPairs = new NativeList <DispatchPairSequencer.DispatchPair>(Allocator.Temp);
DispatchPairSequencer.CreateDispatchPairs(ref dynamicVsDynamicBodyPairs, ref dynamicVsStaticBodyPairs,
input.World.NumDynamicBodies, input.World.Joints, ref dispatchPairs);
// Apply gravity and copy input velocities
Solver.ApplyGravityAndCopyInputVelocities(input.World.DynamicsWorld.MotionVelocities,
simulationContext.InputVelocities, input.TimeStep * input.Gravity);
// Narrow phase
var contacts = new NativeStream(1, Allocator.Temp);
{
var contactsWriter = contacts.AsWriter();
NarrowPhase.CreateContacts(ref input.World, dispatchPairs.AsArray(), input.TimeStep, ref contactsWriter);
}
// Build Jacobians
var jacobians = new NativeStream(1, Allocator.Temp);
{
var contactsReader = contacts.AsReader();
var jacobiansWriter = jacobians.AsWriter();
Solver.BuildJacobians(ref input.World, input.TimeStep, input.Gravity, input.NumSolverIterations,
dispatchPairs.AsArray(), ref contactsReader, ref jacobiansWriter);
}
// Solve Jacobians
{
var jacobiansReader = jacobians.AsReader();
var collisionEventsWriter = simulationContext.CollisionEventDataStream.AsWriter();
var triggerEventsWriter = simulationContext.TriggerEventDataStream.AsWriter();
Solver.StabilizationData solverStabilizationData = new Solver.StabilizationData(input, simulationContext);
Solver.SolveJacobians(ref jacobiansReader, input.World.DynamicsWorld.MotionVelocities,
input.TimeStep, input.NumSolverIterations, ref collisionEventsWriter, ref triggerEventsWriter, solverStabilizationData);
}
// Integrate motions
Integrator.Integrate(input.World.DynamicsWorld.MotionDatas, input.World.DynamicsWorld.MotionVelocities, input.TimeStep);
// Synchronize the collision world if asked for
if (input.SynchronizeCollisionWorld)
{
input.World.CollisionWorld.UpdateDynamicTree(ref input.World, input.TimeStep, input.Gravity);
}
}
