/***************************************************************************************** *                                      SEU-3D *                     ------------------------------------------------- * Copyright (c) 2005, Yuan XU<xychn15@yahoo.com.cn>,Chang'e SHI<evelinesce@yahoo.com.cn> * Copyright (c) 2006, Yuan XU<xuyuan.cn@gmail.com>,Chunlu JIANG<JamAceWatermelon@gmail.com> * 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 defenceAction 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-25float 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);