/*************************************************************************
	>?File?Name:?a_star.cpp
	>?Author:?TAI?Lei
	>?Mail:?ltai@ust.hk
	>?Created?Time:?Sat?Jul?20?12:38:43?2019
?************************************************************************/

#include?
#include?
#include?
#include?
#include?
#include?
#include?
#include?
using?namespace?std;


class?Node{
public:
??int?x;
??int?y;
??float?sum_cost;
??Node*?p_node;

??Node（int?x_?int?y_?float?sum_cost_=0?Node*?p_node_=NULL）:x（x_）?y（y_）?sum_cost（sum_cost_）?p_node（p_node_）{};
};


std::vector?>?calc_final_path（Node?*?goal?float?reso?cv::Mat&?img?float?img_reso）{
??std::vector?rx;
??std::vector?ry;
??Node*?node?=?goal;
??while?（node->p_node?!=?NULL）{
????node?=?node->p_node;
????rx.push_back（node->x?*?reso）;
????ry.push_back（node->y?*?reso）;
????cv::rectangle（img
????????cv::Point（node->x*img_reso+1?node->y*img_reso+1）
????????cv::Point（（node->x+1）*img_reso?（node->y+1）*img_reso）
????????cv::Scalar（255?0?0）?-1）;
??}
??return?{rx?ry};
}


std::vector?>?calc_obstacle_map（
????std::vector?ox?std::vector?oy
????const?int?min_ox?const?int?max_ox
????const?int?min_oy?const?int?max_oy
????float?reso?float?vr
????cv::Mat&?img?int?img_reso）{

??int?xwidth?=?max_ox-min_ox;
??int?ywidth?=?max_oy-min_oy;

??std::vector?>?obmap（ywidth?vector（xwidth?0））;

??for（int?i=0;?i????int?x?=?i?+?min_ox;
????for（int?j=0;?j??????int?y?=?j?+?min_oy;
??????for（int?k=0;?k????????float?d?=?std::sqrt（std::pow（（ox[k]-x）?2）+std::pow（（oy[k]-y）?2））;
????????if?（d?<=?vr/reso）{
??????????obmap[i][j]?=?1;
??????????cv::rectangle（img
????????????????????????cv::Point（i*img_reso+1?j*img_reso+1）
????????????????????????cv::Point（（i+1）*img_reso?（j+1）*img_reso）
????????????????????????cv::Scalar（0?0?0）?-1）;
??????????break;
????????}
??????}
????}
??}
??return?obmap;
}


bool?verify_node（Node*?node
?????????????????vector?>?obmap
?????????????????int?min_ox?int?max_ox
?????????????????int?min_oy?int?max_oy）{
??if?（node->x?y?x?>=?max_ox?||?node->y?>=?max_oy）{
????return?false;
??}

??if?（obmap[node->x-min_ox][node->y-min_oy]）?return?false;

??return?true;
}


float?calc_heristic（Node*?n1?Node*?n2?float?w=1.0）{
??return?w?*?std::sqrt（std::pow（n1->x-n2->x?2）+std::pow（n1->y-n2->y?2））;
}

std::vector?get_motion_model（）{
??return?{Node（1???0??1）
??????????Node（0???1??1）
??????????Node（-1???0??1）
??????????Node（0???-1??1）
??????????Node（-1???-1??std::sqrt（2））
??????????Node（-1???1??std::sqrt（2））
??????????Node（1???-1??std::sqrt（2））
??????????Node（1????1??std::sqrt（2））};
}

void?a_star_planning（float?sx?float?sy
?????????????????????float?gx?float?gy
?????????????????????

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2019-07-29?06:44??CppRobotics-master\
?????文件??????????40??2019-07-29?06:44??CppRobotics-master\.gitignore
?????文件????????2064??2019-07-29?06:44??CppRobotics-master\CMakeLists.txt
?????文件????????1074??2019-07-29?06:44??CppRobotics-master\LICENSE.md
?????目錄???????????0??2019-07-29?06:44??CppRobotics-master\gif\
?????文件?????1424868??2019-07-29?06:44??CppRobotics-master\gif\a_star.gif
?????文件???????41995??2019-07-29?06:44??CppRobotics-master\gif\csp.png
?????文件?????1965595??2019-07-29?06:44??CppRobotics-master\gif\dijkstra.gif
?????文件??????162704??2019-07-29?06:44??CppRobotics-master\gif\dwa.gif
?????文件??????548715??2019-07-29?06:44??CppRobotics-master\gif\ekf.gif
?????文件??????379287??2019-07-29?06:44??CppRobotics-master\gif\frenet.gif
?????文件??????186300??2019-07-29?06:44??CppRobotics-master\gif\lqr_full.gif
?????文件??????159746??2019-07-29?06:44??CppRobotics-master\gif\lqr_steering.gif
?????文件??????257280??2019-07-29?06:44??CppRobotics-master\gif\mpc.gif
?????文件???????26447??2019-07-29?06:44??CppRobotics-master\gif\mptg.gif
?????文件?????1160296??2019-07-29?06:44??CppRobotics-master\gif\pf.gif
?????文件??????110500??2019-07-29?06:44??CppRobotics-master\gif\rrt.gif
?????文件??????130987??2019-07-29?06:44??CppRobotics-master\gif\slp.gif
?????目錄???????????0??2019-07-29?06:44??CppRobotics-master\include\
?????文件?????????556??2019-07-29?06:44??CppRobotics-master\include\cpprobotics_types.h
?????文件????????2279??2019-07-29?06:44??CppRobotics-master\include\csv_reader.h
?????文件????????4244??2019-07-29?06:44??CppRobotics-master\include\cubic_spline.h
?????文件?????????809??2019-07-29?06:44??CppRobotics-master\include\frenet_path.h
?????文件????????4013??2019-07-29?06:44??CppRobotics-master\include\motion_model.h
?????文件????????1591??2019-07-29?06:44??CppRobotics-master\include\quartic_polynomial.h
?????文件????????1913??2019-07-29?06:44??CppRobotics-master\include\quintic_polynomial.h
?????文件????????5138??2019-07-29?06:44??CppRobotics-master\include\rrt.h
?????文件????????5817??2019-07-29?06:44??CppRobotics-master\include\trajectory_optimizer.h
?????文件?????????745??2019-07-29?06:44??CppRobotics-master\include\visualization.h
?????文件????????2713??2019-07-29?06:44??CppRobotics-master\lookuptable.csv
?????文件????????5320??2019-07-29?06:44??CppRobotics-master\readme.md
............此處省略14個文件信息