C实现简易运动轨迹检测的程序
2011-4-25
这几天，XXX的项目让我做了一个运动轨迹检测的程序，主要用来检测汽车和给定路线的偏移问题，和定位汽车主要在什么路段上。
如下图
下面是一些算法和资料：
首先，要确定移动的点，也就是汽车和线段之间的距离，不能直接理解成点到直线的距离，因为当出现下面的情况的时候，点P到BC和到A的距离都是一样
的，这样就区分不点到底靠近哪条线上了。很显然，点固然靠近的应该是线段AB，所以我对这种情况进行了一些改进，如果遇到这种情况，那么直接计算P点到B之间的距离。其实这个程序的关键都是算点到线段的距离，而不是点到线段的高，这些要分清楚。
点到线段距离的计算：
点到直线的距离可以直接做垂线求取，但线段是有首尾点的，若要求距离则要考虑首尾点。
点和线段的关系大致可以有下面几种：
有特殊情况，是P点和A或B重合，否则，根据∠PAB或∠PBA的角度可以有图1、图2、图4三种情况（包括点在AB之间，算是零度角）。
我做的算法可以快速、简洁地判断角度是钝角还是锐角。
在求垂线距离时，用海伦公式取代三角函数，使得程序看起来很简洁。
1. double GetPointDistance(CPoint p1, CPoint p2)
2. {
3. return sqrt((p1.x-p2.x)*(p1.x-p2.x)+(p1.y-p2.y)*(p1.y-p2.y));
4. }
5. float GetNearestDistance(CPoint PA, CPoint PB, CPoint P3)
6. {
7.
8. //----------图2--------------------
9. float a,b,c;
10. a=GetPointDistance(PB,P3);
11. if(a<=0.00001)
12. return 0.0f;
13. b=GetPointDistance(PA,P3);
14. if(b<=0.00001)
15. return 0.0f;
16. c=GetPointDistance(PA,PB);
17. if(c<=0.00001)
18. return a;//如果PA和PB坐标相同，则退出函数，并返回距离
19. //------------------------------
20.
21. if(a*a>=b*b+c*c)//--------图3--------
22. return b; //如果是钝角返回b
23. if(b*b>=a*a+c*c)//--------图4-------
24. return a; //如果是钝角返回a
25.
26. //图1
27. float l=(a+b+c)/2; //周长的一半
28. float s=sqrt(l*(l-a)*(l-b)*(l-c)); //海伦公式求面积，也可以用矢量求
29. return 2*s/c;
30. }
下面的程序虽然没用上，还是用经纬度来判断偏移的，不过也贴上，怕以后用到哈。
根据经纬度坐标计算实际距离
double hypot(double x, double y) {
return sqrt(x * x + y * y);
}
double distance(double wd1, double jd1, double wd2, double jd2) {// 根据经纬度坐标计算实际距离
double x, y, out;
double PI = 3.1415926535898;
double R = 6.371229 * 1e6;
x = (jd2 - jd1) * PI * R * cos( ( (wd1 + wd2) / 2) * PI / 180) / 180;
y = (wd2 - wd1) * PI * R / 180;
out = hypot(x, y);
return out;
}
判断汽车轨迹是否偏离的算法其实不难，因为通过计算点到每个线段的距离，找出最小的距离，判断是否小于门限0.0001度（就是50米）就好了。然后返回一个0或者1。嘿嘿，简单吧。
举个例子，比如有8个点，必然有7个线段，依次计算7个线段的长度，然后计算点到7个线段的距离，找出最小的距离，再拿来和门限比较就OK了。
最近我程序构架思想大改，虽然是C语言，但是我引入C++的面向对象思想，运用封装和抽象的思想，大量使用指针和结构体。我用结构体封装了私有变量和函数，同时也提供公共变量和函数。我尽量给用户最简单的和最傻瓜的使用方法，也方便以后自己。
测试平台：Eclipse CDT
三个文件，.h .c 还有个test.c（主要用来演示）
下面是程序：
//Track_Detect.h
#ifndef TRACK_DETECT_H_
#define TRACK_DETECT_H_
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <stdint.h>
#define THRESHOLD_MILES 50
#define THRESHOLD_DEGREE 0.0001
struct Road_coordinate
{
double x;
double y;
};
struct GetNearestDistance_Private
{
double TempA,TempB,TempC;
double perimeter;
double area;
double (*GetPointDistance)(struct Road_coordinate p1, struct Road_coordinate p2);
};
struct MinDistance_Of_Roads_Private
{
uint32_t Data_Size;
uint32_t Data_Resolution;
double *Temp_Data;
double minTemp;
};
struct GPS_Track_Detect_Private
{
//struct Road_coordinate *RXY;
double (*GetNearestDistance)(struct Road_coordinate PA, struct Road_coordinate PB, struct Road_coordinate PC);
struct MinDistance_Of_Roads_Private MinDistance_Of_Roads;
};
struct GPS_Track_Detect_Public
{
struct Road_coordinate *set_p;
uint32_t num;
struct Road_coordinate *move_p;
uint8_t MinNum;
double (*MinDistance_Of_Roads)(struct Road_coordinate *set_p, uint32_t num, struct Road_coordinate *move_p, uint8_t *MinNum);
};
//外部函数
extern double MinDistance_Of_Roads(struct Road_coordinate *set_p, uint32_t num, struct Road_coordinate *move_p, uint8_t *MinNum);
extern uint32_t GPS_Track_Detect(uint32_t *deviate_flag, double *minValue, struct GPS_Track_Detect_Public *p);
//注意set_p和move_p是指针
#define GPS_Track_Detect_MinDistance(set_p, num, move_p, MinNum) MinDistance_Of_Roads(set_p, num, move_p, MinNum)
#endif
//Track_Detect.c
//门限是0.0001度，就是50米
#include "Track_Detect.h"
//私有函数声明区
static double GetPointDistance(struct Road_coordinate p1, struct Road_coordinate p2);
static double GetNearestDistance(struct Road_coordinate PA, struct Road_coordinate PB, struct Road_coordinate PC);
static double GetPointDistance(struct Road_coordinate p1, struct Road_coordinate p2)
{
return sqrt((p1.x-p2.x)*(p1.x-p2.x)+(p1.y-p2.y)*(p1.y-p2.y));
}
static double GetNearestDistance(struct Road_coordinate PA, struct Road_coordinate PB, struct Road_coordinate PC)
{
struct GetNearestDistance_Private *this = (struct GetNearestDistance_Private *) malloc(sizeof(struct GetNearestDistance_Private));
this->GetPointDistance = GetPointDistance;
this->TempA = this->GetPointDistance(PB,PC);
double return_value = 0.0;
if(this->TempA <= 0.00001)
{
free(this);
return 0.0f;
}
this->TempB = this->GetPointDistance(PA,PC);
if(this->TempB <=0.00001 )
{
//return_value = this->TempB;
free(this);
return 0.0f;
}
this->TempC = this->GetPointDistance(PA,PB);
if(this->TempC <= 0.00001)
{
return_value = this->TempA;
free(this);
return return_value;//如果PA和PB坐标相同，则退出函数，并返回距离
}
if(this->TempA * this->TempA >= this->TempB * this->TempB + this->TempC * this->TempC)
{
return_value = this->TempB; //如果是钝角返回TempB
free(this);
return return_value;
}
if(this->TempB * this->TempB >= this->TempA * this->TempA + this->TempC * this->TempC)
{
return_value = this->TempA;
free(this);
return return_value; //如果是钝角返回TempA
}
this->perimeter = (this->TempA + this->TempB + this->TempC) / 2; //周长的一半
this->area = sqrt(this->perimeter * (this->perimeter - this->TempA) * (this->perimeter - this->TempB) * (this->perimeter - this->TempC)); //海伦公式求面积，也可以用矢量求
return_value = 2 * this->area / this->TempC;
free(this);
return return_value;
}
double MinDistance_Of_Roads(struct Road_coordinate *set_p, uint32_t num, struct Road_coordinate *move_p, uint8_t *MinNum)
{
uint32_t i = 0;
double return_value = 0.0;
struct GPS_Track_Detect_Private *this = (struct GPS_Track_Detect_Private *) malloc(sizeof(struct GPS_Track_Detect_Private));
this->GetNearestDistance = GetNearestDistance;
this->MinDistance_Of_Roads.Data_Resolution = sizeof(set_p[0]);
this->MinDistance_Of_Roads.Data_Size = this->MinDistance_Of_Roads.Data_Resolution * num;//动态分配数据空间
this->MinDistance_Of_Roads.Temp_Data = (double *)malloc(this->MinDistance_Of_Roads.Data_Size);
//printf("%d\n", this->MinDistance_Of_Roads.Data_Size);
//printf("%d\n", this->MinDistance_Of_Roads.Data_Resolution);
for(i = 0; i < (this->MinDistance_Of_Roads.Data_Size / this->MinDistance_Of_Roads.Data_Resolution - 1 ); i++)
{
*(this->MinDistance_Of_Roads.Temp_Data + i) = this->GetNearestDistance(set_p[i], set_p[i + 1], *move_p);
//printf("%f\n",*(this->MinDistance_Of_Roads.Temp_Data + i));
}
this->MinDistance_Of_Roads.minTemp = *(this->MinDistance_Of_Roads.Temp_Data + 0);
for(i = 1; i < (this->MinDistance_Of_Roads.Data_Size / this->MinDistance_Of_Roads.Data_Resolution - 1 ); i++)
{
if(*(this->MinDistance_Of_Roads.Temp_Data + i) < this->MinDistance_Of_Roads.minTemp)
{
this->MinDistance_Of_Roads.minTemp = *(this->MinDistance_Of_Roads.Temp_Data + i);
if(MinNum != NULL)
*MinNum = i;
}
}
free(this->MinDistance_Of_Roads.Temp_Data);
return_value = this->MinDistance_Of_Roads.minTemp;
free(this);
//printf("\n\n\n");
return return_value;
}
//deviate_flag中 0 表示不偏移 1表示偏移
uint32_t GPS_Track_Detect(uint32_t *deviate_flag, double *minValue, struct GPS_Track_Detect_Public *p)
{
p->MinDistance_Of_Roads = MinDistance_Of_Roads;
*minValue = p->MinDistance_Of_Roads(p->set_p, p->num, p->move_p, &p->MinNum);
*deviate_flag = THRESHOLD_DEGREE >= *minValue ? 0 : 1;
return 0;
}
// 测试文件，使用范例Track_Detect_test.c
#include "Track_Detect.h"
struct Road_coordinate RoadXY[] = {
{25.037936,98.431538},
{25.035104,98.429070},
{25.046580,98.411377},
{25.049696,98.409866},
{25.050273,98.407812},
{25.053200,98.406574},
{25.056470,98.406300},
{25.056452,98.404327}
};
int main(void) {
struct Road_coordinate test_dat = {.x = 25.050273, .y = 98.407712};
struct GPS_Track_Detect_Public GTDtest_dat;
GTDtest_dat.move_p = &test_dat;
GTDtest_dat.num = 8;
GTDtest_dat.set_p = RoadXY;
double minValue;
uint32_t deviate_flag;
GPS_Track_Detect(&deviate_flag, &minValue, >Dtest_dat);
printf("%d\n",GTDtest_dat.MinNum);
printf("\n\n");
printf("%f\n",minValue);
printf("%d\n",deviate_flag);
printf("\n\n");
uint8_t num;
printf("%f\n", GPS_Track_Detect_MinDistance(RoadXY, 8, &test_dat, &num));
printf("%d\n",num);
return EXIT_SUCCESS;
}
