Ejemplo n.º 1
0
            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);
                        }
                    }
                }
            }
Ejemplo n.º 2
0
            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();
            }