// I might not want to get the closest enemy, but a specific enemy. // might use entity.id + entity.version of an entity and get position of entity // if current target is destroyed, maybe seach for new target or destroy bullet // add another option for auto mode // start with auto and maybe can focus on specifics later public void CheckForObstacle(int index) { CollisionFilter obstacleFilter = new CollisionFilter() { BelongsTo = ~0u, CollidesWith = 16u + groupIndex, GroupIndex = 0 }; PointDistanceInput obstacePointDistanceInput = new PointDistanceInput { Position = cellSeparation[index], MaxDistance = boidsData[index].obstacleAversionDistance, Filter = obstacleFilter }; if (physicsWorld.CalculateDistance(obstacePointDistanceInput, out DistanceHit obstaceHit)) { cellObstaclePositions[index] = obstaceHit.Position; cellObstacleDistance[index] = obstaceHit.Distance; if (obstaceHit.Distance < settings.boidRadius) { Entity targetEntity = physicsWorld.CollisionWorld.Bodies[obstaceHit.RigidBodyIndex].Entity; var hash = (int)math.hash(new int2(targetEntity.Index, targetEntity.Version)); damageDict.Add(hash, 1); killTrigger[index] = 1; } } else { cellObstacleDistance[index] = boidsData[index].obstacleAversionDistance + 1; } }
public void Execute() { if (CollectAllHits) { World.CalculateDistance(PointDistanceInput, ref DistanceHits); } else if (World.CalculateDistance(PointDistanceInput, out DistanceHit hit)) { DistanceHits.Add(hit); } }
protected override JobHandle OnUpdate(JobHandle inputDeps) { PhysicsWorld physicsWorld = buildPhysicsWorld.PhysicsWorld; inputDeps = JobHandle.CombineDependencies(inputDeps, buildPhysicsWorld.FinalJobHandle); inputDeps = JobHandle.CombineDependencies(inputDeps, stepPhysicsWorld.FinalJobHandle); var jobHandle = Entities.ForEach((ref DynamicBuffer <EntityBufferElement> adjacentEntities, in Translation position, in BugComponent bug) => { NativeList <DistanceHit> distanceHits = new NativeList <DistanceHit>(Allocator.Temp); var pointDistanceInput = new PointDistanceInput { Position = position.Value, MaxDistance = bug.Radius, Filter = LayerFilter(Layer.Bug, Layer.Obstacle) }; physicsWorld.CalculateDistance(pointDistanceInput, ref distanceHits); adjacentEntities.Clear(); var adjacent = adjacentEntities.Reinterpret <Entity>(); for (int i = 0; i < distanceHits.Length; i++) { adjacent.Add(distanceHits[i].Entity); } distanceHits.Dispose(); })
public void Execute() { var colliderDistanceInput = new ColliderDistanceInput { Collider = Collider, Transform = Transform, MaxDistance = MaxDistance }; if (CollectAllHits) { World.CalculateDistance(colliderDistanceInput, ref DistanceHits); } else if (World.CalculateDistance(colliderDistanceInput, out DistanceHit hit)) { DistanceHits.Add(hit); } }
public void Execute() { var pointDistanceInput = new PointDistanceInput { Position = transData.Value, MaxDistance = laserData.range, Filter = CollisionFilter.Default }; if (collectAll) { physWorld.CalculateDistance(pointDistanceInput, ref distanceHits); } else { if (physWorld.CalculateDistance(pointDistanceInput, out Unity.Physics.DistanceHit hit)) { distanceHits.Add(hit); } } }
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; } } } } }
protected override void OnUpdate() { Entities.ForEach((ref Proximity proximity, ref Translation translation, ref PhysicsCollider collider) => { ref PhysicsWorld physicsWorld = ref World.DefaultGameObjectInjectionWorld.GetExistingSystem <BuildPhysicsWorld>().PhysicsWorld; if (collider.Value.IsCreated) { var pointDistanceInput = new PointDistanceInput { Position = translation.Value, MaxDistance = proximity.maxDistance, Filter = collider.Value.Value.Filter }; // Assign DistanceHit data to proximiy component physicsWorld.CalculateDistance(pointDistanceInput, out proximity.distanceHit); } }).WithoutBurst().Run();
public void Execute(ref Translation translation) { NativeList <DistanceHit> hits = new NativeList <DistanceHit>(Allocator.Temp); CollisionFilter targetFilter = new CollisionFilter() { BelongsTo = ~0u, CollidesWith = 8u, GroupIndex = 0 }; var pointDistanceInput = new PointDistanceInput { Position = translation.Value, MaxDistance = 50, Filter = targetFilter }; physicsWorld.CalculateDistance(pointDistanceInput, ref hits); hits.Dispose(); }
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 void Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex) { NativeArray <CharacterController> chunkCharacterControllers = chunk.GetNativeArray(CharacterControllerType); NativeArray <PhysicsCollider> chunkPhysicsColliders = chunk.GetNativeArray(PhysicsColliderType); NativeArray <Translation> chunkTranslations = chunk.GetNativeArray(TranslationType); NativeArray <Rotation> chunkRotations = chunk.GetNativeArray(RotationType); for (int i = 0; i < chunk.Count; i++) { CharacterController controller = chunkCharacterControllers[i]; PhysicsCollider collider = chunkPhysicsColliders[i]; Translation translation = chunkTranslations[i]; Rotation rotation = chunkRotations[i]; RigidTransform transform = new RigidTransform { pos = translation.Value, rot = rotation.Value }; unsafe { Collider *queryCollider; { Collider *colliderPtr = collider.ColliderPtr; byte *copiedColliderMemory = stackalloc byte[colliderPtr->MemorySize]; queryCollider = (Collider *)(copiedColliderMemory); UnsafeUtility.MemCpy(queryCollider, colliderPtr, colliderPtr->MemorySize); queryCollider->Filter = CollisionFilter.Default; } KinematicMotorUtilities.MaxHitCollector <DistanceHit> distanceHitCollector = new KinematicMotorUtilities.MaxHitCollector <DistanceHit>(controller.GroundTollerance, ref DistanceHits); { ColliderDistanceInput input = new ColliderDistanceInput { MaxDistance = controller.GroundTollerance, Transform = transform, Collider = queryCollider }; World.CalculateDistance(input, ref distanceHitCollector); } for (int hitIndex = 0; hitIndex < distanceHitCollector.NumHits; hitIndex++) { DistanceHit hit = distanceHitCollector.AllHits[hitIndex]; KinematicMotorUtilities.CreateConstraintFromHit(World, hit.ColliderKey, hit.RigidBodyIndex, hit.Position, float3.zero, hit.SurfaceNormal, hit.Distance, DeltaTime, out SurfaceConstraintInfo constraint); SurfaceConstraintInfos[hitIndex] = constraint; } float3 outPosition = transform.pos; float3 outVelocity = -math.up(); SimplexSolver.Solve(World, DeltaTime, math.up(), distanceHitCollector.NumHits, ref SurfaceConstraintInfos, ref outPosition, ref outVelocity, out float integratedTime); if (distanceHitCollector.NumHits == 0) { controller.State = CharacterControllerState.NONE; } else { outVelocity = math.normalize(outVelocity); float slopeAngleSin = math.dot(outVelocity, -math.up()); float slopeAngleCosSq = 1 - slopeAngleSin * slopeAngleSin; float maxSlopeCos = math.cos(controller.MaxSlope); controller.State = CharacterControllerState.GROUNDED; } } // Apply data back to chunk { chunkCharacterControllers[i] = controller; } } }
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; }
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; }
protected override void OnUpdate() { EntityQuery eq = GetEntityQuery(typeof(Boid_ComponentData)); cellVsEntityPositions.Clear(); if (eq.CalculateEntityCount() > cellVsEntityPositions.Capacity) { cellVsEntityPositions.Capacity = eq.CalculateEntityCount(); } unsafe { PhysicsWorld pw = bpw.PhysicsWorld; Entities.ForEach((ref Boid_ComponentData bc, ref Translation trans, ref Rotation rot) => { bc.obstacleAvoidance = float3.zero; PhysicsWorld pWorld = pw; float angle; ColliderDistanceInput colliderDistanceInput = new ColliderDistanceInput { Collider = (Unity.Physics.Collider *)(bc.colliderCast.GetUnsafePtr()), Transform = new RigidTransform(rot.Value, trans.Value), MaxDistance = bc.boidManagerReference.Value.blobManagerArray[0].maxObstacleDistance }; if (pWorld.CalculateDistance(colliderDistanceInput, out DistanceHit hit)) { angle = math.acos( math.dot(bc.velocity, (hit.Position - trans.Value)) / (math.length(bc.velocity) * math.length(hit.Position - trans.Value)) ); if (math.abs(angle) <= bc.boidManagerReference.Value.blobManagerArray[0].fieldOfView) { bc.obstacleAvoidance = (trans.Value - hit.Position) / (math.distance(trans.Value, hit.Position)); bc.obstacleAvoidance = (math.length(bc.velocity) * (angle / bc.boidManagerReference.Value.blobManagerArray[0].fieldOfView)) * bc.obstacleAvoidance; } } }).ScheduleParallel(); } NativeMultiHashMap <int, Boid_ComponentData> .ParallelWriter cellVsEntityPositionsParallel = cellVsEntityPositions.AsParallelWriter(); Entities.ForEach((ref Boid_ComponentData bc, ref Translation trans) => { Boid_ComponentData bcValues = new Boid_ComponentData(); bcValues = bc; bcValues.currentPosition = trans.Value; cellVsEntityPositionsParallel.Add(GetUniqueKeyForPosition(trans.Value, bc.boidManagerReference.Value.blobManagerArray[0].cellSize), bcValues); }).ScheduleParallel(); float deltaTime = Time.DeltaTime; NativeMultiHashMap <int, Boid_ComponentData> cellVsEntityPositionsForJob = cellVsEntityPositions; Entities.WithBurst().WithReadOnly(cellVsEntityPositionsForJob).ForEach((ref Boid_ComponentData bc, ref Translation trans, ref Rotation rot) => { int key = GetUniqueKeyForPosition(trans.Value, bc.boidManagerReference.Value.blobManagerArray[0].cellSize); NativeMultiHashMapIterator <int> nmhKeyIterator; Boid_ComponentData neighbour; int total = 0; float3 separation = float3.zero; float3 alignment = float3.zero; float3 coheshion = float3.zero; float angle; if (cellVsEntityPositionsForJob.TryGetFirstValue(key, out neighbour, out nmhKeyIterator)) { do { if (!trans.Value.Equals(neighbour.currentPosition) && math.distance(trans.Value, neighbour.currentPosition) < bc.boidManagerReference.Value.blobManagerArray[0].perceptionRadius) { angle = math.acos( math.dot(bc.velocity, (neighbour.currentPosition - trans.Value)) / (math.length(bc.velocity) * math.length(neighbour.currentPosition - trans.Value)) ); if (math.abs(angle) <= bc.boidManagerReference.Value.blobManagerArray[0].fieldOfView) { if (total >= bc.boidManagerReference.Value.blobManagerArray[0].maxPercived) { break; } float3 distanceFromTo = trans.Value - neighbour.currentPosition; separation += (distanceFromTo / math.distance(trans.Value, neighbour.currentPosition)); coheshion += neighbour.currentPosition; alignment += neighbour.velocity; total++; bc.debug = angle; } } } while (cellVsEntityPositionsForJob.TryGetNextValue(out neighbour, ref nmhKeyIterator)); if (total > 0) { coheshion = coheshion / total; coheshion = coheshion - (trans.Value + bc.velocity); coheshion = math.normalize(coheshion) * bc.boidManagerReference.Value.blobManagerArray[0].cohesionBias; separation = separation / total; separation = separation - bc.velocity; separation = math.normalize(separation) * bc.boidManagerReference.Value.blobManagerArray[0].separationBias; alignment = alignment / total; alignment = alignment - bc.velocity; alignment = math.normalize(alignment) * bc.boidManagerReference.Value.blobManagerArray[0].alignmentBias; } bc.acceleration += (coheshion + alignment + separation) + bc.obstacleAvoidance; rot.Value = math.slerp(rot.Value, quaternion.LookRotation(math.normalize(bc.velocity), math.up()), deltaTime * math.length(bc.velocity)); bc.velocity = bc.velocity + bc.acceleration; bc.velocity = math.normalize(bc.velocity) * bc.speed; trans.Value = math.lerp(trans.Value, (trans.Value + bc.velocity), deltaTime * bc.boidManagerReference.Value.blobManagerArray[0].step); bc.acceleration = math.normalize(bc.target - trans.Value) * bc.boidManagerReference.Value.blobManagerArray[0].targetBias; } }).ScheduleParallel(); }
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 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(); }
protected override void OnUpdate() { var ecb = es_ecb.CreateCommandBuffer(); var parallelECB = ecb.ToConcurrent(); PhysicsWorld pw = bpw.PhysicsWorld; float deltaTime = Time.DeltaTime; float3 directionToShoot = float3.zero; unsafe { Entities.ForEach((ref ShooterComponentData scd, ref Translation trans, ref Rotation rot, in LocalToWorld ltw) => { scd.elapsedTime += deltaTime; ColliderDistanceInput colliderDistanceInput = new ColliderDistanceInput { Collider = (Unity.Physics.Collider *)(scd.colliderCast.GetUnsafePtr()), Transform = new RigidTransform(rot.Value, trans.Value), MaxDistance = 0.25f }; if (pw.CalculateDistance(colliderDistanceInput, out DistanceHit hit)) { directionToShoot = math.normalize(hit.Position - trans.Value); rot.Value = math.slerp(rot.Value, quaternion.LookRotation(directionToShoot, math.up()), deltaTime * 25); scd.projectileSpawnPosition = math.transform(ltw.Value, new float3(0, 0.3f, 0.7f)); if (scd.elapsedTime >= scd.firingInterval) { scd.elapsedTime = 0; Entity projectile = ecb.Instantiate(scd.projectile); ecb.SetComponent(projectile, new Translation { Value = scd.projectileSpawnPosition }); ecb.SetComponent(projectile, new Rotation { Value = quaternion.LookRotation(ltw.Forward, math.up()) }); ecb.AddComponent <ProjectileFired>(projectile); ecb.SetComponent(projectile, new ProjectileFired { elapsedTime = 0, projectileSpeed = scd.projectileSpeed, projectileLifeTime = scd.projectileLifeTime }); } } }).Run(); } Entities.ForEach((Entity e, int entityInQueryIndex, ref ProjectileFired pf, ref Translation trans, in LocalToWorld ltw) => { pf.elapsedTime += deltaTime; trans.Value += ltw.Forward * pf.projectileSpeed * deltaTime; if (pf.elapsedTime > pf.projectileLifeTime) { parallelECB.DestroyEntity(entityInQueryIndex, e); } }).ScheduleParallel(); es_ecb.AddJobHandleForProducer(Dependency); JobHandle jh = new ProjectileCollisionJob() { enemyGroup = GetComponentDataFromEntity <EnemyComponentData>(), activeProjectileGroup = GetComponentDataFromEntity <ProjectileFired>(), ecb = es_ecb_Job.CreateCommandBuffer() }.Schedule(spw.Simulation, ref bpw.PhysicsWorld, Dependency); es_ecb_Job.AddJobHandleForProducer(jh); }
protected override void OnUpdate() { PhysicsWorld pw = bpw.PhysicsWorld; float deltaTime = Time.DeltaTime; var parallelECB = es_ecb.CreateCommandBuffer().ToConcurrent(); unsafe { Entities.ForEach((Entity e, int entityInQueryIndex, ref RangedWeaponParentData rwpd, ref Translation trans, ref Rotation rot, in LocalToWorld ltw) => { rwpd.elapsedTime += deltaTime; float3 directionToShoot = float3.zero; ColliderDistanceInput colliderDistanceInput = new ColliderDistanceInput { Collider = (Unity.Physics.Collider *)(rwpd.colliderCast.GetUnsafePtr()), Transform = new RigidTransform(rot.Value, trans.Value), MaxDistance = 0.25f }; if (pw.CalculateDistance(colliderDistanceInput, out DistanceHit hit)) { rwpd.currentTargetDistance = math.length(hit.Position - trans.Value); directionToShoot = math.normalize(hit.Position - trans.Value); rot.Value = math.slerp(rot.Value, quaternion.LookRotation(directionToShoot, math.up()), deltaTime * 5); if (rwpd.currentTargetDistance >= 1) { float height = rwpd.currentTargetDistance / 4; float denom = math.sqrt((2 * height) / 9.8f); rwpd.initialVelocity = new float3(0, math.sqrt(2 * 9.8f * height), rwpd.currentTargetDistance / (2 * denom)); rwpd.initialVelocity = math.mul(quaternion.LookRotation(directionToShoot, math.up()), rwpd.initialVelocity); if (rwpd.elapsedTime >= rwpd.firingInterval) { rwpd.elapsedTime = 0; Entity defEntity = parallelECB.Instantiate(entityInQueryIndex, rwpd.cannonBall); float3 spawnPosition = math.transform(ltw.Value, new float3(0, 5.5f, 0)); parallelECB.SetComponent <Translation>(entityInQueryIndex, defEntity, new Translation { Value = spawnPosition }); parallelECB.AddComponent <CannonBallTag>(entityInQueryIndex, defEntity); parallelECB.SetComponent <CannonBallTag>(entityInQueryIndex, defEntity, new CannonBallTag { initialVelocity = rwpd.initialVelocity, cannonBallColliderCast = rwpd.cannonBallColliderCast }); } } } }).ScheduleParallel(); } Entities.ForEach((ref CannonComponentData ccd, ref Rotation rot, ref Translation trans, in LocalToWorld ltw, in Parent p) => { if (HasComponent <RangedWeaponParentData>(p.Value)) { RangedWeaponParentData rwpd = GetComponent <RangedWeaponParentData>(p.Value); if (rwpd.currentTargetDistance >= 1) { float3 localUpDirection = math.transform(math.inverse(ltw.Value), ltw.Up); float angle = math.acos(math.dot(localUpDirection, rwpd.initialVelocity) / (math.length(localUpDirection) * (math.length(rwpd.initialVelocity)))); rot.Value = math.slerp(rot.Value, quaternion.Euler(angle, 0, 0), deltaTime * 5); } } }).ScheduleParallel();
public void Execute(ArchetypeChunk batchInChunk, int batchIndex) { NativeArray <Translation> translations = batchInChunk. GetNativeArray(translationTypeHandle); NativeArray <MoveData> moveDatas = batchInChunk. GetNativeArray(moveDataTypeHandle); NativeArray <SensorData> sensorDatas = batchInChunk. GetNativeArray(sensorDataTypeHandle); for (int i = 0; i < batchInChunk.Count; i++) { int left = 0; int right = 0; int center = 0; float3 leftPosition = GetDeltaVector( sensorDatas[i].leftSensorDistance, sensorDatas[i].leftSensorAngle); float3 centerPosition = GetDeltaVector( sensorDatas[i].centralSensorDistance, sensorDatas[i].centralSensorAngle); float3 rightPosition = GetDeltaVector( sensorDatas[i].rightSensorDistance, sensorDatas[i].rightSensorAngle); NativeList <DistanceHit> distanceHits = new NativeList <DistanceHit>(Allocator.Temp); if (physicsWorld.CalculateDistance(new PointDistanceInput() { Filter = CollisionFilter.Default, MaxDistance = sensorDatas[i].centralSensorSize, Position = translations[i].Value + centerPosition }, ref distanceHits)) { center = distanceHits.Length; } if (physicsWorld.CalculateDistance(new PointDistanceInput() { Filter = CollisionFilter.Default, MaxDistance = sensorDatas[i].leftSensorSize, Position = translations[i].Value + leftPosition }, ref distanceHits)) { left = distanceHits.Length; } if (physicsWorld.CalculateDistance(new PointDistanceInput() { Filter = CollisionFilter.Default, MaxDistance = sensorDatas[i].rightSensorSize, Position = translations[i].Value + rightPosition }, ref distanceHits)) { right = distanceHits.Length; } distanceHits.Dispose(); float angle = DeltaTime * moveDatas[i].rotSpeed; //Debug.Log($"Hits are {left}, {center}, {right}"); if (right > center && right > left) { //Debug.Log("Turn right "); moveDatas[i] = new MoveData() { speed = moveDatas[i].speed, rotSpeed = moveDatas[i].rotSpeed, angle = moveDatas[i].angle - angle }; } else if (left > right && left > center) { //Debug.Log("Turn left"); moveDatas[i] = new MoveData() { speed = moveDatas[i].speed, rotSpeed = moveDatas[i].rotSpeed, angle = moveDatas[i].angle + angle }; } else if (left == right && left > center) { if (UnityEngine.Random.value > 0.5) { moveDatas[i] = new MoveData() { speed = moveDatas[i].speed, rotSpeed = moveDatas[i].rotSpeed, angle = moveDatas[i].angle - angle }; } else { moveDatas[i] = new MoveData() { speed = moveDatas[i].speed, rotSpeed = moveDatas[i].rotSpeed, angle = moveDatas[i].angle + angle }; } } } }
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 CheckSupport( ref PhysicsWorld world, ref PhysicsCollider collider, CharacterControllerStepInput stepInput, RigidTransform transform, float maxSlope, out CharacterSupportState characterState, out float3 surfaceNormal, out float3 surfaceVelocity) { surfaceNormal = float3.zero; surfaceVelocity = float3.zero; // Query the world 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.ColliderPtr }; world.CalculateDistance(input, ref distanceHitsCollector); } // 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)); float maxSlopeCos = math.cos(maxSlope); // Iterate over distance hits and create constraints from them NativeList <SurfaceConstraintInfo> constraints = new NativeList <SurfaceConstraintInfo>(k_DefaultConstraintsCapacity, Allocator.Temp); for (int i = 0; i < distanceHitsCollector.NumHits; i++) { DistanceHit hit = distanceHitsCollector.AllHits[i]; 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); } 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, 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) { numSupportingPlanes++; surfaceNormal += constraint.Plane.Normal; surfaceVelocity += constraint.Velocity; } } 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)); float slopeAngleCosSq = 1 - slopeAngleSin * slopeAngleSin; if (slopeAngleCosSq < maxSlopeCos * maxSlopeCos) { characterState = CharacterSupportState.Sliding; } else { characterState = CharacterSupportState.Supported; } } } }