public static unsafe void CheckSupport(CharacterControllerStepInput stepInput,
RigidTransform transform, float maxSlope, MaxHitsCollector <DistanceHit> distanceHitsCollector,
ref NativeArray <SurfaceConstraintInfo> constraints, out CharacterSupportState characterState)
{
CheckSupport(stepInput, transform, maxSlope, distanceHitsCollector, ref constraints, out int numConstraints,
out characterState, out float3 surfaceNormal, out float3 surfaceVelocity);
}
public static unsafe void CollideAndIntegrate(
CharacterControllerStepInput stepInput, float characterMass, bool affectBodies, Collider *collider,
MaxHitsCollector <DistanceHit> distanceHitsCollector, ref NativeArray <ColliderCastHit> castHits, ref NativeArray <SurfaceConstraintInfo> constraints,
ref RigidTransform transform, ref float3 linearVelocity, ref BlockStream.Writer deferredImpulseWriter)
{
CollideAndIntegrate(stepInput, characterMass, affectBodies, collider,
ref castHits, ref constraints, distanceHitsCollector.NumHits,
ref transform, ref linearVelocity, ref deferredImpulseWriter);
}
public void Execute()
{
for (int i = 0; i < DamageEntities.Length; i++)
{
MaxHitsCollector <DistanceHit> collector = new MaxHitsCollector <DistanceHit>(10.0f, ref DistanceHits);
if (!DamageAreas[i].WasUsed)
{
CollisionFilter filter = CollisionFilter.Default;
filter.CollidesWith = DamageAreas[i].CollisionFilter;
pointDistanceInput.Position = Translations[i].Value;
pointDistanceInput.Filter = filter;
PhysicsWorld.CalculateDistance(pointDistanceInput, ref collector);
for (int j = 0; j < collector.NumHits; j++)
{
Entity hitEntity = PhysicsWorld.Bodies[DistanceHits[j].RigidBodyIndex].Entity;
if (DistanceHits[j].Fraction <= DamageAreas[i].Radius)
{
// deal damage
if (HealthsFromEntity.Exists(hitEntity))
{
Health h = HealthsFromEntity[hitEntity];
h.Value -= DamageAreas[i].Damage;
HealthsFromEntity[hitEntity] = h;
}
}
}
Entity damageEntity = DamageEntities[i];
if (DamageAreasFromEntity.Exists(damageEntity))
{
DamageArea d = DamageAreasFromEntity[damageEntity];
if (d.SingleUse)
{
d.WasUsed = true;
DamageAreasFromEntity[damageEntity] = d;
}
}
}
}
}
public void Execute()
{
for (int i = 0; i < Projectiles.Length; i++)
{
CollisionFilter filter = CollisionFilter.Default;
filter.CollidesWith = 4;
RaycastInput raycastInput = new RaycastInput
{
Start = Projectiles[i].PreviousPosition,
End = ProjectileTranslations[i].Value,
Filter = filter
};
MaxHitsCollector <RaycastHit> collector = new MaxHitsCollector <RaycastHit>(1.0f, ref RaycastHits);
if (PhysicsWorld.CastRay(raycastInput, ref collector))
{
if (collector.NumHits > 0)
{
RaycastHit closestHit = new RaycastHit();
closestHit.Fraction = 2f;
for (int j = 0; j < collector.NumHits; j++)
{
if (RaycastHits[j].Fraction < closestHit.Fraction)
{
closestHit = RaycastHits[j];
}
}
// Apply damage to hit rigidbody/collider
Entity hitEntity = PhysicsWorld.Bodies[closestHit.RigidBodyIndex].Entity;
if (HealthsFromEntity.Exists(hitEntity))
{
Health h = HealthsFromEntity[hitEntity];
h.Value -= Projectiles[i].Damage;
HealthsFromEntity[hitEntity] = h;
}
// Destroy projectile
entityCommandBuffer.DestroyEntity(ProjectileEntities[i]);
}
}
}
}
public static unsafe void CheckSupport(PhysicsWorld world, float deltaTime, RigidTransform transform,
float3 downwardsDirection, float maxSlope, float contactTolerance, Collider *collider, ref NativeArray <SurfaceConstraintInfo> constraints,
ref NativeArray <DistanceHit> checkSupportHits, out CharacterSupportState characterState)
{
// Downwards direction must be normalized
Assert.IsTrue(Math.IsNormalized(downwardsDirection));
// "Broad phase"
MaxHitsCollector <DistanceHit> collector = new MaxHitsCollector <DistanceHit>(contactTolerance, ref checkSupportHits);
{
ColliderDistanceInput input = new ColliderDistanceInput()
{
MaxDistance = contactTolerance,
Transform = transform,
Collider = collider
};
world.CalculateDistance(input, ref collector);
}
// Iterate over hits and create constraints from them
for (int i = 0; i < collector.NumHits; i++)
{
DistanceHit hit = collector.AllHits[i];
CreateConstraintFromHit(world, float3.zero, deltaTime, hit.RigidBodyIndex, hit.ColliderKey,
hit.Position, hit.SurfaceNormal, hit.Distance, true, out SurfaceConstraintInfo constraint);
constraints[i] = constraint;
}
// Solve downwards
float3 outVelocity = downwardsDirection;
float3 outPosition = transform.pos;
SimplexSolver.Solve(world, deltaTime, -downwardsDirection, collector.NumHits, ref constraints, ref outPosition, ref outVelocity, out float integratedTime);
// If no hits, proclaim unsupported state
if (collector.NumHits == 0)
{
characterState = CharacterSupportState.Unsupported;
}
else
{
if (math.lengthsq(downwardsDirection - outVelocity) < SimplexSolver.c_SimplexSolverEpsilon)
{
// If velocity hasn't changed significantly, declare unsupported state
characterState = CharacterSupportState.Unsupported;
}
else if (math.lengthsq(outVelocity) < SimplexSolver.c_SimplexSolverEpsilon)
{
// If velocity is very small, declare supported state
characterState = CharacterSupportState.Supported;
}
else
{
// Check if sliding or supported
outVelocity = math.normalize(outVelocity);
float slopeAngleSin = math.dot(outVelocity, downwardsDirection);
float slopeAngleCosSq = 1 - slopeAngleSin * slopeAngleSin;
float maxSlopeCosine = math.cos(maxSlope);
if (slopeAngleCosSq < maxSlopeCosine * maxSlopeCosine)
{
characterState = CharacterSupportState.Sliding;
}
else
{
characterState = CharacterSupportState.Supported;
}
}
}
}
public static unsafe void CollideAndIntegrate(PhysicsWorld world, float deltaTime,
int maxIterations, float3 up, float3 gravity,
float characterMass, float tau, float damping, float maxSlope, bool affectBodies, Collider *collider,
ref NativeArray <DistanceHit> distanceHits, ref NativeArray <ColliderCastHit> castHits, ref NativeArray <SurfaceConstraintInfo> constraints,
ref RigidTransform transform, ref float3 linearVelocity, ref BlockStream.Writer deferredImpulseWriter)
{
float remainingTime = deltaTime;
float3 lastDisplacement = linearVelocity * remainingTime;
float3 newPosition = transform.pos;
quaternion orientation = transform.rot;
float3 newVelocity = linearVelocity;
float maxSlopeCos = math.cos(maxSlope);
const float timeEpsilon = 0.000001f;
for (int i = 0; i < maxIterations && remainingTime > timeEpsilon; i++)
{
// First do distance query for penetration recovery
MaxHitsCollector <DistanceHit> distanceHitsCollector = new MaxHitsCollector <DistanceHit>(0.0f, ref distanceHits);
int numConstraints = 0;
{
ColliderDistanceInput input = new ColliderDistanceInput()
{
MaxDistance = 0.0f,
Transform = new RigidTransform
{
pos = newPosition,
rot = orientation,
},
Collider = collider
};
world.CalculateDistance(input, ref distanceHitsCollector);
// Iterate over hits and create constraints from them
for (int hitIndex = 0; hitIndex < distanceHitsCollector.NumHits; hitIndex++)
{
DistanceHit hit = distanceHitsCollector.AllHits[hitIndex];
CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position,
hit.SurfaceNormal, hit.Distance, false, out SurfaceConstraintInfo constraint);
// Potentially add a max slope constraint
AddMaxSlopeConstraint(up, maxSlopeCos, ref constraint, ref constraints, ref numConstraints);
// Add original constraint to the list
constraints[numConstraints++] = constraint;
}
}
float3 gravityMovement = gravity * remainingTime * remainingTime * 0.5f;
// Then do a collider cast
{
float3 displacement = lastDisplacement + gravityMovement;
float3 endPosition = newPosition + displacement;
MaxHitsCollector <ColliderCastHit> collector = new MaxHitsCollector <ColliderCastHit>(1.0f, ref castHits);
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider,
Orientation = orientation,
Position = newPosition,
Direction = 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];
bool found = false;
for (int distanceHitIndex = 0; distanceHitIndex < distanceHitsCollector.NumHits; distanceHitIndex++)
{
DistanceHit dHit = distanceHitsCollector.AllHits[distanceHitIndex];
if (dHit.RigidBodyIndex == hit.RigidBodyIndex &&
dHit.ColliderKey.Equals(hit.ColliderKey))
{
found = true;
break;
}
}
// Skip duplicate hits
if (!found)
{
CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal,
hit.Fraction * math.length(lastDisplacement), false, out SurfaceConstraintInfo constraint);
// Potentially add a max slope constraint
AddMaxSlopeConstraint(up, maxSlopeCos, ref constraint, ref constraints, ref numConstraints);
// Add original constraint to the list
constraints[numConstraints++] = constraint;
}
}
}
// Solve
float3 prevVelocity = newVelocity;
float3 prevPosition = newPosition;
SimplexSolver.Solve(world, remainingTime, up, numConstraints, ref constraints, ref newPosition, ref newVelocity, out float integratedTime);
// Apply impulses to hit bodies
if (affectBodies)
{
ResolveContacts(world, remainingTime, gravity, tau, damping, characterMass, prevVelocity, numConstraints, ref constraints, ref deferredImpulseWriter);
}
float3 newDisplacement = newPosition - prevPosition;
// Check if we can walk to the position simplex solver has suggested
MaxHitsCollector <ColliderCastHit> newCollector = new MaxHitsCollector <ColliderCastHit>(1.0f, ref castHits);
int newContactIndex = -1;
// If simplex solver moved the character we need to re-cast to make sure it can move to new position
if (math.lengthsq(newDisplacement) > SimplexSolver.c_SimplexSolverEpsilon)
{
float3 displacement = newDisplacement + gravityMovement;
float3 endPosition = prevPosition + displacement;
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider,
Orientation = orientation,
Position = prevPosition,
Direction = displacement
};
world.CastCollider(input, ref newCollector);
for (int hitIndex = 0; hitIndex < newCollector.NumHits; hitIndex++)
{
ColliderCastHit hit = newCollector.AllHits[hitIndex];
bool found = false;
for (int constraintIndex = 0; constraintIndex < numConstraints; constraintIndex++)
{
SurfaceConstraintInfo constraint = constraints[constraintIndex];
if (constraint.RigidBodyIndex == hit.RigidBodyIndex &&
constraint.ColliderKey.Equals(hit.ColliderKey))
{
found = true;
break;
}
}
if (!found)
{
newContactIndex = hitIndex;
break;
}
}
}
// Move character along the newDisplacement direction until it reaches this new contact
if (newContactIndex >= 0)
{
ColliderCastHit newContact = newCollector.AllHits[newContactIndex];
float fraction = newContact.Fraction / math.length(newDisplacement);
integratedTime *= fraction;
float3 displacement = newDisplacement * fraction;
newPosition = prevPosition + displacement;
}
remainingTime -= integratedTime;
// Remember last displacement for next iteration
lastDisplacement = newVelocity * remainingTime;
}
// Write back position and velocity
transform.pos = newPosition;
linearVelocity = newVelocity;
}
public static unsafe void CheckSupport(CharacterControllerStepInput stepInput,
RigidTransform transform, float maxSlope, MaxHitsCollector <DistanceHit> distanceHitsCollector,
ref NativeArray <SurfaceConstraintInfo> constraints, out CharacterSupportState characterState)
{
// 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));
// Iterate over distance hits and create constraints from them
for (int i = 0; i < distanceHitsCollector.NumHits; i++)
{
DistanceHit hit = distanceHitsCollector.AllHits[i];
CreateConstraintFromHit(stepInput.World, float3.zero, stepInput.DeltaTime, hit.RigidBodyIndex, hit.ColliderKey,
hit.Position, hit.SurfaceNormal, hit.Distance, true, out SurfaceConstraintInfo constraint);
constraints[i] = constraint;
}
// Solve downwards (don't respect min delta time, try to solve full step)
float3 outVelocity = downwardsDirection;
float3 outPosition = transform.pos;
SimplexSolver.Solve(stepInput.World, stepInput.DeltaTime, stepInput.Up, distanceHitsCollector.NumHits,
ref constraints, ref outPosition, ref outVelocity, out float integratedTime, false);
// Check support state
{
if (math.lengthsq(downwardsDirection - outVelocity) < SimplexSolver.c_SimplexSolverEpsilon)
{
// If velocity hasn't changed significantly, declare unsupported state
characterState = CharacterSupportState.Unsupported;
}
else if (math.lengthsq(outVelocity) < SimplexSolver.c_SimplexSolverEpsilon)
{
// If velocity is very small, declare supported state
characterState = CharacterSupportState.Supported;
}
else
{
// Check if sliding or supported
outVelocity = math.normalize(outVelocity);
float slopeAngleSin = math.dot(outVelocity, downwardsDirection);
float slopeAngleCosSq = 1 - slopeAngleSin * slopeAngleSin;
float maxSlopeCosine = math.cos(maxSlope);
if (slopeAngleCosSq < maxSlopeCosine * maxSlopeCosine - SimplexSolver.c_SimplexSolverEpsilon)
{
characterState = CharacterSupportState.Sliding;
}
else
{
characterState = CharacterSupportState.Supported;
}
}
}
}
public static unsafe void CollideAndIntegrate(
CharacterControllerStepInput stepInput, float characterMass, bool affectBodies, Collider *collider,
MaxHitsCollector <DistanceHit> distanceHitsCollector, ref NativeArray <ColliderCastHit> castHits, ref NativeArray <SurfaceConstraintInfo> constraints,
ref RigidTransform transform, ref float3 linearVelocity, ref BlockStream.Writer deferredImpulseWriter)
{
// Copy parameters
float deltaTime = stepInput.DeltaTime;
float3 gravity = stepInput.Gravity;
float3 up = stepInput.Up;
PhysicsWorld world = stepInput.World;
float remainingTime = deltaTime;
float3 lastDisplacement = linearVelocity * remainingTime;
float3 newPosition = transform.pos;
quaternion orientation = transform.rot;
float3 newVelocity = linearVelocity;
float maxSlopeCos = math.cos(stepInput.MaxSlope);
// Iterate over hits and create constraints from them
int numDistanceConstraints = 0;
for (int hitIndex = 0; hitIndex < distanceHitsCollector.NumHits; hitIndex++)
{
DistanceHit hit = distanceHitsCollector.AllHits[hitIndex];
CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position,
hit.SurfaceNormal, hit.Distance, false, out SurfaceConstraintInfo constraint);
// Check if max slope plane is required
float verticalComponent = math.dot(constraint.Plane.Normal, up);
bool shouldAddPlane = verticalComponent > SimplexSolver.c_SimplexSolverEpsilon && verticalComponent < maxSlopeCos;
if (shouldAddPlane)
{
AddMaxSlopeConstraint(up, ref constraint, ref constraints, ref numDistanceConstraints);
}
// Add original constraint to the list
constraints[numDistanceConstraints++] = constraint;
}
const float timeEpsilon = 0.000001f;
for (int i = 0; i < stepInput.MaxIterations && remainingTime > timeEpsilon; i++)
{
int numConstraints = numDistanceConstraints;
float3 gravityMovement = gravity * remainingTime * remainingTime * 0.5f;
// Then do a collider cast (but not in first iteration)
if (i > 0)
{
float3 displacement = lastDisplacement + gravityMovement;
MaxHitsCollector <ColliderCastHit> collector = new MaxHitsCollector <ColliderCastHit>(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];
bool found = false;
for (int distanceHitIndex = 0; distanceHitIndex < distanceHitsCollector.NumHits; distanceHitIndex++)
{
DistanceHit dHit = distanceHitsCollector.AllHits[distanceHitIndex];
if (dHit.RigidBodyIndex == hit.RigidBodyIndex &&
dHit.ColliderKey.Equals(hit.ColliderKey))
{
found = true;
break;
}
}
// Skip duplicate hits
if (!found)
{
CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal,
hit.Fraction * math.length(lastDisplacement), false, out SurfaceConstraintInfo constraint);
// Check if max slope plane is required
float verticalComponent = math.dot(constraint.Plane.Normal, up);
bool shouldAddPlane = verticalComponent > SimplexSolver.c_SimplexSolverEpsilon && verticalComponent < maxSlopeCos;
if (shouldAddPlane)
{
AddMaxSlopeConstraint(up, ref constraint, ref constraints, ref numConstraints);
}
// Add original constraint to the list
constraints[numConstraints++] = constraint;
}
}
}
// Solve
float3 prevVelocity = newVelocity;
float3 prevPosition = newPosition;
SimplexSolver.Solve(world, remainingTime, up, numConstraints, ref constraints, ref newPosition, ref newVelocity, out float integratedTime);
// Apply impulses to hit bodies
if (affectBodies)
{
CalculateAndStoreDeferredImpulses(stepInput, characterMass, prevVelocity, numConstraints, ref constraints, ref deferredImpulseWriter);
}
float3 newDisplacement = newPosition - prevPosition;
// Check if we can walk to the position simplex solver has suggested
MaxHitsCollector <ColliderCastHit> newCollector = new MaxHitsCollector <ColliderCastHit>(1.0f, ref castHits);
int newContactIndex = -1;
// If simplex solver moved the character we need to re-cast to make sure it can move to new position
if (math.lengthsq(newDisplacement) > SimplexSolver.c_SimplexSolverEpsilon)
{
float3 displacement = newDisplacement + gravityMovement;
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider,
Orientation = orientation,
Start = prevPosition,
End = prevPosition + displacement
};
world.CastCollider(input, ref newCollector);
for (int hitIndex = 0; hitIndex < newCollector.NumHits; hitIndex++)
{
ColliderCastHit hit = newCollector.AllHits[hitIndex];
bool found = false;
for (int constraintIndex = 0; constraintIndex < numConstraints; constraintIndex++)
{
SurfaceConstraintInfo constraint = constraints[constraintIndex];
if (constraint.RigidBodyIndex == hit.RigidBodyIndex &&
constraint.ColliderKey.Equals(hit.ColliderKey))
{
found = true;
break;
}
}
if (!found)
{
newContactIndex = hitIndex;
break;
}
}
}
// Move character along the newDisplacement direction until it reaches this new contact
if (newContactIndex >= 0)
{
ColliderCastHit newContact = newCollector.AllHits[newContactIndex];
Assert.IsTrue(newContact.Fraction >= 0.0f && newContact.Fraction <= 1.0f);
integratedTime *= newContact.Fraction;
newPosition = prevPosition + newDisplacement * newContact.Fraction;
}
remainingTime -= integratedTime;
// Remember last displacement for next iteration
lastDisplacement = newVelocity * remainingTime;
}
// Write back position and velocity
transform.pos = newPosition;
linearVelocity = newVelocity;
}
public static unsafe void CollideAndIntegrate(
CharacterControllerStepInput stepInput, float characterMass, bool affectBodies, Collider *collider,
ref NativeArray <ColliderCastHit> castHits, ref NativeArray <SurfaceConstraintInfo> constraints, int numConstraints,
ref RigidTransform transform, ref float3 linearVelocity, ref BlockStream.Writer deferredImpulseWriter)
{
// Copy parameters
float deltaTime = stepInput.DeltaTime;
float3 gravity = stepInput.Gravity;
float3 up = stepInput.Up;
PhysicsWorld world = stepInput.World;
float remainingTime = deltaTime;
float3 lastDisplacement = linearVelocity * remainingTime;
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++)
{
float3 gravityMovement = gravity * remainingTime * remainingTime * 0.5f;
// Then do a collider cast (but not in first iteration)
if (i > 0)
{
int numCastConstraints = 0;
float3 displacement = lastDisplacement + gravityMovement;
MaxHitsCollector <ColliderCastHit> collector = new MaxHitsCollector <ColliderCastHit>(stepInput.RigidBodyIndex, 1.0f, ref castHits);
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider,
Orientation = orientation,
Start = newPosition,
End = newPosition + displacement * (1.0f + stepInput.ContactTolerance),
};
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];
CreateConstraint(stepInput.World, stepInput.Up,
hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Fraction * math.length(lastDisplacement),
stepInput.SkinWidth, maxSlopeCos, ref constraints, ref numCastConstraints);
}
numConstraints = numCastConstraints;
}
// 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, numConstraints, ref constraints, ref newPosition, ref newVelocity, out float integratedTime);
// Apply impulses to hit bodies
if (affectBodies)
{
CalculateAndStoreDeferredImpulses(stepInput, characterMass, prevVelocity, numConstraints, ref constraints, ref deferredImpulseWriter);
}
float3 newDisplacement = newPosition - prevPosition;
// Check if we can walk to the position simplex solver has suggested
MaxHitsCollector <ColliderCastHit> newCollector = new MaxHitsCollector <ColliderCastHit>(stepInput.RigidBodyIndex, 1.0f, ref castHits);
int newContactIndex = -1;
// 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)
{
float3 displacement = newDisplacement + gravityMovement;
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider,
Orientation = orientation,
Start = prevPosition,
End = prevPosition + displacement * (1.0f + stepInput.ContactTolerance)
};
world.CastCollider(input, ref newCollector);
float minFraction = float.MaxValue;
for (int hitIndex = 0; hitIndex < newCollector.NumHits; hitIndex++)
{
ColliderCastHit hit = newCollector.AllHits[hitIndex];
if (hit.Fraction < minFraction)
{
bool found = false;
for (int constraintIndex = 0; constraintIndex < numConstraints; constraintIndex++)
{
SurfaceConstraintInfo constraint = constraints[constraintIndex];
if (constraint.RigidBodyIndex == hit.RigidBodyIndex &&
constraint.ColliderKey.Equals(hit.ColliderKey))
{
found = true;
break;
}
}
if (!found)
{
minFraction = hit.Fraction;
newContactIndex = hitIndex;
}
}
}
}
// Move character along the newDisplacement direction until it reaches this new contact
if (newContactIndex >= 0)
{
ColliderCastHit newContact = newCollector.AllHits[newContactIndex];
Assert.IsTrue(newContact.Fraction >= 0.0f && newContact.Fraction <= 1.0f);
integratedTime *= newContact.Fraction;
newPosition = prevPosition + newDisplacement * newContact.Fraction;
}
remainingTime -= integratedTime;
// Remember last displacement for next iteration
lastDisplacement = newVelocity * remainingTime;
}
// Write back position and velocity
transform.pos = newPosition;
linearVelocity = newVelocity;
}
public static unsafe void CheckSupport(CharacterControllerStepInput stepInput,
RigidTransform transform, float maxSlope, MaxHitsCollector <DistanceHit> distanceHitsCollector,
ref NativeArray <SurfaceConstraintInfo> constraints, out int numConstraints, out CharacterSupportState characterState,
out float3 surfaceNormal, out float3 surfaceVelocity)
{
surfaceNormal = float3.zero;
surfaceVelocity = float3.zero;
// If no hits, proclaim unsupported state
if (distanceHitsCollector.NumHits == 0)
{
characterState = CharacterSupportState.Unsupported;
numConstraints = 0;
return;
}
// Downwards direction must be normalized
float3 downwardsDirection = -stepInput.Up;
Assert.IsTrue(Unity.Physics.Math.IsNormalized(downwardsDirection));
float maxSlopeCos = math.cos(maxSlope);
// Iterate over distance hits and create constraints from them
numConstraints = 0;
for (int i = 0; i < distanceHitsCollector.NumHits; i++)
{
DistanceHit hit = distanceHitsCollector.AllHits[i];
CreateConstraint(stepInput.World, stepInput.Up,
hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Distance,
stepInput.SkinWidth, maxSlopeCos, ref constraints, ref numConstraints);
}
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, numConstraints,
ref constraints, ref outPosition, ref outVelocity, out float integratedTime, false);
// Reset touched state of constraints and get info on surface
{
int numSupportingPlanes = 0;
for (int j = 0; j < numConstraints; j++)
{
var constraint = constraints[j];
if (constraint.Touched)
{
numSupportingPlanes++;
surfaceNormal += constraint.Plane.Normal;
surfaceVelocity += constraint.Velocity;
constraint.Touched = false;
constraints[j] = constraint;
}
}
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) - k_SimplexSolverEpsilon);
float slopeAngleCosSq = 1 - slopeAngleSin * slopeAngleSin;
if (slopeAngleCosSq < maxSlopeCos * maxSlopeCos)
{
characterState = CharacterSupportState.Sliding;
}
else
{
characterState = CharacterSupportState.Supported;
}
}
}
}
public unsafe void Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex)
{
float3 up = math.up();
var chunkCCData = chunk.GetNativeArray(CharacterControllerComponentType);
var chunkCCInternalData = chunk.GetNativeArray(CharacterControllerInternalType);
var chunkPhysicsColliderData = chunk.GetNativeArray(PhysicsColliderType);
var chunkTranslationData = chunk.GetNativeArray(TranslationType);
var chunkRotationData = chunk.GetNativeArray(RotationType);
DeferredImpulseWriter.BeginForEachIndex(chunkIndex);
// Maximum number of hits character controller can store in world queries
const int maxQueryHits = 128;
var distanceHits = new NativeArray <DistanceHit>(maxQueryHits, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
var castHits = new NativeArray <ColliderCastHit>(maxQueryHits, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
var constraints = new NativeArray <SurfaceConstraintInfo>(4 * maxQueryHits, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
for (int i = 0; i < chunk.Count; i++)
{
var ccComponentData = chunkCCData[i];
var ccInternalData = chunkCCInternalData[i];
var collider = chunkPhysicsColliderData[i];
var position = chunkTranslationData[i];
var rotation = chunkRotationData[i];
// Collision filter must be valid
Assert.IsTrue(collider.ColliderPtr->Filter.IsValid);
// Character step input
CharacterControllerStepInput stepInput = new CharacterControllerStepInput
{
World = PhysicsWorld,
DeltaTime = DeltaTime,
Up = math.up(),
Gravity = ccComponentData.Gravity,
MaxIterations = ccComponentData.MaxIterations,
Tau = k_DefaultTau,
Damping = k_DefaultDamping,
SkinWidth = ccComponentData.SkinWidth,
ContactTolerance = ccComponentData.ContactTolerance,
MaxSlope = ccComponentData.MaxSlope,
RigidBodyIndex = PhysicsWorld.GetRigidBodyIndex(ccInternalData.Entity),
CurrentVelocity = ccInternalData.LinearVelocity
};
// Character transform
RigidTransform transform = new RigidTransform
{
pos = position.Value,
rot = rotation.Value
};
// "Broad phase" (used both for checking support and actual character collide and integrate).
MaxHitsCollector <DistanceHit> distanceHitsCollector = new MaxHitsCollector <DistanceHit>(
stepInput.RigidBodyIndex, ccComponentData.ContactTolerance, ref distanceHits);
{
ColliderDistanceInput input = new ColliderDistanceInput()
{
MaxDistance = ccComponentData.ContactTolerance,
Transform = transform,
Collider = collider.ColliderPtr
};
PhysicsWorld.CalculateDistance(input, ref distanceHitsCollector);
}
// Check support
CheckSupport(stepInput, transform, ccComponentData.MaxSlope, distanceHitsCollector,
ref constraints, out int numConstraints, out ccInternalData.SupportedState, out float3 surfaceNormal, out float3 surfaceVelocity);
// User input
float3 desiredVelocity = ccInternalData.LinearVelocity;
HandleUserInput(ccComponentData, stepInput.Up, surfaceVelocity, ref ccInternalData, ref desiredVelocity);
// Calculate actual velocity with respect to surface
if (ccInternalData.SupportedState == CharacterSupportState.Supported)
{
CalculateMovement(ccInternalData.CurrentRotationAngle, stepInput.Up, ccInternalData.IsJumping,
ccInternalData.LinearVelocity, desiredVelocity, surfaceNormal, surfaceVelocity, out ccInternalData.LinearVelocity);
}
else
{
ccInternalData.LinearVelocity = desiredVelocity;
}
// World collision + integrate
CollideAndIntegrate(stepInput, ccComponentData.CharacterMass, ccComponentData.AffectsPhysicsBodies > 0,
collider.ColliderPtr, ref castHits, ref constraints, numConstraints,
ref transform, ref ccInternalData.LinearVelocity, ref DeferredImpulseWriter);
// Write back and orientation integration
position.Value = transform.pos;
rotation.Value = quaternion.AxisAngle(up, ccInternalData.CurrentRotationAngle);
// Write back to chunk data
{
chunkCCInternalData[i] = ccInternalData;
chunkTranslationData[i] = position;
chunkRotationData[i] = rotation;
}
}
DeferredImpulseWriter.EndForEachIndex();
}
public static unsafe void CollideAndIntegrate(PhysicsWorld world, float deltaTime,
int maxIterations, float3 up, float3 gravity,
float characterMass, float tau, float damping, bool affectBodies, Collider *collider,
ref NativeArray <DistanceHit> distanceHits, ref NativeArray <ColliderCastHit> castHits, ref NativeArray <SurfaceConstraintInfo> constraints,
ref RigidTransform transform, ref float3 linearVelocity, ref BlockStream.Writer deferredImpulseWriter)
{
float remainingTime = deltaTime;
float3 lastDisplacement = linearVelocity * remainingTime;
float3 newPosition = transform.pos;
quaternion orientation = transform.rot;
float3 newVelocity = linearVelocity;
const float timeEpsilon = 0.000001f;
for (int i = 0; i < maxIterations && remainingTime > timeEpsilon; i++)
{
// First do distance query for penetration recovery
MaxHitsCollector <DistanceHit> distanceHitsCollector = new MaxHitsCollector <DistanceHit>(0.0f, ref distanceHits);
int numConstraints = 0;
{
ColliderDistanceInput input = new ColliderDistanceInput()
{
MaxDistance = 0.0f,
Transform = new RigidTransform
{
pos = newPosition,
rot = orientation,
},
Collider = collider
};
world.CalculateDistance(input, ref distanceHitsCollector);
// Iterate over hits and create constraints from them
for (int hitIndex = 0; hitIndex < distanceHitsCollector.NumHits; hitIndex++)
{
DistanceHit hit = distanceHitsCollector.AllHits[hitIndex];
CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position,
hit.SurfaceNormal, hit.Distance, false, out SurfaceConstraintInfo constraint);
constraints[numConstraints++] = constraint;
}
}
// Then do a collider cast
{
float3 displacement = lastDisplacement - up * timeEpsilon;
float3 endPosition = newPosition + displacement;
MaxHitsCollector <ColliderCastHit> collector = new MaxHitsCollector <ColliderCastHit>(1.0f, ref castHits);
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider,
Orientation = orientation,
Position = newPosition,
Direction = 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];
bool found = false;
for (int distanceHitIndex = 0; distanceHitIndex < distanceHitsCollector.NumHits; distanceHitIndex++)
{
DistanceHit dHit = distanceHitsCollector.AllHits[distanceHitIndex];
if (dHit.RigidBodyIndex == hit.RigidBodyIndex &&
dHit.ColliderKey.Equals(hit.ColliderKey))
{
found = true;
break;
}
}
// Skip duplicate hits
if (!found)
{
CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal,
hit.Fraction * math.length(lastDisplacement), false, out SurfaceConstraintInfo constraint);
constraints[numConstraints++] = constraint;
}
}
}
// petarm.todo: Add max slope plane to avoid climbing the not allowed slopes
// Solve
float3 prevVelocity = newVelocity;
SimplexSolver.Solve(world, deltaTime, up, numConstraints, ref constraints, ref newPosition, ref newVelocity, out float integratedTime);
remainingTime -= integratedTime;
lastDisplacement = newVelocity * remainingTime;
// Apply impulses to hit bodies
if (affectBodies)
{
ResolveContacts(world, deltaTime, gravity, tau, damping, characterMass, prevVelocity, numConstraints, ref constraints, ref deferredImpulseWriter);
}
}
// Write back position and velocity
transform.pos = newPosition;
linearVelocity = newVelocity;
}
