worldsim/src/renderwobjecthierarchy.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 "physphere.h"
#include "phybox.h"
#include "phycylinder.h"
#include "phycone.h"
#include "phyicub.h"
#include "wcamera.h"
#include "phycompoundobject.h"
#include "wmesh.h"
#include "graphicalwobject.h"
#include "private/renderwobjecthierarchy.h"

#include <QImage>
#include <QColor>
#include <QList>
#include <QVector>
#include <cmath>

// 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 glMultMatrixd

namespace farsa {

class RenderGenericObject : public RenderWObject {
public:
    RenderGenericObject( WObject* wobj, RenderWObjectContainer* container ) : RenderWObject( wobj, container ) {
        rad = 0.02f;
    };
    virtual ~RenderGenericObject() { };
    virtual void render( QGLContext* /*gw*/ ) {
    };
    virtual void renderAABB( RenderWorld* gw ) {
        wVector minpoint, maxpoint;
        calculateAABB( minpoint, maxpoint, obj->matrix() );
        gw->drawWireBox( minpoint, maxpoint );
    };
    virtual void calculateAABB( wVector& minPoint, wVector& maxPoint, const wMatrix tm ) {
        wVector rds( rad, rad, rad );
        minPoint = tm.w_pos - rds;
        maxPoint = tm.w_pos + rds;
    };

