Merge branch 'master' into develop

Author: xtaci

include/astar.h

@@ -86,8 +86,8 @@ namespace alg {
 				// The map of navigated nodes.	
 				HashTable<uint32_t, uint32_t> came_from(nrow*ncol);
 
-				g_score(x1,y1) = 0.0f;
-				f_score(x1,y1) = g_score(x1,y1) + estimate(x1,y1,x2,y2);
+				g_score(y1,x1) = 0.0f;
+				f_score(y1,x1) = g_score(y1,x1) + estimate(x1,y1,x2,y2);
 
 				AStarResult * as = new AStarResult;
 				as->path = NULL;
@@ -117,7 +117,7 @@ namespace alg {
 					}
 
 					openset.delete_min();
-					m_closedset(cx, cy) = true;
+					m_closedset(cy, cx) = true;
 
 					// for each neighbor
 					int nx, ny;
@@ -127,26 +127,26 @@ namespace alg {
 							if (ny<0 || ny>=(int)nrow) continue;
 
 							// except the wall;
-							if(m_grid(nx,ny) == WALL) continue;
+							if(m_grid(ny,nx) == WALL) continue;
 							// except the cur itself
 							if(nx == cx && ny==cy) continue;
 							// if neighbour in the closed set	
-							if(m_closedset(nx,ny)) continue;	
+							if(m_closedset(ny,nx)) continue;
 
-							float tentative = g_score(cx,cy);
+							float tentative = g_score(cy,cx);
 							if (nx == cx || ny == cy) {
-								tentative += 1;
+								tentative += 1 + m_grid(ny,nx);
 							} else {
-								tentative += SQRT2;
+								tentative += (1 + m_grid(ny,nx)) * SQRT2;
 							}
 
 							// if neighbour not in the openset or dist < g_score[neighbour]	
-							if (!openset.contains(nx*ncol+ny) || tentative < g_score(nx,ny)) {
+							if (!openset.contains(nx*ncol+ny) || tentative < g_score(ny,nx)) {
 								came_from[nx*ncol+ny] = cx*ncol+cy; // record path
-								g_score(nx,ny) = tentative;
-								f_score(nx,ny) = tentative + estimate(nx,ny,x2,y2);
+								g_score(ny,nx) = tentative;
+								f_score(ny,nx) = tentative + estimate(nx,ny,x2,y2);
 								if (!openset.contains(nx*ncol+ny)) {
-									openset.insert(f_score(nx,ny), nx*ncol+ny);
+									openset.insert(f_score(ny,nx), nx*ncol+ny);
 								}
 							}
 						}