worldsim/src/renderworld.cpp

/********************************************************************************
 *  WorldSim -- library for robot simulations                                   *
 *  Copyright (C) 2008-2011 Gianluca Massera <emmegian@yahoo.it>                *
 *                                                                              *
 *  This program is free software; you can redistribute it and/or modify        *
 *  it under the terms of the GNU General Public License as published by        *
 *  the Free Software Foundation; either version 2 of the License, or           *
 *  (at your option) any later version.                                         *
 *                                                                              *
 *  This program is distributed in the hope that it will be useful,             *
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of              *
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               *
 *  GNU General Public License for more details.                                *
 *                                                                              *
 *  You should have received a copy of the GNU General Public License           *
 *  along with this program; if not, write to the Free Software                 *
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA  *
 ********************************************************************************/

#include "renderworld.h"
#include "phyjoint.h"

#include <QImage>
#include <QColor>
#include <QKeyEvent>
#include <QMenu>
#include <QAction>
#include <QList>
#include <cmath>
#include <QDir>
#include <QLinkedList>
#include <QMutexLocker>
#include <QEvent>

// These instructions are needed because QT 4.8 no longer depends on glu, so we
// have to include it here explicitly
#ifdef FARSA_MAC
# include <GLUT/glut.h>
#else
# include <GL/glu.h>
#endif

using namespace qglviewer;

#define GLMultMatrix glMultMatrixf
// for double use #define GLMultMatrix glMultMatrixd

#include "qglviewer/camera.h"
#if QT_VERSION >= 0x040000
# include <QWheelEvent>
#endif

namespace farsa {
 
QMap<QString, QImage>* RenderWObjectContainer::textmap = NULL;
unsigned int RenderWObjectContainer::textmapRefCounter = 0;

RenderWObjectContainer::RenderWObjectContainer( QString wResName ) :
    worldResourceName(wResName),
    worldv(NULL),
    mutex(QMutex::Recursive)
{
    if ( textmap == NULL ) {
        textmap = new QMap<QString, QImage>();
        (*textmap)["tile1"].load( ":/tiles/16tile10.jpg" );
        (*textmap)["tile2"].load( ":/tiles/16tile07.jpg" );
        (*textmap)["white"].load( ":/white.jpg" );
        (*textmap)["tile3"].load( ":/tiles/16tile11.jpg" );
        (*textmap)["tile4"].load( ":/tiles/16tile-B.jpg" );
        (*textmap)["tile5"].load( ":/tiles/16tile12.jpg" );
        (*textmap)["tile6"].load( ":/tiles/16tile04.jpg" );
        (*textmap)["tile7"].load( ":/tiles/tile01.jpg" );
        (*textmap)["tile8"].load( ":/tiles/16tile02.jpg" );
        (*textmap)["tile9"].load( ":/tiles/16tile05.jpg" );
        (*textmap)["tile10"].load( ":/tiles/16tile08.jpg" );
        (*textmap)["icub"].load( ":/tiles/16tile11.jpg" ); //.load( ":/metal/iron05.jpg" );
        (*textmap)["icubFace"].load( ":/covers/face.jpg" );
        (*textmap)["blueye"].load( ":/covers/eyep2_b.jpg" );
        (*textmap)["metal"].load( ":/metal/iron05.jpg" );
        (*textmap)["marXbot_12leds"].load( ":/covers/marxbot_12leds.jpg" );
        //--- The order of the texture is:
        // 0 => TOP
        // 1 => BACK
        // 2 => FRONT
        // 3 => BOTTOM
        // 4 => RIGHT
        // 5 => LEFT
    /*  skyb[0].load( ":/skybox/sb_top.jpg" );
        skyb[1].load( ":/skybox/sb_back.jpg" );
        skyb[2].load( ":/skybox/sb_front.jpg" );
        skyb[3].load( ":/skybox/sb_bottom.jpg" );
        skyb[4].load( ":/skybox/sb_right.jpg" );
        skyb[5].load( ":/skybox/sb_left.jpg" );*/
        (*textmap)["skyb0"].load( ":/skybox/sb2_top.jpg" );
        (*textmap)["skyb1"].load( ":/skybox/sb2_back.jpg" );
        (*textmap)["skyb2"].load( ":/skybox/sb2_front.jpg" );
        (*textmap)["skyb3"].load( ":/ground/cobbles01.jpg" );
        (*textmap)["skyb4"].load( ":/skybox/sb2_right.jpg" );
        (*textmap)["skyb5"].load( ":/skybox/sb2_left.jpg" );
    }
    textmapRefCounter++;

    usableResources(QStringList() << worldResourceName);
}

RenderWObjectContainer::~RenderWObjectContainer() {
    foreach( RenderWObject* ro, graphs ) {
        delete ro;
    }
    textmapRefCounter--;
    if (textmapRefCounter == 0) {
        delete textmap;
        textmap = NULL;
    }
}

bool RenderWObjectContainer::addTextureImage( QString filename, QString texturename ) {
    QMutexLocker locker(&mutex);

    return ((*textmap)[texturename].load( filename ));
}

void RenderWObjectContainer::setWorld( World* newworld ) {
    // Here we simply declare a resource, the resourceChanged handler will take care of setting things up
    declareResource( worldResourceName, newworld );
}

RenderWObject* RenderWObjectContainer::operator[]( const WObject* obj ) {
    QMutexLocker locker(&mutex);

    //--- not efficient !! OPTIMIZE ME
    for( int i=0; i<graphs.size(); i++ ) {
        if ( graphs[i]->object() == obj ) {
            return graphs[i];
        }
    }
    return NULL;
}

RenderWObject* RenderWObjectContainer::createRenderWObjectFor( const WObject* obj, RenderWObjectContainer* container ) {
    initFactory();
    const QMetaObject* metaObj = obj->metaObject();
    while( metaObj ) {
        QString classname = metaObj->className();
        if (fac->contains(classname)) {
            return (*fac)[classname]->create( (WObject*)obj, container );
        }
        metaObj = metaObj->superClass();
    }

    // Cannot find a specific renderer, using the generic one (which doesn't render anything...
    // at least we save a crash)
    return (*fac)["farsa::WObject"]->create( (WObject*)obj, container );
}

void RenderWObjectContainer::addObject( WObject* wobj ) {
    // Always take the resource mutex first and then our mutex to avoid deadlocks
    ResourcesLocker resourceLocker(this); // Here because perhaps the RenderWObject need to access the world
    QMutexLocker locker(&mutex);

    graphs.append( createRenderWObjectFor( wobj, this ) );
}

void RenderWObjectContainer::removeObject( WObject* wobj ) {
    // Always take the resource mutex first and then our mutex to avoid deadlocks
    ResourcesLocker resourceLocker(this); // Here because perhaps the RenderWObject need to access the world
    QMutexLocker locker(&mutex);

    for( int i=0; i<graphs.size(); i++ ) {
        if ( graphs[i]->object() == wobj ) {
            RenderWObject* ro = graphs[i];
            graphs.remove( i );
            ro->objectAlreadyDestroyed();
            delete ro;
            break;
        }
    }
}

void RenderWObjectContainer::resourceChanged(QString name, ResourceChangeType changeType)
{
    QMutexLocker locker(&mutex);

    // We ignore all resources except world
    if (name != worldResourceName) {
        return;
    }

    // Removing all objects, they refer to the old world
    foreach( RenderWObject* ro, graphs ) {
        delete ro;
    }
    graphs.clear();

    // If world was deleted, we simply set the pointer to NULL, otherwise we set the pointer
    // to the new world
    if (changeType == Deleted) {
        worldv = NULL;
    } else {
        worldv = getResource<World>();
    }

    // Creating renderes for all objects
    if (worldv != NULL) {
        foreach( WObject* obj, worldv->objects() ) {
            graphs.append( createRenderWObjectFor( obj, this ) );
        }
    }
}

void RenderWObjectContainer::applyTexture( QGLContext* gw, QString texts ) {
    QMutexLocker locker(&mutex);

    if ( textmap->contains( texts ) ) {
        if ( !textGLId.contains( texts ) ) {
            textGLId[texts] = gw->bindTexture( (*textmap)[texts], GL_TEXTURE_2D, GL_RGB );
        }
        glBindTexture( GL_TEXTURE_2D, textGLId[texts] );
        glEnable( GL_TEXTURE_2D );
    }/* else {
        glDisable( GL_TEXTURE_2D );
    }*/
}

void RenderWObjectContainer::setupColorTexture( QGLContext* gw, RenderWObject* robj ) {
    // Always take the resource mutex first and then our mutex to avoid deadlocks
    ResourcesLocker resourceLocker(this);
    QMutexLocker locker(&mutex);

    // set the color
    glShadeModel( GL_SMOOTH );
    glEnable( GL_BLEND );
    glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );
    WObject* obj = robj->object();
    while ( obj->useColorTextureOfOwner() && ( obj->owner() != NULL ) ) {
        WObject *const owner = dynamic_cast<WObject *>(obj->owner());
        if ( owner != NULL ) {
            obj = owner;
        } else {
            break;
        }
    }
    QColor colorv = obj->color();
    glColor4f( colorv.redF(), colorv.greenF(), colorv.blueF(), colorv.alphaF() );
    applyTexture( gw, obj->texture() );
}

