worldsim/src/wcamera.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  *
 ********************************************************************************/

#ifdef FARSA_USE_YARP_AND_ICUB

#include "wcamera.h"
#include <QApplication>
#include <QGLPixelBuffer>

// 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 yarp::dev;
using namespace yarp::sig;
using namespace yarp::os;

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

namespace farsa {

RenderCamera::RenderCamera( WCamera* wcamera, WObject* attachTo, unsigned int width, unsigned int height )
    : RenderWObjectContainer()/*, framev(width, height, QImage::Format_RGB32)*/ {
    connect( world(), SIGNAL( removedObject( WObject* ) ),
            this, SLOT( slotRemoveObject( WObject* ) ), Qt::DirectConnection );
    connect( world(), SIGNAL( addedObject( WObject* ) ),
            this, SLOT( slotAddObject( WObject* ) ), Qt::DirectConnection );
    //--- remove the attachTo WObject to renderized objects
    this->attachTo = attachTo;
    this->widthv = width;
    this->heightv = height;
    aspectRatio = 112.0f/94.0f;
    this->wcamera = wcamera;

    frameBuffer = new unsigned char[width*height*3];
    frameImage.setQuantum(1);
    frameImage.setExternal( frameBuffer, width, height );
    frameImage.setTopIsLowIndex( true );
    pbuffer = new QGLPixelBuffer( width, height );
    initializeGL();
}

RenderCamera::~RenderCamera() {
    delete[] frameBuffer;
    // I don't know why, but on some platform the following delete create a SIGSEV
    delete pbuffer;
}

void RenderCamera::moveRotatePointOfView( const wMatrix& tm ) {
    QMutexLocker locker(&mutex);

    // The translation along z is needed to avoid the camera being placed in the center of a component
    // of the head composite object which is inside the eye.
    // NOTE: The above is not very nice... Any solution better than this???
    eyep = tm.w_pos + tm.z_ax.scale(0.021f);
    atp = eyep + tm.z_ax;
    upv = -tm.x_ax;
}

void RenderCamera::initializeGL() {
    pbuffer->makeCurrent();
    glEnable(GL_LIGHT0);
    //glEnable(GL_LIGHTING);
    //glEnable(GL_COLOR_MATERIAL);
    glDisable(GL_LIGHTING);
    glDisable(GL_COLOR_MATERIAL);
    
    glEnable(GL_DEPTH_TEST);
    glClearColor( 0.2f, 0.2f, 0.2f, 1.0f );
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glViewport( 0, 0, GLint(widthv), GLint(heightv) );
    pbuffer->doneCurrent();
}

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

    moveRotatePointOfView( wcamera->matrix() );
    pbuffer->makeCurrent();
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    // GL_PROJECTION matrix
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective( 94.0, aspectRatio, 0.001 /*zNear*/, 1000.0 /*zFar*/ );
    // GL_MODELVIEW matrix
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
    gluLookAt( eyep[0], eyep[1], eyep[2], atp[0], atp[1], atp[2], upv[0], upv[1], upv[2] );

    //-------- Drawing SkyBox
    drawSkyGroundBox( (QGLContext*)(QGLContext::currentContext()) );

    // Render the WObject presents in the World
    for( int i=0; i<graphics().size(); i++ ) {
        RenderWObject* r = graphics()[i];
        if ( r->object() == attachTo ) continue;
        if ( r->object() == wcamera ) continue;
        if ( r->object()->isInvisible() ) continue;
        if ( r->object()->owner() ) continue;
        r->render( (QGLContext*)(QGLContext::currentContext()) );
    }
    glReadPixels( 0, 0, widthv, heightv, GL_RGB, GL_UNSIGNED_BYTE, frameBuffer );
    pbuffer->doneCurrent();
}

void RenderCamera::slotRemoveObject( WObject* w ) {
    removeObject( w );
}

void RenderCamera::slotAddObject( WObject* w ) {
    addObject( w );
}

bool RenderCamera::open( yarp::os::Searchable & ) {
    return true;
}

bool RenderCamera::close( ) {
    return true;
}

bool RenderCamera::configure( yarp::os::Searchable & ) {
    return true;
}

int RenderCamera::height() const {
    return heightv;
}

int RenderCamera::width() const {
    return widthv;
}
    
bool RenderCamera::getImage( yarp::sig::ImageOf<yarp::sig::PixelRgb>& image ) {
    image.setTopIsLowIndex( false );
    image.copy( frameImage );
    return true;
}

WCamera::WCamera( World* w, QString name, WObject* attachTo, unsigned int width, unsigned int height )
    : YarpObject( w, name ) {
    //Q_ASSERT_X( QGLPixelBuffer::hasOpenGLPbuffers(),
    //          "WCamera Constructor",
    //          "You must have a graphic card that support pbuffer OpenGL extension" );
    if ( !qApp ) {
        images = NULL;
    } else {
        images = new RenderCamera( this, attachTo, width, height );
        images->setWorld(w);
    }
    setColor( Qt::green );
    this->attachTo = attachTo;
    setMatrix( attachTo->matrix() );
    w->pushObject( this );
    //registerServerFrameGrabber( images, "" );
    porty.open( QString( "/%1/%2" ).arg( world()->name() ).arg( name ).toAscii().data() );
}

WCamera::WCamera( World* w, QString name, unsigned int width, unsigned int height, const wMatrix& tm )
    : YarpObject( w, name ) {
    //Q_ASSERT_X( QGLPixelBuffer::hasOpenGLPbuffers(),
    //          "WCamera Constructor",
    //          "You must have a graphic card that support pbuffer OpenGL extension" );
    if ( !qApp ) {
        images = NULL;
    } else {
        images = new RenderCamera( this, NULL, width, height );
        images->setWorld(w);
    }
    this->attachTo = NULL;
    setMatrix( tm );
    w->pushObject( this );
    //registerServerFrameGrabber( images, "" );
    porty.open( QString( "/%1/%2" ).arg( world()->name() ).arg( name ).toAscii().data() );
}

WCamera::~WCamera() {
    delete images;
}

void WCamera::preUpdate() {
    if ( attachTo ) {
        setMatrix( attachTo->matrix() );
    }
}

void WCamera::postUpdate() {
    if ( !images ) return;
    images->paintGL();
    ImageOf<PixelRgb>& frame = porty.prepare();
    images->getImage( frame );
    porty.write();
}

} // end namespace farsa

#endif // FARSA_USE_YARP_AND_ICUB