Esempi in C++ (Cpp) per CPose2D::distance2DTo

Esempio n. 1

File: PathAgent.cpp Progetto: WangRobo/S2BOT

// 判断路径起点，即当前位置能否找到校正路标
bool CPathAgent::StartCalibCheck(void)
{
	TPose2D tpose = g_pGyro->ReadPose();	// 机器人当前位置的大地坐标
	CPose2D robot(tpose);	
	CPose2D center2robot(m_dCalibCenterDistance, 0, 0);		// 查找范围的中心点在机器人坐标系内的坐标
	CPose2D center = robot + center2robot;	// 查找范围的中心点在大地坐标系内的坐标
	// 遍历路标，找到距离0.4米内的路标
	for (int j=0; j<m_iLandMarkNum; j++)
	{
		TPose2D Land = m_pLandMarkPose[j];
		double dis = center.distance2DTo(Land.x, Land.y);
		if (dis <= 0.5) // 0.4m半径的圆范围
		{
			//// 这个路标点满足基本的距离条件，需要进一步判断是否在矩形ROI区域
			//CPoint2D Land2Center(Land.x - center.x(), Land.y - center.y());	// 路标在center坐标系的坐标
			//CPose2D  TurnPose(0,0,-center.phi());							// 旋转到机器人朝向的center坐标系
			//CPoint2D newLand2Center = TurnPose + Land2Center;
			//if (abs(newLand2Center.x()) < 0.2)	// 在矩形ROI区域
			//	return true;
			//else
			//	return false;
			return true;
		}
	}
	return false;
}