void RenderWObjectContainer::drawSkyGroundBox( QGLContext* gw ) {
    // Always take the resource mutex first and then our mutex to avoid deadlocks
    ResourcesLocker resourceLocker(this);
    QMutexLocker locker(&mutex);

    if (worldv == NULL) {
        return;
    }
    wVector min, max;
    worldv->size( min, max );
    wVector m_size = wVector( fabs(max[0]-min[0]), fabs(max[1]-min[1]), fabs(max[1]-min[1]) );

    resourceLocker.unlock(); // We don't need it anymore

    glDisable( GL_LIGHTING );
    glShadeModel( GL_FLAT );
    glColor4f( 1.0f, 1.0f, 1.0f, 1.0f );
    //glEnable(GL_TEXTURE_2D);
    //glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
    //glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
    //glTexEnvf (GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_DECAL);
    // the cube will just be drawn as six quads for the sake of simplicity
    // for each face, we specify the quad's normal (for lighting), then
    // specify the quad's 4 vertices's and associated texture coordinates
    // TOP
    applyTexture( gw, "skyb0" );
    glBegin(GL_QUADS);
    glTexCoord2f(0.0, 1.0); glVertex3f( min.x,  max.y, max.z);
    glTexCoord2f(0.0, 0.0); glVertex3f( max.x,  max.y, max.z);
    glTexCoord2f(1.0, 0.0); glVertex3f( max.x,  min.y, max.z);
    glTexCoord2f(1.0, 1.0); glVertex3f( min.x,  min.y, max.z);
    glEnd();
    // BACK
    applyTexture( gw, "skyb1" );
    glBegin(GL_QUADS);
    glTexCoord2f(1.0, 0.0); glVertex3f( min.x,  max.y,  min.z);
    glTexCoord2f(1.0, 1.0); glVertex3f( min.x,  max.y,  max.z);
    glTexCoord2f(0.0, 1.0); glVertex3f( min.x,  min.y,  max.z);
    glTexCoord2f(0.0, 0.0); glVertex3f( min.x,  min.y,  min.z);
    glEnd();
    // FRONT
    applyTexture( gw, "skyb2" );
    glBegin(GL_QUADS);
    glTexCoord2f(0.0, 1.0); glVertex3f(max.x,   max.y,  max.z);
    glTexCoord2f(0.0, 0.0); glVertex3f(max.x,   max.y,  min.z);
    glTexCoord2f(1.0, 0.0); glVertex3f(max.x,   min.y,  min.z);
    glTexCoord2f(1.0, 1.0); glVertex3f(max.x,   min.y,  max.z);
    glEnd();
    // BOTTOM
    applyTexture( gw, "skyb3" );
    //--- suppose the bottom texture will represent 40x40 cm of ground
    //--- and calculate repeating accordlying
    float bfs = m_size[1]/0.4;
    float bft = m_size[0]/0.4;
    glBegin(GL_QUADS);
    glTexCoord2f(0.0, bft); glVertex3f( max.x,  max.y,  min.z);
    glTexCoord2f(0.0, 0.0); glVertex3f( min.x,  max.y,  min.z);
    glTexCoord2f(bfs, 0.0); glVertex3f( min.x,  min.y,  min.z);
    glTexCoord2f(bfs, bft); glVertex3f( max.x,  min.y,  min.z);
    glEnd();
    // RIGHT
    applyTexture( gw, "skyb4" );
    glBegin(GL_QUADS);
    glTexCoord2f(1.0, 1.0); glVertex3f( min.x,  min.y,  max.z);
    glTexCoord2f(0.0, 1.0); glVertex3f( max.x,  min.y,  max.z);
    glTexCoord2f(0.0, 0.0); glVertex3f( max.x,  min.y,  min.z);
    glTexCoord2f(1.0, 0.0); glVertex3f( min.x,  min.y,  min.z);
    glEnd();
    // LEFT
    applyTexture( gw, "skyb5" );
    glBegin(GL_QUADS);
    glTexCoord2f(0.0, 0.0); glVertex3f( min.x,  max.y,  min.z);
    glTexCoord2f(1.0, 0.0); glVertex3f( max.x,  max.y,  min.z);
    glTexCoord2f(1.0, 1.0); glVertex3f( max.x,  max.y,  max.z);
    glTexCoord2f(0.0, 1.0); glVertex3f( min.x,  max.y,  max.z);
    glEnd();
}

