【ROS】costmap之（1）costmap_math

costmap_2d: costmap_math.cpp

costmap_2d: costmap_math.h

1、内联函数[double sign(double x)]

功能：输入x如果小于0返回-1，否则返回1。

2、内联函数[double sign0(double x)]

功能：输入x如果小于0返回-1，如果大于0返回1，否则返回0。

3、内联函数[double distance(double x0, double y0, double x1, double y1)]

功能：输入坐标点P0(x0,y0)、P1(x1,y1)，返回|P0P1|距离。

4、函数[double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1)]

功能：输入坐标点P0(pX,pY)与线段L的坐标点L0(x0,y0)、L1(x1,y1)，返回P0到线段的距离。

设垂点LD(xx,yy)，dot为向量L0P0·L0L1（即|L0L1|和L0P0的投影长度的乘积），len_sq为向量L0L1·L0L1（即|L0L1|和L0L1的投影长度的乘积，其实就是向量L0L1模的平方），param即为L0LD与L0L1模长比例。

当L0P0·L0L1的夹角大于90°时param<0，此时P0距离线段L最近点为L0；

当L0LD的模比L0L1的模大时param>1，此时P0距离线段L最近点为L1；

剩下的情况通过比例可求得垂点为最近点；

由此可求得垂点LD，通过求垂点LD与P0的距离可得P0到直线得距离。

5、函数[bool intersects(std::vector<geometry_msgs::Point>& polygon, float testx, float testy)]

功能：输入多边形顶点集和test点，返回该点是否与多边形相交

判断一个点是否在多边形内部，可以使用引射线的方法

测试点Ptest(testx,testy)

条件1：(yi > testy) != (yj > testy)

只有当yi<esty<yj或者yj<testy<yi时条件成立。

条件2：testx < (xj - xi) * (testy - yi) / (yj - yi) + xi

只有当测试点Ptest在线段i,j下方时，即过测试点Ptest向上画射线(Y轴)，条件1、2同时成立时射线与线段i,j相交。

遍历多边形的边，每次条件1和条件2同时成立时（即射线与边相交）把布尔量取反；当相交次数为奇数时，测试点Ptest在多边形内，否则不在多边形内。（参考PNPoly算法）

6、函数[bool intersects(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2)]

功能：输入两个多边形顶点集，返回两个多边形是否相交

分别取polygon1的每个顶点判断是否在多边形polygon2内部，然后再分别取polygon2的每个顶点判断是否在多边形polygon1内部。