public void Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex) { if (chunk.Has(collisionEventBufferType)) { BufferAccessor <StatefulCollisionEvent> collisionEventsBufferAccessor = chunk.GetBufferAccessor(collisionEventBufferType); for (var i = 0; i < chunk.Count; i++) { DynamicBuffer <StatefulCollisionEvent> collisionEventsBuffer = collisionEventsBufferAccessor[i]; for (var j = collisionEventsBuffer.Length - 1; j >= 0; j--) { StatefulCollisionEvent collisionEvent = collisionEventsBuffer[j]; if (collisionEvent._isStale) { if (collisionEvent.State == PhysicsEventState.Exit) { collisionEventsBuffer.RemoveAt(j); } else { collisionEvent.State = PhysicsEventState.Exit; collisionEventsBuffer[j] = collisionEvent; } } } } } }
private void ProcessForEntity(Entity entity, Entity otherEntity, float3 normal, bool hasDetails, CollisionEvent.Details collisionEventDetails) { DynamicBuffer <StatefulCollisionEvent> collisionEventBuffer = collisionEventBufferFromEntity[entity]; var foundMatch = false; for (var i = 0; i < collisionEventBuffer.Length; i++) { StatefulCollisionEvent collisionEvent = collisionEventBuffer[i]; // If entity is already there, update to Stay if (collisionEvent.Entity == otherEntity) { foundMatch = true; collisionEvent.Normal = normal; collisionEvent.HasCollisionDetails = hasDetails; collisionEvent.AverageContactPointPosition = collisionEventDetails.AverageContactPointPosition; collisionEvent.EstimatedImpulse = collisionEventDetails.EstimatedImpulse; collisionEvent.State = PhysicsEventState.Stay; collisionEvent._isStale = false; collisionEventBuffer[i] = collisionEvent; break; } } // If it's a new entity, add as Enter if (!foundMatch) { collisionEventBuffer.Add( new StatefulCollisionEvent { Entity = otherEntity, Normal = normal, HasCollisionDetails = hasDetails, AverageContactPointPosition = collisionEventDetails.AverageContactPointPosition, EstimatedImpulse = collisionEventDetails.EstimatedImpulse, State = PhysicsEventState.Enter, _isStale = false } ); } }
public void Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex) { BufferAccessor <StatefulCollisionEvent> collisionEventsBufferAccessor = chunk.GetBufferAccessor(collisionEventBufferType); for (var i = 0; i < chunk.Count; i++) { DynamicBuffer <StatefulCollisionEvent> collisionEventsBuffer = collisionEventsBufferAccessor[i]; for (var j = collisionEventsBuffer.Length - 1; j >= 0; j--) { StatefulCollisionEvent collisionEventElement = collisionEventsBuffer[j]; collisionEventElement._isStale = true; collisionEventsBuffer[j] = collisionEventElement; } } }
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); } } } }
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; } } } }