class StandardCamera : public qglviewer::Camera {
public :
    StandardCamera() {
        orthoSize = 1.0;
    };
    virtual float zNear() const {
        return 0.001f;
    };
    virtual float zFar() const {
        return 1000.0f;
    };
    void changeOrthoFrustumSize(int delta) {
        if (delta > 0) {
            orthoSize *= 1.1f;
        } else {
            orthoSize /= 1.1f;
        }
    };
    virtual void getOrthoWidthHeight(GLdouble &halfWidth, GLdouble &halfHeight) const {
        halfHeight = orthoSize;
        halfWidth = aspectRatio() * orthoSize;
    };
private :
    float orthoSize;
};

class MyCameraConstraint : public qglviewer::Constraint {
public:
    MyCameraConstraint( qglviewer::Vec min, qglviewer::Vec max ) {
        //--- the pad allow to avoid to reach exactly the bounding-box of the world
        qglviewer::Vec pad(0.1,0.1,0.1);
        minP = min + pad;
        maxP = max - pad;
    };
    virtual void constrainTranslation( qglviewer::Vec& t, qglviewer::Frame* const fr ) {
        //--- Local System
        const qglviewer::Frame* ls = fr;
        if ( fr->referenceFrame() != NULL ) {
            qDebug() << "Using Reference";
            ls = fr->referenceFrame();
        }
        //--- Convert t to world coordinate system
        qglviewer::Vec tw = ls->inverseTransformOf( t );
        qglviewer::Vec pos = fr->position();
        if ( (pos.x + tw.x > maxP.x && t.x > 0) || (pos.x + tw.x < minP.x && t.x < 0) ||
             (pos.y + tw.y > maxP.y && t.y > 0) || (pos.y + tw.y < minP.y && t.y < 0) ||
             (pos.z + tw.z > maxP.z && t.z > 0) || (pos.z + tw.z < minP.z && t.z < 0) ) {
            t.z = 0.0;
            t.x = 0.0;
            t.y = 0.0;
        }
    };
    //--- boundings
    qglviewer::Vec minP;
    qglviewer::Vec maxP;
};

namespace {
    // A custom QEvent to force update of RenderWorldWrapperWidget
    class ForceRenderWorldUpdateEvent : public QEvent
    {
    public:
        ForceRenderWorldUpdateEvent() :
            QEvent(QEvent::User)
        {
        }

        virtual ~ForceRenderWorldUpdateEvent()
        {
        }
    };
}


RenderWorld::RenderWorld( QWidget* parent, QString wResName )
    : QGLViewer( parent ), RenderWObjectContainer( wResName ) {
    wiref = false;
    showskygroundbox = true;
    showobjs = true;
    showjoints = false;
    showaabbs = false;
    showcontacts = true;
    showforces = false;
    currentSelected = -1;

    setStateFileName( QString() );

    setCamera( new StandardCamera() );

    setContextMenuPolicy( Qt::CustomContextMenu );
}

RenderWorld::~RenderWorld() {
    if ( ! stateFileName().isNull() ) {
        saveStateToFile();
    }
}

void RenderWorld::slotRemoveObject( WObject* w ) {
    // removeObject is already thread-safe, no need to lock mutex here
    removeObject( w );
}

void RenderWorld::slotAddObject( WObject* w ) {
    // removeObject is already thread-safe, no need to lock mutex here
    addObject( w );
}

void RenderWorld::onWorldResize() {
    // Always take the resource mutex first and then our mutex to avoid deadlocks
    ResourcesLocker resourceLocker(this);
    QMutexLocker locker(&mutex);

    if (world() == NULL) {
        return;
    }

    MyCameraConstraint* tmp = dynamic_cast<MyCameraConstraint*>( camera()->frame()->constraint() );
    if ( tmp != NULL ) {
        wVector min, max;
        world()->size( min, max );
        setSceneBoundingBox( qglviewer::Vec( min[0], min[1], min[2] ),
                             qglviewer::Vec( max[0], max[1], max[2] ) );
        tmp->minP = qglviewer::Vec( min[0], min[1], min[2] );
        tmp->maxP = qglviewer::Vec( max[0], max[1], max[2] );
    }
}

void RenderWorld::wireframe( bool b ) {
    QMutexLocker locker(&mutex);
    wiref = b;
}

