skill.cpp

东南大学最新的校内比赛的源代码

/*****************************************************************************************
 *                                      SEU-3D
 *                     -------------------------------------------------
 * Copyright (c) 2005, Yuan XU<[e-mail]>,Chang'e SHI<[e-mail]>
 * Copyright (c) 2006, Yuan XU<[e-mail]>,Chunlu JIANG<[e-mail]>
 * Southeast University ,China
 * All rights reserved.
 *
 * Additionally,this program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Library General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 ****************************************************************************************/



#include "Skill.h"
#include "Logger.h"
#include "Formation.h"


/*****************************************************************/
/************************** SKILL ********************************/
/*****************************************************************/

Skill::Skill()
{
	
}

Skill::~Skill()
{

}

Vector3f Skill::getPowerForVel( Vector3f driveForce, Vector3f myVel[3], const Vector3f &endVel )
{
	//endVel = NormalizeVel(endVel);
	
	Vector3f ek = endVel - myVel[0];
	Vector3f ek_1 = endVel - myVel[1];
	Vector3f ek_2 = endVel - myVel[2];
	
	Vector3f drivePower;
	drivePower[0] = velPID( ek[0],ek_1[0],ek_2[0] );
	drivePower[1] = velPID( ek[1],ek_1[1],ek_2[1] );
	drivePower[2] = 0;

	//drivePower = normalizeDriveForce( drivePower );//-* no need -- only for test
	
	drivePower = drivePower + driveForce;
	drivePower[2] = 0;

	return normalizeDriveForce( drivePower );
}

const float posPID_thr = 1.9;//1.62;//1.8;

Vector3f Skill::getPowerForVel( const Vector3f &endVel ) const
{
	Vector3f driveForce = WM->getMyDriveForce();
	Vector3f myVel[3];
	myVel[0] = WM->getMyGlobalVel(0);
	myVel[1] = WM->getMyGlobalVel(1);
	myVel[2] = WM->getMyGlobalVel(2);
	
	return getPowerForVel( driveForce, myVel, endVel);
}

float Skill::velPID( float ek, float ek_1, float ek_2 )
{
	const float Kp = 12;//62;
		  float Ki = 10;//100;
	const float Kd = 10;//*5;
	const float A  = 0.4;
	const float B  = 0.1;

	if ( abs(ek) <= B)
		Ki *= 1;
	else
		Ki *= ( A - abs(ek) + B )/A;
	
	float Uk;
	if ( abs(ek) > A )
		Uk = algorithmP( Kp, ek);//P
		//Uk = algorithmPD( Kp, Kd, ek, ek_1, ek_2);//PD
	else
		Uk = algorithmPID( Kp, Ki, Kd, ek, ek_1, ek_2 );//PID

	return Uk;
}

Vector3f Skill::gotoNStop( Vector3f driveForce, Vector3f myVel[3], Vector3f myPos[3], Vector3f endPos )
{	
	Vector3f ek = endPos - myPos[0];
	Vector3f ek_1 = endPos - myPos[1];
	Vector3f ek_2 = endPos - myPos[2];
	
	Vector3f drivePower,driveSpeed;
	if ( ek.Length() < posPID_thr )
	{
		driveSpeed[0] = posPID( ek[0],ek_1[0],ek_2[0]) + (myVel[0])[0];
		driveSpeed[1] = posPID( ek[1],ek_1[1],ek_2[1]) + (myVel[0])[1];
		LOG( 12,"driveSpeed: %f, %f",driveSpeed.x(),driveSpeed.y());
	}
	else
	{
		driveSpeed = setVector3fLength( ek, 10);
		LOG( 12,"full speed!");
	}
	driveSpeed[2] = 0;
	
	drivePower = getPowerForVel( driveForce, myVel, driveSpeed );
	drivePower.z() = 0;
	
	return normalizeDriveForce(drivePower);
}

