object.cpp

本程序是2005年参加中国机器人大赛的比赛程序

// Object.cpp: implementation of the CObject class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "Robocup.h"
#include "Object.h"
#include "afx.h"
#include "robocupdoc.h"
#include "robocupview.h"
#include "debugdlg.h"
#include "Global.h"
#include "object.h"
#include "memory.h"
#include "Decision.h"

#include "math.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif


//////////////////////////////////////////////////////////////////////
// Player Class
//////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
HANDLE Player::m_hComm=NULL;
//FILE *Player::pFile=NULL;

FieldStruct Player::FieldStruct;
BOOL Player::My_Side=FALSE;
UINT Player::wAccuracy=0;
UINT Player::depth[5]={0};





BOOL Player::bIsHaveObstacle(Vector obstacle, Vector destPos )
{
	double a1, a2;
	Vector v1, v2, v3;
	
	v1 = destPos - m_vPos;
	v2 = obstacle - m_vPos;
	v3 = v1 - v2;

	a1 = v1.GetAngle() - v2.GetAngle();
	a2 = v1.GetAngle() - v3.GetAngle();

	if( v2.mod()<10 )
	{
		return FALSE;
	}
	if(fabs(a1) > 180)
		a1 = 360 - fabs(a1);

	if(fabs(a2) > 180 && a2 > 0)
		a2 = a2 - 360;
	if(fabs(a2) > 180 && a2 < 0)
		a2 = 360 + a2;
	
	if( ( fabs(a1) < 90) && (fabs(a2) < 90) )
	{
		double dist=0;
		dist = DistFromPointToLine(obstacle, destPos, m_vPos);
		if(dist < LENGTH )		return TRUE;
	}
	return FALSE;
}


BOOL Player::AvoidObstacle(Vector obstacle, Vector destPos, Vector& TurnPt)
{//机器人到destPos之间是否有障碍obstacle(假设obstacle为障碍),若有，返回TRUE,且设置转折点TurnPt
	TurnPt = destPos;

	if( (obstacle-destPos).mod() < 80)//若障碍和目标的距离小于5cm，则认为没有障碍
		return FALSE;

	double a1, a2;
	Vector v1, v2, v3;
	
	//v1,v2,v3为obstacle,destpos,m_vPos所构成三角形的三个边
	v1 = destPos - m_vPos;//目标和机器人的连线
	v2 = obstacle - m_vPos;//障碍和机器人连线
	v3 = v1 - v2;//目标和障碍的连线

	a1 = v1.GetAngle() - v2.GetAngle();//三角形的一个角
	a2 = v1.GetAngle() - v3.GetAngle();//另一个角

	if(fabs(a1) > 180)//不可能大于180
		a1 = 360 - fabs(a1);

	if(fabs(a2) > 180 && a2 > 0)
		a2 = a2 - 360;
	if(fabs(a2) > 180 && a2 < 0)
		a2 = 360 + a2;
	
	if( (fabs(a1) < 90) && (fabs(a2) < 90) )//都小于90度才有可能是障碍，否则障碍不在机器人和目标之间，即可判断它不是障碍
	{
		double dist;
		dist = DistFromPointToLine(obstacle, destPos, m_vPos);

		if(dist < m_Length*5/4)//obstacle到destPos和m_vPos的连线的距离较小时,认为有障碍
		{
			if(a2 > 0)//设置转折点，障碍点到连线（destPos和m_vPos）的垂线的延长线上一点，此处为3/2*m_Length
			{
				Vector tv = v1.Rotate( -90 );
				tv.Normalize();
				TurnPt = tv * (m_Length*3/2) + v2 + m_vPos;
			}
			else
			{
				Vector tv = v1.Rotate( 90 );
				tv.Normalize();
				TurnPt = tv * (m_Length*3/2) + v2 + m_vPos;
			}

			return TRUE;
		}
	}
	//其他任何一种情况均为无障碍
	return FALSE;
}

