public unsafe void ConvexConvexDistanceTest()
{
Random rnd = new Random(0x12345678);
uint dbgTest = 0;
int numTests = 5000;
if (dbgTest > 0)
{
numTests = 1;
}
for (int i = 0; i < numTests; i++)
{
// Save state to repro this query without doing everything that came before it
if (dbgTest > 0)
{
rnd.state = dbgTest;
}
uint state = rnd.state;
// Generate random query inputs
ConvexCollider *target = (ConvexCollider *)TestUtils.GenerateRandomConvex(ref rnd).GetUnsafePtr();
ConvexCollider *query = (ConvexCollider *)TestUtils.GenerateRandomConvex(ref rnd).GetUnsafePtr();
MTransform queryFromTarget = new MTransform(
(rnd.NextInt(10) > 0) ? rnd.NextQuaternionRotation() : quaternion.identity,
rnd.NextFloat3(-3.0f, 3.0f));
TestConvexConvexDistance(target, query, queryFromTarget, "ConvexConvexDistanceTest failed " + i + " (" + state.ToString() + ")");
}
}
public ConvexHullGeometryCollector(NavMeshNativeInputBuilder inputBuilder, ConvexCollider *collider, RigidTransform transform, DtBoundingBox bounds)
{
InputBuilder = inputBuilder;
Collider = collider;
Transform = transform;
Bounds = bounds;
}
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 void HandleConvex(ConvexCollider *collider, ref Translation translation, ref Rotation rotation)
{
RigidTransform colliderTransform = new RigidTransform(rotation.Value, translation.Value);
Aabb aabb = collider->CalculateAabb(colliderTransform);
DtBoundingBox colliderBox = new DtBoundingBox(aabb.Min, aabb.Max);
if (DtBoundingBox.Intersects(ref TileBounds.Bounds, ref colliderBox))
{
ConvexHullGeometryCollector collector = new ConvexHullGeometryCollector(InputBuilder, collider, colliderTransform, TileBounds.Bounds);
collector.Collect();
}
}
public unsafe void ConvexConvexDistanceEdgeCaseTest()
{
Random rnd = new Random(0x90456148);
uint dbgShape = 0;
uint dbgTest = 0;
int numShapes = 500;
int numTests = 50;
if (dbgShape > 0)
{
numShapes = 1;
numTests = 1;
}
for (int iShape = 0; iShape < numShapes; iShape++)
{
if (dbgShape > 0)
{
rnd.state = dbgShape;
}
uint shapeState = rnd.state;
// Generate a random collider
ConvexCollider *collider = (ConvexCollider *)TestUtils.GenerateRandomConvex(ref rnd).GetUnsafePtr();
for (int iTest = 0; iTest < numTests; iTest++)
{
if (dbgTest > 0)
{
rnd.state = dbgTest;
}
uint testState = rnd.state;
// Generate random transform
float distance = math.pow(10.0f, rnd.NextFloat(-15.0f, -1.0f));
float angle = math.pow(10.0f, rnd.NextFloat(-15.0f, 0.0f));
MTransform queryFromTarget = new MTransform(
(rnd.NextInt(10) > 0) ? quaternion.AxisAngle(rnd.NextFloat3Direction(), angle) : quaternion.identity,
(rnd.NextInt(10) > 0) ? rnd.NextFloat3Direction() * distance : float3.zero);
TestConvexConvexDistance(collider, collider, queryFromTarget, "ConvexConvexDistanceEdgeCaseTest failed " + iShape + ", " + iTest + " (" + shapeState + ", " + testState + ")");
}
}
}
// followed by variable sized convex hull data
#region Construction
// Create a convex collider from the given point cloud.
public static unsafe BlobAssetReference <Collider> Create(
NativeArray <float3> points, float convexRadius,
float3?scale = null, CollisionFilter?filter = null, Material?material = null)
{
if (convexRadius < 0.0f || !math.isfinite(convexRadius))
{
throw new ArgumentException("Tried to create ConvexCollider with invalid convex radius");
}
// Build convex hull
int verticesCapacity = points.Length;
int triangleCapacity = 2 * verticesCapacity;
var vertices = (ConvexHullBuilder.Vertex *)UnsafeUtility.Malloc(verticesCapacity * sizeof(ConvexHullBuilder.Vertex), 16, Allocator.Temp);
var triangles = (ConvexHullBuilder.Triangle *)UnsafeUtility.Malloc(triangleCapacity * sizeof(ConvexHullBuilder.Triangle), 16, Allocator.Temp);
var builder = new ConvexHullBuilder(vertices, verticesCapacity, triangles, triangleCapacity);
float3 s = scale ?? new float3(1);
// Build the points' AABB and validate them
var domain = new Aabb();
foreach (var point in points)
{
if (math.any(!math.isfinite(point)))
{
throw new ArgumentException("Tried to create ConvexCollider with invalid points");
}
domain.Include(point * s);
}
// Add points to the hull
builder.IntegerSpaceAabb = domain;
foreach (float3 point in points)
{
builder.AddPoint(point * s);
}
// TODO: shrink by convex radius
// Build face information
float maxAngle = 0.1f * (float)math.PI / 180.0f;
builder.BuildFaceIndices(maxAngle);
// Simplify the hull until it's under the max vertices requirement
// TODO.ma this is just a failsafe. We need to think about user-controlled simplification settings & how to warn the user if their shape is too complex.
{
const int maxVertices = 252; // as per Havok
float maxSimplificationError = 1e-3f;
int iterations = 0;
while (builder.Vertices.PeakCount > maxVertices)
{
if (iterations++ > 10) // don't loop forever
{
Assert.IsTrue(false);
return(new BlobAssetReference <Collider>());
}
builder.SimplifyVertices(maxSimplificationError);
builder.BuildFaceIndices();
maxSimplificationError *= 2.0f;
}
}
// Convert hull to compact format
var tempHull = new TempHull(ref builder);
// Allocate collider
int totalSize = UnsafeUtility.SizeOf <ConvexCollider>();
totalSize += tempHull.Vertices.Count * sizeof(float3);
totalSize = Math.NextMultipleOf16(totalSize); // planes currently must be aligned for Havok
totalSize += tempHull.Planes.Count * sizeof(Plane);
totalSize += tempHull.Faces.Count * sizeof(ConvexHull.Face);
totalSize += tempHull.FaceVertexIndices.Count * sizeof(short);
totalSize += tempHull.VertexEdges.Count * sizeof(ConvexHull.Edge);
totalSize += tempHull.FaceLinks.Count * sizeof(ConvexHull.Edge);
ConvexCollider *collider = (ConvexCollider *)UnsafeUtility.Malloc(totalSize, 16, Allocator.Temp);
// Initialize it
{
UnsafeUtility.MemClear(collider, totalSize);
collider->MemorySize = totalSize;
collider->m_Header.Type = ColliderType.Convex;
collider->m_Header.CollisionType = CollisionType.Convex;
collider->m_Header.Version = 0;
collider->m_Header.Magic = 0xff;
collider->m_Header.Filter = filter ?? CollisionFilter.Default;
collider->m_Header.Material = material ?? Material.Default;
ref var hull = ref collider->ConvexHull;
hull.ConvexRadius = convexRadius;
// Initialize blob arrays
{
byte *end = (byte *)collider + UnsafeUtility.SizeOf <ConvexCollider>();
hull.VerticesBlob.Offset = (int)(end - (byte *)UnsafeUtility.AddressOf(ref hull.VerticesBlob.Offset));
hull.VerticesBlob.Length = tempHull.Vertices.Count;
end += sizeof(float3) * tempHull.Vertices.Count;
end = (byte *)Math.NextMultipleOf16((ulong)end); // planes currently must be aligned for Havok
hull.FacePlanesBlob.Offset = (int)(end - (byte *)UnsafeUtility.AddressOf(ref hull.FacePlanesBlob.Offset));
hull.FacePlanesBlob.Length = tempHull.Planes.Count;
end += sizeof(Plane) * tempHull.Planes.Count;
hull.FacesBlob.Offset = (int)(end - (byte *)UnsafeUtility.AddressOf(ref hull.FacesBlob.Offset));
hull.FacesBlob.Length = tempHull.Faces.Count;
end += sizeof(ConvexHull.Face) * tempHull.Faces.Count;
hull.FaceVertexIndicesBlob.Offset = (int)(end - (byte *)UnsafeUtility.AddressOf(ref hull.FaceVertexIndicesBlob.Offset));
hull.FaceVertexIndicesBlob.Length = tempHull.FaceVertexIndices.Count;
end += sizeof(byte) * tempHull.FaceVertexIndices.Count;
hull.VertexEdgesBlob.Offset = (int)(end - (byte *)UnsafeUtility.AddressOf(ref hull.VertexEdgesBlob.Offset));
hull.VertexEdgesBlob.Length = tempHull.VertexEdges.Count;
end += sizeof(ConvexHull.Edge) * tempHull.VertexEdges.Count;
hull.FaceLinksBlob.Offset = (int)(end - (byte *)UnsafeUtility.AddressOf(ref hull.FaceLinksBlob.Offset));
hull.FaceLinksBlob.Length = tempHull.FaceLinks.Count;
end += sizeof(ConvexHull.Edge) * tempHull.FaceLinks.Count;
}
// Fill blob arrays
{
for (int i = 0; i < tempHull.Vertices.Count; i++)
{
hull.Vertices[i] = tempHull.Vertices[i];
hull.VertexEdges[i] = tempHull.VertexEdges[i];
}
for (int i = 0; i < tempHull.Faces.Count; i++)
{
hull.Planes[i] = tempHull.Planes[i];
hull.Faces[i] = tempHull.Faces[i];
}
for (int i = 0; i < tempHull.FaceVertexIndices.Count; i++)
{
hull.FaceVertexIndices[i] = tempHull.FaceVertexIndices[i];
hull.FaceLinks[i] = tempHull.FaceLinks[i];
}
}
// Fill mass properties
{
var massProperties = builder.ComputeMassProperties();
Math.DiagonalizeSymmetricApproximation(massProperties.InertiaTensor, out float3x3 orientation, out float3 inertia);
float maxLengthSquared = 0.0f;
foreach (float3 vertex in hull.Vertices)
{
maxLengthSquared = math.max(maxLengthSquared, math.lengthsq(vertex - massProperties.CenterOfMass));
}
collider->MassProperties = new MassProperties
{
MassDistribution = new MassDistribution
{
Transform = new RigidTransform(orientation, massProperties.CenterOfMass),
InertiaTensor = inertia
},
Volume = massProperties.Volume,
AngularExpansionFactor = math.sqrt(maxLengthSquared)
};
}
}
protected override void OnUpdate()
{
// Complete the simulation
Dependency.Complete();
NativeList <TriggerEvent> triggerEvents = new NativeList <TriggerEvent>(Allocator.TempJob);
var collectTriggerEventsJob = new CollectTriggerEventsJob
{
m_TriggerEvents = triggerEvents
};
// Collect all events
var handle = collectTriggerEventsJob.Schedule(m_StepPhysicsWorldSystem.Simulation, Dependency);
handle.Complete();
var physicsWorld = m_BuildPhysicsWorldSystem.PhysicsWorld;
int expectedNumberOfTriggerEvents = 0;
Entities
.WithName("ValidateTriggerEventsJob")
.WithReadOnly(physicsWorld)
.WithReadOnly(triggerEvents)
.WithoutBurst()
.ForEach((ref Entity entity, ref TriggerEventChecker component) =>
{
int numTriggerEvents = 0;
TriggerEvent triggerEvent = default;
expectedNumberOfTriggerEvents += component.NumExpectedEvents;
for (int i = 0; i < triggerEvents.Length; i++)
{
if (triggerEvents[i].EntityA == entity || triggerEvents[i].EntityB == entity)
{
triggerEvent = triggerEvents[i];
numTriggerEvents++;
}
}
Assert.IsTrue(numTriggerEvents == component.NumExpectedEvents, "Missing events!");
if (numTriggerEvents == 0)
{
return;
}
// Even if component.NumExpectedEvents is > 1, we still take one trigger event, and not all, because the only
// difference will be in ColliderKeys which we're not checking here
int nonTriggerBodyIndex = triggerEvent.EntityA == entity ? triggerEvent.BodyIndexA : triggerEvent.BodyIndexB;
int triggerBodyIndex = triggerEvent.EntityA == entity ? triggerEvent.BodyIndexB : triggerEvent.BodyIndexA;
Assert.IsTrue(nonTriggerBodyIndex == physicsWorld.GetRigidBodyIndex(entity), "Wrong body index!");
RigidBody nonTriggerBody = physicsWorld.Bodies[nonTriggerBodyIndex];
RigidBody triggerBody = physicsWorld.Bodies[triggerBodyIndex];
bool isTrigger = false;
unsafe
{
ConvexCollider *colliderPtr = (ConvexCollider *)triggerBody.Collider.GetUnsafePtr();
var material = colliderPtr->Material;
isTrigger = colliderPtr->Material.CollisionResponse == CollisionResponsePolicy.RaiseTriggerEvents;
}
Assert.IsTrue(isTrigger, "Event doesn't have valid trigger index");
float distance = math.distance(triggerBody.WorldFromBody.pos, nonTriggerBody.WorldFromBody.pos);
Assert.IsTrue(distance < 10.0f, "The trigger index is wrong!");
}).Run();
Assert.IsTrue(expectedNumberOfTriggerEvents == triggerEvents.Length, "Incorrect number of events: Expected: " + expectedNumberOfTriggerEvents + " Actual: " + triggerEvents.Length);
triggerEvents.Dispose();
}
