/// <summary> /// Gets the desired steering output. /// </summary> /// <param name="input">The steering input containing relevant information to use when calculating the steering output.</param> /// <param name="output">The steering output to be populated.</param> public override void GetDesiredSteering(SteeringInput input, SteeringOutput output) { if (!_jumping) { if (!_unit.isGrounded) { return; } var scanPoint = _unit.position + (_unit.forward * this.scanDistance); var unitElevation = _unit.position.y; _targetHeight = _heightSampler.SampleHeight(scanPoint) + _unit.baseToPositionOffset; if (_targetHeight - unitElevation < this.minimumHeightToJump || ((_targetHeight - unitElevation) - _unit.heightNavigationCapability.maxClimbHeight) > 0.0001f) { return; } var halfDistance = this.scanDistance / 2f; var speed = _unit.velocity.magnitude; var timeToTarget = halfDistance / speed; //Calculate the distance the unit will drop due to gravity and adjust the target height accordingly //Since gravity is assumed negative we do -0.5 instead of just 0.5 var drop = -0.5f * input.gravity * timeToTarget * timeToTarget; _targetHeight += drop; _force = _targetHeight / (Time.fixedDeltaTime * timeToTarget); } _jumping = _unit.position.y < _targetHeight; output.overrideHeightNavigation = true; output.verticalForce = _force; }
public HeightOutput GetHeightOutput(SteeringInput input, float effectiveMaxSpeed) { return new HeightOutput { finalVelocity = input.currentSpatialVelocity, isGrounded = true }; }
/// <summary> /// Calculates the arrive acceleration vector for the specified destination. /// </summary> /// <param name="destination">The destination.</param> /// <param name="input">The input.</param> /// <param name="desiredSpeed">The desired speed.</param> /// <returns>The acceleration vector</returns> protected Vector3 Arrive(Vector3 destination, SteeringInput input, float desiredSpeed) { var dir = input.unit.position.DirToXZ(destination); var distance = dir.magnitude; var arriveDistance = distance - this.arrivalDistance; //Arrival is accurate within 10 centimeters this.hasArrived = arriveDistance <= 0.01f; if (this.hasArrived) { if (this.autoCalculateSlowingDistance) { this.slowingDistance = 0f; } return -input.currentPlanarVelocity / Time.fixedDeltaTime; } //Calculate slowing distance if applicable if (this.autoCalculateSlowingDistance) { CalculateRequiredSlowingDistance(input); } //Find the target speed for arrival var targetSpeed = desiredSpeed; if (arriveDistance < this.slowingDistance) { if (this.slowingAlgorithm == SlowingAlgorithm.Logarithmic) { targetSpeed *= Mathf.Log10(((9.0f / this.slowingDistance) * arriveDistance) + 1.0f); } else { targetSpeed *= (arriveDistance / this.slowingDistance); } } var desiredVelocity = (dir / distance) * Mathf.Max(targetSpeed, input.unit.minimumSpeed); //Before determining the delta we need to evaluate if we are on arrival float targetAcceleration; if (desiredVelocity.sqrMagnitude < input.currentPlanarVelocity.sqrMagnitude) { targetAcceleration = input.maxDeceleration; } else { targetAcceleration = input.maxAcceleration; } desiredVelocity = (desiredVelocity - input.currentPlanarVelocity) / Time.fixedDeltaTime; return Vector3.ClampMagnitude(desiredVelocity, targetAcceleration); }
/// <summary> /// Gets the desired steering output. /// </summary> /// <param name="input">The steering input containing relevant information to use when calculating the steering output.</param> /// <param name="output">The steering output to be populated.</param> public override void GetDesiredSteering(SteeringInput input, SteeringOutput output) { if (this.target == null) { return; } Vector3 targetPos = this.target.position; Vector3 unitPos = input.unit.position; var squaredDistance = (targetPos - unitPos).sqrMagnitude; if (this.awarenessRadius > 0f && squaredDistance > (this.awarenessRadius * this.awarenessRadius)) { // if target is outside awareness radius return; } if (squaredDistance <= (this.stopRadius * this.stopRadius)) { // if inside stop radius, then start arriving output.desiredAcceleration = Arrive(this.stopTimeFrame, input); return; } Vector3 pursuePos = Vector3.zero; var targetUnit = target.GetUnitFacade(false); if (targetUnit == null) { // if target has no unit facade, then it is probably not a moving target anyway pursuePos = targetPos; } else { var distToOther = (targetPos - unitPos).magnitude; var targetSpeed = targetUnit.velocity.magnitude; var predictionTime = 0.1f; if (targetSpeed > 0f) { //Half the prediction time for better behavior predictionTime = (distToOther / targetSpeed) / 2f; } pursuePos = targetPos + (targetUnit.velocity * predictionTime); } if (pursuePos.sqrMagnitude == 0f) { return; } // seek the pursue position and allow speed-up output.desiredAcceleration = Seek(pursuePos, input); output.maxAllowedSpeed = input.unit.maximumSpeed; }
public float GetHeightDelta(SteeringInput input) { var grid = input.grid; if (grid == null) { return 0f; } var unit = input.unit; return grid.origin.y - (unit.position.y - unit.baseToPositionOffset); }
/// <summary> /// Gets the orientation. /// </summary> /// <param name="input">The input.</param> /// <param name="output">The output.</param> public override void GetOrientation(SteeringInput input, OrientationOutput output) { if (input.currentPlanarVelocity.sqrMagnitude < this.minimumSpeedToTurn * this.minimumSpeedToTurn) { output.desiredAngularAcceleration = -input.currentAngularSpeed / Time.fixedDeltaTime; return; } var align = this.alignWithElevation && input.unit.isGrounded; var targetOrientation = align ? input.currentFullVelocity.normalized : input.currentPlanarVelocity.normalized; output.desiredOrientation = targetOrientation; output.desiredAngularAcceleration = GetAngularAcceleration(targetOrientation, input); }
/// <summary> /// Gets the desired steering output. /// </summary> /// <param name="input">The steering input containing relevant information to use when calculating the steering output.</param> /// <param name="output">The steering output to be populated.</param> public override void GetDesiredSteering(SteeringInput input, SteeringOutput output) { if (!_jumping) { // exit early if not jumping return; } // start jumping if unit is below the target height _jumping = _unit.position.y < _targetHeight; output.overrideHeightNavigation = true; output.verticalForce = _force; }
/// <summary> /// Gets the desired steering output. /// </summary> /// <param name="input">The steering input containing relevant information to use when calculating the steering output.</param> /// <param name="output">The steering output to be populated.</param> public override void GetDesiredSteering(SteeringInput input, SteeringOutput output) { if (this.target == null) { return; } Vector3 targetPos = this.target.position; // if target is within awareness radius - or awareness radius has been set to 0 or below, then seek if ((targetPos - input.unit.position).sqrMagnitude < (this.awarenessRadius * this.awarenessRadius) || this.awarenessRadius <= 0f) { // arrive at the target position output.desiredAcceleration = Arrive(targetPos, input); } }
/// <summary> /// Gets the desired steering output. /// </summary> /// <param name="input">The steering input containing relevant information to use when calculating the steering output.</param> /// <param name="output">The steering output to be populated.</param> public override void GetDesiredSteering(SteeringInput input, SteeringOutput output) { if (this.target == null) { // if no target, exit early return; } Vector3 targetPos = this.target.position; Vector3 unitPos = input.unit.position; if (this.awarenessRadius > 0f && (targetPos - unitPos).sqrMagnitude > (this.awarenessRadius * this.awarenessRadius)) { // if distance is more than awareness radius, then exit return; } Vector3 evadePos = Vector3.zero; var targetUnit = this.target.GetUnitFacade(); if (targetUnit == null) { // if target has no unit facade, then it is probably not a moving target anyway evadePos = targetPos; } else { var distToOther = (targetPos - unitPos).magnitude; var otherSpeed = targetUnit.velocity.magnitude; var predictionTime = 0.1f; if (otherSpeed > 0f) { //Half the prediction time for better behavior predictionTime = (distToOther / otherSpeed) / 2f; } evadePos = targetPos + (targetUnit.velocity * predictionTime); } if (evadePos.sqrMagnitude == 0f) { return; } // flee evade position and allow speed-up output.desiredAcceleration = Flee(evadePos, input); output.maxAllowedSpeed = input.unit.maximumSpeed; }
/// <summary> /// Gets the desired steering output. /// </summary> /// <param name="input">The steering input containing relevant information to use when calculating the steering output.</param> /// <param name="output">The steering output to be populated.</param> public override void GetDesiredSteering(SteeringInput input, SteeringOutput output) { if (this.target == null) { return; } Vector3 targetPos = this.target.position; // if target is within awareness radius - or awareness radius has been set to 0 or below, then flee if ((targetPos - input.unit.position).sqrMagnitude < (this.awarenessRadius * this.awarenessRadius) || this.awarenessRadius <= 0f) { // flee and allow speed-up output.desiredAcceleration = Flee(targetPos, input); output.maxAllowedSpeed = input.unit.maximumSpeed; } }
/// <summary> /// Gets the desired steering output. /// </summary> /// <param name="input">The steering input containing relevant information to use when calculating the steering output.</param> /// <param name="output">The steering output to be populated.</param> public override void GetDesiredSteering(SteeringInput input, SteeringOutput output) { var grid = input.grid; if (grid == null) { _targetCell = null; return; } var unit = input.unit; var pos = unit.position; var cell = grid.GetCell(pos, true); if (cell.isWalkable(unit.attributes)) { if (_targetCell == null) { return; } var dir = pos.DirToXZ(cell.position); var distanceTreshold = (grid.cellSize / 2f) - unit.radius; if (dir.sqrMagnitude > distanceTreshold * distanceTreshold) { _targetCell = cell; } else { _targetCell = null; return; } } else if (_targetCell == null || _targetCell == cell) { _targetCell = grid.GetNearestWalkableCell(pos, pos, true, this.fleeMaxRadius, unit); } output.desiredAcceleration = Seek(_targetCell.position, input); output.maxAllowedSpeed = input.unit.maximumSpeed; }
public float GetHeightDelta(SteeringInput input) { var unit = input.unit; var baseY = unit.basePosition.y; var maxClimb = unit.heightNavigationCapability.maxClimbHeight; //Put the start at the top of the grid bounds or at the unit's head or max climb above its center, depending on whether we are on a grid or not var grid = input.grid; var matrix = grid != null ? grid.cellMatrix : null; var startY = matrix != null ? matrix.origin.y + matrix.upperBoundary : baseY + Mathf.Max(unit.height, _radius + maxClimb); //We need to sample at the position we predict we are going to be after this frame given the current velocity //If movement is vertical (or as good as) we want to look ahead a minimum distance var lookAhead = input.currentFullVelocity.OnlyXZ() * input.deltaTime; if (lookAhead.sqrMagnitude < 0.0001f && input.currentFullVelocity.y > 0f) { lookAhead = input.currentPlanarVelocity.normalized * 0.01f; } var endOne = unit.position + _toEndPointOne + lookAhead; var endTwo = unit.position + _toEndPointTwo + lookAhead; endOne.y = endTwo.y = startY; RaycastHit hit; if (!Physics.CapsuleCast(endOne, endTwo, _radius, Vector3.down, out hit, Mathf.Infinity, Layers.terrain)) { return Consts.InfiniteDrop; } //Make sure the height difference is not greater than what the unit can climb var diff = hit.point.y - baseY; var slope = Vector3.Dot(Vector3.up, hit.normal); var minSlope = Mathf.Cos(unit.heightNavigationCapability.maxSlopeAngle * Mathf.Deg2Rad); if (slope > minSlope || diff <= maxClimb) { //Get the target position we want to be at (circle center) var center = hit.point + (hit.normal * _radius); return center.y - (unit.position.y + _centerOffsetY) + unit.groundOffset; } //If the height ahead is too high, we just wanna stay at the height we are at. return 0f; }
/// <summary> /// Requests a path. /// </summary> /// <param name="destination">The destination.</param> /// <param name="input">The steering input.</param> private void RequestPath(Vector3 destination, SteeringInput input) { var unit = input.unit; // Request a path normally from the unit's current position Action <PathResult> callback = (result) => { //TODO: handle partial completion if (result.status == PathingStatus.Complete) { FollowPath(result.path, unit); return; } PathRequestCallback(result, unit, unit.position, destination, input.grid); }; QueuePathRequest(unit.position, destination, unit, callback); }
public override void GetDesiredSteering(SteeringInput input, SteeringOutput output) { var grid = input.grid; if (grid == null) { _targetCell = null; return; } var unit = input.unit; var pos = unit.position; var cell = grid.GetCell(pos, true); if (cell.isWalkable(unit.attributes)) { if (_targetCell == null) { return; } var dir = pos.DirToXZ(cell.position); var distanceTreshold = (grid.cellSize / 2f) - unit.radius; if (dir.sqrMagnitude > distanceTreshold * distanceTreshold) { _targetCell = cell; } else { _targetCell = null; return; } } else if (_targetCell == null || _targetCell == cell) { _targetCell = grid.GetNearestWalkableCell(pos, pos, true, this.fleeMaxRadius, unit); } output.desiredAcceleration = Seek(_targetCell.position, input); output.maxAllowedSpeed = input.unit.maximumSpeed; }
public float PerformAlign(SteeringInput input) { float rotation = input.targetOrientation - input.orientation; rotation = MapToRange(rotation); float rotationSize = Mathf.Abs(rotation); if (rotationSize < SATISFACTION_ORIENTATION) { return(0f); } float targetRotation; if (rotationSize > SLOW_RADIUS) { targetRotation = input.maxRotation; } else { targetRotation = input.maxRotation * rotationSize / SLOW_RADIUS; } targetRotation *= rotation / rotationSize; float angularAcceleration = targetRotation - input.rotation; angularAcceleration /= TIME_TO_TARGET; float absAngular = Mathf.Abs(angularAcceleration); if (absAngular > input.maxAngularAcceleration) { angularAcceleration /= absAngular; angularAcceleration *= input.maxAngularAcceleration; } return(angularAcceleration); }
/// <summary> /// Gets the orientation output. /// </summary> /// <param name="input">The steering input containing relevant information to use when calculating the orientation output.</param> /// <param name="output">The orientation output to be populated.</param> public override void GetOrientation(SteeringInput input, OrientationOutput output) { if (input.currentPlanarVelocity.sqrMagnitude > 0f) { return; } var grp = input.unit.transientGroup; if (grp == null) { return; } var model = grp.modelUnit; if (model == null) { return; } var targetOrientation = model.forward; output.desiredOrientation = targetOrientation; output.desiredAngularAcceleration = GetAngularAcceleration(targetOrientation, input); }
public override void GetOrientation(SteeringInput input, OrientationOutput output) { var unit = input.unit; if (!unit.nextNodePosition.HasValue) { unit.Resume(); return; } var targetOrientation = (unit.nextNodePosition.Value - unit.position).OnlyXZ().normalized; if (Vector3.Dot(unit.forward, targetOrientation) < 0.985f) { unit.Wait(null); } else { unit.Resume(); } output.desiredOrientation = targetOrientation; output.desiredAngularAcceleration = GetAngularAcceleration(targetOrientation, input); }
private SteeringInput BuildInput(Follower follower) { Vector2 targetPosition = follower.hasFollowerScript ? follower.followerTarget.position : Vector2.zero; if (!follower.hasFollowerScript && follower.target != null) { targetPosition = follower.target.transform.position.XZ(); } Vector2 targetVelocity = follower.hasFollowerScript ? follower.followerTarget.velocity : Vector2.zero; float targetOrientation = follower.hasFollowerScript ? follower.followerTarget.orientation : 0f; SteeringInput followerInput = new SteeringInput { position = follower.position, velocity = follower.velocity, orientation = follower.orientation, rotation = follower.rotation, maxVelocity = follower.maxVelocity, maxRotation = follower.maxRotation, maxAcceleration = follower.maxAcceleration, maxAngularAcceleration = follower.maxAngularAcceleration, targetPosition = targetPosition, targetVelocity = targetVelocity, targetOrientation = targetOrientation }; return(followerInput); }
/// <summary> /// Gets the desired steering output. /// </summary> /// <param name="input">The steering input containing relevant information to use when calculating the steering output.</param> /// <param name="output">The steering output to be populated.</param> public override void GetDesiredSteering(SteeringInput input, SteeringOutput output) { output.desiredAcceleration = Arrive(this.stopTimeFrame, input); }
/// <summary> /// Gets the desired steering output. /// </summary> /// <param name="input">The steering input containing relevant information to use when calculating the steering output.</param> /// <param name="output">The steering output to be populated.</param> public override void GetDesiredSteering(SteeringInput input, SteeringOutput output) { _lastSeparationVector = Vector3.zero; var unit = input.unit; Vector3 selfPos = unit.position; var targetDistance = this.separationDistance; if (this.blockedNeighboursBehaviour != SeparationMode.MaintainFullSeparation && input.grid != null) { // only do this if on a valid grid var selfCell = input.grid.GetCell(selfPos, true); if (selfCell != null) { // only do this if on a valid cell for (int x = -1; x <= 1; x++) { for (int z = -1; z <= 1; z++) { if (x == 0 && z == 0) { continue; } // visit all neighbour cells ... var neighbour = selfCell.GetNeighbour(x, z); if (neighbour != null && !neighbour.IsWalkableFromWithClearance(selfCell, unit)) { // if any neighbour cell is missing or unwalkable, return here and stop separating if (this.blockedNeighboursBehaviour == SeparationMode.Disable) { return; } targetDistance = 0f; //Break out z = x = 2; } } } } } // prepare local variables Vector3 steeringVector = Vector3.zero; int avoidedCount = 0; var others = _scanner.units; int othersCount = _scanner.sortUnitsWithDistance ? Math.Min(others.count, this.maximumUnitsToConsider) : others.count; for (int i = 0; i < othersCount; i++) { var other = others[i]; if (!other.isAlive) { continue; } if (!unit.hasArrivedAtDestination) { // as long as this unit has not arrived... if (unit.transientGroup == null || !object.ReferenceEquals(unit.transientGroup, other.transientGroup)) { // ignore units in other groups continue; } if (other.hasArrivedAtDestination) { // ignore other units that have arrived continue; } } if (other.determination < unit.determination) { // ignore units with less determination continue; } Vector3 memberPos = other.position; Vector3 direction = (selfPos - memberPos); float combinedRadius = targetDistance + unit.radius + other.radius; if (direction.sqrMagnitude >= (combinedRadius * combinedRadius)) { // ignore any and all units that this unit does not overlap continue; } // sum up separation vectors avoidedCount++; var dirMag = direction.magnitude; if (dirMag > 0f) { steeringVector += (direction / dirMag) * (combinedRadius - dirMag); } } if (avoidedCount == 0) { // if no avoidance vectors were computed, then return early return; } steeringVector /= (float)avoidedCount; if (steeringVector.sqrMagnitude <= (minimumForceMagnitude * minimumForceMagnitude)) { // if avoidance vector is too small, then return early return; } // Separation uses variable vector magnitudes // if the unit has arrived (swarming mode), use max acceleration, while unit is moving then use the separation strength factor as a percentage of the max acceleration float maxAcc = unit.hasArrivedAtDestination ? input.maxAcceleration : separationStrength * input.maxAcceleration; steeringVector = Seek(selfPos + steeringVector, input, maxAcc); _lastSeparationVector = steeringVector; output.desiredAcceleration = steeringVector; }
/// <summary> /// Gets the desired steering output. /// </summary> /// <param name="input">The steering input containing relevant information to use when calculating the steering output.</param> /// <param name="output">The steering output to be populated.</param> public override void GetDesiredSteering(SteeringInput input, SteeringOutput output) { // Set the formation position to null to make sure that it is invalid when it's supposed to be input.unit.formationPos = null; _formationPos = null; // set instance variables to be used in load balanced update method _unitData = input.unit; _grid = input.grid; if (_grid == null) { // if not on a grid, drop formation return; } // must cast the group to have access to GetFormationPosition var group = _unitData.transientGroup as DefaultSteeringTransientUnitGroup; if (group == null) { // if not in a group, drop formation return; } int count = group.count; if (count == 0) { // if there are no members in the group, drop formation return; } var modelUnit = group.modelUnit; if (modelUnit == null) { // if group's model unit is missing, then drop formation return; } if (_unitData.formationIndex < 0) { // if this unit has not been given a valid formation index, then drop formation return; } if (_unitData.hasArrivedAtDestination && this.dropFormationOnArrival) { return; } _formationPos = group.GetFormationPosition(_unitData.formationIndex); if (_formationPos == null || _stopped) { // if there is no valid formation position calculated or it is invalid currently, then drop formation return; } // make sure desiredSpeed does not spill over or under var desiredSpeed = Mathf.Min(1.5f * input.desiredSpeed, _unitData.maximumSpeed); _unitData.formationPos = _formationPos; Vector3 arrivalVector = Arrive(_formationPos.position, input, desiredSpeed); if (modelUnit.hasArrivedAtDestination && this.hasArrived) { // When the unit arrives at the designated formation position, make sure hasArrivedAtDestination becomes true // Also, must return here, to avoid outputting a result _unitData.hasArrivedAtDestination = true; return; } output.maxAllowedSpeed = desiredSpeed; output.desiredAcceleration = arrivalVector; }
/// <summary> /// Gets the desired steering output. /// </summary> /// <param name="input">The steering input containing relevant information to use when calculating the steering output.</param> /// <param name="output">The steering output to be populated.</param> public override void GetDesiredSteering(SteeringInput input, SteeringOutput output) { // Set the formation position to null to make sure that it is invalid when it's supposed to be input.unit.formationPos = null; _formationPos = null; // set instance variables to be used in load balanced update method _unitData = input.unit; _grid = input.grid; if (_grid == null) { // if not on a grid, drop formation return; } // must cast the group to have access to GetFormationPosition var group = _unitData.transientGroup as DefaultSteeringTransientUnitGroup; if (group == null) { // if not in a group, drop formation return; } int count = group.count; if (count == 0) { // if there are no members in the group, drop formation return; } var modelUnit = group.modelUnit; if (modelUnit == null) { // if group's model unit is missing, then drop formation return; } if (_unitData.formationIndex < 0) { // if this unit has not been given a valid formation index, then drop formation return; } if (_unitData.hasArrivedAtDestination && this.dropFormationOnArrival) { return; } _formationPos = group.GetFormationPosition(_unitData.formationIndex); if (_formationPos == null || _stopped) { // if there is no valid formation position calculated or it is invalid currently, then drop formation return; } // make sure desiredSpeed does not spill over or under var desiredSpeed = Mathf.Min(1.5f * input.desiredSpeed, _unitData.maximumSpeed); _unitData.formationPos = _formationPos; Vector3 arrivalVector = Arrive(_formationPos.position, input, desiredSpeed); if (modelUnit.hasArrivedAtDestination && this.hasArrived) { // When the unit arrives at the designated formation position, make sure hasArrivedAtDestination becomes true // Also, must return here, to avoid outputting a result _unitData.hasArrivedAtDestination = true; return; } output.maxAllowedSpeed = desiredSpeed; output.desiredAcceleration = arrivalVector; }
/// <summary> /// Gets the desired steering output. /// </summary> /// <param name="input">The steering input containing relevant information to use when calculating the steering output.</param> /// <param name="output">The steering output to be populated.</param> public override void GetDesiredSteering(SteeringInput input, SteeringOutput output) { if (_isPortaling) { output.pause = true; return; } if (_stopped) { return; } if (_stop) { StopInternal(); return; } if (!ResolveNextPoint()) { return; } //Handle waypoints if (_wayPoints.hasWaypoint && _pendingPathRequest == null) { //waypoint request are done if we are close to the point where we need to slow down for arrival as we want to consider the entire length of the path including waypoints when deciding when to slow down. if (_remainingTotalDistance < this.slowingDistance + _pathSettings.requestNextWaypointDistance) { //For way points we cannot use the desired end of path, but must use the actual end as the starting point. RequestPath(_endOfResolvedPath, _wayPoints.NextWaypoint(), InternalPathRequest.Type.Waypoint); } } HandlePathReplan(); if (Arrive(_currentDestination.position, _remainingTotalDistance, input, output)) { if (_pendingPathRequest != null) { return; } //TODO: this should be set whenever the unit stops, however since it may not be the only locomotion component a smarter way must be found //The SterrableUnitComponent ought to handle this, so if there is no locomotion component that have out put it is set to true; otherwise false. //Also change the description of the property _unit.hasArrivedAtDestination = true; StopAndAnnounceArrival(); } }
public float GetHeightDelta(SteeringInput input) { var unit = input.unit; var heightMap = HeightMapManager.instance.GetHeightMap(unit.position); //We need to sample at the position we predict we are going to be after this frame given the current velocity //For the front we need to lookahead at least the granularity of the height map adjusted for angle. For the sake of simplicity we just assume the worst case of 45 degrees, i.e. root(2) //If movement is vertical (or as good as) we want to look ahead a minimum distance var velo = input.currentFullVelocity.OnlyXZ(); if (velo.sqrMagnitude < 0.0001f && velo.y > 0f) { velo = input.currentPlanarVelocity; } var reqLookAhead = heightMap.granularity * Consts.SquareRootTwo; var lookAheadActual = velo * input.deltaTime; var lookAheadFixed = lookAheadActual.sqrMagnitude < reqLookAhead * reqLookAhead ? velo.normalized * reqLookAhead : lookAheadActual; var t = input.unit.transform; var center = _samplePoints[0] = t.TransformPoint(_points[0]) + lookAheadActual; _samplePoints[1] = t.TransformPoint(_points[1]) + lookAheadFixed; _samplePoints[2] = t.TransformPoint(_points[2]) + lookAheadActual; float maxClimb = unit.heightNavigationCapability.maxClimbHeight; float groundOffset = unit.groundOffset; float maxHeight = Consts.InfiniteDrop; int highIdx = 0; for (int i = 0; i < 3; i++) { float sampledHeight = heightMap.SampleHeight(_samplePoints[i]) + groundOffset; if (sampledHeight > maxHeight) { maxHeight = sampledHeight; highIdx = i; } _samplePoints[i].y = sampledHeight; } //When ascending there are situations where we need to continue the current rate of ascent even though the high point no longer dictates it. //This happens when moving from a slope onto a lesser slope or platform, we need to continue to ascend until the base is free otherwise the unit will collide with the terrain. _pendingHighMaxes.RegisterHighpoint(_samplePoints[1]); if (highIdx != 1 && _pendingHighMaxes.count > 0 && center.DirToXZ(_pendingHighMaxes.current).sqrMagnitude > _radius * _radius) { _pendingHighMaxes.MoveNext(); } var hp = _samplePoints[highIdx]; if (_pendingHighMaxes.count > 0 && (_pendingHighMaxes.currentHeight > maxHeight || (hp - center).sqrMagnitude > (_pendingHighMaxes.current - center).sqrMagnitude)) { hp = _pendingHighMaxes.current; } else if (highIdx == 0) { //If the center is the highest point and we have reached the pinnacle of the ascent, then the center is in reality the highest point return maxHeight - (center.y - _radius); } //From the base point (on the ground) find the slope vector and the center vector var bp = _samplePoints[0]; var slopeVector = (bp - hp).normalized; var centerVector = center - hp; //Find the closest point on the line and from that the closest point of the sphere periferi var dp = Vector3.Dot(centerVector, slopeVector); var closestPointLine = (dp > 0f) ? hp + (slopeVector * dp) : hp; var closestPoint = center + ((closestPointLine - center).normalized * _radius); //Find the plane normal for the plane represented by the slope line and an extra point (simply a point perpendicular to one of the other two at the same y) var pp3 = new Vector3(hp.z, hp.y, -hp.x); var pn = Vector3.Cross(pp3 - hp, hp - bp); //Get the closest y coordinate, that is the point where the sphere should rest, using the plane formula a(x-x0) + b(y-y0) + c(z-z0) = 0, where (a,b,c) is the normal vector and (x0 y0, z0) is a known point on the plane. //Since we know the x and z of the closest point we can find the proper y. var closestY = ((pn.x * (closestPoint.x - hp.x)) + (pn.z * (closestPoint.z - hp.z)) - (pn.y * hp.y)) / -pn.y; var delta = closestY - closestPoint.y; var slope = Vector3.Dot(Vector3.up, -slopeVector); var allowedSlope = Mathf.Cos((90f - unit.heightNavigationCapability.maxSlopeAngle) * Mathf.Deg2Rad); if (slope > allowedSlope && delta > maxClimb) { return 0f; } return delta; }
public float GetHeightDelta(SteeringInput input) { var unit = input.unit; var heightMap = HeightMapManager.instance.GetHeightMap(unit.position); //We need to sample at the position we predict we are going to be after this frame given the current velocity //For the front we need to lookahead at least the granularity of the height map adjusted for angle. For the sake of simplicity we just assume the worst case of 45 degrees, i.e. root(2) //If movement is vertical (or as good as) we want to look ahead a minimum distance var velo = input.currentFullVelocity.OnlyXZ(); if (velo.sqrMagnitude < 0.0001f && velo.y > 0f) { velo = input.currentPlanarVelocity; } var reqLookAhead = heightMap.granularity * Consts.SquareRootTwo; var lookAheadActual = velo * input.deltaTime; var lookAheadFixed = lookAheadActual.sqrMagnitude < reqLookAhead * reqLookAhead ? velo.normalized * reqLookAhead : lookAheadActual; var t = input.unit.transform; var center = _samplePoints[0] = t.TransformPoint(_points[0]) + lookAheadActual; _samplePoints[1] = t.TransformPoint(_points[1]) + lookAheadFixed; _samplePoints[2] = t.TransformPoint(_points[2]) + lookAheadActual; float maxClimb = unit.heightNavigationCapability.maxClimbHeight; float groundOffset = unit.groundOffset; float maxHeight = Consts.InfiniteDrop; int highIdx = 0; for (int i = 0; i < 3; i++) { float sampledHeight = heightMap.SampleHeight(_samplePoints[i]) + groundOffset; if (sampledHeight > maxHeight) { maxHeight = sampledHeight; highIdx = i; } _samplePoints[i].y = sampledHeight; } //When ascending there are situations where we need to continue the current rate of ascent even though the high point no longer dictates it. //This happens when moving from a slope onto a lesser slope or platform, we need to continue to ascend until the base is free otherwise the unit will collide with the terrain. _pendingHighMaxes.RegisterHighpoint(_samplePoints[1]); if (highIdx != 1 && _pendingHighMaxes.count > 0 && center.DirToXZ(_pendingHighMaxes.current).sqrMagnitude > _radius * _radius) { _pendingHighMaxes.MoveNext(); } var hp = _samplePoints[highIdx]; if (_pendingHighMaxes.count > 0 && (_pendingHighMaxes.currentHeight > maxHeight || (hp - center).sqrMagnitude > (_pendingHighMaxes.current - center).sqrMagnitude)) { hp = _pendingHighMaxes.current; } else if (highIdx == 0) { //If the center is the highest point and we have reached the pinnacle of the ascent, then the center is in reality the highest point return(maxHeight - (center.y - _radius)); } //From the base point (on the ground) find the slope vector and the center vector var bp = _samplePoints[0]; var slopeVector = (bp - hp).normalized; var centerVector = center - hp; //Find the closest point on the line and from that the closest point of the sphere periferi var dp = Vector3.Dot(centerVector, slopeVector); var closestPointLine = (dp > 0f) ? hp + (slopeVector * dp) : hp; var closestPoint = center + ((closestPointLine - center).normalized * _radius); //Find the plane normal for the plane represented by the slope line and an extra point (simply a point perpendicular to one of the other two at the same y) var pp3 = new Vector3(hp.z, hp.y, -hp.x); var pn = Vector3.Cross(pp3 - hp, hp - bp); //Get the closest y coordinate, that is the point where the sphere should rest, using the plane formula a(x-x0) + b(y-y0) + c(z-z0) = 0, where (a,b,c) is the normal vector and (x0 y0, z0) is a known point on the plane. //Since we know the x and z of the closest point we can find the proper y. var closestY = ((pn.x * (closestPoint.x - hp.x)) + (pn.z * (closestPoint.z - hp.z)) - (pn.y * hp.y)) / -pn.y; var delta = closestY - closestPoint.y; var slope = Vector3.Dot(Vector3.up, -slopeVector); var allowedSlope = Mathf.Cos((90f - unit.heightNavigationCapability.maxSlopeAngle) * Mathf.Deg2Rad); if (slope > allowedSlope && delta > maxClimb) { return(0f); } return(delta); }
/// <summary> /// Handles the situation where there is a missing vector from the vector field. /// </summary> /// <param name="selfCell">This unit's current cell.</param> /// <param name="vectorField">The vector field.</param> /// <param name="input">The steering input.</param> /// <param name="output">The steering output.</param> private void HandleMissingVectorFromField(Cell selfCell, IVectorField vectorField, SteeringInput input, SteeringOutput output) { var unit = input.unit; Vector3 unitPos = unit.position; // find all (up to) 8 neighbours _neighbours.Clear(); input.grid.GetConcentricNeighbours(selfCell, 1, _neighbours); // sort the neighbours depending on their distance to the unit, i.e. nearest cell neighbours first _cellComparer.compareTo = unitPos; _neighbours.Sort(_cellComparer); // loop through cell neighbours and try to escape to the nearest one that is walkable and has a vector field cell int neighboursCount = _neighbours.count; for (int i = 0; i < neighboursCount; i++) { var neighbour = _neighbours[i]; Vector3 neighbourPos = neighbour.position; if (neighbour.isWalkable(unit.attributes) && vectorField.GetFieldCellAtPos(neighbour).direction.sqrMagnitude != 0f && neighbourPos.y <= unitPos.y) { // if the neighbour cell is walkable, has a vector field vector and is at a lower or same height as the unit output.maxAllowedSpeed = unit.maximumSpeed; output.desiredAcceleration = Seek(neighbourPos, input); return; } } // only request path if we can't find a neighbouring cell to escape to, if there is a valid destination and if the unit is actually movable if (unit.isMovable && vectorField.destination != null) { RequestPath(vectorField.destination.position, input); } }
/// <summary> /// Convenience method for seeking towards a position while also calculating the required slowing distance /// </summary> /// <param name="to">The target seek position</param> /// <param name="input">The steering input.</param> /// <returns>A seek vector in the direction towards the target seek position 'to' from this unit.</returns> private Vector3 SeekWithVectorFieldVector(Vector3 to, SteeringInput input) { // not arriving yet, but calculate slowing distance if (this.autoCalculateSlowingDistance) { CalculateRequiredSlowingDistance(input); } // Seek in the direction of the vector field cell return Seek(to, input); }
public override void UpdateTargetHunt(Follower follower, bool fleeing) { SteeringInput followerInput = BuildInput(follower); SteeringOutput output = new SteeringOutput(); if (fleeing) { // C.1 if (Vector2.Distance(followerInput.position, followerInput.targetPosition) < SLOW_RADIUS) { // Debug.Log("C1"); output = PerformFlee(followerInput); output.angularAcceleration = PerformLookWhereYouGoing(followerInput); } else { // C.2 // Debug.Log("C2"); output = PerformEvade(followerInput); output.angularAcceleration = PerformFaceAway(followerInput); } } else if (followerInput.velocity.magnitude < SLOW_SPEED) { // A.1 if (Vector2.Distance(followerInput.position, followerInput.targetPosition) < SLOW_RADIUS) { // Debug.Log("A1"); output = PerformArrive(followerInput); } else { // A.2 // Debug.Log("A2"); Vector2 targetRotationVect = followerInput.targetPosition - followerInput.position; float targetOrientation = Mathf.Atan2(targetRotationVect.y, targetRotationVect.x); const float ANGLE_TOLERANCE = 10f; // if (AngleDifferenceNegligible(followerInput.orientation, targetOrientation, ANGLE_TOLERANCE)) { output = PerformArrive(followerInput); // } else { // output.velocity = Vector2.zero; // output.orientation = Mathf.LerpAngle(followerInput.orientation * Mathf.Rad2Deg, // targetOrientation * Mathf.Rad2Deg, Time.deltaTime * 5f); // output.orientation *= Mathf.Deg2Rad; // output.rotation = 0f; // } output.angularAcceleration = PerformFace(followerInput); } } else { Vector2 targetRotationVect = followerInput.targetPosition - followerInput.position; float targetOrientation = Mathf.Atan2(targetRotationVect.y, targetRotationVect.x); output = PerformArrive(followerInput); const float ANGLE_TOLERANCE = 10f; // B.1 if (AngleDifferenceNegligible(followerInput.orientation, targetOrientation, ANGLE_TOLERANCE)) { // Debug.Log("B1"); // output = PerformArrive(followerInput); output.angularAcceleration = PerformLookWhereYouGoing(followerInput); } else { // B.2 // Debug.Log("B2"); // output.velocity = Vector2.zero; // output.orientation = Mathf.LerpAngle(followerInput.orientation * Mathf.Rad2Deg, targetOrientation, // Time.deltaTime * 5f); // output.orientation *= Mathf.Deg2Rad; // output.rotation = 0f; output.angularAcceleration = PerformFace(followerInput); } } ApplyOutput(follower, output); }
public override void GetDesiredSteering(SteeringInput input, SteeringOutput output) { if (input.currentPlanarVelocity.sqrMagnitude < _minSpeedSquared) { return; } var otherUnits = _scanner.Units; float maxAdj = 0f; float adjSum = 0f; Vector3 moveVector = Vector3.zero; for (int i = 0; i < otherUnits.Length; i++) { var other = otherUnits[i]; if (other.Equals(null) || other.Equals(input.unit.collider)) { continue; } Vector3 evadePos; var otherVelo = other.GetUnitFacade(); if (otherVelo == null) { evadePos = other.transform.position; } else { var otherPos = otherVelo.position; var distToOther = (otherPos - input.unit.position).magnitude; var otherSpeed = otherVelo.velocity.magnitude; var predictionTime = 0.1f; if (otherSpeed > 0f) { //Half the prediction time for better behavior predictionTime = (distToOther / otherSpeed) / 2f; } evadePos = otherPos + (otherVelo.velocity * predictionTime); } var offset = input.unit.position - evadePos; var offsetMagnitude = offset.magnitude; //Only do avoidance if inside vision cone or very close to the unit if (offsetMagnitude > _omniAwareRadius && Vector3.Dot(input.unit.forward, offset / offsetMagnitude) > _fovReverseAngleCos) { continue; } //The adjustment normalizes the offset and adjusts its impact according to the offset length, i.e. the further the other unit is away the less it will impact the steering var adj = 1f / (offsetMagnitude * offsetMagnitude); adjSum += adj; if (adj > maxAdj) { maxAdj = adj; } moveVector += (offset * adj); } if (maxAdj > 0f) { //Lastly we average out the move vector based on adjustments moveVector = moveVector / (adjSum / maxAdj); output.desiredAcceleration = Seek(input.unit.position + moveVector, input); } }
public float GetHeightDelta(SteeringInput input) { var unit = input.unit; //We need to sample at the position we predict we are going to be after this frame given the current velocity //If movement is vertical (or as good as) we want to look ahead a minimum distance var lookAhead = input.currentFullVelocity.OnlyXZ() * input.deltaTime; if (lookAhead.sqrMagnitude < 0.0001f && input.currentFullVelocity.y > 0f) { lookAhead = input.currentPlanarVelocity.normalized * 0.01f; } //Get the sample points var t = input.unit.transform; var center = _samplePoints[0] = t.TransformPoint(_points[0]) + lookAhead; _samplePoints[1] = t.TransformPoint(_points[1]) + lookAhead; _samplePoints[2] = t.TransformPoint(_points[2]) + lookAhead; float maxClimb = unit.heightNavigationCapability.maxClimbHeight; float groundOffset = unit.groundOffset; //Do the height sampling var grid = input.grid; float rayStart = grid != null ? grid.cellMatrix.origin.y + grid.cellMatrix.upperBoundary : center.y + maxClimb; float maxHeight = Consts.InfiniteDrop; RaycastHit hit; int highIdx = 0; Vector3 highNormal = Vector3.zero; for (int i = 0; i < 3; i++) { var point = _samplePoints[i]; point.y = rayStart; if (Physics.Raycast(point, Vector3.down, out hit, Mathf.Infinity, Layers.terrain)) { var sampledHeight = hit.point.y + groundOffset; if (sampledHeight > maxHeight) { maxHeight = sampledHeight; highNormal = hit.normal; highIdx = i; } _samplePoints[i].y = sampledHeight; } } //When ascending there are situations where we need to continue the current rate of ascent even though the high point no longer dictates it. //This happens when moving from a slope onto a lesser slope or platform, we need to continue to ascend until the base is free otherwise the unit will collide with the terrain. _pendingHighMaxes.RegisterHighpoint(_samplePoints[1]); if (highIdx != 1 && _pendingHighMaxes.count > 0 && center.DirToXZ(_pendingHighMaxes.current).sqrMagnitude > _radius * _radius) { _pendingHighMaxes.MoveNext(); } var hp = _samplePoints[highIdx]; if (_pendingHighMaxes.count > 0 && (_pendingHighMaxes.currentHeight > maxHeight || (hp - center).sqrMagnitude > (_pendingHighMaxes.current - center).sqrMagnitude)) { hp = _pendingHighMaxes.current; } else if (highIdx == 0) { //If the center is the highest point and we have reached the pinnacle of the ascent, then the center is in reality the highest point return maxHeight - (center.y - _radius); } //From the base point (on the ground) find the slope vector and the center vector var bp = _samplePoints[0]; var slopeVector = (bp - hp).normalized; var centerVector = center - hp; //Find the closest point on the line and from that the closest point of the sphere periferi var dp = Vector3.Dot(centerVector, slopeVector); var closestPointLine = (dp > 0f) ? hp + (slopeVector * dp) : hp; var closestPoint = center + ((closestPointLine - center).normalized * _radius); //Find the plane normal for the plane represented by the slope line and an extra point (simply a point perpendicular to one of the other two at the same y) var pp3 = new Vector3(hp.z, hp.y, -hp.x); var pn = Vector3.Cross(pp3 - hp, hp - bp); //Get the closest y coordinate, that is the point where the sphere should rest, using the plane formula a(x-x0) + b(y-y0) + c(z-z0) = 0, where (a,b,c) is the normal vector and (x0 y0, z0) is a known point on the plane. //Since we know the x and z of the closest point we can find the proper y. var closestY = ((pn.x * (closestPoint.x - hp.x)) + (pn.z * (closestPoint.z - hp.z)) - (pn.y * hp.y)) / -pn.y; var delta = closestY - closestPoint.y; var slope = Vector3.Dot(Vector3.up, highNormal); var minSlope = Mathf.Cos(unit.heightNavigationCapability.maxSlopeAngle * Mathf.Deg2Rad); if (slope < minSlope && delta > maxClimb) { return 0f; } return delta; }
public float GetHeightDelta(SteeringInput input) { var unit = input.unit; //We need to sample at the position we predict we are going to be after this frame given the current velocity //If movement is vertical (or as good as) we want to look ahead a minimum distance var lookAhead = input.currentFullVelocity.OnlyXZ() * input.deltaTime; if (lookAhead.sqrMagnitude < 0.0001f && input.currentFullVelocity.y > 0f) { lookAhead = input.currentPlanarVelocity.normalized * 0.01f; } //Get the sample points var t = input.unit.transform; _samplePoints[0] = t.TransformPoint(_points[0]) + lookAhead; _samplePoints[1] = t.TransformPoint(_points[1]) + lookAhead; _samplePoints[2] = t.TransformPoint(_points[2]) + lookAhead; float baseY = _samplePoints[0].y; float maxClimb = unit.heightNavigationCapability.maxClimbHeight; float groundOffset = unit.groundOffset; //Do the height sampling var grid = input.grid; float rayStart = grid != null ? grid.cellMatrix.origin.y + grid.cellMatrix.upperBoundary : _samplePoints[0].y + maxClimb; float maxHeight = Consts.InfiniteDrop; RaycastHit hit; int highIdx = 0; Vector3 highNormal = Vector3.zero; for (int i = 0; i < 3; i++) { var point = _samplePoints[i]; point.y = rayStart; if (Physics.Raycast(point, Vector3.down, out hit, Mathf.Infinity, Layers.terrain)) { var sampledHeight = hit.point.y + groundOffset; if (sampledHeight > maxHeight) { maxHeight = sampledHeight; highNormal = hit.normal; highIdx = i; } _samplePoints[i].y = sampledHeight; } } //When ascending there are situations where we need to continue the current rate of ascent even though the high point no longer dictates it. //This happens when moving from a slope onto a lesser slope or platform, we need to continue to ascend until the base is free otherwise the unit will collide with the terrain. _pendingHighMaxes.RegisterHighpoint(_samplePoints[1]); if (highIdx != 1 && _pendingHighMaxes.count > 0 && Vector3.Dot(lookAhead, _pendingHighMaxes.current - _samplePoints[2]) < 0f) { _pendingHighMaxes.MoveNext(); } if (_pendingHighMaxes.count > 0 && _pendingHighMaxes.currentHeight > maxHeight) { maxHeight = _pendingHighMaxes.currentHeight; } var delta = maxHeight - baseY; var slope = Vector3.Dot(Vector3.up, highNormal); var minSlope = Mathf.Cos(unit.heightNavigationCapability.maxSlopeAngle * Mathf.Deg2Rad); if (slope < minSlope && delta > maxClimb) { return 0f; } return delta; }
/// <summary> /// Calculates the arrive acceleration vector for the specified destination. /// </summary> /// <param name="destination">The destination.</param> /// <param name="input">The input.</param> /// <returns>The acceleration vector</returns> protected Vector3 Arrive(Vector3 destination, SteeringInput input) { return Arrive(destination, input, input.desiredSpeed); }
/// <summary> /// Handles the situation where there is a missing vector from the vector field. /// </summary> /// <param name="selfCell">This unit's current cell.</param> /// <param name="vectorField">The vector field.</param> /// <param name="input">The steering input.</param> /// <param name="output">The steering output.</param> private void HandleMissingVectorFromField(Cell selfCell, IVectorField vectorField, SteeringInput input, SteeringOutput output) { var unit = input.unit; Vector3 unitPos = unit.position; // find all (up to) 8 neighbours _neighbours.Clear(); input.grid.GetConcentricNeighbours(selfCell, 1, _neighbours); // sort the neighbours depending on their distance to the unit, i.e. nearest cell neighbours first _cellComparer.compareTo = unitPos; _neighbours.Sort(_cellComparer); // loop through cell neighbours and try to escape to the nearest one that is walkable and has a vector field cell int neighboursCount = _neighbours.count; for (int i = 0; i < neighboursCount; i++) { var neighbour = _neighbours[i]; Vector3 neighbourPos = neighbour.position; if (neighbour.IsWalkableWithClearance(unit) && vectorField.GetFieldCellAtPos(neighbour).direction.sqrMagnitude != 0f && neighbourPos.y <= unitPos.y) { // if the neighbour cell is walkable, has a vector field vector and is at a lower or same height as the unit output.maxAllowedSpeed = unit.maximumSpeed; output.desiredAcceleration = Seek(neighbourPos, input); return; } } // only request path if we can't find a neighbouring cell to escape to, if there is a valid destination and if the unit is actually movable if (unit.isMovable && vectorField.destination != null) { RequestPath(vectorField.destination.position, input); } }
protected Vector3 Arrive(Vector3 destination, SteeringInput input) { return(Arrive(destination, input, input.desiredSpeed)); }
/// <summary> /// Gets the height delta, i.e. the difference in height between where the unit will be at the end of the frame and the height the unit should aim to be at.. /// </summary> /// <param name="input">The steering input</param> /// <returns> /// The height delta /// </returns> public float GetHeightDelta(SteeringInput input) { var unit = input.unit; //We need to sample at the position we predict we are going to be after this frame given the current velocity //If movement is vertical (or as good as) we want to look ahead a minimum distance var lookAhead = input.currentFullVelocity.OnlyXZ() * input.deltaTime; if (lookAhead.sqrMagnitude < 0.0001f && input.currentFullVelocity.y > 0f) { lookAhead = input.currentPlanarVelocity.normalized * 0.01f; } //Get the sample points var t = input.unit.transform; var center = _samplePoints[0] = t.TransformPoint(_points[0]) + lookAhead; _samplePoints[1] = t.TransformPoint(_points[1]) + lookAhead; _samplePoints[2] = t.TransformPoint(_points[2]) + lookAhead; float maxClimb = unit.heightNavigationCapability.maxClimbHeight; float groundOffset = unit.groundOffset; //Do the height sampling var grid = input.grid; float rayStart = grid != null ? grid.cellMatrix.origin.y + grid.cellMatrix.upperBoundary : center.y + maxClimb; float maxHeight = Consts.InfiniteDrop; RaycastHit hit; int highIdx = 0; Vector3 highNormal = Vector3.zero; for (int i = 0; i < 3; i++) { var point = _samplePoints[i]; point.y = rayStart; if (Physics.Raycast(point, Vector3.down, out hit, Mathf.Infinity, Layers.terrain)) { var sampledHeight = hit.point.y + groundOffset; if (sampledHeight > maxHeight) { maxHeight = sampledHeight; highNormal = hit.normal; highIdx = i; } _samplePoints[i].y = sampledHeight; } } //When ascending there are situations where we need to continue the current rate of ascent even though the high point no longer dictates it. //This happens when moving from a slope onto a lesser slope or platform, we need to continue to ascend until the base is free otherwise the unit will collide with the terrain. _pendingHighMaxes.RegisterHighpoint(_samplePoints[1]); if (highIdx != 1 && _pendingHighMaxes.count > 0 && center.DirToXZ(_pendingHighMaxes.current).sqrMagnitude > _radius * _radius) { _pendingHighMaxes.MoveNext(); } var hp = _samplePoints[highIdx]; if (_pendingHighMaxes.count > 0 && (_pendingHighMaxes.currentHeight > maxHeight || (hp - center).sqrMagnitude > (_pendingHighMaxes.current - center).sqrMagnitude)) { hp = _pendingHighMaxes.current; } else if (highIdx == 0) { //If the center is the highest point and we have reached the pinnacle of the ascent, then the center is in reality the highest point return(maxHeight - (center.y - _radius)); } //From the base point (on the ground) find the slope vector and the center vector var bp = _samplePoints[0]; var slopeVector = (bp - hp).normalized; var centerVector = center - hp; //Find the closest point on the line and from that the closest point of the sphere periferi var dp = Vector3.Dot(centerVector, slopeVector); var closestPointLine = (dp > 0f) ? hp + (slopeVector * dp) : hp; var closestPoint = center + ((closestPointLine - center).normalized * _radius); //Find the plane normal for the plane represented by the slope line and an extra point (simply a point perpendicular to one of the other two at the same y) var pp3 = new Vector3(hp.z, hp.y, -hp.x); var pn = Vector3.Cross(pp3 - hp, hp - bp); //Get the closest y coordinate, that is the point where the sphere should rest, using the plane formula a(x-x0) + b(y-y0) + c(z-z0) = 0, where (a,b,c) is the normal vector and (x0 y0, z0) is a known point on the plane. //Since we know the x and z of the closest point we can find the proper y. var closestY = ((pn.x * (closestPoint.x - hp.x)) + (pn.z * (closestPoint.z - hp.z)) - (pn.y * hp.y)) / -pn.y; var delta = closestY - closestPoint.y; var slope = Vector3.Dot(Vector3.up, highNormal); var minSlope = Mathf.Cos(unit.heightNavigationCapability.maxSlopeAngle * Mathf.Deg2Rad); if (slope < minSlope && delta > maxClimb) { return(0f); } return(delta); }
public float GetHeightDelta(SteeringInput input) { var unit = input.unit; var heightMap = HeightMapManager.instance.GetHeightMap(unit.position); //We need to sample at the position we predict we are going to be after this frame given the current velocity //If movement is vertical (or as good as) we want to look ahead a minimum distance var velo = input.currentFullVelocity.OnlyXZ(); if (velo.sqrMagnitude < 0.0001f && velo.y > 0f) { velo = input.currentPlanarVelocity; } var reqLookAhead = heightMap.granularity * Consts.SquareRootTwo; var lookAheadActual = velo * input.deltaTime; var lookAheadFixed = lookAheadActual.sqrMagnitude < reqLookAhead * reqLookAhead ? velo.normalized * reqLookAhead : lookAheadActual; var t = input.unit.transform; _samplePoints[0] = t.TransformPoint(_points[0]) + lookAheadActual; _samplePoints[1] = t.TransformPoint(_points[1]) + lookAheadFixed; _samplePoints[2] = t.TransformPoint(_points[2]) + lookAheadActual; float baseY = _samplePoints[0].y; float maxClimb = unit.heightNavigationCapability.maxClimbHeight; float groundOffset = unit.groundOffset; //Do the height sampling float maxHeight = Consts.InfiniteDrop; int highIdx = 0; for (int i = 0; i < 3; i++) { float sampledHeight = heightMap.SampleHeight(_samplePoints[i]) + groundOffset; if (sampledHeight > maxHeight) { maxHeight = sampledHeight; highIdx = i; } _samplePoints[i].y = sampledHeight; } //When ascending there are situations where we need to continue the current rate of ascent even though the high point no longer dictates it. //This happens when moving from a slope onto a lesser slope or platform, we need to continue to ascend until the base is free otherwise the unit will collide with the terrain. _pendingHighMaxes.RegisterHighpoint(_samplePoints[1]); if (highIdx != 1 && _pendingHighMaxes.count > 0 && Vector3.Dot(lookAheadActual, _pendingHighMaxes.current - _samplePoints[2]) < 0f) { _pendingHighMaxes.MoveNext(); } var hp = _samplePoints[highIdx]; if (_pendingHighMaxes.count > 0 && _pendingHighMaxes.currentHeight > maxHeight) { hp = _pendingHighMaxes.current; } var delta = hp.y - baseY; var slopeVector = (hp - _samplePoints[0]).normalized; var slope = Vector3.Dot(Vector3.up, slopeVector); var allowedSlope = Mathf.Cos((90f - unit.heightNavigationCapability.maxSlopeAngle) * Mathf.Deg2Rad); if (slope > allowedSlope && delta > maxClimb) { return 0f; } return delta; }
/// <summary> /// Gets the desired steering output. /// </summary> /// <param name="input">The steering input containing relevant information to use when calculating the steering output.</param> /// <param name="output">The steering output to be populated.</param> public override void GetDesiredSteering(SteeringInput input, SteeringOutput output) { if (_isPortalling) { // if portalling, pause the output and exit early output.pause = true; return; } if (_requestedPath) { // if waiting for a path out of a tight spot, then just wait for it to complete output.pause = true; return; } var unit = input.unit; var group = unit.transientGroup as DefaultSteeringTransientUnitGroup; if (group == null || group.count == 0) { // if the group type is not DefaultSteeringTransientUnitGroup (or a derived type) return return; } var grid = input.grid; if (grid == null) { // disable steer for vector field while off grid return; } var vectorField = group.vectorField; if (vectorField == null) { // if there is no vector field, exit return; } Path currentPath = vectorField.currentPath; if (currentPath == null || currentPath.count == 0) { // if the vector field has no path, exit return; } Vector3 selfPos = unit.position; var selfCell = grid.GetCell(selfPos); if (selfCell == null) { // if there is no cell beneath this unit, exit early return; } if (!object.ReferenceEquals(currentPath, _currentPath)) { // Vector field has gotten a new path _remainingSquaredDistance = currentPath.CalculateSquaredLength(); _endOfResolvedPath = currentPath.Last().position; _stopped = false; _lastPathPortalIndex = -1; _currentPath = currentPath; unit.hasArrivedAtDestination = false; if (_enabledSoloPath) { // we get a new group path while being on the solo, then stop solo pathing EndSoloPath(); } } if (_stopped || unit.hasArrivedAtDestination) { // if this unit has not stopped, it should stop return; } // get the vector field cell - note that the position passed must be a cell center position var vectorFieldCell = vectorField.GetFieldCellAtPos(selfCell); Vector3 vectorFieldVector = vectorFieldCell.direction; if (vectorFieldVector.sqrMagnitude == 0f) { // if the cell this unit is standing on has not been visited if (!_enabledSoloPath) { // if no vector field vector for the current cell exists - and we are not currently on a solo path - handle it HandleMissingVectorFromField(selfCell, vectorField, input, output); } else { // if we are on a solo path, and the vector field vector is still missing, then speed up to max speed output.maxAllowedSpeed = unit.maximumSpeed; } // don't proceed if missing a vector field cell return; } int pathPortalIndex = vectorFieldCell.pathPortalIndex; if (pathPortalIndex != -1 && pathPortalIndex != _lastPathPortalIndex) { // if we are on a portal cell that is different from the last portal _lastPathPortalIndex = pathPortalIndex; HandlePortal(unit, group, vectorField, pathPortalIndex, input.grid); return; } Vector3 nextNodePosition = vectorField.nextNodePosition.position; if (_nextPoint != nextNodePosition) { // Vector field has switched to the next point in the path _remainingSquaredDistance -= (_nextPoint - nextNodePosition).sqrMagnitude; _stopped = false; _nextPoint = nextNodePosition; } if (_enabledSoloPath) { // there is a valid vector field with valid vectors - so stop solo pathing EndSoloPath(); } var modelUnit = group.modelUnit; Vector3 steeringVector = Vector3.zero; Vector3 unitVectorFieldVector = selfPos + vectorFieldVector; if (object.ReferenceEquals(unit, modelUnit)) { // run arrival for model unit float slowingDistanceSqr = this.slowingDistance * this.slowingDistance; Vector3 dirEnd = selfPos.DirToXZ(_endOfResolvedPath); if ((dirEnd.sqrMagnitude < slowingDistanceSqr) && (_remainingSquaredDistance < slowingDistanceSqr || vectorField.isOnFinalApproach)) { // arrive normally (I am the first in my group) steeringVector = Arrive(nextNodePosition, input); if (this.hasArrived) { // inform the group that it has arrived and stop group.hasArrived = true; _stopped = true; unit.hasArrivedAtDestination = true; } } else { steeringVector = SeekWithVectorFieldVector(unitVectorFieldVector, input); } } else { // evaluate whether we have arrived at/near the final destination float rad = unit.radius + this.arrivalDistance + arrivalRadiusMargin; var finalFormPos = group.GetFinalFormationPosition(unit.formationIndex); Vector3 arrivalPos = finalFormPos != null && unit.formationPos != null ? finalFormPos.position : vectorField.destination.position; if (arrivalPos.DirToXZ(selfPos).sqrMagnitude < (rad * rad)) { unit.hasArrivedAtDestination = true; return; } // if not model unit, just seek in direction of the vector field vector steeringVector = SeekWithVectorFieldVector(unitVectorFieldVector, input); } if (steeringVector.sqrMagnitude == 0f) { return; } output.desiredAcceleration = steeringVector; }
/// <summary> /// Gets the desired steering output. /// </summary> /// <param name="input">The steering input containing relevant information to use when calculating the steering output.</param> /// <param name="output">The steering output to be populated.</param> public override void GetDesiredSteering(SteeringInput input, SteeringOutput output) { _lastAvoidVector = Vector3.zero; var grid = input.grid; if (grid == null) { // if no grid is available, return return; } _unitData = input.unit; if (!_unitData.isGrounded) { // in the air, give up return; } Vector3 selfPos = _unitData.position; var selfCell = grid.GetCell(selfPos); if (selfCell == null || !selfCell.isWalkable(_unitData.attributes)) { // already within a blocked or missing cell, give up return; } // Find and avoid 4 neighbour cells - if they are blocked _neighbours.Clear(); _neighbours.Add(selfCell.GetNeighbour(-1, 0)); _neighbours.Add(selfCell.GetNeighbour(0, -1)); _neighbours.Add(selfCell.GetNeighbour(1, 0)); _neighbours.Add(selfCell.GetNeighbour(0, 1)); // the total radius is the unit's radius + the radius margin + half of the current grid's cell size float selfRadius = _unitData.radius + radiusMargin + (grid.cellSize / 2f); float selfRadiusSqr = selfRadius * selfRadius; Vector3 avoidVector = Vector3.zero; int neighboursCount = _neighbours.count; for (int i = 0; i < neighboursCount; i++) { var neighbourCell = _neighbours[i]; if (neighbourCell == null) { // ignore missing cells, this is not containment continue; } var dir = neighbourCell.position.DirToXZ(selfPos); float distanceSqr = dir.sqrMagnitude; if (distanceSqr < selfRadiusSqr && !neighbourCell.isWalkableFrom(selfCell, _unitData)) { // sum up a vector comprising of all avoid vectors avoidVector += dir; } } if (avoidVector.sqrMagnitude == 0f) { return; } // set the repulsion vector's magnitude to repulsionStrength Vector3 steeringVector = avoidVector.normalized * repulsionStrength; _lastAvoidVector = steeringVector; output.desiredAcceleration = steeringVector; }
/// <summary> /// Gets the height delta, i.e. the difference in height between where the unit will be at the end of the frame and the height the unit should aim to be at.. /// </summary> /// <param name="input">The steering input</param> /// <returns> /// The height delta /// </returns> public float GetHeightDelta(SteeringInput input) { var unit = input.unit; //We need to sample at the position we predict we are going to be after this frame given the current velocity //If movement is vertical (or as good as) we want to look ahead a minimum distance var lookAhead = input.currentFullVelocity.OnlyXZ() * input.deltaTime; if (lookAhead.sqrMagnitude < 0.0001f && input.currentFullVelocity.y > 0f) { lookAhead = input.currentPlanarVelocity.normalized * 0.01f; } //Get the sample points var t = input.unit.transform; _samplePoints[0] = t.TransformPoint(_points[0]) + lookAhead; _samplePoints[1] = t.TransformPoint(_points[1]) + lookAhead; _samplePoints[2] = t.TransformPoint(_points[2]) + lookAhead; _samplePoints[3] = t.TransformPoint(_points[3]) + lookAhead; _samplePoints[4] = t.TransformPoint(_points[4]) + lookAhead; float baseY = _samplePoints[0].y; float maxClimb = unit.heightNavigationCapability.maxClimbHeight; float groundOffset = unit.groundOffset; //Do the height sampling var grid = input.grid; float rayStart = grid != null ? grid.cellMatrix.origin.y + grid.cellMatrix.upperBoundary : _samplePoints[0].y + maxClimb; float maxHeight = Consts.InfiniteDrop; RaycastHit hit; int highIdx = 0; Vector3 highNormal = Vector3.zero; for (int i = 0; i < 5; i++) { var point = _samplePoints[i]; point.y = rayStart; if (Physics.Raycast(point, Vector3.down, out hit, Mathf.Infinity, Layers.terrain)) { var sampledHeight = hit.point.y + groundOffset; if (sampledHeight > maxHeight) { maxHeight = sampledHeight; highNormal = hit.normal; highIdx = i; } _samplePoints[i].y = sampledHeight; } } //When ascending there are situations where we need to continue the current rate of ascent even though the high point no longer dictates it. //This happens when moving from a slope onto a lesser slope or platform, we need to continue to ascend until the base is free otherwise the unit will collide with the terrain. var fhp = _samplePoints[3].y > _samplePoints[4].y ? _samplePoints[3] : _samplePoints[4]; _pendingHighMaxes.RegisterHighpoint(fhp); if (highIdx < 3 && _pendingHighMaxes.count > 0 && _samplePoints[0].DirToXZ(_pendingHighMaxes.current).sqrMagnitude > _samplePoints[0].DirToXZ(_samplePoints[1]).sqrMagnitude) { _pendingHighMaxes.MoveNext(); } if (_pendingHighMaxes.count > 0 && _pendingHighMaxes.currentHeight > maxHeight) { maxHeight = _pendingHighMaxes.currentHeight; } var delta = maxHeight - baseY; var slope = Vector3.Dot(Vector3.up, highNormal); var minSlope = Mathf.Cos(unit.heightNavigationCapability.maxSlopeAngle * Mathf.Deg2Rad); if (slope < minSlope && delta > maxClimb) { return(0f); } return(delta); }
/// <summary> /// Gets the desired steering output. /// </summary> /// <param name="input">The steering input containing relevant information to use when calculating the steering output.</param> /// <param name="output">The steering output to be populated.</param> public override void GetDesiredSteering(SteeringInput input, SteeringOutput output) { if (_isPortalling) { // if portalling, pause the output and exit early output.pause = true; return; } if (_requestedPath) { // if waiting for a path out of a tight spot, then just wait for it to complete output.pause = true; return; } var unit = input.unit; var group = unit.transientGroup as DefaultSteeringTransientUnitGroup; if (group == null || group.count == 0) { // if the group type is not DefaultSteeringTransientUnitGroup (or a derived type) return return; } var grid = input.grid; if (grid == null) { // disable steer for vector field while off grid return; } var vectorField = group.vectorField; if (vectorField == null) { // if there is no vector field, exit return; } Path currentPath = vectorField.currentPath; if (currentPath == null || currentPath.count == 0) { // if the vector field has no path, exit return; } Vector3 selfPos = unit.position; var selfCell = grid.GetCell(selfPos); if (selfCell == null) { // if there is no cell beneath this unit, exit early return; } if (!object.ReferenceEquals(currentPath, _currentPath)) { // Vector field has gotten a new path _remainingSquaredDistance = currentPath.CalculateSquaredLength(); _endOfResolvedPath = currentPath.Last().position; _stopped = false; _lastPathPortalIndex = -1; _currentPath = currentPath; unit.hasArrivedAtDestination = false; if (_enabledSoloPath) { // we get a new group path while being on the solo, then stop solo pathing EndSoloPath(); } } if (_stopped || unit.hasArrivedAtDestination) { // if this unit has not stopped, it should stop return; } // get the vector field cell - note that the position passed must be a cell center position var vectorFieldCell = vectorField.GetFieldCellAtPos(selfCell); Vector3 vectorFieldVector = vectorFieldCell.direction; if (vectorFieldVector.sqrMagnitude == 0f) { // if the cell this unit is standing on has not been visited if (!_enabledSoloPath) { // if no vector field vector for the current cell exists - and we are not currently on a solo path - handle it HandleMissingVectorFromField(selfCell, vectorField, input, output); } else { // if we are on a solo path, and the vector field vector is still missing, then speed up to max speed output.maxAllowedSpeed = unit.maximumSpeed; } // don't proceed if missing a vector field cell return; } int pathPortalIndex = vectorFieldCell.pathPortalIndex; if (pathPortalIndex != -1 && pathPortalIndex != _lastPathPortalIndex) { // if we are on a portal cell that is different from the last portal _lastPathPortalIndex = pathPortalIndex; HandlePortal(unit, group, vectorField, pathPortalIndex, input.grid); return; } Vector3 nextNodePosition = vectorField.nextNodePosition.position; if (_nextPoint != nextNodePosition) { // Vector field has switched to the next point in the path _remainingSquaredDistance -= (_nextPoint - nextNodePosition).sqrMagnitude; _stopped = false; _nextPoint = nextNodePosition; } if (_enabledSoloPath) { // there is a valid vector field with valid vectors - so stop solo pathing EndSoloPath(); } var modelUnit = group.modelUnit; Vector3 steeringVector = Vector3.zero; Vector3 unitVectorFieldVector = selfPos + vectorFieldVector; if (object.ReferenceEquals(unit, modelUnit)) { // run arrival for model unit float slowingDistanceSqr = this.slowingDistance * this.slowingDistance; Vector3 dirEnd = selfPos.DirToXZ(_endOfResolvedPath); if ((dirEnd.sqrMagnitude < slowingDistanceSqr) && (_remainingSquaredDistance < slowingDistanceSqr || vectorField.isOnFinalApproach)) { // arrive normally (I am the first in my group) steeringVector = Arrive(nextNodePosition, input); if (this.hasArrived) { // inform the group that it has arrived and stop group.hasArrived = true; _stopped = true; unit.hasArrivedAtDestination = true; } } else { steeringVector = SeekWithVectorFieldVector(unitVectorFieldVector, input); } } else { // evaluate whether we have arrived at/near the final destination float rad = unit.radius + this.arrivalDistance + arrivalRadiusMargin; var finalFormPos = group.GetFinalFormationPosition(unit.formationIndex); Vector3 arrivalPos = finalFormPos != null && unit.formationPos != null ? finalFormPos.position : vectorField.destination.position; if ((arrivalPos - selfPos).sqrMagnitude < (rad * rad)) { unit.hasArrivedAtDestination = true; return; } // if not model unit, just seek in direction of the vector field vector steeringVector = SeekWithVectorFieldVector(unitVectorFieldVector, input); } if (steeringVector.sqrMagnitude == 0f) { return; } output.desiredAcceleration = steeringVector; }
public override void GetDesiredSteering(SteeringInput input, SteeringOutput output) { if (_isPortaling) { output.pause = true; return; } if (_stopped) { return; } if (_stop) { StopInternal(); return; } if (_currentPath == null && !ResolveNextPoint()) { return; } //Get the remaining distance _currentDestinationDistanceSquared = _transform.position.DirToXZ(_currentDestination.position).sqrMagnitude; HandleWaypointsAndArrival(); if (_currentDestinationDistanceSquared < _pathSettings.nextNodeDistance * _pathSettings.nextNodeDistance) { if (!ResolveNextPoint()) { return; } } else { HandlePathReplan(); } if (_onFinalApproach) { output.desiredAcceleration = Arrive(_currentDestination.position, input); if (this.hasArrived) { _unit.hasArrivedAtDestination = true; AnnounceEvent(UnitNavigationEventMessage.Event.DestinationReached, _transform.position, null); StopInternal(); } } else { //Calculate slowing distance since we don't start arrival until this it known and within range. if (this.autoCalculateSlowingDistance) { CalculateRequiredSlowingDistance(input); } output.desiredAcceleration = Seek(_currentDestination.position, input); } }
/// <summary> /// Gets the height output. /// </summary> /// <param name="input">The steering input.</param> /// <param name="effectiveMaxSpeed">The effective maximum speed of the unit, this may be higher than the desired speed.</param> /// <returns> /// The height output /// </returns> public HeightOutput GetHeightOutput(SteeringInput input, float effectiveMaxSpeed) { var finalVelocity = input.currentSpatialVelocity; var heightDiff = _heightProvider.GetHeightDelta(input); var deltaTime = input.deltaTime; var freefallThreshold = -effectiveMaxSpeed * deltaTime * this.groundStickynessFactor; var isGrounded = (heightDiff >= freefallThreshold); //Apply the needed vertical force to handle gravity or ascent/descent if (!isGrounded) { if (_gravitationVelocity > -this.terminalVelocity) { _gravitationVelocity += _gravity * deltaTime; } if (_gravitationVelocity * deltaTime < heightDiff) { _gravitationVelocity = heightDiff / deltaTime; } finalVelocity.y += _gravitationVelocity; } else { _gravitationVelocity = 0f; if (Mathf.Abs(heightDiff) > 1E-6f) { finalVelocity.y += heightDiff / deltaTime; finalVelocity = Vector3.ClampMagnitude(finalVelocity, effectiveMaxSpeed); } } return new HeightOutput { finalVelocity = finalVelocity, isGrounded = isGrounded }; }
public float GetHeightDelta(SteeringInput input) { var unit = input.unit; var heightMap = HeightMapManager.instance.GetHeightMap(unit.position); //We need to sample at the position we predict we are going to be after this frame given the current velocity //If movement is vertical (or as good as) we want to look ahead a minimum distance var velo = input.currentFullVelocity.OnlyXZ(); if (velo.sqrMagnitude < 0.0001f && velo.y > 0f) { velo = input.currentPlanarVelocity; } var reqLookAhead = heightMap.granularity * Consts.SquareRootTwo; var lookAheadActual = velo * input.deltaTime; var lookAheadFixed = lookAheadActual.sqrMagnitude < reqLookAhead * reqLookAhead ? velo.normalized * reqLookAhead : lookAheadActual; var t = input.unit.transform; _samplePoints[0] = t.TransformPoint(_points[0]) + lookAheadActual; _samplePoints[1] = t.TransformPoint(_points[1]) + lookAheadFixed; _samplePoints[2] = t.TransformPoint(_points[2]) + lookAheadActual; float baseY = _samplePoints[0].y; float maxClimb = unit.heightNavigationCapability.maxClimbHeight; float groundOffset = unit.groundOffset; //Do the height sampling float maxHeight = Consts.InfiniteDrop; int highIdx = 0; for (int i = 0; i < 3; i++) { float sampledHeight = heightMap.SampleHeight(_samplePoints[i]) + groundOffset; if (sampledHeight > maxHeight) { maxHeight = sampledHeight; highIdx = i; } _samplePoints[i].y = sampledHeight; } //When ascending there are situations where we need to continue the current rate of ascent even though the high point no longer dictates it. //This happens when moving from a slope onto a lesser slope or platform, we need to continue to ascend until the base is free otherwise the unit will collide with the terrain. _pendingHighMaxes.RegisterHighpoint(_samplePoints[1]); if (highIdx != 1 && _pendingHighMaxes.count > 0 && Vector3.Dot(lookAheadActual, _pendingHighMaxes.current - _samplePoints[2]) < 0f) { _pendingHighMaxes.MoveNext(); } var hp = _samplePoints[highIdx]; if (_pendingHighMaxes.count > 0 && _pendingHighMaxes.currentHeight > maxHeight) { hp = _pendingHighMaxes.current; } var delta = hp.y - baseY; var slopeVector = (hp - _samplePoints[0]).normalized; var slope = Vector3.Dot(Vector3.up, slopeVector); var allowedSlope = Mathf.Cos((90f - unit.heightNavigationCapability.maxSlopeAngle) * Mathf.Deg2Rad); if (slope > allowedSlope && delta > maxClimb) { return(0f); } return(delta); }
/// <summary> /// Gets the desired steering output. /// </summary> /// <param name="input">The steering input containing relevant information to use when calculating the steering output.</param> /// <param name="output">The steering output to be populated.</param> public override void GetDesiredSteering(SteeringInput input, SteeringOutput output) { _selfCollisionPos = Vector3.zero; _lastAvoidVector = Vector3.zero; _lastAvoidPos = Vector3.zero; var otherUnits = _scanner.units; int othersCount = otherUnits.count; if (othersCount == 0) { // if the scanner has found no units to avoid, exit return; } _unitData = input.unit; Vector3 avoidVector = Avoid(otherUnits, othersCount, input.currentPlanarVelocity); if (avoidVector.sqrMagnitude < (this.minimumAvoidVectorMagnitude * this.minimumAvoidVectorMagnitude)) { // if the computed avoid vector's magnitude is less than the minimumAvoidVectorMagnitude, then discard it return; } // apply the avoidance force as a full deceleration capped force (not over time) Vector3 steeringVector = Vector3.ClampMagnitude(avoidVector / Time.fixedDeltaTime, input.maxDeceleration); _lastAvoidVector = steeringVector; output.desiredAcceleration = steeringVector; }
/// <summary> /// Gets the desired steering output. /// </summary> /// <param name="input">The steering input containing relevant information to use when calculating the steering output.</param> /// <param name="output">The steering output to be populated.</param> public override void GetDesiredSteering(SteeringInput input, SteeringOutput output) { _lastAvoidVector = Vector3.zero; var grid = input.grid; if (grid == null) { // if no grid is available, return return; } _unitData = input.unit; if (!_unitData.isGrounded) { // in the air, give up return; } Vector3 selfPos = _unitData.position; var selfCell = grid.GetCell(selfPos); if (selfCell == null || !selfCell.isWalkable(_unitData.attributes)) { // already within a blocked or missing cell, give up return; } // Find and avoid 4 neighbour cells - if they are blocked _neighbours.Clear(); _neighbours.Add(selfCell.GetNeighbour(-1, 0)); _neighbours.Add(selfCell.GetNeighbour(0, -1)); _neighbours.Add(selfCell.GetNeighbour(1, 0)); _neighbours.Add(selfCell.GetNeighbour(0, 1)); // the total radius is the unit's radius + the radius margin + half of the current grid's cell size float selfRadius = _unitData.radius + radiusMargin + (grid.cellSize / 2f); float selfRadiusSqr = selfRadius * selfRadius; Vector3 avoidVector = Vector3.zero; int neighboursCount = _neighbours.count; for (int i = 0; i < neighboursCount; i++) { var neighbourCell = _neighbours[i]; if (neighbourCell == null) { // ignore missing cells, this is not containment continue; } var dir = neighbourCell.position.DirToXZ(selfPos); float distanceSqr = dir.sqrMagnitude; if (distanceSqr < selfRadiusSqr && !neighbourCell.isWalkableFrom(selfCell, _unitData)) { // sum up a vector comprising of all avoid vectors avoidVector += dir; } } if (avoidVector.sqrMagnitude == 0f) { return; } // set the repulsion vector's magnitude to repulsionStrength Vector3 steeringVector = avoidVector.normalized * repulsionStrength; _lastAvoidVector = steeringVector; output.desiredAcceleration = steeringVector; }
/// <summary> /// Calculates the arrive acceleration vector for the specified destination. /// </summary> /// <param name="destination">The destination.</param> /// <param name="remainingDistance">The remaining distance.</param> /// <param name="input">The input.</param> /// <param name="output">The output.</param> /// <returns>The acceleration vector</returns> protected bool Arrive(Vector3 destination, float remainingDistance, SteeringInput input, SteeringOutput output) { var arriveDistance = remainingDistance - this.arrivalDistance; this.hasArrived = arriveDistance <= 0.01f; if (this.hasArrived) { return true; } var currentVelocity = input.currentPlanarVelocity; this.slowingDistance = currentVelocity.sqrMagnitude / (2 * input.maxDeceleration); //We want to start stopping when the deceleration capabilities dictate it, so applying v² = u² + 2a * d //Since v is 0 (target velocity) and a is negative (deceleration) the acceptable current velocity u can be expressed as u = sqrt(2a * d). var maxSpeed = Mathf.Sqrt(2 * input.maxDeceleration * arriveDistance); var desiredSpeed = Mathf.Min(maxSpeed, input.desiredSpeed); var dir = input.unit.position.DirToXZ(destination); var desiredVelocity = dir.normalized * desiredSpeed; //While in theory we want to clamp to max deceleration when decelerating, in practice we just want to decelerate by whatever is needed since the deceleration capabilities are already considered above. //So this ensures that floating point inaccuracies do not cause imprecise stopping. if (desiredSpeed < input.desiredSpeed) { output.desiredAcceleration = (desiredVelocity - input.currentPlanarVelocity) / input.deltaTime; } else { output.desiredAcceleration = Vector3.ClampMagnitude((desiredVelocity - input.currentPlanarVelocity) / input.deltaTime, input.maxAcceleration); } return false; }
/// <summary> /// Gets the desired steering output. /// </summary> /// <param name="input">The steering input containing relevant information to use when calculating the steering output.</param> /// <param name="output">The steering output to be populated.</param> public override void GetDesiredSteering(SteeringInput input, SteeringOutput output) { _lastContainVector = Vector3.zero; if (input.grid == null) { // if off-grid, exit early return; } _unitData = input.unit; if (!_unitData.isGrounded) { // while in the air, exit early return; } // prepare local variables Vector3 selfPos = _unitData.position; Vector3 containVector = Vector3.zero; float selfHeight = _unitData.basePosition.y; // generate positions to the left and right Vector3 rightPos = selfPos + (Vector3.right * bufferDistance); Vector3 leftPos = selfPos + (Vector3.left * bufferDistance); float rightHeight, leftHeight; // sample left and right bool rightContain = _heightSampler.TrySampleHeight(rightPos, out rightHeight); bool leftContain = _heightSampler.TrySampleHeight(leftPos, out leftHeight); // if cell is missing or drop is more than allowed drop height or climb is more than allowed climb height, then compute an axis-aligned containment vector if (!rightContain || (selfHeight - rightHeight > _heightCaps.maxDropHeight) || (rightHeight - selfHeight > _heightCaps.maxClimbHeight)) { containVector = Vector3.left; } else if (!leftContain || (selfHeight - leftHeight > _heightCaps.maxDropHeight) || (leftHeight - selfHeight > _heightCaps.maxClimbHeight)) { containVector = Vector3.right; } // generate positions forward and backwards Vector3 forwardPos = selfPos + (Vector3.forward * bufferDistance); Vector3 backwardPos = selfPos + (Vector3.back * bufferDistance); float forwardHeight, backHeight; // sample forward and backwards bool forwardContain = _heightSampler.TrySampleHeight(forwardPos, out forwardHeight); bool backContain = _heightSampler.TrySampleHeight(backwardPos, out backHeight); // if cell is missing or drop is more than allowed drop height or climb is more than allowed climb height, then compute an axis-aligned containment vector if (!forwardContain || (selfHeight - forwardHeight > _heightCaps.maxDropHeight) || (forwardHeight - selfHeight > _heightCaps.maxClimbHeight)) { // we need to check whether containVector has a value beforehand, in which case we need to normalize containVector = containVector.sqrMagnitude != 0f ? (containVector + Vector3.back).normalized : Vector3.back; } else if (!backContain || (selfHeight - backHeight > _heightCaps.maxDropHeight) || (backHeight - selfHeight > _heightCaps.maxClimbHeight)) { // we need to check whether containVector has a value beforehand, in which case we need to normalize containVector = containVector.sqrMagnitude != 0f ? (containVector + Vector3.forward).normalized : Vector3.forward; } if (containVector.sqrMagnitude == 0f) { // no contain vectors to worry about - no containment necessary return; } // Containment vectors are always "full strength" Vector3 steeringVector = containVector * input.maxAcceleration; _lastContainVector = steeringVector; output.desiredAcceleration = steeringVector; }
/// <summary> /// Calculates the required slowing distance. /// </summary> /// <param name="input">The input.</param> protected void CalculateRequiredSlowingDistance(SteeringInput input) { float currentSpeed = input.currentPlanarVelocity.magnitude; if (this.slowingAlgorithm == SlowingAlgorithm.Logarithmic) { this.slowingDistance = Mathf.Max(this.slowingDistance, Mathf.Ceil(((currentSpeed * currentSpeed) / (2 * input.maxDeceleration)) * 1.1f) + 1f); } else { this.slowingDistance = Mathf.Max(this.slowingDistance, (currentSpeed * currentSpeed) / (2 * input.maxDeceleration)); } }
/// <summary> /// Requests a path. /// </summary> /// <param name="destination">The destination.</param> /// <param name="input">The steering input.</param> private void RequestPath(Vector3 destination, SteeringInput input) { var unit = input.unit; // Request a path normally from the unit's current position Action<PathResult> callback = (result) => { if (result.status == PathingStatus.Complete) { FollowPath(result.path, unit); return; } PathRequestCallback(result, unit, unit.position, destination, input.grid); }; QueuePathRequest(unit.position, destination, unit, callback); }
/// <summary> /// Gets the desired steering output. /// </summary> /// <param name="input">The steering input containing relevant information to use when calculating the steering output.</param> /// <param name="output">The steering output to be populated.</param> public override void GetDesiredSteering(SteeringInput input, SteeringOutput output) { _lastContainVector = Vector3.zero; if (input.grid == null) { // if off-grid, exit early return; } _unitData = input.unit; if (!_unitData.isGrounded) { // while in the air, exit early return; } // prepare local variables Vector3 selfPos = _unitData.position; Vector3 containVector = Vector3.zero; float selfHeight = _unitData.basePosition.y; // generate positions to the left and right Vector3 rightPos = selfPos + (Vector3.right * bufferDistance); Vector3 leftPos = selfPos + (Vector3.left * bufferDistance); float rightHeight, leftHeight; // sample left and right bool rightContain = _heightSampler.TrySampleHeight(rightPos, out rightHeight); bool leftContain = _heightSampler.TrySampleHeight(leftPos, out leftHeight); // if cell is missing or drop is more than allowed drop height or climb is more than allowed climb height, then compute an axis-aligned containment vector if (!rightContain || (selfHeight - rightHeight > _heightCaps.maxDropHeight) || (rightHeight - selfHeight > _heightCaps.maxClimbHeight)) { containVector = Vector3.left; } else if (!leftContain || (selfHeight - leftHeight > _heightCaps.maxDropHeight) || (leftHeight - selfHeight > _heightCaps.maxClimbHeight)) { containVector = Vector3.right; } // generate positions forward and backwards Vector3 forwardPos = selfPos + (Vector3.forward * bufferDistance); Vector3 backwardPos = selfPos + (Vector3.back * bufferDistance); float forwardHeight, backHeight; // sample forward and backwards bool forwardContain = _heightSampler.TrySampleHeight(forwardPos, out forwardHeight); bool backContain = _heightSampler.TrySampleHeight(backwardPos, out backHeight); // if cell is missing or drop is more than allowed drop height or climb is more than allowed climb height, then compute an axis-aligned containment vector if (!forwardContain || (selfHeight - forwardHeight > _heightCaps.maxDropHeight) || (forwardHeight - selfHeight > _heightCaps.maxClimbHeight)) { // we need to check whether containVector has a value beforehand, in which case we need to normalize containVector = containVector.sqrMagnitude != 0f ? (containVector + Vector3.back).normalized : Vector3.back; } else if (!backContain || (selfHeight - backHeight > _heightCaps.maxDropHeight) || (backHeight - selfHeight > _heightCaps.maxClimbHeight)) { // we need to check whether containVector has a value beforehand, in which case we need to normalize containVector = containVector.sqrMagnitude != 0f ? (containVector + Vector3.forward).normalized : Vector3.forward; } if (containVector.sqrMagnitude == 0f) { // no contain vectors to worry about - no containment necessary return; } // Containment vectors are always "full strength" Vector3 steeringVector = containVector * input.maxAcceleration; _lastContainVector = steeringVector; output.desiredAcceleration = steeringVector; }