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

TacticTest::TacticTest(WorldModel *worldmodel, QObject *parent) :
    Tactic("TacticTest", worldmodel, parent)
{
    circularBorder.assign(Vector2D(1500,0),1700/2);
    circularBorderOut.assign(Vector2D(1500,0),2100/2);// a circle may use to push balls with some risks
    circularMid.assign(Vector2D(1500,0),720); // a circle which is between holes and border circle
    hole1.assign(Vector2D(1500,1700/4),250);
    hole2.assign(Vector2D(1500,-1700/4),250);
    Vector2D Cdist = (hole1.center() - circularBorder.center());
    double deltaAngle=1.1*asin(hole1.radius()/(Cdist.length())); // 1.1 is safety factor
    Uangle1=Cdist.dir().radian() + deltaAngle ;
    Dangle1=Uangle1 - 2*deltaAngle;
    Cdist = (hole2.center() - circularBorder.center());
    Uangle2=Cdist.dir().radian() + deltaAngle ;
    Dangle2=Uangle2 - 2*deltaAngle;
    state=0;

}

Vector2D Knowledge::PredictDestination(Vector2D sourcePos, Vector2D targetPos, double sourceSpeed, Vector2D targetSpeed)
{
    double factor = _wm->var[3] / 250.0;
    if(factor < 0)
    {
        factor = 0;
    }

    double Vm = sourceSpeed;
    double k = Vm / targetSpeed.length();
    double gamaT = targetSpeed.dir().radian();
    Vector2D delta;

    delta = targetPos - sourcePos;
    double landa = atan2(delta.y, delta.x);
    double theta = gamaT - landa;

    if (theta > AngleDeg::PI)
    {
        theta -= 2 * AngleDeg::PI;
    }

    if (theta < - AngleDeg::PI)
    {
        theta += 2 * AngleDeg::PI;
    }

    double dlta = 0;
    if(k != 0 && fabs(sin(theta) / k) < 1)
    {
        dlta = asin(sin(theta)/k);
    }
    // No solution.
    else
    {
        //qDebug() << "Prediction: No solution.";
        return targetPos;//Vector2D::INVALIDATED;
    }

    double tf = factor * (delta.length() / 1000) / (Vm*cos(dlta) - targetSpeed.length() * cos(theta));

    // No solution.
    if(tf < 0)
    {
        //qDebug() << "Prediction: No solution.";
        return targetPos;   //Vector2D(0,0); //INVALIDATED;
    }

    double catchDist = targetSpeed.length() * tf * 1000;
    Vector2D catchDiff(catchDist * cos(gamaT), catchDist * sin(gamaT));
    Vector2D Pred_pos=targetPos + catchDiff;

    //_wm->predict_pos.append(Pred_pos);
    return Pred_pos;
}

        // =================================================================================
        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;

        }

