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