private static unsafe void CompositeComposite( Context context, Collider *compositeColliderA, Collider *compositeColliderB, MTransform worldFromA, MTransform worldFromB, MotionExpansion expansion, bool flipped) { // Flip the order if necessary, so that A has fewer leaves than B if (compositeColliderA->NumColliderKeyBits > compositeColliderB->NumColliderKeyBits) { Collider *c = compositeColliderA; compositeColliderA = compositeColliderB; compositeColliderB = c; MTransform t = worldFromA; worldFromA = worldFromB; worldFromB = t; expansion.Linear *= -1.0f; flipped = !flipped; } var collector = new CompositeCompositeLeafCollector( context, compositeColliderA, compositeColliderB, worldFromA, worldFromB, expansion, flipped); compositeColliderA->GetLeaves(ref collector); }
private static unsafe void ConvexComposite( Context context, ColliderKey convexKeyA, Collider *convexColliderA, Collider *compositeColliderB, MTransform worldFromA, MTransform worldFromB, MotionExpansion expansion, bool flipped, ref BlockStream.Writer contactWriter) { // Calculate AABB of A in B MTransform bFromWorld = Inverse(worldFromB); MTransform bFromA = Mul(bFromWorld, worldFromA); var transform = new RigidTransform(new quaternion(bFromA.Rotation), bFromA.Translation); // TODO: avoid this conversion to and back from float3x3 Aabb aabbAinB = expansion.ExpandAabb(convexColliderA->CalculateAabb(transform)); // Do the midphase query and build manifolds for any overlapping leaf colliders var input = new OverlapAabbInput { Aabb = aabbAinB, Filter = convexColliderA->Filter }; var collector = new ConvexCompositeOverlapCollector( context, convexColliderA, convexKeyA, compositeColliderB, worldFromA, worldFromB, expansion.MaxDistance, flipped, contactWriter); OverlapQueries.AabbCollider(input, compositeColliderB, ref collector); // Keep updated writer state contactWriter = collector.m_ContactWriter; }
private static unsafe void CompositeConvex( Context context, Collider *compositeColliderA, Collider *convexColliderB, MTransform worldFromA, MTransform worldFromB, MotionExpansion expansion, bool flipped) { // Flip the relevant inputs and call convex-vs-composite expansion.Linear *= -1.0f; ConvexComposite(context, ColliderKey.Empty, convexColliderB, compositeColliderA, worldFromB, worldFromA, expansion, !flipped); }
public CompositeCompositeLeafCollector( Context context, Collider *compositeColliderA, Collider *compositeColliderB, MTransform worldFromA, MTransform worldFromB, MotionExpansion expansion, bool flipped) { m_Context = context; m_CompositeColliderA = compositeColliderA; m_CompositeColliderB = compositeColliderB; m_WorldFromA = worldFromA; m_WorldFromB = worldFromB; m_Expansion = expansion; m_Flipped = flipped; m_KeyPath = ColliderKeyPath.Empty; }
internal static unsafe void ExecuteImpl(int index, float aabbMargin, float3 gravity, float timeStep, NativeArray <RigidBody> rigidBodies, NativeArray <MotionVelocity> motionVelocities, NativeArray <Aabb> aabbs, NativeArray <PointAndIndex> points, NativeArray <CollisionFilter> filtersOut, NativeArray <bool> respondsToCollisionOut) { RigidBody body = rigidBodies[index]; Aabb aabb; if (body.Collider.IsCreated) { var mv = motionVelocities[index]; // Apply gravity only on a copy to get proper expansion for the AABB, // actual applying of gravity will be done later in the physics step mv.LinearVelocity += gravity * timeStep * mv.GravityFactor; MotionExpansion expansion = mv.CalculateExpansion(timeStep); aabb = expansion.ExpandAabb(body.Collider.Value.CalculateAabb(body.WorldFromBody)); aabb.Expand(aabbMargin); filtersOut[index] = body.Collider.Value.Filter; respondsToCollisionOut[index] = body.Collider.Value.RespondsToCollision; } else { aabb.Min = body.WorldFromBody.pos; aabb.Max = body.WorldFromBody.pos; filtersOut[index] = CollisionFilter.Zero; respondsToCollisionOut[index] = false; } aabbs[index] = aabb; points[index] = new BoundingVolumeHierarchy.PointAndIndex { Position = aabb.Center, Index = index }; }
// 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; } }