void SimFieldView::drawTeamSpace(QPainter& p) {
FieldView::drawTeamSpace(p);
// Simulator drag-to-shoot
std::shared_ptr<LogFrame> frame = currentFrame();
if (_dragMode == DRAG_SHOOT && frame) {
p.setPen(QPen(Qt::white, 0.1f));
Geometry2d::Point ball = frame->ball().pos();
p.drawLine(ball.toQPointF(), _dragTo.toQPointF());
if (ball != _dragTo) {
p.setPen(QPen(Qt::gray, 0.1f));
_shot = (ball - _dragTo) * ShootScale;
float speed = _shot.mag();
Geometry2d::Point shotExtension = ball + _shot / speed * 8;
p.drawLine(ball.toQPointF(), shotExtension.toQPointF());
p.setPen(Qt::black);
drawText(p, _dragTo.toQPointF(),
QString("%1 m/s").arg(speed, 0, 'f', 1));
}
}
}
void FieldView::drawTeamSpace(QPainter& p) {
// Get the latest LogFrame
const LogFrame* frame = _history->at(0).get();
if (showTeamNames) {
// Draw Team Names
QFont savedFont = p.font();
QFont fontstyle = p.font();
fontstyle.setPointSize(20);
p.setFont(fontstyle);
p.setPen(bluePen);
drawText(p, QPointF(0, 4.75), QString(frame->team_name_blue().c_str()),
true); // Blue
p.setPen(yellowPen);
drawText(p, QPointF(0, 1.75),
QString(frame->team_name_yellow().c_str()),
true); // Yellow
p.setFont(savedFont);
}
// Block off half the field
if (!frame->use_our_half()) {
const float FX = Field_Dimensions::Current_Dimensions.FloorWidth() / 2;
const float FY1 = -Field_Dimensions::Current_Dimensions.Border();
const float FY2 = Field_Dimensions::Current_Dimensions.Length() / 2;
p.fillRect(QRectF(QPointF(-FX, FY1), QPointF(FX, FY2)),
QColor(0, 0, 0, 128));
}
if (!frame->use_opponent_half()) {
const float FX = Field_Dimensions::Current_Dimensions.FloorWidth() / 2;
const float FY1 = Field_Dimensions::Current_Dimensions.Length() / 2;
const float FY2 = Field_Dimensions::Current_Dimensions.Length() +
Field_Dimensions::Current_Dimensions.Border();
p.fillRect(QRectF(QPointF(-FX, FY1), QPointF(FX, FY2)),
QColor(0, 0, 0, 128));
}
if (showCoords) {
drawCoords(p);
}
// History
p.setBrush(Qt::NoBrush);
QPainterPath ballTrail;
for (unsigned int i = 0; i < 200 && i < _history->size(); ++i) {
const LogFrame* oldFrame = _history->at(i).get();
if (oldFrame && oldFrame->has_ball()) {
QPointF pos = qpointf(oldFrame->ball().pos());
if (i == 0)
ballTrail.moveTo(pos);
else
ballTrail.lineTo(pos);
}
}
QPen ballTrailPen(ballColor, 0.03);
ballTrailPen.setCapStyle(Qt::RoundCap);
p.setPen(ballTrailPen);
p.drawPath(ballTrail);
// Debug lines
for (const DebugPath& path : frame->debug_paths()) {
if (path.layer() < 0 || layerVisible(path.layer())) {
tempPen.setColor(qcolor(path.color()));
p.setPen(tempPen);
std::vector<QPointF> pts;
for (int i = 0; i < path.points_size(); ++i) {
pts.push_back(qpointf(path.points(i)));
}
p.drawPolyline(pts.data(), pts.size());
}
}
for (const DebugRobotPath& path : frame->debug_robot_paths()) {
if (path.layer() < 0 || layerVisible(path.layer())) {
for (int i = 0; i < path.points_size() - 1; ++i) {
const DebugRobotPath::DebugRobotPathPoint& from =
path.points(i);
const DebugRobotPath::DebugRobotPathPoint& to =
path.points(i + 1);
Geometry2d::Point avgVel =
(Geometry2d::Point(path.points(i).vel()) +
Geometry2d::Point(path.points(i + 1).vel())) /
2;
float pcntMaxSpd =
avgVel.mag() / MotionConstraints::defaultMaxSpeed();
QColor mixedColor(std::min((int)(255 * pcntMaxSpd), 255), 0,
std::min((int)(255 * (1 - pcntMaxSpd)), 255));
QPen pen(mixedColor);
pen.setCapStyle(Qt::RoundCap);
pen.setWidthF(0.03);
p.setPen(pen);
const Geometry2d::Point fromPos = Geometry2d::Point(from.pos());
const Geometry2d::Point toPos = Geometry2d::Point(to.pos());
p.drawLine(fromPos.toQPointF(), toPos.toQPointF());
}
}
}
// Debug circles
for (const DebugCircle& c : frame->debug_circles()) {
if (c.layer() < 0 || layerVisible(c.layer())) {
tempPen.setColor(c.color());
p.setPen(tempPen);
p.drawEllipse(qpointf(c.center()), c.radius(), c.radius());
}
}
// Debug arcs
for (const DebugArc& a : frame->debug_arcs()) {
if (a.layer() < 0 || layerVisible(a.layer())) {
tempPen.setColor(a.color());
p.setPen(tempPen);
auto c = a.center();
auto t1 = a.start();
auto t2 = a.end();
auto R = a.radius();
QRectF rect;
rect.setX(-R + c.x());
rect.setY(-R + c.y());
rect.setWidth(R * 2);
rect.setHeight(R * 2);
t1 *= -(180 / M_PI) * 16;
t2 *= -(180 / M_PI) * 16;
p.drawArc(rect, t1, t2 - t1);
}
}
// Debug text
for (const DebugText& text : frame->debug_texts()) {
if (text.layer() < 0 || layerVisible(text.layer())) {
tempPen.setColor(text.color());
p.setPen(tempPen);
drawText(p, qpointf(text.pos()),
QString::fromStdString(text.text()), text.center());
}
}
// Debug polygons
p.setPen(Qt::NoPen);
for (const DebugPath& path : frame->debug_polygons()) {
if (path.layer() < 0 || layerVisible(path.layer())) {
if (path.points_size() < 3) {
fprintf(stderr, "Ignoring DebugPolygon with %d points\n",
path.points_size());
continue;
}
QColor color = qcolor(path.color());
color.setAlpha(64);
p.setBrush(color);
std::vector<QPointF> pts;
for (int i = 0; i < path.points_size(); ++i) {
pts.push_back(qpointf(path.points(i)));
}
p.drawConvexPolygon(pts.data(), pts.size());
}
}
p.setBrush(Qt::NoBrush);
// maps robots to their comet trails, so we can draw a path of where each
// robot has been over the past X frames the pair used as a key is of the
// form (team, robot_id). Blue team = 1, yellow = 2. we only draw trails
// for robots that exist in the current frame
map<pair<int, int>, QPainterPath> cometTrails;
/// populate @cometTrails with the past locations of each robot
int pastLocationCount = 40; // number of past locations to show
for (int i = 0; i < pastLocationCount + 1 && i < _history->size(); i++) {
const LogFrame* oldFrame = _history->at(i).get();
if (oldFrame) {
for (const LogFrame::Robot& r : oldFrame->self()) {
pair<int, int> key(1, r.shell());
if (cometTrails.find(key) != cometTrails.end() || i == 0) {
QPointF pt = qpointf(r.pos());
if (i == 0)
cometTrails[key].moveTo(pt);
else
cometTrails[key].lineTo(pt);
}
}
for (const LogFrame::Robot& r : oldFrame->self()) {
pair<int, int> key(2, r.shell());
if (cometTrails.find(key) != cometTrails.end() || i == 0) {
QPointF pt = qpointf(r.pos());
if (i == 0)
cometTrails[key].moveTo(pt);
else
cometTrails[key].lineTo(pt);
}
}
}
}
// draw robot comet trails
const float cometTrailPenSize = 0.07;
for (auto& kv : cometTrails) {
QColor color = kv.first.first == 1 ? Qt::blue : Qt::yellow;
QPen pen(color, cometTrailPenSize);
pen.setCapStyle(Qt::RoundCap);
p.setPen(pen);
p.drawPath(kv.second);
}
// Text positioning vectors
QPointF rtX = qpointf(Geometry2d::Point(0, 1).rotated(-_rotate * 90));
QPointF rtY = qpointf(Geometry2d::Point(-1, 0).rotated(-_rotate * 90));
// Opponent robots
for (const LogFrame::Robot& r : frame->opp()) {
drawRobot(p, !frame->blue_team(), r.shell(), qpointf(r.pos()),
r.angle(), r.ball_sense_status() == HasBall);
}
// Our robots
int manualID = frame->manual_id();
for (const LogFrame::Robot& r : frame->self()) {
QPointF center = qpointf(r.pos());
bool faulty = false;
if (r.has_ball_sense_status() && (r.ball_sense_status() == Dazzled ||
r.ball_sense_status() == Failed)) {
faulty = true;
}
if (r.has_kicker_works() && !r.kicker_works()) {
// faulty = true;
}
for (int i = 0; i < r.motor_status().size(); ++i) {
if (r.motor_status(i) != Good) {
faulty = true;
}
}
if (r.has_battery_voltage() && r.battery_voltage() <= 14.3f) {
faulty = true;
}
drawRobot(p, frame->blue_team(), r.shell(), center, r.angle(),
r.ball_sense_status() == HasBall, faulty);
// Highlight the manually controlled robot
if (manualID == r.shell()) {
p.setPen(greenPen);
const float r = Robot_Radius + .05;
p.drawEllipse(center, r, r);
}
// Robot text
QPointF textPos = center - rtX * 0.2 - rtY * (Robot_Radius + 0.1);
for (const DebugText& text : r.text()) {
if (text.layer() < 0 || layerVisible(text.layer())) {
tempPen.setColor(text.color());
p.setPen(tempPen);
drawText(p, textPos, QString::fromStdString(text.text()),
false);
textPos -= rtY * 0.1;
}
}
}
// Current ball position and velocity
if (frame->has_ball()) {
QPointF pos = qpointf(frame->ball().pos());
QPointF vel = qpointf(frame->ball().vel());
p.setPen(ballPen);
p.setBrush(ballColor);
p.drawEllipse(QRectF(-Ball_Radius + pos.x(), -Ball_Radius + pos.y(),
Ball_Diameter, Ball_Diameter));
if (!vel.isNull()) {
p.drawLine(pos, QPointF(pos.x() + vel.x(), pos.y() + vel.y()));
}
}
}