sir_pf_share.h

来自「InnovLabSimu在vc++下实现」· C头文件 代码 · 共 86 行

#pragma once
#include <vector>
#include <math.h>
namespace PF_SPA
{
	/**
	*状态变量的范围，及部分与之相关的函数,状态范围的单位为像素数，状态变量范围的坐标系为状态的坐标系，比如机器人世界坐标系
	*/
	struct StateRange 
	{
		StateRange(float Xmin,float Xmax,float Ymin,float Ymax,float OriMin,float OriMax)//三维的默认构造函数
		{
			AddStateRange(0,Xmin,Xmax);
			AddStateRange(1,Ymin,Ymax);
			AddStateRange(2,OriMin,OriMax);
		}
		~StateRange()
		{

		}
		std::vector<std::vector<float>>m_vStateRage;///<状态空间每一维的大小范围
		bool CheckRangeVal(float MinVal,float MaxVal)
		{
			return MaxVal>=MinVal?true:false;
		}
		/**
		*@brief 添加状态范围到范围变量m_vStateRage中
		*
		*@parm DimensionIndex状态维数下标0开始
		*@parm MinVal 当前状态最小值
		*@parm MaxVal 当前状态最大值
		*/
		bool AddStateRange(unsigned DimensionIndex,float MinVal,float MaxVal)
		{
			if (CheckRangeVal(MinVal,MaxVal))
			{			
				
				if (DimensionIndex>=m_vStateRage.size())//若输入的状态维数下标超过了已有范围则直接压入
				{
					std::vector<float>Range;
					Range.push_back(MinVal);
					Range.push_back(MaxVal);
					m_vStateRage.push_back(Range);
				}
				else
				{
					m_vStateRage[DimensionIndex][0]=MinVal;
					m_vStateRage[DimensionIndex][1]=MaxVal;
				}
			}
			return true;
		}	

		/**
		*@brief 查找范围变量m_vStateRage内容，有则为真，否则返回假
		*
		*@parm DimensionIndex状态维数下标0开始
		*@parm MinVal 当前状态最小值
		*@parm MaxVal 当前状态最大值
		*/
		bool QueryStateRange(unsigned DimensionIndex,float& MinVal,float& MaxVal)
		{
			if (DimensionIndex<m_vStateRage.size())//有效范围内
			{
				MinVal=m_vStateRage[DimensionIndex][0];
				MaxVal=m_vStateRage[DimensionIndex][1];
				return true;
			}
			else
			{
				return false;
			}
		}
		/**
		*@brief 查找范围变量m_vStateRage的维数,返回的维数是从1开始的
		*
		*/
		unsigned QueryStateDimension()
		{
			return m_vStateRage.size();
		}

	};
	

}