    virtual void calculateOBB( wVector& dimension, wVector& minPoint, wVector& maxPoint ) {
        dimension[0] = rad*2.0;
        dimension[1] = rad*2.0;
        dimension[2] = rad*2.0;
        wVector rds = wVector( rad, rad, rad );
        minPoint = -rds;
        maxPoint = +rds;
    };
protected:
    real rad;
};

class RenderWMesh : public RenderWObject {
public:
    RenderWMesh( WObject* wobj, RenderWObjectContainer* container ) : RenderWObject( wobj, container ) {
        wmesh = (WMesh*)wobj;
    };
    virtual ~RenderWMesh() { };
    virtual void render( QGLContext* gw ) {
        glPushMatrix();
        wMatrix m = wmesh->matrix();
        if ( wmesh->attachedTo() ) {
            m = m * wmesh->attachedTo()->matrix();
        }
        GLMultMatrix(&m[0][0]);
        int m_numMeshes = wmesh->meshesCount();
        WMesh::Mesh *m_pMeshes = wmesh->meshes();
//      int m_numMaterials = wmesh->materialsCount();
        WMesh::Material *m_pMaterials = wmesh->materials();
//      int m_numTriangles = wmesh->trianglesCount();
        WMesh::Triangle *m_pTriangles = wmesh->triangles();
//      int m_numVertices = wmesh->verticesCount();
        WMesh::Vertex *m_pVertices = wmesh->vertices();
        for ( int i = 0; i < m_numMeshes; i++ ) {
            int materialIndex = m_pMeshes[i].m_materialIndex;
            if ( materialIndex >= 0 ) {
                glMaterialfv( GL_FRONT, GL_AMBIENT,   m_pMaterials[materialIndex].m_ambient );
                glMaterialfv( GL_FRONT, GL_DIFFUSE,   m_pMaterials[materialIndex].m_diffuse );
                glMaterialfv( GL_FRONT, GL_SPECULAR,  m_pMaterials[materialIndex].m_specular );
                glMaterialfv( GL_FRONT, GL_EMISSION,  m_pMaterials[materialIndex].m_emissive );
                glMaterialf(  GL_FRONT, GL_SHININESS, m_pMaterials[materialIndex].m_shininess );
                //container()->applyTexture( gw, wmesh->texture() );
                container()->setupColorTexture( gw, this );
            }
            glBegin( GL_TRIANGLES );
            for ( int j = 0; j < m_pMeshes[i].m_numTriangles; j++ ) {
                int triangleIndex = m_pMeshes[i].m_pTriangleIndices[j];
                const WMesh::Triangle* pTri = &m_pTriangles[triangleIndex];
                for ( int k = 0; k < 3; k++ ) {
                    int index = pTri->m_vertexIndices[k];
                    glNormal3fv( pTri->m_vertexNormals[k] );
                    glTexCoord2f( pTri->m_s[k], pTri->m_t[k] );
                    glVertex3fv( m_pVertices[index].m_location );
                }
            }
            glEnd();
        }
        glPopMatrix();
    };
    virtual void renderAABB( RenderWorld* /*gw*/ ) {
    };
    virtual void calculateAABB( wVector& /*minPoint*/, wVector& /*maxPoint*/, const wMatrix /*tm*/ ) {
    };
    virtual void calculateOBB( wVector& /*dimension*/, wVector& /*minPoint*/, wVector& /*maxPoint*/ ) {
    };
private:
    WMesh* wmesh;
};

class RenderPhyBox : public RenderWObject {
public:
    RenderPhyBox( WObject* wobj, RenderWObjectContainer* container ) : RenderWObject( wobj, container ) {
        box = (PhyBox*)wobj;
        dx = box->sideX();
        dy = box->sideY();
        dz = box->sideZ();
    };
    virtual ~RenderPhyBox() { };
    virtual void render( QGLContext* gw ) {
        glPushMatrix();
        container()->setupColorTexture( gw, this );
        const wMatrix& m = box->matrix();
        GLMultMatrix(&m[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);
        float hdx = (dx/2.0);
        float hdy = (dy/2.0);
        float hdz = (dz/2.0);
        // front
        glNormal3f(0.0, 0.0, 1.0);
        glTexCoord2f(0.0, 0.0); glVertex3f(-hdx, -hdy, hdz);
        glTexCoord2f(1.0, 0.0); glVertex3f( hdx, -hdy, hdz);
        glTexCoord2f(1.0, 1.0); glVertex3f( hdx,  hdy, hdz);
        glTexCoord2f(0.0, 1.0); glVertex3f(-hdx,  hdy, hdz);
        
        // back
        glNormal3f(0.0, 0.0, -1.0);
        glTexCoord2f(0.0, 0.0); glVertex3f( hdx, -hdy, -hdz);
        glTexCoord2f(1.0, 0.0); glVertex3f(-hdx, -hdy, -hdz);
        glTexCoord2f(1.0, 1.0); glVertex3f(-hdx,  hdy, -hdz);
        glTexCoord2f(0.0, 1.0); glVertex3f( hdx,  hdy, -hdz);
        
        // top
        glNormal3f(0.0, 1.0, 0.0);
        glTexCoord2f(0.0, 0.0); glVertex3f(-hdx,  hdy,  hdz);
        glTexCoord2f(1.0, 0.0); glVertex3f( hdx,  hdy,  hdz);
        glTexCoord2f(1.0, 1.0); glVertex3f( hdx,  hdy, -hdz);
        glTexCoord2f(0.0, 1.0); glVertex3f(-hdx,  hdy, -hdz);
        
        // bottom
        glNormal3f(0.0, -1.0, 0.0);
        glTexCoord2f(0.0, 0.0); glVertex3f(-hdx, -hdy, -hdz);
        glTexCoord2f(1.0, 0.0); glVertex3f( hdx, -hdy, -hdz);
        glTexCoord2f(1.0, 1.0); glVertex3f( hdx, -hdy,  hdz);
        glTexCoord2f(0.0, 1.0); glVertex3f(-hdx, -hdy,  hdz);
        
        // left
        glNormal3f(-1.0, 0.0, 0.0);
        glTexCoord2f(0.0, 0.0); glVertex3f(-hdx, -hdy, -hdz);
        glTexCoord2f(1.0, 0.0); glVertex3f(-hdx, -hdy,  hdz);
        glTexCoord2f(1.0, 1.0); glVertex3f(-hdx,  hdy,  hdz);
        glTexCoord2f(0.0, 1.0); glVertex3f(-hdx,  hdy, -hdz);
        
        // right
        glNormal3f(1.0, 0.0, 0.0);
        glTexCoord2f(0.0, 0.0); glVertex3f(hdx,  -hdy,  hdz);
        glTexCoord2f(1.0, 0.0); glVertex3f(hdx,  -hdy, -hdz);
        glTexCoord2f(1.0, 1.0); glVertex3f(hdx,   hdy, -hdz);
        glTexCoord2f(0.0, 1.0); glVertex3f(hdx,   hdy,  hdz);
        
        glEnd();
        glPopMatrix();
    };
    virtual void renderAABB( RenderWorld* gw ) {
        wVector minpoint, maxpoint;
        calculateAABB( minpoint, maxpoint, obj->matrix() );
        gw->drawWireBox( minpoint, maxpoint );
    };
    virtual void calculateAABB( wVector& minPoint, wVector& maxPoint, const wMatrix tm ) {
        real tdx = fabs(tm.x_ax[0]*dx) + fabs(tm.y_ax[0]*dy) + fabs(tm.z_ax[0]*dz);
        real tdy = fabs(tm.x_ax[1]*dx) + fabs(tm.y_ax[1]*dy) + fabs(tm.z_ax[1]*dz);
        real tdz = fabs(tm.x_ax[2]*dx) + fabs(tm.y_ax[2]*dy) + fabs(tm.z_ax[2]*dz);

        wVector hds( tdx/2.0, tdy/2.0, tdz/2.0 );
        minPoint = tm.w_pos - hds;
        maxPoint = tm.w_pos + hds;
    };

    virtual void calculateOBB( wVector& dimension, wVector& minPoint, wVector& maxPoint ) {
        dimension[0] = dx;
        dimension[1] = dy;
        dimension[2] = dz;
        wVector hds( dx/2.0, dy/2.0, dz/2.0 );
        minPoint = -hds;
        maxPoint = +hds;
    };

protected:
    PhyBox* box;
    real dx, dy, dz;
};

class RenderPhySphere : public RenderWObject {
public:
    RenderPhySphere( WObject* wobj, RenderWObjectContainer* container ) : RenderWObject( wobj, container ) {
        sph = (PhySphere*)wobj;
        rad = sph->radius();
    };
    virtual ~RenderPhySphere() { };
    virtual void render( QGLContext* gw ) {
        glPushMatrix();
        GLUquadricObj *pObj;
        container()->setupColorTexture( gw, this );
        wMatrix mat = sph->matrix();
        mat.x_ax = mat.x_ax.scale( rad );
        mat.y_ax = mat.y_ax.scale( rad );
        mat.z_ax = mat.z_ax.scale( rad );
        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();
    };
    virtual void renderAABB( RenderWorld* gw ) {
        wVector minpoint, maxpoint;
        calculateAABB( minpoint, maxpoint, obj->matrix() );
        gw->drawWireBox( minpoint, maxpoint );
    };
    virtual void calculateAABB( wVector& minPoint, wVector& maxPoint, const wMatrix tm ) {
        wVector rds( rad, rad, rad );
        minPoint = tm.w_pos - rds;
        maxPoint = tm.w_pos + rds;
    };

