public FastList(FastList <T> CopyList)
{
innerArray = (T[])CopyList.innerArray.Clone();
Count = innerArray.Length;
Capacity = innerArray.Length;
}
public void Enumerate(FastList <T> output)
{
output.FastClear();
output.AddRange(this);
}
public void CopyTo(FastList <T> target)
{
Array.Copy(innerArray, 0, target.innerArray, 0, Count);
target.Count = Count;
target.Capacity = Capacity;
}
public static bool FindPath(Vector2d Start, Vector2d End, FastList <GridNode> outputPath)
{
currentNode = GridManager.GetNode(Start.x, Start.y);
if (currentNode.Unwalkable)
{
//If the start node is unwalkable, attempt to locate the nearest walkable node
if (Start.x > currentNode.WorldPos.x)
{
IndexX = 1;
}
else
{
IndexX = -1;
}
if (Start.y > currentNode.WorldPos.y)
{
IndexY = 1;
}
else
{
IndexY = -1;
}
node1 = currentNode.NeighborNodes [GridNode.GetNeighborIndex(IndexX, IndexY)];
if (node1 == null || node1.Unwalkable)
{
node1 = currentNode.NeighborNodes[GridNode.GetNeighborIndex(IndexX, 0)];
if (node1 == null || node1.Unwalkable)
{
node1 = currentNode.NeighborNodes[GridNode.GetNeighborIndex(0, IndexY)];
if (node1 == null || node1.Unwalkable)
{
return(false);
}
}
}
}
else
{
node1 = currentNode;
}
currentNode = GridManager.GetNode(End.x, End.y);
if (currentNode.Unwalkable)
{
//If the start node is unwalkable, attempt to locate the nearest walkable node
if (End.x > currentNode.WorldPos.x)
{
IndexX = 1;
}
else
{
IndexX = -1;
}
if (End.y > currentNode.WorldPos.y)
{
IndexY = 1;
}
else
{
IndexY = -1;
}
node2 = currentNode.NeighborNodes [GridNode.GetNeighborIndex(IndexX, IndexY)];
if (node2 == null || node2.Unwalkable)
{
node2 = currentNode.NeighborNodes[GridNode.GetNeighborIndex(IndexX, 0)];
if (node2 == null || node2.Unwalkable)
{
node2 = currentNode.NeighborNodes[GridNode.GetNeighborIndex(0, IndexY)];
if (node2 == null || node2.Unwalkable)
{
return(false);
}
}
}
}
else
{
node2 = currentNode;
}
OutputPath = outputPath;
return(FindPath(node1, node2, OutputPath));
}
public static void ScanAll(Vector2d position, long radius, Func <LSAgent, bool> agentConditional, Func <byte, bool> bucketConditional, FastList <LSAgent> output)
{
output.FastClear();
int xMin = ((position.x - radius - GridManager.OffsetX) / (long)GridManager.ScanResolution).ToInt();
int xMax = ((position.x + radius - GridManager.OffsetX) / (long)GridManager.ScanResolution).CeilToInt();
int yMin = ((position.y - radius - GridManager.OffsetY) / (long)GridManager.ScanResolution).ToInt();
int yMax = ((position.y + radius - GridManager.OffsetY) / (long)GridManager.ScanResolution).CeilToInt();
long fastRadius = radius * radius;
for (int x = xMin; x <= xMax; x++)
{
for (int y = yMin; y <= yMax; y++)
{
ScanNode tempNode = GridManager.GetScanNode(
x,
y);
if (tempNode.IsNotNull())
{
if (tempNode.AgentCount > 0)
{
bufferBuckets.FastClear();
tempNode.GetBucketsWithAllegiance(bucketConditional, bufferBuckets);
for (int i = 0; i < bufferBuckets.Count; i++)
{
FastBucket <LSInfluencer> tempBucket = bufferBuckets[i];
BitArray arrayAllocation = tempBucket.arrayAllocation;
for (int j = 0; j < tempBucket.PeakCount; j++)
{
if (arrayAllocation.Get(j))
{
LSAgent tempAgent = tempBucket[j].Agent;
long distance = (tempAgent.Body.Position - position).FastMagnitude();
if (distance < fastRadius)
{
if (agentConditional(tempAgent))
{
output.Add(tempAgent);
}
}
else
{
}
}
}
}
}
}
}
}
}
public static bool FindPath(Vector2d End, GridNode startNode, GridNode endNode, FastList <Vector2d> outputVectorPath, int unitSize = 1)
{
if (FindPath(startNode, endNode, OutputPath, unitSize))
{
outputVectorPath.FastClear();
length = OutputPath.Count;
for (i = 0; i < length; i++)
{
outputVectorPath.Add(OutputPath[i].WorldPos);
}
outputVectorPath.Add(End);
return(true);
}
return(false);
}
/// <summary>
/// Finds a path and outputs it to <c>OutputPath</c>. Note: OutputPath is unpredictably changed.
/// </summary>
/// <returns>
/// Returns <c>true</c> if path was found and necessary, <c>false</c> if path to End is impossible or not found.
/// </returns>
/// <param name="startNode">Start node.</param>
/// <param name="endNode">End node.</param>
/// <param name="OutputPath">Return path.</param>
public static bool FindPath(GridNode startNode, GridNode endNode, FastList <GridNode> OutputPath)
{
#region Broadphase and Preperation
if (endNode.Unwalkable)
{
return(false);
}
if (startNode.Unwalkable)
{
return(false);
}
if (true)
{
#region Obstruction Test
//Tests if there is a direct path. If there is, no need to run AStar.
x0 = startNode.gridX;
y0 = startNode.gridY;
x1 = endNode.gridX;
y1 = endNode.gridY;
if (y1 > y0)
{
compare1 = y1 - y0;
}
else
{
compare1 = y0 - y1;
}
if (x1 > x0)
{
compare2 = x1 - x0;
}
else
{
compare2 = x0 - x1;
}
steep = compare1 > compare2;
if (steep)
{
t = x0; // swap x0 and y0
x0 = y0;
y0 = t;
t = x1; // swap x1 and y1
x1 = y1;
y1 = t;
}
if (x0 > x1)
{
t = x0; // swap x0 and x1
x0 = x1;
x1 = t;
t = y0; // swap y0 and y1
y0 = y1;
y1 = t;
}
dx = x1 - x0;
dy = (y1 - y0);
if (dy < 0)
{
dy = -dy;
}
error = dx / 2;
ystep = (y0 < y1) ? 1 : -1;
y = y0;
for (x = x0; x <= x1; x++)
{
retX = (steep ? y : x);
retY = (steep ? x : y);
if (GridManager.Grid [retX * GridManager.NodeCount + retY].Unwalkable)
{
break;
}
else if (x == x1)
{
OutputPath.FastClear();
OutputPath.Add(startNode);
OutputPath.Add(endNode);
return(true);
}
error = error - dy;
if (error < 0)
{
y += ystep;
error += dx;
}
}
#endregion
}
GridHeap.FastClear();
GridClosedSet.FastClear();
#endregion
#region AStar Algorithm
GridHeap.Add(startNode);
GridNode.HeuristicTargetX = endNode.gridX;
GridNode.HeuristicTargetY = endNode.gridY;
while (GridHeap.Count > 0)
{
currentNode = GridHeap.RemoveFirst();
GridClosedSet.Add(currentNode);
if (currentNode.gridIndex == endNode.gridIndex)
{
OutputPath.FastClear();
//Retraces the path then outputs it into OutputPath
//Also Simplifies the path
oldNode = endNode;
currentNode = endNode.parent;
oldX = int.MaxValue;
oldY = int.MaxValue;
StartNodeIndex = startNode.gridIndex;
//if (!endNode.Obstructed) OutputPath.Add (endNode);
while (oldNode.gridIndex != StartNodeIndex)
{
newX = currentNode.gridX - oldNode.gridX;
newY = currentNode.gridY - oldNode.gridY;
if ((newX != oldX || newY != oldY))
{
OutputPath.Add(oldNode);
oldX = newX;
oldY = newY;
}
oldNode = currentNode;
currentNode = currentNode.parent;
}
OutputPath.Add(startNode);
OutputPath.Reverse();
return(true);
}
for (i = 0; i < 8; i++)
{
neighbor = currentNode.NeighborNodes [i];
if (neighbor == null || neighbor.Unwalkable || GridClosedSet.Contains(neighbor))
{
continue;
}
newMovementCostToNeighbor = currentNode.gCost + (currentNode.NeighborDiagnal [i] ? 141 : 100);
if (!GridHeap.Contains(neighbor))
{
neighbor.gCost = newMovementCostToNeighbor;
//Optimized heuristic calculation
neighbor.CalculateHeurustic();
neighbor.parent = currentNode;
GridHeap.Add(neighbor);
}
else if (newMovementCostToNeighbor < neighbor.gCost)
{
neighbor.gCost = newMovementCostToNeighbor;
//Optimized heuristic calculation
neighbor.CalculateHeurustic();
neighbor.parent = currentNode;
GridHeap.UpdateItem(neighbor);
}
}
}
#endregion
return(false);
}
public static void ScanAll(Vector2d position, long radius, Func <LSAgent, bool> agentConditional, Func <byte, bool> bucketConditional, FastList <LSAgent> output)
{
//If radius is too big and we scan too many tiles, performance will be bad
const long circleCastRadius = FixedMath.One * 16;
output.FastClear();
if (radius >= circleCastRadius)
{
bufferBodies.FastClear();
PhysicsTool.CircleCast(position, radius, bufferBodies);
for (int i = 0; i < bufferBodies.Count; i++)
{
var body = bufferBodies [i];
var agent = body.Agent;
//we have to check agent's controller since we did not filter it through buckets
if (bucketConditional(agent.Controller.ControllerID))
{
if (agentConditional(agent))
{
output.Add(agent);
}
}
}
return;
}
int xMin = ((position.x - radius - GridManager.OffsetX) / (long)GridManager.ScanResolution).ToInt();
int xMax = ((position.x + radius - GridManager.OffsetX) / (long)GridManager.ScanResolution).CeilToInt();
int yMin = ((position.y - radius - GridManager.OffsetY) / (long)GridManager.ScanResolution).ToInt();
int yMax = ((position.y + radius - GridManager.OffsetY) / (long)GridManager.ScanResolution).CeilToInt();
long fastRadius = radius * radius;
for (int x = xMin; x <= xMax; x++)
{
for (int y = yMin; y <= yMax; y++)
{
ScanNode tempNode = GridManager.GetScanNode(
x,
y);
if (tempNode.IsNotNull())
{
if (tempNode.AgentCount > 0)
{
bufferBuckets.FastClear();
tempNode.GetBucketsWithAllegiance(bucketConditional, bufferBuckets);
for (int i = 0; i < bufferBuckets.Count; i++)
{
FastBucket <LSInfluencer> tempBucket = bufferBuckets[i];
BitArray arrayAllocation = tempBucket.arrayAllocation;
for (int j = 0; j < tempBucket.PeakCount; j++)
{
if (arrayAllocation.Get(j))
{
LSAgent tempAgent = tempBucket[j].Agent;
long distance = (tempAgent.Body.Position - position).FastMagnitude();
if (distance < fastRadius)
{
if (agentConditional(tempAgent))
{
output.Add(tempAgent);
}
}
else
{
}
}
}
}
}
}
}
}
}
public static bool FindPath(Vector2d End, GridNode startNode, GridNode endNode, FastList <Vector2d> outputVectorPath)
{
if (startNode.Unwalkable || endNode.Unwalkable)
{
return(false);
}
if (FindPath(startNode, endNode, OutputPath))
{
outputVectorPath.FastClear();
length = OutputPath.Count - 1;
for (i = 0; i < length; i++)
{
outputVectorPath.Add(OutputPath [i].WorldPos);
}
outputVectorPath.Add(End);
return(true);
}
return(false);
}
public Writer(FastList <byte> canvas)
{
this.Initialize(canvas);
}
/// <summary>
/// For re-useability
/// </summary>
/// <param name="canvas">Canvas.</param>
public void Initialize(FastList <byte> canvas)
{
canvas.FastClear();
Canvas = canvas;
}
static void HandleonDataDetailed(ushort sender, byte tag, ushort subject, object data)
{
ReceivedBytes = new FastList <byte> ((byte[])data);
}
/// <summary>
/// Finds a path and outputs it to <c>outputPath</c>. Note: outputPath is unpredictably changed.
/// </summary>
/// <returns>
/// Returns <c>true</c> if path was found and necessary, <c>false</c> if path to End is impossible or not found.
/// </returns>
/// <param name="startNode">Start node.</param>
/// <param name="endNode">End node.</param>
/// <param name="outputPath">Return path.</param>
public static bool FindPath(GridNode _startNode, GridNode _endNode, FastList <GridNode> _outputPath, int _unitSize = 1)
{
startNode = _startNode;
endNode = _endNode;
outputPath = _outputPath;
unitSize = _unitSize;
#region Broadphase and Preperation
if (endNode.Unwalkable)
{
return(false);
}
if (startNode.Unwalkable)
{
return(false);
}
outputPath.FastClear();
if (System.Object.ReferenceEquals(startNode, endNode))
{
outputPath.Add(endNode);
return(true);
}
GridHeap.FastClear();
GridClosedSet.FastClear();
#endregion
#region AStar Algorithm
GridHeap.Add(startNode);
GridNode.HeuristicTargetX = endNode.gridX;
GridNode.HeuristicTargetY = endNode.gridY;
GridNode.PrepareUnpassableCheck(unitSize); //Prepare Unpassable check optimizations
while (GridHeap.Count > 0)
{
currentNode = GridHeap.RemoveFirst();
GridClosedSet.Add(currentNode);
if (currentNode.gridIndex == endNode.gridIndex)
{
//Retraces the path then outputs it into outputPath
//Also Simplifies the path
DestinationReached();
return(true);
}
for (i = 0; i < 8; i++)
{
neighbor = currentNode.NeighborNodes [i];
if (neighbor.IsNull() || currentNode.Unpassable() || GridClosedSet.Contains(neighbor))
{
continue;
}
//0-3 = sides, 4-7 = diagonals
if (i < 4)
{
newMovementCostToNeighbor = currentNode.gCost + 100;
}
else
{
if (i == 4)
{
if (!GridManager.UseDiagonalConnections)
{
break;
}
}
newMovementCostToNeighbor = currentNode.gCost + 141;
}
if (!GridHeap.Contains(neighbor))
{
neighbor.gCost = newMovementCostToNeighbor;
//Optimized heuristic calculation
neighbor.CalculateHeuristic();
neighbor.parent = currentNode;
GridHeap.Add(neighbor);
}
else if (newMovementCostToNeighbor < neighbor.gCost)
{
neighbor.gCost = newMovementCostToNeighbor;
//Optimized heuristic calculation
neighbor.CalculateHeuristic();
neighbor.parent = currentNode;
GridHeap.UpdateItem(neighbor);
}
}
}
#endregion
return(false);
}
public void Initialize(Command com)
{
Destination = com.GetData <Vector2d> ();;
calculatedBehaviors = false;
movers = new FastList <Move>(com.GetData <Selection>().selectedAgentLocalIDs.Count);
}
public Writer(FastList <byte> Canvas)
{
canvas = Canvas;
}
/// <summary>
/// Finds a path and outputs it to <c>outputPath</c>. Note: outputPath is unpredictably changed.
/// </summary>
/// <returns>
/// Returns <c>true</c> if path was found and necessary, <c>false</c> if path to End is impossible or not found.
/// </returns>
/// <param name="startNode">Start node.</param>
/// <param name="endNode">End node.</param>
/// <param name="outputPath">Return path.</param>
public static bool FindPath(GridNode startNode, GridNode endNode, FastList <GridNode> outputPath)
{
#region Broadphase and Preperation
if (endNode.Unwalkable)
{
return(false);
}
if (startNode.Unwalkable)
{
return(false);
}
outputPath.FastClear();
if (System.Object.ReferenceEquals(startNode, endNode))
{
outputPath.Add(endNode);
return(true);
}
System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
sw.Start();
GridHeap.FastClear();
GridClosedSet.FastClear();
#endregion
#region AStar Algorithm
GridHeap.Add(startNode);
GridNode.HeuristicTargetX = endNode.gridX;
GridNode.HeuristicTargetY = endNode.gridY;
while (GridHeap.Count > 0)
{
currentNode = GridHeap.RemoveFirst();
GridClosedSet.Add(currentNode);
if (currentNode.gridIndex == endNode.gridIndex)
{
//Retraces the path then outputs it into outputPath
//Also Simplifies the path
outputPath.FastClear();
TracePath.FastClear();
currentNode = endNode;
StartNodeIndex = startNode.gridIndex;
while (currentNode.gridIndex != StartNodeIndex)
{
TracePath.Add(currentNode);
oldNode = currentNode;
currentNode = currentNode.parent;
}
oldNode = startNode;
currentNode = TracePath [TracePath.Count - 1];
oldX = currentNode.gridX - oldNode.gridX;
oldY = currentNode.gridY - oldNode.gridY;
for (i = TracePath.Count - 2; i >= 0; i--)
{
oldNode = currentNode;
currentNode = TracePath.innerArray [i];
newX = currentNode.gridX - oldNode.gridX;
newY = currentNode.gridY - oldNode.gridY;
if (newX != oldX || newY != oldY)
{
outputPath.Add(oldNode);
oldX = newX;
oldY = newY;
}
//outputPath.Add (currentNode);
}
outputPath.Add(endNode);
return(true);
}
for (i = 0; i < 8; i++)
{
neighbor = currentNode.NeighborNodes [i];
if (neighbor == null || neighbor.Unwalkable || GridClosedSet.Contains(neighbor))
{
continue;
}
newMovementCostToNeighbor = currentNode.gCost + (GridNode.IsNeighborDiagnal [i] ? 141 : 100);
if (!GridHeap.Contains(neighbor))
{
neighbor.gCost = newMovementCostToNeighbor;
//Optimized heuristic calculation
neighbor.CalculateHeuristic();
neighbor.parent = currentNode;
GridHeap.Add(neighbor);
}
else if (newMovementCostToNeighbor < neighbor.gCost)
{
neighbor.gCost = newMovementCostToNeighbor;
//Optimized heuristic calculation
neighbor.CalculateHeuristic();
neighbor.parent = currentNode;
GridHeap.UpdateItem(neighbor);
}
}
}
#endregion
return(false);
}
public bool Overlaps(FastList <Vector2d> outputIntersectionPoints)
{
outputIntersectionPoints.FastClear();
//Checks if this object overlaps the line formed by p1 and p2
switch (this.Shape)
{
case ColliderType.Circle:
{
bool overlaps = false;
//Check if the circle completely fits between the line
long projPos = this._position.Dot(cacheAxis.x, cacheAxis.y);
//Circle withing bounds?
if (projPos >= axisMin && projPos <= axisMax)
{
long projPerp = this._position.Dot(cacheAxisNormal.x, cacheAxisNormal.y);
long perpDif = (cacheProjPerp - projPerp);
long perpDist = perpDif.Abs();
if (perpDist <= _radius)
{
overlaps = true;
}
if (overlaps)
{
long sin = (perpDif);
long cos = FixedMath.Sqrt(_radius.Mul(_radius) - sin.Mul(sin));
if (cos == 0)
{
outputIntersectionPoints.Add((cacheAxis * projPos) + perpVector);
}
else
{
outputIntersectionPoints.Add(cacheAxis * (projPos - cos) + perpVector);
outputIntersectionPoints.Add(cacheAxis * (projPos + cos) + perpVector);
}
}
}
else
{
//If not, check distances to points
long p1Dist = _position.FastDistance(cacheP1.x, cacheP2.y);
if (p1Dist <= this.FastRadius)
{
outputIntersectionPoints.Add(cacheP1);
overlaps = true;
}
long p2Dist = _position.FastDistance(cacheP2.x, cacheP2.y);
if (p2Dist <= this.FastRadius)
{
outputIntersectionPoints.Add(cacheP2);
overlaps = true;
}
}
return(overlaps);
}
//break;
case ColliderType.AABox:
{
}
break;
case ColliderType.Polygon:
{
bool intersected = false;
for (int i = 0; i < this.Vertices.Length; i++)
{
int edgeIndex = i;
Vector2d pivot = this.RealPoints[edgeIndex];
Vector2d edge = this.Edges[edgeIndex];
long proj1 = 0;
int nextIndex = edgeIndex + 1 < this.RealPoints.Length ? edgeIndex + 1 : 0;
Vector2d nextPoint = RealPoints[nextIndex];
long proj2 = (nextPoint - pivot).Dot(edge);
long min;
long max;
if (proj1 < proj2)
{
min = proj1;
max = proj2;
}
else
{
min = proj2;
max = proj1;
}
long lineProj1 = (cacheP1 - pivot).Dot(edge);
long lineProj2 = (cacheP2 - pivot).Dot(edge);
long lineMin;
long lineMax;
if (lineProj1 < lineProj2)
{
lineMin = lineProj1;
lineMax = lineProj2;
}
else
{
lineMin = lineProj2;
lineMax = lineProj1;
}
if (CollisionPair.CheckOverlap(min, max, lineMin, lineMax))
{
Vector2d edgeNorm = this.EdgeNorms[edgeIndex];
long normProj = 0;
long normLineProj1 = (cacheP1 - pivot).Dot(edgeNorm);
long normLineProj2 = (cacheP2 - pivot).Dot(edgeNorm);
long normLineMin;
long normLineMax;
if (normLineProj1 < normLineProj2)
{
normLineMin = normLineProj1;
normLineMax = normLineProj2;
}
else
{
normLineMin = normLineProj2;
normLineMax = normLineProj1;
}
if (normProj >= normLineMin && normProj <= normLineMax)
{
long revProj1 = pivot.Dot(LSBody.cacheAxisNormal);
long revProj2 = nextPoint.Dot(cacheAxisNormal);
long revMin;
long revMax;
if (revProj1 < revProj2)
{
revMin = revProj1;
revMax = revProj2;
}
else
{
revMin = revProj2;
revMax = revProj1;
}
if (LSBody.cacheProjPerp >= revMin && LSBody.cacheProjPerp <= revMax)
{
intersected = true;
if (LSBody.calculateIntersections)
{
long fraction = normLineProj1.Abs().Div(normLineMax - normLineMin);
long intersectionProj = FixedMath.Lerp(lineProj1, lineProj2, fraction);
outputIntersectionPoints.Add(edge * intersectionProj + pivot);
if (outputIntersectionPoints.Count == 2)
{
break;
}
}
}
}
}
}
return(intersected);
}
//break;
}
return(false);
}
public Writer()
{
Canvas = new FastList <byte>();
}
