/// <summary>
/// Finds a path from any of the points in startPointList to goal, such that the path has no links that are steeper up than maxIncline
/// nor steeper down than maxDecline.
/// </summary>
/// <returns>The path.</returns>
/// <param name="startPointList">PointWithDistance structs for starting area</param>
/// <param name="goalPoint">map indices of point to go to</param>
List <PointWithDistance> FindPath(List <PointWithDistance> startPointList, InclineIndexPoint goalPoint, float maxIncline, float maxDecline)
{
List <PointWithDistance> path = new List <PointWithDistance>();
HeapQueue <PointWithDistance> openHeap = new HeapQueue <PointWithDistance>();
Dictionary <InclineIndexPoint, float> bestDistanceDict = new Dictionary <InclineIndexPoint, float>();
Dictionary <InclineIndexPoint, PointWithDistance> bestPrevPoint = new Dictionary <InclineIndexPoint, PointWithDistance>();
Point goalMapPoint = InclineIndicesToMapIndices(goalPoint);
Vector3 worldGoalPoint = mapMetaData.getWorldPos(goalMapPoint);
for (int startIndex = 0; startIndex < startPointList.Count; ++startIndex)
{
PointWithDistance pwd = startPointList[startIndex];
openHeap.Push(pwd);
bestDistanceDict[pwd.point] = pwd.distance;
bestPrevPoint[pwd.point] = null;
}
float bestPathLength = -1;
PointWithDistance bestGoalPoint = new PointWithDistance(new InclineIndexPoint(-1024, -1024),
float.MaxValue,
float.MaxValue);
while (!openHeap.IsEmpty())
{
PointWithDistance ptWithDist = openHeap.PopMinimum();
if ((bestPathLength > 0) && ((ptWithDist.estimatedTotalDistance > bestPathLength) || TAKE_FIRST_PATH))
{
break;
}
int[] xOffsets = { 1, 0, -1, 0 };
int[] zOffsets = { 0, 1, 0, -1 };
InclineMeshNode node = nodes[ptWithDist.point.X, ptWithDist.point.Z];
Vector3 worldNodePoint = InclineIndicesToWorldPoint(ptWithDist.point);
for (int direction = 0; direction < 4; ++direction)
{
int dx = xOffsets[direction];
int dz = zOffsets[direction];
int nx = ptWithDist.point.X + dx;
int nz = ptWithDist.point.Z + dz;
InclineIndexPoint neighborPoint = new InclineIndexPoint(nx, nz);
Point mapNeighborPoint = InclineIndicesToMapIndices(neighborPoint);
if ((!mapMetaData.IsWithinBounds(mapNeighborPoint)) || (node.NeighborLinks[direction] == null))
{
continue;
}
Vector3 worldNeighborPoint = InclineIndicesToWorldPoint(neighborPoint);
for (int linkIndex = 0; linkIndex < node.NeighborLinks[direction].Count; ++linkIndex)
{
Debug.DrawLine(worldNodePoint, worldNeighborPoint, Color.yellow, 15.0f);
InclineLinkData link = node.NeighborLinks[direction][linkIndex];
if ((link.declineAsFloat() > maxDecline) || (link.inclineAsFloat() > maxIncline))
{
continue;
}
float linkDistance = (worldNeighborPoint - worldNodePoint).magnitude;
float totalDistance = ptWithDist.distance + linkDistance;
if ((bestPathLength >= 0) &&
(totalDistance >= bestPathLength))
{
continue;
}
if ((!bestDistanceDict.ContainsKey(neighborPoint)) ||
(totalDistance < bestDistanceDict[neighborPoint]))
{
bestDistanceDict[neighborPoint] = totalDistance;
bestPrevPoint[neighborPoint] = ptWithDist;
float distanceToGoal = (worldNeighborPoint - worldGoalPoint).magnitude;
if (neighborPoint.Equals(goalPoint))
{
if ((bestPathLength < 0) ||
(totalDistance < bestPathLength))
{
bestPathLength = totalDistance;
bestGoalPoint = new PointWithDistance(neighborPoint, totalDistance, 0.0f);
}
}
else
{
openHeap.Push(new PointWithDistance(neighborPoint, totalDistance, totalDistance + distanceToGoal));
}
}
break;
}
}
}
if (bestPathLength >= 0)
{
PointWithDistance p = bestGoalPoint;
path.Add(p);
while (bestPrevPoint.ContainsKey(p.point))
{
PointWithDistance prevPoint = bestPrevPoint[p.point];
if ((prevPoint == null) || (path.Contains(prevPoint)))
{
break;
}
path.Insert(0, prevPoint);
p = prevPoint;
}
}
return(path);
}
void processNode(InclineMeshData meshData, MapMetaData mapMetaData, InclineIndexPoint inclinePoint)
{
Point startPointMapIndices = meshData.InclineIndicesToMapIndices(inclinePoint);
if (!mapMetaData.IsWithinBounds(startPointMapIndices))
{
return;
}
Dictionary <Point, List <InclineLinkData> > bestPaths = new Dictionary <Point, List <InclineLinkData> >();
List <InclineLinkData> starterList = new List <InclineLinkData>();
starterList.Add(new InclineLinkData());
bestPaths[startPointMapIndices] = starterList;
List <Point> openPoints = new List <Point>();
openPoints.Add(startPointMapIndices);
float maximumPathDistance = overshootMultiplier * getDistanceBetweenTwoInclinePoints(meshData, mapMetaData);
while (openPoints.Count > 0)
{
Point p = openPoints[0];
openPoints.RemoveAt(0);
//Debug.LogFormat("dequeueing {0} {1}", p.X, p.Z);
for (int d = 0; d < 4; ++d)
{
int dx = 0;
int dz = 0;
switch (d)
{
case 0: dx = 1; dz = 0; break;
case 1: dx = 0; dz = 1; break;
case 2: dx = -1; dz = 0; break;
case 3: dx = 0; dz = -1; break;
default: Debug.LogError("invalid direction: " + d); continue;
}
Point newPoint = new Point(p.X + dx, p.Z + dz);
if ((!mapMetaData.IsWithinBounds(newPoint)) ||
(!mapMetaData.IsWithinBounds(p)))
{
continue;
}
float incline, decline, distance;
getIncrementalInclines(mapMetaData, p, newPoint, out incline, out decline, out distance);
bool pointIsDirty = false;
for (int pathIndex = 0; pathIndex < bestPaths[p].Count; pathIndex++)
{
InclineLinkData pathSoFar = bestPaths[p][pathIndex];
float oldFloatIncline = pathSoFar.inclineAsFloat();
float oldFloatDecline = pathSoFar.declineAsFloat();
float oldFloatDistance = pathSoFar.distanceAsFloat();
float newFloatIncline = Mathf.Max(incline, oldFloatIncline);
float newFloatDecline = Mathf.Max(decline, oldFloatDecline);
float newFloatDistance = oldFloatDistance + distance;
// if the distance is too far, also stop recursing.
if (newFloatDistance > maximumPathDistance)
{
continue;
}
InclineLinkData newPathLinkData = new InclineLinkData(newFloatIncline, newFloatDecline, newFloatDistance);
bool foundAnyBetter = false;
int iAmBetterThanThisIndex = -1;
if (bestPaths.ContainsKey(newPoint))
{
for (int existingPathIndex = 0; existingPathIndex < bestPaths[newPoint].Count; ++existingPathIndex)
{
InclineLinkData existingPath = bestPaths[newPoint][existingPathIndex];
// If an existing path is as good or better than us, then stop recursing.
if (existingPath.Equals(newPathLinkData) ||
existingPath.Dominates(newPathLinkData))
{
foundAnyBetter = true;
break;
}
if (newPathLinkData.Dominates(existingPath))
{
iAmBetterThanThisIndex = existingPathIndex;
break;
}
}
}
else
{
bestPaths[newPoint] = new List <InclineLinkData>();
}
if (iAmBetterThanThisIndex >= 0)
{
// found a better path than an existing one, remove the old one and recompute.
bestPaths[newPoint][iAmBetterThanThisIndex] = newPathLinkData;
pointIsDirty = true;
}
else if (!foundAnyBetter)
{
// Otherwise, add this path to the list of paths, and recurse.
bestPaths[newPoint].Add(newPathLinkData);
pointIsDirty = true;
}
}
if (pointIsDirty)
{
openPoints.Add(newPoint);
}
}
}
meshData.nodes[inclinePoint.X, inclinePoint.Z] = new InclineMeshNode();
// now, grab the links from our neighbors
for (int d = 0; d < 4; ++d)
{
int dx = 0;
int dz = 0;
switch (d)
{
case 0: dx = 1; dz = 0; break;
case 1: dx = 0; dz = 1; break;
case 2: dx = -1; dz = 0; break;
case 3: dx = 0; dz = -1; break;
default: Debug.LogError("invalid direction: " + d); continue;
}
InclineIndexPoint neighborInclinePoint = new InclineIndexPoint(inclinePoint.X + dx, inclinePoint.Z + dz);
Point neighborMapIndices = meshData.InclineIndicesToMapIndices(neighborInclinePoint);
if (!mapMetaData.IsWithinBounds(neighborMapIndices))
{
continue;
}
if (bestPaths.ContainsKey(neighborMapIndices))
{
/*
* foreach (InclineLinkData ild in bestPaths[neighborWorldPoint])
* {
* //Debug.LogFormat("from {0} {1} in dir {2}: {3}/{4}/{5}", inclinePoint.X, inclinePoint.Z, d, ild.incline, ild.decline, ild.distance);
* }
*/
meshData.nodes[inclinePoint.X, inclinePoint.Z].NeighborLinks[d] = bestPaths[neighborMapIndices];
// draw debug
//Debug.DrawLine(meshData.InclineIndicesToWorldPoint(inclinePoint), meshData.mapMetaData.getWorldPos(neighborMapIndices), Color.white, 25.0f);
}
}
}