Action Skill::gotoNStop( const Vector3f &endPos ) const
{
	Vector3f driveForce = WM->getMyDriveForce();
	Vector3f myPos[3], myVel[3];
	myPos[0] = WM->getMyGlobalPos(0);//WM->getMyGlobalPos(0*20);//-* need more test
	myPos[1] = WM->getMyGlobalPos(1);//WM->getMyGlobalPos(1*20);
	myPos[2] = WM->getMyGlobalPos(2);//WM->getMyGlobalPos(2*20);
	myVel[0] = WM->getMyGlobalVel(0);//WM->getMyGlobalVel(0*20);
	myVel[1] = WM->getMyGlobalVel(1);//WM->getMyGlobalVel(1*20);
	myVel[2] = WM->getMyGlobalVel(2);//WM->getMyGlobalVel(2*20);

	Action act;
	Vector3f power = gotoNStop( driveForce, myVel, myPos, endPos );
	act.setDriveForce( power );
	act.setTime( WM->getRealTime() );
	return act;
}

Vector3f Skill::gotoWithMinSpeed( Vector3f driveForce, Vector3f myVel[3], Vector3f myPos[3], Vector3f endPos, float minSpeed )
{
	Vector3f ek = endPos - myPos[0];
	ek.z() = 0;
	Vector3f ek_1 = endPos - myPos[1];
	Vector3f ek_2 = endPos - myPos[2];
	
	Vector3f drivePower,driveSpeed;
	if ( ek.Length() < posPID_thr )
	{
		driveSpeed[0] = posPID( ek[0],ek_1[0],ek_2[0]) + (myVel[0])[0];
		driveSpeed[1] = posPID( ek[1],ek_1[1],ek_2[1]) + (myVel[0])[1];
	}
	else
	{
		driveSpeed = setVector3fLength( ek, 10);
	}
	driveSpeed[2] = 0;
	
	//-* differnt with gotoNStop
	if ( driveSpeed.Length() < minSpeed )
		driveSpeed = setVector3fLength( ek,minSpeed );
	LOG( 12,"driveSpeed: %f, %f, :",driveSpeed.x(),driveSpeed.y());
	//float max_agent_speed = 10;
	//if ( ek.Length()>1.5 ) driveSpeed = setVector3fLength(driveSpeed,max_agent_speed);
	
	drivePower = getPowerForVel( driveForce, myVel, driveSpeed );
	
	return normalizeDriveForce(drivePower);
	
}

Action Skill::gotoWithMinSpeed( Vector3f endPos, float minSpeed )
{
	Vector3f driveForce = WM->getMyDriveForce();
	Vector3f myPos[3], myVel[3];
	myPos[0] = WM->getMyGlobalPos(0);//WM->getMyGlobalPos(0*20);//-* need more test
	myPos[1] = WM->getMyGlobalPos(1);//WM->getMyGlobalPos(1*20);
	myPos[2] = WM->getMyGlobalPos(2);//WM->getMyGlobalPos(2*20);
	myVel[0] = WM->getMyGlobalVel(0);//WM->getMyGlobalVel(0*20);
	myVel[1] = WM->getMyGlobalVel(1);//WM->getMyGlobalVel(1*20);
	myVel[2] = WM->getMyGlobalVel(2);//WM->getMyGlobalVel(2*20);

	Action act;
	Vector3f power = gotoWithMinSpeed( driveForce, myVel, myPos, endPos, minSpeed );
	act.setDriveForce( power );
	act.setTime( WM->getRealTime() );
	return act;
}