RobotCommand tacticControl::getCommand()
{
RobotCommand rc;
if(!wm->ourRobot[id].isValid) return rc;
////moving on width direction to ball////////////////////////////////////////////
// switch (state) {
// case 0:
// rc.fin_pos.loc = Vector2D(0,-1700);
// if((wm->ourRobot[id].pos.loc-rc.fin_pos.loc).length()<100) state=1;
// break;
// case 1:
// rc.fin_pos.loc=Vector2D(0,1700);
// if((wm->ourRobot[id].pos.loc-rc.fin_pos.loc).length()<100) state=0;
// break;
// }
// Vector2D a =wm->ball.pos.loc - wm->ourRobot[id].pos.loc;
// rc.fin_pos.dir=a.dir().radian();
/////////////////////////////////////////////////////////////////////////////////
////going to center & loking to ball/////////////////////////////////////////////
rc.fin_pos.loc = {0,0};
Vector2D a =wm->ball.pos.loc - wm->ourRobot[id].pos.loc;
rc.fin_pos.dir=a.dir().radian();
/////////////////////////////////////////////////////////////////////////////////
////just looking at ball/////////////////////////////////////////////////////////
// Vector2D a =wm->ball.pos.loc - wm->ourRobot[id].pos.loc;
// rc.fin_pos.loc = wm->ourRobot[this->id].pos.loc;
// rc.fin_pos.dir=a.dir().radian();
/////////////////////////////////////////////////////////////////////////////////
////just looking at ball/////////////////////////////////////////////////////////
// Vector2D a =wm->ball.pos.loc - wm->ourRobot[id].pos.loc;
// Vector2D b ;
// b = {1,1};
// b.setLength(200);
// b.setDir(sin(alfa/100)*180);
// alfa++;
// rc.fin_pos.dir=a.dir().radian();
// rc.fin_pos.loc = wm->ball.pos.loc + b;
/////////////////////////////////////////////////////////////////////////////////
rc.maxSpeed=2.5;
rc.useNav = true;false;
rc.isBallObs = true;
rc.isKickObs = true;
return rc;
}

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 TacticTransferObject::getCommand()
{
    AllInMargin=true;
    RobotCommand rc;
    if(!wm->ourRobot[id].isValid) return rc;
    rc.useNav=true;
    rc.maxSpeed = 1;
    rc.fin_pos.loc=wm->endPoint;//Vector2D(300,0);
    int object;

    addData();
    mergeData();
    sortData();


    //    if(wm->balls.size() > 0)
    //    {
    //            qDebug()<< " BALLL . X = " << wm->balls.at(0)->pos.loc.x << " BALLL . Y = " <<  wm->balls.at(0)->pos.loc.y;
    //                    qDebug() << " MAX x : " << region[1].maxX() << "  MIN X : " << region[1].minX() ;
    //                    qDebug() << " MAX y : " << region[1].maxY() << "  MIN y : " << region[1].minY() ;
    //            if(region[0].IsInside(wm->balls.at(0)->pos.loc)) qDebug() << " THE BALLLLL ISSS INNNNN SIDE !!!!!!!!!!!!!!!!!!!!!!1";
    //    }
    index = -1;
    for(int i=0;i<mergedList.size();i++)
    {
//        qDebug() << i << " AT : (" << mergedList.at(i).pos.x << "," << mergedList.at(i).pos.y << ")";
        temp=0;
        if(!region[mergedList.at(i).goalRegion].IsInside(mergedList.at(i).pos) && !IsInmargins(mergedList.at(i).pos,300))
        {
            //qDebug() <<" OBJECT :    " <<  mergedList.at(i).pos.x << " ------ Y = " << mergedList.at(i).pos.y;// TOOOOOOOOOOOOOOOOOOOSHE !!!!!!!" << index ;
//            AllInMargin=false;
            index=i;
            goalRegion=mergedList.at(i).goalRegion;
            temp=1;
            break;
        }


    }
    for(int i=0; i<mergedList.size(); i++)
    {
        if(!IsInmargins(mergedList.at(i).pos,300))
        {
            AllInMargin=false;
        }
    }
    if(AllInMargin)
    {
        for(int i=0;i<mergedList.size();i++)
        {
            if(!region[mergedList.at(i).goalRegion].IsInside(mergedList.at(i).pos))
            {
                index=i;
                goalRegion=mergedList.at(i).goalRegion;
                break;
            }
        }
    }
//    if(index ==-1)
//    {
//        for(int i=0;i<wm->Chasbideh.size(); i++)
//        {
//            if(!region[0].IsInside(wm->Chasbideh.at(i).position) && !region[1].IsInside(wm->Chasbideh.at(i).position))
//            {
//                //qDebug() <<" OBJECT :    " <<  mergedList.at(i).pos.x << " ------ Y = " << mergedList.at(i).pos.y;// TOOOOOOOOOOOOOOOOOOOSHE !!!!!!!" << index ;
//                index=i;
//                goalRegion=0;//mergedList.at(i).goalRegion;
//                temp=1;
//                break;
//            }
//        }
//    }




//     qDebug() << mergedList.size() << " MERGED SIZE " ;
    if(index != -1)
    {
        Vector2D point2 = mergedList.at(index).pos;
        Vector2D diff2 = region[goalRegion].center() - point2;
        bool reach=false;


        if(temp!=0)
        {
            switch(state)
            {
            case 0:{ //Go Behind the Object

                Vector2D space2=diff2;
                space2.setLength(300);
                rc.maxSpeed=1.4;
                rc.useNav = true;
                rc.fin_pos.loc=point2 - space2;
                rc.fin_pos.dir=diff2.dir().radian();

                object=findnearestObject(mergedShapeList,wm->ourRobot[id].pos.loc);
                if(object!=-1) ObsC=Circle2D(mergedShapeList.at(object).position,(mergedShapeList.at(object).roundedRadios+ROBOT_RADIUS+150));
                rc.fin_pos.loc=AvoidtoEnterCircle(ObsC,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);
//.........这里部分代码省略.........

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;
//.........这里部分代码省略.........