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

2011-4-25

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

如下图

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

 

下面是一些算法和资料:

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

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

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

点到线段距离的计算:

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

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

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

    有特殊情况,是P点和AB重合,否则,根据PABPBA的角度可以有图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_pmove_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;//如果PAPB坐标相同,则退出函数,并返回距离

 

       }

       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_flag0 表示不偏移   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("%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;

}

 

 

 

 

 

 

 

 

 

 

 

 

 

转载于:https://www.cnblogs.com/nickchan/archive/2011/08/19/3104536.html

  • 0
    点赞
  • 0
    评论
  • 0
    收藏
  • 一键三连
    一键三连
  • 扫一扫,分享海报

表情包
插入表情
评论将由博主筛选后显示,对所有人可见 | 还能输入1000个字符
©️2021 CSDN 皮肤主题: 编程工作室 设计师:CSDN官方博客 返回首页
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值