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(); }
public unsafe static void Execute(ref BodyPairsJobData <T> jobData, IntPtr additionalData, IntPtr bufferRangePatchData, ref JobRanges ranges, int jobIndex) { if (jobData.BlockStreamStart == null || !jobData.BlockStreamStart->HasElements) { return; } var blockStreamReader = new Havok.Physics.HpBlockStreamReader(jobData.BlockStreamStart); int *pluginIndexToLocal = jobData.PluginIndexToLocal->Data; while (blockStreamReader.HasItems) { BodyIndexPair indices = blockStreamReader.Read <BodyIndexPair>(); // Really an hknpBodyIdPair int bodyIndexA = pluginIndexToLocal[indices.BodyIndexA & 0x00ffffff]; int bodyIndexB = pluginIndexToLocal[indices.BodyIndexB & 0x00ffffff]; var pair = new ModifiableBodyPair { BodyIndexPair = new BodyIndexPair { BodyIndexA = bodyIndexA, BodyIndexB = bodyIndexB }, EntityPair = new EntityPair { EntityA = jobData.Bodies[bodyIndexA].Entity, EntityB = jobData.Bodies[bodyIndexB].Entity } }; jobData.UserJobData.Execute(ref pair); if (pair.BodyIndexA == -1 || pair.BodyIndexB == -1) { blockStreamReader.Write(BodyIndexPair.Invalid); } } }
public void Disable() { BodyIndexPair = BodyIndexPair.Invalid; }
// Write a set of contact manifolds for a pair of bodies to the given stream. public static unsafe void BodyBody(RigidBody rigidBodyA, RigidBody rigidBodyB, MotionVelocity motionVelocityA, MotionVelocity motionVelocityB, float collisionTolerance, float timeStep, BodyIndexPair pair, ref NativeStream.Writer contactWriter) { Collider *colliderA = rigidBodyA.Collider; Collider *colliderB = rigidBodyB.Collider; if (colliderA == null || colliderB == null || !CollisionFilter.IsCollisionEnabled(colliderA->Filter, colliderB->Filter)) { return; } // Build combined motion expansion MotionExpansion expansion; { MotionExpansion expansionA = motionVelocityA.CalculateExpansion(timeStep); MotionExpansion expansionB = motionVelocityB.CalculateExpansion(timeStep); expansion = new MotionExpansion { Linear = expansionA.Linear - expansionB.Linear, Uniform = expansionA.Uniform + expansionB.Uniform + collisionTolerance }; } var context = new Context { BodyIndices = pair, BodyCustomTags = new CustomTagsPair { CustomTagsA = rigidBodyA.CustomTags, CustomTagsB = rigidBodyB.CustomTags }, BodiesHaveInfiniteMass = !math.any(motionVelocityA.InverseInertiaAndMass) && !math.any(motionVelocityB.InverseInertiaAndMass), ContactWriter = (NativeStream.Writer *)UnsafeUtility.AddressOf(ref contactWriter) }; var worldFromA = new MTransform(rigidBodyA.WorldFromBody); var worldFromB = new MTransform(rigidBodyB.WorldFromBody); // Dispatch to appropriate manifold generator switch (colliderA->CollisionType) { case CollisionType.Convex: switch (colliderB->CollisionType) { case CollisionType.Convex: ConvexConvex(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false); break; case CollisionType.Composite: ConvexComposite(context, ColliderKey.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion, false); break; case CollisionType.Terrain: ConvexTerrain(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false); break; } break; case CollisionType.Composite: switch (colliderB->CollisionType) { case CollisionType.Convex: CompositeConvex(context, colliderA, colliderB, worldFromA, worldFromB, expansion, false); break; case CollisionType.Composite: CompositeComposite(context, colliderA, colliderB, worldFromA, worldFromB, expansion, false); break; case CollisionType.Terrain: CompositeTerrain(context, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false); break; } break; case CollisionType.Terrain: switch (colliderB->CollisionType) { case CollisionType.Convex: TerrainConvex(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false); break; case CollisionType.Composite: TerrainComposite(context, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false); break; case CollisionType.Terrain: UnityEngine.Assertions.Assert.IsTrue(false); break; } break; } }
// Write a set of contact manifolds for a pair of bodies to the given stream. public static unsafe void BodyBody(ref PhysicsWorld world, BodyIndexPair pair, float timeStep, ref BlockStream.Writer contactWriter) { RigidBody rigidBodyA = world.Bodies[pair.BodyAIndex]; RigidBody rigidBodyB = world.Bodies[pair.BodyBIndex]; Collider *colliderA = rigidBodyA.Collider; Collider *colliderB = rigidBodyB.Collider; if (colliderA == null || colliderB == null || !CollisionFilter.IsCollisionEnabled(colliderA->Filter, colliderB->Filter)) { return; } // Build combined motion expansion MotionExpansion expansion; { MotionExpansion GetBodyExpansion(int bodyIndex, NativeSlice <MotionVelocity> mvs) { return(bodyIndex < mvs.Length ? mvs[bodyIndex].CalculateExpansion(timeStep) : MotionExpansion.Zero); } MotionExpansion expansionA = GetBodyExpansion(pair.BodyAIndex, world.MotionVelocities); MotionExpansion expansionB = GetBodyExpansion(pair.BodyBIndex, world.MotionVelocities); expansion = new MotionExpansion { Linear = expansionA.Linear - expansionB.Linear, Uniform = expansionA.Uniform + expansionB.Uniform + world.CollisionTolerance }; } var context = new Context { BodyIndices = pair, BodyCustomDatas = new CustomDataPair { CustomDataA = rigidBodyA.CustomData, CustomDataB = rigidBodyB.CustomData } }; var worldFromA = new MTransform(rigidBodyA.WorldFromBody); var worldFromB = new MTransform(rigidBodyB.WorldFromBody); // Dispatch to appropriate manifold generator switch (colliderA->CollisionType) { case CollisionType.Convex: switch (colliderB->CollisionType) { case CollisionType.Convex: ConvexConvex(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false, ref contactWriter); break; case CollisionType.Composite: ConvexComposite(context, ColliderKey.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion, false, ref contactWriter); break; } break; case CollisionType.Composite: switch (colliderB->CollisionType) { case CollisionType.Convex: CompositeConvex(context, colliderA, colliderB, worldFromA, worldFromB, expansion, false, ref contactWriter); break; case CollisionType.Composite: CompositeComposite(context, colliderA, colliderB, worldFromA, worldFromB, expansion, false, ref contactWriter); break; } break; } }