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