一个简单的镜头稳定算法

需求是这样的：

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

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

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

 

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

 

 

代码如下：

 

/************************************************************************
**	时  间:		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__