Search Unity

  1. Megacity Metro Demo now available. Download now.
    Dismiss Notice
  2. Unity support for visionOS is now available. Learn more in our blog post.
    Dismiss Notice

A* heuristic problem

Discussion in 'Scripting' started by Pavlon, Jul 18, 2015.

  1. Pavlon

    Pavlon

    Joined:
    Apr 15, 2015
    Posts:
    191
    After not geting what i need from unitys build in path finding i wrote a small A* path finding algorithm and now i am standing for a new problem.

    After playing around with it for some hours i am pretty sure that my heuristic is the problem but i am not able to get it right...

    So here is what gave me the best result so far.
    Code (CSharp):
    1.     public void SetNodeCost(int x, int y, bool diagonal)
    2.     {
    3.         int dif_x = Mathf.Abs (this.x - x);
    4.         int dif_y = Mathf.Abs (this.y - y);
    5. //         heuristic_distance = (dif_x + dif_y) * 1.10f;
    6.         distance = parent_node.distance + 0.10f;
    7.  
    8.         if (diagonal == true)
    9.         {
    10.             //distance = parent_node.distance + 0.75f;
    11.             if(dif_x > dif_y){
    12.                 heuristic_distance = 1.4f*dif_y + (dif_x-dif_y);
    13.             } else {
    14.                 heuristic_distance = 1.4f*dif_x + (dif_y-dif_x);
    15.             }
    16.         }
    17.         else
    18.         {
    19.             heuristic_distance = (dif_x + dif_y);
    20. //            heuristic_distance = Vector2.Distance ( new Vector2(this.x,this.y),new Vector2(x,y));
    21. //            distance = parent_node.distance + 1.00f;
    22.         }
    23. //        heuristic_distance = heuristic_distance * 1.1f;
    24.         cost = heuristic_distance + distance;
    25.         cost = cost * 1.1f;
    26.     }

    To get a better understanding of what goes wrong here 2 images
    the black line is the path the blue "grid" are the searched nodes.
    Nodes deep in the tree get brighter until they white..

    without diagonal on
    path1.jpg

    with diagonal
    path2.jpg
     
  2. LeftyRighty

    LeftyRighty

    Joined:
    Nov 2, 2012
    Posts:
    5,148
    Code (csharp):
    1.  
    2. if(dif_x > dif_y)
    3. {
    4.             heuristic_distance = 1.4f*dif_y + (dif_x-dif_y);
    5. }
    6. else
    7. {              
    8.            heuristic_distance = 1.4f*dif_x + (dif_y-dif_x);          
    9. }
    10.  
    can you explain what you are trying to do here... because this isn't how you work out diagonal distance... ??
     
  3. Pavlon

    Pavlon

    Joined:
    Apr 15, 2015
    Posts:
    191
    Last edited: Jul 18, 2015
  4. RiokuTheSlayer

    RiokuTheSlayer

    Joined:
    Aug 22, 2013
    Posts:
    356
    Make the cost to move diagonally 1.01 times as expensive. This will cause the pathfinding algorithm to see the cardinal directions as ever so slightly cheaper, and prefer that over the weird zigzag pattern you have
     
  5. Pavlon

    Pavlon

    Joined:
    Apr 15, 2015
    Posts:
    191
    Ok i found a small bug that reduced the steps he need to find a path from 8000 to about 1500.
    But i think it is still to mutch here is a update.

    Code (CSharp):
    1.     public void SetNodeCost(int x, int y, bool diagonal)
    2.     {
    3.         int dif_x = Mathf.Abs (this.x - x);
    4.         int dif_y = Mathf.Abs (this.y - y);
    5.         distance = parent_node.distance + 0.15f;
    6.  
    7.         if (diagonal == true)
    8.         {
    9.             // if i set this below 2.00f he will move in zig zags
    10.             if(dif_x > dif_y)
    11.             {
    12.                 heuristic_distance = 2.00f* dif_y + (dif_x-dif_y);
    13.             }
    14.             else
    15.             {
    16.                 heuristic_distance = 2.00f* dif_x + (dif_y-dif_x);
    17.             }
    18.         }
    19.         else
    20.         {
    21.             heuristic_distance  = (dif_x + dif_y) ;
    22.         }
    23.  
    24.         cost = heuristic_distance + distance;
    25.     }


    Also i am pretty sure that this is not the optimal path.
    path4.jpg
     
  6. chelnok

    chelnok

    Joined:
    Jul 2, 2012
    Posts:
    680
    If you just need solution, and not interested creating your own path finding, check out Aron Granberg's A* pathfinding project:
    http://arongranberg.com/astar/features

    Even the free version got tons of features that are missing from unity's build in pathfinding.
     
  7. Pavlon

    Pavlon

    Joined:
    Apr 15, 2015
    Posts:
    191
    He he i decided to make my own after testing A* Pathfinding Project but thanks any way

    Edit:

    And after some more testing i got him away from double checking nodes now he needs about 1200 steps
    but now he check the whole map and still dont get the optimal path..

    cause i am not sure any more that it have only to do with the heuristic here is the full script
    (it is a bit messy)
    Code (CSharp):
    1. using UnityEngine;
    2. using System.Collections;
    3. using System.Collections.Generic;
    4.  
    5. public class A_Star : MonoBehaviour
    6. {
    7.     List<A_StarNode> closed_list;
    8.     List<A_StarNode> open_list;
    9.     List<A_StarNode> new_nodes_list;
    10.     List<Vector2> path = new List<Vector2>();
    11.     int map_size_x;
    12.     int map_size_y;
    13.     int [,] map;
    14.  
    15.     private void LoadTestMap()
    16.     {
    17.         Object sample= Resources.Load ("map3");
    18.         Texture2D pixmap = sample as Texture2D;
    19.         map_size_x = 50;
    20.         map_size_y = 50;
    21.         map = new int[map_size_x,map_size_y];
    22.  
    23.         for (int x = 0; x < map_size_x; x ++)
    24.         {
    25.             for (int y = 0; y < map_size_y; y ++)
    26.             {
    27.                 if(pixmap.GetPixel(x,y).r != 1)
    28.                 {
    29.                     map[x,y] = 1;
    30.                     GameObject obj = Instantiate(Resources.Load ("Cube"),new Vector3(x * 4 + 2, 2,y *4 + 2),Quaternion.AngleAxis(180,new Vector3(0,1,0))) as GameObject;
    31.                     obj.transform.localScale = new Vector3(4,4,4);
    32.                 }
    33.             }
    34.         }
    35.     }
    36.  
    37.  
    38.     void Start ()
    39.     {
    40.  
    41.         LoadTestMap ();
    42.  
    43.  
    44.         A_StarNode node = CalculatePath (0, 0, map, 49, 49, true);
    45.         OptimizePath (node);
    46.  
    47.         //Debuging
    48.         for (int i = 0; i < path.Count; i ++)
    49.         {
    50.             GameObject.Instantiate(Resources.Load ("Cube"),new Vector3(path[i].x * 4 + 2,1,path[i].y *4 + 2),Quaternion.AngleAxis(180,new Vector3(0,1,0)));
    51.         }
    52.  
    53.         for (int i = 0; i < path.Count -1; i ++)
    54.         {
    55.             Debug.DrawLine (new Vector3 (path [i].x * 4 + 2, 2, path [i].y * 4 + 2), new Vector3 (path [i+ 1].x * 4 + 2, 2, path [i + 1].y * 4 + 2), new Color (0.0f, 0.0f, 0.0f), 10, false);    
    56.         }
    57.         //-------
    58.     }
    59.  
    60.     public void OptimizePath(A_StarNode node)
    61.     {
    62.         int direction = 0;
    63.         int tmp_direction = 0;
    64.         while (node.parent_node != null)
    65.         {
    66.             node = node.parent_node;
    67.             if(node.parent_node != null)
    68.             {
    69.                 if (node.y + 1 == node.parent_node.y)
    70.                     tmp_direction = 360;
    71.                 if (node.x + 1 == node.parent_node.x)
    72.                     tmp_direction = 90;
    73.                 if (node.y - 1 == node.parent_node.y)
    74.                     tmp_direction = 180;
    75.                 if (node.x - 1 == node.parent_node.x)
    76.                     tmp_direction = 270;
    77.                 if (node.y + 1 == node.parent_node.y && node.x + 1 == node.parent_node.x)
    78.                     tmp_direction = 45;
    79.                 if (node.y - 1 == node.parent_node.y && node.x + 1 == node.parent_node.x)
    80.                     tmp_direction = 135;
    81.                 if (node.y - 1 == node.parent_node.y && node.x - 1 == node.parent_node.x)
    82.                     tmp_direction = 225;
    83.                 if (node.y + 1 == node.parent_node.y && node.x - 1 == node.parent_node.x)
    84.                     tmp_direction = 315;
    85.             }
    86.             else
    87.             {
    88.                 path.Add(new Vector2(node.x,node.y));
    89.             }
    90.             if(tmp_direction != direction)
    91.             {
    92.                 direction = tmp_direction;
    93.                 path.Add(new Vector2(node.x,node.y));
    94.             }
    95.         }
    96.     }
    97.  
    98.     public A_StarNode CalculatePath(int start_x, int start_y, int [,] map, int goal_x, int goal_y, bool allow_diagonal)
    99.     {
    100.         closed_list     = new List<A_StarNode>();
    101.         open_list         = new List<A_StarNode>();
    102.         new_nodes_list     = new List<A_StarNode>();
    103.         open_list.Add (new A_StarNode (start_x, start_y, null));
    104.         A_StarNode tmp_node;
    105.         int counter = 0;
    106.  
    107.         while (open_list.Count != 0)
    108.         {
    109.             counter ++;
    110.             A_StarNode node = GetCheapestNode(open_list);
    111.             open_list.Remove (node);
    112.             if(node.y + 1 < map_size_y)
    113.             {
    114.                 if(map[node.x,node.y + 1] != 1)
    115.                 {
    116.                     tmp_node = new A_StarNode(node.x, node.y + 1, node);
    117.                     tmp_node.SetNodeCost(goal_x,goal_y,allow_diagonal);
    118.                     new_nodes_list.Add (tmp_node);
    119.                 }
    120.             }
    121.             if(node.x  + 1  < map_size_x)
    122.             {
    123.                 if(map[node.x + 1,node.y] != 1)
    124.                 {
    125.                     tmp_node = new A_StarNode(node.x + 1, node.y, node);
    126.                     tmp_node.SetNodeCost(goal_x,goal_y,allow_diagonal);
    127.                     new_nodes_list.Add (tmp_node);
    128.                 }
    129.             }
    130.             if(node.y - 1 > 0)
    131.             {
    132.                 if(map[node.x,node.y - 1] != 1)
    133.                 {
    134.                     tmp_node = new A_StarNode(node.x, node.y - 1, node);
    135.                     tmp_node.SetNodeCost(goal_x,goal_y,allow_diagonal);
    136.                     new_nodes_list.Add (tmp_node);
    137.                 }
    138.             }
    139.             if(node.x - 1 > 0)
    140.             {
    141.                 if(map[node.x - 1,node.y] != 1)
    142.                 {
    143.                     tmp_node = new A_StarNode(node.x - 1, node.y, node);
    144.                     tmp_node.SetNodeCost(goal_x,goal_y,allow_diagonal);
    145.                     new_nodes_list.Add (tmp_node);
    146.                 }
    147.             }
    148.  
    149.             if(allow_diagonal == true)
    150.             {
    151.                 if(node.x + 1 < map_size_x && node.y + 1 < map_size_y)
    152.                 {
    153.                     if(map[node.x + 1,node.y + 1] != 1)
    154.                     {
    155.                         tmp_node = new A_StarNode(node.x + 1, node.y + 1, node);
    156.                         tmp_node.SetNodeCost(goal_x,goal_y,allow_diagonal);
    157.                         new_nodes_list.Add (tmp_node);
    158.                     }
    159.                 }
    160.                 if( node.x + 1 < map_size_x && node.y - 1 > 0)
    161.                 {
    162.                     if(map[node.x + 1,node.y - 1] != 1)
    163.                     {
    164.                         tmp_node = new A_StarNode(node.x + 1, node.y - 1, node);
    165.                         tmp_node.SetNodeCost(goal_x,goal_y,allow_diagonal);
    166.                         new_nodes_list.Add (tmp_node);
    167.                     }
    168.                 }
    169.                 if(node.x - 1 > 0 && node.y - 1 > 0)
    170.                 {
    171.                     if(map[node.x - 1,node.y - 1] != 1)
    172.                     {
    173.                         tmp_node = new A_StarNode(node.x - 1, node.y - 1, node);
    174.                         tmp_node.SetNodeCost(goal_x,goal_y,allow_diagonal);
    175.                         new_nodes_list.Add (tmp_node);
    176.                     }
    177.                 }
    178.                 if(node.x - 1 > 0 && node.y + 1 < map_size_y)
    179.                 {
    180.                     if(map[node.x - 1,node.y + 1] != 1)
    181.                     {
    182.                         tmp_node = new A_StarNode(node.x - 1, node.y + 1, node);
    183.                         tmp_node.SetNodeCost(goal_x,goal_y,allow_diagonal);
    184.                         new_nodes_list.Add (tmp_node);
    185.                     }
    186.                 }
    187.             }
    188.             for(int i = 0; i < new_nodes_list.Count; i ++)
    189.             {
    190.  
    191.                 if(new_nodes_list[i].x == goal_x && new_nodes_list[i].y == goal_y)
    192.                 {
    193.                     Debug.Log (counter);
    194.                     return new_nodes_list[i];
    195.                 }
    196.                 if(ShorterPathIn(open_list,new_nodes_list[i]) == false && ShorterPathIn(closed_list,new_nodes_list[i]) == false)
    197.                 {            
    198.                     //Debug
    199.                     float c = (float) counter / 1155f;
    200.                     Debug.DrawLine (new Vector3 (node.x * 4 + 2, 1, node.y * 4 + 2), new Vector3 (new_nodes_list[i].x * 4 + 2, 1 , new_nodes_list[i].y * 4 + 2), new Color (c, c, 1), 10, false);
    201.                     //-------------------
    202.                     open_list.Add (new_nodes_list[i]);
    203.                 }
    204.             }
    205.             new_nodes_list.Clear ();
    206.             closed_list.Add (node);
    207.         }
    208.         Debug.Log (counter);
    209.         return null;
    210.     }
    211.  
    212.     private bool ShorterPathIn(List<A_StarNode> node_list, A_StarNode node)
    213.     {
    214.         for (int i = 0; i < node_list.Count; i ++)
    215.         {
    216.             if(node.x == node_list[i].x && node.y == node_list[i].y &&  node_list[i].GetCost() <= node.GetCost())
    217.                 return true;
    218.         }
    219.         return false;
    220.     }
    221.  
    222.     private A_StarNode GetCheapestNode(List<A_StarNode> node_list)
    223.     {
    224.         A_StarNode node = null;
    225.         for (int i = 0; i < node_list.Count; i ++)
    226.         {
    227.             if(node == null || node.GetCost() > node_list[i].GetCost())
    228.                 node = node_list[i];
    229.         }
    230.         return node;
    231.     }
    232. }
    233.  
    234.  
    235.  
    236.  
    237.  
    238. public class A_StarNode
    239. {
    240.     public     int         x;
    241.     public     int         y;
    242.     private float         cost;
    243.     private float         distance;
    244.     private float         heuristic_distance;
    245.     public A_StarNode     parent_node;
    246.  
    247.     public A_StarNode(int x, int y, A_StarNode parent_node)
    248.     {
    249.         this.x = x;
    250.         this.y = y;
    251.         this.parent_node = parent_node;
    252.     }
    253. //    public void SetNodeCost(int x, int y, bool diagonal)
    254. //    {
    255. //        distance = parent_node.distance + 0.5f;
    256. //        int dx1 = this.x - 49;
    257. //        int dy1 = this.y - 49;
    258. //        int dx2 = 0 - 49;
    259. //        int dy2 = 0 - 49;
    260. //        float cross = Mathf.Abs (dx1 * dy2 - dx2 * dy1);
    261. //        int dif_x = Mathf.Abs (this.x - x);
    262. //        int dif_y = Mathf.Abs (this.y - y);
    263. //        heuristic_distance = (dif_x + dif_y);
    264. //        cost = heuristic_distance + distance + cross * 0.01f;
    265. //        //cost =  distance + cross * 0.01f;
    266. //    }
    267.  
    268.     public void SetNodeCost(int x, int y, bool diagonal)
    269.     {
    270.         int dif_x = Mathf.Abs (this.x - x);
    271.         int dif_y = Mathf.Abs (this.y - y);
    272.         if (diagonal == true)
    273.         {
    274.             distance = parent_node.cost + 0.1f;
    275.  
    276.             if (dif_x > dif_y)
    277.             {
    278.                 heuristic_distance = 1.4f * dif_y + (dif_x - dif_y);
    279.             }
    280.             else
    281.             {
    282.                 heuristic_distance = 1.4f * dif_x + (dif_y - dif_x);
    283.             }
    284.         }
    285.         else
    286.         {
    287.             distance = parent_node.cost + 1f;
    288.             heuristic_distance = (dif_x + dif_y);
    289.         }
    290.  
    291.  
    292.         cost = heuristic_distance + distance;
    293.     }
    294.  
    295.     public float GetCost()
    296.     {
    297.         return cost;
    298.     }

    And here is the current progress
    path5.jpg

    I also uploaded the map it is a bmp normaly but i needed to rename for upload...
     

    Attached Files:

    Last edited: Jul 18, 2015
  8. LeftyRighty

    LeftyRighty

    Joined:
    Nov 2, 2012
    Posts:
    5,148
    the equation you're using doesn't appear anywhere in that article... :confused:

    edit: actually looking at it again, given that you are using a regular grid, you could replace the entire section with just

    Code (csharp):
    1.  
    2. if (diagonal == true)
    3.        {
    4.             distance = parent_node.cost + 0.1f;
    5.             heuristic_distance = 1.4f;
    6.         }
    7.        else
    8.        {
    9.             distance = parent_node.cost + 1f;
    10.             heuristic_distance = 1;
    11.        }
    12.  
     
    Last edited: Jul 18, 2015
  9. Pavlon

    Pavlon

    Joined:
    Apr 15, 2015
    Posts:
    191
  10. LeftyRighty

    LeftyRighty

    Joined:
    Nov 2, 2012
    Posts:
    5,148
    ah so not in that article then :p

    are you intending to use variable terrain costs?
     
  11. Pavlon

    Pavlon

    Joined:
    Apr 15, 2015
    Posts:
    191
    Later on when i stop working with a test map but this will be a small step ones i get the basic working
    (i hope this will be a small stepp :p)

    Edit:

    I just testet the writing method from http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html
    with a terrain cost of 1 and it gave me the same result

    Code (CSharp):
    1. heuristic_distance = (dif_y + dif_x) + (1 - 2 * 1) * Mathf.Min(dif_x, dif_y);
    2.  
    Also for the aktuall case you can use

    Code (CSharp):
    1. heuristic_distance = (dif_y + dif_x) - Mathf.Min(dif_x, dif_y);
    2.  
     
    Last edited: Jul 18, 2015
  12. Pavlon

    Pavlon

    Joined:
    Apr 15, 2015
    Posts:
    191
    Ok here is a break down what i got so far

    Code (CSharp):
    1. public void SetNodeCost(int x, int y, bool diagonal)
    2.     {
    3.         if (type == 0)
    4.         {
    5.             int dif_x = Mathf.Abs (this.x - x);
    6.             int dif_y = Mathf.Abs (this.y - y);
    7.             float p_dist = 0;
    8.             if (diagonal == true)
    9.             {
    10.                 p_dist = Mathf.Sqrt(Mathf.Pow(this.x - parent_node.x, 2) + Mathf.Pow(this.y - parent_node.y, 2));
    11.                 heuristic_distance = Mathf.Sqrt(dif_x*dif_x + dif_y*dif_y);
    12.             }
    13.             else
    14.             {
    15.                 p_dist = parent_node.distance + 1f;
    16.                 heuristic_distance = (dif_x + dif_y);
    17.             }
    18.            
    19.             distance = parent_node.distance + p_dist;
    20.             cost = heuristic_distance + distance;
    21.  
    22.  
    23.         }
    24.         // 1,2,3 lead to the same result ...
    25.         if (type == 1)
    26.         {
    27.             int dif_x = Mathf.Abs (this.x - x);
    28.             int dif_y = Mathf.Abs (this.y - y);
    29.             if (diagonal == true)
    30.             {
    31.                 distance = parent_node.distance + 1f;
    32.                 if (dif_x > dif_y)
    33.                 {
    34.                     heuristic_distance = dif_y + (dif_x - dif_y);
    35.                 }
    36.                 else
    37.                 {
    38.                     heuristic_distance = dif_x + (dif_y - dif_x);
    39.                 }
    40.             }
    41.             else
    42.             {
    43.                 distance = parent_node.distance + 1f;
    44.                 heuristic_distance = (dif_x + dif_y);
    45.             }
    46.  
    47.             distance = parent_node.distance + distance;
    48.             cost = heuristic_distance + distance;
    49.         }
    50.         if (type == 2)
    51.         {
    52.             int dif_x = Mathf.Abs (this.x - x);
    53.             int dif_y = Mathf.Abs (this.y - y);
    54.    
    55.             distance = parent_node.distance + 1f;
    56.             heuristic_distance = (dif_x + dif_y);
    57.            
    58.             distance = parent_node.distance + distance;
    59.             cost = heuristic_distance + distance;
    60.         }
    61.         if (type == 3)
    62.         {
    63.             distance = parent_node.distance + 1f;
    64.             heuristic_distance = Vector2.Distance (new Vector2(this.x,this.y),new Vector2(x,y));
    65.             distance = parent_node.distance + distance;
    66.             cost = heuristic_distance + distance;
    67.         }
    68.         if (type == 4)
    69.         {
    70.             int dif_x = Mathf.Abs (this.x - x);
    71.             int dif_y = Mathf.Abs (this.y - y);
    72.             heuristic_distance = Mathf.Sqrt(dif_x*dif_x + dif_y*dif_y);
    73.             distance = parent_node.distance + 0.5f;
    74.             cost = heuristic_distance + distance;
    75.         }
    76.         if (type == 5)
    77.         {
    78.             distance = parent_node.distance + 1f;
    79.             int dx1 = this.x - x;
    80.             int dy1 = this.y - y;
    81.             int dx2 = 0 - x;
    82.             int dy2 = 0 - y;
    83.             float cross = Mathf.Abs (dx1 * dy2 - dx2 * dy1);
    84.             heuristic_distance = (dx1 + dy1);
    85.             cost = heuristic_distance + distance + cross * 0.01f;
    86.         }
    87.     }

    Results

    map_a.jpg

    map_b.jpg
     
  13. Pavlon

    Pavlon

    Joined:
    Apr 15, 2015
    Posts:
    191
    DonLoquacious likes this.
  14. DonLoquacious

    DonLoquacious

    Joined:
    Feb 24, 2013
    Posts:
    1,667
    Burn him with fire!!! (Also, well done, and thanks for the contribution to science.)