public void PulsePoint(int index)
{
if (AStarHelper.Invalid(sources[index]))
{
return;
}
DrawHelper.DrawCube(sources[index].Position, Vector3.one * 2.0f, pulseColor);
}
static int Closest(List <PathNode> inNodes, Vector3 toPoint)
{
int closestIndex = 0;
float minDist = float.MaxValue;
for (int i = 0; i < inNodes.Count; i++)
{
if (AStarHelper.Invalid(inNodes[i]))
{
continue;
}
float thisDist = Vector3.Distance(toPoint, inNodes[i].Position);
if (thisDist > minDist)
{
continue;
}
minDist = thisDist;
closestIndex = i;
}
return(closestIndex);
}
// Calculate the A* path
public static List <T> Calculate <T>(T start, T goal) where T : IPathNode <T>
{
List <T> closedset = new List <T>(); // The set of nodes already evaluated.
List <T> openset = new List <T>(); // The set of tentative nodes to be evaluated.
openset.Add(start);
Dictionary <T, T> came_from = new Dictionary <T, T>(); // The map of navigated nodes.
Dictionary <T, float> g_score = new Dictionary <T, float>();
g_score[start] = 0.0f; // Cost from start along best known path.
Dictionary <T, float> h_score = new Dictionary <T, float>();
h_score[start] = HeuristicCostEstimate(start, goal);
Dictionary <T, float> f_score = new Dictionary <T, float>();
f_score[start] = h_score[start]; // Estimated total cost from start to goal through y.
while (openset.Count != 0)
{
T x = LowestScore(openset, f_score);
if (x.Equals(goal))
{
List <T> result = new List <T>();
ReconstructPath(came_from, x, ref result);
return(result);
}
openset.Remove(x);
closedset.Add(x);
foreach (T y in x.Connections)
{
if (AStarHelper.Invalid(y) || closedset.Contains(y))
{
continue;
}
float tentative_g_score = g_score[x] + Distance(x, y);
bool tentative_is_better = false;
if (!openset.Contains(y))
{
openset.Add(y);
tentative_is_better = true;
}
else if (tentative_g_score < g_score[y])
{
tentative_is_better = true;
}
if (tentative_is_better)
{
came_from[y] = x;
g_score[y] = tentative_g_score;
h_score[y] = HeuristicCostEstimate(y, goal);
f_score[y] = g_score[y] + h_score[y];
}
}
}
return(null);
}
public void Update()
{
if (reset)
{
donePath = false;
ArrayFunc.Clear(ref solvedPath);
reset = false;
}
if (start == null || end == null)
{
Debug.LogWarning("Need 'start' and or 'end' defined!");
enabled = false;
return;
}
startIndex = Closest(sources, start.transform.position);
endIndex = Closest(sources, end.transform.position);
if (startIndex != lastStartIndex || endIndex != lastEndIndex)
{
reset = true;
lastStartIndex = startIndex;
lastEndIndex = endIndex;
return;
}
for (int i = 0; i < sources.Count; i++)
{
if (AStarHelper.Invalid(sources[i]))
{
continue;
}
sources[i].nodeColor = nodeColor;
}
PulsePoint(lastStartIndex);
PulsePoint(lastEndIndex);
if (!donePath)
{
solvedPath = AStarHelper.Calculate(sources[lastStartIndex], sources[lastEndIndex]);
donePath = true;
}
// Invalid path
if (solvedPath == null || solvedPath.Count < 1)
{
Debug.LogWarning("Invalid path!");
reset = true;
enabled = false;
return;
}
//Draw path
for (int i = 0; i < solvedPath.Count - 1; i++)
{
if (AStarHelper.Invalid(solvedPath[i]) || AStarHelper.Invalid(solvedPath[i + 1]))
{
reset = true;
return;
}
Debug.DrawLine(solvedPath[i].Position, solvedPath[i + 1].Position, Color.cyan * new Color(1.0f, 1.0f, 1.0f, 0.5f));
}
}
public static List <PathNode> CreateGrid(Vector3 center, Vector3 spacing, int[] dim, float randomSpace)
{
GameObject groupObject = new GameObject("grid");
Random.seed = 1337;
int xCount = dim[0];
int yCount = dim[1];
float xWidth = spacing.x * xCount;
float yWidth = spacing.z * yCount;
float xStart = center.x - (xWidth / 2.0f) + (spacing.x / 2.0f);
float yStart = center.z - (yWidth / 2.0f) + (spacing.z / 2.0f);
List <PathNode> result = new List <PathNode>();
for (int x = 0; x < xCount; x++)
{
float xPos = (x * spacing.x) + xStart;
for (int y = 0; y < yCount; y++)
{
if (randomSpace > 0.0f)
{
if (Random.value <= randomSpace)
{
result.Add(null);
continue;
}
}
float yPos = (y * spacing.z) + yStart;
PathNode newNode = Spawn(new Vector3(xPos, 0.0f, yPos));
result.Add(newNode);
newNode.transform.parent = groupObject.transform;
}
}
for (int x = 0; x < xCount; x++)
{
for (int y = 0; y < yCount; y++)
{
int thisIndex = (x * yCount) + y;
List <int> connectedIndicies = new List <int>();
PathNode thisNode = result[thisIndex];
if (AStarHelper.Invalid(thisNode))
{
continue;
}
if (x != 0)
{
connectedIndicies.Add(((x - 1) * yCount) + y);
}
if (x != xCount - 1)
{
connectedIndicies.Add(((x + 1) * yCount) + y);
}
if (y != 0)
{
connectedIndicies.Add((x * yCount) + (y - 1));
}
if (y != yCount - 1)
{
connectedIndicies.Add((x * yCount) + (y + 1));
}
if (x != 0 && y != 0)
{
connectedIndicies.Add(((x - 1) * yCount) + (y - 1));
}
if (x != xCount - 1 && y != yCount - 1)
{
connectedIndicies.Add(((x + 1) * yCount) + (y + 1));
}
if (x != 0 && y != yCount - 1)
{
connectedIndicies.Add(((x - 1) * yCount) + (y + 1));
}
if (x != xCount - 1 && y != 0)
{
connectedIndicies.Add(((x + 1) * yCount) + (y - 1));
}
for (int i = 0; i < connectedIndicies.Count; i++)
{
PathNode thisConnection = result[connectedIndicies[i]];
if (AStarHelper.Invalid(thisConnection))
{
continue;
}
thisNode.Connections.Add(thisConnection);
}
}
}
return(result);
}
