public unsafe void Execute(CollisionEvent collisionEvent)
{
CollisionEvent.Details details = collisionEvent.CalculateDetails(ref World);
// Color code the impulse depending on the collision feature
// vertex - blue
// edge - cyan
// face - magenta
UnityEngine.Color color;
switch (details.EstimatedContactPointPositions.Length)
{
case 1:
color = UnityEngine.Color.blue;
break;
case 2:
color = UnityEngine.Color.cyan;
break;
default:
color = UnityEngine.Color.magenta;
break;
}
var averageContactPosition = details.AverageContactPointPosition;
OutputStreamContext->Point(averageContactPosition, 0.01f, color);
OutputStreamContext->Arrow(averageContactPosition, collisionEvent.Normal * details.EstimatedImpulse, color);
}
private void ProcessForEntity(Entity entity,
Entity otherEntity,
float3 normal,
bool hasDetails,
CollisionEvent.Details collisionEventDetails)
{
DynamicBuffer <StatefulCollisionEvent> collisionEventBuffer =
collisionEventBufferFromEntity[entity];
var foundMatch = false;
for (var i = 0; i < collisionEventBuffer.Length; i++)
{
StatefulCollisionEvent collisionEvent = collisionEventBuffer[i];
// If entity is already there, update to Stay
if (collisionEvent.Entity == otherEntity)
{
foundMatch = true;
collisionEvent.Normal = normal;
collisionEvent.HasCollisionDetails = hasDetails;
collisionEvent.AverageContactPointPosition =
collisionEventDetails.AverageContactPointPosition;
collisionEvent.EstimatedImpulse =
collisionEventDetails.EstimatedImpulse;
collisionEvent.State = PhysicsEventState.Stay;
collisionEvent._isStale = false;
collisionEventBuffer[i] = collisionEvent;
break;
}
}
// If it's a new entity, add as Enter
if (!foundMatch)
{
collisionEventBuffer.Add(
new StatefulCollisionEvent
{
Entity = otherEntity,
Normal = normal,
HasCollisionDetails = hasDetails,
AverageContactPointPosition =
collisionEventDetails.AverageContactPointPosition,
EstimatedImpulse = collisionEventDetails.EstimatedImpulse,
State = PhysicsEventState.Enter,
_isStale = false
}
);
}
}
public void Execute(CollisionEvent collisionEvent)
{
CollisionEvent.Details collisionEventDetails = default;
var aHasDetails = false;
var bHasDetails = false;
if (collisionEventsReceiverPropertiesFromEntity
.HasComponent(collisionEvent.EntityA))
{
aHasDetails =
collisionEventsReceiverPropertiesFromEntity[collisionEvent.EntityA]
.UsesCollisionDetails;
}
if (collisionEventsReceiverPropertiesFromEntity
.HasComponent(collisionEvent.EntityB))
{
bHasDetails =
collisionEventsReceiverPropertiesFromEntity[collisionEvent.EntityB]
.UsesCollisionDetails;
}
if (aHasDetails || bHasDetails)
{
collisionEventDetails = collisionEvent.CalculateDetails(ref physicsWorld);
}
if (collisionEventBufferFromEntity.HasComponent(collisionEvent.EntityA))
{
ProcessForEntity(
collisionEvent.EntityA,
collisionEvent.EntityB,
collisionEvent.Normal,
aHasDetails,
collisionEventDetails
);
}
if (collisionEventBufferFromEntity.HasComponent(collisionEvent.EntityB))
{
ProcessForEntity(
collisionEvent.EntityB,
collisionEvent.EntityA,
collisionEvent.Normal,
bHasDetails,
collisionEventDetails
);
}
}
public void Execute(CollisionEvent collisionEvent)
{
// Collision event is between a static and dynamic box.
// Verify all data in the provided event struct.
CollisionEvent.Details details = collisionEvent.CalculateDetails(ref World);
Assert.IsTrue(details.EstimatedImpulse >= 0.0f);
Assert.IsTrue(details.EstimatedContactPointPositions.Length == 4);
Assert.AreNotEqual(collisionEvent.BodyIndexA, collisionEvent.BodyIndexB);
Assert.AreEqual(collisionEvent.ColliderKeyA.Value, ColliderKey.Empty.Value);
Assert.AreEqual(collisionEvent.ColliderKeyB.Value, ColliderKey.Empty.Value);
Assert.AreEqual(collisionEvent.EntityA, Bodies[collisionEvent.BodyIndexA].Entity);
Assert.AreEqual(collisionEvent.EntityB, Bodies[collisionEvent.BodyIndexB].Entity);
Assert.AreApproximatelyEqual(collisionEvent.Normal.x, 0.0f, 0.01f);
Assert.AreApproximatelyEqual(collisionEvent.Normal.y, 1.0f, 0.01f);
Assert.AreApproximatelyEqual(collisionEvent.Normal.z, 0.0f, 0.01f);
}
// Calculate extra details about the collision, by re-integrating the leaf colliders to the time of collision
internal unsafe CollisionEvent.Details CalculateDetails(
ref PhysicsWorld physicsWorld, float timeStep, Velocity inputVelocityA, Velocity inputVelocityB, NativeArray <ContactPoint> narrowPhaseContactPoints)
{
int bodyIndexA = BodyIndices.BodyIndexA;
int bodyIndexB = BodyIndices.BodyIndexB;
bool bodyAIsDynamic = bodyIndexA < physicsWorld.MotionVelocities.Length;
bool bodyBIsDynamic = bodyIndexB < physicsWorld.MotionVelocities.Length;
MotionVelocity motionVelocityA = bodyAIsDynamic ? physicsWorld.MotionVelocities[bodyIndexA] : MotionVelocity.Zero;
MotionVelocity motionVelocityB = bodyBIsDynamic ? physicsWorld.MotionVelocities[bodyIndexB] : MotionVelocity.Zero;
MotionData motionDataA = bodyAIsDynamic ? physicsWorld.MotionDatas[bodyIndexA] : MotionData.Zero;
MotionData motionDataB = bodyBIsDynamic ? physicsWorld.MotionDatas[bodyIndexB] : MotionData.Zero;
float estimatedImpulse = SolverImpulse;
// First calculate minimum time of impact and estimate the impulse
float toi = timeStep;
{
float sumRemainingVelocities = 0.0f;
float numRemainingVelocities = 0.0f;
for (int i = 0; i < narrowPhaseContactPoints.Length; i++)
{
var cp = narrowPhaseContactPoints[i];
// Collect data for impulse estimation
{
float3 pointVelA = GetPointVelocity(motionDataA.WorldFromMotion,
motionVelocityA.LinearVelocity, motionVelocityA.AngularVelocity, cp.Position + Normal * cp.Distance);
float3 pointVelB = GetPointVelocity(motionDataB.WorldFromMotion,
motionVelocityB.LinearVelocity, motionVelocityB.AngularVelocity, cp.Position);
float projRelVel = math.dot(pointVelB - pointVelA, Normal);
if (projRelVel > 0.0f)
{
sumRemainingVelocities += projRelVel;
numRemainingVelocities += 1.0f;
}
}
// Get minimum time of impact
{
float3 pointVelA = GetPointVelocity(motionDataA.WorldFromMotion,
inputVelocityA.Linear, inputVelocityA.Angular, cp.Position + Normal * cp.Distance);
float3 pointVelB = GetPointVelocity(motionDataB.WorldFromMotion,
inputVelocityB.Linear, inputVelocityB.Angular, cp.Position);
float projRelVel = math.dot(pointVelB - pointVelA, Normal);
if (projRelVel > 0.0f)
{
float newToi = math.max(0.0f, cp.Distance / projRelVel);
toi = math.min(toi, newToi);
}
else if (cp.Distance <= 0.0f)
{
// If in penetration, time of impact is 0 for sure
toi = 0.0f;
}
}
}
if (numRemainingVelocities > 0.0f)
{
float sumInvMass = motionVelocityA.InverseMass + motionVelocityB.InverseMass;
estimatedImpulse += sumRemainingVelocities / (numRemainingVelocities * sumInvMass);
}
}
// Then, sub-integrate for time of impact and keep contact points closer than hitDistanceThreshold
int closestContactIndex = -1;
float minDistance = float.MaxValue;
{
int estimatedContactPointCount = 0;
for (int i = 0; i < narrowPhaseContactPoints.Length; i++)
{
// Estimate new position
var cp = narrowPhaseContactPoints[i];
{
float3 pointVelA = GetPointVelocity(motionDataA.WorldFromMotion, inputVelocityA.Linear, inputVelocityA.Angular, cp.Position + Normal * cp.Distance);
float3 pointVelB = GetPointVelocity(motionDataB.WorldFromMotion, inputVelocityB.Linear, inputVelocityB.Angular, cp.Position);
float3 relVel = pointVelB - pointVelA;
float projRelVel = math.dot(relVel, Normal);
// Only sub integrate if approaching, otherwise leave it as is
// (it can happen that input velocity was separating but there
// still was a collision event - penetration recovery, or other
// body pushing in different direction).
if (projRelVel > 0.0f)
{
// Position the point on body A
cp.Position += Normal * cp.Distance;
// Sub integrate the point
cp.Position -= relVel * toi;
// Reduce the distance
cp.Distance -= projRelVel * toi;
}
// Filter out contacts that are still too far away
if (cp.Distance <= physicsWorld.CollisionWorld.CollisionTolerance)
{
narrowPhaseContactPoints[estimatedContactPointCount++] = cp;
}
else if (cp.Distance < minDistance)
{
minDistance = cp.Distance;
closestContactIndex = i;
}
}
}
// If due to estimation of relative velocity no contact points will
// get closer than the tolerance, we need to export the closest one
// to make sure at least one contact point is reported.
if (estimatedContactPointCount == 0)
{
narrowPhaseContactPoints[estimatedContactPointCount++] = narrowPhaseContactPoints[closestContactIndex];
}
// Instantiate collision details and allocate memory
var details = new CollisionEvent.Details
{
EstimatedContactPointPositions = new NativeArray <float3>(estimatedContactPointCount, Allocator.Temp),
EstimatedImpulse = estimatedImpulse
};
// Fill the contact point positions array
for (int i = 0; i < estimatedContactPointCount; i++)
{
details.EstimatedContactPointPositions[i] = narrowPhaseContactPoints[i].Position;
}
return(details);
}
}
