Kick让robot把球踢到ToPos的位置.docVIP

//////////// /* Kick 让robot把球踢到ToPos的位置 */ //////////// void Kick(Environment *env , int robot , Vector3D ToPos ) { Mydata * p; p=(Mydata *)env-userData; PredictBall(env,2); Vector3D ball = p-preball; Vector3D RobotToBall; //人和球的相对位置 RobotToBall.x = ball.x - p-robot[robot].pos.x ; RobotToBall.y = ball.y - p-robot[robot].pos.y ; RobotToBall.z = Atan(p-robot[robot].pos , ball); Vector3D BallToGate ; //球和球门的相对位置 BallToGate.x = ToPos.x - ball.x ; BallToGate.y = ToPos.y - ball.y ; BallToGate.z = Atan(ball , ToPos); double RunAngle ; RunAngle = RobotToBall.z - BallToGate.z; RegulateAngle(RunAngle); RunAngle = RobotToBall.z + RunAngle /2; // 可以调整 2 RegulateAngle(RunAngle); PAngle(env,robot,RunAngle,125); } void Kick(Environment *env , int robot ,int robot1)//踢人 { Mydata * p; p=(Mydata *)env-userData; Vector3D RobotToBall; //人和球的相对位置 RobotToBall.x = p-robot[robot1].pos.x- p-robot[robot].pos.x ; RobotToBall.y = p-robot[robot1].pos.y- p-robot[robot].pos.y ; RobotToBall.z = Atan(p-robot[robot].pos , p-robot[robot1].pos); Vector3D BallToGate ; //球和球门的相对位置 BallToGate.x = CONSTGATE.x- p-robot[robot1].pos.x; BallToGate.y = CONSTGATE.y- p-robot[robot1].pos.y; BallToGate.z = Atan(p-robot[robot1].pos,CONSTGATE); double RunAngle ; RunAngle = RobotToBall.z - BallToGate.z; RegulateAngle(RunAngle); RunAngle = RobotToBall.z + RunAngle /2 ; // 可以调整 2 RegulateAngle(RunAngle); PAngle(env,robot,RunAngle,125); } void Kick(Environment *env,int robot,int steps,double limits){ Mydata *p=(Mydata*)env-userData; double dx,dy,angle; dx=p-curball.x-p-robot[robot].pos.x; dy=p-curball.y-p-robot[robot].pos.y; angle=Atan(dy,dx); PredictBall(env,steps); if(angle90angle-90){ if(p-curball.y41.8) PositionBallX(env,robot,p-preball,-125,3); else PositionBallX(env,robot,p-preball,125,3); } else shoot(env,robot); } void PositionAndThrough(Environment *env,int robot,Vector3D pos ,double MAX) { Mydata * p; p=(