float Skill::posPID( float ek, float ek_1, float ek_2)
{
	const float Kp = 5.5;//4;//8;//5;//10;//15;//*3;//4;//km=5;
		  float Ki = 0.47;//0.48;//0.45;//0.5;//0.4;//2.0;//1.0;//*0.4;//0.35;//0.3;//0.4;//0.6;//0.8;//1;//2;//3;//*0.574;//0.287*2;
	const float Kd = 1.0;//0.5;//2;//1.0;//3;//2;//1.5;//1.0;//0.8;//1.0;//1.1;//1.2;//1.5;//2;//1;//*0.5233;//0.785/1.5;	
	const float A  = posPID_thr;//1.5;
	const float B  = 0.1;

	if ( abs(ek) <= B)
		Ki *= 1;
	else
		Ki *= ( A - abs(ek) + B )/A;
	
	float Uk;
	if ( abs(ek) > A )
		Uk = algorithmP( Kp, ek);//P
	else 
		Uk = algorithmPID( Kp, Ki, Kd, ek, ek_1, ek_2 );//PID
	
	return Uk;
}

Action Skill::runStrategicPos() const
{
	return runStrategicPos( WM->getBallGlobalPos());
}

Action Skill::runStrategicPos( const Vector3f &posBall) const
{
	int num = WM->getMyNum();
	Vector3f posStrategic = FM->getStrategicPosition( num, posBall );
	return gotoNStop( posStrategic );	
}

//-* the old intercept, which is good in defence
Action Skill::interceptBall0( Vector3f goal )
{
	Vector3f posIntercept = WM->getInterceptPos( goal );
	posIntercept = projectInterceptPos( posIntercept );
	Vector3f velEnd(0,0,0);
	return runTo( posIntercept, velEnd );
}

Action Skill::interceptBall( Vector3f goal, const bool isKicking)  const
{
	//-* const
	//-- const of close intercept
	const float dist_close_intercept = 1.87;//1.33;//1.87;//1.0f;
	const AngDeg ang_close_intercept = 20.42;//18.83;//20.42;//22.5f;
	const AngDeg velAng_close_intercept = 5.62;//18.95;//5.62;//45.0f;
	const float ball_stop_vel = 1.02;//1.14;//1.02;//1.5f;
	//-- const of my end vel
	const float my_min_end_speed = 0.52;//0.19;//0.52;//0.35f;
	
	Vector3f posInterceptBall = WM->getInterceptBallPos();
	Vector3f posKick = WM->getInterceptPos( goal );
	bool isClostIntercept = false;
	// close intercept
	if ( (WM->getMyGlobalPos()-WM->getBallGlobalPos()).Length() < dist_close_intercept//1.0f 
		&& isThreePointOneLine(WM->getMyGlobalPos(),WM->getBallGlobalPos(),goal,ang_close_intercept)//22.5)//15)
		&& abs(getClipAng(WM->getBallGlobalVel(),WM->getMyGlobalPos()-goal)) < velAng_close_intercept//45
		&& WM->isBallStop(ball_stop_vel)//(1.5f)//(1.0f)//(0.5f)
		)
	{
		isClostIntercept = true;
		posInterceptBall = WM->getBallGlobalPos();
		posKick = posInterceptBall + (goal-posInterceptBall).Normalized()*kick_pos_dist;
	}
	
	AngDeg dribbleAng = getVector3fHorizontalAng(goal-posInterceptBall);
	Vector3f newPos = projectDribblePos(WM->getMyGlobalPos(),
											posInterceptBall,
											dribbleAng);
	LOG( 14,"posInterceptBall: %f, %f, %f",posInterceptBall.x(),posInterceptBall.y(),posInterceptBall.z());
	LOG( 14,"newPos: %f, %f %f",newPos.x(),newPos.y(),newPos.z());

	// for dribble test
	// kick_margin/sim_cycle_time
	float minSpeed = my_min_end_speed;//0.35;
	float speedK = cosDeg(getClipAng(WM->getMyGlobalPos(),newPos,goal));
	minSpeed *= speedK;
	Vector3f velEnd(0,0,0);
	velEnd = goal - posKick;
	velEnd = setVector3fLength( velEnd, minSpeed);
	Action driveAction = runTo( newPos, velEnd );
	
	//-* 2005-11-12
	// very close to the ball, need turn around ball
	/*if ( (WM->getMyGlobalPos()-WM->getBallGlobalPos()).Length() < dist2ball_turn_around//0.35f 
		&& (WM->getMyGlobalPos()-posKick).Length() < dist2kickPos_turn_around//0.35f
		)
	{
		AngDeg angErr = getClipAng(goal-WM->getBallGlobalPos(),WM->getBallGlobalPos()-WM->getMyGlobalPos());
		bool turnClockWise = ( angErr < 0 );
		driveAction = turnAroundPos(WM->getBallGlobalPos(),radious_turn_around,turnClockWise);
	}
	*/
	//-* 2005-11-13
	// ready to kick, dash to the ball
	if ( (  isKicking || shoudIKick(goal))
		&& isClostIntercept )
	{
		driveAction = dashToBall();
	}
	
	return driveAction;
}

