C实现简易运动轨迹检测的程序

2011-4-25

这几天，XXX的项目让我做了一个运动轨迹检测的程序，主要用来检测汽车和给定路线的偏移问题，和定位汽车主要在什么路段上。

如下图

下面是一些算法和资料：

首先，要确定移动的点，也就是汽车和线段之间的距离，不能直接理解成点到直线的距离，因为当出现下面的情况的时候，点P到BC和到A的距离都是一样

的，这样就区分不点到底靠近哪条线上了。很显然，点固然靠近的应该是线段AB，所以我对这种情况进行了一些改进，如果遇到这种情况，那么直接计算P点到B之间的距离。其实这个程序的关键都是算点到线段的距离，而不是点到线段的高，这些要分清楚。

点到线段距离的计算：

点到直线的距离可以直接做垂线求取，但线段是有首尾点的，若要求距离则要考虑首尾点。

点和线段的关系大致可以有下面几种：

有特殊情况，是P点和A或B重合，否则，根据∠PAB或∠PBA的角度可以有图1、图2、图4三种情况（包括点在AB之间，算是零度角）。

我做的算法可以快速、简洁地判断角度是钝角还是锐角。

在求垂线距离时，用海伦公式取代三角函数，使得程序看起来很简洁。

1.doubleGetPointDistance(CPoint p1, CPoint p2)

2.{

3.returnsqrt((p1.x-p2.x)(p1.x-p2.x)+(p1.y-p2.y)(p1.y-p2.y));

4.}

5.floatGetNearestDistance(CPoint PA, CPoint PB, CPoint P3)

6.{

7.

8.//----------图2--------------------

9.floata,b,c;

10.a=GetPointDistance(PB,P3);

11.if(a<=0.00001)

12.return0.0f;

13.b=GetPointDistance(PA,P3);

14.if(b<=0.00001)

15.return0.0f;

16.c=GetPointDistance(PA,PB);

17.if(c<=0.00001)

18.returna;//如果PA和PB坐标相同，则退出函数，并返回距离

19.//------------------------------

20.

21.if(aa>=bb+c*c)//--------图3--------

22.returnb;//如果是钝角返回b

23.if(bb>=aa+c*c)//--------图4-------

24.returna;//如果是钝角返回a

25.

26.//图1

27.floatl=(a+b+c)/2;//周长的一半

28.floats=sqrt(l(l-a)(l-b)*(l-c));//海伦公式求面积，也可以用矢量求

29.return2*s/c;

30.}

下面的程序虽然没用上，还是用经纬度来判断偏移的，不过也贴上，怕以后用到哈。

根据经纬度坐标计算实际距离

doublehypot(doublex,doubley) {

returnsqrt(x * x + y * y);

}



doubledistance(doublewd1,doublejd1,doublewd2,doublejd2) {//根据经纬度坐标计算实际距离

doublex, y,out;

doublePI = 3.1415926535898;

doubleR = 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);

returnout;

}

判断汽车轨迹是否偏离的算法其实不难，因为通过计算点到每个线段的距离，找出最小的距离，判断是否小于门限0.0001度（就是50米）就好了。然后返回一个0或者1。嘿嘿，简单吧。

举个例子，比如有8个点，必然有7个线段，依次计算7个线段的长度，然后计算点到7个线段的距离，找出最小的距离，再拿来和门限比较就OK了。

最近我程序构架思想大改，虽然是C语言，但是我引入C++的面向对象思想，运用封装和抽象的思想，大量使用指针和结构体。我用结构体封装了私有变量和函数，同时也提供公共变量和函数。我尽量给用户最简单的和最傻瓜的使用方法，也方便以后自己。

测试平台：EclipseCDT

三个文件，.h .c还有个test.c（主要用来演示）

下面是程序：

//Track_Detect.h

#ifndef TRACK_DETECT_H_

#define TRACK_DETECT_H_

#include

#include

#include

#include

#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("%dn", this->MinDistance_Of_Roads.Data_Size);

//printf("%dn", 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("%fn",*(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("nnn");

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, &GTDtest_dat);

printf("%dn",GTDtest_dat.MinNum);

printf("nn");

printf("%fn",minValue);

printf("%dn",deviate_flag);

printf("nn");

uint8_t num;

printf("%fn", GPS_Track_Detect_MinDistance(RoadXY, 8, &test_dat, &num));

printf("%dn",num);

return EXIT_SUCCESS;

}

原文链接: https://www.cnblogs.com/nickchan/archive/2011/08/19/3104536.html

欢迎关注

微信关注下方公众号，第一时间获取干货硬货；公众号内回复【pdf】免费获取数百本计算机经典书籍