    virtual void calculateOBB( wVector& dimension, wVector& minPoint, wVector& maxPoint ) {
        dimension[0] = rad*2.0;
        dimension[1] = rad*2.0;
        dimension[2] = rad*2.0;
        wVector rds = wVector( rad, rad, rad );
        minPoint = -rds;
        maxPoint = +rds;
    };
protected:
    PhySphere* sph;
    real rad;
};

class RenderPhyCylinder : public RenderWObject {
public:
    RenderPhyCylinder( WObject* wobj, RenderWObjectContainer* container ) : RenderWObject( wobj, container ) {
        cil = (PhyCylinder*)wobj;
        rad = cil->radius();
        hei = cil->height();
    };
    virtual ~RenderPhyCylinder() { };
    virtual void render( QGLContext* gw ) {
        glPushMatrix();
        GLUquadricObj *pObj;
        container()->setupColorTexture( gw, this );
    
        // opengl cylinder are aligned alogn the z axis, we want it along the x axis,
        // we create a rotation matrix to do the alignment
        wMatrix matrix = wMatrix::yaw( PI_GRECO * 0.5f );
        matrix.w_pos = matrix.rotateVector( wVector(0.0, 0.0, -hei*0.5f) );
        matrix = matrix * cil->matrix();
        GLMultMatrix(&matrix[0][0]);
            
        // Get a new Quadric off the stack
        pObj = gluNewQuadric();
        gluQuadricTexture(pObj, true);
        gluCylinder(pObj, rad, rad, hei, 20, 2);
    
        // render the caps
        gluQuadricOrientation(pObj, GLU_INSIDE);
        gluDisk(pObj, 0.0f, rad, 20, 1);
    
        glTranslatef (0.0f, 0.0f, hei);
        gluQuadricOrientation(pObj, GLU_OUTSIDE);
        gluDisk(pObj, 0.0f, rad, 20, 1);
    
        gluDeleteQuadric(pObj);
        glPopMatrix();
    };
    virtual void renderAABB( RenderWorld* gw ) {
        wVector minpoint, maxpoint;
        calculateAABB( minpoint, maxpoint, obj->matrix() );
        gw->drawWireBox( minpoint, maxpoint );
    };
    virtual void calculateAABB( wVector& minPoint, wVector& maxPoint, const wMatrix tm ) {
        real h2 = hei/2.0;
        real tdx = fabs(tm.x_ax[0]*h2) + fabs(tm.y_ax[0]*rad) + fabs(tm.z_ax[0]*rad);
        real tdy = fabs(tm.x_ax[1]*h2) + fabs(tm.y_ax[1]*rad) + fabs(tm.z_ax[1]*rad);
        real tdz = fabs(tm.x_ax[2]*h2) + fabs(tm.y_ax[2]*rad) + fabs(tm.z_ax[2]*rad);
        wVector hds( tdx, tdy, tdz );
        minPoint = tm.w_pos - hds;
        maxPoint = tm.w_pos + hds;
    };