Vector3f Skill::projectInterceptPos( Vector3f posIntercept ) const
{
	//-* geometry algorithm-------------------------
	const float block_dist = 0.4;//0.4;//0.33;
	Vector3f posMe = WM->getMyGlobalPos();
	Vector3f posBall = WM->getBallGlobalPos();
	//-* just now, consider 2D only;
	posMe[2] = posBall[2] = posIntercept[2] = 0;
	
	//-* may block the ball
	float distIntercept = ( posIntercept-posMe).Length();
	//LOG( 12,"posBall: %f,%f,%f \tvelBall: %f,%f,%f",posBall[0],posBall[1],posBall[2],WM->getBallGlobalVel()[0],WM->getBallGlobalVel()[1],WM->getBallGlobalVel()[2]);
	//LOG( 12,"distIntercept:%f",distIntercept);
	//LOG( 12,"distToBall:%f",(posBall-posMe).Length() );
	//float const ball_vel_thr = 1.0;
	if ( isThreePointOneLine( posMe, posBall, posIntercept, 90 )
		&& ( distIntercept - (posBall-posMe).Length() > WM->getBallRadius()*2 ) 
		)
	{
		//LOG( 12,"may block");
		float dist = getDistToLine( posBall, posMe, posIntercept );
		//LOG( 12,"dist:%f",dist);
		if ( fabs(dist) < block_dist )//-* block
		{
			//LOG( 12,"block!!!!");
			AngDeg direction = getVector3fHorizontalAng( posIntercept - posMe );
			//LOG( 12,"direction:%f",direction);
			direction += sign( dist ) * Rad2Deg( block_dist / distIntercept );
			//LOG( 12,"new direction:%f",direction);
			direction = normalizeAngle( direction );
			//LOG( 12,"new direction:%f",direction);
			VisionSense pol;
			pol.distance = distIntercept;
			pol.theta = direction;
			pol.phi = 0;
			posIntercept = posMe + getPosRelativeFromVision( pol );
		}
	}
	
	posIntercept.z() = WM->getMyRadius();
	//LOG( 12,"new distIntercept:%f", (posMe-posIntercept).Length());
	//LOG( 12,"new dist:%f",getDistToLine(posBall,posMe,posIntercept));
	return posIntercept;
}

Action Skill::runTo( const Vector3f &endPos, const Vector3f &endVel ) const
{
	Action act;
	act.setTime( WM->getRealTime() );
	act.setDriveForce( runTo( WM->getMyGlobalPos(), WM->getMyGlobalVel(), endPos, endVel) );
	return act;
}

Vector3f Skill::runTo( const Vector3f &beginPos, const Vector3f &beginVel, const Vector3f &endPos, const Vector3f &endVel) const
{
	//const float minPowerOneAxes = 0.1;//meaning 10% of maxPower
	const float maxPower = max_drive_force;
	Vector3f direction = endPos - beginPos;
	if ( direction.Length() < 0.005 ) return Vector3f(0,0,0);
	Vector3f power = direction.Normalized();
	power.x() = abs( power.x());//max( minPowerOneAxes, abs( power.x()));
	power.y() = abs( power.y());//max( minPowerOneAxes, abs( power.y()));
	power.x() *= getRunPower( beginPos.x(),beginVel.x(),endPos.x(),endVel.x(),maxPower);
	power.y() *= getRunPower( beginPos.y(),beginVel.y(),endPos.y(),endVel.y(),maxPower);
	power.z() = 0;
	return power;
}

