21ic电子技术开发论坛 › 单片机与嵌入式系统 › 新唐MCU › C语言实现A*算法
返回列表 发新帖我要提问

[技术问答] C语言实现A*算法

[复制链接]
1869|2
 楼主| modesty3jonah 发表于 2024-5-22 20:00 | 显示全部楼层 |阅读模式
TI, TE, ST, No, C语言



复制
  1. /******************************************************************************* 
    

  2. * Description: A*寻路算法 测试类  
    

  3. *******************************************************************************/   
    

  4. #include <stdlib.h>   
    

  5. #include <stdio.h>   
    

  6. #include <math.h>   
    

  7. #define FALSE   0   
    

  8. #define TRUE    1   
    

  9. #define NULL    0   
    

  10. typedef int BOOL;   
    

  11.    
    

  12. int map[20][20] =    
    

  13. {   
    

  14.     { 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0 },   
    

  15.     { 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 0 },   
    

  16.     { 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0 },   
    

  17.     { 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 1, 0, 0, 0, 0 },   
    

  18.     { 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },   
    

  19.     { 0, 0, 0, 1, 0, 1, 1, 1, 1, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },   
    

  20.     { 0, 0, 0, 1, 0, 1, 0, 0, 0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },   
    

  21.     { 0, 0, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },   
    

  22.     { 0, 0, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },   
    

  23.     { 0, 0, 0, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },   
    

  24.     { 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },   
    

  25.     { 0, 0, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 1, 0, 0, 0, 0 },   
    

  26.     { 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0 },   
    

  27.     { 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 0, 0, 0 },   
    

  28.     { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0 },   
    

  29.     { 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 },   
    

  30.     { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0 },   
    

  31.     { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0 },   
    

  32.     { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0 },   
    

  33.     { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }   
    

  34. };   
    

  35.     typedef struct Node   
    

  36.     {//节点结构体   
    

  37.         int f,g,h;   
    

  38.         int row;    //该节点所在行   
    

  39.         int col;    //该节点所在列   
    

  40.         int direction;//parent节点要移动的方向就能到达本节点   
    

  41.         struct Node * parent;   
    

  42.     }Node, *Lnode;   
    

  43.        
    

  44.     typedef struct Stack   
    

  45.     {//OPEN CLOSED 表结构体   
    

  46.         Node * npoint;   
    

  47.         struct Stack * next;   
    

  48.     }Stack, *Lstack;   
    

  49.     int rows = 20;          //地图行数   
    

  50.     int cols = 20;          //地图列数   
    

  51.     int G_OFFSET = 1;       //每个图块G值的增加值   
    

  52.     int destinationRow;     //目标所在行   
    

  53.     int destinationCol;     //目标所在列   
    

  54.     int canMoveIndex = 0;   //可以通行的地图图块索引   
    

  55.     int tileSize = 1;       //图块大小   
    

  56.        
    

  57.     Lstack Open = NULL;   
    

  58.     Lstack Closed = NULL;   
    

  59.     Node * getNodeFromOpen()   
    

  60.     {//选取OPEN表上f值最小的节点，返回该节点地址   
    

  61.         Lstack temp = Open->next,min = Open->next,minp = Open;   
    

  62.         Node * minx;   
    

  63.         if( temp == NULL )   
    

  64.             return NULL;   
    

  65.            
    

  66.         while(temp->next != NULL)   
    

  67.         {   
    

  68.             if( (temp->next ->npoint->f) < (min->npoint->f) )   
    

  69.             {   
    

  70.                 min = temp->next;   
    

  71.                 minp = temp;   
    

  72.             }   
    

  73.             temp = temp->next;   
    

  74.         }   
    

  75.         minx = min->npoint;   
    

  76.         temp = minp->next;   
    

  77.         minp->next = minp->next->next;   
    

  78.         free(temp);   
    

  79.         return minx;   
    

  80.     }   
    

  81.        
    

  82.     BOOL Equal(Node * suc,Node * goal)   
    

  83.     {//判断节点是否相等，相等，不相等   
    

  84.         if ( (suc->row == goal->row) && (suc->col == goal->col)  )   
    

  85.         {   
    

  86.             return TRUE;   
    

  87.         }      
    

  88.         else   
    

  89.         {   
    

  90.             return FALSE;   
    

  91.         }   
    

  92.     }   
    

  93.        
    

  94.     Node * BelongInOpen( Node * suc )   
    

  95.     {//判断节点是否属于OPEN表或CLOSED表，是则返回节点地址，否则返回空地址   
    

  96.         Lstack temp = Open -> next ;   
    

  97.         if(temp == NULL)   
    

  98.             return NULL;   
    

  99.         while( temp != NULL )   
    

  100.         {   
    

  101.             if( Equal(suc,temp->npoint) )   
    

  102.             {   
    

  103.                 return temp -> npoint;   
    

  104.             }   
    

  105.             else   
    

  106.             {   
    

  107.                 temp = temp->next;      
    

  108.             }   
    

  109.         }   
    

  110.         return NULL;   
    

  111.     }   
    

  112.        
    

  113.     Node * BelongInClosed( Node * suc )   
    

  114.     {//判断节点是否属于OPEN表或CLOSED表，是则返回节点地址，否则返回空地址   
    

  115.         Lstack temp = Closed -> next ;   
    

  116.         if(temp == NULL)   
    

  117.             return NULL;   
    

  118.         while(temp != NULL)   
    

  119.         {   
    

  120.             if( Equal(suc,temp->npoint) )   
    

  121.             {   
    

  122.                 return temp -> npoint;   
    

  123.             }   
    

  124.             else   
    

  125.             {   
    

  126.                 temp = temp->next;      
    

  127.             }   
    

  128.         }   
    

  129.         return NULL;   
    

  130.     }   
    

  131.        
    

  132.     void PutintoOpen(Node * suc )   
    

  133.     {//把节点放入OPEN 或CLOSED 表中   
    

  134.         Stack * temp;   
    

  135.         temp =(Stack *) malloc(sizeof(Stack));   
    

  136.         temp->npoint = suc;   
    

  137.        
    

  138.         temp->next = Open->next;   
    

  139.         Open->next = temp;   
    

  140.     }   
    

  141.     void PutintoClosed(Node * suc )   
    

  142.     {//把节点放入OPEN 或CLOSED 表中   
    

  143.         Stack * temp;   
    

  144.         temp =(Stack *) malloc(sizeof(Stack));   
    

  145.         temp->npoint = suc;   
    

  146.         temp->next = Closed->next;   
    

  147.         Closed->next = temp;   
    

  148.     }   
    

  149.        
    

  150.     //得到该图块的H值   
    

  151.     int getH(int row, int col)   
    

  152.     {   
    

  153.         return (abs(destinationRow - row) + abs(destinationCol - col));   
    

  154.     }   
    

  155.        
    

  156.     //得到该位置所在地图行   
    

  157.     int getRowPosition(int y)   
    

  158.     {   
    

  159.         return (y / tileSize);   
    

  160.     }   
    

  161.        
    

  162.     //得到该位置所在地图列   
    

  163.     int getColPosition(int x)   
    

  164.     {   
    

  165.         return (x / tileSize);   
    

  166.     }   
    

  167.     //检测该图块是否可通行   
    

  168.     BOOL isCanMove(int col, int row)   
    

  169.     {   
    

  170.         if(col < 0 || col >= cols)   
    

  171.             return FALSE;   
    

  172.         if(row < 0 || row >= rows)   
    

  173.             return FALSE;   
    

  174.         return map[col][row] == canMoveIndex;   
    

  175.     }   
    

  176.        
    

  177.     Node* checkOpen(int row, int col)   
    

  178.     {   
    

  179.         Lstack temp = Open -> next;   
    

  180.         if ( temp == NULL )   
    

  181.             return NULL;   
    

  182.         while (temp != NULL)    
    

  183.         {   
    

  184.             if ( (temp->npoint->row==row) && (temp->npoint->col == col)  )   
    

  185.             {   
    

  186.                 return temp -> npoint;   
    

  187.             }      
    

  188.             else   
    

  189.             {   
    

  190.                 temp = temp->next;   
    

  191.             }   
    

  192.         }   
    

  193.         return NULL;   
    

  194.     }   
    

  195.        
    

  196.     BOOL isInClose(int row, int col)   
    

  197.     {   
    

  198.         Lstack temp = Closed -> next;   
    

  199.         if ( temp == NULL )   
    

  200.             return FALSE;   
    

  201.         while (temp != NULL)    
    

  202.         {   
    

  203.             if ( (temp->npoint->row==row) && (temp->npoint->col == col)  )   
    

  204.             {   
    

  205.                 return TRUE;   
    

  206.             }      
    

  207.             else   
    

  208.             {   
    

  209.                 temp = temp->next;   
    

  210.             }   
    

  211.         }   
    

  212.         return FALSE;   
    

  213.     }   
    

  214.     int directionIndex =0;   
    

  215.     int direction[256];   
    

  216.     void creatSeccessionNode(Node *bestNode, int row, int col)   
    

  217.     {   
    

  218.         int g = bestNode->g + G_OFFSET;   
    

  219.         if(!isInClose(row, col))   
    

  220.         {   
    

  221.             Node *oldNode = NULL;   
    

  222.             if((oldNode = checkOpen(row, col)) != NULL)   
    

  223.             {   
    

  224.                 if(oldNode->g < g)   
    

  225.                 {   
    

  226.                     oldNode->parent = bestNode;   
    

  227.                     oldNode->g = g;   
    

  228.                     oldNode->f = g + oldNode->h;   
    

  229.                 }   
    

  230.             }   
    

  231.             else   
    

  232.             {   
    

  233.                 Node *node = (Node *) malloc(sizeof(Node));   
    

  234.                 node->parent = bestNode;   
    

  235.                 node->g = g;   
    

  236.                 node->h = getH(row, col);   
    

  237.                 node->f = node->g + node->h;   
    

  238.                 node->row = row;   
    

  239.                 node->col = col;   
    

  240.                 directionIndex++;   
    

  241.                 node->direction = directionIndex;   
    

  242. //              openNode.addElement(node);   
    

  243.                 PutintoOpen( node );   
    

  244.             }   
    

  245.         }   
    

  246.     }   
    

  247.        
    

  248.     /**  
    

  249.      * 根据传入的节点生成子节点  
    

  250.      * @param bestNode  
    

  251.      * @param destinationRow  
    

  252.      * @param destinationCol  
    

  253.      */   
    

  254.     void seachSeccessionNode(Node *bestNode)   
    

  255.     {   
    

  256.         int row, col;   
    

  257. //      Node *bestNodeInOpen = NULL;   
    

  258.         //上部节点   
    

  259.         if(isCanMove(row = bestNode->row - 1, col = bestNode->col))   
    

  260.         {   
    

  261.             creatSeccessionNode(bestNode, row, col);   
    

  262.         }   
    

  263.         //下部节点   
    

  264.         if(isCanMove(row = bestNode->row + 1, col = bestNode->col))   
    

  265.         {   
    

  266.             creatSeccessionNode(bestNode, row, col);   
    

  267.         }   
    

  268.         //左部节点   
    

  269.         if(isCanMove(row = bestNode->row, col = bestNode->col - 1))   
    

  270.         {   
    

  271.             creatSeccessionNode(bestNode, row, col);   
    

  272.         }   
    

  273.         //右部节点   
    

  274.         if(isCanMove(row = bestNode->row, col = bestNode->col + 1))   
    

  275.         {   
    

  276.             creatSeccessionNode(bestNode, row, col);   
    

  277.         }   
    

  278.         PutintoClosed( bestNode );   
    

  279.     }   
    

  280.     //正序遍历链表   
    

  281.     void printfOpenData()   
    

  282.     {//判断节点是否属于OPEN表或CLOSED表，是则返回节点地址，否则返回空地址   
    

  283.         Lstack temp = Open -> next;   
    

  284.         Node *p_node;   
    

  285.         if(temp == NULL)   
    

  286.             return;   
    

  287.         while(temp != NULL)   
    

  288.         {   
    

  289.             Lstack head = temp;   
    

  290.             temp = temp->next;   
    

  291.             p_node = head->npoint;   
    

  292.             printf("Open库数据！[%d,%d]\n", p_node->col, p_node->row );   
    

  293.             free(p_node);   
    

  294.             free( head );   
    

  295.             Open->next = temp;   
    

  296.         }   
    

  297.         printf("\n Open库数据 数据全部清楚 \n");   
    

  298.         return;   
    

  299.     }   
    

  300.     void printfClosedData()   
    

  301.     {//判断节点是否属于OPEN表或CLOSED表，是则返回节点地址，否则返回空地址   
    

  302.         Lstack temp = Closed -> next ;   
    

  303.         Node *p_node;   
    

  304.         if(temp == NULL)   
    

  305.             return;   
    

  306.         while(temp != NULL)   
    

  307.         {   
    

  308.             Lstack head = temp;   
    

  309.             temp = temp->next;   
    

  310.             p_node = head->npoint;   
    

  311.             printf("Closed库数据！[%d,%d]\n", p_node->col, p_node->row );   
    

  312.             free(p_node);   
    

  313.             free( head );   
    

  314.             Closed -> next = temp;   
    

  315.         }   
    

  316.         printf("\n Closed库数据 数据全部清楚 \n");   
    

  317.         /*  
    

  318.         temp = Closed -> next;  
    

  319.         while(temp != NULL)  
    

  320.         {  
    

  321.             printf("Closed库数据！节点");  
    

  322.             temp = temp->next;  
    

  323.         }*/   
    

  324.         return;   
    

  325.     }   
    

  326. void getPath(int startX, int StartY, int destinationX, int destinationY)   
    

  327. {   
    

  328.     Node *startNode = (Node *) malloc(sizeof(Node));   
    

  329.     Node *bestNode  = NULL;   
    

  330.     int index = 0;   
    

  331.     destinationRow = getRowPosition(destinationY);   
    

  332.     destinationCol = getColPosition(destinationX);   
    

  333.        
    

  334.     startNode->parent= NULL;   
    

  335.     startNode->row = getRowPosition(StartY);   
    

  336.     startNode->col = getColPosition(startX);   
    

  337.     startNode->g = 0;   
    

  338.     startNode->h = getH( startNode->row, startNode->col );   
    

  339.     startNode->f = startNode->g + startNode->h;   
    

  340.     startNode->direction = 0;   
    

  341.     PutintoOpen( startNode );// openNode.add(startNode);   
    

  342.        
    

  343.     while(TRUE)   
    

  344.     {   
    

  345.         bestNode = getNodeFromOpen(); //从OPEN表中取出f值最小的节点   
    

  346.         if(bestNode == NULL)//未找到路径   
    

  347.         {   
    

  348.             printf("未找到路径\n");   
    

  349.             return;   
    

  350.         }   
    

  351.         else if(bestNode->row == destinationRow   
    

  352.                 && bestNode->col == destinationCol )   
    

  353.         {   
    

  354.             Node *_Node = bestNode;   
    

  355.             int nodeSum = 0;   
    

  356.             int nodeIndex =0;   
    

  357.             printf("程序运行次数=%d\n",index);   
    

  358.             while( _Node->parent != NULL )   
    

  359.             {   
    

  360.                 printf("x:%d  y:%d  direction = %d \n", _Node->col, _Node->row, _Node->direction );   
    

  361.                 _Node = _Node->parent;   
    

  362.                 nodeSum += 1;   
    

  363.             }   
    

  364.             printf("节点数量=%d\n",nodeSum);   
    

  365.             _Node = bestNode;   
    

  366.             nodeIndex = nodeSum-1;   
    

  367.             while( _Node->parent != NULL && nodeIndex>=0)   
    

  368.             {   
    

  369.                 Node *_NodeParent = _Node->parent;   
    

  370.                 printf("x:%d  y:%d  direction = %d \n", _Node->col, _Node->row, _Node->direction );   
    

  371.                 if( _NodeParent->col - _Node->col == 0 && _NodeParent->row - _Node->row == +1 )   
    

  372.                 {//从父节点到本节点的操作是  上   
    

  373.                     direction[nodeIndex] = 1;   
    

  374.                 }   
    

  375.                 else if( _NodeParent->col - _Node->col == 0 && _NodeParent->row - _Node->row == -1 )   
    

  376.                 {//从父节点到本节点的操作是  下   
    

  377.                     direction[nodeIndex] = 2;   
    

  378.                 }   
    

  379.                 else if( _NodeParent->col - _Node->col == +1 && _NodeParent->row - _Node->row == 0 )   
    

  380.                 {//从父节点到本节点的操作是  左   
    

  381.                     direction[nodeIndex] = 3;   
    

  382.                 }   
    

  383.                 else if( _NodeParent->col - _Node->col == -1 && _NodeParent->row - _Node->row == 0 )   
    

  384.                 {//从父节点到本节点的操作是  右   
    

  385.                     direction[nodeIndex] = 4;   
    

  386.                 }   
    

  387.                 else   
    

  388.                 {   
    

  389.                     direction[nodeIndex] = 0;   
    

  390.                 }   
    

  391.                 nodeIndex -= 1;   
    

  392.                 _Node = _Node->parent;   
    

  393.             }   
    

  394.             for( nodeIndex=0; nodeIndex<nodeSum; nodeIndex++ )   
    

  395.             {   
    

  396.                 printf("direction[%d]=%d\n",nodeIndex,direction[nodeIndex]);   
    

  397.             }   
    

  398.             return ;   
    

  399.         }   
    

  400.         index++;   
    

  401.         seachSeccessionNode(bestNode);   
    

  402.     }   
    

  403. }   
    

  404. void main()   
    

  405. {//主函数   
    

  406.     //初始操作，建立open和closed表   
    

  407.     Open = (Stack *) malloc(sizeof(Stack));   
    

  408.     Open->next = NULL;   
    

  409.     Closed = (Stack *) malloc(sizeof(Stack));   
    

  410.     Closed->next = NULL;   
    

  411. //-----------------------------------   
    

  412.     getPath( 0, 0, 19, 19 );   
    

  413.     printf("程序认定该起始状态无法道达目标状态!\n");   
    

  414.        
    

  415.     printfOpenData();   
    

  416.     printfClosedData();   
    

  417.     free(Open);   
    

  418.     free(Closed);   
    

  419. //--------------------------------------   
    

  420. }   
    


  421.


回复

收藏 举报

相关帖子
勇敢的大白菜 发表于 2024-5-23 10:16 | 显示全部楼层
寻路算法应该使用在扫地机器人上比较多吧
回复

收藏 举报

自己造声卡 发表于 2024-5-24 10:11 | 显示全部楼层
自动寻路功能，应该是扫地机器人上使用的多一些
回复

收藏 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 注册

本版积分规则

modesty3jonah

32

主题

1794

帖子

2

粉丝
快速回复 在线客服 返回列表 返回顶部