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;
}
public static unsafe void CheckSupport(
ref PhysicsWorld world, ref PhysicsCollider collider, CharacterControllerStepInput stepInput, RigidTransform transform, float maxSlope,
out CharacterSupportState characterState, out float3 surfaceNormal, out float3 surfaceVelocity)
{
surfaceNormal = float3.zero;
surfaceVelocity = float3.zero;
// Query the world
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.ColliderPtr
};
world.CalculateDistance(input, ref distanceHitsCollector);
}
// If no hits, proclaim unsupported state
if (distanceHitsCollector.NumHits == 0)
{
characterState = CharacterSupportState.Unsupported;
return;
}
// Downwards direction must be normalized
float3 downwardsDirection = -stepInput.Up;
Assert.IsTrue(Math.IsNormalized(downwardsDirection));
float maxSlopeCos = math.cos(maxSlope);
// Iterate over distance hits and create constraints from them
NativeList <SurfaceConstraintInfo> constraints = new NativeList <SurfaceConstraintInfo>(k_DefaultConstraintsCapacity, Allocator.Temp);
for (int i = 0; i < distanceHitsCollector.NumHits; i++)
{
DistanceHit hit = distanceHitsCollector.AllHits[i];
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);
}
float3 initialVelocity;
{
float velAlongDownwardsDir = math.dot(stepInput.CurrentVelocity, downwardsDirection);
bool velocityIsAlongDownwardsDirection = velAlongDownwardsDir > 0.0f;
if (velocityIsAlongDownwardsDirection)
{
float3 downwardsVelocity = velAlongDownwardsDir * downwardsDirection;
initialVelocity =
math.select(downwardsVelocity, downwardsDirection, math.abs(velAlongDownwardsDir) > 1.0f) +
stepInput.Gravity * stepInput.DeltaTime;
}
else
{
initialVelocity = downwardsDirection;
}
}
// Solve downwards (don't use min delta time, try to solve full step)
float3 outVelocity = initialVelocity;
float3 outPosition = transform.pos;
SimplexSolver.Solve(stepInput.World, 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)
{
numSupportingPlanes++;
surfaceNormal += constraint.Plane.Normal;
surfaceVelocity += constraint.Velocity;
}
}
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)
{
// If velocity is very small, declare supported state
characterState = CharacterSupportState.Supported;
}
else
{
// Check if sliding or supported
outVelocity = math.normalize(outVelocity);
float slopeAngleSin = math.max(0.0f, math.dot(outVelocity, downwardsDirection));
float slopeAngleCosSq = 1 - slopeAngleSin * slopeAngleSin;
if (slopeAngleCosSq < maxSlopeCos * maxSlopeCos)
{
characterState = CharacterSupportState.Sliding;
}
else
{
characterState = CharacterSupportState.Supported;
}
}
}
}