A* pathfinding strange behavior

Hi All,

I am implementing a simple A* pathfinding algorithm (not much different from the simplified version found on wikipedia) and I have some really strange behaviors.

Most of the time it seems to work quite well, as you can see in the first pic (the sphere is the test start and the cube is the arrival). This path is not supposed to follow the roads, so it is correct apparently.


In another cases, specially if the path has to change slightly more often direction (see the following pics)


it fails to recreate the full walked path (the one being drawn with the white dots). I tried to draw the full searched nodes path, and that once seems to be instead correct (next pic, same identical positions and algo as the previous pic)


There must therefore be something really stupid that is happening. Considering that the path backwards reconstruction stops when i reach a grid cells whose parent has X and Y equal to the starting point, i think for some reason, it happens "by the bridge", but after 2 hours of debugging it, i feel more stupid but i couldn't get to see the problem.

Here's the code of the full test class. Any help or hint is more than welcome! Thanks in advance,
Best,
H

Code (CSharp):

1. using UnityEngine;
2. using System.Collections;
3. using System.Collections.Generic;
4. using System.Linq;
5.  
6. public class CellGrid {
7.     public int Y;
8.     public int X;
9.     public int value;
10.     public float mod;
11.  
12.     public CellGrid(int y, int x, int val){
13.         this.Y=y;
14.         this.X=x;
15.         this.value=val;
16.         if(value==0){
17.             this.mod = 1.3f;
18.         }else{
19.             this.mod = 1f;
20.         }
21.     }
22. }
23.  
24. public class AStarTest : MonoBehaviour {
25.    
26.     MapManager mainMapController;
27.  
28.     int[,] grid;
29.     int[] gridPos;
30.     int minRowsGrid;
31.     int minColsGrid;
32.     int maxRowsGrid;
33.     int maxColsGrid;
34.  
35.     public List<CellGrid> pathToTarget;
36.  
37.     CellGrid startPoint;
38.     CellGrid arrivalPoint;
39.  
40.     List<CellGrid> closedSet;
41.     List<CellGrid> openSet;
42.     List<CellGrid> map;
43.     Dictionary<CellGrid,float> gScore;
44.     Dictionary<CellGrid,float> fScore;
45.     Dictionary<CellGrid,CellGrid> comeFrom;
46.     float[] snappedCoords;
47.  
48.     // Use this for initialization
49.     void Start () {
50.         snappedCoords = new float[2];
51.         gridPos = new int[2];
52.         mainMapController=GameObject.Find("MapManager").GetComponent<MapManager>();
53.         grid=mainMapController.grid;
54.         pathToTarget = new List<CellGrid>();
55.  
56.         minRowsGrid = grid.GetLowerBound(0);
57.         minColsGrid = grid.GetLowerBound(1);
58.         maxRowsGrid = grid.GetUpperBound(0);
59.         maxColsGrid = grid.GetUpperBound(1);
60.  
61.         mainMapController.gridController.swapCoordToGrid(this.transform.position.y,this.transform.position.x,gridPos);
62.         mainMapController.gridController.swapGridToCoor(gridPos[0],gridPos[1],snappedCoords);
63.  
64.         float xArr = GameObject.Find("ArrivalPath").transform.position.x;
65.         float yArr = GameObject.Find("ArrivalPath").transform.position.y;
66.         int[] arrGrid = new int[2];
67.         mainMapController.gridController.swapCoordToGrid(yArr,xArr,arrGrid);
68.  
69.         startPoint = new CellGrid(gridPos[0],gridPos[1],grid[gridPos[0],gridPos[1]]);
70.         arrivalPoint = new CellGrid(arrGrid[0],arrGrid[1],grid[arrGrid[0],arrGrid[1]]);
71.  
72.         Debug.Log("Starting point : "+startPoint.Y+","+startPoint.X+" - "+startPoint.value);
73.         Debug.Log("Arrival point : "+arrivalPoint.Y+","+arrivalPoint.X+" - "+arrivalPoint.value);
74.  
75.         closedSet = new List<CellGrid>();
76.         openSet = new List<CellGrid>();
77.         map = new List<CellGrid>();
78.         gScore = new Dictionary<CellGrid, float>();
79.         fScore = new Dictionary<CellGrid, float>();
80.         comeFrom = new Dictionary<CellGrid, CellGrid>();
81.  
82.         if(startPoint.value==0 || startPoint.value==1 || startPoint.value==4){
83.             if(arrivalPoint.value==0 || arrivalPoint.value==1 || arrivalPoint.value==4){
84.                 pathToTarget = SeekPath(startPoint,arrivalPoint);
85.             }
86.             else{
87.                 Debug.Log("Arrival unreachable");
88.             }
89.         }else{
90.             Debug.Log("Problem start");
91.         }
92.         if(pathToTarget.Count>0){
93.             Debug.Log("Path length : "+pathToTarget.Count);
94.             foreach(CellGrid elem in pathToTarget){
95.                 Debug.Log(elem.Y+" - "+elem.X);
96.             }
97.         }
98.  
99.     }
100.  
101.     //get the node with the best fscore in the openset
102.     public CellGrid GetBestNode(List<CellGrid> set){
103.         float cost = 1000f;
104.         int index = 0;
105.         for(int i=0;i<set.Count;i++){
106.             if(fScore[set[i]]<cost){
107.                 cost=fScore[set[i]];
108.                 index=i;
109.             }
110.         }
111.         return set[index];
112.     }
113.  
114.     //calculate euclidean distance between two cell grids
115.     public float DistanceNodes(CellGrid nodeA, CellGrid nodeB){
116.         return Mathf.Sqrt(Mathf.Pow((Mathf.Abs(nodeB.Y)-Mathf.Abs(nodeA.Y)),2)+Mathf.Pow((Mathf.Abs(nodeB.X)-Mathf.Abs(nodeA.X)),2));
117.     }
118.  
119.     //given a grid cell return a list with all the neighbors that have not yet been analyzed
120.     public List<CellGrid> GetNeighbors(CellGrid current){
121.         List<CellGrid> neighbors = new List<CellGrid>();
122.         int nodeX = current.X;
123.         int nodeY = current.Y;
124.         float dist;
125.         for(int i=-1;i<=1;i++){
126.             for(int j=-1;j<=1;j++){
127.                 if((j!=0 || i!=0) && (nodeX+j)>=0 && (nodeX+j)<=maxColsGrid && (nodeY+i)>=0 && (nodeY+i)<=maxRowsGrid){
128.                     CellGrid neigCell = new CellGrid(nodeY+i,nodeX+j,grid[nodeY+i,nodeX+j]);
129.                     if(neigCell.value==0 || neigCell.value==1 || neigCell.value==4){
130.                         dist=DistanceNodes(neigCell,arrivalPoint);
131.                         fScore.Add(neigCell,dist);
132.                         gScore.Add(neigCell,DistanceNodes(neigCell,current));
133.                         if(!SetContains(neigCell,openSet)){
134.                             neighbors.Add(neigCell);
135.                         }
136.                     }
137.                 }
138.             }
139.         }
140.         return neighbors;
141.     }
142.  
143.     //generate the path between start and arrival
144.     public List<CellGrid> SeekPath(CellGrid start, CellGrid arrival){
145.         openSet.Add (start);
146.         gScore[start]=0f;
147.         fScore[start]=gScore[start]+DistanceNodes(start,arrival);
148.         while(openSet.Count>0){
149.             CellGrid curr = GetBestNode(openSet);
150.             if (curr.X==arrival.X && curr.Y==arrival.Y){
151.                 Debug.Log("Path found, reconstructing it...");
152.                 return ReconstructPath(curr,start);
153.                 break;
154.             }
155.             openSet.Remove(curr);
156.             closedSet.Add(curr);
157.             List<CellGrid> neighbors = GetNeighbors(curr);
158.             foreach(CellGrid neighbor in neighbors){
159.                 if(!SetContains(neighbor,closedSet)){
160.                     float tentativeGScore;
161.                     tentativeGScore = gScore[curr]+DistanceNodes(neighbor,curr);
162.                     if(!SetContains(neighbor,openSet)){
163.                         openSet.Add(neighbor);
164.                         comeFrom[neighbor]=curr;
165.                         if(tentativeGScore<gScore[neighbor]){
166.                             gScore[neighbor]=tentativeGScore;
167.                             fScore[neighbor]=gScore[neighbor]+DistanceNodes(neighbor,arrival);
168.                         }
169.                     }
170.                 }
171.             }
172.         }
173.         return null;
174.     }
175.  
176.     //check if a set (openset or closedset) contains the given Cellgrid Element
177.     public bool SetContains(CellGrid elemToCheck, List<CellGrid> set){
178.         foreach(CellGrid elem in set){
179.             if(elem.Y==elemToCheck.Y && elem.X==elemToCheck.X){
180.                 return true;
181.             }
182.         }
183.         return false;
184.     }
185.  
186.     //reconstruct backwards the correct path from the arrival point to the starting point
187.     public List<CellGrid> ReconstructPath(CellGrid lastElem, CellGrid startElem){
188.         List<CellGrid> path = new List<CellGrid>();
189.         path.Add(lastElem);
190.         CellGrid comingFrom = comeFrom[lastElem];
191.         while(comingFrom.X!=startElem.X && comingFrom.Y!=startElem.Y){
192.                 //PRINT DEBUG STARTS TO VIEW THE WALKABLE PATH
193.                 GameObject testPath = (GameObject)Resources.Load("GUI/TestPath") as GameObject;
194.                 mainMapController.gridController.swapGridToCoor(comingFrom.Y,comingFrom.X,snappedCoords);
195.                 GameObject insTest = (GameObject)Instantiate(testPath,new Vector2(snappedCoords[1],snappedCoords[0]),Quaternion.identity);
196.                 insTest.GetComponentInChildren<TextMesh>().text=" * ";
197.                 path.Add(comingFrom);
198.                 comingFrom=comeFrom[comingFrom];
199.         }
200.         return path;
201.     }
202.  
203.     // Update is called once per frame
204.     void Update () {
205.    
206.     }
207. }
208.