internal static unsafe void ExecuteImpl(ref PhysicsWorld world, float timeStep,
NativeArray <DispatchPairSequencer.DispatchPair> dispatchPairs,
int dispatchPairReadOffset, int numPairsToRead, ref NativeStream.Writer contactWriter)
{
for (int i = 0; i < numPairsToRead; i++)
{
DispatchPairSequencer.DispatchPair dispatchPair = dispatchPairs[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
{
BodyIndexA = dispatchPair.BodyIndexA,
BodyIndexB = dispatchPair.BodyIndexB
};
RigidBody rigidBodyA = world.Bodies[pair.BodyIndexA];
RigidBody rigidBodyB = world.Bodies[pair.BodyIndexB];
MotionVelocity motionVelocityA = pair.BodyIndexA < world.MotionVelocities.Length ?
world.MotionVelocities[pair.BodyIndexA] : MotionVelocity.Zero;
MotionVelocity motionVelocityB = pair.BodyIndexB < world.MotionVelocities.Length ?
world.MotionVelocities[pair.BodyIndexB] : MotionVelocity.Zero;
ManifoldQueries.BodyBody(rigidBodyA, rigidBodyB, motionVelocityA, motionVelocityB,
world.CollisionWorld.CollisionTolerance, timeStep, pair, ref contactWriter);
}
}
}
}
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();
}