static unsafe void CheckColliderCastHit(ref Physics.PhysicsWorld world, ColliderCastInput input, ColliderCastHit hit, string failureMessage)
{
// Fetch the leaf collider and convert the shape cast result into a distance result at the hit transform
ChildCollider leaf;
MTransform queryFromWorld = Math.Inverse(new MTransform(input.Orientation, input.Position + input.Direction * hit.Fraction));
GetHitLeaf(ref world, hit.RigidBodyIndex, hit.ColliderKey, queryFromWorld, out leaf, out MTransform queryFromTarget);
DistanceQueries.Result result = new DistanceQueries.Result
{
PositionOnAinA = Math.Mul(queryFromWorld, hit.Position),
NormalInA = math.mul(queryFromWorld.Rotation, hit.SurfaceNormal),
Distance = 0.0f
};
// If the fraction is zero then the shapes should penetrate, otherwise they should have zero distance
if (hit.Fraction == 0.0f)
{
// Do a distance query to verify initial penetration
result.Distance = DistanceQueries.ConvexConvex(input.Collider, leaf.Collider, queryFromTarget).Distance;
Assert.Less(result.Distance, tolerance, failureMessage + ": zero fraction with positive distance");
}
// Verify the distance at the hit transform
ValidateDistanceResult(result, ref ((ConvexCollider *)input.Collider)->ConvexHull, ref ((ConvexCollider *)leaf.Collider)->ConvexHull, queryFromTarget, result.Distance, failureMessage);
}
private static unsafe void TestConvexConvexDistance(ConvexCollider *target, ConvexCollider *query, MTransform queryFromTarget, string failureMessage)
{
// Do the query, API version and reference version, then validate the result
DistanceQueries.Result result = DistanceQueries.ConvexConvex((Collider *)query, (Collider *)target, queryFromTarget);
float referenceDistance = RefConvexConvexDistance(ref query->ConvexHull, ref target->ConvexHull, queryFromTarget);
ValidateDistanceResult(result, ref query->ConvexHull, ref target->ConvexHull, queryFromTarget, referenceDistance, failureMessage);
}
private static unsafe bool ConvexConvex(ColliderCastInput input, Collider *target, sfloat maxFraction, out ColliderCastHit hit)
{
hit = default;
// Get the current transform
MTransform targetFromQuery = new MTransform(input.Orientation, input.Start);
// Conservative advancement
sfloat tolerance = sfloat.FromRaw(0x3a83126f); // return if this close to a hit
sfloat keepDistance = sfloat.FromRaw(0x38d1b717); // avoid bad cases for GJK (penetration / exact hit)
int iterations = 10; // return after this many advances, regardless of accuracy
sfloat fraction = sfloat.Zero;
while (true)
{
if (fraction >= maxFraction)
{
// Exceeded the maximum fraction without a hit
return(false);
}
// Find the current distance
DistanceQueries.Result distanceResult = DistanceQueries.ConvexConvex(target, input.Collider, targetFromQuery);
// Check for a hit
if (distanceResult.Distance < tolerance || --iterations == 0)
{
targetFromQuery.Translation = input.Start;
hit.Position = Mul(input.QueryContext.WorldFromLocalTransform, distanceResult.PositionOnBinA);
hit.SurfaceNormal = math.mul(input.QueryContext.WorldFromLocalTransform.Rotation, -distanceResult.NormalInA);
hit.Fraction = fraction;
hit.RigidBodyIndex = input.QueryContext.RigidBodyIndex;
hit.ColliderKey = input.QueryContext.ColliderKey;
hit.Material = ((ConvexColliderHeader *)target)->Material;
hit.Entity = input.QueryContext.Entity;
return(true);
}
// Check for a miss
sfloat dot = math.dot(distanceResult.NormalInA, input.Ray.Displacement);
if (dot <= sfloat.Zero)
{
// Collider is moving away from the target, it will never hit
return(false);
}
// Advance
fraction += (distanceResult.Distance - keepDistance) / dot;
if (fraction >= maxFraction)
{
// Exceeded the maximum fraction without a hit
return(false);
}
targetFromQuery.Translation = math.lerp(input.Start, input.End, fraction);
}
}
private static unsafe bool ConvexConvex <T>(ColliderCastInput input, Collider *target, ref T collector) where T : struct, ICollector <ColliderCastHit>
{
//Assert.IsTrue(target->CollisionType == CollisionType.Convex && input.Collider->CollisionType == CollisionType.Convex, "ColliderCast.ConvexConvex can only process convex colliders");
// Get the current transform
MTransform targetFromQuery = new MTransform(input.Orientation, input.Start);
// Conservative advancement
const float tolerance = 1e-3f; // return if this close to a hit
const float keepDistance = 1e-4f; // avoid bad cases for GJK (penetration / exact hit)
int iterations = 10; // return after this many advances, regardless of accuracy
float fraction = 0.0f;
while (true)
{
if (fraction >= collector.MaxFraction)
{
// Exceeded the maximum fraction without a hit
return(false);
}
// Find the current distance
DistanceQueries.Result distanceResult = DistanceQueries.ConvexConvex(target, input.Collider, targetFromQuery);
// Check for a hit
if (distanceResult.Distance < tolerance || --iterations == 0)
{
targetFromQuery.Translation = input.Start;
return(collector.AddHit(new ColliderCastHit
{
Position = distanceResult.PositionOnBinA,
SurfaceNormal = -distanceResult.NormalInA,
Fraction = fraction,
ColliderKey = ColliderKey.Empty,
RigidBodyIndex = -1
}));
}
// Check for a miss
float dot = math.dot(distanceResult.NormalInA, input.Ray.Displacement);
if (dot <= 0.0f)
{
// Collider is moving away from the target, it will never hit
return(false);
}
// Advance
fraction += (distanceResult.Distance - keepDistance) / dot;
if (fraction >= collector.MaxFraction)
{
// Exceeded the maximum fraction without a hit
return(false);
}
targetFromQuery.Translation = math.lerp(input.Start, input.End, fraction);
}
}
// Does distance queries and checks some properties of the results:
// - Closest hit returned from the all hits query has the same fraction as the hit returned from the closest hit query
// - Any hit and closest hit queries return a hit if and only if the all hits query does
// - Hit distance is the same as the support distance in the hit normal direction
// - Fetching the shapes from any world query hit and querying them directly gives a matching result
static unsafe void WorldCalculateDistanceTest(ref Physics.PhysicsWorld world, ColliderDistanceInput input, ref NativeList <DistanceHit> hits, string failureMessage)
{
// Do an all-hits query
hits.Clear();
world.CalculateDistance(input, ref hits);
// Check each hit and find the closest
float closestDistance = float.MaxValue;
MTransform queryFromWorld = Math.Inverse(new MTransform(input.Transform));
for (int iHit = 0; iHit < hits.Length; iHit++)
{
DistanceHit hit = hits[iHit];
closestDistance = math.min(closestDistance, hit.Distance);
// Fetch the leaf collider and query it directly
ChildCollider leaf;
MTransform queryFromTarget;
GetHitLeaf(ref world, hit.RigidBodyIndex, hit.ColliderKey, queryFromWorld, out leaf, out queryFromTarget);
float referenceDistance = DistanceQueries.ConvexConvex(input.Collider, leaf.Collider, queryFromTarget).Distance;
// Compare to the world query result
DistanceQueries.Result result = DistanceResultFromDistanceHit(hit, queryFromWorld);
ValidateDistanceResult(result, ref ((ConvexCollider *)input.Collider)->ConvexHull, ref ((ConvexCollider *)leaf.Collider)->ConvexHull, queryFromTarget, referenceDistance,
failureMessage + ", hits[" + iHit + "]");
}
// Do a closest-hit query and check that the distance matches
DistanceHit closestHit;
bool hasClosestHit = world.CalculateDistance(input, out closestHit);
if (hits.Length == 0)
{
Assert.IsFalse(hasClosestHit, failureMessage + ", closestHit: no matching result in hits");
}
else
{
ChildCollider leaf;
MTransform queryFromTarget;
GetHitLeaf(ref world, closestHit.RigidBodyIndex, closestHit.ColliderKey, queryFromWorld, out leaf, out queryFromTarget);
DistanceQueries.Result result = DistanceResultFromDistanceHit(closestHit, queryFromWorld);
ValidateDistanceResult(result, ref ((ConvexCollider *)input.Collider)->ConvexHull, ref ((ConvexCollider *)leaf.Collider)->ConvexHull, queryFromTarget, closestDistance,
failureMessage + ", closestHit");
}
// Do an any-hit query and check that it is consistent with the others
bool hasAnyHit = world.CalculateDistance(input);
Assert.AreEqual(hasAnyHit, hasClosestHit, failureMessage + ": any hit result inconsistent with the others");
// TODO - this test can't catch false misses. We could do brute-force broadphase / midphase search to cover those.
}
//
// Reference implementations of queries using simple brute-force methods
//
static unsafe float RefConvexConvexDistance(ref ConvexHull a, ref ConvexHull b, MTransform aFromB)
{
bool success = false;
if (a.NumVertices + b.NumVertices < 64) // too slow without burst
{
// Build the minkowski difference in a-space
int maxNumVertices = a.NumVertices * b.NumVertices;
Aabb aabb = Aabb.Empty;
for (int iB = 0; iB < b.NumVertices; iB++)
{
float3 vertexB = Math.Mul(aFromB, b.Vertices[iB]);
for (int iA = 0; iA < a.NumVertices; iA++)
{
float3 vertexA = a.Vertices[iA];
aabb.Include(vertexA - vertexB);
}
}
ConvexHullBuilderStorage diffStorage = new ConvexHullBuilderStorage(maxNumVertices, Allocator.Temp, aabb, 0.0f, ConvexHullBuilder.IntResolution.Low);
ref ConvexHullBuilder diff = ref diffStorage.Builder;
success = true;
for (int iB = 0; iB < b.NumVertices; iB++)
{
float3 vertexB = Math.Mul(aFromB, b.Vertices[iB]);
for (int iA = 0; iA < a.NumVertices; iA++)
{
float3 vertexA = a.Vertices[iA];
diff.AddPoint(vertexA - vertexB, (uint)(iA | iB << 16));
}
}
float distance = 0.0f;
if (success && diff.Dimension == 3)
{
// Find the closest triangle to the origin
distance = float.MaxValue;
bool penetrating = true;
foreach (int t in diff.Triangles.Indices)
{
ConvexHullBuilder.Triangle triangle = diff.Triangles[t];
float3 v0 = diff.Vertices[triangle.GetVertex(0)].Position;
float3 v1 = diff.Vertices[triangle.GetVertex(1)].Position;
float3 v2 = diff.Vertices[triangle.GetVertex(2)].Position;
float3 n = diff.ComputePlane(t).Normal;
DistanceQueries.Result result = DistanceQueries.TriangleSphere(v0, v1, v2, n, float3.zero, 0.0f, MTransform.Identity);
if (result.Distance < distance)
{
distance = result.Distance;
}
penetrating = penetrating & (math.dot(n, -result.NormalInA) < 0.0f); // only penetrating if inside of all planes
}
if (penetrating)
{
distance = -distance;
}
distance -= a.ConvexRadius + b.ConvexRadius;
}
else
{
success = false;
}
diffStorage.Dispose();
if (success)
{
return(distance);
}
}
//
// Reference implementations of queries using simple brute-force methods
//
static unsafe float RefConvexConvexDistance(ref ConvexHull a, ref ConvexHull b, MTransform aFromB)
{
// Build the minkowski difference in a-space
int maxNumVertices = a.NumVertices * b.NumVertices;
ConvexHullBuilder diff = new ConvexHullBuilder(maxNumVertices, 2 * maxNumVertices, Allocator.Temp);
bool success = true;
Aabb aabb = Aabb.Empty;
for (int iB = 0; iB < b.NumVertices; iB++)
{
float3 vertexB = Math.Mul(aFromB, b.Vertices[iB]);
for (int iA = 0; iA < a.NumVertices; iA++)
{
float3 vertexA = a.Vertices[iA];
aabb.Include(vertexA - vertexB);
}
}
diff.IntegerSpaceAabb = aabb;
for (int iB = 0; iB < b.NumVertices; iB++)
{
float3 vertexB = Math.Mul(aFromB, b.Vertices[iB]);
for (int iA = 0; iA < a.NumVertices; iA++)
{
float3 vertexA = a.Vertices[iA];
if (!diff.AddPoint(vertexA - vertexB, (uint)(iA | iB << 16)))
{
// TODO - coplanar vertices are tripping up ConvexHullBuilder, we should fix it but for now fall back to DistanceQueries.ConvexConvex()
success = false;
}
}
}
float distance;
if (!success || diff.Triangles.GetFirstIndex() == -1)
{
// No triangles unless the difference is 3D, fall back to GJK
// Most of the time this happens for cases like sphere-sphere, capsule-capsule, etc. which have special implementations,
// so comparing those to GJK still validates the results of different API queries against each other.
distance = DistanceQueries.ConvexConvex(ref a, ref b, aFromB).Distance;
}
else
{
// Find the closest triangle to the origin
distance = float.MaxValue;
bool penetrating = true;
for (int t = diff.Triangles.GetFirstIndex(); t != -1; t = diff.Triangles.GetNextIndex(t))
{
ConvexHullBuilder.Triangle triangle = diff.Triangles[t];
float3 v0 = diff.Vertices[triangle.GetVertex(0)].Position;
float3 v1 = diff.Vertices[triangle.GetVertex(1)].Position;
float3 v2 = diff.Vertices[triangle.GetVertex(2)].Position;
float3 n = diff.ComputePlane(t).Normal;
DistanceQueries.Result result = DistanceQueries.TriangleSphere(v0, v1, v2, n, float3.zero, 0.0f, MTransform.Identity);
if (result.Distance < distance)
{
distance = result.Distance;
}
penetrating = penetrating & (math.dot(n, -result.NormalInA) < 0.0f); // only penetrating if inside of all planes
}
if (penetrating)
{
distance = -distance;
}
distance -= a.ConvexRadius + b.ConvexRadius;
}
diff.Dispose();
return(distance);
}
public unsafe void ManifoldQueryTest()
{
const uint seed = 0x98765432;
Random rnd = new Random(seed);
int numWorlds = 1000;
uint dbgWorld = 0;
if (dbgWorld > 0)
{
numWorlds = 1;
}
for (int iWorld = 0; iWorld < numWorlds; iWorld++)
{
// Save state to repro this query without doing everything that came before it
if (dbgWorld > 0)
{
rnd.state = dbgWorld;
}
uint worldState = rnd.state;
Physics.PhysicsWorld world = TestUtils.GenerateRandomWorld(ref rnd, rnd.NextInt(1, 20), 3.0f);
// Manifold test
// TODO would be nice if we could change the world collision tolerance
for (int iBodyA = 0; iBodyA < world.NumBodies; iBodyA++)
{
for (int iBodyB = iBodyA + 1; iBodyB < world.NumBodies; iBodyB++)
{
Physics.RigidBody bodyA = world.Bodies[iBodyA];
Physics.RigidBody bodyB = world.Bodies[iBodyB];
if (bodyA.Collider->Type == ColliderType.Mesh && bodyB.Collider->Type == ColliderType.Mesh)
{
continue; // TODO - no mesh-mesh manifold support yet
}
// Build manifolds
BlockStream contacts = new BlockStream(1, 0, Allocator.Temp);
BlockStream.Writer contactWriter = contacts;
contactWriter.BeginForEachIndex(0);
ManifoldQueries.BodyBody(ref world, new BodyIndexPair {
BodyAIndex = iBodyA, BodyBIndex = iBodyB
}, 1.0f, ref contactWriter);
contactWriter.EndForEachIndex();
// Read each manifold
BlockStream.Reader contactReader = contacts;
contactReader.BeginForEachIndex(0);
int manifoldIndex = 0;
while (contactReader.RemainingItemCount > 0)
{
string failureMessage = iWorld + " (" + worldState + ") " + iBodyA + " vs " + iBodyB + " #" + manifoldIndex;
manifoldIndex++;
// Read the manifold header
ContactHeader header = contactReader.Read <ContactHeader>();
ConvexConvexManifoldQueries.Manifold manifold = new ConvexConvexManifoldQueries.Manifold();
manifold.NumContacts = header.NumContacts;
manifold.Normal = header.Normal;
// Get the leaf shapes
ChildCollider leafA, leafB;
{
Collider.GetLeafCollider(bodyA.Collider, bodyA.WorldFromBody, header.ColliderKeys.ColliderKeyA, out leafA);
Collider.GetLeafCollider(bodyB.Collider, bodyB.WorldFromBody, header.ColliderKeys.ColliderKeyB, out leafB);
}
// Read each contact point
int minIndex = 0;
for (int iContact = 0; iContact < header.NumContacts; iContact++)
{
// Read the contact and find the closest
ContactPoint contact = contactReader.Read <ContactPoint>();
manifold[iContact] = contact;
if (contact.Distance < manifold[minIndex].Distance)
{
minIndex = iContact;
}
// Check that the contact point is on or inside the shape
CheckPointOnSurface(ref leafA, contact.Position + manifold.Normal * contact.Distance, failureMessage + " contact " + iContact + " leaf A");
CheckPointOnSurface(ref leafB, contact.Position, failureMessage + " contact " + iContact + " leaf B");
}
// Check the closest point
{
ContactPoint closestPoint = manifold[minIndex];
RigidTransform aFromWorld = math.inverse(leafA.TransformFromChild);
DistanceQueries.Result result = new DistanceQueries.Result
{
PositionOnAinA = math.transform(aFromWorld, closestPoint.Position + manifold.Normal * closestPoint.Distance),
NormalInA = math.mul(aFromWorld.rot, manifold.Normal),
Distance = closestPoint.Distance
};
MTransform aFromB = new MTransform(math.mul(aFromWorld, leafB.TransformFromChild));
float referenceDistance = DistanceQueries.ConvexConvex(leafA.Collider, leafB.Collider, aFromB).Distance;
ValidateDistanceResult(result, ref ((ConvexCollider *)leafA.Collider)->ConvexHull, ref ((ConvexCollider *)leafB.Collider)->ConvexHull, aFromB, referenceDistance, failureMessage + " closest point");
}
// Check that the manifold is flat
CheckManifoldFlat(ref manifold, manifold.Normal, failureMessage + ": non-flat A");
CheckManifoldFlat(ref manifold, float3.zero, failureMessage + ": non-flat B");
}
contacts.Dispose();
}
}
world.Dispose(); // TODO leaking memory if the test fails
}
}
