public static unsafe void AabbCollider <T>(OverlapAabbInput input, [NoAlias] Collider *collider, [NoAlias] ref T collector)
where T : struct, IOverlapCollector
{
if (!CollisionFilter.IsCollisionEnabled(input.Filter, collider->Filter))
{
return;
}
switch (collider->Type)
{
case ColliderType.Mesh:
AabbMesh(input, (MeshCollider *)collider, ref collector);
break;
case ColliderType.Compound:
AabbCompound(input, (CompoundCollider *)collider, ref collector);
break;
case ColliderType.Terrain:
AabbTerrain(input, (TerrainCollider *)collider, ref collector);
break;
default:
SafetyChecks.ThrowNotImplementedException();
return;
}
}
private void Flush()
{
if (m_Count != 0)
{
fixed(int *l = m_PairsLeft)
{
fixed(int *r = m_PairsRight)
{
for (int i = 0; i < m_Count; i++)
{
int bodyALocalIndex = l[i];
int bodyBLocalIndex = r[i];
if (CollisionFilter.IsCollisionEnabled(m_BodyFiltersLeft[bodyALocalIndex], m_BodyFiltersRight[bodyBLocalIndex]))
{
m_CollidingPairs->Write(new BodyIndexPair
{
BodyAIndex = bodyALocalIndex + m_BodyAIndexBase,
BodyBIndex = bodyBLocalIndex + m_BodyBIndexBase
});
}
}
}
}
m_Count = 0;
}
}
public unsafe void AabbLeaf <T>(OverlapAabbInput input, int rigidBodyIndex, ref T collector)
where T : struct, IOverlapCollector
{
rigidBodyIndex += BaseRigidBodyIndex;
RigidBody body = m_Bodies[rigidBodyIndex];
if (body.Collider.IsCreated && CollisionFilter.IsCollisionEnabled(input.Filter, body.Collider.Value.Filter))
{
collector.AddRigidBodyIndices(&rigidBodyIndex, 1);
}
}
public static unsafe bool ColliderCollider <T>(ColliderDistanceInput input, Collider *target, ref T collector) where T : struct, ICollector <DistanceHit>
{
if (!CollisionFilter.IsCollisionEnabled(input.Collider->Filter, target->Filter))
{
return(false);
}
switch (input.Collider->CollisionType)
{
case CollisionType.Convex:
switch (target->Type)
{
case ColliderType.Convex:
case ColliderType.Sphere:
case ColliderType.Capsule:
case ColliderType.Triangle:
case ColliderType.Quad:
case ColliderType.Box:
case ColliderType.Cylinder:
MTransform targetFromQuery = new MTransform(input.Transform);
Result result = ConvexConvex(target, input.Collider, targetFromQuery);
if (result.Distance < collector.MaxFraction)
{
collector.AddHit(new DistanceHit
{
Fraction = result.Distance,
SurfaceNormal = -result.NormalInA,
Position = result.PositionOnAinA,
ColliderKey = ColliderKey.Empty
});
return(true);
}
return(false);
case ColliderType.Mesh:
return(ConvexMesh(input, (MeshCollider *)target, ref collector));
case ColliderType.Compound:
return(ConvexCompound(input, (CompoundCollider *)target, ref collector));
default:
throw new NotImplementedException();
}
case CollisionType.Composite:
// no support for composite query shapes
throw new NotImplementedException();
default:
throw new NotImplementedException();
}
}
public bool DistanceLeaf <T>(ColliderDistanceInput input, int primitiveKey, ref T collector)
where T : struct, ICollector <DistanceHit>
{
m_Mesh->GetPrimitive(primitiveKey, out float3x4 vertices, out Mesh.PrimitiveFlags flags, out CollisionFilter filter);
if (!CollisionFilter.IsCollisionEnabled(input.Collider->Filter, filter)) // TODO: could do this check within GetPrimitive()
{
return(false);
}
int numPolygons = Mesh.GetNumPolygonsInPrimitive(flags);
bool isQuad = Mesh.IsPrimitveFlagSet(flags, Mesh.PrimitiveFlags.IsQuad);
float3 *v = stackalloc float3[4];
bool acceptHit = false;
ref ConvexHull inputHull = ref ((ConvexCollider *)input.Collider)->ConvexHull;
public bool DistanceLeaf <T>(PointDistanceInput input, int primitiveKey, ref T collector)
where T : struct, ICollector <DistanceHit>
{
m_Mesh->GetPrimitive(primitiveKey, out float3x4 vertices, out Mesh.PrimitiveFlags flags, out CollisionFilter filter);
if (!CollisionFilter.IsCollisionEnabled(input.Filter, filter)) // TODO: could do this check within GetPrimitive()
{
return(false);
}
int numPolygons = Mesh.GetNumPolygonsInPrimitive(flags);
bool isQuad = Mesh.IsPrimitveFlagSet(flags, Mesh.PrimitiveFlags.IsQuad);
float3 normalDirection = math.cross(vertices[1] - vertices[0], vertices[2] - vertices[0]);
bool acceptHit = false;
for (int polygonIndex = 0; polygonIndex < numPolygons; polygonIndex++)
{
Result result;
if (isQuad)
{
result = QuadSphere(
vertices[0], vertices[1], vertices[2], vertices[3], normalDirection,
input.Position, 0.0f, MTransform.Identity);
}
else
{
result = TriangleSphere(
vertices[0], vertices[1], vertices[2], normalDirection,
input.Position, 0.0f, MTransform.Identity);
}
if (result.Distance < collector.MaxFraction)
{
acceptHit |= collector.AddHit(new DistanceHit
{
Fraction = result.Distance,
Position = result.PositionOnAinA,
SurfaceNormal = -result.NormalInA,
ColliderKey = new ColliderKey(m_NumColliderKeyBits, (uint)(primitiveKey << 1 | polygonIndex))
});
}
}
return(acceptHit);
}
public void AddColliderKeys(ColliderKey *keys, int count)
{
var colliderKeys = new ColliderKeyPair {
ColliderKeyA = m_ConvexColliderKey, ColliderKeyB = m_ConvexColliderKey
};
CollisionFilter filter = m_ConvexColliderA->Filter;
// Collide the convex A with all overlapping leaves of B
switch (m_CompositeColliderB->Type)
{
// Special case meshes (since we know all polygons will be built on the fly)
case ColliderType.Mesh:
{
Mesh *mesh = &((MeshCollider *)m_CompositeColliderB)->Mesh;
uint numMeshKeyBits = mesh->NumColliderKeyBits;
var polygon = new PolygonCollider();
polygon.InitEmpty();
for (int i = 0; i < count; i++)
{
ColliderKey compositeKey = m_CompositeColliderKeyPath.GetLeafKey(keys[i]);
uint meshKey = compositeKey.Value >> (32 - (int)numMeshKeyBits);
if (mesh->GetPolygon(meshKey, filter, ref polygon))
{
if (m_Flipped)
{
colliderKeys.ColliderKeyA = compositeKey;
}
else
{
colliderKeys.ColliderKeyB = compositeKey;
}
switch (m_ConvexColliderA->CollisionType)
{
case CollisionType.Convex:
ConvexConvex(
m_Context, colliderKeys, m_ConvexColliderA, (Collider *)&polygon,
m_WorldFromA, m_WorldFromB, m_CollisionTolerance, m_Flipped);
break;
case CollisionType.Terrain:
TerrainConvex(
m_Context, colliderKeys, m_ConvexColliderA, (Collider *)&polygon,
m_WorldFromA, m_WorldFromB, m_CollisionTolerance, m_Flipped);
break;
default: // GetLeaf() may not return a composite collider
throw new NotImplementedException();
}
}
}
}
break;
// General case for all other composites (compounds, compounds of meshes, etc)
default:
{
for (int i = 0; i < count; i++)
{
ColliderKey compositeKey = m_CompositeColliderKeyPath.GetLeafKey(keys[i]);
m_CompositeColliderB->GetLeaf(compositeKey, out ChildCollider leaf);
if (CollisionFilter.IsCollisionEnabled(filter, leaf.Collider->Filter)) // TODO: shouldn't be needed if/when filtering is done fully by the BVH query
{
if (m_Flipped)
{
colliderKeys.ColliderKeyA = compositeKey;
}
else
{
colliderKeys.ColliderKeyB = compositeKey;
}
MTransform worldFromLeafB = Mul(m_WorldFromB, new MTransform(leaf.TransformFromChild));
switch (leaf.Collider->CollisionType)
{
case CollisionType.Convex:
ConvexConvex(
m_Context, colliderKeys, m_ConvexColliderA, leaf.Collider,
m_WorldFromA, worldFromLeafB, m_CollisionTolerance, m_Flipped);
break;
case CollisionType.Terrain:
ConvexTerrain(
m_Context, colliderKeys, m_ConvexColliderA, leaf.Collider,
m_WorldFromA, worldFromLeafB, m_CollisionTolerance, m_Flipped);
break;
default: // GetLeaf() may not return a composite collider
throw new NotImplementedException();
}
}
}
}
break;
}
}
// 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;
}
}
public static unsafe bool PointCollider <T>(PointDistanceInput input, Collider *target, ref T collector) where T : struct, ICollector <DistanceHit>
{
if (!CollisionFilter.IsCollisionEnabled(input.Filter, target->Filter))
{
return(false);
}
Result result;
switch (target->Type)
{
case ColliderType.Sphere:
var sphere = (SphereCollider *)target;
result = PointPoint(sphere->Center, input.Position, sphere->Radius, sphere->Radius);
break;
case ColliderType.Capsule:
var capsule = (CapsuleCollider *)target;
result = CapsuleSphere(capsule->Vertex0, capsule->Vertex1, capsule->Radius, input.Position, 0.0f, MTransform.Identity);
break;
case ColliderType.Triangle:
var triangle = (PolygonCollider *)target;
result = TriangleSphere(
triangle->Vertices[0], triangle->Vertices[1], triangle->Vertices[2], triangle->Planes[0].Normal,
input.Position, 0.0f, MTransform.Identity);
break;
case ColliderType.Quad:
var quad = (PolygonCollider *)target;
result = QuadSphere(
quad->Vertices[0], quad->Vertices[1], quad->Vertices[2], quad->Vertices[3], quad->Planes[0].Normal,
input.Position, 0.0f, MTransform.Identity);
break;
case ColliderType.Convex:
case ColliderType.Box:
case ColliderType.Cylinder:
ref ConvexHull hull = ref ((ConvexCollider *)target)->ConvexHull;
result = ConvexConvex(hull.VerticesPtr, hull.NumVertices, hull.ConvexRadius, &input.Position, 1, 0.0f, MTransform.Identity);
break;
case ColliderType.Mesh:
return(PointMesh(input, (MeshCollider *)target, ref collector));
case ColliderType.Compound:
return(PointCompound(input, (CompoundCollider *)target, ref collector));
default:
throw new NotImplementedException();
}
if (result.Distance < collector.MaxFraction)
{
collector.AddHit(new DistanceHit
{
Fraction = result.Distance,
SurfaceNormal = -result.NormalInA,
Position = result.PositionOnAinA,
ColliderKey = ColliderKey.Empty
});
return(true);
}
return(false);
}
// 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;
}
}