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(); }