public void UpdateWorldOneDynamicBoxTest()
{
Physics.PhysicsWorld world = BroadPhaseTests.createTestWorld(0, 1);
BroadPhaseTests.addDynamicBoxToWorld(world, 0, Vector3.zero, quaternion.identity, new Vector3(10, .1f, 10));
world.CollisionWorld.UpdateDynamicTree(ref world, 1 / 60, -9.81f * math.up());
world.Dispose();
}
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);
}
public void WorldTest()
{
var world = new Physics.PhysicsWorld(10, 5, 10);
Assert.IsTrue((world.NumDynamicBodies == 5) && (world.NumStaticBodies == 10) && (world.NumBodies == 15) && (world.NumJoints == 10));
world.Reset(0, 0, 0);
Assert.IsTrue((world.NumDynamicBodies == 0) && (world.NumStaticBodies == 0) && (world.NumBodies == 0) && (world.NumJoints == 0));
world.Reset(5, 1, 7);
Assert.IsTrue((world.NumDynamicBodies == 1) && (world.NumStaticBodies == 5) && (world.NumBodies == 6) && (world.NumJoints == 7));
// clone world
var worldClone = new Physics.PhysicsWorld(0, 0, 0);
Assert.IsTrue((worldClone.NumDynamicBodies == 0) && (worldClone.NumStaticBodies == 0) && (worldClone.NumBodies == 0) && (worldClone.NumJoints == 0));
worldClone.Dispose();
worldClone = (Physics.PhysicsWorld)world.Clone();
Assert.IsTrue((worldClone.NumDynamicBodies == 1) && (worldClone.NumStaticBodies == 5) && (worldClone.NumBodies == 6) && (worldClone.NumJoints == 7));
// dispose cloned world
worldClone.Dispose();
world.Dispose();
}
public void BuildBPEmptyWorldTest()
{
Physics.PhysicsWorld world = createTestWorld();
world.CollisionWorld.Broadphase.Build(world.StaticBodies, world.DynamicBodies, world.MotionVelocities,
world.CollisionWorld.CollisionTolerance, 1 / 60, -9.81f * math.up());
world.Dispose();
}
static unsafe void CheckRaycastHit(ref Physics.PhysicsWorld world, RaycastInput input, RaycastHit hit, string failureMessage)
{
// Fetch the leaf collider
ChildCollider leaf;
{
Physics.RigidBody body = world.Bodies[hit.RigidBodyIndex];
Collider.GetLeafCollider(body.Collider, body.WorldFromBody, hit.ColliderKey, out leaf);
}
// Check that the hit position matches the fraction
float3 hitPosition = input.Ray.Origin + input.Ray.Direction * hit.Fraction;
Assert.Less(math.length(hitPosition - hit.Position), tolerance, failureMessage + ": inconsistent fraction and position");
// Query the hit position and check that it's on the surface of the shape
PointDistanceInput pointInput = new PointDistanceInput
{
Position = math.transform(math.inverse(leaf.TransformFromChild), hit.Position),
MaxDistance = float.MaxValue
};
DistanceHit distanceHit;
leaf.Collider->CalculateDistance(pointInput, out distanceHit);
if (((ConvexCollider *)leaf.Collider)->ConvexHull.ConvexRadius > 0.0f)
{
// Convex raycast approximates radius, so it's possible that the hit position is not exactly on the shape, but must at least be outside
Assert.Greater(distanceHit.Distance, -tolerance, failureMessage);
}
else
{
Assert.AreEqual(distanceHit.Distance, 0.0f, tolerance, failureMessage);
}
}
public void load(Physics.PhysicsWorld physics_world)
{
_player.controlAndWatch(_character_1, _camera_1);
_player.getPhysical(physics_world);
_scene.load(physics_world, _config.near_far.X);
_scene.toggleFlashlight(_player.enable_flashlight);
}
public void BuildBPOneDynamicBoxTest()
{
Physics.PhysicsWorld world = createTestWorld(0, 1);
addDynamicBoxToWorld(world, 0, new Vector3(0, 0, 0), Quaternion.identity, new Vector3(10, 10, 10));
world.CollisionWorld.Broadphase.Build(world.StaticBodies, world.DynamicBodies, world.MotionVelocities,
world.CollisionWorld.CollisionTolerance, 1 / 60, -9.81f * math.up());
world.Dispose();
}
static unsafe void GetHitLeaf(ref Physics.PhysicsWorld world, int rigidBodyIndex, ColliderKey colliderKey, MTransform queryFromWorld, out ChildCollider leaf, out MTransform queryFromTarget)
{
Physics.RigidBody body = world.Bodies[rigidBodyIndex];
Collider.GetLeafCollider(body.Collider, body.WorldFromBody, colliderKey, out leaf);
MTransform worldFromLeaf = new MTransform(leaf.TransformFromChild);
queryFromTarget = Math.Mul(queryFromWorld, worldFromLeaf);
}
public void ScheduleBuildJobsOneStaticBoxTest()
{
Physics.PhysicsWorld world = createTestWorld(1);
addStaticBoxToWorld(world, 0, new Vector3(0, 0, 0), Quaternion.identity, new Vector3(10, 0.1f, 10));
Unity.Jobs.JobHandle handle = new Unity.Jobs.JobHandle();
Unity.Jobs.JobHandle result = world.CollisionWorld.Broadphase.ScheduleBuildJobs(ref world, 1 / 60, 1, true, handle);
result.Complete();
Assert.IsTrue(result.IsCompleted);
world.Dispose();
}
public void WorldTest()
{
var world = new Physics.PhysicsWorld(1, 1, 0);
Assert.IsTrue((world.NumStaticBodies == 1) && (world.NumDynamicBodies == 1) && (world.NumBodies == 2));
// TODO: add test for dynamics
world.Dispose();
}
//Adds a dynamic box to the world
static public unsafe void addDynamicBoxToWorld(Physics.PhysicsWorld world, int index, Vector3 pos, Quaternion orientation, Vector3 size)
{
Assert.IsTrue(index < world.NumDynamicBodies, "Dynamic body index is out of range in addDynamicBoxToWorld");
Unity.Collections.NativeSlice <Physics.RigidBody> dynamicBodies = world.DynamicBodies;
Physics.RigidBody rb = dynamicBodies[index];
BlobAssetReference <Physics.Collider> collider = Unity.Physics.BoxCollider.Create(pos, orientation, size, .01f);
rb.Collider = (Collider *)collider.GetUnsafePtr();
dynamicBodies[index] = rb;
}
public void UpdateWorldOneHundredDynamicBoxesTest()
{
Physics.PhysicsWorld world = BroadPhaseTests.createTestWorld(0, 100);
for (int i = 0; i < 100; ++i)
{
BroadPhaseTests.addDynamicBoxToWorld(world, i, new Vector3(11 * i, 0, 0), quaternion.identity, new Vector3(10, .1f, 10));
}
world.CollisionWorld.UpdateDynamicTree(ref world, 1 / 60, -9.81f * math.up());
world.Dispose();
}
public void BuildBPOneHundredDynamicBoxesTest()
{
Physics.PhysicsWorld world = createTestWorld(0, 100);
for (int i = 0; i < 100; ++i)
{
addDynamicBoxToWorld(world, i, new Vector3(i * 11, 0, 0), Quaternion.identity, new Vector3(10, 10, 10));
}
world.CollisionWorld.Broadphase.Build(world.StaticBodies, world.DynamicBodies, world.MotionVelocities,
world.CollisionWorld.CollisionTolerance, 1 / 60, -9.81f * math.up());
world.Dispose();
}
public void SheduleUpdateJobsEmptyWorldTest()
{
for (int numThreads = 0; numThreads <= 8; numThreads++)
{
Physics.PhysicsWorld world = BroadPhaseTests.createTestWorld();
Unity.Jobs.JobHandle handle = new Unity.Jobs.JobHandle();
Unity.Jobs.JobHandle worldJobHandle = world.CollisionWorld.ScheduleUpdateDynamicTree(ref world, 1 / 60, -9.81f * math.up(), handle, numThreads);
worldJobHandle.Complete();
Assert.IsTrue(worldJobHandle.IsCompleted);
world.Dispose();
}
}
// 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.
}
public void SheduleUpdateJobsOneHundredStaticBoxesTest()
{
Physics.PhysicsWorld world = BroadPhaseTests.createTestWorld(100);
for (int i = 0; i < 100; ++i)
{
BroadPhaseTests.addStaticBoxToWorld(world, i, new Vector3(11 * i, 0, 0), quaternion.identity, new Vector3(10, .1f, 10));
}
Unity.Jobs.JobHandle handle = new Unity.Jobs.JobHandle();
Unity.Jobs.JobHandle worldJobHandle = world.CollisionWorld.ScheduleUpdateDynamicLayer(ref world, 1 / 60, 1, handle);
worldJobHandle.Complete();
Assert.IsTrue(worldJobHandle.IsCompleted);
world.Dispose();
}
public void SheduleUpdateJobsOneDynamicBoxTest()
{
for (int numThreads = 0; numThreads <= 8; numThreads++)
{
Physics.PhysicsWorld world = BroadPhaseTests.createTestWorld(0, 1);
BroadPhaseTests.addDynamicBoxToWorld(world, 0, Vector3.zero, quaternion.identity, new Vector3(10, .1f, 10));
Unity.Jobs.JobHandle handle = new Unity.Jobs.JobHandle();
Unity.Jobs.JobHandle worldJobHandle = world.CollisionWorld.ScheduleUpdateDynamicTree(ref world, 1 / 60, -9.81f * math.up(), handle, numThreads);
worldJobHandle.Complete();
Assert.IsTrue(worldJobHandle.IsCompleted);
world.Dispose();
}
}
public void ScheduleBuildJobsEmptyWorldTest()
{
for (int numThreads = 0; numThreads <= 8; numThreads++)
{
Physics.PhysicsWorld world = createTestWorld();
var handle = new JobHandle();
var buildStaticTree = new NativeArray <int>(1, Allocator.TempJob);
buildStaticTree[0] = 1;
JobHandle result = world.CollisionWorld.Broadphase.ScheduleBuildJobs(ref world, 1 / 60, -9.81f * math.up(), buildStaticTree, handle, numThreads);
result.Complete();
world.Dispose();
buildStaticTree.Dispose();
}
}
internal World(bool makeCurrent = false, bool hasPhysics = true)
{
entities = new HashSet <Entity>();
NativePtr = CreateWorld_Native(makeCurrent, hasPhysics);
if (hasPhysics)
{
PhysicsWorld = new Physics.PhysicsWorld();
PhysicsWorld.NativePtr = GetPhysicsWorld_Native(NativePtr);
}
else
{
PhysicsWorld = null;
}
}
public void SheduleUpdateJobsTenStaticBoxesTest()
{
for (int numThreads = 0; numThreads <= 1; numThreads++)
{
Physics.PhysicsWorld world = BroadPhaseTests.createTestWorld(10);
for (int i = 0; i < 10; ++i)
{
BroadPhaseTests.addStaticBoxToWorld(world, i, new Vector3(11 * i, 0, 0), quaternion.identity, new Vector3(10, .1f, 10));
}
Unity.Jobs.JobHandle handle = new Unity.Jobs.JobHandle();
Unity.Jobs.JobHandle worldJobHandle = world.CollisionWorld.ScheduleUpdateDynamicTree(ref world, 1 / 60, -9.81f * math.up(), handle, numThreads == 1);
worldJobHandle.Complete();
Assert.IsTrue(worldJobHandle.IsCompleted);
world.Dispose();
}
}
//Adds a static box to the world
static public unsafe void addStaticBoxToWorld(Physics.PhysicsWorld world, int index, Vector3 pos, Quaternion orientation, Vector3 size)
{
Assert.IsTrue(index < world.NumStaticBodies, "Static body index is out of range in addStaticBoxToWorld");
Unity.Collections.NativeSlice <Physics.RigidBody> staticBodies = world.StaticBodies;
Physics.RigidBody rb = staticBodies[index];
BlobAssetReference <Physics.Collider> collider = Unity.Physics.BoxCollider.Create(new BoxGeometry
{
Center = pos,
Orientation = orientation,
Size = size,
BevelRadius = 0.01f
});
rb.Collider = (Collider *)collider.GetUnsafePtr();
staticBodies[index] = rb;
}
public void ScheduleBuildJobsOneStaticBoxTest()
{
for (int numThreads = 0; numThreads <= 8; numThreads++)
{
Physics.PhysicsWorld world = createTestWorld(1);
addStaticBoxToWorld(world, 0, new Vector3(0, 0, 0), Quaternion.identity, new Vector3(10, 0.1f, 10));
var handle = new JobHandle();
var buildStaticTree = new NativeArray <int>(1, Allocator.TempJob);
buildStaticTree[0] = 1;
JobHandle result = world.CollisionWorld.Broadphase.ScheduleBuildJobs(ref world, 1 / 60, -9.81f * math.up(), buildStaticTree, handle, numThreads);
result.Complete();
Assert.IsTrue(result.IsCompleted);
world.Dispose();
buildStaticTree.Dispose();
}
}
public void ScheduleBuildJobsOneStaticBoxTest()
{
Physics.PhysicsWorld world = createTestWorld(1);
addStaticBoxToWorld(world, 0, new Vector3(0, 0, 0), Quaternion.identity, new Vector3(10, 0.1f, 10));
JobHandle handle = new JobHandle();
StaticLayerChangeInfo staticLayerChangeInfo = new StaticLayerChangeInfo();
staticLayerChangeInfo.Init(Allocator.TempJob);
staticLayerChangeInfo.NumStaticBodies = 1;
staticLayerChangeInfo.HaveStaticBodiesChanged = 1;
JobHandle result = world.CollisionWorld.Broadphase.ScheduleBuildJobs(ref world, 1 / 60, -9.81f * math.up(), 1, ref staticLayerChangeInfo, handle);
result.Complete();
Assert.IsTrue(result.IsCompleted);
world.Dispose();
staticLayerChangeInfo.Deallocate();
}
//Adds a dynamic box to the world
static public unsafe void addDynamicBoxToWorld(Physics.PhysicsWorld world, int index, Vector3 pos, Quaternion orientation, Vector3 size)
{
Assert.IsTrue(index < world.NumDynamicBodies, "Dynamic body index is out of range in addDynamicBoxToWorld");
NativeArray <Physics.RigidBody> dynamicBodies = world.DynamicBodies;
Physics.RigidBody rb = dynamicBodies[index];
BlobAssetReference <Collider> collider = BoxCollider.Create(new BoxGeometry
{
Center = pos,
Orientation = orientation,
Size = size,
BevelRadius = 0.01f
});
rb.Collider = collider;
dynamicBodies[index] = rb;
}
// Does collider casts 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
// - Distance between the shapes at the hit fraction is zero
static unsafe void WorldColliderCastTest(ref Physics.PhysicsWorld world, ColliderCastInput input, ref NativeList <ColliderCastHit> hits, string failureMessage)
{
// Do an all-hits query
hits.Clear();
world.CastCollider(input, ref hits);
// Check each hit and find the earliest
float minFraction = float.MaxValue;
RigidTransform worldFromQuery = new RigidTransform(input.Orientation, input.Position);
for (int iHit = 0; iHit < hits.Length; iHit++)
{
ColliderCastHit hit = hits[iHit];
minFraction = math.min(minFraction, hit.Fraction);
CheckColliderCastHit(ref world, input, hit, failureMessage + ", hits[" + iHit + "]");
}
// Do a closest-hit query and check that the fraction matches
ColliderCastHit closestHit;
bool hasClosestHit = world.CastCollider(input, out closestHit);
if (hits.Length == 0)
{
Assert.IsFalse(hasClosestHit, failureMessage + ", closestHit: no matching result in hits");
}
else
{
Assert.AreEqual(closestHit.Fraction, minFraction, tolerance * math.length(input.Direction), failureMessage + ", closestHit: fraction does not match");
CheckColliderCastHit(ref world, input, closestHit, failureMessage + ", closestHit");
}
// Do an any-hit query and check that it is consistent with the others
bool hasAnyHit = world.CastCollider(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.
}
public void UpdateEmptyWorldTest()
{
Physics.PhysicsWorld world = BroadPhaseTests.createTestWorld();
world.CollisionWorld.UpdateDynamicTree(ref world, 1 / 60, -9.81f * math.up());
world.Dispose();
}
public unsafe void WorldQueryTest()
{
const uint seed = 0x12345678;
uint dbgWorld = 0; // set dbgWorld, dbgTest to the seed reported from a failure message to repeat the failing case alone
uint dbgTest = 0;
int numWorlds = 200;
int numTests = 5000;
if (dbgWorld > 0)
{
numWorlds = 1;
numTests = 1;
}
Random rnd = new Random(seed);
NativeList <DistanceHit> distanceHits = new NativeList <DistanceHit>(Allocator.Temp);
NativeList <ColliderCastHit> colliderCastHits = new NativeList <ColliderCastHit>(Allocator.Temp);
NativeList <RaycastHit> raycastHits = new NativeList <RaycastHit>(Allocator.Temp);
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), 10.0f);
for (int iTest = 0; iTest < (numTests / numWorlds); iTest++)
{
if (dbgTest > 0)
{
rnd.state = dbgTest;
}
uint testState = rnd.state;
string failureMessage = iWorld + ", " + iTest + " (" + worldState.ToString() + ", " + testState.ToString() + ")";
// Generate common random query inputs
Collider * collider = (Collider *)TestUtils.GenerateRandomConvex(ref rnd).GetUnsafePtr();
RigidTransform transform = new RigidTransform
{
pos = rnd.NextFloat3(-10.0f, 10.0f),
rot = (rnd.NextInt(10) > 0) ? rnd.NextQuaternionRotation() : quaternion.identity,
};
Ray ray = new Ray(transform.pos, rnd.NextFloat3(-5.0f, 5.0f));
// Distance test
{
ColliderDistanceInput input = new ColliderDistanceInput
{
Collider = collider,
Transform = transform,
MaxDistance = (rnd.NextInt(4) > 0) ? rnd.NextFloat(5.0f) : 0.0f
};
WorldCalculateDistanceTest(ref world, input, ref distanceHits, "WorldQueryTest failed CalculateDistance " + failureMessage);
}
// Collider cast test
{
ColliderCastInput input = new ColliderCastInput
{
Collider = collider,
Position = transform.pos,
Orientation = transform.rot,
Direction = ray.Direction
};
WorldColliderCastTest(ref world, input, ref colliderCastHits, "WorldQueryTest failed ColliderCast " + failureMessage);
}
// Ray cast test
{
RaycastInput input = new RaycastInput
{
Ray = ray,
Filter = CollisionFilter.Default
};
WorldRaycastTest(ref world, input, ref raycastHits, "WorldQueryTest failed Raycast " + failureMessage);
}
}
world.Dispose(); // TODO leaking memory if the test fails
}
distanceHits.Dispose(); // TODO leaking memory if the test fails
colliderCastHits.Dispose();
raycastHits.Dispose();
}
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
}
}
public void CalculateDetailsTest()
{
// Simple collision event with straight normal and 3 contact points
LowLevel.CollisionEvent lowLevelEvent = new LowLevel.CollisionEvent();
lowLevelEvent.BodyIndices.BodyAIndex = 0;
lowLevelEvent.BodyIndices.BodyBIndex = 1;
lowLevelEvent.ColliderKeys.ColliderKeyA = ColliderKey.Empty;
lowLevelEvent.ColliderKeys.ColliderKeyB = ColliderKey.Empty;
lowLevelEvent.Normal = new float3(0.0f, -1.00000f, 0.0f);
lowLevelEvent.NumNarrowPhaseContactPoints = 3;
lowLevelEvent.SolverImpulse = 1.0f;
// Wrapping collision event
CollisionEvent collisionEvent = new CollisionEvent();
collisionEvent.EventData = lowLevelEvent;
collisionEvent.TimeStep = 1.0f / 60.0f;
// Input velocity is obviously separating, but high angular velocity still caused an event
collisionEvent.InputVelocityA = new Velocity
{
Angular = new float3(-0.00064f, 11.17604f, 0.02133f),
Linear = new float3(-3.81205f, -0.56607f, 9.14945f)
};
collisionEvent.InputVelocityB = new Velocity
{
Angular = new float3(0.00000f, 0.00000f, 0.00000f),
Linear = new float3(0.00000f, 0.00000f, 0.00000f)
};
// Initialize 3 contact points
collisionEvent.NarrowPhaseContactPoints = new NativeArray <ContactPoint>(3, Allocator.Temp)
{
[0] = new ContactPoint
{
Distance = 0.177905f,
Position = new float3(-22.744950f, 2.585318f, -50.108990f)
},
[1] = new ContactPoint
{
Distance = 0.276652f,
Position = new float3(-20.731140f, 2.486506f, -50.322240f)
},
[2] = new ContactPoint
{
Distance = 0.278534f,
Position = new float3(-20.766140f, 2.484623f, -50.652630f)
}
};
// Allocate a simple world of 1 dynamic and 1 static body
var simpleWorld = new Physics.PhysicsWorld(1, 1, 0);
var motionVelocities = simpleWorld.MotionVelocities;
var motionDatas = simpleWorld.MotionDatas;
motionDatas[0] = new MotionData
{
LinearDamping = 0.0f,
AngularDamping = 0.0f,
GravityFactor = 1.0f,
BodyFromMotion = new RigidTransform(new quaternion(0.0f, 0.0f, 0.0f, 1.0f), new float3(0.0f, 0.0f, 0.0f)),
WorldFromMotion = new RigidTransform(new quaternion(0.09212853f, 0.1400256f, -0.006776567f, -0.9858292f), new float3(-22.17587f, 0.5172966f, -52.24425f))
};
motionVelocities[0] = new MotionVelocity
{
LinearVelocity = new float3(-3.81221f, -1.37538f, -15.41893f),
AngularVelocity = new float3(-7.30913f, -4.78899f, 1.14168f),
InverseInertiaAndMass = new float4(0.00045f, 0.00045f, 0.00045f, 0.00018f),
AngularExpansionFactor = 2.05061f
};
// Calculate the collision event details and make sure 1 contact point is returned
var details = collisionEvent.CalculateDetails(ref simpleWorld);
Assert.AreEqual(details.EstimatedContactPointPositions.Length, 1);
// Dispose the world data
simpleWorld.Dispose();
}
