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