void FieldPainter::drawRobotAbs (const AbsCoord &pos, QColor colour, bool ellipse, QColor varColour)
{
static const int robot_radius = 120;
QRect roboRect(-robot_radius,-robot_radius,robot_radius*2,robot_radius*2);
QPen variancePen(varColour);
variancePen.setWidth(20);
// draw pacman
save();
translate(pos.x(), pos.y());
setPen("black");
int fov;
if (pos.var(2,2) > 0) {
fov = MIN(160, RAD2DEG(sqrt(pos.var(2,2))));
} else {
fov = 0;
}
setBrush(QBrush(colour));
rotate((RAD2DEG(pos.theta())-(fov/2)));
drawPie(roboRect, 0, (360-fov)*16);
restore();
/* Converting covariance matrix to ellipse representation
* Eigenvalues of the matrix are the major and minor axis lengths
* Eigenvectors are the vectors from the center along the axes (should be perpendicular)
*/
if (ellipse) {
Eigen::VectorXf eigenvalues = pos.var.block<2, 2>(0,0).marked<Eigen::SelfAdjoint>().eigenvalues();
int r_major_axis = MAX(sqrt(MAX(eigenvalues[0], eigenvalues[1])), robot_radius);
int r_minor_axis = MAX(sqrt(MIN(eigenvalues[0], eigenvalues[1])), robot_radius);
float theta = atan2(2*pos.var(0,1), pos.var(0,0)-pos.var(1,1))/2.0;
// draw position covariance ellipse
save();
translate(pos.x(), pos.y());
rotate(RAD2DEG(theta));
QRect varRect(-r_major_axis, -r_minor_axis, r_major_axis*2, r_minor_axis*2);
setPen(variancePen);
setBrush(QBrush(Qt::NoBrush));
drawEllipse(varRect);
restore();
}
}
void WPainter::drawPie(double x, double y, double width, double height,
int startAngle, int spanAngle)
{
drawPie(WRectF(x, y, width, height), startAngle, spanAngle);
}
void KivioShapePainter::drawShape( KivioShape *pShape, float x, float y, float w, float h )
{
KivioShapeData *pShapeData;
m_x = x;
m_y = y;
m_w = w;
m_h = h;
m_pShape = pShape;
pShapeData = pShape->shapeData();
switch( pShapeData->shapeType() )
{
case KivioShapeData::kstArc:
drawArc();
break;
case KivioShapeData::kstPie:
drawPie();
break;
case KivioShapeData::kstLineArray:
drawLineArray();
break;
case KivioShapeData::kstPolyline:
drawPolyline();
break;
case KivioShapeData::kstPolygon:
drawPolygon();
break;
case KivioShapeData::kstBezier:
drawBezier();
break;
case KivioShapeData::kstRectangle:
drawRectangle();
break;
case KivioShapeData::kstRoundRectangle:
drawRoundRectangle();
break;
case KivioShapeData::kstEllipse:
drawEllipse();
break;
case KivioShapeData::kstOpenPath:
drawOpenPath();
break;
case KivioShapeData::kstClosedPath:
drawClosedPath();
break;
case KivioShapeData::kstTextBox:
drawTextBox();
break;
case KivioShapeData::kstNone:
default:
break;
}
}
