public unsafe static void Execute(ref BodyPairsJobData<T> jobData, IntPtr additionalData,
IntPtr bufferRangePatchData, ref JobRanges ranges, int jobIndex)
{
int currentIdx = 0;
while (currentIdx < jobData.PhasedDispatchPairs.Length)
{
Scheduler.DispatchPair dispatchPair = jobData.PhasedDispatchPairs[currentIdx];
var pair = new ModifiableBodyPair
{
BodyIndices = new BodyIndexPair { BodyAIndex = dispatchPair.BodyAIndex, BodyBIndex = dispatchPair.BodyBIndex },
Entities = new EntityPair
{
EntityA = jobData.Bodies[dispatchPair.BodyAIndex].Entity,
EntityB = jobData.Bodies[dispatchPair.BodyBIndex].Entity
}
};
jobData.UserJobData.Execute(ref pair);
if (pair.BodyIndices.BodyAIndex == -1 || pair.BodyIndices.BodyBIndex == -1)
{
jobData.PhasedDispatchPairs[currentIdx] = Scheduler.DispatchPair.Invalid;
}
do
{
currentIdx++;
} while (currentIdx < jobData.PhasedDispatchPairs.Length && jobData.PhasedDispatchPairs[currentIdx].IsJoint);
}
}
// Move to the next body pair
public unsafe BodyIndexPair NextPair()
{
if (m_Pairs.m_SimulationType == SimulationType.UnityPhysics)
{
Scheduler.DispatchPair pair = m_Pairs.m_PhasedDispatchPairs[m_CurrentPairIndex++];
return(new BodyIndexPair {
BodyAIndex = pair.BodyAIndex, BodyBIndex = pair.BodyBIndex
});
}
return(BodyIndexPair.Invalid);
}
public unsafe void Execute(int workItemIndex)
{
int dispatchPairReadOffset = SolverSchedulerInfo.GetWorkItemReadOffset(workItemIndex, out int numPairsToRead);
ContactWriter.BeginForEachIndex(workItemIndex);
JointJacobianWriter.BeginForEachIndex(workItemIndex);
for (int i = 0; i < numPairsToRead; i++)
{
Scheduler.DispatchPair dispatchPair = PhasedDispatchPairs[dispatchPairReadOffset + i];
// Invalid pairs can exist by being disabled by users
if (dispatchPair.IsValid)
{
if (dispatchPair.IsContact)
{
// Create contact manifolds for this pair of bodies
var pair = new BodyIndexPair
{
BodyAIndex = dispatchPair.BodyAIndex,
BodyBIndex = dispatchPair.BodyBIndex
};
RigidBody rigidBodyA = World.Bodies[pair.BodyAIndex];
RigidBody rigidBodyB = World.Bodies[pair.BodyBIndex];
MotionVelocity motionVelocityA = pair.BodyAIndex < World.MotionVelocities.Length ?
World.MotionVelocities[pair.BodyAIndex] : MotionVelocity.Zero;
MotionVelocity motionVelocityB = pair.BodyBIndex < World.MotionVelocities.Length ?
World.MotionVelocities[pair.BodyBIndex] : MotionVelocity.Zero;
ManifoldQueries.BodyBody(rigidBodyA, rigidBodyB, motionVelocityA, motionVelocityB,
World.CollisionWorld.CollisionTolerance, TimeStep, pair, ref ContactWriter);
}
else
{
Joint joint = World.Joints[dispatchPair.JointIndex];
// Need to fetch the real body indices from the joint, as the scheduler may have reordered them
int bodyAIndex = joint.BodyPair.BodyAIndex;
int bodyBIndex = joint.BodyPair.BodyBIndex;
GetMotion(ref World, bodyAIndex, out MotionVelocity velocityA, out MotionData motionA);
GetMotion(ref World, bodyBIndex, out MotionVelocity velocityB, out MotionData motionB);
Solver.BuildJointJacobian(joint.JointData, joint.BodyPair, velocityA, velocityB, motionA, motionB, TimeStep, NumIterations, ref JointJacobianWriter);
}
}
}
JointJacobianWriter.EndForEachIndex();
ContactWriter.EndForEachIndex();
}
