public override bool Equals(object obj)
{
if (!(obj is PlaneVector))
{
return(false);
}
return(PlaneVector.SqrMagnitude((PlaneVector)obj - this) < 9.99999944E-11f);
}
/// <summary>
/// Returns a normalized <see cref="PlaneVector"/> (divided by its Magnitude)
/// </summary>
/// <param name="value">The <see cref="PlaneVector"/> to normalize.</param>
/// <returns></returns>
public static PlaneVector Normalize(PlaneVector value)
{
float single = PlaneVector.Magnitude(value);
if (single <= 1E-05f)
{
return(PlaneVector.zero);
}
return(value / single);
}
/// <summary>
/// Gets the squared magnitude of the vector
/// </summary>
/// <param name="a">The vector</param>
/// <returns>The squared magnitude</returns>
public static float SqrMagnitude(PlaneVector a)
{
return (a.x * a.x) + (a.z * a.z);
}
/// <summary>
/// Returns the magnitude of a <see cref="PlaneVector"/>.
/// </summary>
/// <param name="a"></param>
/// <returns></returns>
public static float Magnitude(PlaneVector a)
{
return Mathf.Sqrt((a.x * a.x) + (a.z * a.z));
}
/// <summary>
/// Returns a normalized <see cref="PlaneVector"/> (divided by its Magnitude)
/// </summary>
/// <param name="value">The <see cref="PlaneVector"/> to normalize.</param>
/// <returns></returns>
public static PlaneVector Normalize(PlaneVector value)
{
float single = PlaneVector.Magnitude(value);
if (single <= 1E-05f)
{
return PlaneVector.zero;
}
return value / single;
}
/// <summary>
/// Initializes a new instance of the <see cref="VectorFieldCell"/> struct.
/// </summary>
/// <param name="portalIndex">Index of the portal.</param>
/// The plane vector defaults to a PlaneVector.zero (0f, 0f) in this case.
public VectorFieldCell(int portalIndex)
{
this.direction = PlaneVector.zero;
this.pathPortalIndex = portalIndex;
}
/// <summary>
/// Initializes a new instance of the <see cref="VectorFieldCell"/> struct.
/// </summary>
/// <param name="direction">The direction.</param>
/// The path portal index defaults to -1 in this case.
public VectorFieldCell(PlaneVector direction)
{
this.direction = direction;
this.pathPortalIndex = -1;
}
/// <summary>
/// Initializes a new instance of the <see cref="VectorFieldCell"/> struct.
/// </summary>
/// <param name="direction">The direction.</param>
/// <param name="portalIndex">Index of the portal.</param>
public VectorFieldCell(PlaneVector direction, int portalIndex)
{
this.direction = direction;
this.pathPortalIndex = portalIndex;
}
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;
}
}
public static float SqrMagnitude(PlaneVector a)
{
return((a.x * a.x) + (a.z * a.z));
}
/// <summary>
/// Returns the magnitude of a <see cref="PlaneVector"/>.
/// </summary>
/// <param name="a"></param>
/// <returns></returns>
public static float Magnitude(PlaneVector a)
{
return(Mathf.Sqrt((a.x * a.x) + (a.z * a.z)));
}
public static bool operator !=(PlaneVector lhs, PlaneVector rhs)
{
return(PlaneVector.SqrMagnitude(lhs - rhs) >= 9.99999944E-11f);
}
