凸多边形最小距离计算方法

凸多边形不相交最新距离计算实现

头文件定义

#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;
}

参考:

计算凸多边形间的距离 - 知乎