void RenderWorld::showSkyGround( bool b ) {
    QMutexLocker locker(&mutex);
    showskygroundbox = b;
}

void RenderWorld::showObjects( bool b ) {
    QMutexLocker locker(&mutex);
    showobjs = b;
}

void RenderWorld::showJoints( bool b ) {
    QMutexLocker locker(&mutex);
    showjoints = b;
}

void RenderWorld::showAABBs( bool b ) {
    QMutexLocker locker(&mutex);
    showaabbs = b;
}

void RenderWorld::showContacts( bool b ) {
    QMutexLocker locker(&mutex);
    showcontacts = b;
}

void RenderWorld::showForces( bool b ) {
    QMutexLocker locker(&mutex);
    showforces = b;
}

void RenderWorld::contextMenu( const QPoint& pos ) {
    QList<QAction*> acts;
    QAction* act;
    if ( showskygroundbox ) {
        act = new QAction( "Hide Sky-Ground", this );
    } else {
        act = new QAction( "Show Sky-Ground", this );
    }
    act->setCheckable( true );
    act->setChecked( showskygroundbox );
    connect( act, SIGNAL( toggled(bool) ),
             this, SLOT( showSkyGround(bool) ) );
    acts.append( act );
    if ( wiref ) {
        act = new QAction( "Hide Wireframe", this );
    } else {
        act = new QAction( "Show Wireframe", this );
    }
    act->setCheckable( true );
    act->setChecked( wiref );
    connect( act, SIGNAL( toggled(bool) ),
             this, SLOT( wireframe(bool) ) );
    acts.append( act );

    if ( showobjs ) {
        act = new QAction( "Hide Objects", this );
    } else {
        act = new QAction( "Show Objects", this );
    }
    act->setCheckable( true );
    act->setChecked( showobjs );
    connect( act, SIGNAL( toggled(bool) ),
             this, SLOT( showObjects(bool) ) );
    acts.append( act );

    if ( showjoints ) {
        act = new QAction( "Hide Joints", this );
    } else {
        act = new QAction( "Show Joints", this );
    }
    act->setCheckable( true );
    act->setChecked( showjoints );
    connect( act, SIGNAL( toggled(bool) ),
             this, SLOT( showJoints(bool) ) );
    acts.append( act );

    if ( showaabbs ) {
        act = new QAction( "Hide AABBs", this );
    } else {
        act = new QAction( "Show AABBs", this );
    }
    act->setCheckable( true );
    act->setChecked( showaabbs );
    connect( act, SIGNAL( toggled(bool) ),
             this, SLOT( showAABBs(bool) ) );
    acts.append( act );

    if ( showcontacts ) {
        act = new QAction( "Hide Contacts", this );
    } else {
        act = new QAction( "Show Contacts", this );
    }
    act->setCheckable( true );
    act->setChecked( showcontacts );
    connect( act, SIGNAL( toggled(bool) ),
             this, SLOT( showContacts(bool) ) );
    acts.append( act );

    if ( showforces ) {
        act = new QAction( "Hide Forces", this );
    } else {
        act = new QAction( "Show Forces", this );
    }
    act->setCheckable( true );
    act->setChecked( showforces );
    connect( act, SIGNAL( toggled(bool) ),
             this, SLOT( showForces(bool) ) );
    acts.append( act );

    QMenu::exec( acts, pos );
}

void RenderWorld::init() {
    // Always take the resource mutex first and then our mutex to avoid deadlocks
    ResourcesLocker resourceLocker(this);
    QMutexLocker locker(&mutex);

    if (world() == NULL) {
        return;
    }

    if ( ! stateFileName().isNull() ) {
        if ( !restoreStateFromFile() ) {
        }
    }
    wVector min, max;
    world()->size( min, max );
    camera()->frame()->setConstraint( new MyCameraConstraint( qglviewer::Vec( min[0], min[1], min[2] ), qglviewer::Vec( max[0], max[1], max[2] ) ) );
    // Light0 is the default ambient light
    glEnable(GL_LIGHT0);
}

void RenderWorld::draw() {
    // Always take the resource mutex first and then our mutex to avoid deadlocks
    ResourcesLocker resourceLocker(this);
    QMutexLocker locker(&mutex);

    if (world() == NULL) {
        return;
    }

    if ( showskygroundbox ) {
        glPushAttrib( GL_ALL_ATTRIB_BITS );
        glPushMatrix();
        drawSkyGroundBox( (QGLContext*)( QGLContext::currentContext() ) );
        glPopMatrix();
        glPopAttrib();
    }

    //--- the light follow the movement of the camera
    float pos[4] = {1.0, 0.5, 1.0, 0.0};
    // Directionnal light
    camera()->frame()->getPosition(pos[0], pos[1], pos[2]); 
    glLightfv( GL_LIGHT0, GL_POSITION, pos );

    //--- Display Contacts - Never in wireframe
    if ( showcontacts ) {
        glPushAttrib( GL_ALL_ATTRIB_BITS );
        //--- wireframe is ignored when drawing Contacts and Joints
        glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
        glDisable( GL_LIGHTING );
        // --- draws Contacts
        contactMapIterator itera( world()->contacts() );
        while( itera.hasNext() ) {
            itera.next();
            const contactVec& vec = itera.value();
            for( int i=0; i<vec.size(); i++ ) {
                drawSphere( vec[i].worldPos, 0.008f );
            }
        }
        glPopAttrib();
    }
    //--- Display Joint and Kinematic chains
    int alpha = 255;
    if ( showjoints ) {
        glPushAttrib( GL_ALL_ATTRIB_BITS );
        //--- wireframe is ignored when drawing Contacts and Joints
        glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
        glDisable( GL_LIGHTING );
        drawKineChains();
        alpha = 130;
        glPopAttrib();
    }
    //--- display objects
    if ( showobjs ) {
        glPushAttrib( GL_ALL_ATTRIB_BITS );
        //--- wireframe only from this point to end
        if ( wiref ) {
            glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
        } else {
            glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
        }
        glEnable( GL_LIGHTING );
        PhyObject* pr;
        for( int i=0; i<graphics().size(); i++ ) {
            RenderWObject* r = graphics()[i];
            if ( r->object()->isInvisible() ) continue;
            if ( showforces && ( pr=dynamic_cast<PhyObject*>(r->object()) ) ) {
                glPushAttrib( GL_ALL_ATTRIB_BITS );
                glColor4f( 1.0, 0.0, 0.0, 1.0 );
                drawArrow( pr->matrix().w_pos, pr->matrix().w_pos + pr->force(), 0.1f );
                glColor4f( 0.0, 1.0, 0.0, 1.0 );
                drawArrow( pr->matrix().w_pos, pr->matrix().w_pos + pr->torque(), 0.1f );
                alpha = 130;
                glPopAttrib();
            }
            r->object()->setAlpha( alpha );
            glPushAttrib( GL_ALL_ATTRIB_BITS );
            r->render( (QGLContext*)( QGLContext::currentContext() ) );
            glPopAttrib();
            if ( showaabbs && currentSelected == i ) {
                glPushAttrib( GL_ALL_ATTRIB_BITS );
                r->renderAABB( this );
                glPopAttrib();
                wVector dims, minPoint, maxPoint;
                r->calculateOBB( dims, minPoint, maxPoint );
                glPushAttrib( GL_ALL_ATTRIB_BITS );
                drawWireBox( minPoint, maxPoint, r->object()->matrix() );
                glPopAttrib();
            }
        }
    }

    //--- draw some text
    glPushAttrib( GL_ALL_ATTRIB_BITS );
    glColor4f( 1.0, 0.0, 0.0, 1.0 );
    drawText( 80, 15, QString("time: %1  step: %2")
        .arg(world()->elapsedTime())
        .arg((int)(world()->elapsedTime()/world()->timeStep()))
    );
    glPopAttrib();
}

