if ( abs( d1+d2 ) > dist )                		vr = vm;            		else                		vl = vm;				}			}    		f_dash = changeSpeed(v0,vm,maxF,d1,t1);    		f_brake = changeSpeed(vm,vf,maxF,d2,t2);    		float tm = ( dist - abs(d1+d2) )/abs(vm);    		t = t1+tm+t2;    		f= f_dash;		}	}		return setMaxNMin(f,maxF,-maxF);}Vector3f Skill::driveTo( const Vector3f &beginPos, const Vector3f &beginVel, const Vector3f &endPos, const Vector3f &endVel,Time &time){	const float maxPower = max_drive_force;	Vector3f direction = endPos - beginPos;	//if ( direction.Length() < 0.05 ) return Vector3f(0,0,0);	Vector3f power = direction.Normalized();	float fx = abs( power.x())*maxPower;	float fy = abs( power.y())*maxPower;		Time tx=0,ty=0;	power.x() = driveTo( beginPos.x(),beginVel.x(),endPos.x(),endVel.x(),fx,tx);	power.y() = driveTo( beginPos.y(),beginVel.y(),endPos.y(),endVel.y(),fy,ty);	power.z() = 0;	time = max(tx,ty);	//LOG( 13,"tx=%f, ty=%f, drive_t = %f",tx,ty,time);	return power;}Action Skill::driveTo( const Vector3f &endPos, const Vector3f &endVel, Time &time) const{	Action act;	act.setTime( WM->getRealTime() );	act.setDriveForce( driveTo( WM->getMyGlobalPos(), WM->getMyGlobalVel(), endPos, endVel, time) );	return act;}//2005-10-30-by-XuYuan//-* this function use a geometry alogrithm to project a run position//-- when the agent trying to dribbleVector3f Skill::projectDribblePos( const Vector3f &posPlayer, const Vector3f &posBall,  const AngDeg &ang ) const{	//-* const	//const float max_project_dist = 2;	//const AngDeg ang_error_thr = 10;		const float max_dist_2_ball = 0.5;//1.0;//0.5;//1.0;//0.981179;//1.0;	const float cut_point_dist_to_ball = 0.3;//0.292476;//0.5;	const float cut_circle_radius = 0.5;//0.507844;//0.5;			Vector3f posI;	Vector3f L1 = posBall - posPlayer;	L1.z() = 0;	AngDeg angL1 = getVector3fHorizontalAng(L1);	AngDeg k = normalizeAngle(angL1-ang);	float dL1 = L1.Length();	float dL2 = 0;	if ( isAngInInterval( k,-70, 70 ) )	{		LOG( 14,"projectDribblePos:zhong chui xian");		dL2 = dL1*0.5/cosDeg(k);		if ( dL2>max_dist_2_ball)		{			dL2 = max_dist_2_ball;		}		else		{			dL2 = kick_pos_dist;		}		//dL2 = (1-exp(-dL2))*max_dist_2_ball;		//dL2 = max(dL2,kick_pos_dist);		posI = posBall - getPosRelativeFromVision(VisionSense(dL2,ang,0.0f));	}	else	{		LOG( 14,"projectDribblePos:qie ru yuan");		Vector3f posC = posBall - getPosRelativeFromVision(VisionSense(cut_point_dist_to_ball,ang,0.0f));		AngDeg angOC = ang - sign(k)*90;		Vector3f posO = posC + getPosRelativeFromVision(VisionSense(cut_circle_radius,angOC,0.0f));		if ( abs( posC.x()-posPlayer.x() ) < EPSILON )		{			LOG( 14,"projectDribblePos:chui zhi");			posI.y() = ( posPlayer.y()+posC.y())*0.5;			posI.x() = posO.x()-sign(ang)*sqrt(pow2(cut_circle_radius)-pow2(posI.y()-posO.y()));		}		else		{			LOG( 14,"projectDribblePos:not chui zhi");			float kL1 = ( posPlayer.y()-posC.y())/(posPlayer.x()-posC.x());			float a = pow2(kL1)+1;			float b = 2*kL1*posO.x()-2;			float c = pow2(posO.x())+pow2(posO.y())-pow2(cut_circle_radius);			float delta = pow2(b)-4*a*c;			LOG( 14,"k=%f, a=%f, b=%f, c=%f, delta:",kL1,a,b,c,delta);			if ( delta >= 0 )			{				posI.y() = (-b-sign(k)*sqrt(delta))/a*0.5;				posI.x() = -kL1*posI.y();			}			else//-- no root			{				posI = (posC+posO)*0.5;			}		}		//-new add @2005-11-05		//- inorder to smooth the point		if ( isAngInInterval( k,70, 110 ) || isAngInInterval( k,-110, -70 ) )		{			Vector3f posImax = posBall - getPosRelativeFromVision(VisionSense(max_dist_2_ball,ang,0.0f));			posI = ( posImax + posI )*0.5;		}	}	posI.z() = WM->getMyRadius();	return posI;}//2005-11-01-by-XuYuan/*! this function generate an action which makes the agent to kick the ball	1. run to the kick position	2. should I kick?	*TODO*	the maxErrAng should depend on the suitation and opponent model*/Actions Skill::kickTo( const Vector3f &goal ,const float kickForce,const AngDeg kickAng, const AngDeg maxAngErr) const{	Actions kickToAction;		Time kickTime = WM->getRealTime();	bool isKicking = shoudIKick(goal,maxAngErr);	if ( isKicking )	{		Action kickAction;		kickAction.setTime(kickTime);		kickAction.setKick(kickAng,kickForce);		for ( Step i=0; i<step_num_per_cycle; i+=step_num_per_kick )		{			kickAction.setTime( kickTime+i );			kickToAction += kickAction;		}	}	Action driveAction = interceptBall(goal,isKicking);	kickToAction += driveAction;		return kickToAction;}bool Skill::shoudIKick( const Vector3f &goal, const AngDeg maxAngErr ) const{	//-* const	//const AngDeg max_ang_err = 12;//6;		Vector3f posMe = WM->getMyGlobalPos();	Vector3f posBall = WM->getBallGlobalPos();	//-* if the ball is just above my head, the angle error is too big	//-* so, i should not kick at this time	if ( (pow2(posBall.x()-posMe.x())+pow2(posBall.y()+posMe.y())) < 0.03 )	{		return false;	}		return isThreePointOneLine(posMe,posBall,goal,maxAngErr);}/*! calculate the needed kick force by a physical formulate	f=u*v=m*a=m*v'  --> v(t) = C*exp(-u*t/m)	d = Sigma[v(t)]=-m/u*(vf-v1) --> v1 = u*d/m + vf	k*F*t = m*(v1-v0) --> F = m*(u*d/m+vf-v0)/k/t = (u*d+m*(vf-v0))/k/t	\param kick dist	\param the speed of ball before kick	\param the speed of ball when it reach the dist,meaning end speed	\return the kick force*/float Skill::calGroundKickForce( const float &dist, const float &v0, const float &vf ) const{	return ( ground_friction_factor*dist + WM->getBallMass()*(vf-v0))/kick_effect_time/kick_force_factor;}/*! compute a good approximation of need power	see RoboLog	\param kick distance	\return kick force*/float Skill::calGroundKickForce( const float &dist) const{	float f = (0.02518+sqrt(0.00063-0.0137*(0.41129-dist)))/(0.00685);	return setMaxNMin(f,max_kick_force,0.0f);}/*! dribble -- only test version	kick when can kick, so keep controling the ball	allow the kick ang is not point to the goal, only try to keep kicking	\param goal position	\return actions of kick and drive*/Actions Skill::dribble( const Vector3f &goal ) const{//	if(!WM->isSpaceAhead(goal))//	{//		return kickTo(Vector3f(45,45,0));//	}	if ( isThreePointOneLine( WM->getMyGlobalPos(),WM->getInterceptBallPos(),goal,15 ) )	{		return fastDribble(goal);	}	//else	//{	//-* drive action//	Action driveAction = interceptBall(goal);//	driveAction.setTime(WM->getRealTime());//	driveAction.setActionType(ACT_DRIVE);//	kickToAction += driveAction;	else	{		Actions kickToAction;/*		Num oppNum = WM->getClosestOppNum2Ball();		Vector3f posOpp = WM->getOpponentGlobalPos(oppNum);		Vector3f velOpp = WM->getOpponentGlobalVel(oppNum);		Vector3f posBall = WM->getBallGlobalPos();		Vector3f velBall = WM->getBallGlobalVel();		float opp2Ball = pow2(posOpp[0] - posBall[0]) + pow2(posOpp[1] - posBall[1]);		if(opp2Ball < 2.5 && posOpp[0] < posBall[0] && false)		{			Vector3f posTemp = (posBall - posOpp).Normalized();			posTemp += posBall;			kickToAction += interceptBall(posTemp);		}		else		*/		kickToAction += interceptBall(goal);	//-* kick action		if(WM->getBallGlobalVel().x() < 0.5f)		{//		cout<<WM->getGameTime()<<vel<<endl;			Action kickAction;			kickAction.setTime(WM->getRealTime());			kickAction.setKick(0,5.0f);			kickToAction += kickAction;		}//	kickAction.setKick(20,15);//20);			return kickToAction;	}}/*! turn around a position	\param the center position of the cycle 	\param the radious of the cycle	\param the trun direction	\return the drive action*/Action Skill::turnAroundPos( const Vector3f &pos, const float &radious, bool clockWise ) const{	const AngDeg turnAng = 33.63;//20.0f;//20.0f;//10.0f;	const float turnSpeed = 3.03;//6;		Vector3f relPos = WM->getMyGlobalPos() - pos;	AngDeg ang2Pos = getVector3fHorizontalAng(relPos);	AngDeg incAngle = (clockWise? -turnAng:turnAng);	AngDeg newAng = normalizeAngle( ang2Pos+incAngle);	AngDeg newNewAng = normalizeAngle( newAng+incAngle);	Vector3f newPos,newNewPos;	newPos = pol2xyz(VisionSense(radious,newAng,0.0f)) + pos;	newNewPos = pol2xyz(VisionSense(radious,newNewAng,0.0f)) + pos;	Vector3f vel = newNewPos - newPos;	vel.z() = 0;	vel.Normalized();	vel*=turnSpeed;	LOG( 15,"turnforce: %f",runTo(newPos,vel).getDriveForce().Length());	return runTo(newPos,vel);}/*! direct pass to teammate	only kick to teammate's position	\param teammate's num	\return pass actions*/Actions Skill::directPass( Num num ){	//if (WM->getMyFreedom()<3)	//{	//	return fastPass(num);	//}	//return pass(num);		Vector3f posTeammate = WM->getTeammateGlobalPos(num);	Vector3f posBall = WM->getBallGlobalPos();	Vector3f attack = WM->getOppGoalCenter() - posTeammate;	attack.Normalize();	attack*=(0.3*(posBall-posTeammate).Length());	attack+=posTeammate;	float kickForce = calGroundKickForce((attack-posBall).Length());		return kickBetween(WM->getOppGoalCenter(),posTeammate,kickForce);}/*! dash to the direction by max drive power	it is needed when try kick and direct dash	\param dash aim pos	\return drive power*/Action Skill::dashTo( const Vector3f &pos)const{	AngDeg dashDir = getVector3fHorizontalAng(pos-WM->getMyGlobalPos());	Vector3f endVel = pol2xyz(Polar(real_agent_max_speed,dashDir,0.0f));	return runTo( pos,endVel);}/*! no predicat, just dash to the ball's current position	\return drive action*/Action	Skill::dashToBall() const{	return dashTo(WM->getBallGlobalPos());}/*! this skill enable the agent kick the ball between two goals	this is useful in shoot	\param two goals	\return actions of drive and kick*/Actions Skill::kickBetween( const  Vector3f &goalLeft, const Vector3f &goalRight, const float kickForce, const AngDeg kickAng) const{	Actions kickToAction;	//-* kick action	Time kickTime = WM->getRealTime();	bool isKicking = false;	if ( shoudIKick(goalLeft,goalRight) )	{		LOG( 17,"kicking!!!!!");		Action kickAction;		kickAction.setTime(kickTime);		kickAction.setKick(kickAng,kickForce);		for ( Step i=0; i<step_num_per_cycle; i+=step_num_per_kick )		{			kickAction.setTime( kickTime+i );			kickToAction += kickAction;		}		isKicking = true;	}		//-* drive action	LOG( 17,"driveAction");	Vector3f posInterceptBall = WM->getInterceptBallPos();	const AngDeg angLeft = normalizeAngle(getVector3fHorizontalAng(goalLeft-posInterceptBall));	const AngDeg angRight = normalizeAngle(getVector3fHorizontalAng(goalRight-posInterceptBall));	const AngDeg angBisector = getBisectorTwoAngles(angLeft,angRight);	Vector3f goal = pol2xyz(VisionSense(10.0f,angBisector,0))+posInterceptBall;	Action driveAction = interceptBall(goal,isKicking);		kickToAction += driveAction;	return kickToAction;}bool Skill::shoudIKick( const Vector3f &goalLeft, const Vector3f &goalRight) const{	LOG( 17,"shoudIKick");		Vector3f posBall = WM->getBallGlobalPos();	Vector3f posMe = WM->getMyGlobalPos();	//-* if the ball is just above my head, the angle error is too big	//-* so, i should not kick at this time	if ( (pow2(posBall.x()-posMe.x())+pow2(posBall.y()+posMe.y())) < 0.03 )	{		return false;	}		if ( (goalLeft-posBall).Length() > max_kick_distance		&& (goalRight-posBall).Length() > max_kick_distance		)//-* too far away	{		return shoudIKick(0.5f*(goalLeft+goalRight));//(shoudIKick(goalLeft)||shoudIKick(goalRight));	}	else	{		AngDeg angLeft = getVector3fHorizontalAng(goalLeft-posBall);		AngDeg angRight = getVector3fHorizontalAng(goalRight-posBall);		AngDeg angHeading = getVector3fHorizontalAng(posBall-posMe);				AngDeg angClip = normalizeAngle(angLeft-angRight );		if ( angClip < 0 )		{			swap(angLeft,angRight);		}				return isAngInInterval(angHeading,angRight,angLeft);	}}Action Skill::interceptClearBall() const{	const float interceptFreeChange = 0;//-1.3;//gene1[5];//0	const float interceptFree = -1.0f;//-0.55f;//gene1[6];//-0.5f	if ( ( WM->getMyFreedomChange() > interceptFreeChange)		|| WM->getMyFreedom() > interceptFree		)	{			LOG( 18,"my freedom = %f; [%d]",WM->getMyFreedom(),WM->getMyFreeState());		Vector3f posIntercept = WM->getInterceptPos( WM->getOppGoalCenter() );				posIntercept = projectInterceptPos( posIntercept );		Vector3f velEnd(0,0,0);		posIntercept = setInterceptPosBeforeGoal(posIntercept);		return runTo( posIntercept, velEnd );	}	else	{		Vector3f posIntercept = WM->getInterceptBallPos( );		posIntercept = setInterceptPosBeforeGoal(posIntercept);		return dashTo(posIntercept);	}}/*! if the intercept position is after goal, it is no useful,	so try intercept the ball before goal	\param the old intercept position	\return the new intercept position*/Vector3f Skill::setInterceptPosBeforeGoal(const Vector3f &posIntercept) const{	if ( posIntercept.x() > WM->getOurBaseLine() )		return posIntercept;		AngDeg angShoot = getVector3fHorizontalAng(posIntercept-WM->getBallGlobalPos());	Vector3f newPos(0,0,0);	newPos.x() = WM->getOurBaseLine();	newPos.y() = WM->getBallGlobalPos().y() - tanDeg(angShoot)*(WM->getBallGlobalPos().x()-newPos.x());	if ( abs(newPos.y()) > half_goal_width + 0.2 )		return posIntercept;		return newPos;}Actions Skill::clearBall(Num num ) const{	Actions clearAction;	Vector3f posBall = WM->getBallGlobalPos();		Time kickTime = WM->getRealTime();	if ( canIClear() )	{		//-* calculate clear force,can't let the ball out		float clearForce = max_kick_force;		float clearDist = WM->getOppBaseLine()-posBall.x();		if ( 0 == num )		{			if ( clearDist < max_kick_distance && (!shoudIKick(WM->getOppGoalLeft(),WM->getOppGoalRight())) )			{				clearDist = clearDist/cosDeg(WM->getMyKickHorizontalAng());				clearForce = clearDist/max_kick_distance*max_kick_force;			}		}		else		{