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

bool Knowledge::IsInsideGoalShape(Vector2D pos, double goalLeftX, double goalRadius, double goalCcOffset)
{
    double x = pos.x - goalLeftX;
    Vector2D ccl(goalLeftX, goalCcOffset / 2), ccr(goalLeftX, -goalCcOffset / 2);

    return (pos.dist(ccl) <= goalRadius || pos.dist(ccr) <= goalRadius ||
            (x >= 0 && x <= goalRadius && fabs(pos.y) <= goalCcOffset / 2));
}

/*!

 */
int
TackleGenerator::predictOpponentsReachStep( const WorldModel & wm,
                                            const Vector2D & first_ball_pos,
                                            const Vector2D & first_ball_vel,
                                            const AngleDeg & ball_move_angle )
{
    int first_min_step = 50;

#if 1
    const ServerParam & SP = ServerParam::i();
    const Vector2D ball_end_point = inertia_final_point( first_ball_pos,
                                                         first_ball_vel,
                                                         SP.ballDecay() );
    if ( ball_end_point.absX() > SP.pitchHalfLength()
         || ball_end_point.absY() > SP.pitchHalfWidth() )
    {
        Rect2D pitch = Rect2D::from_center( 0.0, 0.0, SP.pitchLength(), SP.pitchWidth() );
        Ray2D ball_ray( first_ball_pos, ball_move_angle );
        Vector2D sol1, sol2;
        int n_sol = pitch.intersection( ball_ray, &sol1, &sol2 );
        if ( n_sol == 1 )
        {
            first_min_step = SP.ballMoveStep( first_ball_vel.r(), first_ball_pos.dist( sol1 ) );
#ifdef DEBUG_PRINT
            dlog.addText( Logger::CLEAR,
                          "(predictOpponent) ball will be out. step=%d reach_point=(%.2f %.2f)",
                          first_min_step,
                          sol1.x, sol1.y );
#endif
        }
    }
#endif

    int min_step = first_min_step;
    for ( AbstractPlayerCont::const_iterator
              o = wm.theirPlayers().begin(),
              end = wm.theirPlayers().end();
          o != end;
          ++o )
    {
        int step = predictOpponentReachStep( *o,
                                             first_ball_pos,
                                             first_ball_vel,
                                             ball_move_angle,
                                             min_step );
        if ( step < min_step )
        {
            min_step = step;
        }
    }

    return ( min_step == first_min_step
             ? 1000
             : min_step );
}

/*!

*/
bool
Neck_ForMark::execute( PlayerAgent * agent )
{
    dlog.addText( Logger::ACTION,
                  __FILE__": Neck_ForMark count_thr=%d",
                  M_count_thr );

    const WorldModel & wm = agent->world();

    const PlayerPtrCont & opps = wm.opponentsFromSelf();
    const PlayerObject * nearest_opp = ( opps.empty() ? static_cast< PlayerObject * >( 0 ) : opps.front() );
    const Vector2D opp = ( nearest_opp ? nearest_opp->pos() : Vector2D( -1000.0, 0.0 ) );
    if ( wm.opponentsFromSelf().front()->posCount() > 1 )
    {
        return  Neck_TurnToPoint( opp ).execute( agent );
    }
    if ( wm.ball().posCount() <= M_count_thr )
    {
        dlog.addText( Logger::ACTION,
                      __FILE__": pos_count=%d <= count_thr=%d",
                      wm.ball().posCount(), M_count_thr );
        return Neck_ScanField().execute( agent );
    }

    const ServerParam & SP = ServerParam::i();

    const Vector2D ball_next = agent->effector().queuedNextBallPos();
    const Vector2D my_next = agent->effector().queuedNextSelfPos();

    if ( wm.ball().posCount() <= 0
         && my_next.dist( ball_next ) < SP.visibleDistance() - 0.2 )
    {
        dlog.addText( Logger::ACTION,
                      __FILE__": in visible distance." );
        return Neck_ScanField().execute( agent );
    }

    const AngleDeg my_next_body = agent->effector().queuedNextSelfBody();
    const double next_view_width = agent->effector().queuedNextViewWidth().width();

    if ( ( ( ball_next - my_next ).th() - my_next_body ).abs()
         > SP.maxNeckAngle() + next_view_width * 0.5 + 2.0 )
    {
        dlog.addText( Logger::ACTION,
                      __FILE__": never face to ball" );
        return Neck_ScanField().execute( agent );
    }

    dlog.addText( Logger::ACTION,
                  __FILE__": turn neck to ball" );
    return Neck_TurnToBall().execute( agent );
}