void RenderWorld::drawWithNames() {
    QMutexLocker locker(&mutex);

    for( int i=0; i< graphics().size(); i++ ) {
        glPushName(i);
        graphics()[i]->render( (QGLContext*)( QGLContext::currentContext() ) );
        glPopName();
    }
}

void RenderWorld::postSelection(const QPoint& point) {
    QMutexLocker locker(&mutex);

    UNUSED_PARAM( point );
    int i = selectedName();
    if ( i != -1 && i != currentSelected ) {
        graphics()[qMax(0,currentSelected)]->object()->setColor( Qt::white );
        graphics()[i]->object()->setColor( Qt::yellow );
        currentSelected = i;
    }
}

void RenderWorld::keyPressEvent(QKeyEvent *e) {
    QMutexLocker locker(&mutex);

    // Get event modifiers key
#if QT_VERSION < 0x040000
    // Bug in Qt : use 0x0f00 instead of Qt::KeyButtonMask with Qt versions < 3.1
    const Qt::ButtonState modifiers = (Qt::ButtonState)(e->state() & Qt::KeyButtonMask);
#else
    const Qt::KeyboardModifiers modifiers = e->modifiers();
#endif

    // A simple switch on e->key() is not sufficient if we want to take state key into account.
    // With a switch, it would have been impossible to separate 'F' from 'CTRL+F'.
    // That's why we use imbricated if...else and a "handled" boolean.
    bool handled = false;
    if ((e->key()==Qt::Key_Left) && (modifiers==Qt::NoButton)) {
        // rotate camera
        Quaternion qcur = camera()->orientation();
        Quaternion qnew = qcur * Quaternion( Vec(0,1,0), 3.1415/30 );
        camera()->setOrientation( qnew );
        handled = true;
        updateGL();
    } else if ((e->key()==Qt::Key_Right) && (modifiers==Qt::NoButton)) {
        // rotate camera
        Quaternion qcur = camera()->orientation();
        Quaternion qnew = qcur * Quaternion( Vec(0,1,0), -3.1415/30 );
        camera()->setOrientation( qnew );
        handled = true;
        updateGL();
    }
    if ((e->key()==Qt::Key_Up) && (modifiers==Qt::NoButton)) {
        // rotate camera
        Quaternion qcur = camera()->orientation();
        Quaternion qnew = qcur * Quaternion( Vec(1,0,0), 3.1415/30 );
        camera()->setOrientation( qnew );
        handled = true;
        updateGL();
    } else if ((e->key()==Qt::Key_Down) && (modifiers==Qt::NoButton)) {
        // rotate camera
        Quaternion qcur = camera()->orientation();
        Quaternion qnew = qcur * Quaternion( Vec(1,0,0), -3.1415/30 );
        camera()->setOrientation( qnew );
        handled = true;
        updateGL();
    }
    //--- Context Menu (not accessible with right-click, because right button as different meaning)
    if ((e->key()==Qt::Key_M) && (modifiers==Qt::NoButton)) {
        contextMenu( mapToGlobal(QPoint(10,10)) );
        handled = true;
    }

    if (!handled) {
        QGLViewer::keyPressEvent(e);
    }
}

void RenderWorld::resourceChanged(QString name, ResourceChangeType changeType)
{
    QMutexLocker locker(&mutex);

    // Calling parent function
    RenderWObjectContainer::resourceChanged(name, changeType);

    if (world() == NULL) {
        return;
    }

    // Connecting slots and updating world dimensions

    // These connections are direct because syncronization is done with Mutexes
    connect( world(), SIGNAL( removedObject( WObject* ) ),
             this, SLOT( slotRemoveObject( WObject* ) ), Qt::DirectConnection );
    connect( world(), SIGNAL( addedObject( WObject* ) ),
             this, SLOT( slotAddObject( WObject* ) ), Qt::DirectConnection );
    connect( world(), SIGNAL( resized() ),
             this, SLOT( onWorldResize() ), Qt::DirectConnection );

    connect( world(), SIGNAL( advanced() ),
             this, SLOT( update() ) );

    // We can't call onWorldResize because that function tries to take a lock (and we can't from inside resourceChanged)
    MyCameraConstraint* tmp = dynamic_cast<MyCameraConstraint*>( camera()->frame()->constraint() );
    if ( tmp != NULL ) {
        wVector min, max;
        world()->size( min, max );
        setSceneBoundingBox( qglviewer::Vec( min[0], min[1], min[2] ),
                             qglviewer::Vec( max[0], max[1], max[2] ) );
        tmp->minP = qglviewer::Vec( min[0], min[1], min[2] );
        tmp->maxP = qglviewer::Vec( max[0], max[1], max[2] );
    }

    // Forcing renderworld update (we cannot call update() because here we could be in a different thread)
    QCoreApplication::postEvent(this, new ForceRenderWorldUpdateEvent());
}

