public void Execute(ArchetypeChunk chunk,
int chunkIndex,
int firstEntityIndex)
{
if (chunk.Has(collisionEventBufferType))
{
BufferAccessor <StatefulCollisionEvent> collisionEventsBufferAccessor =
chunk.GetBufferAccessor(collisionEventBufferType);
for (var i = 0; i < chunk.Count; i++)
{
DynamicBuffer <StatefulCollisionEvent> collisionEventsBuffer =
collisionEventsBufferAccessor[i];
for (var j = collisionEventsBuffer.Length - 1; j >= 0; j--)
{
StatefulCollisionEvent collisionEvent = collisionEventsBuffer[j];
if (collisionEvent._isStale)
{
if (collisionEvent.State == PhysicsEventState.Exit)
{
collisionEventsBuffer.RemoveAt(j);
}
else
{
collisionEvent.State = PhysicsEventState.Exit;
collisionEventsBuffer[j] = collisionEvent;
}
}
}
}
}
}
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(ArchetypeChunk chunk,
int chunkIndex,
int firstEntityIndex)
{
BufferAccessor <StatefulCollisionEvent> collisionEventsBufferAccessor =
chunk.GetBufferAccessor(collisionEventBufferType);
for (var i = 0; i < chunk.Count; i++)
{
DynamicBuffer <StatefulCollisionEvent> collisionEventsBuffer =
collisionEventsBufferAccessor[i];
for (var j = collisionEventsBuffer.Length - 1; j >= 0; j--)
{
StatefulCollisionEvent collisionEventElement = collisionEventsBuffer[j];
collisionEventElement._isStale = true;
collisionEventsBuffer[j] = collisionEventElement;
}
}
}
private static unsafe void CalculateAndStoreDeferredImpulsesAndCollisionEvents(
CharacterControllerStepInput stepInput, bool affectBodies, float characterMass,
float3 linearVelocity, NativeList <SurfaceConstraintInfo> constraints, ref NativeStream.Writer deferredImpulseWriter,
NativeList <StatefulCollisionEvent> collisionEvents)
{
PhysicsWorld world = stepInput.World;
for (int i = 0; i < constraints.Length; i++)
{
SurfaceConstraintInfo constraint = constraints[i];
int rigidBodyIndex = constraint.RigidBodyIndex;
float3 impulse = float3.zero;
if (rigidBodyIndex < 0)
{
continue;
}
// Skip static bodies if needed to calculate impulse
if (affectBodies && (rigidBodyIndex < world.NumDynamicBodies))
{
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];
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
});
}
if (collisionEvents.IsCreated && constraint.Touched && !constraint.IsMaxSlope)
{
var collisionEvent = new StatefulCollisionEvent(world.Bodies[stepInput.RigidBodyIndex].Entity,
world.Bodies[rigidBodyIndex].Entity, stepInput.RigidBodyIndex, rigidBodyIndex, ColliderKey.Empty,
constraint.ColliderKey, constraint.Plane.Normal);
collisionEvent.CollisionDetails = new StatefulCollisionEvent.Details(
1, math.dot(impulse, collisionEvent.Normal), constraint.HitPosition);
// check if collision event exists for the same bodyID and colliderKey
// although this is a nested for, number of solved constraints shouldn't be high
// if the same constraint (same entities, rigidbody indices and collider keys)
// is solved in multiple solver iterations, pick the one from latest iteration
bool newEvent = true;
for (int j = 0; j < collisionEvents.Length; j++)
{
if (collisionEvents[j].CompareTo(collisionEvent) == 0)
{
collisionEvents[j] = collisionEvent;
newEvent = false;
break;
}
}
if (newEvent)
{
collisionEvents.Add(collisionEvent);
}
}
}
}
public static unsafe void CheckSupport(
ref PhysicsWorld world, ref PhysicsCollider collider, CharacterControllerStepInput stepInput, RigidTransform transform,
out CharacterSupportState characterState, out float3 surfaceNormal, out float3 surfaceVelocity,
NativeList <StatefulCollisionEvent> collisionEvents = default)
{
surfaceNormal = float3.zero;
surfaceVelocity = float3.zero;
// Up direction must be normalized
Assert.IsTrue(Unity.Physics.Math.IsNormalized(stepInput.Up));
// Query the world
NativeList <ColliderCastHit> castHits = new NativeList <ColliderCastHit>(k_DefaultQueryHitsCapacity, Allocator.Temp);
CharacterControllerAllHitsCollector <ColliderCastHit> castHitsCollector = new CharacterControllerAllHitsCollector <ColliderCastHit>(
stepInput.RigidBodyIndex, 1.0f, ref castHits, world);
var maxDisplacement = -stepInput.ContactTolerance * stepInput.Up;
{
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider.ColliderPtr,
Orientation = transform.rot,
Start = transform.pos,
End = transform.pos + maxDisplacement
};
world.CastCollider(input, ref castHitsCollector);
}
// If no hits, proclaim unsupported state
if (castHitsCollector.NumHits == 0)
{
characterState = CharacterSupportState.Unsupported;
return;
}
float maxSlopeCos = math.cos(stepInput.MaxSlope);
// Iterate over distance hits and create constraints from them
NativeList <SurfaceConstraintInfo> constraints = new NativeList <SurfaceConstraintInfo>(k_DefaultConstraintsCapacity, Allocator.Temp);
float maxDisplacementLength = math.length(maxDisplacement);
for (int i = 0; i < castHitsCollector.NumHits; i++)
{
ColliderCastHit hit = castHitsCollector.AllHits[i];
CreateConstraint(stepInput.World, stepInput.Up,
hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Fraction * maxDisplacementLength,
stepInput.SkinWidth, maxSlopeCos, ref constraints);
}
// Velocity for support checking
float3 initialVelocity = maxDisplacement / stepInput.DeltaTime;
// Solve downwards (don't use min delta time, try to solve full step)
float3 outVelocity = initialVelocity;
float3 outPosition = transform.pos;
SimplexSolver.Solve(stepInput.DeltaTime, stepInput.DeltaTime, stepInput.Up, stepInput.MaxMovementSpeed,
constraints, ref outPosition, ref outVelocity, out float integratedTime, false);
// Get info on surface
int numSupportingPlanes = 0;
{
for (int j = 0; j < constraints.Length; j++)
{
var constraint = constraints[j];
if (constraint.Touched && !constraint.IsTooSteep && !constraint.IsMaxSlope)
{
numSupportingPlanes++;
surfaceNormal += constraint.Plane.Normal;
surfaceVelocity += constraint.Velocity;
// Add supporting planes to collision events
if (collisionEvents.IsCreated)
{
var collisionEvent = new StatefulCollisionEvent(stepInput.World.Bodies[stepInput.RigidBodyIndex].Entity,
stepInput.World.Bodies[constraint.RigidBodyIndex].Entity, stepInput.RigidBodyIndex, constraint.RigidBodyIndex,
ColliderKey.Empty, constraint.ColliderKey, constraint.Plane.Normal);
collisionEvent.CollisionDetails = new StatefulCollisionEvent.Details(1, 0, constraint.HitPosition);
collisionEvents.Add(collisionEvent);
}
}
}
if (numSupportingPlanes > 0)
{
float invNumSupportingPlanes = 1.0f / numSupportingPlanes;
surfaceNormal *= invNumSupportingPlanes;
surfaceVelocity *= invNumSupportingPlanes;
surfaceNormal = math.normalize(surfaceNormal);
}
}
// Check support state
{
if (math.lengthsq(initialVelocity - outVelocity) < k_SimplexSolverEpsilonSq)
{
// If velocity hasn't changed significantly, declare unsupported state
characterState = CharacterSupportState.Unsupported;
}
else if (math.lengthsq(outVelocity) < k_SimplexSolverEpsilonSq && numSupportingPlanes > 0)
{
// If velocity is very small, declare supported state
characterState = CharacterSupportState.Supported;
}
else
{
// Check if sliding
outVelocity = math.normalize(outVelocity);
float slopeAngleSin = math.max(0.0f, math.dot(outVelocity, -stepInput.Up));
float slopeAngleCosSq = 1 - slopeAngleSin * slopeAngleSin;
if (slopeAngleCosSq <= maxSlopeCos * maxSlopeCos)
{
characterState = CharacterSupportState.Sliding;
}
else if (numSupportingPlanes > 0)
{
characterState = CharacterSupportState.Supported;
}
else
{
// If numSupportingPlanes is 0, surface normal is invalid, so state is unsupported
characterState = CharacterSupportState.Unsupported;
}
}
}
}
