objectanglecheck.cpp

InnovLabSimu在vc++下实现

// ObjectAngleCheck.cpp: implementation of the ObjectAngleCheck class.
//
//////////////////////////////////////////////////////////////////////
#pragma once
//#include "stdafx.h"
#include "ObjectAngleCheck.h"
#include <math.h>
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

ObjectAngleCheck::ObjectAngleCheck()
{

}

ObjectAngleCheck::~ObjectAngleCheck()
{

}
double ObjectAngleCheck::GetShortDistanceBetween2Angle(double Angle1, double Angle2)
{
		double DisOfTarA2SelfA=fabs(Angle1-Angle2);
		if (DisOfTarA2SelfA>180)
		{
			DisOfTarA2SelfA=360-DisOfTarA2SelfA;
		}
		return DisOfTarA2SelfA;
}
// ***************************************************************
//  关于是否横跨分界线的判明原则 date: 02/24/2007
//  -------------------------------------------------------------
//  原来:
// 			假定障碍物不跨，求出左右边界，若满足CenterAngle>RBAngle或Center<LBAngle
// 			则判定障碍物为横跨左右边界，那么再球一次，这次改为如下原则找左右边界
// 			四个顶点所成角度的正角度中最小的为右边界，负角度中最大的为左边界*/
// 	问题:
// 			四个顶点依次为:168,139,-172,155,中心为157.882(跨了边界)
// 			假设不垮计算得到的左右边界分别为-172,168，此时判断不满足横跨条件
// 			原因在于，此时横跨，不横跨都有可能。规则不适合
//   -------------------------------------------------------------
//	改进:   其实并无完善的解决，现更具仿真情况，做出假定：物体的横跨角度不可能超过200度
// 			因此，在判断是否横跨时，添加判定条件：左右边界角有正有负时，判断CenterAngle离0更近还是离+-180更近
// 			与已有条件的关系为或
//  Copyright (C) 2007 - All Rights Reserved
// ***************************************************************
// 
// ***************************************************************
bool ObjectAngleCheck::CheckObstacleIsBehind(double dLBoundaryAngle,double dCenterAngle,double dRightBoundaryAngle)
{
//新添加的横跨360度分界线的判断标准，见GetObstacleList中的说明
	if ((dCenterAngle<dLBoundaryAngle||dCenterAngle>dRightBoundaryAngle)||
		(fabs(dRightBoundaryAngle-dLBoundaryAngle)>fabs(dRightBoundaryAngle))&&fabs(dCenterAngle-180)>110)
	{
		return true;
	}
	else
	{
		return false;
	}
}
double ObjectAngleCheck::GetAngulardisplacementIn2Angle(double AnglePre, double AngleLatter)
{
	double AngleV=AngleLatter-AnglePre;
    if (AngleV>3.141592678)
	{
		AngleV=AngleV-6.283185356;
	}        
    else if (AngleV<-3.141592678)
	{
		AngleV=AngleV+6.283185356;
	}
    return AngleV;
}
double ObjectAngleCheck::GetAngulardisplacementIn2Angle_Degree(double AnglePre, double AngleLatter)
{
	double AngleV=AngleLatter-AnglePre;
	if (AngleV>180)
	{
		AngleV=AngleV-360;
	}        
	else if (AngleV<-180)
	{
		AngleV=AngleV+360;
	}
	return AngleV;
}
void ObjectAngleCheck::LimitAngleIn0_2Pi(double &AngleInRadius)
{
	if (AngleInRadius<0)
	{
		double k=ceil(-AngleInRadius/(CONSTTWOPI));
		AngleInRadius=AngleInRadius+k*CONSTTWOPI;
		return;
	}
	if (AngleInRadius>0)
	{
		double k=floor(AngleInRadius/(CONSTTWOPI));
		AngleInRadius=AngleInRadius-k*CONSTTWOPI;
		return;
	}
	/*theAngle=a;*/
}
//template <class type>
void ObjectAngleCheck::LimitAngleIn0_2Pi(float &AngleInRadius)
{
	if (AngleInRadius<0)
	{
		double k=ceil(-AngleInRadius/(CONSTTWOPI));
		AngleInRadius=AngleInRadius+k*CONSTTWOPI;
		return;
	}
	if (AngleInRadius>0)
	{
		double k=floor(AngleInRadius/(CONSTTWOPI));
		AngleInRadius=AngleInRadius-k*CONSTTWOPI;
		return;
	}
	/*theAngle=a;*/
}
void ObjectAngleCheck::LimitAngleIn0_360Degree(double &AngleInDegree)
{
	if (AngleInDegree<0)
	{
		double k=ceil(-AngleInDegree/360);
		AngleInDegree=AngleInDegree+k*360;
		return;
	}
	if (AngleInDegree>0)
	{
		double k=floor(AngleInDegree/(360));
		AngleInDegree=AngleInDegree-k*360;
		return;
	}
	//if (AngleInDegree>360)//->i，j越界处理
	//{
	//	AngleInDegree=AngleInDegree-360;
	//	return ;//true;//->返回过界标志
	//}//->错误报警			
	//else if (AngleInDegree<0) 
	//{
	//	AngleInDegree=360+AngleInDegree;
	//	return ;//true;//->返回过界标志
	//}
	//else
	//{
	//	return ;//false;//->未过界
	//}
	/*theAngle=a;*/
}
double ObjectAngleCheck::TransAngleToDegree(double Angle)
{
	return Angle*180/PI;
}
#define DEGREE2ARC_PARM 0.017453292519943294444444444444444//PI/180
double ObjectAngleCheck::TransAngleToArc(double Angle)
{
	return Angle*DEGREE2ARC_PARM;
}