void RenderWorld::drawDOF( PhyDOF* dof, bool drawAxes ) {
    // Used inside draw(), no need to lock here

    if ( dof->joint() == NULL ) return;

    //--- FIX ME: this method handles only rotational joints :-(
    if ( dof->translate() ) return;

    wMatrix mat;
    mat.x_ax = dof->xAxis();
    mat.y_ax = dof->yAxis();
    mat.z_ax = dof->axis();
    mat.w_pos = dof->centre();
    mat.sanitifize();

    PhyJoint* joint = dof->joint();
    PhyObject* child = joint->child();
    PhyObject* parent= joint->parent();
    RenderWorld& rw = *this;
    wVector dims, minP, maxP;
//  wVector lax = dof->axis();
    rw[ child ]->calculateOBB( dims, minP, maxP );
    real cube = (dims[0]+dims[1]+dims[2])/3.0;
    if ( parent ) {
        rw[ parent ]->calculateOBB( dims, minP, maxP );
        real cube2 = (dims[0]+dims[1]+dims[2])/3.0;
        cube = min( cube2, cube );
    }
    real len = cube * 0.70;
    real rad = len * 0.25;

    mat.w_pos = dof->centre() - dof->axis().scale(len/2.0);
    drawCylinder( mat, len, rad, QColor(Qt::cyan) );
    if ( dof->isLimited() ) {
        //--- draw indication about limits
        real lo, hi;
        dof->limits( lo, hi );
        real ang = hi-lo;
        mat.w_pos = wVector(0,0,0);
        mat = mat * wMatrix( wQuaternion( dof->axis(), lo ), wVector(0,0,0) );
        mat.w_pos = dof->centre() + dof->axis().scale(len/2.0);
        drawTorus( rad, rad*0.6, mat, ang );
    } else {
        mat.w_pos = dof->centre() + dof->axis().scale(len/2.0);
        drawTorus( rad, rad*0.6, mat );
    }
    //--- draw indication about current position
    mat.x_ax = dof->xAxis();
    mat.y_ax = dof->yAxis();
    mat.z_ax = dof->axis();
    mat.w_pos = wVector(0,0,0);
    mat.sanitifize();
    mat = mat * wMatrix( wQuaternion( dof->axis(), dof->position()-0.05 ), wVector(0,0,0) );
    mat.w_pos = dof->centre() + dof->axis().scale(len/2.0);
    drawTorus( rad, rad*0.55f, mat, 0.1f, Qt::green );

    if ( drawAxes ) {
        drawArrow( dof->centre(), dof->centre()+dof->xAxis().scale(len*1.2), rad*0.4, 12, Qt::magenta );
        drawArrow( dof->centre(), dof->centre()+dof->yAxis().scale(len*1.2), rad*0.5, 12, Qt::yellow );
    }
}

void RenderWorld::drawKineChains() {
    // Used inside draw(), no need to lock here

    if (world() == NULL) {
        return;
    }

    foreach( PhyJoint* jn, world()->joints()) {
        //if ( ! jn->isEnabled() ) continue;
        float dist1 = 0.0f, dist2 = 0.0f;
        wVector start = jn->centre();
        wVector end1 = jn->child()->matrix().w_pos;
        wVector end2 = start;
        dist1 = wVector::distance( start, end1 );
        if ( jn->parent() ) {
            end2 = jn->parent()->matrix().w_pos;
            dist2 = wVector::distance( start, end2 );
        }
        real rad = (dist1 + dist2) * 0.04;
        drawCylinder( start, end1, rad*0.6 );
        drawCylinder( start, end2, rad*0.6 );

        QVector<PhyDOF*> ds = jn->dofs();
        for( int k=0; k<ds.size(); k++ ) {
            drawDOF( ds[k], true );
        }
    }
}

void RenderWorld::customEvent(QEvent* event)
{
    if (event->type() == QEvent::User) {
        // Forcing RenderWorld update
        update();
    }
}

void RenderWObjectContainer::drawSphere( wVector pos, real radius ) {
    GLUquadricObj *pObj;
    
    // set the color
    glShadeModel( GL_SMOOTH );
    glColor4f( 0.0f, 1.0f, 0.0f, 1.0f );
    
    wMatrix mat = wMatrix::identity();
    mat.w_pos = pos;
    mat.x_ax = mat.x_ax.scale( radius );
    mat.y_ax = mat.y_ax.scale( radius );
    mat.z_ax = mat.z_ax.scale( radius );
    glPushMatrix();
    GLMultMatrix(&mat[0][0]);
    
    // Get a new Quadric off the stack
    pObj = gluNewQuadric();
    // Get a new Quadric off the stack
    gluQuadricTexture(pObj, true);
    gluSphere(pObj, 1.0f, 20, 20);
    
    gluDeleteQuadric(pObj);
    glPopMatrix();
}

