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 "phyheightfield.h"
#include "phyicub.h"
#include "wcamera.h"
#include "phycompoundobject.h"
#include "wmesh.h"
#include "graphicalwobject.h"
#include "private/renderwobjecthierarchy.h"
#include "Newton.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();
//      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]);
//
//      GLUquadricObj *pObj;
//
//      // 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 RenderPhyCylinder : public RenderWObject {
public:
    RenderPhyCylinder(WObject* wobj, RenderWObjectContainer* container) :
        RenderWObject(wobj, container),
        m_cylinder(dynamic_cast<PhyCylinder*>(wobj)),
        m_height(m_cylinder->height()),
        m_radius(m_cylinder->radius()),
        m_cylinderVertexes(),
        m_cylinderNormals(),
        m_cylinderColors(),
        m_cylinderTextureCoords(),
        m_upperBaseVertexes(),
        m_upperBaseNormals(),
        m_upperBaseColors(),
        m_upperBaseTextureCoords(),
        m_upperBaseSegmentsLength(),
        m_lowerBaseVertexes(),
        m_lowerBaseNormals(),
        m_lowerBaseColors(),
        m_lowerBaseTextureCoords(),
        m_lowerBaseSegmentsLength()
    {
        // The cylinder representation will be updated in the first call to render()
    }

    virtual ~RenderPhyCylinder()
    {
        // Nothing to do here
    }

    virtual void render(QGLContext* gw)
    {
        // Checking if we have to regenerate the representation of the cylinder
        if (m_cylinder->graphicalRepresentationNeedsUpdate()) {
            updateRepresentation();
        }

        glPushMatrix();
        container()->setupColorTexture(gw, this);
        GLMultMatrix(&(m_cylinder->matrix()[0][0]));

        // First drawing the cylinder. We need to enable the vertex arrays, set the
        // starting point for vertex, normals and colors and then actually draw triangles
        glEnableClientState(GL_NORMAL_ARRAY);
        glEnableClientState(GL_COLOR_ARRAY);
        glEnableClientState(GL_VERTEX_ARRAY);
        glEnableClientState(GL_TEXTURE_COORD_ARRAY);

        glNormalPointer(GL_FLOAT, 0, m_cylinderNormals.data());
        glColorPointer(3, GL_FLOAT, 0, m_cylinderColors.data());
        glVertexPointer(3, GL_FLOAT, 0, m_cylinderVertexes.data());
        glTexCoordPointer(2, GL_FLOAT, 0, m_cylinderTextureCoords.data());

        glDrawArrays(GL_TRIANGLE_STRIP, 0, m_cylinderVertexes.size() / 3);

        // Now drawing the upper base
        glNormalPointer(GL_FLOAT, 0, m_upperBaseNormals.data());
        glColorPointer(3, GL_FLOAT, 0, m_upperBaseColors.data());
        glVertexPointer(3, GL_FLOAT, 0, m_upperBaseVertexes.data());
        glTexCoordPointer(2, GL_FLOAT, 0, m_upperBaseTextureCoords.data());

        unsigned int startIndex = 0;
        foreach (unsigned int curLength, m_upperBaseSegmentsLength) {
            glDrawArrays(GL_TRIANGLE_FAN, startIndex, curLength);
            startIndex += curLength;
        }

        // Finally drawing the lower base
        glNormalPointer(GL_FLOAT, 0, m_lowerBaseNormals.data());
        glColorPointer(3, GL_FLOAT, 0, m_lowerBaseColors.data());
        glVertexPointer(3, GL_FLOAT, 0, m_lowerBaseVertexes.data());
        glTexCoordPointer(2, GL_FLOAT, 0, m_lowerBaseTextureCoords.data());

        startIndex = 0;
        foreach (unsigned int curLength, m_lowerBaseSegmentsLength) {
            glDrawArrays(GL_TRIANGLE_FAN, startIndex, curLength);
            startIndex += curLength;
        }

        glDisableClientState(GL_VERTEX_ARRAY);
        glDisableClientState(GL_COLOR_ARRAY);
        glDisableClientState(GL_NORMAL_ARRAY);
        glDisableClientState(GL_TEXTURE_COORD_ARRAY);

        glPopMatrix();
    }

    virtual void renderAABB(RenderWorld* gw)
    {
        // Checking if we have to regenerate the representation of the cylinder
        if (m_cylinder->graphicalRepresentationNeedsUpdate()) {
            updateRepresentation();
        }

        wVector minpoint, maxpoint;
        calculateAABB(minpoint, maxpoint, obj->matrix());
        gw->drawWireBox(minpoint, maxpoint);
    }

    virtual void calculateAABB(wVector& minPoint, wVector& maxPoint, const wMatrix tm)
    {
        // Checking if we have to regenerate the representation of the cylinder
        if (m_cylinder->graphicalRepresentationNeedsUpdate()) {
            updateRepresentation();
        }

        const real h2 = m_height / 2.0;
        const real tdx = fabs(tm.x_ax[0] * h2) + fabs(tm.y_ax[0] * m_radius) + fabs(tm.z_ax[0] * m_radius);
        const real tdy = fabs(tm.x_ax[1] * h2) + fabs(tm.y_ax[1] * m_radius) + fabs(tm.z_ax[1] * m_radius);
        const real tdz = fabs(tm.x_ax[2] * h2) + fabs(tm.y_ax[2] * m_radius) + fabs(tm.z_ax[2] * m_radius);
        const wVector hds(tdx, tdy, tdz);
        minPoint = tm.w_pos - hds;
        maxPoint = tm.w_pos + hds;
    }

    virtual void calculateOBB(wVector& dimension, wVector& minPoint, wVector& maxPoint)
    {
        // Checking if we have to regenerate the representation of the cylinder
        if (m_cylinder->graphicalRepresentationNeedsUpdate()) {
            updateRepresentation();
        }

        dimension[0] = m_height;
        dimension[1] = m_radius * 2.0;
        dimension[2] = m_radius * 2.0;
        const wVector rds(m_height / 2.0, m_radius, m_radius);
        minPoint = -rds;
        maxPoint = +rds;
    }