    virtual void calculateOBB( wVector& dimension, wVector& minPoint, wVector& maxPoint ) {
        dimension[0] = hei;
        dimension[1] = rad*2.0;
        dimension[2] = rad*2.0;
        wVector rds = wVector( hei/2.0, rad, rad );
        minPoint = -rds;
        maxPoint = +rds;
    };
protected:
    PhyCylinder* cil;
    real rad;
    real hei;
};

class RenderPhyCone : public RenderWObject {
public:
    RenderPhyCone( WObject* wobj, RenderWObjectContainer* container ) : RenderWObject( wobj, container ) {
        cone = (PhyCone*)wobj;
        rad = cone->radius();
        hei = cone->height();
    };
    virtual ~RenderPhyCone() { };
    virtual void render( QGLContext* gw ) {
        glPushMatrix();
        GLUquadricObj *pObj;
        container()->setupColorTexture( gw, this );
    
        // opengl cylinder are aligned alogn the z axis, we want it along the x axis,
        // we create a rotation matrix to do the alignment
        wMatrix matrix = wMatrix::yaw( PI_GRECO * 0.5 );
        matrix.w_pos = matrix.rotateVector( wVector(0.0, 0.0, -hei*0.5) );
        matrix = matrix * cone->matrix();
        GLMultMatrix(&matrix[0][0]);
            
        // Get a new Quadric off the stack
        pObj = gluNewQuadric();
        gluQuadricTexture(pObj, true);
        gluCylinder(pObj, rad, 0, hei, 20, 2);
    
        // render the caps
        gluQuadricOrientation(pObj, GLU_INSIDE);
        gluDisk(pObj, 0.0f, rad, 20, 1);
        
        gluDeleteQuadric(pObj);
        glPopMatrix();
    };
    virtual void renderAABB( RenderWorld* gw ) {
        wVector minpoint, maxpoint;
        calculateAABB( minpoint, maxpoint, obj->matrix() );
        gw->drawWireBox( minpoint, maxpoint );
    };
    virtual void calculateAABB( wVector& minPoint, wVector& maxPoint, const wMatrix tm ) {
        real h2 = hei/2.0;
        real tdx = fabs(tm.x_ax[0]*h2) + fabs(tm.y_ax[0]*rad) + fabs(tm.z_ax[0]*rad);
        real tdy = fabs(tm.x_ax[1]*h2) + fabs(tm.y_ax[1]*rad) + fabs(tm.z_ax[1]*rad);
        real tdz = fabs(tm.x_ax[2]*h2) + fabs(tm.y_ax[2]*rad) + fabs(tm.z_ax[2]*rad);
        wVector hds( tdx, tdy, tdz );
        minPoint = tm.w_pos - hds;
        maxPoint = tm.w_pos + hds;
    };