void RenderWObjectContainer::drawCylinder( wVector axis, wVector centre, float h, float rad, QColor c ) {
    GLUquadricObj *pObj;
    
    glPushMatrix();
    
    // set the color
    glShadeModel( GL_SMOOTH );
    glColor4f( c.redF(), c.greenF(), c.blueF(), c.alphaF() );
    
    wVector recentre = axis.scale( -h*0.5 ) + centre;
    glTranslatef( recentre[0], recentre[1], recentre[2] );
    
    Vec xg(0,0,1);
    Vec ax( axis[0], axis[1], axis[2] );
    Quaternion quad( xg, ax );
    glMultMatrixd( quad.matrix() );
        
    // Get a new Quadric off the stack
    pObj = gluNewQuadric();             
    gluQuadricTexture(pObj, true);                      
    gluCylinder(pObj, rad, rad, h, 20, 2);
    
    // render the caps
    gluQuadricOrientation(pObj, GLU_INSIDE);
    gluDisk(pObj, 0.0f, rad, 20, 1);
    
    glTranslatef (0.0f, 0.0f, h);
    gluQuadricOrientation(pObj, GLU_OUTSIDE);
    gluDisk(pObj, 0.0f, rad, 20, 1);

    gluDeleteQuadric(pObj);
    glPopMatrix();
}

void RenderWObjectContainer::drawCylinder( wVector start, wVector end, float rad, QColor c ) {
    float h = wVector::distance( start, end );
    if ( h < 0.0001 ) return;

    GLUquadricObj *pObj;
    
    glPushMatrix();
    
    // set the color
    glShadeModel( GL_SMOOTH );
    glColor4f( c.redF(), c.greenF(), c.blueF(), c.alphaF() );
    
    wVector zaxis = end - start;
    zaxis.normalize();
    wMatrix tm = wMatrix::grammSchmidt( zaxis );
    tm.w_pos = start;
    GLMultMatrix( &tm[0][0] );
        
    // Get a new Quadric off the stack
    pObj = gluNewQuadric();             
    gluQuadricTexture(pObj, true);                      
    gluCylinder(pObj, rad, rad, h, 20, 2);
    
    // render the caps
    gluQuadricOrientation(pObj, GLU_INSIDE);
    gluDisk(pObj, 0.0f, rad, 20, 1);
    
    glTranslatef (0.0f, 0.0f, h);
    gluQuadricOrientation(pObj, GLU_OUTSIDE);
    gluDisk(pObj, 0.0f, rad, 20, 1);
    
    gluDeleteQuadric(pObj); 
    glPopMatrix();
}

void RenderWObjectContainer::drawCylinder( const wMatrix& mat, float h, float rad, QColor c ) {
    GLUquadricObj *pObj;
    
    glPushMatrix();
    
    // set the color
    glShadeModel( GL_SMOOTH );
    glColor4f( c.redF(), c.greenF(), c.blueF(), c.alphaF() );
    
    GLMultMatrix(&mat[0][0]);
        
    // Get a new Quadric off the stack
    pObj = gluNewQuadric();             
    gluQuadricTexture(pObj, true);                      
    gluCylinder(pObj, rad, rad, h, 20, 2);
    
    // render the caps
    gluQuadricOrientation(pObj, GLU_INSIDE);
    gluDisk(pObj, 0.0f, rad, 20, 1);
    
    glTranslatef (0.0f, 0.0f, h);
    gluQuadricOrientation(pObj, GLU_OUTSIDE);
    gluDisk(pObj, 0.0f, rad, 20, 1);
    
    gluDeleteQuadric(pObj); 
    glPopMatrix();
}

void RenderWObjectContainer::drawWireBox( wVector dims, wMatrix matrix ) {
    glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
    glPushMatrix();
    
    // set the color
    glShadeModel( GL_SMOOTH );
    glColor4f( 1.0f, 1.0f, 0.0f, 1.0f );
    
    float hdx = (dims[0]/2.0);
    float hdy = (dims[1]/2.0);
    float hdz = (dims[2]/2.0);
    GLMultMatrix(&matrix[0][0]);
    
    // the cube will just be drawn as six quads for the sake of simplicity
    // for each face, we specify the quad's normal (for lighting), then
    // specify the quad's 4 vertices and associated texture coordinates
    glBegin(GL_QUADS);
    // front
    glNormal3f(0.0, 0.0, 1.0);
    glVertex3f(-hdx, -hdy, hdz);
    glVertex3f( hdx, -hdy, hdz);
    glVertex3f( hdx,  hdy, hdz);
    glVertex3f(-hdx,  hdy, hdz);
    
    // back
    glNormal3f(0.0, 0.0, -1.0);
    glVertex3f( hdx, -hdy, -hdz);
    glVertex3f(-hdx, -hdy, -hdz);
    glVertex3f(-hdx,  hdy, -hdz);
    glVertex3f( hdx,  hdy, -hdz);
    
    // top
    glNormal3f(0.0, 1.0, 0.0);
    glVertex3f(-hdx,  hdy,  hdz);
    glVertex3f( hdx,  hdy,  hdz);
    glVertex3f( hdx,  hdy, -hdz);
    glVertex3f(-hdx,  hdy, -hdz);
    
    // bottom
    glNormal3f(0.0, -1.0, 0.0);
    glVertex3f(-hdx, -hdy, -hdz);
    glVertex3f( hdx, -hdy, -hdz);
    glVertex3f( hdx, -hdy,  hdz);
    glVertex3f(-hdx, -hdy,  hdz);
    
    // left
    glNormal3f(-1.0, 0.0, 0.0);
    glVertex3f(-hdx, -hdy, -hdz);
    glVertex3f(-hdx, -hdy,  hdz);
    glVertex3f(-hdx,  hdy,  hdz);
    glVertex3f(-hdx,  hdy, -hdz);
    
    // right
    glNormal3f(1.0, 0.0, 0.0);
    glVertex3f(hdx,  -hdy,  hdz);
    glVertex3f(hdx,  -hdy, -hdz);
    glVertex3f(hdx,   hdy, -hdz);
    glVertex3f(hdx,   hdy,  hdz);
    glEnd();
    
    glPopMatrix();
}

void RenderWObjectContainer::drawWireBox( wVector min, wVector max, const wMatrix& matrix ) {
    glPushMatrix();
    GLMultMatrix(&matrix[0][0]);
    drawWireBox( min, max );
    glPopMatrix();
}

