예제 #1
0
void SharedBallModule::incorporateWorldModel(messages::WorldModel newModel) {
    if(newModel.ball_on()) {
        // heading + bearing
        float hb = TO_RAD*newModel.my_h() + TO_RAD*newModel.ball_bearing();
        float sinHB, cosHB;
        sincosf(hb, &sinHB, &cosHB);

        float globalX = newModel.my_x() + newModel.ball_dist()*cosHB;
        float globalY = newModel.my_y() + newModel.ball_dist()*sinHB;

        x = ALPHA*globalX + (1-ALPHA)*x;
        y = ALPHA*globalY + (1-ALPHA)*y;
        updatedThisFrame = true;
    }
}
예제 #2
0
void SharedBallModule::incorporateGoalieWorldModel(messages::WorldModel newModel) {
    if(newModel.ball_on()) {
        // heading + bearing

        // Assume goalie in position (FIELD_WHITE_LEFT_SIDELINE,
        //                            CENTER_FIELD_Y,
        //                            HEADING_RIGHT

        float hb = TO_RAD*HEADING_RIGHT + TO_RAD*newModel.ball_bearing();
        float sinHB, cosHB;
        sincosf(hb, &sinHB, &cosHB);

        float globalX = FIELD_WHITE_LEFT_SIDELINE_X + newModel.ball_dist()*cosHB;
        float globalY = CENTER_FIELD_Y + newModel.ball_dist()*sinHB;

        x = ALPHA*globalX + (1-ALPHA)*x;
        y = ALPHA*globalY + (1-ALPHA)*y;
        updatedThisFrame = true;
    }
}
예제 #3
0
void WorldViewPainter::paintRobotLocation(QPaintEvent* event,
                                          messages::WorldModel msg,
                                          QString playerNum)
{
    if (!msg.active())
    {
        return; // Don't paint robots that aren't there.
    }

    QPainter painter(this);
    if (flipped)
    {
        painter.translate(FIELD_GREEN_WIDTH*scaleFactor, 0);
        painter.scale(-scaleFactor, scaleFactor);
    }
    else
    {
        painter.translate(0, FIELD_GREEN_HEIGHT*scaleFactor);
        painter.scale(scaleFactor, -scaleFactor);
    }

    Qt::GlobalColor brushColor = Qt::cyan;
    if (msg.fallen())
        brushColor = Qt::red;
    else if (msg.in_kicking_state())
        brushColor = Qt::blue;

    const QPoint locCenter(msg.my_x(), msg.my_y());

    painter.setBrush(Qt::Dense7Pattern);

    // Draw my uncertainty
    painter.drawEllipse(locCenter,
                        (int)msg.my_uncert(),
                        (int)msg.my_uncert());


    if (!msg.fallen()){
        // Draw boundaries of my field of vision
        painter.setPen(QPen(Qt::cyan, 3));
        painter.drawLine(msg.my_x(),
                         msg.my_y(),
                         VISION_DISTANCE * std::cos(TO_RAD*(msg.my_h()+VISION_SPAN/2)) + msg.my_x(),
                         VISION_DISTANCE * std::sin(TO_RAD*(msg.my_h()+VISION_SPAN/2)) + msg.my_y());
        painter.drawLine(msg.my_x(),
                         msg.my_y(),
                         VISION_DISTANCE * std::cos(TO_RAD*(msg.my_h()-VISION_SPAN/2)) + msg.my_x(),
                         VISION_DISTANCE * std::sin(TO_RAD*(msg.my_h()-VISION_SPAN/2)) + msg.my_y());

        // Draw where I'm walking to
        painter.setPen(QPen(Qt::black, 2));
        const QPoint walkingEnd(msg.walking_to_x(), msg.walking_to_y());
        painter.drawLine(locCenter, walkingEnd);
    }

    painter.setBrush(brushColor);
    painter.setPen(brushColor);

    // Draw myself
    painter.drawEllipse(locCenter,
                        ROBOT_WIDTH,
                        ROBOT_WIDTH);

    painter.setPen(Qt::black);
    // Draw my heading
    painter.drawLine(msg.my_x(),
                     msg.my_y(),
                     ROBOT_WIDTH * std::cos(TO_RAD*msg.my_h()) + msg.my_x(),
                     ROBOT_WIDTH * std::sin(TO_RAD*msg.my_h()) + msg.my_y());

    // Draw my number
    painter.setPen(brushColor);
    painter.scale(1, -1); // Scale y so that the number is right-side up.
    painter.drawText(locCenter.x() + 15, -locCenter.y(), playerNum);
    painter.scale(1,-1);
    painter.setPen(Qt::black);

    // Draw the ball
    if(msg.ball_on()){
        QPoint ballCenter(msg.my_x()+msg.ball_dist()*std::cos(TO_RAD*msg.my_h()+TO_RAD*msg.ball_bearing()),
                          msg.my_y()+msg.ball_dist()*std::sin(TO_RAD*msg.my_h()+TO_RAD*msg.ball_bearing()));

        //draw where I think the ball is
        //ball color: orange(unclaimed), red (claimed)
        if(msg.claimed_ball()) {
            painter.setBrush(QColor::fromRgb(255,0,0));
        }
        painter.setBrush(QColor::fromRgb(205,140,0));
        painter.drawEllipse(ballCenter,
                            8,
                            8);

        // Draw my number
        painter.setPen(brushColor);
        painter.scale(1, -1); // Scale y so that the number is right-side up.
        painter.drawText(ballCenter.x() + 15, -ballCenter.y(), playerNum);
        painter.scale(1,-1);
        painter.setPen(Qt::black);
    }

    // Draw where I am kicking
    if(msg.in_kicking_state()){
        const QPoint ballCenter(msg.my_x()+msg.ball_dist()*std::cos(TO_RAD*msg.my_h()+TO_RAD*msg.ball_bearing()),
                                msg.my_y()+msg.ball_dist()*std::sin(TO_RAD*msg.my_h()+TO_RAD*msg.ball_bearing()));
        const QPoint kickingEnd(msg.kicking_to_x(), msg.kicking_to_y());

        // Draw line to where I am kicking
        painter.setPen(QPen(Qt::blue, 2));
        painter.drawLine(ballCenter, kickingEnd);
    }
}