public void Read(BinaryReader br) { NeighborLinks = new List <InclineLinkData> [4]; for (int direction = 0; direction < 4; ++direction) { int linkCount = br.ReadInt32(); if (linkCount == 0) { NeighborLinks [direction] = null; } else { NeighborLinks [direction] = new List <InclineLinkData> (linkCount); for (int linkIndex = 0; linkIndex < linkCount; ++linkIndex) { InclineLinkData linkData = new InclineLinkData(); linkData.Read(br); NeighborLinks[direction].Add(linkData); } } } }
/// <summary> /// Returns true if the existing path is as good or better than the new path - that is, if the existing path's /// upwards steepness, downward steepness, and distance are each equal or less than the new path's steepnesses /// and distance. /// </summary> /// <param name="existingPath">existing path</param> /// <param name="newPath">new path</param> bool isExistingPathAsGoodOrBetterThan(InclineLinkData existingPath, InclineLinkData newPath) { if ((existingPath.decline > newPath.decline) || (existingPath.incline > newPath.incline) || (existingPath.distance > newPath.distance)) { return(false); } return(true); }
/// <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); } } }