private static void CreateConstraintFromHit(PhysicsWorld world, int rigidBodyIndex, ColliderKey colliderKey,
float3 hitPosition, float3 normal, float distance, float skinWidth, out SurfaceConstraintInfo constraint)
{
bool bodyIsDynamic = 0 <= rigidBodyIndex && rigidBodyIndex < world.NumDynamicBodies;
constraint = new SurfaceConstraintInfo()
{
Plane = new Plane
{
Normal = normal,
Distance = distance - skinWidth,
},
RigidBodyIndex = rigidBodyIndex,
ColliderKey = colliderKey,
HitPosition = hitPosition,
Velocity = bodyIsDynamic ?
world.GetLinearVelocity(rigidBodyIndex, hitPosition) :
float3.zero,
Priority = bodyIsDynamic ? 1 : 0
};
}
public static void CreateConstraintFromHit(PhysicsWorld world, ColliderKey key, int rigidbodyIndex, float3 position, float3 velocity, float3 normal, float distance, float deltaTime, out SurfaceConstraintInfo constraint)
{
bool dynamicBody = 0 <= rigidbodyIndex && rigidbodyIndex < world.NumDynamicBodies;
constraint = new SurfaceConstraintInfo
{
Plane = new Plane
{
Normal = normal,
Distance = distance
},
RigidBodyIndex = rigidbodyIndex,
ColliderKey = key,
HitPosition = position,
Velocity = dynamicBody ? world.MotionVelocities[rigidbodyIndex].LinearVelocity : velocity
};
if (distance < 0.0f)
{
constraint.Velocity = constraint.Velocity - constraint.Plane.Normal * distance;
}
}
public void TestSolve1d()
{
SurfaceConstraintInfo plane = CreateConstraint(ColliderKey.Empty, float3.zero, new float3(0, 1, 0), 1, 0, 0, false, float3.zero);
// Approaching velocity should lose all vertical speed
float3 velocity = new float3(0, -1, 0);
Solve1d(plane, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, 0);
Assert.AreApproximatelyEqual(velocity.y, 0);
Assert.AreApproximatelyEqual(velocity.z, 0);
// Test that only vertical component of velocity is killed
velocity = new float3(-1, -1, 0);
Solve1d(plane, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, -1);
Assert.AreApproximatelyEqual(velocity.y, 0);
Assert.AreApproximatelyEqual(velocity.z, 0);
plane = CreateConstraint(ColliderKey.Empty, float3.zero, new float3(0, 1, 0), 1, 0, 0, false, new float3(0, 0.5f, 0));
// Check that vertical component of velocity changes direction when ground velocity is directed towards the body
velocity = new float3(0, -1, 0);
Solve1d(plane, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, 0);
Assert.AreApproximatelyEqual(velocity.y, 0.5f);
Assert.AreApproximatelyEqual(velocity.z, 0);
plane = CreateConstraint(ColliderKey.Empty, float3.zero, new float3(0, 1, 0), 1, 0, 0, false, new float3(0, -0.5f, 0));
// Check that only part of vertical component of velocity is killed when ground velocity is going away
velocity = new float3(0, -1, 0);
Solve1d(plane, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, 0);
Assert.AreApproximatelyEqual(velocity.y, -0.5f);
Assert.AreApproximatelyEqual(velocity.z, 0);
}
public void TestSort3d()
{
SurfaceConstraintInfo plane1 = CreateConstraint(ColliderKey.Empty, float3.zero, new float3(0, 1, 0), 1, 0, 0, false, float3.zero);
SurfaceConstraintInfo plane2 = CreateConstraint(ColliderKey.Empty, float3.zero, new float3(0, 1, 0), 1, 1, 0, false, float3.zero);
SurfaceConstraintInfo plane3 = CreateConstraint(ColliderKey.Empty, float3.zero, new float3(0, 1, 0), 1, 2, 0, false, float3.zero);
// Leftmost plane should have smallest priority after the sort
Sort3d(ref plane1, ref plane2, ref plane3);
Assert.IsTrue(plane1.Priority < plane2.Priority &&
plane2.Priority < plane3.Priority);
// Leftmost plane should have smallest priority after the sort
Sort3d(ref plane1, ref plane3, ref plane2);
Assert.IsTrue(plane1.Priority < plane3.Priority &&
plane3.Priority < plane2.Priority);
// Leftmost plane should have smallest priority after the sort
Sort3d(ref plane2, ref plane1, ref plane3);
Assert.IsTrue(plane2.Priority < plane1.Priority &&
plane1.Priority < plane3.Priority);
// Leftmost plane should have smallest priority after the sort
Sort3d(ref plane2, ref plane3, ref plane1);
Assert.IsTrue(plane2.Priority < plane3.Priority &&
plane3.Priority < plane1.Priority);
// Leftmost plane should have smallest priority after the sort
Sort3d(ref plane3, ref plane1, ref plane2);
Assert.IsTrue(plane3.Priority < plane1.Priority &&
plane1.Priority < plane2.Priority);
// Leftmost plane should have smallest priority after the sort
Sort3d(ref plane3, ref plane2, ref plane1);
Assert.IsTrue(plane3.Priority < plane2.Priority &&
plane2.Priority < plane1.Priority);
}
private static unsafe void ResolveContacts(PhysicsWorld world, float deltaTime, float3 gravity, float tau, float damping,
float characterMass, float3 linearVelocity, int numConstraints, ref NativeArray <SurfaceConstraintInfo> constraints, ref BlockStream.Writer deferredImpulseWriter)
{
for (int i = 0; i < numConstraints; i++)
{
SurfaceConstraintInfo constraint = constraints[i];
int rigidBodyIndex = constraint.RigidBodyIndex;
if (rigidBodyIndex < 0 || rigidBodyIndex >= world.NumDynamicBodies)
{
// Invalid and static bodies should be skipped
continue;
}
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 * damping;
float distance = constraint.Plane.Distance;
if (distance < 0.0f)
{
deltaVelocity += (distance / deltaTime) * tau;
}
// Calculate impulse
MotionVelocity mv = world.MotionVelocities[rigidBodyIndex];
float3 impulse = float3.zero;
if (deltaVelocity < 0.0f)
{
// Impulse magnitude
float impulseMagnitude = 0.0f;
{
float3 arm = constraint.HitPosition - (body.WorldFromBody.pos + body.Collider->MassProperties.MassDistribution.Transform.pos);
float3 jacAng = math.cross(arm, constraint.Plane.Normal);
float3 armC = jacAng * mv.InverseInertiaAndMass.xyz;
float objectMassInv = math.dot(armC, jacAng);
objectMassInv += mv.InverseInertiaAndMass.w;
impulseMagnitude = deltaVelocity / objectMassInv;
}
impulse = impulseMagnitude * constraint.Plane.Normal;
}
// Add gravity
{
// Effect of gravity on character velocity in the normal direction
float3 charVelDown = gravity * 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
});
}
}
private static void CreateConstraintFromHit(PhysicsWorld world, float3 gravity, float deltaTime, int rigidBodyIndex, ColliderKey colliderKey,
float3 hitPosition, float3 normal, float distance, bool zeroVelocity, out SurfaceConstraintInfo constraint)
{
bool bodyIsDynamic = 0 <= rigidBodyIndex && rigidBodyIndex < world.NumDynamicBodies;
constraint = new SurfaceConstraintInfo()
{
Plane = new Plane
{
Normal = normal,
Distance = distance,
},
RigidBodyIndex = rigidBodyIndex,
ColliderKey = colliderKey,
HitPosition = hitPosition,
Velocity = bodyIsDynamic && !zeroVelocity ?
world.MotionVelocities[rigidBodyIndex].LinearVelocity - world.MotionDatas[rigidBodyIndex].GravityFactor * gravity * deltaTime :
constraint.Velocity = float3.zero,
Priority = bodyIsDynamic ? 1 : 0
};
// Fix up the velocity to enable penetration recovery
if (distance < 0.0f)
{
float3 newVel = constraint.Velocity - constraint.Plane.Normal * distance;
constraint.Velocity = newVel;
}
}
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 unsafe void TestExamineActivePlanes()
{
float3 up = new float3(0, 1, 0);
SurfaceConstraintInfo plane1 = CreateConstraint(ColliderKey.Empty, float3.zero, new float3(0, 1, 0), 1, 0, 0, false, float3.zero);
SurfaceConstraintInfo plane2 = CreateConstraint(ColliderKey.Empty, float3.zero, new float3(0, -1, 0), 1, 0, 0, false, float3.zero);
SurfaceConstraintInfo plane3 = CreateConstraint(ColliderKey.Empty, float3.zero, new float3(1, 0, 0), 1, 0, 0, false, float3.zero);
SurfaceConstraintInfo plane4 = CreateConstraint(ColliderKey.Empty, float3.zero, math.normalize(new float3(1, 1, 0)), 1, 0, 0, false, float3.zero);
SurfaceConstraintInfo plane5 = CreateConstraint(ColliderKey.Empty, float3.zero, math.normalize(new float3(-1, 1, 0)), 1, 0, 0, false, float3.zero);
SurfaceConstraintInfo plane6 = CreateConstraint(ColliderKey.Empty, float3.zero, new float3(0, 0, 1), 1, 0, 0, false, float3.zero);
SurfaceConstraintInfo plane7 = CreateConstraint(ColliderKey.Empty, float3.zero, new float3(0, 0, -1), 1, 0, 0, false, new float3(0, 0, 1));
// Test the single plane
SurfaceConstraintInfo *supportPlanes = stackalloc SurfaceConstraintInfo[4];
int numSupportPlanes = 1;
supportPlanes[0] = plane1;
float3 velocity = new float3(0, -1, 0);
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, 0);
Assert.AreApproximatelyEqual(velocity.y, 0);
Assert.AreApproximatelyEqual(velocity.z, 0);
Assert.AreEqual(1, numSupportPlanes);
// 2 planes, one unnecessary
numSupportPlanes = 2;
supportPlanes[0] = plane1;
supportPlanes[1] = plane1;
velocity = new float3(0, -1, 0);
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, 0);
Assert.AreApproximatelyEqual(velocity.y, 0);
Assert.AreApproximatelyEqual(velocity.z, 0);
Assert.AreEqual(1, numSupportPlanes);
// 2 planes, both necessary
numSupportPlanes = 2;
supportPlanes[0] = plane5;
supportPlanes[1] = plane4;
velocity = new float3(0, -1, 0);
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, 0);
Assert.AreApproximatelyEqual(velocity.y, 0);
Assert.AreApproximatelyEqual(velocity.z, 0);
Assert.AreEqual(2, numSupportPlanes);
// 3 planes, 2 unnecessary
numSupportPlanes = 3;
supportPlanes[0] = plane1;
supportPlanes[1] = plane1;
supportPlanes[2] = plane1;
velocity = new float3(0, -1, 0);
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, 0);
Assert.AreApproximatelyEqual(velocity.y, 0);
Assert.AreApproximatelyEqual(velocity.z, 0);
Assert.AreEqual(1, numSupportPlanes);
// 3 planes, 1 unnecessary
numSupportPlanes = 3;
supportPlanes[0] = plane1;
supportPlanes[1] = plane1;
supportPlanes[2] = plane3;
velocity = new float3(0, -1, 0);
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, 0);
Assert.AreApproximatelyEqual(velocity.y, 0);
Assert.AreApproximatelyEqual(velocity.z, 0);
Assert.AreEqual(2, numSupportPlanes);
// 3 planes, all necessary
numSupportPlanes = 3;
supportPlanes[0] = plane4;
supportPlanes[1] = plane5;
supportPlanes[2] = plane6;
velocity = new float3(0, -1, 0);
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, 0);
Assert.AreApproximatelyEqual(velocity.y, 0);
Assert.AreApproximatelyEqual(velocity.z, 0);
Assert.AreEqual(3, numSupportPlanes);
// 4 planes, 3 unnecessary
numSupportPlanes = 4;
supportPlanes[0] = plane1;
supportPlanes[1] = plane1;
supportPlanes[2] = plane1;
supportPlanes[3] = plane1;
velocity = new float3(0, -1, 0);
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, 0);
Assert.AreApproximatelyEqual(velocity.y, 0);
Assert.AreApproximatelyEqual(velocity.z, 0);
Assert.AreEqual(1, numSupportPlanes);
// 4 planes, 2 unnecessary
numSupportPlanes = 4;
supportPlanes[0] = plane1;
supportPlanes[1] = plane1;
supportPlanes[2] = plane1;
supportPlanes[3] = plane3;
velocity = new float3(0, -1, 0);
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, 0);
Assert.AreApproximatelyEqual(velocity.y, 0);
Assert.AreApproximatelyEqual(velocity.z, 0);
Assert.AreEqual(2, numSupportPlanes);
// 4 planes, 1 unnecessary
numSupportPlanes = 4;
supportPlanes[0] = plane1;
supportPlanes[1] = plane4;
supportPlanes[2] = plane5;
supportPlanes[3] = plane6;
velocity = new float3(0, -1, 0);
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, 0);
Assert.AreApproximatelyEqual(velocity.y, 0);
Assert.AreApproximatelyEqual(velocity.z, 0);
Assert.AreEqual(3, numSupportPlanes);
// 4 planes, all necessary
numSupportPlanes = 4;
supportPlanes[0] = plane4;
supportPlanes[1] = plane5;
supportPlanes[2] = plane6;
supportPlanes[3] = plane7;
velocity = new float3(0, -1, 0);
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
Assert.AreApproximatelyEqual(velocity.x, 0);
Assert.AreApproximatelyEqual(velocity.y, 0);
Assert.AreApproximatelyEqual(velocity.z, 0);
Assert.AreEqual(4, numSupportPlanes);
}
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);
}
}
}
}
private static void CreateMaxSlopeConstraint(float3 up, ref SurfaceConstraintInfo constraint, out SurfaceConstraintInfo maxSlopeConstraint)
{
float verticalComponent = math.dot(constraint.Plane.Normal, up);
SurfaceConstraintInfo newConstraint = constraint;
newConstraint.Plane.Normal = math.normalize(newConstraint.Plane.Normal - verticalComponent * up);
newConstraint.IsMaxSlope = true;
float distance = newConstraint.Plane.Distance;
// Calculate distance to the original plane along the new normal.
// Clamp the new distance to 2x the old distance to avoid penetration recovery explosions.
newConstraint.Plane.Distance = distance / math.max(math.dot(newConstraint.Plane.Normal, constraint.Plane.Normal), 0.5f);
if (newConstraint.Plane.Distance < 0.0f)
{
// Disable penetration recovery for the original plane
constraint.Plane.Distance = 0.0f;
// Prepare velocity to resolve penetration
ResolveConstraintPenetration(ref newConstraint);
}
// Output max slope constraint
maxSlopeConstraint = newConstraint;
}
public static unsafe void CollideAndIntegrate(
CharacterControllerStepInput stepInput, float characterMass, bool affectBodies, Unity.Physics.Collider *collider,
ref RigidTransform transform, ref float3 linearVelocity, ref NativeStream.Writer deferredImpulseWriter,
NativeList <StatefulCollisionEvent> collisionEvents = default, NativeList <StatefulTriggerEvent> triggerEvents = default)
{
// 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> triggerHits = default;
if (triggerEvents.IsCreated)
{
triggerHits = new NativeList <ColliderCastHit>(k_DefaultQueryHitsCapacity / 4, Allocator.Temp);
}
NativeList <ColliderCastHit> castHits = new NativeList <ColliderCastHit>(k_DefaultQueryHitsCapacity, Allocator.Temp);
CharacterControllerAllHitsCollector <ColliderCastHit> collector = new CharacterControllerAllHitsCollector <ColliderCastHit>(
stepInput.RigidBodyIndex, 1.0f, ref castHits, world, triggerHits);
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];
CreateConstraint(stepInput.World, stepInput.Up,
hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, math.dot(-hit.SurfaceNormal, hit.Fraction * displacement),
stepInput.SkinWidth, maxSlopeCos, ref constraints);
}
// Update trigger events
if (triggerEvents.IsCreated)
{
UpdateTriggersSeen(stepInput, triggerHits, triggerEvents, collector.MinHitFraction);
}
}
// 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);
CharacterControllerAllHitsCollector <DistanceHit> distanceHitsCollector = new CharacterControllerAllHitsCollector <DistanceHit>(
stepInput.RigidBodyIndex, stepInput.ContactTolerance, ref distanceHits, world);
{
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)
{
// 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)
{
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(remainingTime, minDeltaTime, up, stepInput.MaxMovementSpeed, constraints, ref newPosition, ref newVelocity, out float integratedTime);
// Apply impulses to hit bodies and store collision events
if (affectBodies || collisionEvents.IsCreated)
{
CalculateAndStoreDeferredImpulsesAndCollisionEvents(stepInput, affectBodies, characterMass,
prevVelocity, constraints, ref deferredImpulseWriter, collisionEvents);
}
// 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 CharacterControllerClosestHitCollector <ColliderCastHit>(constraints, world, 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;
// Move character along the newDisplacement direction until it reaches this new contact
{
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 so that the distance query will update results
transform.pos = newPosition;
}
// Write back final velocity
linearVelocity = newVelocity;
}
private static unsafe void CalculateAndStoreDeferredImpulses(
CharacterControllerStepInput stepInput, float characterMass, float3 linearVelocity, int numConstraints,
ref NativeArray <SurfaceConstraintInfo> constraints, ref BlockStream.Writer deferredImpulseWriter)
{
PhysicsWorld world = stepInput.World;
for (int i = 0; i < numConstraints; i++)
{
SurfaceConstraintInfo constraint = constraints[i];
int rigidBodyIndex = constraint.RigidBodyIndex;
if (rigidBodyIndex < 0 || rigidBodyIndex >= world.NumDynamicBodies)
{
// Invalid and static bodies should be skipped
continue;
}
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];
float3 impulse = float3.zero;
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
});
}
}
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 SolveCollisionConstraints(PhysicsWorld world, float deltaTime, int maxIterations, float skinWidth, float maxSlope, Collider *collider, ref RigidTransform transform, ref float3 velocity, ref NativeArray <DistanceHit> distanceHits, ref NativeArray <ColliderCastHit> colliderHits, ref NativeArray <SurfaceConstraintInfo> surfaceConstraints)
{
float remainingTime = deltaTime;
float3 previousDisplacement = velocity * remainingTime;
float3 outPosition = transform.pos;
float3 outVelocity = velocity;
quaternion orientation = transform.rot;
const float timeEpsilon = 0.000001f;
for (int i = 0; i < maxIterations && remainingTime > timeEpsilon; i++)
{
MaxHitCollector <DistanceHit> distanceHitCollector = new MaxHitCollector <DistanceHit>(skinWidth, ref distanceHits);
int constraintCount = 0;
// Handle distance checks
{
ColliderDistanceInput input = new ColliderDistanceInput
{
Collider = collider,
MaxDistance = skinWidth,
Transform = new RigidTransform
{
pos = outPosition,
rot = orientation
}
};
world.CalculateDistance(input, ref distanceHitCollector);
for (int hitIndex = 0; hitIndex < distanceHitCollector.NumHits; hitIndex++)
{
DistanceHit hit = distanceHitCollector.AllHits[hitIndex];
CreateConstraintFromHit(world, hit.ColliderKey, hit.RigidBodyIndex, hit.Position, float3.zero, hit.SurfaceNormal, hit.Distance, deltaTime, out SurfaceConstraintInfo constraint);
CreateSlopeConstraint(math.up(), math.cos(maxSlope), ref constraint, ref surfaceConstraints, ref constraintCount);
surfaceConstraints[constraintCount++] = constraint;
}
}
// Handle Collider
{
float3 displacement = previousDisplacement;
MaxHitCollector <ColliderCastHit> colliderHitCollector = new MaxHitCollector <ColliderCastHit>(1.0f, ref colliderHits);
ColliderCastInput input = new ColliderCastInput
{
Collider = collider,
Position = outPosition,
Direction = velocity,
Orientation = orientation
};
world.CastCollider(input, ref colliderHitCollector);
for (int hitIndex = 0; hitIndex < colliderHitCollector.NumHits; hitIndex++)
{
ColliderCastHit hit = colliderHitCollector.AllHits[hitIndex];
bool duplicate = false;
for (int distanceHitIndex = 0; distanceHitIndex < distanceHitCollector.NumHits; distanceHitIndex++)
{
DistanceHit distanceHit = distanceHitCollector.AllHits[distanceHitIndex];
if (distanceHit.RigidBodyIndex == hit.RigidBodyIndex && distanceHit.ColliderKey.Equals(hit.ColliderKey))
{
duplicate = true;
break;
}
}
if (!duplicate)
{
CreateConstraintFromHit(world, hit.ColliderKey, hit.RigidBodyIndex, hit.Position, outVelocity, hit.SurfaceNormal, hit.Fraction * math.length(previousDisplacement), deltaTime, out SurfaceConstraintInfo constraint);
CreateSlopeConstraint(math.up(), math.cos(maxSlope), ref constraint, ref surfaceConstraints, ref constraintCount);
surfaceConstraints[constraintCount++] = constraint;
}
}
}
float3 previousPosition = outPosition;
float3 previousVelocity = outVelocity;
SimplexSolver.Solve(world, remainingTime, math.up(), constraintCount, ref surfaceConstraints, ref outPosition, ref outVelocity, out float integratedTime);
float3 currentDisplacement = outPosition - previousPosition;
MaxHitCollector <ColliderCastHit> displacementHitCollector = new MaxHitCollector <ColliderCastHit>(1.0f, ref colliderHits);
int displacementContactIndex = -1;
if (math.lengthsq(currentDisplacement) > SimplexSolver.c_SimplexSolverEpsilon)
{
ColliderCastInput input = new ColliderCastInput
{
Collider = collider,
Position = previousPosition,
Direction = currentDisplacement,
Orientation = orientation
};
world.CastCollider(input, ref displacementHitCollector);
for (int hitIndex = 0; hitIndex < distanceHitCollector.NumHits; hitIndex++)
{
ColliderCastHit hit = displacementHitCollector.AllHits[hitIndex];
bool duplicate = false;
for (int constrainIndex = 0; constrainIndex < constraintCount; constrainIndex++)
{
SurfaceConstraintInfo constraint = surfaceConstraints[constrainIndex];
if (constraint.RigidBodyIndex == hit.RigidBodyIndex && constraint.ColliderKey.Equals(hit.ColliderKey))
{
duplicate = true;
break;
}
if (!duplicate)
{
displacementContactIndex = hitIndex;
break;
}
}
}
if (displacementContactIndex >= 0)
{
ColliderCastHit newContact = displacementHitCollector.AllHits[displacementContactIndex];
float fraction = newContact.Fraction / math.length(currentDisplacement);
integratedTime *= fraction;
float3 displacement = currentDisplacement * fraction;
outPosition = previousPosition + displacement;
}
}
remainingTime -= integratedTime;
previousDisplacement = outVelocity * remainingTime;
}
transform.pos = outPosition;
velocity = outVelocity;
}
public static unsafe void Solve(PhysicsWorld world, float deltaTime, float3 up, int numConstraints,
ref NativeArray <SurfaceConstraintInfo> constraints, ref float3 position, ref float3 velocity, out float integratedTime)
{
// List of planes to solve against (up to 4)
byte * supportPlanesMemory = stackalloc byte[4 * sizeof(SupportPlane)];
SupportPlane *supportPlanes = (SupportPlane *)supportPlanesMemory;
int numSupportPlanes = 0;
float remainingTime = deltaTime;
float currentTime = 0.0f;
// petarm.todo: introduce minDeltaTime after which solver breaks
while (remainingTime > 0.0f)
{
int hitIndex = -1;
float minCollisionTime = remainingTime;
// Iterate over constraints and solve them
for (int i = 0; i < numConstraints; i++)
{
if (constraints[i].Touched)
{
continue;
}
if (numSupportPlanes >= 1 && supportPlanes[0].Index == i)
{
continue;
}
if (numSupportPlanes >= 2 && supportPlanes[1].Index == i)
{
continue;
}
if (numSupportPlanes >= 3 && supportPlanes[2].Index == i)
{
continue;
}
SurfaceConstraintInfo constraint = constraints[i];
float3 relVel = velocity - constraint.Velocity;
float relProjVel = -math.dot(relVel, constraint.Plane.Normal);
if (relProjVel <= 0.0f)
{
continue;
}
// Clamp distance to 0, since penetration is handled by constraint.Velocity already
float distance = math.max(constraint.Plane.Distance, 0.0f);
if (distance <= minCollisionTime * relProjVel)
{
minCollisionTime = distance / relProjVel;
hitIndex = i;
}
}
// Integrate
{
float minCollisionTimeClamped = math.max(minCollisionTime, 0.0f);
currentTime += minCollisionTimeClamped;
remainingTime -= minCollisionTimeClamped;
position += minCollisionTime * velocity;
}
if (hitIndex < 0)
{
break;
}
// Mark constraint as touched
{
var constraint = constraints[hitIndex];
constraint.Touched = true;
constraints[hitIndex] = constraint;
}
// Add the hit to the current list of active planes
{
SupportPlane supportPlane = new SupportPlane()
{
Index = hitIndex,
SurfaceConstraintInfo = constraints[hitIndex]
};
supportPlanes[numSupportPlanes] = supportPlane;
numSupportPlanes++;
}
// Solve support planes
ExamineActivePlanes(up, supportPlanes, ref numSupportPlanes, ref velocity);
// Can't handle more than 4 support planes
if (numSupportPlanes == 4)
{
break;
}
}
integratedTime = currentTime;
}
