#include "SelfLoclization.h"
CSelfLoclization::CSelfLoclization(unsigned MapWidth,unsigned MapHeight):LocRange(400,580,0,70,2.0000001,2*PI),//狀態(tài)的范圍
m_cParticleFlt(PARTICLE_NUM,CSelfLoclization::LocRange),m_cGlobalMap(MapHeight,MapWidth)
{

}
CSelfLoclization::~CSelfLoclization(void)
{

}
bool CSelfLoclization::InitSelfLoc()
{
	m_cParticleFlt.InitParticleFlt(m_cGlobalMap);
	//CParticle InitPart;
	//InitPart.XCoord=26740;
	//InitPart.YCoord=2949.5;
	//InitPart.Orientation=1.5*3.14159-(3.14159-2.912);
	//InitPart.Weight=1;
	////m_cParticleFlt.InitParticleFlt(InitPart);
	//m_cParticleFlt.InitParticleFltWithOneInitLoc(InitPart,m_cGlobalMap);
	return true;
}
RobotLoc CSelfLoclization::LoclizeSelf(CCoderBasePosePredict& PosePredictMod,NoisyWheelDis<uniform_random_type1> WheelDisVal,CGlobalMap &GlobalMap)
{
	RobotLoc SelfRobotLoc;
	if (m_cParticleFlt.RunFlt(PosePredictMod,WheelDisVal,GlobalMap,m_LaserData,NumOfData,m_cResamplerForPF))//定位成功運(yùn)行
	{
		SelfRobotLoc=m_cParticleFlt.GetStateExpect();
	}
	else//定位不成功
	{
		//沒定義
	}
	return SelfRobotLoc;
	
}

RobotLoc CSelfLoclization::LoclizeSelf(NoisyWheelDis<uniform_random_type1> WheelDisVal,float* pLaserData)
{
	RobotLoc SelfRobotLoc;
	if (m_cParticleFlt.RunFlt(m_cPosePredictMod,WheelDisVal,m_cGlobalMap,pLaserData,NumOfData,m_cResamplerForPF))//定位成功運(yùn)行
	{
		SelfRobotLoc=m_cParticleFlt.GetStateExpect();
	}
	else//定位不成功
	{
		//沒定義
	}
	return SelfRobotLoc;
}