private RichCell GetNeighbourFromIdx(Cell c, int selfIdx)
{
switch (selfIdx)
{
case 0:
{
return c.GetNeighbour(-1, -1) as RichCell;
}
case 1:
{
return c.GetNeighbour(0, -1) as RichCell;
}
case 3:
{
return c.GetNeighbour(-1, 0) as RichCell;
}
case 6:
{
return c.GetNeighbour(-1, 1) as RichCell;
}
default:
{
throw new InvalidOperationException("A fault in the data recovery logic has occurred, this is a fatal error.");
}
}
}
/// <summary>
/// Gets the movement vector.
/// </summary>
/// <param name="currentVelocity">The current velocity.</param>
/// <returns>
/// The movement vector
/// </returns>
protected override Vector3 GetMovementVector(Vector3 currentVelocity)
{
var pos = this.transformCached.position;
var grid = GridManager.instance.GetGrid(pos);
if (grid == null)
{
_targetCell = null;
return Vector3.zero;
}
var cell = grid.GetCell(pos);
if (cell.isWalkable(this.attributes))
{
if (_targetCell != null)
{
var moveVector = (_targetCell.position - pos).AdjustAxis(0.0f, this.excludedAxis);
var dist = moveVector.magnitude + this.radius;
if (dist > grid.cellSize / 2.0f)
{
return moveVector;
}
}
_targetCell = null;
return Vector3.zero;
}
if (_targetCell == null || _targetCell == cell)
{
_targetCell = grid.GetNearestWalkableCell(pos, pos, true, this.fleeMaxRadius, this.attributes);
}
return (_targetCell.position - pos).AdjustAxis(0.0f, this.excludedAxis);
}
public InputTarget(Character character, Cell cell)
{
if (character != null || cell != null)
{
isOverHUD = false;
this.character = character;
this.cell = cell;
}
else
isOverHUD = true;
}
/// <summary>
/// Records the cell data.
/// </summary>
/// <param name="c">The cell.</param>
/// <param name="cellIdx">Index of the cell.</param>
protected override void RecordCellData(Cell c, int cellIdx)
{
//Since opposing cells share the same data, just with opposite values,
//we only need to store half the height data of a cell. The remaining ones can be obtained from neighbours
var cell = c as RichCell;
var source = cell.GetHeightData();
//Since the matrix is filled one column at a time, we store data that way too.
StoreData(source, 0, cellIdx, 0x01);
StoreData(source, 1, cellIdx, 0x02);
StoreData(source, 3, cellIdx, 0x04);
StoreData(source, 6, cellIdx, 0x08);
}
/// <summary>
/// Injects the cell data.
/// </summary>
/// <param name="c">The cell.</param>
/// <param name="cellIdx">Index of the cell.</param>
protected override void InjectCellData(Cell c, int cellIdx)
{
var cell = c as RichCell;
var indicesMask = _heightDataIndices[cellIdx];
var dataIdx = indicesMask >> 4;
//Since the matrix is filled one column at a time, we process neighbours that way too.
dataIdx = SetData(cell, 0, 8, dataIdx, (indicesMask & 0x01) > 0);
dataIdx = SetData(cell, 1, 7, dataIdx, (indicesMask & 0x02) > 0);
dataIdx = SetData(cell, 3, 5, dataIdx, (indicesMask & 0x04) > 0);
dataIdx = SetData(cell, 6, 2, dataIdx, (indicesMask & 0x08) > 0);
}
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;
}
private bool GetCellNeighboursAndUnwalkability(Cell cell, DynamicArray<Cell> walkableCells, bool preventDiagonals)
{
if (walkableCells.count != 0)
{
// if the list for holding the cells is not empty, make it empty
walkableCells.Clear();
}
var unitAttributes = _unitProperties.attributes;
int mx = cell.matrixPosX;
int mz = cell.matrixPosZ;
for (int x = -1; x <= 1; x++)
{
for (int z = -1; z <= 1; z++)
{
if (x == 0 && z == 0)
{
// cell in the middle is the cell which neighbours we want, thus ignore the center cell
continue;
}
if (x != 0 && z != 0 && preventDiagonals)
{
// if we are supposed to prevent diagonal moves, then ignore diagonal cell neighbours
continue;
}
var neighbour = _grid.cellMatrix[mx + x, mz + z];
if (neighbour != null)
{
if (neighbour.isWalkable(unitAttributes) && cell.isWalkableFrom(neighbour, _unitProperties))
{
// if the neighbour is walkable, and the center cell is walkable from this neighbour, then it is of interest
walkableCells.Add(neighbour);
}
else
{
// if we find just a single neighbour that is blocked, then we return false
return false;
}
}
else if (_builtInContainment)
{
// if there is a missing cell, and we want to use built-in containment, then return false on first missing cell (off-grid)
return false;
}
}
}
return true;
}
public bool Update(Cell c)
{
var x = c.matrixPosX;
var z = c.matrixPosZ;
if (_lastCoverage.Contains(x, z) && !_newCoverage.Contains(x, z))
{
return c.RemoveDynamicObstacle(_parent);
}
else if (!_lastCoverage.Contains(x, z) && _newCoverage.Contains(x, z))
{
return c.AddDynamicObstacle(_parent);
}
return false;
}
private void FastMarchingMethod(Cell currentCell)
{
var cellPos = currentCell.position;
// get all walkable neighbours - this method internally checks for whether the cells have been fast marched
GetWalkableCellNeighbours(currentCell, _tempWalkableNeighbours, !_allowDiagonals);
int neighboursCount = _tempWalkableNeighbours.count;
for (int i = 0; i < neighboursCount; i++)
{
var neighbour = _tempWalkableNeighbours[i];
Vector3 neighbourPos = neighbour.position;
// if the cell has unwalkable neighbour cells, then we need to multiply its magnitude with the obstacle strength factor
bool noUnwalkables = GetCellNeighboursAndUnwalkability(neighbour, _extraTempWalkableNeighbours, !_allowDiagonals);
float factor = noUnwalkables ? 1f : _obstacleStrengthFactor;
_fastMarchedCellsSet[neighbourPos] = neighbourPos.DirToXZ(cellPos) * factor;
if (!_groupBounds.Contains(neighbourPos))
{
// only enqueue cell neighbours that lie within the group bounds area
continue;
}
_openSet.Enqueue(neighbour);
}
}
private Cell GetCellNeighbour(Cell cell, int dx, int dz)
{
int mx = cell.matrixPosX + dx;
int mz = cell.matrixPosZ + dz;
var matrix = cell.parent;
if (mx >= 0 && mx < matrix.columns &&
mz >= 0 && mz < matrix.rows)
{
// if the requested neighbour is on the same grid, then just get it normally
return matrix[mx, mz];
}
// TODO: all grids need to have same cell size, for this to work - but how else to know which matrix the mx and mz comes from ?
// if the requested neighbour is not on the same grid, however, we compute the position of the neighbour cell and attempt to get it
float cellSize = matrix.cellSize;
Vector3 pos = cell.position + new Vector3(dx * cellSize, 0f, dz * cellSize);
return GetCellAtPos(pos);
}
public NormalMoveAction(Character character, ActionPrice price, [NotNull]Cell cell)
: base(character, price)
{
_cell = cell;
}
/// <summary>
/// Tries the get a walkable neighbour.
/// </summary>
/// <param name="cell">The reference cell.</param>
/// <param name="dx">The delta x position in the matrix.</param>
/// <param name="dz">The delta z position in the matrix.</param>
/// <param name="requesterAttributes">The requester attributes.</param>
/// <param name="neighbour">The neighbour or null if none is found.</param>
/// <returns>
/// <c>true</c> if a walkable neighbour is found, in which case <paramref name="neighbour" /> will have the reference to that neighbour. Otherwise <c>false</c>.
/// </returns>
public bool TryGetWalkableNeighbour(IGridCell cell, int dx, int dz, IUnitProperties unitProps, out Cell neighbour)
{
var x = cell.matrixPosX + dx;
var z = cell.matrixPosZ + dz;
neighbour = _cellMatrix[x, z];
if (neighbour == null)
{
return(false);
}
return(neighbour.isWalkableFrom(cell, unitProps));
}
/// <summary>
/// Records the cell data.
/// </summary>
/// <param name="c">The cell.</param>
/// <param name="cellIdx">Index of the cell.</param>
protected override void RecordCellData(Cell c, int cellIdx)
{
var cell = c as StandardCell;
_heightBlockStatus[cellIdx] = cell.heightBlockedFrom;
}
private bool DrawLayout(IGrid grid, Cell start, Cell end, Color lineColor)
{
Gizmos.color = lineColor;
var step = grid.cellSize;
var halfCell = (step / 2.0f);
var y = grid.origin.y + 0.05f;
var xMin = start.position.x - halfCell;
var xMax = end.position.x + halfCell;
var zMin = start.position.z - halfCell;
var zMax = end.position.z + halfCell;
for (float x = xMin; x <= xMax; x += step)
{
Gizmos.DrawLine(new Vector3(x, y, zMin), new Vector3(x, y, zMax));
}
for (float z = zMin; z <= zMax; z += step)
{
Gizmos.DrawLine(new Vector3(xMin, y, z), new Vector3(xMax, y, z));
}
var matrix = grid.cellMatrix;
for (int x = start.matrixPosX; x <= end.matrixPosX; x++)
{
for (int z = start.matrixPosZ; z <= end.matrixPosZ; z++)
{
var c = matrix[x, z];
if (c == null)
{
return false;
}
var walkableToSomeone = c.isWalkable(AttributeMask.All);
var walkableToEveryone = c.isWalkable(AttributeMask.None);
if (!walkableToSomeone)
{
Gizmos.color = this.obstacleColor;
Gizmos.DrawCube(c.position, new Vector3(step, 0.05f, step));
}
else if (!walkableToEveryone)
{
var half = this.obstacleColor;
half.a = half.a * 0.5f;
Gizmos.color = half;
Gizmos.DrawCube(c.position, new Vector3(step, 0.05f, step));
}
}
}
return true;
}
private void GetWalkableCellNeighbours(Cell cell, DynamicArray<Cell> walkableCells, bool preventDiagonals)
{
if (walkableCells.count != 0)
{
// if the list for holding the cells is not empty, make it empty
walkableCells.Clear();
}
int mx = cell.matrixPosX;
int mz = cell.matrixPosZ;
// prepare non-diagonal neighbours in all 4 directions (up, down, left, right)
var n1 = _grid.cellMatrix[mx - 1, mz];
var n2 = _grid.cellMatrix[mx, mz - 1];
var n3 = _grid.cellMatrix[mx + 1, mz];
var n4 = _grid.cellMatrix[mx, mz + 1];
var unitAttributes = _unitProperties.attributes;
bool lw = false, rw = false, uw = false, dw = false;
if (n1 != null)
{
// check whether this neighbour has been fast marched
lw = _fastMarchedCellsSet.ContainsKey(n1.position);
if (!lw)
{
// if the cell has not been fast marched...
if (n1.isWalkable(unitAttributes) && cell.isWalkableFrom(n1, _unitProperties))
{
// ... and the cell is walkable, then add it to the list
walkableCells.Add(n1);
lw = true;
}
}
}
if (n2 != null)
{
dw = _fastMarchedCellsSet.ContainsKey(n2.position);
if (!dw)
{
if (n2.isWalkable(unitAttributes) && cell.isWalkableFrom(n2, _unitProperties))
{
walkableCells.Add(n2);
dw = true;
}
}
}
if (n3 != null)
{
rw = _fastMarchedCellsSet.ContainsKey(n3.position);
if (!rw)
{
if (n3.isWalkable(unitAttributes) && cell.isWalkableFrom(n3, _unitProperties))
{
walkableCells.Add(n3);
rw = true;
}
}
}
if (n4 != null)
{
uw = _fastMarchedCellsSet.ContainsKey(n4.position);
if (!uw)
{
if (n4.isWalkable(unitAttributes) && cell.isWalkableFrom(n4, _unitProperties))
{
walkableCells.Add(n4);
uw = true;
}
}
}
if (preventDiagonals)
{
return;
}
bool urw, drw, dlw, ulw;
if (_allowCornerCutting)
{
// if we allow corner cuttting, then just one of the neighbours need to be free to go diagonally
urw = uw || rw;
drw = dw || rw;
dlw = dw || lw;
ulw = uw || lw;
}
else
{
// however, if we don't allow corner cutting, then both neighbours need to be free to allow diagonal neighbour
urw = uw && rw;
drw = dw && rw;
dlw = dw && lw;
ulw = uw && lw;
}
if (dlw)
{
var n5 = _grid.cellMatrix[mx - 1, mz - 1];
if (n5 != null && !_fastMarchedCellsSet.ContainsKey(n5.position))
{
if (n5.isWalkable(unitAttributes) && cell.isWalkableFrom(n5, _unitProperties))
{
walkableCells.Add(n5);
}
}
}
if (ulw)
{
var n6 = _grid.cellMatrix[mx - 1, mz + 1];
if (n6 != null && !_fastMarchedCellsSet.ContainsKey(n6.position))
{
if (n6.isWalkable(unitAttributes) && cell.isWalkableFrom(n6, _unitProperties))
{
walkableCells.Add(n6);
}
}
}
if (drw)
{
var n7 = _grid.cellMatrix[mx + 1, mz - 1];
if (n7 != null && !_fastMarchedCellsSet.ContainsKey(n7.position))
{
if (n7.isWalkable(unitAttributes) && cell.isWalkableFrom(n7, _unitProperties))
{
walkableCells.Add(n7);
}
}
}
if (urw)
{
var n8 = _grid.cellMatrix[mx + 1, mz + 1];
if (n8 != null && !_fastMarchedCellsSet.ContainsKey(n8.position))
{
if (n8.isWalkable(unitAttributes) && cell.isWalkableFrom(n8, _unitProperties))
{
walkableCells.Add(n8);
}
}
}
}
private void FunnelMethod(Cell currentCell, Cell destinationCell)
{
// prepare local variables
// the previous path node is either the actual previous path node, or in the case of portalling where we might receive a path with only 1 point - the destination - then we use the group's center of gravity in place of the previous path node
Vector3 cellPos = currentCell.position;
Vector3 currentPathNode = _currentPath[_currentPathStep].position;
Vector3 previousPathNode = _currentPathStep > 0 ? _currentPath[_currentPathStep - 1].position : this.group.centerOfGravity;
// get all walkable cell neighbours
GetWalkableCellNeighbours(currentCell, _tempWalkableNeighbours, !_allowDiagonals);
int neighboursCount = _tempWalkableNeighbours.count;
for (int i = 0; i < neighboursCount; i++)
{
var neighbour = _tempWalkableNeighbours[i];
Vector3 neighbourPos = neighbour.position;
// check whether the neighbour cell we are visiting has unwalkable neighbours
Vector3 directionVector = Vector3.zero;
bool noUnwalkables = GetCellNeighboursAndUnwalkability(neighbour, _extraTempWalkableNeighbours, !_allowDiagonals);
if (noUnwalkables && (neighbourPos - destinationCell.position).sqrMagnitude > _funnelWidthSqr)
{
// if cell has no unwalkable neighbours...
// Sum up a vector comprised of 3 vectors: 1) fast marching direction (avoid obstacles), 2) goal direction (point to next node path), 3) path direction (point in path's direction)
Vector3 fastMarchVector = neighbourPos.DirToXZ(cellPos).normalized;
Vector3 goalVector = neighbourPos.DirToXZ(currentPathNode).normalized;
Vector3 pathVector = previousPathNode.DirToXZ(currentPathNode).normalized;
directionVector = (fastMarchVector + goalVector + pathVector) / 3f;
}
else
{
// if this cell has unwalkable neighbours, then just use fast marching direction and multiply the magnitude with the obstacle strength factor
directionVector = neighbourPos.DirToXZ(cellPos).normalized * _obstacleStrengthFactor;
}
_fastMarchedCellsSet[neighbourPos] = new PlaneVector(directionVector);
Vector3 closestPos = GetClosestPositionOnLine(previousPathNode, currentPathNode, neighbourPos);
if ((closestPos - neighbourPos).sqrMagnitude > _funnelWidthSqr)
{
// only add neighbour cells within the defined funnel area around the path
continue;
}
_openSet.Enqueue(neighbour);
}
// check whether we need to move backwards along the path (we start at the destination)
// Basically, if we reach a cell that is less than one cell size away from our previous path node, then move on to the next path nodes (decrementally)
if (_currentPathStep > 0 && (previousPathNode - cellPos).sqrMagnitude < (_grid.cellSize * _grid.cellSize))
{
if (_currentPath[_currentPathStep - 1] is IPortalNode)
{
// never move past portal nodes, we never want to remove them because we want to force units to go through the portal
return;
}
_currentPathStep -= 1;
}
}
/// <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);
}
}
public Task(TaskType type, Cell cell)
{
Type = type;
Cell = cell;
}
private void FastMarchingMethod(Cell currentCell)
{
var cellPos = currentCell.position;
// get all walkable cell neighbours
GetWalkableCellNeighbours(currentCell, _tempWalkableNeighbours, !_allowDiagonals);
int neighboursCount = _tempWalkableNeighbours.count;
for (int i = 0; i < neighboursCount; i++)
{
var neighbour = _tempWalkableNeighbours[i];
int mx = neighbour.matrixPosX;
int mz = neighbour.matrixPosZ;
// if the cell has already been fast marched, then ignore it
if (_fastMarchedCells[mx, mz].sqrMagnitude != 0f)
{
continue;
}
// if the cell has unwalkable neighbour cells, then we need to multiply its magnitude with the obstacle strength factor
bool noUnwalkables = GetCellNeighboursAndUnwalkability(neighbour, _extraTempWalkableNeighbours, !_allowDiagonals);
float factor = noUnwalkables ? 1f : _obstacleStrengthFactor;
// make the fast marched cell point towards its traversal-predecessor
Vector3 neighbourPos = neighbour.position;
_fastMarchedCells[mx, mz] = neighbourPos.DirToXZ(cellPos) * factor;
_openSet.Enqueue(neighbour);
}
}
/// <summary>
/// Records the cell data.
/// </summary>
/// <param name="c">The cell.</param>
/// <param name="cellIdx">Index of the cell.</param>
protected override void RecordCellData(Cell c, int cellIdx)
{
/* NOOP */
}
private bool DrawAccessibility(IGrid grid, Cell start, Cell end, Color lineColor)
{
var matrix = grid.cellMatrix;
var cellSize = grid.cellSize;
VectorXZ[] directions = new[] { new VectorXZ(-1, 0), new VectorXZ(-1, 1), new VectorXZ(0, 1), new VectorXZ(1, 1) };
var heightAdj = new Vector3(0.0f, 0.05f, 0.0f);
for (int x = start.matrixPosX; x <= end.matrixPosX; x++)
{
for (int z = start.matrixPosZ; z <= end.matrixPosZ; z++)
{
var c = matrix[x, z];
if (c == null)
{
return false;
}
if (!c.isWalkable(this.modelAttributes))
{
Gizmos.color = this.obstacleColor;
Gizmos.DrawCube(c.position, new Vector3(cellSize, 0.05f, cellSize));
continue;
}
var curPos = new VectorXZ(x, z);
for (int i = 0; i < 4; i++)
{
var checkPos = curPos + directions[i];
var other = matrix[checkPos.x, checkPos.z];
if (other != null)
{
if (!other.isWalkable(this.modelAttributes))
{
continue;
}
//Determine top and bottom, with bottom winning over top if equal so to speak, due to cross height.
var topPos = c;
var bottomPos = other;
if (topPos.position.y <= bottomPos.position.y)
{
topPos = bottomPos;
bottomPos = c;
}
var topDown = bottomPos.isWalkableFrom(topPos, _modelProps);
var downTop = topPos.isWalkableFrom(bottomPos, _modelProps);
if (topDown && downTop)
{
Gizmos.color = lineColor;
}
else if (topDown)
{
Gizmos.color = this.descentOnlyColor;
}
else if (downTop)
{
Gizmos.color = this.ascentOnlyColor;
}
else
{
continue;
}
Gizmos.DrawLine(c.position + heightAdj, other.position + heightAdj);
}
} /* end for each selected neighbour */
}
}
return true;
}
/// <summary>
/// Injects the cell data.
/// </summary>
/// <param name="c">The cell.</param>
/// <param name="cellIdx">Index of the cell.</param>
protected override void InjectCellData(Cell c, int cellIdx)
{
/* NOOP */
}
public InputTarget(Cell cell)
: this(null, cell)
{
}
public virtual void SetDestinationForUnit(Cell cell, bool append)
{
var units = GameServices.gameStateManager.unitSelection.selected;
if(units.memberCount < 1) return;
var unit = units[0].modelUnit;
Debug.Log(unit.character.name);
unit.MoveTo(cell.position, append);
}
/// <summary>
/// Injects the cell data.
/// </summary>
/// <param name="c">The cell.</param>
/// <param name="cellIdx">Index of the cell.</param>
protected override void InjectCellData(Cell c, int cellIdx)
{
var cell = c as StandardCell;
cell.heightBlockedFrom = _heightBlockStatus[cellIdx];
}
public bool ValidateCell(Cell cell)
{
return cell.isWalkable(navigatorFacade.attributes) && MovingOffset.Instance.Validate(cell);
}