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; }