BOOL Player::MoveTo(Vector dest)
{
	DWORD time;
	this->dest=dest;
	time=TurnTo(dest);

	
	Vector a=Vector((dest.x-this->m_vPos.x),(dest.y-this->m_vPos.y));
	double angle=a.GetAngle()-(this->Get_m_Angle());
	if(angle>0)
		angle=(((int)angle%360)>180) ? (int)angle%360-360 : (int)angle%360;

	if(angle<0) 
		angle=(((int)(-angle)%360)>180) ? (int)angle%360+360 : (int)angle%360; 

	double V;
	if(fabs(angle)<=90)
	V=230*MAX_VELOCITY/256;
	if(fabs(angle)>90)
	V=-230*MAX_VELOCITY/256;

//	Velocity_MoveTo=V;
//	fprintf(Player::pFile,"********\n%d,MoveTo time=%u elapse=%u\n********\n",this->Get_Num(),timeGetTime()-timestart,time);
	SetMMTimer(time,(DWORD)this,MMProc_MoveTo);
	return 1;
}

/*
BOOL Player::TurnTo2(Vector v)//以一个轮为轴转动
{
	double angle;

	angle = v.GetAngle() - m_Angle;
	if(fabs(angle) > 180)
	{
		if(angle < 0)
			angle = 360 + angle;
		else
			angle = angle - 360;
	}
	TurnAngle2(angle);

	if(m_vL == 0 && m_vR == 0)
	{
		return TRUE;
	}
	else
		return FALSE;
}

*/


BOOL Player::TurnAngle(double angle)//我的想法：先给一个速度，等一段时间，再令速度为0
{//逆时针角度为正，顺时针角度为负
	if(fabs(angle) < 10)//精度为10度
	{
		angle = 0;
		return TRUE;
	}

	if(angle < 0)//顺时针方向转
	{
		angle = -angle;
		double V;
		if(angle > 45)
			V = MAX_VELOCITY;
		else//角度小于45度，速度就渐渐慢下来。 
			V = MAX_VELOCITY * angle / 45;
		BasicAct(0, TRUE, FALSE, V);
	}
	if(angle>0)
	{
		double V;
		if(angle > 45)
			V = MAX_VELOCITY;
		else//角度小于45度，速度就渐渐慢下来。
			V = MAX_VELOCITY * angle / 45;
		BasicAct(0, FALSE, FALSE, V);
	}                       
	if(m_vL == 0 && m_vR == 0)//在此为不可能达到的条件
		return TRUE;
	else
		return FALSE;
}