void RenderWObjectContainer::drawWireBox( wVector min, wVector max ) {
    glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
    glPushMatrix();
    
    // set the color
    glShadeModel( GL_SMOOTH );
    glColor4f( 1.0f, 1.0f, 0.0f, 1.0f );
    
    float dx = fabs( max[0]-min[0] );
    float dy = fabs( max[1]-min[1] );
    float dz = fabs( max[2]-min[2] );
    float hdx = (dx/2.0);
    float hdy = (dy/2.0);
    float hdz = (dz/2.0);
    float minX = qMin(min[0], max[0]);
    float minY = qMin(min[1], max[1]);
    float minZ = qMin(min[2], max[2]);
    glTranslatef( minX+hdx, minY+hdy, minZ+hdz );
    
    // the cube will just be drawn as six quads for the sake of simplicity
    // for each face, we specify the quad's normal (for lighting), then
    // specify the quad's 4 vertices and associated texture coordinates
    glBegin(GL_QUADS);
    // front
    glNormal3f(0.0, 0.0, 1.0);
    glVertex3f(-hdx, -hdy, hdz);
    glVertex3f( hdx, -hdy, hdz);
    glVertex3f( hdx,  hdy, hdz);
    glVertex3f(-hdx,  hdy, hdz);
    
    // back
    glNormal3f(0.0, 0.0, -1.0);
    glVertex3f( hdx, -hdy, -hdz);
    glVertex3f(-hdx, -hdy, -hdz);
    glVertex3f(-hdx,  hdy, -hdz);
    glVertex3f( hdx,  hdy, -hdz);
    
    // top
    glNormal3f(0.0, 1.0, 0.0);
    glVertex3f(-hdx,  hdy,  hdz);
    glVertex3f( hdx,  hdy,  hdz);
    glVertex3f( hdx,  hdy, -hdz);
    glVertex3f(-hdx,  hdy, -hdz);
    
    // bottom
    glNormal3f(0.0, -1.0, 0.0);
    glVertex3f(-hdx, -hdy, -hdz);
    glVertex3f( hdx, -hdy, -hdz);
    glVertex3f( hdx, -hdy,  hdz);
    glVertex3f(-hdx, -hdy,  hdz);
    
    // left
    glNormal3f(-1.0, 0.0, 0.0);
    glVertex3f(-hdx, -hdy, -hdz);
    glVertex3f(-hdx, -hdy,  hdz);
    glVertex3f(-hdx,  hdy,  hdz);
    glVertex3f(-hdx,  hdy, -hdz);
    
    // right
    glNormal3f(1.0, 0.0, 0.0);
    glVertex3f(hdx,  -hdy,  hdz);
    glVertex3f(hdx,  -hdy, -hdz);
    glVertex3f(hdx,   hdy, -hdz);
    glVertex3f(hdx,   hdy,  hdz);
    glEnd();
    
    glPopMatrix();
}

void RenderWObjectContainer::drawTorus( real outRad, real innRad, const wMatrix& matrix, real angle, QColor c ) {
    glPushMatrix();
    
    // set the color
    glShadeModel( GL_SMOOTH );
    glColor4f( c.redF(), c.greenF(), c.blueF(), c.alphaF() );

    GLMultMatrix(&matrix[0][0]);

    int numc = 8;
    int numt = 25;
    int i, j, k;
    double s, t, x, y, z, twopi;
    real tubeRad = (outRad-innRad)/2.0;
    real toruRad = outRad - tubeRad;
    twopi = 2 * PI_GRECO;
    for (i = 0; i < numc; i++) {
        glBegin(GL_QUAD_STRIP);
        for (j = 0; j <= numt; j++) {
            for (k = 1; k >= 0; k--) {
                s = (i + k) % numc + 0.5;
                t = j; //% numt;

                x = (toruRad+tubeRad*cos(s*twopi/numc))*cos(t*(angle)/numt);
                y = (toruRad+tubeRad*cos(s*twopi/numc))*sin(t*(angle)/numt);
                z = tubeRad * sin(s * twopi / numc);
                glVertex3f(x, y, z);
            }
        }
        glEnd();
    }

    glPopMatrix();
}

void RenderWObjectContainer::drawTorus( wVector axis, wVector centre, real outRad, real innRad, real angle ) {
    glPushMatrix();
    
    // set the color
    glShadeModel( GL_SMOOTH );
    glColor4f( 1.0f, 0.0f, 0.0f, 1.0f );

    wMatrix tm = wMatrix::grammSchmidt( axis );
    tm.w_pos = centre;
    GLMultMatrix( &tm[0][0] );

    int numc = 8;
    int numt = 25;
    int i, j, k;
    double s, t, x, y, z, twopi;
    real tubeRad = (outRad-innRad)/2.0;
    real toruRad = outRad - tubeRad;
    twopi = 2 * PI_GRECO;
    for (i = 0; i < numc; i++) {
        glBegin(GL_QUAD_STRIP);
        for (j = 0; j <= numt; j++) {
            for (k = 1; k >= 0; k--) {
                s = (i + k) % numc + 0.5;
                t = j; //% numt;

                x = (toruRad+tubeRad*cos(s*twopi/numc))*cos(t*(angle)/numt);
                y = (toruRad+tubeRad*cos(s*twopi/numc))*sin(t*(angle)/numt);
                z = tubeRad * sin(s * twopi / numc);
                glVertex3f(x, y, z);
            }
        }
        glEnd();
    }

    glPopMatrix();
}

void RenderWorld::drawArrow( const wVector& start, const wVector& end, float radius, int nbSubdivisions, QColor c ) {
    glPushMatrix();
    glColor4f( c.redF(), c.greenF(), c.blueF(), c.alphaF() );

    wVector zaxis = end - start;
    real len = zaxis.norm();
    zaxis.normalize();
    wMatrix tm = wMatrix::grammSchmidt( zaxis );
    tm.w_pos = start;
    GLMultMatrix( &tm[0][0] );

    QGLViewer::drawArrow( len, radius, nbSubdivisions );

    glPopMatrix();
}

} // end namespace farsa