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();
}
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();
}
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();
}
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();
}
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 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();
}
}
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();
}
}
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 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();
}
}
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();
}
public void UpdateEmptyWorldTest()
{
Physics.PhysicsWorld world = BroadPhaseTests.createTestWorld();
world.CollisionWorld.UpdateDynamicTree(ref world, 1 / 60, -9.81f * math.up());
world.Dispose();
}
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();
}
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 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();
}