bool Knowledge::ReachedToPos(Vector2D current, Vector2D desired, double distThreshold)
{
    if(current.dist(desired) < distThreshold)
    {

            return true;

    }
    else
    {
        return false;
    }
}

/*!

*/
bool
Neck_TurnToBallOrScan::execute( PlayerAgent * agent )
{
    dlog.addText( Logger::ACTION,
                  __FILE__": Neck_TurnToBallOrScan count_thr=%d",
                  M_count_thr );

    const WorldModel & wm = agent->world();

    if ( wm.ball().posCount() <= M_count_thr )
    {
        dlog.addText( Logger::ACTION,
                      __FILE__": pos_count=%d <= count_thr=%d",
                      wm.ball().posCount(), M_count_thr );
        return Neck_ScanField().execute( agent );
    }

    const ServerParam & SP = ServerParam::i();

    const Vector2D ball_next = agent->effector().queuedNextBallPos();
    const Vector2D my_next = agent->effector().queuedNextSelfPos();

    if ( wm.ball().posCount() <= 0
         && my_next.dist( ball_next ) < SP.visibleDistance() - 0.2 )
    {
        dlog.addText( Logger::ACTION,
                      __FILE__": in visible distance." );
        return Neck_ScanField().execute( agent );
    }

    const AngleDeg my_next_body = agent->effector().queuedNextSelfBody();
    const double next_view_width = agent->effector().queuedNextViewWidth().width();

    if ( ( ( ball_next - my_next ).th() - my_next_body ).abs()
         > SP.maxNeckAngle() + next_view_width * 0.5 + 2.0 )
    {
        dlog.addText( Logger::ACTION,
                      __FILE__": never face to ball" );
        return Neck_ScanField().execute( agent );
    }

    dlog.addText( Logger::ACTION,
                  __FILE__": turn neck to ball" );
    return Neck_TurnToBall().execute( agent );
}

/*!

 */
Circle2D
Circle2D::circumcircle( const Vector2D & p0,
                        const Vector2D & p1,
                        const Vector2D & p2 )
{
    Vector2D center = Triangle2D::circumcenter( p0, p1, p2 );

    if ( ! center.isValid() )
    {
        std::cerr << "Circle2D::circumcircle()"
                  << " ***ERROR*** failed to get circumcenter from "
                  << p0 << p1 << p2
                  << std::endl;
        return Circle2D();
    }

    return Circle2D( center, center.dist( p0 ) );
}

/*!

 */
