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