|
|
本帖最后由 jjj 于 2020-2-27 10:32 编辑
这个程序已经STM32上实现，
描述如下，已知一条直线，直线的2端的GPS坐标已经精确测定，分别是(X1,Y1) (X2,Y2), 当另一个点(x,y)慢慢的靠近这条线时，求这个点到线的距离， 这个会动的点的坐标是实时测量的，会变， 要连续的计算点的线的距离，根据距离来做下一步操作，
这个程序计算出来是有误差的，因为它只考虑了纬度的变化，没有完整考虑地球的曲率。
我测试的情况是，点与线的距离范围在3KM之内，且线的长度在3KM内时，误差在cm级。
不知道这个东西用处大不大
先上程序。
/*计算点到线的距离 垂直距离 此计算方法适用于近距离计算，采用的是三角形计算面积
x,y机车所在点 纬度与经度
x1,y1,x2,y2为已知两点的直线 ( 纬度与经度为扩大1亿倍的长整形)
*/
#include <stdlib.h>
#define PI 3.1415926535898
#define EARTHR 6371004
#define DIS_RANGE 5000//2698
long long Distance_point2line(long long x, long long y, long long x1, long long y1, long long x2, long long y2)
{
long long Distance;
long long a,b;
long long h,v; //垂直距离与水平距离
double cos_value;
cos_value=cos(x/100000000*PI/180);
if((x1!=x2)&&(y1!=y2))
{
a=(x2-x1)*(y-y1)/(y2-y1)+x1;
b=(y2-y1)*(x-x1)/(x2-x1)+y1;
v=llabs(x-a); //计算出的单位为cm
h=llabs(y-b)*cos_value;
Distance=(EARTHR)*PI* h*v/sqrt(h*h+v*v)/180000000; //计算出的单位为cm
}
else if(x1==x2)
{
h=0;
v=llabs(x-x1);
Distance=(EARTHR)*PI* v /180000000; //计算出的单位为cm
}
else if(y1==y2)
{ h=llabs(y-y1)*cos_value;
v=0;
Distance=(EARTHR)*PI* h /180000000; //计算出的单位为cm
}
printf("dis:%lldcm\r\n",Distance);
return Distance ;
} |
阿莫论坛20周年了！感谢大家的支持与爱护！！
你熬了10碗粥，别人一桶水倒进去，淘走90碗，剩下10碗给你，你看似没亏，其实你那10碗已经没有之前的裹腹了，人家的一桶水换90碗，继续卖。说白了，通货膨胀就是，你的钱是挣来的，他的钱是印来的，掺和在一起，你的钱就贬值了。
|
[复制链接]
发表于 2020-2-26 14:13:39
楼主
