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; }
public static unsafe void CheckSupport( ref PhysicsWorld world, ref PhysicsCollider collider, CharacterControllerStepInput stepInput, RigidTransform transform, out CharacterSupportState characterState, out float3 surfaceNormal, out float3 surfaceVelocity, NativeList <StatefulCollisionEvent> collisionEvents = default) { surfaceNormal = float3.zero; surfaceVelocity = float3.zero; // Up direction must be normalized Assert.IsTrue(Unity.Physics.Math.IsNormalized(stepInput.Up)); // Query the world NativeList <ColliderCastHit> castHits = new NativeList <ColliderCastHit>(k_DefaultQueryHitsCapacity, Allocator.Temp); CharacterControllerAllHitsCollector <ColliderCastHit> castHitsCollector = new CharacterControllerAllHitsCollector <ColliderCastHit>( stepInput.RigidBodyIndex, 1.0f, ref castHits, world); var maxDisplacement = -stepInput.ContactTolerance * stepInput.Up; { ColliderCastInput input = new ColliderCastInput() { Collider = collider.ColliderPtr, Orientation = transform.rot, Start = transform.pos, End = transform.pos + maxDisplacement }; world.CastCollider(input, ref castHitsCollector); } // If no hits, proclaim unsupported state if (castHitsCollector.NumHits == 0) { characterState = CharacterSupportState.Unsupported; return; } float maxSlopeCos = math.cos(stepInput.MaxSlope); // Iterate over distance hits and create constraints from them NativeList <SurfaceConstraintInfo> constraints = new NativeList <SurfaceConstraintInfo>(k_DefaultConstraintsCapacity, Allocator.Temp); float maxDisplacementLength = math.length(maxDisplacement); for (int i = 0; i < castHitsCollector.NumHits; i++) { ColliderCastHit hit = castHitsCollector.AllHits[i]; CreateConstraint(stepInput.World, stepInput.Up, hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Fraction * maxDisplacementLength, stepInput.SkinWidth, maxSlopeCos, ref constraints); } // Velocity for support checking float3 initialVelocity = maxDisplacement / stepInput.DeltaTime; // Solve downwards (don't use min delta time, try to solve full step) float3 outVelocity = initialVelocity; float3 outPosition = transform.pos; SimplexSolver.Solve(stepInput.DeltaTime, stepInput.DeltaTime, stepInput.Up, stepInput.MaxMovementSpeed, constraints, ref outPosition, ref outVelocity, out float integratedTime, false); // Get info on surface int numSupportingPlanes = 0; { for (int j = 0; j < constraints.Length; j++) { var constraint = constraints[j]; if (constraint.Touched && !constraint.IsTooSteep && !constraint.IsMaxSlope) { numSupportingPlanes++; surfaceNormal += constraint.Plane.Normal; surfaceVelocity += constraint.Velocity; // Add supporting planes to collision events if (collisionEvents.IsCreated) { var collisionEvent = new StatefulCollisionEvent(stepInput.World.Bodies[stepInput.RigidBodyIndex].Entity, stepInput.World.Bodies[constraint.RigidBodyIndex].Entity, stepInput.RigidBodyIndex, constraint.RigidBodyIndex, ColliderKey.Empty, constraint.ColliderKey, constraint.Plane.Normal); collisionEvent.CollisionDetails = new StatefulCollisionEvent.Details(1, 0, constraint.HitPosition); collisionEvents.Add(collisionEvent); } } } if (numSupportingPlanes > 0) { float invNumSupportingPlanes = 1.0f / numSupportingPlanes; surfaceNormal *= invNumSupportingPlanes; surfaceVelocity *= invNumSupportingPlanes; surfaceNormal = math.normalize(surfaceNormal); } } // Check support state { if (math.lengthsq(initialVelocity - outVelocity) < k_SimplexSolverEpsilonSq) { // If velocity hasn't changed significantly, declare unsupported state characterState = CharacterSupportState.Unsupported; } else if (math.lengthsq(outVelocity) < k_SimplexSolverEpsilonSq && numSupportingPlanes > 0) { // If velocity is very small, declare supported state characterState = CharacterSupportState.Supported; } else { // Check if sliding outVelocity = math.normalize(outVelocity); float slopeAngleSin = math.max(0.0f, math.dot(outVelocity, -stepInput.Up)); float slopeAngleCosSq = 1 - slopeAngleSin * slopeAngleSin; if (slopeAngleCosSq <= maxSlopeCos * maxSlopeCos) { characterState = CharacterSupportState.Sliding; } else if (numSupportingPlanes > 0) { characterState = CharacterSupportState.Supported; } else { // If numSupportingPlanes is 0, surface normal is invalid, so state is unsupported characterState = CharacterSupportState.Unsupported; } } } }