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