bool
Bhv_ChainAction::execute( PlayerAgent * agent )
{
    dlog.addText( Logger::TEAM,
                  __FILE__": Bhv_ChainAction" );

    if ( doTurnToForward( agent ) )
    {
        return true;
    }

    const ServerParam & SP = ServerParam::i();
    const WorldModel & wm = agent->world();

    const CooperativeAction & first_action = M_chain_graph.getFirstAction();

    ActionChainGraph::debug_send_chain( agent, M_chain_graph.getAllChain() );

    const Vector2D goal_pos = SP.theirTeamGoalPos();
    agent->setNeckAction( new Neck_TurnToReceiver( M_chain_graph ) );

    switch ( first_action.category() ) {
    case CooperativeAction::Shoot:
        {
            dlog.addText( Logger::TEAM,
                          __FILE__" (Bhv_ChainAction) shoot" );
            if ( Body_ForceShoot().execute( agent ) )
            {
                agent->setNeckAction( new Neck_TurnToGoalieOrScan() );
                return true;
            }

            break;
        }

    case CooperativeAction::Dribble:
        {
            if ( wm.gameMode().type() != GameMode::PlayOn
                 && ! wm.gameMode().isPenaltyKickMode() )
            {
                agent->debugClient().addMessage( "CancelChainDribble" );
                dlog.addText( Logger::TEAM,
                              __FILE__" (Bhv_ChainAction) cancel dribble" );
                return false;
            }

            const Vector2D & dribble_target = first_action.targetPoint();

            dlog.addText( Logger::TEAM,
                          __FILE__" (Bhv_ChainAction) dribble target=(%.1f %.1f)",
                          dribble_target.x, dribble_target.y );

            NeckAction::Ptr neck;
            double goal_dist = goal_pos.dist( dribble_target );
            if ( goal_dist < 18.0 )
            {
                int count_thr = 0;
                if ( goal_dist < 13.0 )
                {
                    count_thr = -1;
                }
                agent->debugClient().addMessage( "ChainDribble:LookGoalie" );
                neck = NeckAction::Ptr( new Neck_TurnToGoalieOrScan( count_thr ) );
            }

            if ( Bhv_NormalDribble( first_action, neck ).execute( agent ) )
            {
                return true;
            }
            break;
        }

    case CooperativeAction::Hold:
        {
            if ( wm.gameMode().type() != GameMode::PlayOn )
            {
                agent->debugClient().addMessage( "CancelChainHold" );
                dlog.addText( Logger::TEAM,
                              __FILE__" (Bhv_ChainAction) cancel hold" );
                return false;
            }

            if ( wm.ball().pos().x < -SP.pitchHalfLength() + 8.0
                 && wm.ball().pos().absY() < SP.goalHalfWidth() + 1.0 )
            {
                agent->debugClient().addMessage( "ChainHold:Clear" );
                dlog.addText( Logger::TEAM,
                              __FILE__" (Bhv_ChainAction) cancel hold. clear ball" );
                Body_ClearBall().execute( agent );
                agent->setNeckAction( new Neck_ScanField() );
                return true;
            }

            agent->debugClient().addMessage( "hold" );
            dlog.addText( Logger::TEAM,
                          __FILE__" (Bhv_ChainAction) hold" );

//.........这里部分代码省略.........

/*!

 */
bool
Neck_TurnToLowConfTeammate::execute( PlayerAgent * agent )
{
    const WorldModel & wm = agent->world();
    const PlayerPtrCont & teammates = wm.teammatesFromSelf();

    if ( teammates.empty() )
    {
        dlog.addText( Logger::ACTION,
                      __FILE__": no teammate" );
        return Neck_ScanField().execute( agent );
    }

    const double next_view_half_width = agent->effector().queuedNextViewWidth().width() * 0.5;
    const double next_neck_half_range = ServerParam::i().maxNeckAngle() + next_view_half_width;
    const Vector2D next_self_pos = agent->effector().queuedNextSelfPos();
    const AngleDeg next_self_body = agent->effector().queuedNextSelfBody();

    int pos_count = 1;
    Vector2D candidate_point( -52.5, 0.0 );
    int candidate_unum = 0;

    const double max_dist = 40.0;

    const PlayerPtrCont::const_iterator t_end = teammates.end();
    for ( PlayerPtrCont::const_iterator it = teammates.begin();
          it != t_end;
          ++it )
    {
        if ( (*it)->isGhost() )
        {
            continue;
        }

        if ( (*it)->distFromSelf() > max_dist )
        {
            break;
        }

        if ( (*it)->posCount() >= pos_count
             && candidate_point.x < (*it)->pos().x )
        {
            Vector2D pos = (*it)->pos() + (*it)->vel();
            AngleDeg angle = ( pos - next_self_pos ).th();

            if ( ( angle - next_self_body ).abs() < next_neck_half_range - 5.0 )
            {
                // can face
                candidate_unum = (*it)->unum();
                candidate_point = pos;
                pos_count = (*it)->posCount();
            }
        }
    }

    if ( pos_count <= 1 )
    {
        dlog.addText( Logger::ACTION,
                      __FILE__": no candidate. pos_count=%d",
                      pos_count );
        return Neck_TurnToBallOrScan().execute( agent );
    }

    dlog.addText( Logger::ACTION,
                  __FILE__": look teammate %d (%.1f, %.1f). pos_count=%d",
                  candidate_unum,
                  candidate_point.x, candidate_point.y,
                  pos_count );

    //
    // search the best angle
    //

    AngleDeg player_angle = ( candidate_point - next_self_pos ).th();
    AngleDeg left_max_angle = player_angle - ( next_view_half_width - 15.0 );
    AngleDeg right_max_angle = player_angle + ( next_view_half_width - 15.0 );

    if ( left_max_angle.isLeftOf( next_self_body + ServerParam::i().minNeckAngle() ) )
    {
        left_max_angle = next_self_body + ServerParam::i().minNeckAngle();
    }

    if ( right_max_angle.isRightOf( next_self_body + ServerParam::i().maxNeckAngle() ) )
    {
        right_max_angle = next_self_body + ServerParam::i().maxNeckAngle();
    }

    dlog.addText( Logger::ACTION,
                  __FILE__": player_angle=%.0f  left_angle=%.0f  right_angle=%.0f",
                  player_angle.degree(),
                  left_max_angle.degree(),
                  right_max_angle.degree() );

    // count players in view cone

    const double angle_range = ( right_max_angle - left_max_angle ).abs();
    const double neck_step = angle_range / 10.0;
//.........这里部分代码省略.........

/*!

 */
bool
Bhv_PenaltyKick::getShootTarget( const PlayerAgent * agent,
                                 Vector2D * point,
                                 double * first_speed )
{
    const WorldModel & wm = agent->world();
    const ServerParam & SP = ServerParam::i();

    if ( SP.theirTeamGoalPos().dist2( wm.ball().pos() ) > std::pow( 35.0, 2 ) )
    {
        dlog.addText( Logger::TEAM,
                      __FILE__" (getShootTarget) too far" );
        return false;
    }

    const PlayerObject * opp_goalie = wm.getOpponentGoalie();

    // goalie is not found.
    if ( ! opp_goalie )
    {
        Vector2D shot_c = SP.theirTeamGoalPos();
        if ( point ) *point = shot_c;
        if ( first_speed ) *first_speed = SP.ballSpeedMax();

        dlog.addText( Logger::TEAM,
                      __FILE__" (getShootTarget) no goalie" );
        return true;
    }

    int best_l_or_r = 0;
    double best_speed = SP.ballSpeedMax() + 1.0;

    double post_buf = 1.0
        + std::min( 2.0,
                    ( SP.pitchHalfLength() - wm.self().pos().absX() ) * 0.1 );

    // consder only 2 angle
    Vector2D shot_l( SP.pitchHalfLength(), -SP.goalHalfWidth() + post_buf );
    Vector2D shot_r( SP.pitchHalfLength(), +SP.goalHalfWidth() - post_buf );

    const AngleDeg angle_l = ( shot_l - wm.ball().pos() ).th();
    const AngleDeg angle_r = ( shot_r - wm.ball().pos() ).th();

    // !!! Magic Number !!!
    const double goalie_max_speed = 1.0;
    // default player speed max * conf decay
    const double goalie_dist_buf
        = goalie_max_speed * std::min( 5, opp_goalie->posCount() )
        + SP.catchAreaLength()
        + 0.2;

    const Vector2D goalie_next_pos = opp_goalie->pos() + opp_goalie->vel();

    for ( int i = 0; i < 2; i++ )
    {
        const Vector2D target = ( i == 0 ? shot_l : shot_r );
        const AngleDeg angle = ( i == 0 ? angle_l : angle_r );

        double dist2goal = wm.ball().pos().dist( target );

        // set minimum speed to reach the goal line
        double tmp_first_speed =  ( dist2goal + 5.0 ) * ( 1.0 - SP.ballDecay() );
        tmp_first_speed = std::max( 1.2, tmp_first_speed );

        bool over_max = false;
        while ( ! over_max )
        {
            if ( tmp_first_speed > SP.ballSpeedMax() )
            {
                over_max = true;
                tmp_first_speed = SP.ballSpeedMax();
            }

            Vector2D ball_pos = wm.ball().pos();
            Vector2D ball_vel = Vector2D::polar2vector( tmp_first_speed, angle );
            ball_pos += ball_vel;
            ball_vel *= SP.ballDecay();

            bool goalie_can_reach = false;

            // goalie move at first step is ignored (cycle is set to ZERO),
            // because goalie must look the ball velocity before chasing action.
            double cycle = 0.0;
            while ( ball_pos.absX() < SP.pitchHalfLength() )
            {
                if ( goalie_next_pos.dist( ball_pos )
                     < goalie_max_speed * cycle + goalie_dist_buf )
                {
                    dlog.addText( Logger::TEAM,
                                  __FILE__" (getShootTarget) goalie can reach. cycle=%.0f"
                                  " target=(%.1f, %.1f) speed=%.1f",
                                  cycle + 1.0, target.x, target.y, tmp_first_speed );
                    goalie_can_reach = true;
                    break;
                }

                ball_pos += ball_vel;
//.........这里部分代码省略.........

/*!
  execute action
*/
bool
Bhv_GoalieChaseBall::execute( PlayerAgent * agent )
{
    dlog.addText( Logger::TEAM,
                  __FILE__": Bhv_GoalieChaseBall" );

    //////////////////////////////////////////////////////////////////
    // tackle
    if ( Bhv_BasicTackle( 0.8, 90.0 ).execute( agent ) )
    {
        dlog.addText( Logger::TEAM,
                      __FILE__": tackle" );
        return true;
    }

    const ServerParam & SP = ServerParam::i();
    const WorldModel & wm = agent->world();

    ////////////////////////////////////////////////////////////////////////
    // get active interception catch point

    Vector2D my_int_pos = wm.ball().inertiaPoint( wm.interceptTable()->selfReachCycle() );
    dlog.addText( Logger::TEAM,
                  __FILE__": execute. intercept point=(%.2f %.2f)",
                  my_int_pos.x, my_int_pos.y );

    double pen_thr = wm.ball().distFromSelf() * 0.1 + 1.0;
    if ( pen_thr < 1.0 ) pen_thr = 1.0;

    //----------------------------------------------------------
    const Line2D ball_line( wm.ball().pos(), wm.ball().vel().th() );
    const Line2D defend_line( Vector2D( wm.self().pos().x, -10.0 ),
                              Vector2D( wm.self().pos().x, 10.0 ) );

    if ( my_int_pos.x > - SP.pitchHalfLength() - 1.0
         && my_int_pos.x < SP.ourPenaltyAreaLineX() - pen_thr
         && my_int_pos.absY() < SP.penaltyAreaHalfWidth() - pen_thr )
    {
        bool save_recovery = false;
        if ( ball_line.dist( wm.self().pos() ) < SP.catchableArea() )
        {
            save_recovery = true;
        }
        dlog.addText( Logger::TEAM,
                      __FILE__": execute normal intercept" );
        agent->debugClient().addMessage( "Intercept(0)" );
        Body_Intercept( save_recovery ).execute( agent );
        agent->setNeckAction( new Neck_TurnToBall() );
        return true;
    }

    int self_goalie_min = wm.interceptTable()->selfReachCycle();
    int opp_min_cyc = wm.interceptTable()->opponentReachCycle();

    Vector2D intersection = ball_line.intersection( defend_line );
    if ( ! intersection.isValid()
         || ball_line.dist( wm.self().pos() ) < SP.catchableArea() * 0.8
         || intersection.absY() > SP.goalHalfWidth() + 3.0
         )
    {
        if ( ! wm.existKickableOpponent() )
        {
            if ( self_goalie_min <= opp_min_cyc + 2
                 && my_int_pos.x > -SP.pitchHalfLength() - 2.0
                 && my_int_pos.x < SP.ourPenaltyAreaLineX() - pen_thr
                 && my_int_pos.absY() < SP.penaltyAreaHalfWidth() - pen_thr
                 )
            {
                if ( Body_Intercept( false ).execute( agent ) )
                {
                    dlog.addText( Logger::TEAM,
                                  __FILE__": execute normal interception" );
                    agent->debugClient().addMessage( "Intercept(1)" );
                    agent->setNeckAction( new Neck_TurnToBall() );
                    return true;
                }
            }

            dlog.addText( Logger::TEAM,
                          __FILE__": execute. ball vel has same slope to my body??"
                          " myvel-ang=%f body=%f. go to ball direct",
                          wm.ball().vel().th().degree(),
                          wm.self().body().degree() );
            // ball vel angle is same to my body angle
            agent->debugClient().addMessage( "GoToCatch(1)" );
            doGoToCatchPoint( agent, wm.ball().pos() );
            return true;
        }
    }

    //----------------------------------------------------------
    // body is already face to intersection
    // only dash to intersection

    // check catch point
    if ( intersection.absX() > SP.pitchHalfLength() + SP.catchableArea()
         || intersection.x > SP.ourPenaltyAreaLineX() - SP.catchableArea()
//.........这里部分代码省略.........

/*!

 */
bool
CrossGenerator::checkOpponent( const Vector2D & first_ball_pos,
                               const rcsc::AbstractPlayerObject * receiver,
                               const Vector2D & receive_pos,
                               const double & first_ball_speed,
                               const AngleDeg & ball_move_angle,
                               const int max_cycle )
{
    static const double CONTROL_AREA_BUF = 0.15;  // buffer for kick table

    const ServerParam & SP = ServerParam::i();

    const double receiver_dist = receiver->pos().dist( first_ball_pos );
    const Vector2D first_ball_vel
        = Vector2D( receive_pos - first_ball_pos ).setLength( first_ball_speed );

    const AbstractPlayerCont::const_iterator end = M_opponents.end();
    for ( AbstractPlayerCont::const_iterator o = M_opponents.begin();
          o != end;
          ++o )
    {
        const PlayerType * ptype = (*o)->playerTypePtr();
        const double control_area = ( (*o)->goalie()
                                      ? SP.catchableArea()
                                      : ptype->kickableArea() );

        const Vector2D opponent_pos = (*o)->inertiaFinalPoint();
        const int min_cycle = FieldAnalyzer::estimate_min_reach_cycle( opponent_pos,
                                                                       ptype->realSpeedMax(),
                                                                       first_ball_pos,
                                                                       ball_move_angle );


        if ( opponent_pos.dist( first_ball_pos ) > receiver_dist + 1.0 )
        {
#ifdef DEBUG_PRINT
            dlog.addText( Logger::CROSS,
                          "__ opponent[%d](%.2f %.2f) skip. distance over",
                          (*o)->unum(),
                          opponent_pos.x, opponent_pos.y );
#endif
            continue;
        }

        for ( int cycle = std::max( 1, min_cycle );
              cycle <= max_cycle;
              ++cycle )
        {
            Vector2D ball_pos = inertia_n_step_point( first_ball_pos,
                                                      first_ball_vel,
                                                      cycle,
                                                      SP.ballDecay() );
            double target_dist = opponent_pos.dist( ball_pos );

            if ( target_dist - control_area - CONTROL_AREA_BUF < 0.001 )
            {
#ifdef DEBUG_PRINT_FAILED_COURSE
                dlog.addText( Logger::CROSS,
                              "%d: xxx recvPos(%.2f %.2f) step=%d/%d"
                              " opponent(%d)(%.2f %.2f) kickable"
                              " ballPos(%.2f %.2f)",
                              M_total_count,
                              receive_pos.x, receive_pos.y,
                              cycle, max_cycle,
                              (*o)->unum(), opponent_pos.x, opponent_pos.y ,
                              ball_pos.x, ball_pos.y );
                debug_paint_failed( M_total_count, receive_pos );
#endif
                return false;
            }

            double dash_dist = target_dist;

            if ( cycle > 1 )
            {
                //dash_dist -= control_area*0.8;
                dash_dist -= control_area*0.6;
                //dash_dist -= control_area*0.5;
            }

            if ( dash_dist > ptype->realSpeedMax() * cycle )
            {
                continue;
            }

            //
            // dash
            //

            int n_dash = ptype->cyclesToReachDistance( dash_dist * 1.05 ); // add penalty

            if ( n_dash > cycle )
            {
                continue;
            }

            //
//.........这里部分代码省略.........