    virtual void calculateOBB( wVector& dimension, wVector& minPoint, wVector& maxPoint ) {
        dimension[0] = hei;
        dimension[1] = rad*2.0;
        dimension[2] = rad*2.0;
        wVector rds = wVector( hei/2.0, rad, rad );
        minPoint = -rds;
        maxPoint = +rds;
    };
protected:
    PhyCone* cone;
    real rad;
    real hei;
};

#ifdef FARSA_USE_YARP_AND_ICUB

RenderICub::RenderICub( WObject* wobj, RenderWObjectContainer* container ) :
    RenderWObject( wobj, container )
{
    icub = (PhyiCub*)wobj;
    icub->forceKinematicChainsUpdate(true);
    initRobjv(container);
}

RenderICub::~RenderICub()
{
    if (icub != NULL) {
        icub->forceKinematicChainsUpdate(false);
    }
}

void RenderICub::render( QGLContext* gw )
{
    // Nothing to do here, all components are rendered by their respective renderers
}

void RenderICub::renderAABB( RenderWorld* gw )
{
    wVector minpoint, maxpoint;
    calculateAABB( minpoint, maxpoint, obj->matrix() );
    gw->drawWireBox( minpoint, maxpoint );
}

void RenderICub::calculateAABB( wVector& minPoint, wVector& maxPoint, const wMatrix tm )
{
    UNUSED_PARAM( tm );
    QList<RenderWObject*> robjs = robjv.values();
    robjs[0]->calculateAABB( minPoint, maxPoint, robjs[0]->object()->matrix() );
    wVector minP, maxP;
    for( int i=1; i<robjs.size(); i++ ) {
        robjs[i]->calculateAABB( minP, maxP, robjs[i]->object()->matrix() );
        mergeAABB( minPoint, maxPoint, minP, maxP );
    }
}

void RenderICub::calculateOBB( wVector& dimension, wVector& minPoint, wVector& maxPoint )
{
    /* ***
    wVector minPoint, maxPoint;
    wMatrix tm = wMatrix::identity();
    for( int i=1; i<robjv.size(); i++ ) {
        robjv[i]->calculateAABB( minP, maxP, tm );
        mergeAABB( minPoint, maxPoint, minP, maxP );
    }
    *** */
    calculateAABB( minPoint, maxPoint, wMatrix::identity() );
    dimension[0] = fabs( maxPoint[0] - minPoint[0] );
    dimension[1] = fabs( maxPoint[1] - minPoint[1] );
    dimension[2] = fabs( maxPoint[2] - minPoint[2] );
}

void RenderICub::objectAlreadyDestroyed()
{
    icub = NULL;
}

void RenderICub::initRobjv( RenderWObjectContainer* container )
{
    robjv.clear();
    foreach( RenderWObject* wobj, container->graphics() ) {
        if ( wobj->object()->owner() == icub ) {
            robjv[wobj->object()] = wobj;
        }
    }
    foreach( WObject* obj, icub->leftLeg() ) {
        obj->setUseColorTextureOfOwner(true);
    }
    foreach( WObject* obj, icub->rightLeg() ) {
        obj->setUseColorTextureOfOwner(true);
    }
    foreach( WObject* obj, icub->torso() ) {
        obj->setUseColorTextureOfOwner(true);
    }
    foreach( WObject* obj, icub->leftArm() ) {
        obj->setUseColorTextureOfOwner(true);
    }
    foreach( WObject* obj, icub->rightArm() ) {
        obj->setUseColorTextureOfOwner(true);
    }
    int i = 0;
    foreach( WObject* obj, icub->headNeck() ) {
        //--- the eye will be displayed with the texture of the eye
        if ( ( i == 4 ) || ( i == 5 ) ) {
            obj->setUseColorTextureOfOwner(false);
        } else {
            obj->setUseColorTextureOfOwner(true);
        }
        i++;
    }
    foreach( WObject* obj, icub->covers() ) {
        obj->setUseColorTextureOfOwner(false);
    }
}

void RenderICub::mergeAABB( wVector& minPointA, wVector& maxPointA, const wVector& minPointB, const wVector& maxPointB )
{
    minPointA[0] = min( minPointA[0], minPointB[0] );
    minPointA[1] = min( minPointA[1], minPointB[1] );
    minPointA[2] = min( minPointA[2], minPointB[2] );
    maxPointA[0] = max( maxPointA[0], maxPointB[0] );
    maxPointA[1] = max( maxPointA[1], maxPointB[1] );
    maxPointA[2] = max( maxPointA[2], maxPointB[2] );
}

#endif

class RenderCompoundObject : public RenderWObject {
public:
    RenderCompoundObject( WObject* wobj, RenderWObjectContainer* container ) : RenderWObject( wobj, container ) {
        co = (PhyCompoundObject*)wobj;
        initRobjv( container );
    };
    virtual ~RenderCompoundObject() {
        foreach( RenderWObject* ro, robjv ) {
            delete ro;
        }
    };
    virtual void render( QGLContext* gw ) {
        glPushMatrix();
        container()->setupColorTexture( gw, this );
        wMatrix matrix = co->matrix();
        GLMultMatrix(&matrix[0][0]);
        foreach( RenderWObject* ro, robjv ) {
            ro->render( gw );
        }
        glPopMatrix();
    };
    virtual void renderAABB( RenderWorld* gw ) {
        wVector minpoint, maxpoint;
        calculateAABB( minpoint, maxpoint, obj->matrix() );
        gw->drawWireBox( minpoint, maxpoint );
    };
    virtual void calculateAABB( wVector& minPoint, wVector& maxPoint, const wMatrix tm ) {
        robjv[0]->calculateAABB( minPoint, maxPoint, robjv[0]->object()->matrix()*tm );
        wVector minP, maxP;
        for( int i=1; i<robjv.size(); i++ ) {
            robjv[i]->calculateAABB( minP, maxP, robjv[i]->object()->matrix()*tm );
            mergeAABB( minPoint, maxPoint, minP, maxP );
        }
    };
    virtual void calculateOBB( wVector& dimension, wVector& minPoint, wVector& maxPoint ) {
        //--- the right calculation is done calculating the AABB when
        //--- the transformation matrix is the identity
        wVector minP, maxP;
        if ( robjv.size() == 0 ) {
            qWarning( "== this point should never reached: renderwobjecthierarchy.cpp:435" );
            return;
        }
        robjv[0]->calculateAABB( minPoint, maxPoint, robjv[0]->object()->matrix() );
        for( int i=1; i<robjv.size(); i++ ) {
            robjv[i]->calculateAABB( minP, maxP, robjv[i]->object()->matrix() );
            mergeAABB( minPoint, maxPoint, minP, maxP );
        }
        dimension[0] = fabs( maxPoint[0] - minPoint[0] );
        dimension[1] = fabs( maxPoint[1] - minPoint[1] );
        dimension[2] = fabs( maxPoint[2] - minPoint[2] );
    };
protected:
    PhyCompoundObject* co;
    QVector<RenderWObject*> robjv;
private:
    void initRobjv( RenderWObjectContainer* container ) {
        foreach( WObject* obj, co->bodies() ) {
            robjv << RenderWObjectContainer::createRenderWObjectFor( obj, container );
        }
    };
    void mergeAABB( wVector& minPointA, wVector& maxPointA, const wVector& minPointB, const wVector& maxPointB ) {
        minPointA[0] = min( minPointA[0], minPointB[0] );
        minPointA[1] = min( minPointA[1], minPointB[1] );
        minPointA[2] = min( minPointA[2], minPointB[2] );
        maxPointA[0] = max( maxPointA[0], maxPointB[0] );
        maxPointA[1] = max( maxPointA[1], maxPointB[1] );
        maxPointA[2] = max( maxPointA[2], maxPointB[2] );
    };
};

#ifdef FARSA_USE_YARP_AND_ICUB

class RenderWCamera : public RenderWObject {
public:
    RenderWCamera( WObject* wobj, RenderWObjectContainer* container ) : RenderWObject( wobj, container ) {
        wcam = (WCamera*)(wobj);
        dx = 0.015f;
        dy = 0.01f;
        dz = 0.025f;
    };
    virtual ~RenderWCamera() { };
    virtual void render( QGLContext* gw ) {
        if ( wcam->attachedTo() != NULL ) return;
        glPushMatrix();
        container()->setupColorTexture( gw, this );
        const wMatrix& m = wcam->matrix();
        GLMultMatrix(&m[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);
        float hdx = (dx/2.0);
        float hdy = (dy/2.0);
        float hdz = (dz/2.0);
        // front
        glNormal3f(0.0, 0.0, 1.0);
        glTexCoord2f(0.0, 0.0); glVertex3f(-hdx, -hdy, hdz);
        glTexCoord2f(1.0, 0.0); glVertex3f( hdx, -hdy, hdz);
        glTexCoord2f(1.0, 1.0); glVertex3f( hdx,  hdy, hdz);
        glTexCoord2f(0.0, 1.0); glVertex3f(-hdx,  hdy, hdz);
        // back
        glNormal3f(0.0, 0.0, -1.0);
        glTexCoord2f(0.0, 0.0); glVertex3f( hdx, -hdy, -hdz);
        glTexCoord2f(1.0, 0.0); glVertex3f(-hdx, -hdy, -hdz);
        glTexCoord2f(1.0, 1.0); glVertex3f(-hdx,  hdy, -hdz);
        glTexCoord2f(0.0, 1.0); glVertex3f( hdx,  hdy, -hdz);
        // top
        glNormal3f(0.0, 1.0, 0.0);
        glTexCoord2f(0.0, 0.0); glVertex3f(-hdx,  hdy,  hdz);
        glTexCoord2f(1.0, 0.0); glVertex3f( hdx,  hdy,  hdz);
        glTexCoord2f(1.0, 1.0); glVertex3f( hdx,  hdy, -hdz);
        glTexCoord2f(0.0, 1.0); glVertex3f(-hdx,  hdy, -hdz);
        // bottom
        glNormal3f(0.0, -1.0, 0.0);
        glTexCoord2f(0.0, 0.0); glVertex3f(-hdx, -hdy, -hdz);
        glTexCoord2f(1.0, 0.0); glVertex3f( hdx, -hdy, -hdz);
        glTexCoord2f(1.0, 1.0); glVertex3f( hdx, -hdy,  hdz);
        glTexCoord2f(0.0, 1.0); glVertex3f(-hdx, -hdy,  hdz);
        // left
        glNormal3f(-1.0, 0.0, 0.0);
        glTexCoord2f(0.0, 0.0); glVertex3f(-hdx, -hdy, -hdz);
        glTexCoord2f(1.0, 0.0); glVertex3f(-hdx, -hdy,  hdz);
        glTexCoord2f(1.0, 1.0); glVertex3f(-hdx,  hdy,  hdz);
        glTexCoord2f(0.0, 1.0); glVertex3f(-hdx,  hdy, -hdz);
        // right
        glNormal3f(1.0, 0.0, 0.0);
        glTexCoord2f(0.0, 0.0); glVertex3f(hdx,  -hdy,  hdz);
        glTexCoord2f(1.0, 0.0); glVertex3f(hdx,  -hdy, -hdz);
        glTexCoord2f(1.0, 1.0); glVertex3f(hdx,   hdy, -hdz);
        glTexCoord2f(0.0, 1.0); glVertex3f(hdx,   hdy,  hdz);
        glEnd();

        //--- some fancy stuff for understanding the orientation of camera
        container()->drawCylinder( wVector(0,1,0), wVector(-hdx-0.006f,0,hdz-0.006f), 0.004f, 0.008f );
        container()->drawCylinder( wVector(0,1,0), wVector(-hdx,0,-hdz), 0.004f, 0.008f );
        container()->drawCylinder( wVector(0,0,1), wVector(0,0.0,hdz+0.003f), 0.006f, 0.004f );
        glPopMatrix();
    };
    virtual void renderAABB( RenderWorld* gw ) {
        wVector minpoint, maxpoint;
        calculateAABB( minpoint, maxpoint, obj->matrix() );
        gw->drawWireBox( minpoint, maxpoint );
    };
    virtual void calculateAABB( wVector& minPoint, wVector& maxPoint, const wMatrix tm ) {
        real tdx = fabs(tm.x_ax[0]*dx) + fabs(tm.y_ax[0]*dy) + fabs(tm.z_ax[0]*dz);
        real tdy = fabs(tm.x_ax[1]*dx) + fabs(tm.y_ax[1]*dy) + fabs(tm.z_ax[1]*dz);
        real tdz = fabs(tm.x_ax[2]*dx) + fabs(tm.y_ax[2]*dy) + fabs(tm.z_ax[2]*dz);
        wVector hds( tdx/2.0, tdy/2.0, tdz/2.0 );
        minPoint = tm.w_pos - hds;
        maxPoint = tm.w_pos + hds;
    };
    virtual void calculateOBB( wVector& dimension, wVector& minPoint, wVector& maxPoint ) {
        dimension[0] = dx;
        dimension[1] = dy;
        dimension[2] = dz;
        wVector hds( dx/2.0, dy/2.0, dz/2.0 );
        minPoint = -hds;
        maxPoint = +hds;
    };
protected:
    WCamera* wcam;
    real dx, dy, dz;
};

#endif // FARSA_USE_YARP_AND_ICUB

// This class is the renderer associated with GraphicalWObject. It simply
// forwards calls to GraphicalWObject functions
class GraphicalWObjectRenderer : public RenderWObject
{
public:
    GraphicalWObjectRenderer(WObject* obj, RenderWObjectContainer* rw) :
        RenderWObject(obj, rw),
        m_graphicalWObject(dynamic_cast<GraphicalWObject*>(obj))
    {
    }