protected:
    void updateRepresentation()
    {
        // A couple of helper constants
        const int numDivs = 20;
        const float anglePerDiv = (2 * M_PI) / float(numDivs);
        const float lowerBaseX = -m_height / 2.0;
        const float upperBaseX = +m_height / 2.0;

        // Resetting all lists
        m_cylinderVertexes.clear();
        m_cylinderNormals.clear();
        m_cylinderColors.clear();
        m_cylinderTextureCoords.clear();
        m_upperBaseVertexes.clear();
        m_upperBaseNormals.clear();
        m_upperBaseColors.clear();
        m_upperBaseTextureCoords.clear();
        m_upperBaseSegmentsLength.clear();
        m_lowerBaseVertexes.clear();
        m_lowerBaseNormals.clear();
        m_lowerBaseColors.clear();
        m_lowerBaseTextureCoords.clear();
        m_lowerBaseSegmentsLength.clear();

        // Getting the list of colors of the cylinder. If the cylinder has only one color, we generate a list to be
        // able to use the code below anyway
        QList<PhyCylinder::SegmentColor> cylinderColors;
        if (m_cylinder->color().isValid()) {
            // Single color for thw whole cylinder
            cylinderColors.append(PhyCylinder::SegmentColor(0.0, m_cylinder->color()));
        } else {
            cylinderColors = m_cylinder->segmentsColor();
        }

        // Now generating vertexes for the cylinder
        float curOffset = 0.0;
        unsigned int curSegment = 0;

        // In the exit condition of the cycle we add +anglePerDiv/2 to a full cycle to be sure to add the final point
        // (it could be skipped because of numerical errors)
        while (curOffset < (2.0 * M_PI + anglePerDiv / 2.0)) {
            const float curAngle = cylinderColors[0].startingAngle + curOffset;

            if (((curSegment + 1) != cylinderColors.size()) && (cylinderColors[curSegment + 1].startingAngle < curAngle)) {
                // Computing the two points on the color division boundary (which will then be duplicated)
                const float normY = sin(cylinderColors[curSegment + 1].startingAngle);
                const float normZ = cos(cylinderColors[curSegment + 1].startingAngle);
                const float y = m_radius * normY;
                const float z = m_radius * normZ;

                // Adding the first couple of points
                m_cylinderVertexes.append(lowerBaseX);
                m_cylinderVertexes.append(y);
                m_cylinderVertexes.append(z);
                m_cylinderNormals.append(0.0);
                m_cylinderNormals.append(normY);
                m_cylinderNormals.append(normZ);
                m_cylinderColors.append(cylinderColors[curSegment].color.redF());
                m_cylinderColors.append(cylinderColors[curSegment].color.greenF());
                m_cylinderColors.append(cylinderColors[curSegment].color.blueF());
                m_cylinderTextureCoords.append(0.0);
                m_cylinderTextureCoords.append(cylinderColors[curSegment + 1].startingAngle / (2.0 * M_PI));
                m_cylinderVertexes.append(upperBaseX);
                m_cylinderVertexes.append(y);
                m_cylinderVertexes.append(z);
                m_cylinderNormals.append(0.0);
                m_cylinderNormals.append(normY);
                m_cylinderNormals.append(normZ);
                m_cylinderColors.append(cylinderColors[curSegment].color.redF());
                m_cylinderColors.append(cylinderColors[curSegment].color.greenF());
                m_cylinderColors.append(cylinderColors[curSegment].color.blueF());
                m_cylinderTextureCoords.append(1.0);
                m_cylinderTextureCoords.append(cylinderColors[curSegment + 1].startingAngle / (2.0 * M_PI));

                // Incrementing the segment
                ++curSegment;

                // Adding again the points (which will have the new color)
                m_cylinderVertexes.append(lowerBaseX);
                m_cylinderVertexes.append(y);
                m_cylinderVertexes.append(z);
                m_cylinderNormals.append(0.0);
                m_cylinderNormals.append(normY);
                m_cylinderNormals.append(normZ);
                m_cylinderColors.append(cylinderColors[curSegment].color.redF());
                m_cylinderColors.append(cylinderColors[curSegment].color.greenF());
                m_cylinderColors.append(cylinderColors[curSegment].color.blueF());
                m_cylinderTextureCoords.append(0.0);
                m_cylinderTextureCoords.append(cylinderColors[curSegment].startingAngle / (2.0 * M_PI));
                m_cylinderVertexes.append(upperBaseX);
                m_cylinderVertexes.append(y);
                m_cylinderVertexes.append(z);
                m_cylinderNormals.append(0.0);
                m_cylinderNormals.append(normY);
                m_cylinderNormals.append(normZ);
                m_cylinderColors.append(cylinderColors[curSegment].color.redF());
                m_cylinderColors.append(cylinderColors[curSegment].color.greenF());
                m_cylinderColors.append(cylinderColors[curSegment].color.blueF());
                m_cylinderTextureCoords.append(1.0);
                m_cylinderTextureCoords.append(cylinderColors[curSegment].startingAngle / (2.0 * M_PI));
            }

            // Adding two points, one on the lower base, the other on the upper base
            const float normY = sin(curAngle);
            const float normZ = cos(curAngle);
            const float y = m_radius * normY;
            const float z = m_radius * normZ;
            m_cylinderVertexes.append(lowerBaseX);
            m_cylinderVertexes.append(y);
            m_cylinderVertexes.append(z);
            m_cylinderNormals.append(0.0);
            m_cylinderNormals.append(normY);
            m_cylinderNormals.append(normZ);
            m_cylinderColors.append(cylinderColors[curSegment].color.redF());
            m_cylinderColors.append(cylinderColors[curSegment].color.greenF());
            m_cylinderColors.append(cylinderColors[curSegment].color.blueF());
            m_cylinderTextureCoords.append(0.0);
            m_cylinderTextureCoords.append(curOffset / (2.0 * M_PI));
            m_cylinderVertexes.append(upperBaseX);
            m_cylinderVertexes.append(y);
            m_cylinderVertexes.append(z);
            m_cylinderNormals.append(0.0);
            m_cylinderNormals.append(normY);
            m_cylinderNormals.append(normZ);
            m_cylinderColors.append(cylinderColors[curSegment].color.redF());
            m_cylinderColors.append(cylinderColors[curSegment].color.greenF());
            m_cylinderColors.append(cylinderColors[curSegment].color.blueF());
            m_cylinderTextureCoords.append(1.0);
            m_cylinderTextureCoords.append(curOffset / (2.0 * M_PI));

            curOffset += anglePerDiv;
        }

        // Now the upper base. First getting the list of colors (as for the cylinder we generate a list of
        // colors so that the code below will work in any case)
        QList<PhyCylinder::SegmentColor> upperBaseColor;
        if (m_cylinder->color().isValid()) {
            // Single color for the whole cylinder
            upperBaseColor.append(PhyCylinder::SegmentColor(0.0, m_cylinder->color()));
        } else if (m_cylinder->upperBaseColor().isValid()) {
            // Single color for the whole base
            upperBaseColor.append(PhyCylinder::SegmentColor(0.0, m_cylinder->upperBaseColor()));
        } else {
            upperBaseColor = m_cylinder->segmentsColor();
        }

        // Now generating vertexes for the upper base
        curOffset = 0.0;
        curSegment = 0;
        unsigned int numVertexesCurSegment = 0;

        // In the exit condition of the cycle we add +anglePerDiv/2 to a full cycle to be sure to add the final point
        // (it could be skipped because of numerical errors). Before entering the cycle, adding the central point (remember
        // that we will draw this list of vertexes as a TRIANGLE_FAN)
        m_upperBaseVertexes.append(upperBaseX);
        m_upperBaseVertexes.append(0.0);
        m_upperBaseVertexes.append(0.0);
        m_upperBaseNormals.append(1.0);
        m_upperBaseNormals.append(0.0);
        m_upperBaseNormals.append(0.0);
        m_upperBaseColors.append(upperBaseColor[curSegment].color.redF());
        m_upperBaseColors.append(upperBaseColor[curSegment].color.greenF());
        m_upperBaseColors.append(upperBaseColor[curSegment].color.blueF());
        m_upperBaseTextureCoords.append(0.5);
        m_upperBaseTextureCoords.append(0.5);
        ++numVertexesCurSegment;
        while (curOffset < (2.0 * M_PI + anglePerDiv / 2.0)) {
            const float curAngle = upperBaseColor[0].startingAngle + curOffset;

            if (((curSegment + 1) != upperBaseColor.size()) && (upperBaseColor[curSegment + 1].startingAngle < curAngle)) {
                // Computing the point on the color division boundary (which will then be duplicated)
                const float normY = sin(upperBaseColor[curSegment + 1].startingAngle);
                const float normZ = cos(upperBaseColor[curSegment + 1].startingAngle);
                const float y = m_radius * normY;
                const float z = m_radius * normZ;

                // Adding the first point
                m_upperBaseVertexes.append(upperBaseX);
                m_upperBaseVertexes.append(y);
                m_upperBaseVertexes.append(z);
                m_upperBaseNormals.append(1.0);
                m_upperBaseNormals.append(0.0);
                m_upperBaseNormals.append(0.0);
                m_upperBaseColors.append(upperBaseColor[curSegment].color.redF());
                m_upperBaseColors.append(upperBaseColor[curSegment].color.greenF());
                m_upperBaseColors.append(upperBaseColor[curSegment].color.blueF());
                m_upperBaseTextureCoords.append((normY + 1.0) / 2.0);
                m_upperBaseTextureCoords.append((normZ + 1.0) / 2.0);
                ++numVertexesCurSegment;
                m_upperBaseSegmentsLength.append(numVertexesCurSegment);

                // Incrementing the segment
                ++curSegment;

                // Adding again the center and then the points (which will have the new color)
                m_upperBaseVertexes.append(upperBaseX);
                m_upperBaseVertexes.append(0.0);
                m_upperBaseVertexes.append(0.0);
                m_upperBaseNormals.append(1.0);
                m_upperBaseNormals.append(0.0);
                m_upperBaseNormals.append(0.0);
                m_upperBaseColors.append(upperBaseColor[curSegment].color.redF());
                m_upperBaseColors.append(upperBaseColor[curSegment].color.greenF());
                m_upperBaseColors.append(upperBaseColor[curSegment].color.blueF());
                m_upperBaseTextureCoords.append(0.5);
                m_upperBaseTextureCoords.append(0.5);
                m_upperBaseVertexes.append(upperBaseX);
                m_upperBaseVertexes.append(y);
                m_upperBaseVertexes.append(z);
                m_upperBaseNormals.append(1.0);
                m_upperBaseNormals.append(0.0);
                m_upperBaseNormals.append(0.0);
                m_upperBaseColors.append(upperBaseColor[curSegment].color.redF());
                m_upperBaseColors.append(upperBaseColor[curSegment].color.greenF());
                m_upperBaseColors.append(upperBaseColor[curSegment].color.blueF());
                m_upperBaseTextureCoords.append((normY + 1.0) / 2.0);
                m_upperBaseTextureCoords.append((normZ + 1.0) / 2.0);
                numVertexesCurSegment = 2;
            }

            // Adding a new point points
            const float normY = sin(curAngle);
            const float normZ = cos(curAngle);
            const float y = m_radius * normY;
            const float z = m_radius * normZ;
            m_upperBaseVertexes.append(upperBaseX);
            m_upperBaseVertexes.append(y);
            m_upperBaseVertexes.append(z);
            m_upperBaseNormals.append(1.0);
            m_upperBaseNormals.append(0.0);
            m_upperBaseNormals.append(0.0);
            m_upperBaseColors.append(upperBaseColor[curSegment].color.redF());
            m_upperBaseColors.append(upperBaseColor[curSegment].color.greenF());
            m_upperBaseColors.append(upperBaseColor[curSegment].color.blueF());
            m_upperBaseTextureCoords.append((normY + 1.0) / 2.0);
            m_upperBaseTextureCoords.append((normZ + 1.0) / 2.0);
            ++numVertexesCurSegment;

            curOffset += anglePerDiv;
        }
        // Adding the length of the last segment
        m_upperBaseSegmentsLength.append(numVertexesCurSegment);

        // Finally the lower base. First getting the list of colors (as for the cylinder we generate a list of
        // colors so that the code below will work in any case)
        QList<PhyCylinder::SegmentColor> lowerBaseColor;
        if (m_cylinder->color().isValid()) {
            // Single color for the whole cylinder
            lowerBaseColor.append(PhyCylinder::SegmentColor(0.0, m_cylinder->color()));
        } else if (m_cylinder->lowerBaseColor().isValid()) {
            // Single color for the whole base
            lowerBaseColor.append(PhyCylinder::SegmentColor(0.0, m_cylinder->lowerBaseColor()));
        } else {
            lowerBaseColor = m_cylinder->segmentsColor();
        }

        // Now generating vertexes for the lower base
        curOffset = 0.0;
        curSegment = 0;
        numVertexesCurSegment = 0;

        // In the exit condition of the cycle we add +anglePerDiv/2 to a full cycle to be sure to add the final point
        // (it could be skipped because of numerical errors). Before entering the cycle, adding the central point (remember
        // that we will draw this list of vertexes as a TRIANGLE_FAN)
        m_lowerBaseVertexes.append(lowerBaseX);
        m_lowerBaseVertexes.append(0.0);
        m_lowerBaseVertexes.append(0.0);
        m_lowerBaseNormals.append(-1.0);
        m_lowerBaseNormals.append(0.0);
        m_lowerBaseNormals.append(0.0);
        m_lowerBaseColors.append(lowerBaseColor[curSegment].color.redF());
        m_lowerBaseColors.append(lowerBaseColor[curSegment].color.greenF());
        m_lowerBaseColors.append(lowerBaseColor[curSegment].color.blueF());
        m_lowerBaseTextureCoords.append(0.5);
        m_lowerBaseTextureCoords.append(0.5);
        ++numVertexesCurSegment;
        while (curOffset < (2.0 * M_PI + anglePerDiv / 2.0)) {
            const float curAngle = lowerBaseColor[0].startingAngle + curOffset;

            if (((curSegment + 1) != lowerBaseColor.size()) && (lowerBaseColor[curSegment + 1].startingAngle < curAngle)) {
                // Computing the point on the color division boundary (which will then be duplicated)
                const float normY = sin(lowerBaseColor[curSegment + 1].startingAngle);
                const float normZ = cos(lowerBaseColor[curSegment + 1].startingAngle);
                const float y = m_radius * normY;
                const float z = m_radius * normZ;

                // Adding the first point
                m_lowerBaseVertexes.append(lowerBaseX);
                m_lowerBaseVertexes.append(y);
                m_lowerBaseVertexes.append(z);
                m_lowerBaseNormals.append(-1.0);
                m_lowerBaseNormals.append(0.0);
                m_lowerBaseNormals.append(0.0);
                m_lowerBaseColors.append(lowerBaseColor[curSegment].color.redF());
                m_lowerBaseColors.append(lowerBaseColor[curSegment].color.greenF());
                m_lowerBaseColors.append(lowerBaseColor[curSegment].color.blueF());
                m_lowerBaseTextureCoords.append((normY + 1.0) / 2.0);
                m_lowerBaseTextureCoords.append((normZ + 1.0) / 2.0);
                ++numVertexesCurSegment;
                m_lowerBaseSegmentsLength.append(numVertexesCurSegment);

                // Incrementing the segment
                ++curSegment;

                // Adding again the center and then the points (which will have the new color)
                m_lowerBaseVertexes.append(lowerBaseX);
                m_lowerBaseVertexes.append(0.0);
                m_lowerBaseVertexes.append(0.0);
                m_lowerBaseNormals.append(-1.0);
                m_lowerBaseNormals.append(0.0);
                m_lowerBaseNormals.append(0.0);
                m_lowerBaseColors.append(lowerBaseColor[curSegment].color.redF());
                m_lowerBaseColors.append(lowerBaseColor[curSegment].color.greenF());
                m_lowerBaseColors.append(lowerBaseColor[curSegment].color.blueF());
                m_lowerBaseTextureCoords.append(0.5);
                m_lowerBaseTextureCoords.append(0.5);
                m_lowerBaseVertexes.append(lowerBaseX);
                m_lowerBaseVertexes.append(y);
                m_lowerBaseVertexes.append(z);
                m_lowerBaseNormals.append(-1.0);
                m_lowerBaseNormals.append(0.0);
                m_lowerBaseNormals.append(0.0);
                m_lowerBaseColors.append(lowerBaseColor[curSegment].color.redF());
                m_lowerBaseColors.append(lowerBaseColor[curSegment].color.greenF());
                m_lowerBaseColors.append(lowerBaseColor[curSegment].color.blueF());
                m_lowerBaseTextureCoords.append((normY + 1.0) / 2.0);
                m_lowerBaseTextureCoords.append((normZ + 1.0) / 2.0);
                numVertexesCurSegment = 2;
            }

            // Adding a new point points
            const float normY = sin(curAngle);
            const float normZ = cos(curAngle);
            const float y = m_radius * normY;
            const float z = m_radius * normZ;
            m_lowerBaseVertexes.append(lowerBaseX);
            m_lowerBaseVertexes.append(y);
            m_lowerBaseVertexes.append(z);
            m_lowerBaseNormals.append(-1.0);
            m_lowerBaseNormals.append(0.0);
            m_lowerBaseNormals.append(0.0);
            m_lowerBaseColors.append(lowerBaseColor[curSegment].color.redF());
            m_lowerBaseColors.append(lowerBaseColor[curSegment].color.greenF());
            m_lowerBaseColors.append(lowerBaseColor[curSegment].color.blueF());
            m_lowerBaseTextureCoords.append((normY + 1.0) / 2.0);
            m_lowerBaseTextureCoords.append((normZ + 1.0) / 2.0);
            ++numVertexesCurSegment;

            curOffset += anglePerDiv;
        }
        // Adding the length of the last segment
        m_lowerBaseSegmentsLength.append(numVertexesCurSegment);
    }

    PhyCylinder* const m_cylinder;

    const real m_height;

    const real m_radius;

    QVector<GLfloat> m_cylinderVertexes;

    QVector<GLfloat> m_cylinderNormals;

    QVector<GLfloat> m_cylinderColors;

    QVector<GLfloat> m_cylinderTextureCoords;

    QVector<GLfloat> m_upperBaseVertexes;

    QVector<GLfloat> m_upperBaseNormals;

    QVector<GLfloat> m_upperBaseColors;

    QVector<GLfloat> m_upperBaseTextureCoords;

    QVector<unsigned int> m_upperBaseSegmentsLength;

    QVector<GLfloat> m_lowerBaseVertexes;

    QVector<GLfloat> m_lowerBaseNormals;

    QVector<GLfloat> m_lowerBaseColors;

    QVector<GLfloat> m_lowerBaseTextureCoords;

    QVector<unsigned int> m_lowerBaseSegmentsLength;
};

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

