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;