void RCDrawRobot::drawRobotTrail(InnerModel * innerModel, int QUEUE_SIZE)
{
QVec p1 (3);
int radio = 320;
//Body
QVec geomCenter = innerModel->transform("world", QVec::vec3(0, 0, -radio), "base");
QVec leftWheelP1 = innerModel->transform("world", QVec::vec3(-radio, 0, 75), "base");
QVec leftWheelP2 = innerModel->transform("world", QVec::vec3(-radio, 0, -75), "base");
QLine leftWheel(QPoint(leftWheelP1.x(), leftWheelP1.z()),QPoint(leftWheelP2.x(),leftWheelP2.z()));
QVec rightWheelP1 = innerModel->transform("world", QVec::vec3(radio, 0, 75), "base");
QVec rightWheelP2 = innerModel->transform("world", QVec::vec3(radio, 0, -75), "base");
QLine rightWheel(QPoint(rightWheelP1.x(), rightWheelP1.z()),QPoint(rightWheelP2.x(),rightWheelP2.z()));
//drawEllipse( QPointF( b(0),b(2) ) , radio, radio, Qt::red );
QVec base = innerModel->transform6D("world", "base");
drawSquare( QPointF( (int) rint(geomCenter(0)), (int) rint(geomCenter(2)) ), 2*radio, 2*radio, Qt::green, true, -1, -base(4) );
drawEllipse( QPointF(base(0), base(2) ), radio/6, radio/6, Qt::red, true );
//Wheels
drawLine( leftWheel, Qt::black, 60);
drawLine( rightWheel, Qt::black, 60);
if((geomCenter(0)-visibleCenter(0))<win.x())
visibleCenter(0)=geomCenter(0);
if((geomCenter(0)-visibleCenter(0))>win.x()+win.width())
visibleCenter(0)=geomCenter(0);
if((geomCenter(2)-visibleCenter(1))<-win.y())
visibleCenter(1)=geomCenter(2);
if((geomCenter(2)-visibleCenter(1))>-win.y()-win.height())
visibleCenter(1)=geomCenter(2);
trajec.enqueue(QPointF(base(0), base(2)));
if( trajec.size() > QUEUE_SIZE and trajec.isEmpty()==false)
trajec.dequeue();
foreach(QPointF p, trajec)
{
drawSquare(p,10,10,Qt::green,true);
}
void RCDrawRobot::drawRobot(InnerModel * innerModel, const QColor &color)
{
QVec p1 (3);
int radio = 320;
//Body
QVec geomCenter = innerModel->transform("world", QVec::vec3(0,0,0), "robotGeometricCenter");
QVec leftWheelP1 = innerModel->transform("world", QVec::vec3(-radio-50, 0, 75), "base");
QVec leftWheelP2 = innerModel->transform("world", QVec::vec3(-radio-50, 0, -75), "base");
QVec headRobot = innerModel->transform("world", QVec::vec3(0, 0, radio), "base" );
QLine leftWheel(QPoint(leftWheelP1.x(), leftWheelP1.z()),QPoint(leftWheelP2.x(),leftWheelP2.z()));
QVec rightWheelP1 = innerModel->transform("world", QVec::vec3(radio+50, 0, 75), "base");
QVec rightWheelP2 = innerModel->transform("world", QVec::vec3(radio+50, 0, -75), "base");
QLine rightWheel(QPoint(rightWheelP1.x(), rightWheelP1.z()),QPoint(rightWheelP2.x(),rightWheelP2.z()));
QMat m = innerModel->getRotationMatrixTo("world", "base");
drawSquare( QPointF( (int) rint(geomCenter(0)), (int) rint(geomCenter(2)) ), 2.*radio, 2.*radio, color, true, -1, atan2f(m(2,0),m(0,0)));
drawSquare( QPointF( headRobot.x(), headRobot.z() ), radio/3, radio/3, Qt::magenta, true );
// drawEllipse( QPointF( innerModel->getBaseX(), innerModel->getBaseZ() ), radio/6, radio/6, Qt::magenta, true );
//Wheels
drawLine( leftWheel, Qt::black, 60);
drawLine( rightWheel, Qt::black, 60);
// drawLeftFieldOfView(innerModel);
// drawRightFieldOfView(innerModel);
if((geomCenter(0)-visibleCenter(0))<win.x())
visibleCenter(0)=geomCenter(0);
if((geomCenter(0)-visibleCenter(0))>win.x()+win.width())
visibleCenter(0)=geomCenter(0);
if((geomCenter(2)-visibleCenter(1))<-win.y())
visibleCenter(1)=geomCenter(2);
if((geomCenter(2)-visibleCenter(1))>-win.y()-win.height())
visibleCenter(1)=geomCenter(2);
}
void RCDraw::drawAxis ( const QColor &c, int w )
{
static int step = 310; //ancho baldosa beta
for ( int i=step+visibleCenter(0); i< effWin.width()+visibleCenter(0); i+=step )
{
drawLine ( QLineF ( i, effWin.y()+visibleCenter(1), i, effWin.height()+visibleCenter(1) ), c, w );
}
for ( int i=-step+visibleCenter(0); i> effWin.x()+visibleCenter(0); i-=step )
{
drawLine ( QLineF ( i, effWin.y()+visibleCenter(1), i, effWin.height()+visibleCenter(1) ), c, w );
}
for ( int i=step+visibleCenter(1); i< effWin.y()+visibleCenter(1); i+=step )
{
drawLine ( QLineF ( effWin.x()+visibleCenter(0), i, effWin.width()+visibleCenter(0), i ), c, w );
}
for ( int i=-step+visibleCenter(1); i> effWin.height()+visibleCenter(1); i-=step )
{
drawLine ( QLineF ( effWin.x()+visibleCenter(0), i, effWin.width()+visibleCenter(0), i ), c, w );
}
drawLine ( QLine ( rint(effWin.x()+visibleCenter(0)), 0, rint(effWin.x() + effWin.width()+visibleCenter(0)), 0 ), Qt::red, w*2 );
drawLine ( QLine ( 0, effWin.y()+visibleCenter(1), 0,effWin.y() + effWin.height() + visibleCenter(1)), Qt::red, w*2 );
}
void RCDraw::paintEvent ( QPaintEvent * )
{
QString s;
QPainter painter ( this );
painter.setRenderHint(QPainter::HighQualityAntialiasing);
if ( qimg != NULL )
{
painter.drawImage ( QRectF(0., 0., imageScale*width, imageScale*height), *qimg, QRectF(0, 0, width, height) );
}
painter.setWindow (effWin.toRect() );
if ( DRAW_PERIMETER )
{
painter.setPen ( Qt::blue );
painter.drawRect ( 0,0,width-1,height-1 );
}
if ( DRAW_AXIS )
{
drawAxis(Qt::blue, 2);
}
//Draw lines
while ( !lineQueue.isEmpty() )
{
TLine l = lineQueue.dequeue();
if (invertedVerticalAxis) l.line = QLineF(l.line.x1()-visibleCenter(0), -l.line.y1()+visibleCenter(1), l.line.x2()-visibleCenter(0), -l.line.y2()+visibleCenter(1));
else l.line.translate(-visibleCenter(0), -visibleCenter(1));
painter.setPen ( QPen ( QBrush ( l.color ),l.width ) );
painter.drawLine ( l.line );
}
//Draw gradient
while ( !gradQueue.isEmpty() )
{
TGrad g = gradQueue.dequeue();
if (invertedVerticalAxis) g.line = QLine(g.line.x1()-visibleCenter(0), -g.line.y1()+visibleCenter(1), g.line.x2()-visibleCenter(0), -g.line.y2()+visibleCenter(1));
else g.line.translate(-visibleCenter(0), -visibleCenter(1));
linGrad.setColorAt ( 0, g.color );
linGrad.setColorAt ( 1, g.color1 );
painter.setBrush ( linGrad );
painter.setPen ( QPen ( linGrad, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin ) );
painter.drawLine ( g.line );
}
//Draw ellipses
while ( !ellipseQueue.isEmpty() )
{
TEllipse e = ellipseQueue.dequeue();
if (invertedVerticalAxis) e.center.setY(-(e.center.y()-visibleCenter(1)));
else e.center = QPointF(e.center.x()-visibleCenter(0), e.center.y()-visibleCenter(1));
if ( e.fill == true )
painter.setBrush ( e.color );
else
painter.setBrush ( Qt::transparent );
painter.setPen ( e.color );
if (fabs(e.ang) > 0.1)
{
painter.setPen ( e.color );
painter.translate( e.center );
painter.rotate( e.ang );
painter.drawEllipse ( QPointF(0,0), e.rx, e.ry );
painter.rotate( -e.ang );
painter.translate( -e.center );
}
else
painter.drawEllipse( e.center, e.rx, e.ry);
}
//Draw squares
{
QPen pen = painter.pen();
int penwidth = pen.width();
while ( !squareQueue.isEmpty() )
{
TRect r = squareQueue.dequeue();
if (invertedVerticalAxis) r.rect = QRect(r.rect.x()-visibleCenter(0), -r.rect.y()+visibleCenter(1)-r.rect.height(), r.rect.width(), r.rect.height());
else r.rect.translate(-visibleCenter(0),-visibleCenter(1));
if ( r.fill == true )
painter.setBrush ( r.color );
else
painter.setBrush ( Qt::transparent );
pen.setColor(r.color);
pen.setWidth(r.width);
painter.setPen(pen);
if (fabs(r.ang) > 0.01 )
{
QPointF center = r.rect.center();
painter.translate( center );
painter.rotate( r.ang );
painter.drawRect ( QRectF( r.rect.topLeft() - center, r.rect.size() ));
painter.rotate( -r.ang );
painter.translate( -center );
}
else
painter.drawRect( r.rect );
}
pen.setWidth(penwidth);
painter.setPen(pen);
}
while ( !lineOnTopQueue.isEmpty() )
{
TLine l = lineOnTopQueue.dequeue();
l.line.translate(-visibleCenter(0), -visibleCenter(1));
painter.setPen ( QPen ( QBrush ( l.color ),l.width ) );
painter.drawLine ( l.line );
}
//Draw text
while ( !textQueue.isEmpty() )
{
TText t = textQueue.dequeue();
painter.setWindow ( effWin.normalized().toRect() );
QFont ant = painter.font();
QFont temp ( "Helvetica", t.size );
painter.setFont ( temp );
QRectF rect = painter.boundingRect(QRectF(t.pos.x(), t.pos.y(), 1, 1), Qt::AlignLeft, t.text);
if (not t.centered)
{
painter.setPen(t.bgc);
painter.setBrush(t.bgc);
painter.drawRect(rect);
painter.setBrush ( Qt::transparent );
painter.setPen ( t.color );
painter.drawText( QRectF(t.pos.x(), t.pos.y(), 0.82*t.text.size()*t.size, 1.2*t.size), Qt::AlignCenter, t.text);
}
else
{
rect.translate(-rect.width()/2., -rect.height()/2.);
painter.setPen(t.bgc);
painter.setBrush(t.bgc);
painter.drawRect(rect);
painter.setBrush ( Qt::transparent );
painter.setPen ( t.color );
painter.drawText(rect, Qt::AlignLeft, t.text);
}
painter.setFont ( ant );
painter.setWindow ( effWin.toRect() );
}
}
