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;

}