public static unsafe void CollideAndIntegrate(
CharacterControllerStepInput stepInput, float characterMass, bool affectBodies, Collider *collider,
ref RigidTransform transform, ref float3 linearVelocity, ref NativeStream.Writer deferredImpulseWriter)
{
// Copy parameters
float deltaTime = stepInput.DeltaTime;
float3 up = stepInput.Up;
PhysicsWorld world = stepInput.World;
float remainingTime = deltaTime;
float3 newPosition = transform.pos;
quaternion orientation = transform.rot;
float3 newVelocity = linearVelocity;
float maxSlopeCos = math.cos(stepInput.MaxSlope);
const float timeEpsilon = 0.000001f;
for (int i = 0; i < stepInput.MaxIterations && remainingTime > timeEpsilon; i++)
{
NativeList <SurfaceConstraintInfo> constraints = new NativeList <SurfaceConstraintInfo>(k_DefaultConstraintsCapacity, Allocator.Temp);
// Do a collider cast
{
float3 displacement = newVelocity * remainingTime;
NativeList <ColliderCastHit> castHits = new NativeList <ColliderCastHit>(k_DefaultQueryHitsCapacity, Allocator.Temp);
SelfFilteringAllHitsCollector <ColliderCastHit> collector = new SelfFilteringAllHitsCollector <ColliderCastHit>(stepInput.RigidBodyIndex, 1.0f, ref castHits);
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider,
Orientation = orientation,
Start = newPosition,
End = newPosition + displacement
};
world.CastCollider(input, ref collector);
// Iterate over hits and create constraints from them
for (int hitIndex = 0; hitIndex < collector.NumHits; hitIndex++)
{
ColliderCastHit hit = collector.AllHits[hitIndex];
if (ColliderUtils.IsTrigger(world.Bodies[hit.RigidBodyIndex].Collider, hit.ColliderKey))
{
continue;
}
CreateConstraint(stepInput.World, stepInput.Up,
hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Fraction * math.length(displacement),
stepInput.SkinWidth, maxSlopeCos, ref constraints);
}
}
// Then do a collider distance for penetration recovery,
// but only fix up penetrating hits
{
// Collider distance query
NativeList <DistanceHit> distanceHits = new NativeList <DistanceHit>(k_DefaultQueryHitsCapacity, Allocator.Temp);
SelfFilteringAllHitsCollector <DistanceHit> distanceHitsCollector = new SelfFilteringAllHitsCollector <DistanceHit>(
stepInput.RigidBodyIndex, stepInput.ContactTolerance, ref distanceHits);
{
ColliderDistanceInput input = new ColliderDistanceInput()
{
MaxDistance = stepInput.ContactTolerance,
Transform = transform,
Collider = collider
};
world.CalculateDistance(input, ref distanceHitsCollector);
}
// Iterate over penetrating hits and fix up distance and normal
int numConstraints = constraints.Length;
for (int hitIndex = 0; hitIndex < distanceHitsCollector.NumHits; hitIndex++)
{
DistanceHit hit = distanceHitsCollector.AllHits[hitIndex];
if (hit.Distance < stepInput.SkinWidth)
{
bool found = false;
// Iterate backwards to locate the original constraint before the max slope constraint
for (int constraintIndex = numConstraints - 1; constraintIndex >= 0; constraintIndex--)
{
SurfaceConstraintInfo constraint = constraints[constraintIndex];
if (constraint.RigidBodyIndex == hit.RigidBodyIndex &&
constraint.ColliderKey.Equals(hit.ColliderKey))
{
// Fix up the constraint (normal, distance)
{
if (ColliderUtils.IsTrigger(world.Bodies[hit.RigidBodyIndex].Collider, hit.ColliderKey))
{
continue;
}
// Create new constraint
CreateConstraintFromHit(world, hit.RigidBodyIndex, hit.ColliderKey,
hit.Position, hit.SurfaceNormal, hit.Distance,
stepInput.SkinWidth, out SurfaceConstraintInfo newConstraint);
// Resolve its penetration
ResolveConstraintPenetration(ref newConstraint);
// Write back
constraints[constraintIndex] = newConstraint;
}
found = true;
break;
}
}
// Add penetrating hit not caught by collider cast
if (!found)
{
if (ColliderUtils.IsTrigger(world.Bodies[hit.RigidBodyIndex].Collider, hit.ColliderKey))
{
continue;
}
CreateConstraint(stepInput.World, stepInput.Up,
hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Distance,
stepInput.SkinWidth, maxSlopeCos, ref constraints);
}
}
}
}
// Min delta time for solver to break
float minDeltaTime = 0.0f;
if (math.lengthsq(newVelocity) > k_SimplexSolverEpsilonSq)
{
// Min delta time to travel at least 1cm
minDeltaTime = 0.01f / math.length(newVelocity);
}
// Solve
float3 prevVelocity = newVelocity;
float3 prevPosition = newPosition;
SimplexSolver.Solve(world, remainingTime, minDeltaTime, up, stepInput.MaxMovementSpeed, constraints, ref newPosition, ref newVelocity, out float integratedTime);
// Apply impulses to hit bodies
if (affectBodies)
{
CalculateAndStoreDeferredImpulses(stepInput, characterMass, prevVelocity, ref constraints, ref deferredImpulseWriter);
}
// Calculate new displacement
float3 newDisplacement = newPosition - prevPosition;
// If simplex solver moved the character we need to re-cast to make sure it can move to new position
if (math.lengthsq(newDisplacement) > k_SimplexSolverEpsilon)
{
// Check if we can walk to the position simplex solver has suggested
var newCollector = new SelfFilteringClosestHitCollector <ColliderCastHit>(stepInput.RigidBodyIndex, 1.0f);
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider,
Orientation = orientation,
Start = prevPosition,
End = prevPosition + newDisplacement
};
world.CastCollider(input, ref newCollector);
if (newCollector.NumHits > 0)
{
ColliderCastHit hit = newCollector.ClosestHit;
bool found = false;
for (int constraintIndex = 0; constraintIndex < constraints.Length; constraintIndex++)
{
SurfaceConstraintInfo constraint = constraints[constraintIndex];
if (constraint.RigidBodyIndex == hit.RigidBodyIndex &&
constraint.ColliderKey.Equals(hit.ColliderKey))
{
found = true;
break;
}
}
// Move character along the newDisplacement direction until it reaches this new contact
if (!found)
{
Assert.IsTrue(hit.Fraction >= 0.0f && hit.Fraction <= 1.0f);
integratedTime *= hit.Fraction;
newPosition = prevPosition + newDisplacement * hit.Fraction;
}
}
}
// Reduce remaining time
remainingTime -= integratedTime;
}
// Write back position and velocity
transform.pos = newPosition;
linearVelocity = newVelocity;
}
protected unsafe override void OnUpdate()
{
PhysicsWorld physicsWorld = World.GetExistingSystem <BuildPhysicsWorld>().PhysicsWorld;
float deltaTime = Time.DeltaTime;
Entities.ForEach((
Entity entity,
ref KCCGrounded grounded,
in KCCGravity gravity,
in PhysicsCollider collider,
in Translation translation,
in Rotation rotation) =>
{
SelfFilteringClosestHitCollector <ColliderCastHit> hitCollector =
new SelfFilteringClosestHitCollector <ColliderCastHit>(entity.Index, 1.0f, physicsWorld.CollisionWorld);
float3 from = translation.Value;
float3 to = from + gravity.Down * grounded.groundCheckDistance;
var input = new ColliderCastInput()
{
End = to,
Start = from,
Collider = collider.ColliderPtr,
Orientation = rotation.Value
};
bool collisionOcurred = physicsWorld.CollisionWorld.CastCollider(input, ref hitCollector);
Unity.Physics.ColliderCastHit hit = hitCollector.ClosestHit;
grounded.previousAngle = grounded.angle;
grounded.previousOnGround = grounded.onGround;
grounded.previousDistanceToGround = grounded.distanceToGround;
grounded.previousHit = grounded.hitEntity;
if (collisionOcurred)
{
float angleBetween = math.abs(math.acos(math.dot(math.normalizesafe(hit.SurfaceNormal), gravity.Up)));
float angleDegrees = math.degrees(angleBetween);
grounded.angle = math.max(0, math.min(angleDegrees, KCCGroundedSystem.MaxAngleFallDegrees));
grounded.onGround = true;
grounded.distanceToGround = hit.Fraction * grounded.groundCheckDistance;
grounded.groundedRBIndex = hit.RigidBodyIndex;
grounded.groundedPoint = hit.Position;
grounded.hitEntity = hit.Entity;
grounded.surfaceNormal = hit.SurfaceNormal;
}
else
{
grounded.onGround = false;
grounded.distanceToGround = -1;
grounded.angle = -1;
grounded.groundedRBIndex = -1;
grounded.groundedPoint = float3.zero;
grounded.hitEntity = Entity.Null;
grounded.surfaceNormal = float3.zero;
}
// Falling is generated from other values, can be falling
// if hitting a steep slope on the ground.
if (grounded.Falling)
{
grounded.elapsedFallTime += deltaTime;
}
else
{
grounded.elapsedFallTime = 0;
}
}
