private void SetPath(Path path, bool isWaypoint)
{
_endOfResolvedPath = path.Last().position;
_currentGrid = GridManager.instance.GetGrid(path.Peek().position);
if (isWaypoint)
{
_remainingPathDistance += path.CalculateLength();
_currentPath = _currentPath.AppendSegment(path);
}
else
{
if (!TrimPath(path))
{
RequestPath(_transform.position, _wayPoints.desiredEndOfPath, InternalPathRequest.Type.Normal);
_currentPath = null;
return;
}
_currentPath = path;
_remainingPathDistance = path.CalculateLength();
//Pop the first node as our next destination.
_currentDestination = path.Pop();
_curPlannedDirection = _transform.position.DirToXZ(_currentDestination.position);
}
_unit.hasArrivedAtDestination = false;
}
/// <summary>
/// Gets the move cost.
/// </summary>
/// <param name="current">The current node.</param>
/// <param name="other">The other node.</param>
/// <returns>
/// The move cost
/// </returns>
public override int GetMoveCost(IPositioned current, IPositioned other)
{
var dx = (current.position.x - other.position.x);
var dz = (current.position.z - other.position.z);
var dy = (current.position.y - other.position.y);
return(Mathf.RoundToInt(this.baseMoveCost * Mathf.Sqrt((dx * dx) + (dz * dz) + (dy * dy))));
}
/// <summary>
/// Gets the heuristic.
/// </summary>
/// <param name="current">The current node.</param>
/// <param name="goal">The goal node.</param>
/// <returns>
/// The heuristic
/// </returns>
public override int GetHeuristic(IPositioned current, IPositioned goal)
{
var dx = (current.position.x - goal.position.x);
var dz = (current.position.z - goal.position.z);
var dy = (current.position.y - goal.position.y);
return Mathf.RoundToInt(this.baseMoveCost * Mathf.Sqrt((dx * dx) + (dz * dz) + (dy * dy)));
}
/// <summary>
/// Gets the move cost.
/// </summary>
/// <param name="current">The current node.</param>
/// <param name="other">The other node.</param>
/// <returns>
/// The move cost
/// </returns>
public override int GetMoveCost(IPositioned current, IPositioned other)
{
var dx = (current.position.x - other.position.x);
var dz = (current.position.z - other.position.z);
var dy = (current.position.y - other.position.y);
return Mathf.RoundToInt(this.baseMoveCost * Mathf.Sqrt((dx * dx) + (dz * dz) + (dy * dy)));
}
/// <summary>
/// Gets the move cost.
/// </summary>
/// <param name="current">The current node.</param>
/// <param name="other">The other node.</param>
/// <returns>
/// The move cost
/// </returns>
public virtual int GetMoveCost(IPositioned current, IPositioned other)
{
var dx = Math.Abs(current.position.x - other.position.x);
var dz = Math.Abs(current.position.z - other.position.z);
var dy = Math.Abs(current.position.y - other.position.y);
return(Mathf.RoundToInt(_cellMoveCost * (Math.Max(dx, dz) + dy)));
}
/// <summary>
/// Gets the heuristic.
/// </summary>
/// <param name="current">The current node.</param>
/// <param name="goal">The goal node.</param>
/// <returns>
/// The heuristic
/// </returns>
public override int GetHeuristic(IPositioned current, IPositioned goal)
{
var dx = Math.Abs(current.position.x - goal.position.x);
var dz = Math.Abs(current.position.z - goal.position.z);
var dy = Math.Abs(current.position.y - goal.position.y);
return(Mathf.RoundToInt((Math.Min(dx, dz) * Consts.SquareRootTwo) + Math.Max(dx, dz) - Math.Min(dx, dz) + dy));
}
/// <summary>
/// Gets the move cost.
/// </summary>
/// <param name="current">The current node.</param>
/// <param name="other">The other node.</param>
/// <returns>
/// The move cost
/// </returns>
public override int GetMoveCost(IPositioned current, IPositioned other)
{
var dx = Math.Abs(current.position.x - other.position.x);
var dz = Math.Abs(current.position.z - other.position.z);
var dy = Math.Abs(current.position.y - other.position.y);
return Mathf.RoundToInt(this.baseMoveCost * (dx + dz + dy));
}
/// <summary>
/// Gets the heuristic.
/// </summary>
/// <param name="current">The current.</param>
/// <param name="goal">The goal.</param>
/// <returns>The heuristic</returns>
public override int GetHeuristic(IPositioned current, IPositioned goal)
{
var dx = Math.Abs(current.position.x - goal.position.x);
var dz = Math.Abs(current.position.z - goal.position.z);
var dy = Math.Abs(current.position.y - goal.position.y);
return Mathf.RoundToInt(this.baseMoveCost * (dx + dz + dy));
}
/// <summary>
/// Gets the heuristic.
/// </summary>
/// <param name="current">The current node.</param>
/// <param name="goal">The goal node.</param>
/// <returns>
/// The heuristic
/// </returns>
public override int GetHeuristic(IPositioned current, IPositioned goal)
{
var dx = (current.position.x - goal.position.x);
var dz = (current.position.z - goal.position.z);
var dy = (current.position.y - goal.position.y);
return(Mathf.RoundToInt(this.baseMoveCost * Mathf.Sqrt((dx * dx) + (dz * dz) + (dy * dy))));
}
/// <summary>
/// Gets the move cost.
/// </summary>
/// <param name="current">The current node.</param>
/// <param name="other">The other node.</param>
/// <returns>
/// The move cost
/// </returns>
public override int GetMoveCost(IPositioned current, IPositioned other)
{
var dx = (int)Math.Abs(current.position.x - other.position.x);
var dz = (int)Math.Abs(current.position.z - other.position.z);
var dy = (int)Math.Abs(current.position.y - other.position.y);
return this.baseMoveCost * (dx + dz + dy);
}
/// <summary>
/// Gets the heuristic.
/// </summary>
/// <param name="current">The current node.</param>
/// <param name="goal">The goal node.</param>
/// <returns>
/// The heuristic
/// </returns>
public override int GetHeuristic(IPositioned current, IPositioned goal)
{
var dx = Math.Abs(current.position.x - goal.position.x);
var dz = Math.Abs(current.position.z - goal.position.z);
var dy = Math.Abs(current.position.y - goal.position.y);
return Mathf.RoundToInt((this.baseMoveCost * (dx + dz + dy)) + ((this.diagonalMoveCost - (2f * this.baseMoveCost)) * Math.Min(dx, dz)));
}
/// <summary>
/// Gets the heuristic.
/// </summary>
/// <param name="current">The current.</param>
/// <param name="goal">The goal.</param>
/// <returns>The heuristic</returns>
public override int GetHeuristic(IPositioned current, IPositioned goal)
{
var dx = (int)Math.Abs(current.position.x - goal.position.x);
var dz = (int)Math.Abs(current.position.z - goal.position.z);
var dy = (int)Math.Abs(current.position.y - goal.position.y);
return this.baseMoveCost * (dx + dz + dy);
}
/// <summary>
/// Gets the heuristic.
/// </summary>
/// <param name="current">The current.</param>
/// <param name="goal">The goal.</param>
/// <returns>The heuristic</returns>
public override int GetHeuristic(IPositioned current, IPositioned goal)
{
var dx = (int)Math.Abs(current.position.x - goal.position.x);
var dz = (int)Math.Abs(current.position.z - goal.position.z);
var dy = (int)Math.Abs(current.position.y - goal.position.y);
return(this.baseMoveCost * (Math.Max(dx, dz) + dy));
}
/// <summary>
/// Gets the move cost.
/// </summary>
/// <param name="current">The current node.</param>
/// <param name="other">The other node.</param>
/// <returns>
/// The move cost
/// </returns>
public override int GetMoveCost(IPositioned current, IPositioned other)
{
var dx = Math.Abs(current.position.x - other.position.x);
var dz = Math.Abs(current.position.z - other.position.z);
var dy = Math.Abs(current.position.y - other.position.y);
return(Mathf.RoundToInt(this.baseMoveCost * (dx + dz + dy)));
}
/// <summary>
/// Gets the move cost.
/// </summary>
/// <param name="current">The current node.</param>
/// <param name="other">The other node.</param>
/// <returns>
/// The move cost
/// </returns>
public override int GetMoveCost(IPositioned current, IPositioned other)
{
var dx = (int)(current.position.x - other.position.x);
var dz = (int)(current.position.z - other.position.z);
var dy = (int)(current.position.y - other.position.y);
return this.baseMoveCost * (int)Math.Sqrt((dx * dx) + (dz * dz) + (dy * dy));
}
/// <summary>
/// Gets the heuristic.
/// </summary>
/// <param name="current">The current node.</param>
/// <param name="goal">The goal node.</param>
/// <returns>
/// The heuristic
/// </returns>
public override int GetHeuristic(IPositioned current, IPositioned goal)
{
var dx = (int)Math.Abs(current.position.x - goal.position.x);
var dz = (int)Math.Abs(current.position.z - goal.position.z);
var dy = (int)Math.Abs(current.position.y - goal.position.y);
return((int)((Math.Min(dx, dz) * 1.41f) + Math.Max(dx, dz) - Math.Min(dx, dz) + dy));
}
/// <summary>
/// Gets the heuristic.
/// </summary>
/// <param name="current">The current node.</param>
/// <param name="goal">The goal node.</param>
/// <returns>
/// The heuristic
/// </returns>
public override int GetHeuristic(IPositioned current, IPositioned goal)
{
var dx = (int)Math.Abs(current.position.x - goal.position.x);
var dz = (int)Math.Abs(current.position.z - goal.position.z);
var dy = (int)Math.Abs(current.position.y - goal.position.y);
return((this.baseMoveCost * (dx + dz + dy)) + ((this.diagonalMoveCost - (2 * this.baseMoveCost)) * Math.Min(dx, dz)));
}
/// <summary>
/// Gets the heuristic.
/// </summary>
/// <param name="current">The current node.</param>
/// <param name="goal">The goal node.</param>
/// <returns>
/// The heuristic
/// </returns>
public override int GetHeuristic(IPositioned current, IPositioned goal)
{
var dx = Math.Abs(current.position.x - goal.position.x);
var dz = Math.Abs(current.position.z - goal.position.z);
var dy = Math.Abs(current.position.y - goal.position.y);
return Mathf.RoundToInt((Math.Min(dx, dz) * Consts.SquareRootTwo) + Math.Max(dx, dz) - Math.Min(dx, dz) + dy);
}
/// <summary>
/// Gets the heuristic.
/// </summary>
/// <param name="current">The current.</param>
/// <param name="goal">The goal.</param>
/// <returns>The heuristic</returns>
public override int GetHeuristic(IPositioned current, IPositioned goal)
{
var dx = Math.Abs(current.position.x - goal.position.x);
var dz = Math.Abs(current.position.z - goal.position.z);
var dy = Math.Abs(current.position.y - goal.position.y);
return(Mathf.RoundToInt(this.baseMoveCost * (dx + dz + dy)));
}
/// <summary>
/// Gets the move cost.
/// </summary>
/// <param name="current">The current node.</param>
/// <param name="other">The other node.</param>
/// <returns>
/// The move cost
/// </returns>
public virtual int GetMoveCost(IPositioned current, IPositioned other)
{
var dx = (int)Math.Abs(current.position.x - other.position.x);
var dz = (int)Math.Abs(current.position.z - other.position.z);
var dy = (int)Math.Abs(current.position.y - other.position.y);
return _cellMoveCost * (Math.Max(dx, dz) + dy);
}
/// <summary>
/// Gets the heuristic.
/// </summary>
/// <param name="current">The current node.</param>
/// <param name="goal">The goal node.</param>
/// <returns>
/// The heuristic
/// </returns>
public override int GetHeuristic(IPositioned current, IPositioned goal)
{
var dx = (int)Math.Abs(current.position.x - goal.position.x);
var dz = (int)Math.Abs(current.position.z - goal.position.z);
var dy = (int)Math.Abs(current.position.y - goal.position.y);
return (int)((Math.Min(dx, dz) * 1.41f) + Math.Max(dx, dz) - Math.Min(dx, dz) + dy);
}
public void Add(IPositioned entity)
{
if (entity == null)
{
throw new ArgumentNullException(nameof(entity));
}
_octree.AddItem(new BoundingBox(
entity.Position - entity.Dimensions,
entity.Position + entity.Dimensions),
entity);
}
/// <summary>
/// Gets the action cost.
/// </summary>
/// <param name="from">The node from which the action will start.</param>
/// <param name="to">The node at which the action will end.</param>
/// <param name="costProvider">The cost provider in use by the path finder.</param>
/// <returns>
/// The cost
/// </returns>
public int GetActionCost(IPositioned from, IPositioned to, IMoveCost costProvider)
{
//This is kind of an arbitrary thing. Under normal circumstances the cost of moving from one cell to another is equal to the distance between the cells.
//Based on that the cost here would be the length of the arc of the jump. However speed is also a factor. One could get a reference to the IDefine speed component and use the current speed setting,
//but we want to use a different speed for jumps, so to be accurate we would need to determine the speed difference and take that into account.
//We do not want this calculation to be overly complex however, so for this example we keep it simple by using assumptions. This will produce a less accurate cost but...
var halfDistance = (to.position - from.position).magnitude / 2.0f;
var hmax = this.heightFactor * halfDistance * 2.0f;
var assumedNormalSpeed = 3.0f;
return((int)(2 * Mathf.Sqrt((halfDistance * halfDistance) + (hmax * hmax)) / (this.groundSpeed / assumedNormalSpeed)) * costProvider.baseMoveCost);
}
private bool ResolveNextPoint()
{
if (_currentPath == null || _currentPath.count == 0)
{
if (_pendingResult != null)
{
//A pending route exists (i.e. next way point) so move on to that one right away
var req = _pendingResult.originalRequest as InternalPathRequest;
if (req.pathType != InternalPathRequest.Type.PathboundWaypoint && _currentDestination != null && _currentPath != null)
{
AnnounceEvent(UnitNavigationEventMessage.Event.WaypointReached, _currentDestination.position, null);
}
return(ConsumeResult());
}
return(_currentDestination != null);
}
if (_currentDestination != null)
{
_remainingSquaredDistance -= (_previousDestination - _currentDestination.position).sqrMagnitude;
_previousDestination = _currentDestination.position;
}
_currentDestination = _currentPath.Pop();
var portal = _currentDestination as IPortalNode;
if (portal != null)
{
_isPortaling = true;
//Since a portal will never be the last node on a path, we can safely pop the next in line as the actual destination of the portal
//doing it like this also caters for the scenario where the destination is the last node.
_currentDestination = _currentPath.Pop();
_currentGrid = portal.Execute(
_transform,
_currentDestination,
() =>
{
_isPortaling = false;
});
}
else if (_pathSettings.announceAllNodes)
{
AnnounceEvent(UnitNavigationEventMessage.Event.NodeReached, _previousDestination, null);
}
return(!_isPortaling);
}
private void StopInternal()
{
lock (_syncLock)
{
_stopped = true;
_wayPoints.Clear();
_currentPath = null;
_pendingPathRequest = null;
_currentDestination = null;
_pendingResult = null;
_manualReplan = null;
}
}
private bool ConsumeResult()
{
//Since result processing may actually repath and consequently a new result may arrive we need to operate on locals and null the pending result
PathResult result;
lock (_syncLock)
{
result = _pendingResult;
_pendingResult = null;
}
var req = result.originalRequest as InternalPathRequest;
//Consume way points if appropriate. This must be done prior to the processing of the result, since if the request was a way point request, the first item in line is the one the result concerns.
if (req.pathType == InternalPathRequest.Type.Waypoint)
{
_wayPoints.Dequeue();
}
else if (req.pathType == InternalPathRequest.Type.PathboundWaypoint)
{
_pathboundWayPoints.Dequeue();
}
//Reset current destination and path no matter what
_previousDestination = _transform.position;
_currentDestination = null;
_currentPath = null;
//Process the result
if (!ProcessAndValidateResult(result))
{
return(false);
}
//Consume the result
_onFinalApproach = false;
_currentPath = result.path;
_currentGrid = req.fromGrid;
_remainingSquaredDistance = _currentPath.CalculateSquaredLength();
_endOfResolvedPath = _currentPath.Last().position;
_endOfPath = _endOfResolvedPath;
//Update pending way points
UpdatePathboundWaypoints(result.pendingWaypoints);
//Pop the first node as our next destination.
_unit.hasArrivedAtDestination = false;
_currentDestination = _currentPath.Pop();
return(true);
}
/// <summary>
/// Executes the action.
/// </summary>
/// <param name="unit">The unit that has entered the portal.</param>
/// <param name="from">The portal cell that was entered.</param>
/// <param name="to">The destination at the other side of the portal.</param>
/// <param name="callWhenComplete">The callback to call when the move is complete.</param>
public void Execute(Transform unit, PortalCell from, IPositioned to, Action callWhenComplete)
{
var heightSampler = GameServices.heightStrategy.heightSampler;
float fromHeight;
if (!heightSampler.TrySampleHeight(unit.position, out fromHeight))
{
fromHeight = to.position.y;
}
var heightAdjustment = to.position.y - fromHeight;
unit.position = new Vector3(to.position.x, unit.position.y + heightAdjustment, to.position.z);
callWhenComplete();
}
/// <summary>
/// Gets the move cost.
/// </summary>
/// <param name="current">The current node.</param>
/// <param name="other">The other node.</param>
/// <returns>
/// The move cost
/// </returns>
public virtual int GetMoveCost(IPositioned current, IPositioned other)
{
var dx = (int)Math.Abs(current.position.x - other.position.x);
var dz = (int)Math.Abs(current.position.z - other.position.z);
var dy = (int)Math.Abs(current.position.y - other.position.y);
//Its not accurate to account for the height difference by simply adding it, but it's faster and since it is the same for all it's fine.
if (dx > 0 && dz > 0)
{
return((dx * _cellDiagonalMoveCost) + (dy * _cellMoveCost));
}
return((Math.Max(dx, dz) + dy) * _cellMoveCost);
}
/// <summary>
/// Gets the move cost.
/// </summary>
/// <param name="current">The current node.</param>
/// <param name="other">The other node.</param>
/// <returns>
/// The move cost
/// </returns>
public virtual int GetMoveCost(IPositioned current, IPositioned other)
{
var dx = (int)Math.Abs(current.position.x - other.position.x);
var dz = (int)Math.Abs(current.position.z - other.position.z);
var dy = (int)Math.Abs(current.position.y - other.position.y);
//Its not accurate to account for the height difference by simply adding it, but it's faster and since it is the same for all it's fine.
if (dx > 0 && dz > 0)
{
return (dx * _cellDiagonalMoveCost) + (dy * _cellMoveCost);
}
return (Math.Max(dx, dz) + dy) * _cellMoveCost;
}
/// <summary>
/// Executes the action.
/// </summary>
/// <param name="unit">The unit that has entered the portal.</param>
/// <param name="from">The portal cell that was entered.</param>
/// <param name="to">The destination at the other side of the portal.</param>
/// <param name="callWhenComplete">The callback to call when the move is complete.</param>
public void Execute(Transform unit, PortalCell from, IPositioned to, Action callWhenComplete)
{
var heightSampler = GameServices.heightStrategy.heightSampler;
float fromHeight;
if (!heightSampler.TrySampleHeight(unit.position, out fromHeight))
{
fromHeight = to.position.y;
}
var heightAdjustment = to.position.y - fromHeight;
unit.position = new Vector3(to.position.x, unit.position.y + heightAdjustment, to.position.z);
callWhenComplete();
}
private void SetManualPath(Path path, ReplanCallback onReplan)
{
if (path == null || path.count == 0)
{
StopInternal();
return;
}
_stop = false;
_stopped = false;
_onFinalApproach = false;
_manualReplan = onReplan;
_currentPath = path;
_endOfResolvedPath = _currentPath.Last().position;
_currentDestination = path.Pop();
_currentGrid = GridManager.instance.GetGrid(_currentDestination.position);
_endOfPath = _endOfResolvedPath;
_lastPathRequestTime = Time.time;
}
/// <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 action cost.
/// </summary>
/// <param name="from">The node from which the action will start.</param>
/// <param name="to">The node at which the action will end.</param>
/// <param name="costProvider">The cost provider in use by the path finder.</param>
/// <returns>The cost</returns>
public int GetCost(IPositioned from, IPositioned to, IMoveCost costProvider)
{
return _action.GetActionCost(from, to, costProvider);
}
private void SetManualPath(Path path, ReplanCallback onReplan)
{
if (path == null || path.count == 0)
{
StopInternal();
return;
}
_stop = false;
_stopped = false;
_onFinalApproach = false;
_manualReplan = onReplan;
_currentPath = path;
_endOfResolvedPath = _currentPath.Last().position;
_currentDestination = path.Pop();
_currentGrid = GridManager.instance.GetGrid(_currentDestination.position);
_endOfPath = _endOfResolvedPath;
_lastPathRequestTime = Time.time;
}
/// <summary>
/// Gets the action cost.
/// </summary>
/// <param name="from">The node from which the action will start.</param>
/// <param name="to">The node at which the action will end.</param>
/// <param name="costProvider">The cost provider in use by the path finder.</param>
/// <returns>The cost</returns>
public int GetCost(IPositioned from, IPositioned to, IMoveCost costProvider)
{
return(_action.GetActionCost(from, to, costProvider));
}
/// <summary>
/// Gets the action cost.
/// </summary>
/// <param name="from">The node from which the action will start.</param>
/// <param name="to">The node at which the action will end.</param>
/// <param name="costProvider">The cost provider in use by the path finder.</param>
/// <returns></returns>
public int GetActionCost(IPositioned from, IPositioned to, IMoveCost costProvider)
{
return costProvider.GetHeuristic(from, to);
}
public static bool CanReducePath(IPositioned point1, IPositioned point3, IUnitProperties unitProps, CellMatrix matrix, ICellCostStrategy costStrategy)
{
Vector3 p1;
Vector3 p3;
//Assign the points so we start with the point with the lowest x-value to simplify things
if (point1.position.x > point3.position.x)
{
p1 = point3.position;
p3 = point1.position;
}
else
{
p1 = point1.position;
p3 = point3.position;
}
var requesterRadius = unitProps.radius;
var tan = Tangents.Create(p1, p3, requesterRadius);
var incZ = tan.slopeDir;
var cellSize = matrix.cellSize;
var halfCell = cellSize / 2.0f;
//Adjust the start and end cells to possibly include their immediate neighbour if the unit's radius crossed said boundary.
var radiusAdjust = new Vector3(requesterRadius, 0.0f, requesterRadius * incZ);
//Get the start and end cells, get the cost of the actual start and end, and then reassign the start and end with the above adjustment.
var startCell = matrix.GetCell(p1, true);
var startCost = costStrategy.GetCellCost(startCell, unitProps);
startCell = matrix.GetCell(p1 - radiusAdjust, true);
var endCell = matrix.GetCell(p3, true);
var endCost = costStrategy.GetCellCost(endCell, unitProps);
endCell = matrix.GetCell(p3 + radiusAdjust, true);
//We want x to end up on cell boundaries, the first of which is this far from the first points position
var xAdj = p1.x + (startCell.position.x - p1.x) + halfCell;
//We want to adjust z so that we correctly count impacted cells, this adjusts z so it starts at the bottom boundary of the first cell (for purposes of calculation)
var zAdj = p1.z - (halfCell + ((p1.z - startCell.position.z) * incZ));
//The movement across the x-axis
float deltaX = 0.0f;
var cellMatrix = matrix.rawMatrix;
int indexX = 0;
for (int x = startCell.matrixPosX; x <= endCell.matrixPosX; x++)
{
//So instead of just checking all cells in the bounding rect defined by the two cells p1 and p3,
//we limit it to the cells immediately surrounding the proposed line (tangents), including enough cells that we ensure the unit will be able to pass through,
//at the extreme routes between the two cells (i.e top corner to top corner and bottom corner to bottom corner
int startZ;
int endZ;
//If the tangents are horizontal or vertical z range is obvious
if (tan.isAxisAligned)
{
startZ = startCell.matrixPosZ;
endZ = endCell.matrixPosZ + incZ;
}
else
{
if (indexX == 0)
{
startZ = startCell.matrixPosZ;
}
else
{
var startCellsPassed = Mathf.FloorToInt((tan.LowTangent(deltaX) - zAdj) / cellSize) * incZ;
startZ = LimitStart(
startCell.matrixPosZ + startCellsPassed,
startCell.matrixPosZ,
incZ);
}
//The movement this step will perform across the x-axis
deltaX = xAdj + (indexX * cellSize);
var endCellsIntercepted = Mathf.FloorToInt((tan.HighTangent(deltaX) - zAdj) / cellSize) * incZ;
endZ = LimitEnd(
startCell.matrixPosZ + endCellsIntercepted,
endCell.matrixPosZ,
incZ) + incZ;
}
indexX++;
for (int z = startZ; z != endZ; z += incZ)
{
var intermediary = cellMatrix[x, z];
var intermediaryCost = costStrategy.GetCellCost(intermediary, unitProps);
if (!intermediary.isWalkableFrom(startCell, unitProps) || (startCost < intermediaryCost && endCost < intermediaryCost))
{
return false;
}
}
}
return true;
}
protected Expression(IPositioned first, IPositioned second) : base(first, second)
{
}
private void StopInternal()
{
lock (_syncLock)
{
_stopped = true;
_wayPoints.Clear();
_currentPath = null;
_pendingPathRequest = null;
_currentDestination = null;
_pendingResult = null;
_manualReplan = null;
}
}
/// <summary>
/// Executes the specified unit.
/// </summary>
/// <param name="unit">The unit that has entered the portal.</param>
/// <param name="from">The portal cell that was entered.</param>
/// <param name="to">The destination at the other side of the portal.</param>
/// <param name="callWhenComplete">The callback to call when the move is complete.</param>
public void Execute(Transform unit, PortalCell from, IPositioned to, Action callWhenComplete)
{
callWhenComplete();
}
/// <summary> /// Gets the heuristic. /// </summary> /// <param name="current">The current node.</param> /// <param name="goal">The goal node.</param> /// <returns> /// The heuristic /// </returns> public abstract int GetHeuristic(IPositioned current, IPositioned goal);
/// <summary>
/// Gets the action cost.
/// </summary>
/// <param name="from">The node from which the action will start.</param>
/// <param name="to">The node at which the action will end.</param>
/// <returns></returns>
public int GetActionCost(IPositioned from, IPositioned to)
{
return 0;
}
/// <summary>
/// Gets the action cost.
/// </summary>
/// <param name="from">The node from which the action will start.</param>
/// <param name="to">The node at which the action will end.</param>
/// <param name="costProvider">The cost provider in use by the path finder.</param>
/// <returns></returns>
public int GetActionCost(IPositioned from, IPositioned to, IMoveCost costProvider)
{
return 0;
}
public ClassVariantNotFoundException(IPositioned element, string className, string[] types)
: base(element, CreateErrorMessage(className, types))
{
}
private bool ResolveNextPoint()
{
if (_pendingResult != null)
{
ConsumeResult();
}
if (_currentPath == null)
{
return false;
}
//Get the direction of movement and the remaining distance
var actualDirection = _transform.position.DirToXZ(_currentDestination.position);
var currentDestinationDistance = actualDirection.magnitude;
_remainingTotalDistance = _remainingPathDistance + currentDestinationDistance;
//If we are on the last node already, there is no more to do here.
if (_currentPath.count == 0)
{
return true;
}
//Are we at a place where we need to get the next node?
if (currentDestinationDistance > _pathSettings.nextNodeDistance &&
(this.strictPathFollowing || Vector3.Dot(_curPlannedDirection, actualDirection) > 0f))
{
return true;
}
var previousDestination = _currentDestination;
_currentDestination = _currentPath.Pop();
var portal = _currentDestination as IPortalNode;
if (portal != null)
{
_isPortaling = true;
//Since a portal will never be the last node on a path, we can safely pop the next in line as the actual destination of the portal
//doing it like this also caters for the scenario where the destination is the last node.
_currentDestination = _currentPath.Pop();
_currentGrid = portal.Execute(
_transform,
_currentDestination,
() =>
{
_isPortaling = false;
});
}
if (previousDestination != null)
{
_remainingPathDistance -= (_currentDestination.position - previousDestination.position).magnitude;
if (previousDestination is Waypoint)
{
_wayPoints.ConsumeViaPoint();
AnnounceEvent(UnitNavigationEventMessage.Event.WaypointReached, previousDestination.position, null);
}
else if (_pathSettings.announceAllNodes)
{
AnnounceEvent(UnitNavigationEventMessage.Event.NodeReached, previousDestination.position, null);
}
}
_curPlannedDirection = _transform.position.DirToXZ(_currentDestination.position);
return !_isPortaling;
}
protected Expression(IPositioned location) : base(location)
{
}
private void SetPath(Path path, bool isWaypoint)
{
_endOfResolvedPath = path.Last().position;
_currentGrid = GridManager.instance.GetGrid(path.Peek().position);
if (isWaypoint)
{
_remainingPathDistance += path.CalculateLength();
_currentPath = _currentPath.AppendSegment(path);
}
else
{
if (!TrimPath(path))
{
RequestPath(_transform.position, _wayPoints.desiredEndOfPath, InternalPathRequest.Type.Normal);
_currentPath = null;
return;
}
_currentPath = path;
_remainingPathDistance = path.CalculateLength();
//Pop the first node as our next destination.
_currentDestination = path.Pop();
_curPlannedDirection = _transform.position.DirToXZ(_currentDestination.position);
}
_unit.hasArrivedAtDestination = false;
}
/// <summary>
/// Initializes a new instance of the <see cref="FormationPositionWithIndex"/> struct.
/// </summary>
/// <param name="pos">The position.</param>
/// <param name="idx">The index.</param>
internal FormationPositionWithIndex(Vector3 pos, int idx)
{
this.position = new Position(pos);
this.index = idx;
}
private bool ResolveNextPoint()
{
if (_pendingResult != null)
{
ConsumeResult();
}
if (_currentPath == null)
{
return(false);
}
//Get the direction of movement and the remaining distance
var actualDirection = _transform.position.DirToXZ(_currentDestination.position);
var currentDestinationDistance = actualDirection.magnitude;
_remainingTotalDistance = _remainingPathDistance + currentDestinationDistance;
//If we are on the last node already, there is no more to do here.
if (_currentPath.count == 0)
{
return(true);
}
//Are we at a place where we need to get the next node?
if (currentDestinationDistance > _pathSettings.nextNodeDistance &&
(this.strictPathFollowing || Vector3.Dot(_curPlannedDirection, actualDirection) > 0f))
{
return(true);
}
var previousDestination = _currentDestination;
_currentDestination = _currentPath.Pop();
var portal = _currentDestination as IPortalNode;
if (portal != null)
{
_isPortaling = true;
//Since a portal will never be the last node on a path, we can safely pop the next in line as the actual destination of the portal
//doing it like this also caters for the scenario where the destination is the last node.
_currentDestination = _currentPath.Pop();
_currentGrid = portal.Execute(
_transform,
_currentDestination,
() =>
{
_isPortaling = false;
});
}
if (previousDestination != null)
{
_remainingPathDistance -= (_currentDestination.position - previousDestination.position).magnitude;
if (previousDestination is Waypoint)
{
_wayPoints.ConsumeViaPoint();
AnnounceEvent(UnitNavigationEventMessage.Event.WaypointReached, previousDestination.position, null);
}
else if (_pathSettings.announceAllNodes)
{
AnnounceEvent(UnitNavigationEventMessage.Event.NodeReached, previousDestination.position, null);
}
}
_curPlannedDirection = _transform.position.DirToXZ(_currentDestination.position);
return(!_isPortaling);
}
/// <summary>
/// Sets up the formation indices, based on the already populated list of formation positions.
/// </summary>
protected virtual void SetupFormationIndices()
{
if (_formationPositions == null || _formationPositions.count == 0)
{
// exit early if we don't have valid formation positions
return;
}
if (_tempFormationPositions == null)
{
// prepare temporary formation positions list
_tempFormationPositions = new List <FormationPositionWithIndex>(count);
}
int cogCount = 0;
Vector3 center = Vector3.zero;
for (int f = 0; f < count; f++)
{
IPositioned formPos = GetFormationPosition(f);
if (formPos == null)
{
continue;
}
// populate list of temporary formation positions along with their index in the list
_tempFormationPositions.Add(new FormationPositionWithIndex(formPos.position, cogCount));
cogCount++;
center += formPos.position;
}
// compute center of gravity for all the formation positions
center /= cogCount;
//Sort the units by their distance to the group center, in descending order,
//i.e. those furthest away are first to choose a formation position
_unitSortComparer.compareTo = center;
this.Sort(_unitSortComparer);
// For each unit group member in the group, check all the remaining available formation positions
// Whatever remaining available formation position is nearest to the unit, the unit picks as its formation position
for (int i = 0; i < this.count; i++)
{
var unit = this[i];
if (unit == null)
{
continue;
}
Vector3 unitPos = unit.position;
float shortestDistance = float.MaxValue;
int formationIndex = -1;
int actualFormationIndex = -1;
int formationsCount = _tempFormationPositions.Count;
for (int j = 0; j < formationsCount; j++)
{
IPositioned formPos = _tempFormationPositions[j].position;
float distance = (formPos.position - unitPos).sqrMagnitude;
if (distance < shortestDistance)
{
shortestDistance = distance;
formationIndex = j;
actualFormationIndex = _tempFormationPositions[j].index;
}
}
_tempFormationPositions.RemoveAt(formationIndex);
unit.formationIndex = actualFormationIndex;
}
// clear the temporary formation positions list of any possible leftovers (although there shouldn't be any)
_tempFormationPositions.Clear();
}
/// <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>
/// Executes the portal move.
/// </summary>
/// <param name="unit">The unit that is entering the portal.</param>
/// <param name="to">The destination at the other side of the portal.</param>
/// <param name="callWhenComplete">The callback to call when the move is complete.</param>
/// <returns>The grid of the destination.</returns>
public IGrid Execute(Transform unit, IPositioned to, Action callWhenComplete)
{
_action.Execute(unit, this, to, callWhenComplete);
return _parentPortal.GetGridFor(this);
}
/// <summary>
/// Gets the action cost.
/// </summary>
/// <param name="from">The node from which the action will start.</param>
/// <param name="to">The node at which the action will end.</param>
/// <param name="costProvider">The cost provider in use by the path finder.</param>
/// <returns></returns>
public int GetActionCost(IPositioned from, IPositioned to, IMoveCost costProvider)
{
return(0);
}
/// <summary>
/// Gets the action cost.
/// </summary>
/// <param name="from">The node from which the action will start.</param>
/// <param name="to">The node at which the action will end.</param>
/// <returns>The cost</returns>
public int GetCost(IPositioned from, IPositioned to)
{
return _action.GetActionCost(from, to);
}
private bool ResolveNextPoint()
{
if (_currentPath == null || _currentPath.count == 0)
{
if (_pendingResult != null)
{
//A pending route exists (i.e. next way point) so move on to that one right away
var req = _pendingResult.originalRequest as InternalPathRequest;
if (req.pathType != InternalPathRequest.Type.PathboundWaypoint && _currentDestination != null && _currentPath != null)
{
AnnounceEvent(UnitNavigationEventMessage.Event.WaypointReached, _currentDestination.position, null);
}
return ConsumeResult();
}
return (_currentDestination != null);
}
if (_currentDestination != null)
{
_remainingSquaredDistance -= (_previousDestination - _currentDestination.position).sqrMagnitude;
_previousDestination = _currentDestination.position;
}
_currentDestination = _currentPath.Pop();
var portal = _currentDestination as IPortalNode;
if (portal != null)
{
_isPortaling = true;
//Since a portal will never be the last node on a path, we can safely pop the next in line as the actual destination of the portal
//doing it like this also caters for the scenario where the destination is the last node.
_currentDestination = _currentPath.Pop();
_currentGrid = portal.Execute(
_transform,
_currentDestination,
() =>
{
_isPortaling = false;
});
}
else if (_pathSettings.announceAllNodes)
{
AnnounceEvent(UnitNavigationEventMessage.Event.NodeReached, _previousDestination, null);
}
return !_isPortaling;
}
/// <summary> /// Gets the heuristic. /// </summary> /// <param name="current">The current node.</param> /// <param name="goal">The goal node.</param> /// <returns> /// The heuristic /// </returns> public abstract int GetHeuristic(IPositioned current, IPositioned goal);
/// <summary>
/// Executes the portal move.
/// </summary>
/// <param name="unit">The unit that is entering the portal.</param>
/// <param name="to">The destination at the other side of the portal.</param>
/// <param name="callWhenComplete">The callback to call when the move is complete.</param>
/// <returns>The grid of the destination.</returns>
public IGrid Execute(Transform unit, IPositioned to, Action callWhenComplete)
{
_action.Execute(unit, this, to, callWhenComplete);
return(_parentPortal.GetGridFor(this));
}
/// <summary>
/// Initializes a new instance of the <see cref="FormationPositionWithIndex"/> struct.
/// </summary>
/// <param name="pos">The position.</param>
/// <param name="idx">The index.</param>
internal FormationPositionWithIndex(Vector3 pos, int idx)
{
this.position = new Position(pos);
this.index = idx;
}
public AddPositionedComponentEvent(IPositioned positioned) => Positioned = positioned;
private bool ConsumeResult()
{
//Since result processing may actually repath and consequently a new result may arrive we need to operate on locals and null the pending result
PathResult result;
lock (_syncLock)
{
result = _pendingResult;
_pendingResult = null;
}
var req = result.originalRequest as InternalPathRequest;
//Consume way points if appropriate. This must be done prior to the processing of the result, since if the request was a way point request, the first item in line is the one the result concerns.
if (req.pathType == InternalPathRequest.Type.Waypoint)
{
_wayPoints.Dequeue();
}
else if (req.pathType == InternalPathRequest.Type.PathboundWaypoint)
{
_pathboundWayPoints.Dequeue();
}
//Reset current destination and path no matter what
_previousDestination = _transform.position;
_currentDestination = null;
_currentPath = null;
//Process the result
if (!ProcessAndValidateResult(result))
{
return false;
}
//Consume the result
_onFinalApproach = false;
_currentPath = result.path;
_currentGrid = req.fromGrid;
_remainingSquaredDistance = _currentPath.CalculateSquaredLength();
_endOfResolvedPath = _currentPath.Last().position;
_endOfPath = _endOfResolvedPath;
//Update pending way points
UpdatePathboundWaypoints(result.pendingWaypoints);
//Pop the first node as our next destination.
_unit.hasArrivedAtDestination = false;
_currentDestination = _currentPath.Pop();
return true;
}
