public void ReadCollisionEvents()
{
// Allocate a block stream for up to 10 parallel writes
BlockStream collisionEventStream = new BlockStream(10, 0xabbaabba);
// Do a couple of writes to different forEach indices
int writeCount = 0;
{
BlockStream.Writer collisionEventWriter = collisionEventStream;
collisionEventWriter.BeginForEachIndex(1);
collisionEventWriter.Write(new CollisionEvent());
writeCount++;
collisionEventWriter.Write(new CollisionEvent());
writeCount++;
collisionEventWriter.EndForEachIndex();
collisionEventWriter.BeginForEachIndex(3);
collisionEventWriter.Write(new CollisionEvent());
writeCount++;
collisionEventWriter.EndForEachIndex();
collisionEventWriter.BeginForEachIndex(5);
collisionEventWriter.Write(new CollisionEvent());
writeCount++;
collisionEventWriter.Write(new CollisionEvent());
writeCount++;
collisionEventWriter.EndForEachIndex();
collisionEventWriter.BeginForEachIndex(7);
collisionEventWriter.Write(new CollisionEvent());
writeCount++;
collisionEventWriter.EndForEachIndex();
collisionEventWriter.BeginForEachIndex(9);
collisionEventWriter.Write(new CollisionEvent());
writeCount++;
collisionEventWriter.Write(new CollisionEvent());
writeCount++;
collisionEventWriter.EndForEachIndex();
}
// Iterate over written events and make sure they are all read
CollisionEvents collisionEvents = new CollisionEvents(collisionEventStream);
int readCount = 0;
foreach (var collisionEvent in collisionEvents)
{
readCount++;
}
Assert.IsTrue(readCount == writeCount);
// Cleanup
var disposeJob = collisionEventStream.ScheduleDispose(default);
static void CreateBlockStream1And2Int(out BlockStream stream)
{
stream = new BlockStream(2, 0x9b98651c);
BlockStream.Writer writer = stream;
writer.BeginForEachIndex(0);
writer.Write(0);
writer.EndForEachIndex();
writer.BeginForEachIndex(1);
writer.Write(1);
writer.Write(2);
writer.EndForEachIndex();
}
unsafe void SolveSingleJoint(JointData *jointData, int numIterations, float timestep,
ref MotionVelocity velocityA, ref MotionVelocity velocityB, ref MotionData motionA, ref MotionData motionB, out BlockStream jacobiansOut)
{
Solver.StepInput stepInput = new Solver.StepInput
{
IsLastIteration = false,
InvNumSolverIterations = 1.0f / numIterations,
Timestep = timestep,
InvTimestep = timestep > 0.0f ? 1.0f / timestep : 0.0f
};
// Build jacobians
jacobiansOut = new BlockStream(1, 0);
{
BlockStream.Writer jacobianWriter = jacobiansOut;
jacobianWriter.BeginForEachIndex(0);
Solver.BuildJointJacobian(jointData, new BodyIndexPair(), velocityA, velocityB, motionA, motionB, timestep, numIterations, ref jacobianWriter);
jacobianWriter.EndForEachIndex();
}
BlockStream.Writer eventWriter = new BlockStream.Writer(); // no events expected
// Solve the joint
for (int iIteration = 0; iIteration < numIterations; iIteration++)
{
stepInput.IsLastIteration = (iIteration == numIterations - 1);
BlockStream.Reader jacobianReader = jacobiansOut;
JacobianIterator jacIterator = new JacobianIterator(jacobianReader, 0);
while (jacIterator.HasJacobiansLeft())
{
ref JacobianHeader header = ref jacIterator.ReadJacobianHeader();
header.Solve(ref velocityA, ref velocityB, stepInput, ref eventWriter, ref eventWriter);
}
}
public void EndForEachIndexWithoutBegin()
{
var stream = new BlockStream(1, 0x9b98651c);
BlockStream.Writer writer = stream;
Assert.Throws <ArgumentException>(() => writer.EndForEachIndex());
stream.Dispose();
}
public void Execute(int index)
{
Writer.BeginForEachIndex(index);
for (int i = 0; i != index; i++)
{
Writer.Write(i);
}
Writer.EndForEachIndex();
}
public unsafe void Execute(int workItemIndex)
{
int dispatchPairReadOffset = SolverSchedulerInfo.GetWorkItemReadOffset(workItemIndex, out int numPairsToRead);
ContactWriter.BeginForEachIndex(workItemIndex);
JointJacobianWriter.BeginForEachIndex(workItemIndex);
for (int i = 0; i < numPairsToRead; i++)
{
Scheduler.DispatchPair dispatchPair = PhasedDispatchPairs[dispatchPairReadOffset + i];
// Invalid pairs can exist by being disabled by users
if (dispatchPair.IsValid)
{
if (dispatchPair.IsContact)
{
// Create contact manifolds for this pair of bodies
var pair = new BodyIndexPair
{
BodyAIndex = dispatchPair.BodyAIndex,
BodyBIndex = dispatchPair.BodyBIndex
};
RigidBody rigidBodyA = World.Bodies[pair.BodyAIndex];
RigidBody rigidBodyB = World.Bodies[pair.BodyBIndex];
MotionVelocity motionVelocityA = pair.BodyAIndex < World.MotionVelocities.Length ?
World.MotionVelocities[pair.BodyAIndex] : MotionVelocity.Zero;
MotionVelocity motionVelocityB = pair.BodyBIndex < World.MotionVelocities.Length ?
World.MotionVelocities[pair.BodyBIndex] : MotionVelocity.Zero;
ManifoldQueries.BodyBody(rigidBodyA, rigidBodyB, motionVelocityA, motionVelocityB,
World.CollisionWorld.CollisionTolerance, TimeStep, pair, ref ContactWriter);
}
else
{
Joint joint = World.Joints[dispatchPair.JointIndex];
// Need to fetch the real body indices from the joint, as the scheduler may have reordered them
int bodyAIndex = joint.BodyPair.BodyAIndex;
int bodyBIndex = joint.BodyPair.BodyBIndex;
GetMotion(ref World, bodyAIndex, out MotionVelocity velocityA, out MotionData motionA);
GetMotion(ref World, bodyBIndex, out MotionVelocity velocityB, out MotionData motionB);
Solver.BuildJointJacobian(joint.JointData, joint.BodyPair, velocityA, velocityB, motionA, motionB, TimeStep, NumIterations, ref JointJacobianWriter);
}
}
}
JointJacobianWriter.EndForEachIndex();
ContactWriter.EndForEachIndex();
}
public void IncorrectTypedReads()
{
var stream = new BlockStream(1);
BlockStream.Writer writer = stream;
writer.BeginForEachIndex(0);
writer.Write <int>(5);
writer.EndForEachIndex();
BlockStream.Reader reader = stream;
reader.BeginForEachIndex(0);
Assert.Throws <UnityEngine.Assertions.AssertionException>(() => reader.Read <float>());
stream.Dispose();
}
public unsafe void Execute()
{
if (DummyRun)
{
return;
}
CollisionPairWriter.BeginForEachIndex(0);
CollisionFilter *bodyFilters = (CollisionFilter *)Filter.GetUnsafePtr();
var pairBuffer = new Broadphase.BodyPairWriter((BlockStream.Writer *)UnsafeUtility.AddressOf(ref CollisionPairWriter), bodyFilters);
Node *nodesPtr = (Node *)Nodes.GetUnsafePtr();
BoundingVolumeHierarchy.TreeOverlap(ref pairBuffer, nodesPtr, nodesPtr);
pairBuffer.Close();
CollisionPairWriter.EndForEachIndex();
}
public unsafe void Execute(int index)
{
Results.BeginForEachIndex(index);
int numRows = (Request.ImageResolution + Results.ForEachCount - 1) / Results.ForEachCount;
const float sphereRadius = 0.005f;
BlobAssetReference <Collider> sphere;
if (Request.CastSphere)
{
sphere = SphereCollider.Create(float3.zero, sphereRadius, Request.CollisionFilter);
}
for (int yCoord = index * numRows; yCoord < math.min(Request.ImageResolution, (index + 1) * numRows); yCoord++)
{
for (int xCoord = 0; xCoord < Request.ImageResolution; xCoord++)
{
float xFrac = 2.0f * ((xCoord / (float)Request.ImageResolution) - 0.5f);
float yFrac = 2.0f * ((yCoord / (float)Request.ImageResolution) - 0.5f);
float3 targetImagePlane = Request.ImageCenter + Request.Up * Request.PlaneHalfExtents * yFrac + Request.Right * Request.PlaneHalfExtents * xFrac;
float3 rayDir = Request.RayLength * (Request.PinHole - targetImagePlane);
RaycastHit hit;
bool hasHit;
if (Request.CastSphere)
{
var input = new ColliderCastInput
{
Collider = (Collider *)sphere.GetUnsafePtr(),
Orientation = quaternion.identity,
Start = Request.PinHole,
End = Request.PinHole + rayDir
};
hasHit = World.CastCollider(input, out ColliderCastHit colliderHit);
hit = new RaycastHit
{
Fraction = colliderHit.Fraction,
Position = colliderHit.Position,
SurfaceNormal = colliderHit.SurfaceNormal,
RigidBodyIndex = colliderHit.RigidBodyIndex,
ColliderKey = colliderHit.ColliderKey
};
}
else
{
var rayCastInput = new RaycastInput
{
Start = Request.PinHole,
End = Request.PinHole + rayDir,
Filter = Request.CollisionFilter
};
hasHit = World.CastRay(rayCastInput, out hit);
}
Color hitColor = Color.black;
if (hasHit)
{
if (hit.RigidBodyIndex < NumDynamicBodies)
{
hitColor = Color.yellow;
}
else
{
hitColor = Color.grey;
}
// Lighten alternate keys
if (Request.AlternateKeys && !hit.ColliderKey.Equals(ColliderKey.Empty))
{
Collider *collider = World.Bodies[hit.RigidBodyIndex].Collider;
hit.ColliderKey.PopSubKey(collider->NumColliderKeyBits, out uint key);
if (key % 2 == 0)
{
Color.RGBToHSV(hitColor, out float h, out float s, out float v);
hitColor = Color.HSVToRGB(h, s, v + 0.25f);
}
}
if (Request.Shadows)
{
float3 hitPos = Request.PinHole + rayDir * hit.Fraction + hit.SurfaceNormal * 0.001f;
bool shadowHit = false;
if (Request.CastSphere)
{
var start = hitPos + hit.SurfaceNormal * sphereRadius;
var input = new ColliderCastInput
{
Collider = (Collider *)sphere.GetUnsafePtr(),
Orientation = quaternion.identity,
Start = start,
End = start + (Request.LightDir * Request.RayLength),
};
ColliderCastHit colliderHit;
shadowHit = World.CastCollider(input, out colliderHit);
}
else
{
var rayCastInput = new RaycastInput
{
Start = hitPos,
End = hitPos + (Request.LightDir * Request.RayLength),
Filter = Request.CollisionFilter
};
RaycastHit shadowOutput;
shadowHit = World.CastRay(rayCastInput, out shadowOutput);
}
if (shadowHit)
{
hitColor *= 0.4f;
}
}
}
float lighting = math.min(1.0f, math.max(Request.AmbientLight, Vector3.Dot(hit.SurfaceNormal, Request.LightDir)));
Results.Write(xCoord);
Results.Write(yCoord);
Results.Write(hitColor * lighting);
}
}
Results.EndForEachIndex();
}
public unsafe void OverlapTaskFilteringTest([Values(2, 10, 33, 100)] int elementCount)
{
elementCount *= 2;
int numNodes = elementCount + Constants.MaxNumTreeBranches;
var points = new NativeArray <PointAndIndex>(elementCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
var aabbs = new NativeArray <Aabb>(elementCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
var bodyFilters = new NativeArray <CollisionFilter>(elementCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
InitInputWithCopyArrays(points, aabbs, bodyFilters);
var nodes = new NativeArray <Node>(numNodes, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
Node *nodesPtr = (Node *)nodes.GetUnsafePtr();
var seenUnfiltered = new HashSet <BodyIndexPair>();
{
var bvhUnfiltered = new BoundingVolumeHierarchy(nodes);
bvhUnfiltered.Build(points, aabbs, out int numNodesOut);
bvhUnfiltered.CheckIntegrity();
EverythingWriter pairWriter = new EverythingWriter {
SeenPairs = seenUnfiltered
};
BoundingVolumeHierarchy.TreeOverlap(ref pairWriter, nodesPtr, nodesPtr);
}
var nodeFilters = new NativeArray <CollisionFilter>(numNodes, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
var bvhFiltered = new BoundingVolumeHierarchy(nodes, nodeFilters);
int numNodesFilteredTree;
bvhFiltered.Build(points, aabbs, out numNodesFilteredTree);
bvhFiltered.BuildCombinedCollisionFilter(bodyFilters, 0, numNodesFilteredTree - 1);
var filteredCollisionPairs = new BlockStream(1, 0xec87b613);
BlockStream.Writer filteredPairWriter = filteredCollisionPairs;
filteredPairWriter.BeginForEachIndex(0);
CollisionFilter *bodyFiltersPtr = (CollisionFilter *)bodyFilters.GetUnsafePtr();
var bufferedPairs = new Broadphase.BodyPairWriter(&filteredPairWriter, bodyFiltersPtr);
CollisionFilter *nodeFiltersPtr = (CollisionFilter *)nodeFilters.GetUnsafePtr();
BoundingVolumeHierarchy.TreeOverlap(ref bufferedPairs, nodesPtr, nodesPtr, nodeFiltersPtr, nodeFiltersPtr);
bufferedPairs.Close();
filteredPairWriter.EndForEachIndex();
BlockStream.Reader filteredPairReader = filteredCollisionPairs;
filteredPairReader.BeginForEachIndex(0);
// Check that every pair in our filtered set also appears in the unfiltered set
while (filteredPairReader.RemainingItemCount > 0)
{
var pair = filteredPairReader.Read <BodyIndexPair>();
Assert.IsTrue(seenUnfiltered.Contains(pair));
seenUnfiltered.Remove(pair); // Remove the pair
}
// Pairs were removed, so the only remaining ones should be filtered
foreach (BodyIndexPair pair in seenUnfiltered)
{
bool shouldCollide = CollisionFilter.IsCollisionEnabled(bodyFilters[pair.BodyAIndex], bodyFilters[pair.BodyBIndex]);
Assert.IsFalse(shouldCollide);
}
nodeFilters.Dispose();
nodes.Dispose();
bodyFilters.Dispose();
aabbs.Dispose();
points.Dispose();
filteredCollisionPairs.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 Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex)
{
float3 up = math.up();
var chunkCCData = chunk.GetNativeArray(CharacterControllerComponentType);
var chunkCCInternalData = chunk.GetNativeArray(CharacterControllerInternalType);
var chunkPhysicsColliderData = chunk.GetNativeArray(PhysicsColliderType);
var chunkTranslationData = chunk.GetNativeArray(TranslationType);
var chunkRotationData = chunk.GetNativeArray(RotationType);
DeferredImpulseWriter.BeginForEachIndex(chunkIndex);
// Maximum number of hits character controller can store in world queries
const int maxQueryHits = 128;
var distanceHits = new NativeArray <DistanceHit>(maxQueryHits, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
var castHits = new NativeArray <ColliderCastHit>(maxQueryHits, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
var constraints = new NativeArray <SurfaceConstraintInfo>(4 * maxQueryHits, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
for (int i = 0; i < chunk.Count; i++)
{
var ccComponentData = chunkCCData[i];
var ccInternalData = chunkCCInternalData[i];
var collider = chunkPhysicsColliderData[i];
var position = chunkTranslationData[i];
var rotation = chunkRotationData[i];
// Collision filter must be valid
Assert.IsTrue(collider.ColliderPtr->Filter.IsValid);
// Character step input
CharacterControllerStepInput stepInput = new CharacterControllerStepInput
{
World = PhysicsWorld,
DeltaTime = DeltaTime,
Up = math.up(),
Gravity = ccComponentData.Gravity,
MaxIterations = ccComponentData.MaxIterations,
Tau = k_DefaultTau,
Damping = k_DefaultDamping,
SkinWidth = ccComponentData.SkinWidth,
ContactTolerance = ccComponentData.ContactTolerance,
MaxSlope = ccComponentData.MaxSlope,
RigidBodyIndex = PhysicsWorld.GetRigidBodyIndex(ccInternalData.Entity),
CurrentVelocity = ccInternalData.LinearVelocity
};
// Character transform
RigidTransform transform = new RigidTransform
{
pos = position.Value,
rot = rotation.Value
};
// "Broad phase" (used both for checking support and actual character collide and integrate).
MaxHitsCollector <DistanceHit> distanceHitsCollector = new MaxHitsCollector <DistanceHit>(
stepInput.RigidBodyIndex, ccComponentData.ContactTolerance, ref distanceHits);
{
ColliderDistanceInput input = new ColliderDistanceInput()
{
MaxDistance = ccComponentData.ContactTolerance,
Transform = transform,
Collider = collider.ColliderPtr
};
PhysicsWorld.CalculateDistance(input, ref distanceHitsCollector);
}
// Check support
CheckSupport(stepInput, transform, ccComponentData.MaxSlope, distanceHitsCollector,
ref constraints, out int numConstraints, out ccInternalData.SupportedState, out float3 surfaceNormal, out float3 surfaceVelocity);
// User input
float3 desiredVelocity = ccInternalData.LinearVelocity;
HandleUserInput(ccComponentData, stepInput.Up, surfaceVelocity, ref ccInternalData, ref desiredVelocity);
// Calculate actual velocity with respect to surface
if (ccInternalData.SupportedState == CharacterSupportState.Supported)
{
CalculateMovement(ccInternalData.CurrentRotationAngle, stepInput.Up, ccInternalData.IsJumping,
ccInternalData.LinearVelocity, desiredVelocity, surfaceNormal, surfaceVelocity, out ccInternalData.LinearVelocity);
}
else
{
ccInternalData.LinearVelocity = desiredVelocity;
}
// World collision + integrate
CollideAndIntegrate(stepInput, ccComponentData.CharacterMass, ccComponentData.AffectsPhysicsBodies > 0,
collider.ColliderPtr, ref castHits, ref constraints, numConstraints,
ref transform, ref ccInternalData.LinearVelocity, ref DeferredImpulseWriter);
// Write back and orientation integration
position.Value = transform.pos;
rotation.Value = quaternion.AxisAngle(up, ccInternalData.CurrentRotationAngle);
// Write back to chunk data
{
chunkCCInternalData[i] = ccInternalData;
chunkTranslationData[i] = position;
chunkRotationData[i] = rotation;
}
}
DeferredImpulseWriter.EndForEachIndex();
}