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();
}
private unsafe void WriteEvent(LowLevel.CollisionEvent collisionEvent, ref BlockStream.Writer collisionEventWriter)
{
int numContactPoints = collisionEvent.NumNarrowPhaseContactPoints;
int size = UnsafeUtility.SizeOf <LowLevel.CollisionEvent>() + numContactPoints * UnsafeUtility.SizeOf <ContactPoint>();
collisionEventWriter.Write(size);
byte *eventPtr = collisionEventWriter.Allocate(size);
ref LowLevel.CollisionEvent eventRef = ref UnsafeUtilityEx.AsRef <LowLevel.CollisionEvent>(eventPtr);
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);
public void WriteWithoutBegin()
{
var stream = new BlockStream(1, 0x9b98651c);
BlockStream.Writer writer = stream;
Assert.Throws <ArgumentException>(() => writer.Write(5));
stream.Dispose();
}
public void Execute(int index)
{
Writer.BeginForEachIndex(index);
for (int i = 0; i != index; i++)
{
Writer.Write(i);
}
Writer.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(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();
}
private static unsafe void ResolveContacts(PhysicsWorld world, float deltaTime, float3 gravity, float tau, float damping,
float characterMass, float3 linearVelocity, int numConstraints, ref NativeArray <SurfaceConstraintInfo> constraints, ref BlockStream.Writer deferredImpulseWriter)
{
for (int i = 0; i < numConstraints; i++)
{
SurfaceConstraintInfo constraint = constraints[i];
int rigidBodyIndex = constraint.RigidBodyIndex;
if (rigidBodyIndex < 0 || rigidBodyIndex >= world.NumDynamicBodies)
{
// Invalid and static bodies should be skipped
continue;
}
RigidBody body = world.Bodies[rigidBodyIndex];
float3 pointRelVel = world.GetLinearVelocity(rigidBodyIndex, constraint.HitPosition);
pointRelVel -= linearVelocity;
float projectedVelocity = math.dot(pointRelVel, constraint.Plane.Normal);
// Required velocity change
float deltaVelocity = -projectedVelocity * damping;
float distance = constraint.Plane.Distance;
if (distance < 0.0f)
{
deltaVelocity += (distance / deltaTime) * tau;
}
// Calculate impulse
MotionVelocity mv = world.MotionVelocities[rigidBodyIndex];
float3 impulse = float3.zero;
if (deltaVelocity < 0.0f)
{
// Impulse magnitude
float impulseMagnitude = 0.0f;
{
float3 arm = constraint.HitPosition - (body.WorldFromBody.pos + body.Collider->MassProperties.MassDistribution.Transform.pos);
float3 jacAng = math.cross(arm, constraint.Plane.Normal);
float3 armC = jacAng * mv.InverseInertiaAndMass.xyz;
float objectMassInv = math.dot(armC, jacAng);
objectMassInv += mv.InverseInertiaAndMass.w;
impulseMagnitude = deltaVelocity / objectMassInv;
}
impulse = impulseMagnitude * constraint.Plane.Normal;
}
// Add gravity
{
// Effect of gravity on character velocity in the normal direction
float3 charVelDown = gravity * deltaTime;
float relVelN = math.dot(charVelDown, constraint.Plane.Normal);
// Subtract separation velocity if separating contact
{
bool isSeparatingContact = projectedVelocity < 0.0f;
float newRelVelN = relVelN - projectedVelocity;
relVelN = math.select(relVelN, newRelVelN, isSeparatingContact);
}
// If resulting velocity is negative, an impulse is applied to stop the character
// from falling into the body
{
float3 newImpulse = impulse;
newImpulse += relVelN * characterMass * constraint.Plane.Normal;
impulse = math.select(impulse, newImpulse, relVelN < 0.0f);
}
}
// Store impulse
deferredImpulseWriter.Write(
new DeferredCharacterControllerImpulse()
{
Entity = body.Entity,
Impulse = impulse,
Point = constraint.HitPosition
});
}
}
private static unsafe void CalculateAndStoreDeferredImpulses(
CharacterControllerStepInput stepInput, float characterMass, float3 linearVelocity, int numConstraints,
ref NativeArray <SurfaceConstraintInfo> constraints, ref BlockStream.Writer deferredImpulseWriter)
{
PhysicsWorld world = stepInput.World;
for (int i = 0; i < numConstraints; i++)
{
SurfaceConstraintInfo constraint = constraints[i];
int rigidBodyIndex = constraint.RigidBodyIndex;
if (rigidBodyIndex < 0 || rigidBodyIndex >= world.NumDynamicBodies)
{
// Invalid and static bodies should be skipped
continue;
}
RigidBody body = world.Bodies[rigidBodyIndex];
float3 pointRelVel = world.GetLinearVelocity(rigidBodyIndex, constraint.HitPosition);
pointRelVel -= linearVelocity;
float projectedVelocity = math.dot(pointRelVel, constraint.Plane.Normal);
// Required velocity change
float deltaVelocity = -projectedVelocity * stepInput.Damping;
float distance = constraint.Plane.Distance;
if (distance < 0.0f)
{
deltaVelocity += (distance / stepInput.DeltaTime) * stepInput.Tau;
}
// Calculate impulse
MotionVelocity mv = world.MotionVelocities[rigidBodyIndex];
float3 impulse = float3.zero;
if (deltaVelocity < 0.0f)
{
// Impulse magnitude
float impulseMagnitude = 0.0f;
{
float objectMassInv = GetInvMassAtPoint(constraint.HitPosition, constraint.Plane.Normal, body, mv);
impulseMagnitude = deltaVelocity / objectMassInv;
}
impulse = impulseMagnitude * constraint.Plane.Normal;
}
// Add gravity
{
// Effect of gravity on character velocity in the normal direction
float3 charVelDown = stepInput.Gravity * stepInput.DeltaTime;
float relVelN = math.dot(charVelDown, constraint.Plane.Normal);
// Subtract separation velocity if separating contact
{
bool isSeparatingContact = projectedVelocity < 0.0f;
float newRelVelN = relVelN - projectedVelocity;
relVelN = math.select(relVelN, newRelVelN, isSeparatingContact);
}
// If resulting velocity is negative, an impulse is applied to stop the character
// from falling into the body
{
float3 newImpulse = impulse;
newImpulse += relVelN * characterMass * constraint.Plane.Normal;
impulse = math.select(impulse, newImpulse, relVelN < 0.0f);
}
}
// Store impulse
deferredImpulseWriter.Write(
new DeferredCharacterControllerImpulse()
{
Entity = body.Entity,
Impulse = impulse,
Point = constraint.HitPosition
});
}
}
private static unsafe void ConvexConvex(
Context context, ColliderKeyPair colliderKeys,
Collider *convexColliderA, Collider *convexColliderB, MTransform worldFromA, MTransform worldFromB,
float maxDistance, bool flipped, ref BlockStream.Writer contactWriter)
{
MTransform aFromB = Mul(Inverse(worldFromA), worldFromB);
ConvexConvexManifoldQueries.Manifold contactManifold;
switch (convexColliderA->Type)
{
case ColliderType.Sphere:
switch (convexColliderB->Type)
{
case ColliderType.Sphere:
ConvexConvexManifoldQueries.SphereSphere(
(SphereCollider *)convexColliderA, (SphereCollider *)convexColliderB,
worldFromA, aFromB, maxDistance, out contactManifold);
break;
case ColliderType.Capsule:
ConvexConvexManifoldQueries.CapsuleSphere(
(CapsuleCollider *)convexColliderB, (SphereCollider *)convexColliderA,
worldFromB, Inverse(aFromB), maxDistance, out contactManifold);
flipped = !flipped;
break;
case ColliderType.Triangle:
ConvexConvexManifoldQueries.TriangleSphere(
(PolygonCollider *)convexColliderB, (SphereCollider *)convexColliderA,
worldFromB, Inverse(aFromB), maxDistance, out contactManifold);
flipped = !flipped;
break;
case ColliderType.Box:
ConvexConvexManifoldQueries.BoxSphere(
(BoxCollider *)convexColliderB, (SphereCollider *)convexColliderA,
worldFromB, Inverse(aFromB), maxDistance, out contactManifold);
flipped = !flipped;
break;
case ColliderType.Quad:
case ColliderType.Cylinder:
case ColliderType.Convex:
ConvexConvexManifoldQueries.ConvexConvex(
ref ((SphereCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull,
worldFromA, aFromB, maxDistance, out contactManifold);
break;
default:
throw new NotImplementedException();
}
break;
case ColliderType.Box:
switch (convexColliderB->Type)
{
case ColliderType.Sphere:
ConvexConvexManifoldQueries.BoxSphere(
(BoxCollider *)convexColliderA, (SphereCollider *)convexColliderB,
worldFromA, aFromB, maxDistance, out contactManifold);
break;
case ColliderType.Triangle:
ConvexConvexManifoldQueries.BoxTriangle(
(BoxCollider *)convexColliderA, (PolygonCollider *)convexColliderB,
worldFromA, aFromB, maxDistance, out contactManifold);
break;
case ColliderType.Box:
ConvexConvexManifoldQueries.BoxBox(
(BoxCollider *)convexColliderA, (BoxCollider *)convexColliderB,
worldFromA, aFromB, maxDistance, out contactManifold);
break;
case ColliderType.Capsule:
case ColliderType.Quad:
case ColliderType.Cylinder:
case ColliderType.Convex:
ConvexConvexManifoldQueries.ConvexConvex(
ref ((BoxCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull,
worldFromA, aFromB, maxDistance, out contactManifold);
break;
default:
throw new NotImplementedException();
}
break;
case ColliderType.Capsule:
switch (convexColliderB->Type)
{
case ColliderType.Sphere:
ConvexConvexManifoldQueries.CapsuleSphere(
(CapsuleCollider *)convexColliderA, (SphereCollider *)convexColliderB,
worldFromA, aFromB, maxDistance, out contactManifold);
break;
case ColliderType.Capsule:
ConvexConvexManifoldQueries.CapsuleCapsule(
(CapsuleCollider *)convexColliderA, (CapsuleCollider *)convexColliderB,
worldFromA, aFromB, maxDistance, out contactManifold);
break;
case ColliderType.Triangle:
ConvexConvexManifoldQueries.CapsuleTriangle(
(CapsuleCollider *)convexColliderA, (PolygonCollider *)convexColliderB,
worldFromA, aFromB, maxDistance, out contactManifold);
break;
case ColliderType.Quad:
case ColliderType.Box:
case ColliderType.Cylinder:
case ColliderType.Convex:
ConvexConvexManifoldQueries.ConvexConvex(
ref ((CapsuleCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull,
worldFromA, aFromB, maxDistance, out contactManifold);
break;
default:
throw new NotImplementedException();
}
break;
case ColliderType.Triangle:
switch (convexColliderB->Type)
{
case ColliderType.Sphere:
ConvexConvexManifoldQueries.TriangleSphere(
(PolygonCollider *)convexColliderA, (SphereCollider *)convexColliderB,
worldFromA, aFromB, maxDistance, out contactManifold);
break;
case ColliderType.Capsule:
ConvexConvexManifoldQueries.CapsuleTriangle(
(CapsuleCollider *)convexColliderB, (PolygonCollider *)convexColliderA,
worldFromB, Inverse(aFromB), maxDistance, out contactManifold);
flipped = !flipped;
break;
case ColliderType.Box:
ConvexConvexManifoldQueries.BoxTriangle(
(BoxCollider *)convexColliderB, (PolygonCollider *)convexColliderA,
worldFromB, Inverse(aFromB), maxDistance, out contactManifold);
flipped = !flipped;
break;
case ColliderType.Triangle:
case ColliderType.Quad:
case ColliderType.Cylinder:
case ColliderType.Convex:
ConvexConvexManifoldQueries.ConvexConvex(
ref ((PolygonCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull,
worldFromA, aFromB, maxDistance, out contactManifold);
break;
default:
throw new NotImplementedException();
}
break;
case ColliderType.Quad:
case ColliderType.Cylinder:
case ColliderType.Convex:
ConvexConvexManifoldQueries.ConvexConvex(
ref ((ConvexCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull,
worldFromA, aFromB, maxDistance, out contactManifold);
break;
default:
throw new NotImplementedException();
}
// Write results to stream
if (contactManifold.NumContacts > 0)
{
if (flipped)
{
contactManifold.Flip();
}
var header = new ContactHeader
{
BodyPair = context.BodyIndices,
BodyCustomDatas = context.BodyCustomDatas,
NumContacts = contactManifold.NumContacts,
Normal = contactManifold.Normal,
ColliderKeys = colliderKeys
};
// Apply materials
{
Material materialA = ((ConvexColliderHeader *)convexColliderA)->Material;
Material materialB = ((ConvexColliderHeader *)convexColliderB)->Material;
Material.MaterialFlags combinedFlags = materialA.Flags | materialB.Flags;
if ((combinedFlags & Material.MaterialFlags.IsTrigger) != 0)
{
header.JacobianFlags |= JacobianFlags.IsTrigger;
}
else
{
if ((combinedFlags & Material.MaterialFlags.EnableCollisionEvents) != 0)
{
header.JacobianFlags |= JacobianFlags.EnableCollisionEvents;
}
if ((combinedFlags & Material.MaterialFlags.EnableMassFactors) != 0)
{
header.JacobianFlags |= JacobianFlags.EnableMassFactors;
}
if ((combinedFlags & Material.MaterialFlags.EnableSurfaceVelocity) != 0)
{
header.JacobianFlags |= JacobianFlags.EnableSurfaceVelocity;
}
if ((combinedFlags & Material.MaterialFlags.EnableMaxImpulse) != 0)
{
header.JacobianFlags |= JacobianFlags.EnableMaxImpulse;
}
header.CoefficientOfFriction = Material.GetCombinedFriction(materialA, materialB);
header.CoefficientOfRestitution = Material.GetCombinedRestitution(materialA, materialB);
}
}
contactWriter.Write(header);
for (int contactIndex = 0; contactIndex < header.NumContacts; contactIndex++)
{
contactWriter.Write(contactManifold[contactIndex]);
}
}
}
private static void WriteManifold(ConvexConvexManifoldQueries.Manifold manifold, Context context, ColliderKeyPair colliderKeys,
Material materialA, Material materialB, bool flipped, ref BlockStream.Writer contactWriter)
{
// Write results to stream
if (manifold.NumContacts > 0)
{
if (flipped)
{
manifold.Flip();
}
var header = new ContactHeader
{
BodyPair = context.BodyIndices,
BodyCustomTags = context.BodyCustomTags,
NumContacts = manifold.NumContacts,
Normal = manifold.Normal,
ColliderKeys = colliderKeys
};
// Apply materials
{
Material.MaterialFlags combinedFlags = materialA.Flags | materialB.Flags;
if ((combinedFlags & Material.MaterialFlags.IsTrigger) != 0)
{
header.JacobianFlags |= JacobianFlags.IsTrigger;
}
else
{
if ((combinedFlags & Material.MaterialFlags.EnableCollisionEvents) != 0)
{
header.JacobianFlags |= JacobianFlags.EnableCollisionEvents;
}
if ((combinedFlags & Material.MaterialFlags.EnableMassFactors) != 0)
{
header.JacobianFlags |= JacobianFlags.EnableMassFactors;
}
if ((combinedFlags & Material.MaterialFlags.EnableSurfaceVelocity) != 0)
{
header.JacobianFlags |= JacobianFlags.EnableSurfaceVelocity;
}
header.CoefficientOfFriction = Material.GetCombinedFriction(materialA, materialB);
header.CoefficientOfRestitution = Material.GetCombinedRestitution(materialA, materialB);
}
}
contactWriter.Write(header);
// Group the contact points in 2s (when 4-6 contact points) and 3s (6 or more contact points)
// to avoid the order forcing the magnitude of the impulse on one side of the face.
// When less than 4 contact points access them in order.
int startIndex = 0;
int increment = header.NumContacts < 6 ?
math.max(header.NumContacts / 2, 1) : (header.NumContacts / 3 + ((header.NumContacts % 3 > 0) ? 1 : 0));
for (int contactIndex = 0; ; contactIndex += increment)
{
if (contactIndex >= header.NumContacts)
{
startIndex++;
if (startIndex == increment)
{
break;
}
contactIndex = startIndex;
}
contactWriter.Write(manifold[contactIndex]);
}
}
}
