凸多边形不相交最新距离计算实现
头文件定义
#ifndef GEOMETRYUTILLS_HPP
#define GEOMETRYUTILLS_HPP
#include <cmath>
#include <algorithm>
#include <vector>
namespace DLLGeometry
{
struct Point
{
public:
float x, y;
Point() {}
Point(float x, float y) : x(x), y(y) {}
Point operator-(const Point &p) { return Point(x - p.x, y - p.y); }
bool operator<(const Point &a) const
{
if (x != a.x)
return x < a.x;
else
return y < a.y;
}
float dot(const Point &p) { return x * p.x + y * p.y; }
float det(const Point &p) { return x * p.y - y * p.x; }
};
/**
* @brief //两凸包求距
*
* @param P 矩形1
* @param Q 矩形2
* @return double
*/
float solve(std::vector<Point>& P, std::vector<Point>& Q);
/**
* @brief 根据中心点,Yaw及宽高求矩形点
*
* @param centorx 目标中心点X
* @param centory 目标中心点Y
* @param width 目标宽度
* @param length 目标长度
* @param yaw 偏航角,与车辆前进方向相同为0,顺时针[0,2Pi]
* @param rect
* @return double
*/
void GetCarRect(float centorx,float centory, float width,float length,float yaw,std::vector<Point>& rect);
} // namespace DLLGeometry
#endif
源文件
#include "geometryutills.hpp"
#define sqr(a) ((a) * (a))
#define dis(a, b) sqrt(sqr(a.x - b.x) + sqr(a.y - b.y))
namespace DLLGeometry
{
const float pi = acos(-1.0);
const float eps = 1e-10;
const int inf = 0x3f3f3f3f;
//AB与AC的叉积,正表示C在向量AB的逆时针方向
float cross(Point A, Point B, Point C)
{
return (B - A).det(C - A);
}
//AB与AC的点积,0则垂直
float multi(Point A, Point B, Point C)
{
return (B - A).dot(C - A);
}
void anticlockwise_sort(Point *p, int N)
{
for (int i = 0; i < N - 2; i++)
{
float tmp = cross(p[i], p[i + 1], p[i + 2]);
if (tmp > eps)
return;
else if (tmp < -eps)
{
std::reverse(p, p + N);
return;
}
}
}
//C到线段AB的距离
float point_to_line(Point A, Point B, Point C)
{
if (dis(A, B) < eps)
return dis(B, C);
if (multi(A, B, C) < -eps)
return dis(A, C);
if (multi(B, A, C) < -eps)
return dis(B, C);
return fabs(cross(A, B, C) / dis(A, B)); //面积
}
//两线段AB到CD的距离
float line_to_line(Point A, Point B, Point C, Point D)
{
return fmin(fmin(point_to_line(A, B, C), point_to_line(A, B, D)),
fmin(point_to_line(C, D, A), point_to_line(C, D, B)));
}
//两凸包求距
//
float solve(std::vector<Point>& P, std::vector<Point>& Q)
{
int n = P.size();
int m = Q.size();
int yminP = 0, ymaxQ = 0;
for (int i = 0; i < n; i++)
if (P[i].y < P[yminP].y)
yminP = i;
for (int i = 0; i < m; i++)
if (Q[i].y > Q[ymaxQ].y)
ymaxQ = i;
std::vector<Point> tmpP =P;
std::vector<Point> tmpQ =Q;
tmpP.push_back(P[0]);
tmpQ.push_back(Q[0]);
float arg, ans = inf;
for (int i = 0; i < n; i++)
{ //枚举p上的点
while ((arg = (cross(tmpP[yminP + 1], tmpQ[ymaxQ + 1], tmpP[yminP]) -
cross(tmpP[yminP + 1], tmpQ[ymaxQ], tmpP[yminP]))) > eps)
{
ymaxQ = (ymaxQ + 1) % m;
}
ans = fmin(ans, line_to_line(tmpP[yminP], tmpP[yminP + 1], tmpQ[ymaxQ], tmpQ[ymaxQ + 1]));
yminP = (yminP + 1) % n;
}
return ans;
}
/**
* @brief 根据中心点,Yaw及宽高求矩形点
*
* @param centorx
* @param centory
* @param width
* @param height
* @param rect
* @return double
*/
void GetCarRect(float centorx,float centory, float width,float length,float yaw,std::vector<Point>& rect)
{
rect.resize(4);
rect[0].x =centorx-width/2 ;
rect[0].y =centory-length/2;
rect[1].x =centorx+width/2;
rect[1].y =centory-length/2;
rect[2].x =centorx+width/2;
rect[2].y =centory+length/2;
rect[3].x =centorx-width/2;
rect[3].y =centory+length/2;
float x,y;
yaw =2*M_PI -yaw;
for(auto& pt :rect)
{
x =(pt.x -centorx)*cos(yaw) -(pt.y -centory) *sin(yaw)+centorx;
y =(pt.x -centorx)*sin(yaw) +(pt.y -centory) *cos(yaw)+centory;
pt.x =x;
pt.y =y;
}
return;
}
}
demo
#include <iostream>
#include <fstream>
#include "geometryutills.hpp"
int main() {
std::vector<DLLGeometry::Point> pts1(4);
pts1.resize(4);
pts1[0] =DLLGeometry::Point{0,0};
pts1[1] =DLLGeometry::Point{-10.0,0.0};
pts1[2] =DLLGeometry::Point{-10.0,-10.0};
pts1[3] =DLLGeometry::Point{0.0,-10.0};
std::vector<DLLGeometry::Point> pts2(4);
pts2.resize(4);
pts2[0] =DLLGeometry::Point{10,0};
pts2[1] =DLLGeometry::Point{0,10};
pts2[2] =DLLGeometry::Point{10.0,15.0};
pts2[3] =DLLGeometry::Point{15.0,10.0};
double dis = DLLGeometry::solve(pts1,pts2);
std::cout<<"distance:"<<dis<<std::endl;
std::vector<DLLGeometry::Point> carpnts1;
GetCarRect(0,0,10,10,0,carpnts1);
std::vector<DLLGeometry::Point> carpnts2;
GetCarRect(20,30,10,10,M_PI,carpnts2);
double dis2 = DLLGeometry::solve(carpnts1,carpnts2);
std::cout<<"distance2:"<<dis2<<std::endl;
return 0;
}
参考: