C++ Vector2D::setLength方法代码示例

/* ---------------------------------------------------------------------- */
Vector2D Circle2D::nearestpoint(Vector2D pnt)
{

        Vector2D diff = pnt - this->center();
        diff.setLength(this->radius());
        Vector2D dest=this->center()+diff;
        return dest;

}

Position Knowledge::AdjustKickPoint(Vector2D ballPos, Vector2D target, int kickSpeed)
{
    Position p;
    Vector2D dir = (ballPos - target);//.normalizedVector();
    dir.setLength(/*scale(*/ROBOT_RADIUS - (35 /*- kickSpeed*/));

    p.loc = ballPos + dir;
    p.dir = (-dir).dir().radian();

    return p;
}

        // =================================================================================
        Vector2D TacticPush2Goal::GoOncircle(Vector2D center, double radius)//, Vector2D Object)
        {
            //for (int k=0;k<1000;k++)

            Vector2D diff = wm->ourRobot[id].pos.loc - center;
            diff.setDir(diff.dir()+1.1);//50);//.radian() + AngleDeg::deg2rad(5));
            diff.setLength(radius);
            Vector2D point = center + diff;
            return point;

        }

bool SkillCircle::execute(RobotCommand &rc)
{
    if(_rid==-1) return false;
    Vector2D vc;
    vc = _wm->ourRobot[_rid].pos.loc;
    vc.setLength(500);
    vc.rotate(10);
    rc.FinalPos.loc=vc;
    rc.TargetPos.loc=vc;
    rc.FinalPos.dir=(_wm->ball.pos.loc - _wm->ourRobot[_rid].pos.loc).dir().radian();
    rc.TargetPos.dir=(_wm->ball.pos.loc - _wm->ourRobot[_rid].pos.loc).dir().radian();
    rc.Speed=1;
    return true;
}

/*!

 */
bool
Bhv_SetPlayGoalKick::doKickToFarSide( PlayerAgent * agent )
{
    const WorldModel & wm = agent->world();

    Vector2D target_point( ServerParam::i().ourPenaltyAreaLineX() - 5.0,
                           ServerParam::i().penaltyAreaHalfWidth() );
    if ( wm.ball().pos().y > 0.0 )
    {
        target_point.y *= -1.0;
    }

    double ball_move_dist = wm.ball().pos().dist( target_point );
    double ball_first_speed = calc_first_term_geom_series_last( 0.7,
                                                                ball_move_dist,
                                                                ServerParam::i().ballDecay() );
    ball_first_speed = std::min( ServerParam::i().ballSpeedMax(),
                                 ball_first_speed );
    ball_first_speed = std::min( wm.self().kickRate() * ServerParam::i().maxPower(),
                                 ball_first_speed );

    Vector2D accel = target_point - wm.ball().pos();
    accel.setLength( ball_first_speed );

    double kick_power = std::min( ServerParam::i().maxPower(),
                                  accel.r() / wm.self().kickRate() );
    AngleDeg kick_angle = accel.th();


    dlog.addText( Logger::TEAM,
                  __FILE__" (doKickToFarSide) target=(%.2f %.2f) dist=%.3f ball_speed=%.3f",
                  target_point.x, target_point.y,
                  ball_move_dist,
                  ball_first_speed );
    dlog.addText( Logger::TEAM,
                  __FILE__" (doKickToFarSide) kick_power=%f kick_angle=%.1f",
                  kick_power,
                  kick_angle.degree() );

    agent->doKick( kick_power, kick_angle - wm.self().body() );

    agent->setNeckAction( new Neck_ScanField() );
    return true;
}

        void SimpleBirdController::affect(float ms)
        {
            Entity *entity = getEntity();
            if(currentPoint >= 0) {
                Vector2D position = entity->getPosition();
                Vector2D target = points[currentPoint];
                Vector2D velocity = target - position;
                velocity.setLength(entity->getSpeed());
                Vector2D destination = position + velocity * ms;

                /* We cross the target. */
                if(destination.getDistance(target) >
                        position.getDistance(target) ||
                        position.getDistance(target) < 0.00001f)
                    currentPoint = (currentPoint + 1) % points.size();

                entity->setVelocity(velocity);
            }
        }

OperatingPosition Knowledge::AdjustKickPointB(Vector2D ballLoc, Vector2D target, Position robotPos)
{
    OperatingPosition KickPos;
    Vector2D KickDir = (target - ballLoc);
    bool shoot_sensor = true;
    double DirErr;
    double DistErr;
    double BallDir = _wm->ball.vel.loc.dir().radian();
    double DAngel = AngleDeg ::deg2rad(80);

    if(_wm->ball.vel.loc.length()>.2 && (-KickDir.dir().radian()-DAngel)<BallDir && (-KickDir.dir().radian()+DAngel)>BallDir)
    {
        KickPos.pos.dir = BallDir-M_PI;
        if (KickPos.pos.dir > M_PI) KickPos.pos.dir -= 2 * M_PI;
        if (KickPos.pos.dir < -M_PI) KickPos.pos.dir += 2 * M_PI;

        KickPos.pos.loc = ballLoc ;
    }
    else
    {
        //possession point >>navigation : ON
        KickDir.setLength( ROBOT_RADIUS + BALL_RADIUS*2);
        KickPos.useNav = true ;

        KickPos.pos.dir = KickDir.dir().radian();
        KickPos.pos.loc = ballLoc - KickDir;

        //possession point check
        DirErr = AngleDeg::rad2deg(fabs(KickPos.pos.dir  - robotPos.dir));
        if(DirErr > 360.0)  DirErr = 360.0 - DirErr ;

        DistErr = (KickPos.pos.loc - robotPos.loc).length();
        if(DirErr < 15 && DistErr < BALL_RADIUS*2) kickPermission = true;
        if(DirErr > 19 && DistErr > BALL_RADIUS*3) kickPermission = false;

    }

    if(kickPermission)
    {
        Vector2D ball_vel_change;

        ball_vel_change =_wm->ball.vel.loc - last_ball_vell;
        last_ball_vell = _wm->ball.vel.loc ;

        if(ball_vel_change.length()>0.02)
            shoot_sensor = false;

        //control point >>navigation : OFF
        KickDir.setLength( ROBOT_RADIUS- BALL_RADIUS);
        KickPos.useNav = false ;
        KickPos.pos.dir = KickDir.dir().radian();//(ballLoc - robotPos.loc).dir().radian();//
        KickPos.pos.loc = ballLoc - KickDir;

        DirErr = AngleDeg::rad2deg(fabs(KickPos.pos.dir  - robotPos.dir));
        if(DirErr > 360.0)  DirErr = 360.0 - DirErr ;

        DistErr = (KickPos.pos.loc - robotPos.loc).length();
        //qDebug()<<"A"<<DirErr<<DistErr<<ball_vel_change.length();

        //#kick distance and angel limits
        if(shoot_sensor)//kicking limits when shooting with sensor
        {

        }
        else//kicking limits when shooting without sensor
        {
            if(DirErr < 10 && DistErr < 45)
            {
                KickPos.readyToShoot = true;
                qDebug()<<"shooooooooooooooooooooooooooooooooooooooot";
            }
            else
            {
                KickPos.readyToShoot = false;
            }

        }
        //##kick distance and angel limits
    }

    return KickPos;
}

RobotCommand TacticPush2Goal::getCommand()
{
    oppIsValid = wm->ourRobot[0].isValid;//wm->theirRobot.IsValid;// opposite robot
    addData();

    sortData();
    if(!oppIsValid) opp = Vector2D(1000000,1000000);
    opp = wm->ourRobot[0].pos.loc;//wm->theirRobot.position;//wm->ourRobot[8].pos.loc;
    OppIsKhoraak=!circularBorder2.contains(opp);//out of his field
    bool reach=false;
    Avoided=false;
    AllIn=true;
    AnyIn=false;
    AllInCorner=true;
    AllUnAccessible=true;

    RobotCommand rc;
    if(!wm->ourRobot[id].isValid) return rc;
    rc.fin_pos.loc=Vector2D(400,0);//circularBorder.center();
    rc.maxSpeed = 1.2;


    index=-1;
    int tah=balls.size()-1;
    if(tah != -1)
    {
        rc.fin_pos.loc=circularBorder2.nearestpoint(balls.at(tah)->pos.loc);
        rc.fin_pos.dir=(circularBorder.center()-rc.fin_pos.loc).dir().radian();
    }


    FindBall(); // find approtiate ball

    if( index != -1 )
    {
        point2 = balls.at(index)->pos.loc;

        FindHole();

        Vector2D nrstpnt = (circularBorder.center()-point2); //nearest Point
        nrstpnt=nrstpnt.setLength(nrstpnt.length()-circularBorder2.radius());
        diff2 = nrstpnt;
        nrstpnt=point2+nrstpnt;
        //        state2=0;
        switch(state)
        {
        case 0:{ //Go Behind the Object

            vec2goal.setLength(250);
            // qDebug()<<"VEC 2 GOAL LENGTH = " << vec2goal.length();
            rc.maxSpeed=1.3;
            rc.useNav = false;//true;
            rc.isBallObs = false;//true;
            rc.isKickObs = false;//true;
            rc.fin_pos.loc=wm->kn->PredictDestination(wm->ourRobot[id].pos.loc,point2  - vec2goal,
                                                      rc.maxSpeed,balls.at(index)->vel.loc);//point2  - vec2goal;
            //            wm->kn->PredictDestination()
            int rad = 150+ROBOT_RADIUS;
            Circle2D c(point2,rad);

            rc.fin_pos.loc=AvoidtoEnterCircle(c,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);
            //            if(Avoided) rc.maxSpeed=rc.maxSpeed;
            rc.fin_pos.dir=vec2goal.dir().radian();
//            KeepInField(rc);
            reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos.loc,rc.fin_pos.loc,100);
            if(reach && !Avoided) state = 1;

        }
            break;
        case 1:{//Push

            rc.useNav = false;
            rc.isBallObs = false;
            rc.isKickObs = false;
            rc.maxSpeed=1.1;
            vec2goal.setLength(100);
            rc.fin_pos.loc=wm->kn->PredictDestination(wm->ourRobot[id].pos.loc,point2  + vec2goal,
                                                      rc.maxSpeed,balls.at(index)->vel.loc);
            rc.fin_pos.dir=vec2goal.dir().radian();

            KeepInField(rc);
            if(((wm->ourRobot[id].pos.loc-point2).length())>800) state=0;
            if(((wm->ourRobot[id].pos.loc-rc.fin_pos.loc).length())<250) state=0;


            if(hole1.contains(rc.fin_pos.loc) || hole2.contains(rc.fin_pos.loc))
            {
                //                vec2goal.setLength(100);
                rc.fin_pos.loc=point2  ;//+ vec2goal;
                //                rc.fin_pos.loc=point2;
            }

            //            reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos.loc,rc.fin_pos.loc,40);
            //            if(reach)
            //                state2 = 0;
        }
            break;
        }
        int mrgn=200;
        Vector2D dlta;
//.........这里部分代码省略.........