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