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;
}
}
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;
}
}
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);
}
}