// class RenderPhyHeightField : public RenderWObject {
// public:
//  RenderPhyHeightField( WObject* wobj, RenderWObjectContainer* container ) : RenderWObject( wobj, container ) {
//      hf = (PhyHeightField*)wobj;
//      dx = hf->sideX();
//      dy = hf->sideY();
//      dz = 0.01f;
//  };
//     virtual ~RenderPhyHeightField() { };
//     virtual void render( QGLContext* gw ) {
//      glPushMatrix();
//      container()->setupColorTexture( gw, this );
//      wMatrix m = hf->matrix().rotateAround( wVector(1,0,0), wVector(0,0,0), toRad(-90) );
//      m.w_pos[0] -= dx/2.0;
//      m.w_pos[1] -= dy/2.0;
//      GLMultMatrix(&m[0][0]);
//
//      NewtonMesh* mesh = hf->mesh;
//      int stride = NewtonMeshGetVertexStrideInByte(mesh) / sizeof (double);
//      const double* const vertexList = NewtonMeshGetVertexArray(mesh);
//
//      glBegin(GL_TRIANGLES);
//      for (void* face = NewtonMeshGetFirstFace(mesh); face; face = NewtonMeshGetNextFace(mesh, face)) {
//          if (!NewtonMeshIsFaceOpen (mesh, face)) {
//              int indices[1024];
//              int vertexCount = NewtonMeshGetFaceIndexCount (mesh, face);
//              Q_ASSERT (vertexCount < sizeof (indices)/sizeof (indices[0]));
//              NewtonMeshGetFaceIndices (mesh, face, indices);
//
//              //dFloat64 normal[4];
//              //NewtonMeshCalculateFaceNormal (mesh, face, normal);
//              //glNormal3f(normal[0], normal[1], normal[2]);
//
//              int i0 =  indices[0];
//              int i1 =  indices[1];
//              wVector p0 ( dFloat(vertexList[i0 * stride + 0]),
//                           dFloat(vertexList[i0 * stride + 1]),
//                           dFloat(vertexList[i0 * stride + 2]),
//                           0.0f );
//              wVector p1 ( dFloat(vertexList[i1 * stride + 0]),
//                           dFloat(vertexList[i1 * stride + 1]),
//                           dFloat(vertexList[i1 * stride + 2]),
//                           0.0f );
//              for (int i = 2; i < vertexCount; i++) {
//                  int i2 = indices[i];
//                  wVector p2 ( dFloat(vertexList[i2 * stride + 0]),
//                               dFloat(vertexList[i2 * stride + 1]),
//                               dFloat(vertexList[i2 * stride + 2]),
//                               0.0f );
//                  glVertex3f(p0[0], p0[1], p0[2]);
//                  glVertex3f(p1[0], p1[1], p1[2]);
//                  glVertex3f(p2[0], p2[1], p2[2]);
//                  p1 = p2;
//              }
//          }
//      }
//
//      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:
//  PhyHeightField* hf;
//  real dx, dy, dz;
// };

#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::PhyHeightField"] = new WCreator<RenderPhyHeightField>();
    (*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