hpm6750/controller_yy_app/controlware/control_inc/geomatry.h

#ifndef GEOMATRY_H
#define GEOMATRY_H

#include <stdbool.h>

//二维坐标或向量
typedef struct
{
    float x;
    float y;
} Coords2df;

//二维坐标或向量
typedef struct
{
    int x;
    int y;
} Coords2dint;

//三维坐标或向量
typedef struct
{
    float x;
    float y;
    float z;
} Coords3df;

//三维坐标或向量
typedef struct
{
    int x;
    int y;
    int z;
} Coords3dint;

typedef struct
{
    double x;
    double y;
} Coords2ddouble;

typedef struct
{
    double x;
    double y;
    double z;
} Coords3ddouble;

typedef Coords2df Vector2df;
typedef Coords2ddouble Vector2ddouble;
typedef Coords3df Vector3df;
typedef Coords3ddouble Vector3ddouble;
typedef Coords2dint Vector2dint;
typedef Coords3dint Vector3dint;

// 两直线相交
Coords2df lineInterLine(const Vector3ddouble *lineParama,
                        const Vector3ddouble *lineParamb);

// 两点求直线方程
Vector3ddouble calLineEquation(const Coords2ddouble *pa,
                               const Coords2ddouble *pb);

// 求点是否在多边行内
bool PointInPolygon(const Coords3dint *point, const Coords3dint *polygon,
                    int count);

// 求二维平面点到线段的最短距离
void PointToSegmentDist(const Coords2dint *p, const Coords2dint *segment_a,
                        const Coords2dint *segment_b, float *dist,
                        float *angle);

// 求点到多边形的最短距离
void PointToPolygonDist(const Coords3dint *p, const Coords3dint *polygon,
                        int count, float *dist, float *angle);

void Cal_LngLat_To_FlatM_Ratio(double lat, double lng, double *ratiox,
                               double *ratioy);

void LanLat_To_Flat_Dist_Bearing(int lon1, int lat1, int lon2, int lat2,
                                 double lonToM, double latToM, float *dist_m,
                                 float *bearing_rad);

#endif