float Skill::getRunPower( const float &x0, const float &v0, const float &xf, const float &vf, const float &maxPower) const
{
	const float u = 62;
	const float m = 30;
	//-* float t1 = ((xf-x0)*u +(vf-v0)*m)*0.5/maxPower;
	//-- float f = t1/sim_cycle_time*maxPower;
	//-- make it simple @2005-11-20-by-XuYuan
	float f = ((xf-x0)*u +(vf-v0)*m)*0.5/sim_cycle_time;
	if ( abs(f) < maxPower )
	{
		f = ( 2/(1+exp(-2.5*f/maxPower))-1 )*maxPower;//2//5//3
		//f = ( 0.5/( 1+( exp( -abs(f) ) ) ) )*maxPower*sign(f);
	}
	
	return setMaxNMin(f,maxPower,-maxPower);
}

//2005-10-25
float Skill::changeSpeed( const float &v0, const float &v1, const float &maxF, float &d, Time &t)
{
	//- const
	const float v_gap = 0.5;//0.3;
	const float m=30;
	const float u=maxF/( real_agent_max_speed + 0.01);//62;

	float f=0;
	t=0;
	d=0;
	if ( abs( v1-v0 ) < v_gap )
	{
    	f = u*v1;
    	t= sim_cycle_time;
    	d= v1*sim_cycle_time;
	}
	else
	{
    	if ( v1 > v0 )
        	f = maxF;
    	else
        	f = -maxF;

    	t = m/u*log((u*v0-f)/(u*v1-f));
    	//d = m/u*(v0-f/m)*(1-exp(-u/m*t))+f/u*t;
		d = m/u*(v0-f/u)*(1-exp(-u/m*t))+f/u*t;
	}
	//LOG( 13,"cd: v0=%f, v1=%f, f=%f",v0,v1,f);
	//LOG( 13,"cs: t=%f ,(u*v0-f)/(u*v1-f)=%f",t,(u*v0-f)/(u*v1-f));
	return f;
}

float Skill::driveTo( const float &x0, const float &v0, const float &xf, const float &vf, const float &maxF, Time &t)
{
	//- const
	const float maxSpeed = real_agent_max_speed*maxF/max_drive_force;
	const float v_gap = 0.1;
	//const float x_gap = 0.1;//0.05;

	//- init
	float f=0;
	float d=0;
	t=0;
	float dist = abs(xf-x0);
	float f_brake,f_dash;
	if ( vf*(xf-x0)<0 ) //-- not the same direction
	{
		float tBack = 0.0f;
		changeSpeed(0.0f,vf,maxF,d,tBack);
		float new_xf = xf - d;
		f = driveTo(x0,v0,new_xf,0.0f,maxF,t);
		t += tBack;
	}
	else //-- the same direction
	{
		f_brake = changeSpeed(v0,vf,maxF,d,t);
		if ( abs(d) >= dist )//--brake
    		f = f_brake;
		else//-- fish dash, then brake
		{
			float vm;
			float d1=0;
			float d2=0;
			Time t1=0;
			Time t2=0;
			if ( dist > 2 )
        		vm = sign(xf-x0)*maxSpeed;
    		else
			{
        		float vr = sign(xf-x0)*(maxSpeed+v_gap);
        		float vl = 0;
        		while ( abs( vl-vr ) > v_gap )
				{
            		vm = ( vl + vr )*0.5;
            		f_dash = changeSpeed(v0,vm,maxF,d1,t1);
            		f_brake = changeSpeed(vm,vf,maxF,d2,t2);