/*

BOOL Player::TurnAngle2(double angle)//以其中一个轮为轴旋转，其余同TurnAngle()
{//逆时针角度为正，顺时针角度为负
	if(fabs(angle) < 10)
		angle = 0;

	if(angle > 0)
	{
		double R;
		BOOL bIsRightHand;
		BOOL bIsLine;
		double V;

		R = m_Length / 2;
		bIsRightHand = TRUE;
		bIsLine = FALSE;
		
		if(angle > 45)
			V = MAX_VELOCITY;
		else//角度小于45度，速度就渐渐慢下来。
			V = MAX_VELOCITY * angle / 45;

		BasicAct(R, bIsRightHand, bIsLine, V);
		
	}
	else
	{
		angle = -angle;
		double R;
		BOOL bIsRightHand;
		BOOL bIsLine;
		double V;
		R = m_Length / 2;
		bIsRightHand = FALSE;
		bIsLine = FALSE;
		if(angle > 45)
			V = MAX_VELOCITY;
		else//角度小于45度，速度就渐渐慢下来。
			V = MAX_VELOCITY * angle / 45;

		BasicAct(R, bIsRightHand, bIsLine, V);

	}

	if(m_vL == 0 && m_vR == 0)
		return TRUE;
	else
		return FALSE;
}
*/
/*
BOOL Player::GoalKeep(double dist)//守门员动到球和球门中点的连线距球门中点dist的地方
{
	Vector temp;

	temp = g_Ball.Get_vPos() - m_MyGoal;
	temp.Normalize();
	temp *= dist;
	temp += m_MyGoal;

	if(MoveTo(temp))
		return TurnTo(temp);

	return FALSE;
}
*/
/*
BOOL Player::Shoot(double dist)
{


	Vector ball_player;
	Vector ball_goal;
	double angle;
	Vector temp;

	temp = g_Ball.Get_vPos() - m_OpponentGoal;
	temp.Normalize();
	temp *= dist;
	temp = g_Ball.Get_vPos() + temp;

	ball_player = m_vPos - g_Ball.Get_vPos();
	ball_goal = m_OpponentGoal - g_Ball.Get_vPos();

	angle = ball_player.GetAngle() - ball_goal.GetAngle();

	if(fabs(angle) > 180)
		angle = 360 - fabs(angle);

	if(DistFromPointToLine( m_vPos,g_Ball.Get_vPos(), m_OpponentGoal) > 10)
	{
		MoveTo(temp);
	}
	else
	{
		if(fabs(angle) < 90)
		{
			MoveTo(temp);
		}
		else
		{
			if( TurnTo(m_OpponentGoal - g_Ball.Get_vPos()) )
				return MoveTo(g_Ball.Get_vPos());
		}
	}
	return FALSE;
}


BOOL Player::SendBall(double dist, Vector dest)
{
	Vector ball_player;
	Vector ball_goal;
	double angle;
	Vector temp;

	temp = g_Ball.Get_vPos() - dest;
	temp.Normalize();//把两点之间的距离单位化
	temp *= dist;
	temp += g_Ball.Get_vPos() ;

	ball_player = m_vPos - g_Ball.Get_vPos();
	ball_goal = dest - g_Ball.Get_vPos();

	angle = ball_player.GetAngle() - ball_goal.GetAngle();

	if(fabs(angle) > 180)
		angle = 360 - fabs(angle);
 
	if(DistFromPointToLine(m_vPos, g_Ball.Get_vPos(), dest) > 10)
	{
		MoveTo(temp);
	}
	else
	{
		if(fabs(angle) < 90)
		{
			MoveTo(temp);
		}
		else
		{
			if( TurnTo(dest - g_Ball.Get_vPos()) )
				return MoveTo(g_Ball.Get_vPos());
		}
	}

	return FALSE;
}



extern unsigned long timestart;


void Player::Shoot4Debug()
{
	static unsigned int depth2=1;
	Vector a=Vector((this->m_vPos).x-g_Ball.m_vPos.x,(this->m_vPos).y-g_Ball.m_vPos.y);
	if(a.mod()<0||depth2>100)
	{
		depth2=1;
		fprintf(Player::pFile,"depth2=%u shoot4Debug else complete\n",depth2);
		BasicAct(0,0,0,0);
		return ;
	}
	
	if(a.mod()>0 && (depth2<=100))
	{
		depth2++;
		fprintf(Player::pFile,"after read %u ms\n",GetTickCount()-timestart);

		TurnTo(g_Ball.Get_vPos());

		BasicAct(0,0,1,6*MAX_VELOCITY/8);
	//	fprintf(Player::pFile,"TurnAngle4Debug stop at %u ms\n",GetTickCount()-timestart);
	//	WaitForSingleObject(basicact,2000);
		
	//	fprintf(Player::pFile,"after Waitforsingleobject stop at %u ms\n",GetTickCount()-timestart);
		
		fprintf(Player::pFile,"after basicact %u ms\n",GetTickCount()-timestart);
		fprintf(Player::pFile,"Shoot4debug before settimer %u ms,nElaspe=100\n",GetTickCount()-timestart);
		SetMMTimer(400*a.mod()/100,(DWORD)this,MMProc_Shoot4Debug);
		fprintf(Player::pFile,"Shoot4debug after settimer %u ms,nElaspe=100\n",GetTickCount()-timestart);
	
	//	Sleep(1000);
	}
	return;
}
*/
/*
BOOL Player::TurnTo(Vector dest,UINT para_depth)//不准确，很难控制，不好用
{
	Player::depth[this->Get_Num()]=para_depth;
	if(para_depth==0)
	{
		BasicAct(0,0,0,0);
		return TRUE;//认为已经转到
	}
	Vector a=dest-this->Get_vPos();
	double angle=a.GetAngle()-(this->Get_m_Angle());
	if(angle>0)
		angle=(((int)angle%360)>180)?(int)angle%360-360:(int)angle%360;
	if(angle<0) 
		angle=(((int)(-angle)%360)>180)?(int)angle%360+360:(int)angle%360; 

	if(fabs(angle)<15 || fabs(angle)>165 )//认为已经转到,停止转动并返回
	{	
		BasicAct(0,0,0,0);
		return TRUE;
	}
	double V=256*MAX_VELOCITY/256;//	double V=(fabs(angle)>90) ? MAX_VELOCITY:(7*MAX_VELOCITY/8);
	UINT elapse=fabs(angle)>90  ?  1.45*(180-fabs(angle))/2  :  1.45*fabs(angle)/2;
	BOOL b;
	(angle>0)? b=(BOOL)(fabs(angle)<90):b=(BOOL)(fabs(angle)>90);
	if(b)
	{
		Set_vR(V);
		Set_vL(0);
	}
	else
	{
		Set_vR(-V);
		Set_vL(0);
	}
	SendCommand(3*elapse+10);
//	BasicAct(0,b,FALSE,V,1.3*elapse+10);
	Sleep(1.3*elapse+10);
	return TRUE;
}
*/
/*
BOOL Player::MoveToNoOA4Debug(Vector dest)
{
	if((destPos-m_vPos).mod() < 5)//若小于5认为已经到达目标点，设置左右轮速度为0
	{
		m_vL = 0;
		m_vR = 0;
		return TRUE;
	}

//	if(TurnTo())//destPos - m_vPos,旋转到目标方向
		BasicAct(0, TRUE, TRUE, MAX_VELOCITY);//以最大速度走直线到目标

	if(m_vL == 0 && m_vR == 0)
	{
		return TRUE;
	}
	else
	{
		return FALSE;
	}

	Vector a=Vector((dest.x-this->m_vPos.x),(dest.y-this->m_vPos.y));
	double angle=a.GetAngle()-(this->Get_m_Angle());
	if(angle>0)
		angle=(((int)angle%360)>180) ? (int)angle%360-360 : (int)angle%360;

	if(angle<0) 
		angle=(((int)(-angle)%360)>180) ? (int)angle%360+360 : (int)angle%360; 
	double V;
	V=MAX_VELOCITY;
	if(angle>90)
	V=-V;
	BasicAct(0,0,1,V,200);
	return 1;
}
*/


