// 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;
}
// ***************************************************************
//  關(guān)于是否橫跨分界線的判明原則 date: 02/24/2007
//  -------------------------------------------------------------
//  原來:
// 			假定障礙物不跨，求出左右邊界，若滿足CenterAngle>RBAngle或Center<LBAngle
// 			則判定障礙物為橫跨左右邊界，那么再球一次，這次改為如下原則找左右邊界
// 			四個(gè)頂點(diǎn)所成角度的正角度中最小的為右邊界，負(fù)角度中最大的為左邊界*/
// 	問題:
// 			四個(gè)頂點(diǎn)依次為:168,139,-172,155,中心為157.882(跨了邊界)
// 			假設(shè)不垮計(jì)算得到的左右邊界分別為-172,168，此時(shí)判斷不滿足橫跨條件
// 			原因在于，此時(shí)橫跨，不橫跨都有可能。規(guī)則不適合
//   -------------------------------------------------------------
//	改進(jìn):   其實(shí)并無完善的解決，現(xiàn)更具仿真情況，做出假定：物體的橫跨角度不可能超過200度
// 			因此，在判斷是否橫跨時(shí)，添加判定條件：左右邊界角有正有負(fù)時(shí)，判斷CenterAngle離0更近還是離+-180更近
// 			與已有條件的關(guān)系為或
//  Copyright (C) 2007 - All Rights Reserved
// ***************************************************************
// 
// ***************************************************************
bool ObjectAngleCheck::CheckObstacleIsBehind(double dLBoundaryAngle,double dCenterAngle,double dRightBoundaryAngle)
{
//新添加的橫跨360度分界線的判斷標(biāo)準(zhǔn)，見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;//->返回過界標(biāo)志
	//}//->錯誤報(bào)警			
	//else if (AngleInDegree<0) 
	//{
	//	AngleInDegree=360+AngleInDegree;
	//	return ;//true;//->返回過界標(biāo)志
	//}
	//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;
}