    virtual ~GraphicalWObjectRenderer()
    {
    }

    virtual void render(QGLContext *gw)
    {
        m_graphicalWObject->updateAndRender(this, gw);
    }

    virtual void renderAABB(RenderWorld *gw)
    {
        m_graphicalWObject->updateAndRenderAABB(this, gw);
    }

    virtual void calculateAABB(wVector &minPoint, wVector &maxPoint, const wMatrix tm)
    {
        m_graphicalWObject->updateAndCalculateAABB(minPoint, maxPoint, tm);
    }

    virtual void calculateOBB(wVector &dimension, wVector &minPoint, wVector &maxPoint)
    {
        m_graphicalWObject->updateAndCalculateOBB(dimension, minPoint, maxPoint);
    }

private:
    GraphicalWObject *const m_graphicalWObject;
};

bool RenderWObjectContainer::facInited = false;
QMap<QString, WAbstractCreator*>* RenderWObjectContainer::fac;
void RenderWObjectContainer::initFactory() {
    if ( facInited ) return;

    fac = new QMap<QString, WAbstractCreator*>();

    (*fac)["farsa::WMesh"] = new WCreator<RenderWMesh>();
    (*fac)["farsa::PhyBox"] = new WCreator<RenderPhyBox>();
    (*fac)["farsa::PhySphere"] = new WCreator<RenderPhySphere>();
    (*fac)["farsa::PhyCylinder"] = new WCreator<RenderPhyCylinder>();
    (*fac)["farsa::PhyCone"] = new WCreator<RenderPhyCone>();
    (*fac)["farsa::PhyCompoundObject"] = new WCreator<RenderCompoundObject>();
#ifdef FARSA_USE_YARP_AND_ICUB
    (*fac)["farsa::WCamera"] = new WCreator<RenderWCamera>();
    (*fac)[PhyiCub::staticMetaObject.className()] = new WCreator<RenderICub>();
#endif
    (*fac)["farsa::GraphicalWObject"] = new WCreator<GraphicalWObjectRenderer>();

    //--- FIXME a specific class should be implemented
    (*fac)["farsa::WObject"] = new WCreator<RenderGenericObject>();
    (*fac)["farsa::WorldController"] = new WCreator<RenderGenericObject>();

    facInited = true;
}

} // end namespace farsa