一个简单的镜头稳定算法

需求是这样的：

比如飞机机头挂者一个摄像头，那么该摄像头自然会随飞机上下左右移动。但一般摄像头会先固定于一个挂架上，挂架内装有陀螺仪，可以稳定姿态。

即当飞机机头方向改变后，摄像头不会马上跟随飞机移动，而是慢慢跟随飞机移动，具有一定的自稳定性。如果机头摆动幅度很大，则会很快跟随，如果机头在一个位置左右小幅度摆动，则摄像头挂架能自己消除这种抖动。

现在任务就是用一段简单的代码来近似模拟这种情况。

基本的思路是这样：摄像头固定于挂架上，方向随挂架方向改变，摄像头本身也可以上下左右改变方向。挂架固定于机头，机头方向改变后，挂架向机头方向运动，运动速度与挂架与机头之间的夹角相关。夹角越大，速度越快。



代码如下：

/************************************************************************
**	时  间:		2014年12月19日   13:28
**	文件名: 	SightTools.h
**	创建人:		limenghua
**	邮  箱:		576566385@qq.com
**	主要功能:	 观瞄具抽象基类
*************************************************************************/
#ifndef SightTools_h__
#define SightTools_h__

namespace gyj{
namespace attack{

inline float ClampValue(float value,float min,float max)
{
return value < min ? min : (value > max ? max : value);
}

class SightTool{
public:
SightTool():
_heading(0),_pitch(0),
_maxHeading(360),_minHeading(-360),
_maxPitch(180),_minPitch(-180)
{
}
SightTool(float minHeading,float maxHeading,float minPitch,float maxPitch):
_heading(0),_pitch(0),
_minHeading(minHeading),_maxHeading(maxHeading),
_minPitch(minPitch),_maxPitch(maxPitch)
{
}

float Heading()
{
return _heading;
}
void  Heading(float value)
{
_heading = ClampValue(value,_minHeading,_maxHeading);
}
float Pitch()
{
return _pitch;
}
void  Pitch(float value)
{
_pitch = ClampValue(value,_minPitch,_maxPitch);
}

private:
float _heading;
float _pitch;
float _maxHeading;
float _minHeading;
float _maxPitch;
float _minPitch;
};

}
}

#endif // SightTools_h__

/************************************************************************
**	时  间:		2014年12月19日   14:06
**	文件名: 	MFDSightTools.h
**	创建人:		limenghua
**	邮  箱:		576566385@qq.com
**	主要功能:	 MFD瞄准具,增加自稳定算法，需要定期调用其Update
*************************************************************************/
#ifndef MFDSightTools_h__
#define MFDSightTools_h__

#include "SightTools.h"

namespace gyj{
namespace attack{
const float  MIN_MFD_SIGHT_HEADING = -120;
const float  MAX_MFD_SIGHT_HEADING = 120;
const float  MIN_MFD_SIGHT_PITCH  = -60;
const float  MAX_MFD_SIGHT_PITCH  = 30;

const double FRAME_PER_SECOND = 60.0;
const double VERY_SMALL_NUMBER   = 0.0001;

const double DISTANCE_QUOTIETY = 1;
//自稳定算法1
//取 x(t) = 1/( t + 1/D)   D > 0
//   x(t) = 1/(1/D - t)    D < 0
//	 x(t) = 0   (D = 0)
//	 取值范围，t(0,正无穷）,x(D,0)
//   函数图像可以参考VelocityFunction.bmp
//可以根据需要替换为其他算法
inline double VelocityFunction(double deviation,double secondsElapsedFromLastUpdate)
{
double x = 0;
double t = secondsElapsedFromLastUpdate / DISTANCE_QUOTIETY;
double D = deviation;
if(deviation > VERY_SMALL_NUMBER)
{
x =   1/( t + 1/D) ;
}
else if(deviation < -VERY_SMALL_NUMBER)
{
x = 1/(1/D - t);
}
else {
x =0;
}
return x;
}

class MFDSightTool:public SightTool
{
public:
MFDSightTool():
SightTool(MIN_MFD_SIGHT_HEADING,MAX_MFD_SIGHT_HEADING,
MIN_MFD_SIGHT_PITCH,MAX_MFD_SIGHT_PITCH),
_holderHeading(0),_holderPitch(0),
_planeHeading(0),_planePitch(0)
{
}

template<class Function>
void Update(double secondsElapsedFromLastUpdate,Function f)
{
double deviation = _holderHeading - _planeHeading;
deviation = f(deviation,secondsElapsedFromLastUpdate);
_holderHeading = _planeHeading + deviation;

deviation = _holderPitch - _planePitch;
deviation = f(deviation,secondsElapsedFromLastUpdate);
_holderPitch = _planePitch + deviation;
}

void Update(double secondsElapsedFromLastUpdate = 1/FRAME_PER_SECOND)
{
Update(secondsElapsedFromLastUpdate,VelocityFunction);
}
double ActureHeading()
{
return Heading() + _holderHeading - _planeHeading;
}
double ActurePitch()
{
return Pitch() + _holderPitch - _planePitch;
}

void PlaneHeading(double value)
{
_planeHeading = value;
}
void PlanePitch(double value)
{
_planePitch = value;
}

private:
double _planePitch;
double _planeHeading;
double _holderPitch;
double _holderHeading;

};

} //namespace gyj
} //namespace atack

#endif // MFDSightTools_h__