diff --git a/cpp/tiledmap.cc b/cpp/tiledmap.cc
index 37c48ec..a59cea4 100644
--- a/cpp/tiledmap.cc
+++ b/cpp/tiledmap.cc
@@ -320,7 +320,7 @@ bool TiledMap::CalcCurrPos(std::vector<int>& path_points, int old_pos_x, int old
             }
 
             std::vector<GridCell*> grid_sort_list = SortGridList(grid_list,sp);
-            if (grid_sort_list.empty()){
+            if (grid_sort_list.size() < 2) {
                 return false;
             }
             if (exc_once) {
@@ -336,8 +336,21 @@ bool TiledMap::CalcCurrPos(std::vector<int>& path_points, int old_pos_x, int old
         return false;
     }
     {
+        bool has_index = false;
+        int index = 0;
+        for (auto& gc : grid_all_list) {
+            if (gc->x == old_pos_x && gc->y == old_pos_y) {
+                has_index = true;
+                break;
+            }
+            index++;
+        }
+        if (!has_index) {
+            return false;
+        }
+
         int grid_all_count = grid_all_list.size();
-        for (int i = 0; i < grid_all_count-1; ++i) {
+        for (int i = index; i < grid_all_count-1; ++i) {
             GridCell* gc_first = grid_all_list[i];
             GridCell* gc_second = grid_all_list[i+1];
             int speed = gc_first->speed;