/*
BOOL Player::MoveToWithAngle(Vector destPos, double destAngle)
{
	//到达某点以后并且满足到达该点的方向，没有避障，主要用来射门
	//首先判断距离,达到的话再判断方向
	int depths;//考虑根据自身和目标点的距离来设定循环的深度
	depths=7;
	if(depths <3)
	{
		m_vL = 0;
		m_vR = 0;
		SendCommand();
		return FALSE;
	}

	if((destPos-m_vPos).mod() < 5)//若小于5认为已经到达目标点
	{//到达目标点后判断方向,进行正向处理
		if( fabs(this->Get_m_Angle()-destAngle)< DEFAULT)//DEFAULT为阀值
		{
			m_vL = 0;
			m_vR = 0;
			SendCommand();
			return TRUE;
		}
		else
		{
			TurnTo4Debug(destAngle);//DirectionProcess;
			depths--;
			MoveToWithAngle( destPos, destAngle);
		}
	}
	if(TurnTo4Debug(destPos))//旋转到目标方向
	{	
		BasicAct(0, TRUE, TRUE, MAX_VELOCITY);//以最大速度走直线到目标
		depths--;
		MoveToWithAngle(destPos, destAngle);
	}
	return TRUE;
}
*/
/*
BOOL Player::MoveToWithAngleOA(Vector destPos, double destAngle)
{
//	static	int depth=7;	
//	if(depth < 3)
//	return TRUE;
	Vector TurnPt;
	if((destPos-m_vPos).mod() < 5)//如果到目标距离小于5CM,则已到达目标点
	{
		if(bIsHaveObstacle(destPos, TurnPt))
		{	//有障碍时设置TurnPt,然后递归调用MoveToWithAngleOA(TurnPt，0)
			MoveTo(TurnPt);
		}
		else
		{
			if(fabs(m_Angle-destAngle)>5 )//先转到朝着目标点然后进行直线运动
			{
				TurnTo(destPos - m_vPos);	
			}
		/*	else
			{
				BasicAct(0, TRUE, TRUE, MAX_VELOCITY);//如何给它一点时间
	//		depth--;
				return TRUE;
		//	}
			
			if( destAngle<1.0 )//对方向没有要求，到达该点即可
			{
				m_vL = 0;
				m_vR = 0;
				SendCommand();
	//		depth--;
				return TRUE;
			}
		}
	}
	else
	{
		
//		depth--;
			if(fabs(m_Angle-destAngle)>5 )//先转到朝着目标点然后进行直线运动
			{
				TurnTo(destPos - m_vPos);	
			}
			MoveTo(destPos);
		return TRUE;
	}
//	MoveToWithAngleOA(destPos, destAngle);
	return TRUE;
}

*/




/*
double Player::ShootPeter( Vector p )
{
	Vector rb=Vector(p.x-this->Get_vPos().x , p.y-this->Get_vPos().y);
	if( this->Get_vPos().x<p.x )//在点的后面，暂时不考虑，情况较简单
	{
		if ( (rb.mod()<10) && (fabs(this->Get_m_Angle() )<45 ) )//正向处理条件