/*
 *--------------------------------------------
 * virtualWorld.cpp
 *
 * Author: Emmanuelle Chapoulie
 *
 * Description:     Creates physics and
 *                  graphics world
 *--------------------------------------------
 */

#include "virtualWorld.h"
#include "compoundShapeVisitor.h"
#include <osg/Light>
#include <osg/LightSource>
#include <osg/ShapeDrawable>
#include <osgDB/ReadFile>
#include <osgUtil/Optimizer>
#include <osgwTools/Shapes.h>
#include <osgbCollision/ComputeShapeVisitor.h>
#include <osgbCollision/RefBulletObject.h>
#include <osgbCollision/CollisionShapes.h>
#include <osgbCollision/Utils.h>
#include <osgbDynamics/RigidBody.h>

osg::Light* light = NULL;

//Colors
osg::Vec4 blue   = osg::Vec4(0., 0., 1., 1.);
osg::Vec4 green  = osg::Vec4(0., 1., 0., 1.);
osg::Vec4 yellow = osg::Vec4(1., 1., 0., 1.);
osg::Vec4 red    = osg::Vec4(1., 0., 0., 1.);
osg::Vec4 white  = osg::Vec4(1., 1., 1., 1.);
osg::Vec4 cyan   = osg::Vec4(0., 1., 1., 1.);
osg::Vec4 black  = black;

VirtualWorld::VirtualWorld(isi::AbstractViewer* _v,
                           std::vector<isi::HandHandler*> _h,
                           isi::Wand* _w,
                           int _t)
{
    //Creates physics world
    btDefaultCollisionConfiguration* collisionConfiguration(new btDefaultCollisionConfiguration);
    btCollisionDispatcher*           dispatcher(new btCollisionDispatcher(collisionConfiguration));
    btGestureConstraintSolver*       solver(new btGestureConstraintSolver);
    btDbvtBroadphase*                interface(new btDbvtBroadphase);
    physicsWorld = new btGestureDynamicsWorld(dispatcher, interface, solver, collisionConfiguration);
    physicsWorld->setGravity(btVector3(0., 0., 0.));
    physicsWorld->getSolverInfo().m_solverMode &= ~SOLVER_USE_WARMSTARTING;

    //Creates graphics world
    graphicsWorld = new osgShadow::ShadowedScene;
    osg::ref_ptr<osgShadow::ShadowMap> shadowMap = new osgShadow::ShadowMap;
    graphicsWorld->setShadowTechnique(shadowMap);
    shadowMap->setTextureSize(osg::Vec2s(4096*2, 4096*2));
    osgUtil::Optimizer optimizer;
    optimizer.optimize(graphicsWorld, osgUtil::Optimizer::INDEX_MESH | osgUtil::Optimizer::VERTEX_PRETRANSFORM | osgUtil::Optimizer::VERTEX_POSTTRANSFORM);

    //Inits viewer
    viewer = _v;

    //Inits trackers
    for(int i = 0; i < _h.size(); i++)
        handTrackers.push_back(_h.at(i));
    wandTracker = _w;

    //Inits selector
    selector = new G2Selector(0, viewer->scene());
    wandTracker->registerHandler(0, selector);

    //Inits object counter
    objCnt = 0;

    //Inits scene type
    task = _t;

    //Inits materials
    defaultMat = new osg::Material;
    defaultMat->setDiffuse(osg::Material::FRONT, white);
    grabMat = new osg::Material;
    grabMat->setDiffuse(osg::Material::FRONT, blue);

    createWorld();
}

VirtualWorld::~VirtualWorld()
{
}

void VirtualWorld::createWorld()
{
    if(viewer){
        //PositionAttitudeTransform for graphicsWorld
        osg::ref_ptr<osg::PositionAttitudeTransform> rootPAT = new osg::PositionAttitudeTransform();
        rootPAT->setPosition(osg::Vec3d(0., 1.20, 0.));
        rootPAT->addChild(graphicsWorld.get());
        rootPAT->computeWorldToLocalMatrix(worldTransformMatrix,NULL);
        selector->setWorldTransformMatrix(worldTransformMatrix);

        //Adds directional lighting
        osg::ref_ptr<osg::Light> dirLight = new osg::Light;
        dirLight->setDiffuse(white);
        dirLight->setPosition(osg::Vec4(0.5, -0.3, 1.3, 1.));
        dirLight->setDirection(osg::Vec3(0., 0., -1.));
        dirLight->setLightNum(1);
        light = dirLight.get();
        osg::ref_ptr<osg::LightSource> dirLightSource = new osg::LightSource;
        dirLightSource->setLight(dirLight);
        dirLightSource->setStateSetModes(*(graphicsWorld->getOrCreateStateSet()), osg::StateAttribute::ON);
        graphicsWorld->addChild(dirLightSource.get());

        //Adds practice indicator
        practiceIndicator = new osg::Switch();
        osg::ref_ptr<osgwTools::AbsoluteModelTransform> amt = new osgwTools::AbsoluteModelTransform;
        osg::ref_ptr<osg::Geode> indicatorGeode = new osg::Geode;
        indicatorGeode->addDrawable(osgwTools::makePlane(osg::Vec3(-1.6, 2.4, /*2.2*/0.), osg::Vec3(/*.2*/3.2, 0., 0.), osg::Vec3(0., 0., /*.2*/2.4)));
        amt->setDataVariance(osg::Object::STATIC);
        amt->setReferenceFrame(osg::Transform::ABSOLUTE_RF_INHERIT_VIEWPOINT);
        amt->addChild(indicatorGeode.get());
        practiceIndicator->addChild(amt.get());
        practiceIndicator->setAllChildrenOff();
        viewer->addChildToWorld(practiceIndicator.get());

        //Adds light ray from laser emitter
        lightRay = new osgwTools::AbsoluteModelTransform;
        osg::ref_ptr<osg::Geode> lightGeode = new osg::Geode;
        lightRay->setDataVariance(osg::Object::DYNAMIC);
        lightRay->setReferenceFrame(osg::Transform::ABSOLUTE_RF_INHERIT_VIEWPOINT);
        lightRay->addChild(lightGeode.get());
        osg::ref_ptr<osg::Material> lightMat = new osg::Material;
        lightMat->setDiffuse(osg::Material::FRONT, cyan);
        lightMat->setSpecular(osg::Material::FRONT, cyan);
        lightMat->setEmission(osg::Material::FRONT, cyan);
        lightGeode->getOrCreateStateSet()->setAttribute(lightMat.get(), osg::StateAttribute::ON);
        viewer->addChildToWorld(lightRay.get());

        //Adds platform to raise the scene
        platform = new osgwTools::AbsoluteModelTransform;
        osg::ref_ptr<osg::Geode> platformGeode = new osg::Geode;
        platform->setDataVariance(osg::Object::STATIC);
        platform->setReferenceFrame(osg::Transform::ABSOLUTE_RF_INHERIT_VIEWPOINT);
        platform->addChild(platformGeode.get());
        osg::ref_ptr<osg::Material> platformMat = new osg::Material;
        platformMat->setDiffuse(osg::Material::FRONT, osg::Vec4(0.54, 0.32, 0.08, 1.));
        platformMat->setAmbient(osg::Material::FRONT, osg::Vec4(0.54, 0.32, 0.08, 0.5));
        platformGeode->getOrCreateStateSet()->setAttribute(platformMat.get(), osg::StateAttribute::ON);
        graphicsWorld->addChild(platform.get());

        //Creates kinematic objects
        createHands();
        createWand();

        //Creates scene
        createScene(task);

        //Adds graphics world to scene
        viewer->addChildToScene(rootPAT);
    }
}

void VirtualWorld::createHands()
{
    for(int i = 0; i < handTrackers.size(); i++){
        Handles newHand;
        for(int j = 0; j < 3; j++){
            //Transform
            osg::ref_ptr<osgwTools::AbsoluteModelTransform> amt = new osgwTools::AbsoluteModelTransform;
            amt->setReferenceFrame(osg::Transform::ABSOLUTE_RF_INHERIT_VIEWPOINT);
            switch(j){
            case 0: amt->addChild(handTrackers[i]->getThumb()->getChild(0)); break;
            case 1: amt->addChild(handTrackers[i]->getIndex()->getChild(0)); break;
            case 2: amt->addChild(handTrackers[i]->getMiddle()->getChild(0)); break;
            }

            //Create physic object from node data
            btRigidBody* body = new btRigidBody(0., NULL, osgbCollision::btConvexTriMeshCollisionShapeFromOSG(amt->getChild(0)));
            body->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
            //Activate contacts only with the thumb and the index
            if(j > 1) body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_NO_CONTACT_RESPONSE);
            body->setActivationState(DISABLE_DEACTIVATION);
            body->getCollisionShape()->setMargin(btScalar(0.001));
            body->setType(BT_HAND, i);
            body->setObjectId(objCnt++);

            //Reference rigid body to OSG
            amt->setUserData(new osgbCollision::RefRigidBody(body));

            //Add hand parts to hand
            switch(j){
            case 0: newHand[body] = handTrackers[i]->getThumb(); break;
            case 1: newHand[body] = handTrackers[i]->getIndex(); break;
            case 2: newHand[body] = handTrackers[i]->getMiddle(); break;
            }

            //Add physic object to physic world
            physicsWorld->addRigidBody(body);
        }

        //Add hand data to hands vector
        hands.push_back(newHand);
    }
}

void VirtualWorld::createWand()
{
    wand = new G2Wand();
    for(int i = 0; i < 2; i++){
        //Create geode
        osg::ref_ptr<osg::Geode> geode = new osg::Geode;
        geode->addDrawable(new osg::ShapeDrawable(
                               new osg::Sphere(osg::Vec3(), 0.005)
                               ));
        osg::ref_ptr<osg::Switch> switchNode = new osg::Switch();
        switchNode->addChild(geode.get());
        switchNode->setAllChildrenOff();
        osg::ref_ptr<osg::PositionAttitudeTransform> pat = new osg::PositionAttitudeTransform;
        pat->addChild(switchNode.release());

        //Transform
        osg::ref_ptr<osgwTools::AbsoluteModelTransform> amt = new osgwTools::AbsoluteModelTransform;
        amt->setDataVariance(osg::Object::DYNAMIC);
        amt->setReferenceFrame(osg::Transform::ABSOLUTE_RF_INHERIT_VIEWPOINT);
        amt->addChild(geode.get());

        //Create physic object from node data
        btRigidBody* body = new btRigidBody(0., NULL, osgbCollision::btSphereCollisionShapeFromOSG(geode.get()));
        body->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
        body->setActivationState(DISABLE_DEACTIVATION);
        body->getCollisionShape()->setMargin(btScalar(0.001));
        body->setType(BT_WAND, 0);
        body->setObjectId(objCnt++);

        //Reference rigid body to OSG
        amt->setUserData(new osgbCollision::RefRigidBody(body));

        //Add handle to wand handles
        wand->handles[body] = pat;

        //Add physic object to physic world
        physicsWorld->addRigidBody(body);

        //Add node to osg world
        viewer->addChildToWorld(pat.get());
    }
}

btRigidBody* VirtualWorld::createCompoundRigidBody(osgbDynamics::CreationRecord* cr)
{
    osg::Node* root = cr->_sceneGraph;
    if(root == NULL){
        osg::notify( osg::WARN ) << "createRigidBody: CreationRecord has NULL scene graph." << std::endl;
        return(NULL);
    }

    osg::BoundingSphere bs = root->getBound();

    osg::Vec3 com;
    if(cr->_comSet) // Use user-specified center of mass.
        com = cr->_com;
    else // Compute from bounding sphere.
        com = bs.center();

    // Create a temporary Transform node containing the center of mass offset and scale vector.
    // Use this as the root of the scene graph for conversion to a collision shape.
    osg::Matrix m(osg::Matrix::translate(-com) * osg::Matrix::scale(cr->_scale));
    osg::ref_ptr<osg::MatrixTransform> tempMtRoot = new osg::MatrixTransform(m);
    tempMtRoot->addChild(root);

    btCollisionShape* shape(NULL);

    CompoundShapeVisitor csv(cr->_shapeType, cr->_marginSet ? cr->_margin : 0., cr->_axis, static_cast<unsigned int>(cr->_reductionLevel));
    tempMtRoot->accept(csv);
    shape = static_cast<btCompoundShape*>(csv.getShape());

    if(shape == NULL){
        std::cerr << "createCompoundRigidBody returned NULL." << std::endl;
        return( NULL );
    }

    return(osgbDynamics::createRigidBody(cr, shape));
}

void VirtualWorld::createObject(const char* filepath,
                                int objectType,
                                int physics,
                                int dof,
                                osgwTools::AbsoluteModelTransform* graphicsParent,
                                osg::Vec3 pos,
                                osg::Vec3 minPos,
                                osg::Vec3 maxPos)
{
    //Translation matrix of the object
    osg::Matrix transform(osg::Matrix::translate(pos.x(), pos.y(), pos.z()));
    if(graphicsParent) transform = graphicsParent->getMatrix() * transform;

    //Transform
    osg::ref_ptr<osgwTools::AbsoluteModelTransform> amt = new osgwTools::AbsoluteModelTransform;
    if(objectType == DYNAMIC_OBJECT)
        amt->setDataVariance(osg::Object::DYNAMIC);
    else
        amt->setDataVariance(osg::Object::STATIC);
    amt->setReferenceFrame(osg::Transform::ABSOLUTE_RF_INHERIT_VIEWPOINT);

    //Get node from osg file
    osg::ref_ptr<osg::Node> modelNode = osgDB::readNodeFile(filepath);
    if(!modelNode.valid()){
        std::cerr << "Can't find " << filepath << std::endl;
        exit(1);
    }
    osg::ref_ptr<osgFX::Scribe> effect = NULL;
    if(physics == PHYSICS){
        effect = new osgFX::Scribe();
        effect->setWireframeColor(black);
        effect->setWireframeLineWidth(2.f);
        effect->setEnabled(false);
        effect->addChild(modelNode.get());
        amt->addChild(effect.get());
    }
    else amt->addChild(modelNode.get());

    //Create physic object from node data
    btRigidBody* body = NULL;
    if(physics == PHYSICS){
        osg::ref_ptr<osgbDynamics::CreationRecord> cr = new osgbDynamics::CreationRecord;
        cr->_sceneGraph = amt.get();
        cr->_shapeType = CONVEX_HULL_SHAPE_PROXYTYPE;
        if(objectType == DYNAMIC_OBJECT){
            cr->_mass = 0.1;
            cr->_restitution = 0.5;
            cr->_friction = 0.5;
        }
        else{
            cr->_mass = 0.;
            cr->_restitution = 0.1;
            cr->_friction = 0.5;
        }
        cr->_parentTransform = transform;
        cr->setMargin(0.001);
        body = createCompoundRigidBody(cr.release());
        if(objectType == DYNAMIC_OBJECT) body->setActivationState(DISABLE_DEACTIVATION);
        body->setName(std::string(filepath));
        body->setType(BT_OTHER);
        body->setObjectId(objCnt++);

        //Reference rigid body to OSG
        amt->setUserData(new osgbCollision::RefRigidBody(body));

        //Add physic object to physic world
        physicsWorld->addRigidBody(body);
    }
    else{
        amt->setMatrix(transform);
    }

    //Add node to osg world
    graphicsWorld->addChild(amt.get());

    //Add object to vector
    objects.push_back(new G2Object(std::string(filepath), body, amt.release(), graphicsParent, dof, osgbCollision::asBtVector3(minPos), osgbCollision::asBtVector3(maxPos)));
    objects[objects.size()-1]->resetPos = osgbCollision::asBtVector3(pos);
    objects[objects.size()-1]->effect = effect.get();
    if(physics == PHYSICS){
        objects[objects.size()-1]->osgToBullet = osgbCollision::asOsgMatrix(objects[objects.size()-1]->body->getWorldTransform()) * osg::Matrix::inverse(objects[objects.size()-1]->representation->getMatrix());
    }
}

G2AngularTarget* VirtualWorld::createAngularTarget(int difficulty,
												   int dof,
                                                   osg::Vec3 normal,
                                                   osg::Vec3 pos,
                                                   double angle,
                                                   double angularLength,
                                                   double targetWidth,
                                                   double targetRadius,
                                                   osg::Vec4 color,
                                                   bool solid,
                                                   bool above)
{
    //Outer geode
    osg::ref_ptr<osg::Material> mat = new osg::Material;
    mat->setDiffuse(osg::Material::FRONT, color);
    osg::ref_ptr<osg::Geode> outerGeode = new osg::Geode;
    outerGeode->getOrCreateStateSet()->setAttribute(mat.get(), osg::StateAttribute::ON);
    {
        osg::ref_ptr<osg::Vec3Array> vertices = new osg::Vec3Array;
        osg::Vec3 center(targetRadius*cos(angle), targetRadius*sin(angle), 0.);
        vertices->push_back(osg::Vec3((targetRadius-(above?0.:targetWidth))*cos(angle+angularLength/2.),
                                      (targetRadius-(above?0.:targetWidth))*sin(angle+angularLength/2.),
                                      0.)
                            -center);
        vertices->push_back(osg::Vec3((targetRadius-(above?0.:targetWidth))*cos(angle),
                                      (targetRadius-(above?0.:targetWidth))*sin(angle),
                                      0.)
                            -center);
        vertices->push_back(osg::Vec3((targetRadius+(above?targetWidth:0.))*cos(angle),
                                      (targetRadius+(above?targetWidth:0.))*sin(angle),
                                      0.)
                            -center);
        vertices->push_back(osg::Vec3((targetRadius+(above?targetWidth:0.))*cos(angle+angularLength/2.),
                                      (targetRadius+(above?targetWidth:0.))*sin(angle+angularLength/2.),
                                      0.)
                            -center);
        vertices->push_back(osg::Vec3((targetRadius-(above?0.:targetWidth))*cos(angle),
                                      (targetRadius-(above?0.:targetWidth))*sin(angle),
                                      0.)
                            -center);
        vertices->push_back(osg::Vec3((targetRadius-(above?0.:targetWidth))*cos(angle-angularLength/2.),
                                      (targetRadius-(above?0.:targetWidth))*sin(angle-angularLength/2.),
                                      0.)
                            -center);
        vertices->push_back(osg::Vec3((targetRadius+(above?targetWidth:0.))*cos(angle-angularLength/2.),
                                      (targetRadius+(above?targetWidth:0.))*sin(angle-angularLength/2.),
                                      0.)
                            -center);
        vertices->push_back(osg::Vec3((targetRadius+(above?targetWidth:0.))*cos(angle),
                                      (targetRadius+(above?targetWidth:0.))*sin(angle),
                                      0.)
                            -center);
        osg::ref_ptr<osg::Vec3Array> normals = new osg::Vec3Array;
        normals->push_back(Z_AXIS);
        osg::ref_ptr<osg::Geometry> quad = new osg::Geometry;
        quad->setVertexArray(vertices.get());
        quad->setNormalArray(normals.get());
        quad->setNormalBinding(osg::Geometry::BIND_OVERALL);
        quad->addPrimitiveSet(new osg::DrawArrays(GL_QUADS, 0, 4));
        quad->addPrimitiveSet(new osg::DrawArrays(GL_QUADS, 4, 4));
        outerGeode->addDrawable(quad.get());
    }
    //Inner geode
    osg::ref_ptr<osg::Geode> innerGeode = NULL;
    if(!solid) {
        osg::ref_ptr<osg::Material> mat2 = new osg::Material;
        mat2->setDiffuse(osg::Material::FRONT, white);
        innerGeode = new osg::Geode;
        innerGeode->getOrCreateStateSet()->setAttribute(mat2.get(), osg::StateAttribute::ON);
        {
            osg::ref_ptr<osg::Vec3Array> vertices = new osg::Vec3Array;
            osg::Vec3 center(targetRadius*cos(angle), targetRadius*sin(angle), 0.);
            vertices->push_back(osg::Vec3((targetRadius-(above?0.:targetWidth)+0.002)*cos(angle+angularLength/2.-M_PI/180.),
                                          (targetRadius-(above?0.:targetWidth)+0.002)*sin(angle+angularLength/2.-M_PI/180.),
                                          0.0005)
                                -center);
            vertices->push_back(osg::Vec3((targetRadius-(above?0.:targetWidth)+0.002)*cos(angle),
                                          (targetRadius-(above?0.:targetWidth)+0.002)*sin(angle),
                                          0.0005)
                                -center);
            vertices->push_back(osg::Vec3((targetRadius+(above?targetWidth:0.)-0.002)*cos(angle),
                                          (targetRadius+(above?targetWidth:0.)-0.002)*sin(angle),
                                          0.0005)
                                -center);
            vertices->push_back(osg::Vec3((targetRadius+(above?targetWidth:0.)-0.002)*cos(angle+angularLength/2.-M_PI/180.),
                                          (targetRadius+(above?targetWidth:0.)-0.002)*sin(angle+angularLength/2.-M_PI/180.),
                                          0.0005)
                                -center);
            vertices->push_back(osg::Vec3((targetRadius-(above?0.:targetWidth)+0.002)*cos(angle),
                                          (targetRadius-(above?0.:targetWidth)+0.002)*sin(angle),
                                          0.0005)
                                -center);
            vertices->push_back(osg::Vec3((targetRadius-(above?0.:targetWidth)+0.002)*cos(angle-angularLength/2.+M_PI/180.),
                                          (targetRadius-(above?0.:targetWidth)+0.002)*sin(angle-angularLength/2.+M_PI/180.),
                                          0.0005)
                                -center);
            vertices->push_back(osg::Vec3((targetRadius+(above?targetWidth:0.)-0.002)*cos(angle-angularLength/2.+M_PI/180.),
                                          (targetRadius+(above?targetWidth:0.)-0.002)*sin(angle-angularLength/2.+M_PI/180.),
                                          0.0005)
                                -center);
            vertices->push_back(osg::Vec3((targetRadius+(above?targetWidth:0.)-0.002)*cos(angle),
                                          (targetRadius+(above?targetWidth:0.)-0.002)*sin(angle),
                                          0.0005)
                                -center);
            osg::ref_ptr<osg::Vec3Array> normals = new osg::Vec3Array;
            normals->push_back(Z_AXIS);
            osg::ref_ptr<osg::Geometry> quad = new osg::Geometry;
            quad->setVertexArray(vertices.get());
            quad->setNormalArray(normals.get());
            quad->setNormalBinding(osg::Geometry::BIND_OVERALL);
            quad->addPrimitiveSet(new osg::DrawArrays(GL_QUADS, 0, 4));
            quad->addPrimitiveSet(new osg::DrawArrays(GL_QUADS, 4, 4));
            innerGeode->addDrawable(quad.get());
        }
    }

    //Add geodes
    osg::ref_ptr<osgwTools::AbsoluteModelTransform> target = new osgwTools::AbsoluteModelTransform;
    target->setDataVariance(osg::Object::STATIC);
    target->setReferenceFrame(osg::Transform::ABSOLUTE_RF_INHERIT_VIEWPOINT);
    osg::Quat rot; rot.makeRotate(Z_AXIS, normal);
    if(normal == X_AXIS) rot = osg::Quat(M_PI/2., osg::Vec3(0., 0., 1.))*rot;
    target->setMatrix(osg::Matrix::rotate(rot)
                      * osg::Matrix::translate(pos));
    target->addChild(outerGeode.get());
    if(!solid) target->addChild(innerGeode.get());
    graphicsWorld->addChild(target.get());

    return new G2AngularTarget(difficulty, dof, target.get(), normal, pos, angle-M_PI/2., angularLength);
}

G2LinearTarget* VirtualWorld::createLinearTarget(int difficulty,
												 int dof,
                                                 osg::Vec3 normal,
                                                 osg::Vec3 pos,
                                                 double length,
                                                 double linearLength,
                                                 double targetWidth,
                                                 osg::Vec4 color,
                                                 bool solid,
                                                 bool above)
{
    //Outer geode
    osg::ref_ptr<osg::Material> mat = new osg::Material;
    mat->setDiffuse(osg::Material::FRONT, color);
    osg::ref_ptr<osg::Geode> outerGeode = new osg::Geode;
    outerGeode->getOrCreateStateSet()->setAttribute(mat.get(), osg::StateAttribute::ON);
    {
        osg::ref_ptr<osg::Vec3Array> vertices = new osg::Vec3Array;
        vertices->push_back(osg::Vec3(-targetWidth/2.-(above?0.02:0.), -linearLength/2., 0.));
        vertices->push_back(osg::Vec3(-targetWidth/2.+(above?0.:0.02), -linearLength/2., 0.));
        vertices->push_back(osg::Vec3(-targetWidth/2.+(above?0.:0.02),  linearLength/2., 0.));
        vertices->push_back(osg::Vec3(-targetWidth/2.-(above?0.02:0.),  linearLength/2., 0.));
        vertices->push_back(osg::Vec3( targetWidth/2.-(above?0.:0.02), -linearLength/2., 0.));
        vertices->push_back(osg::Vec3( targetWidth/2.+(above?0.02:0.), -linearLength/2., 0.));
        vertices->push_back(osg::Vec3( targetWidth/2.+(above?0.02:0.),  linearLength/2., 0.));
        vertices->push_back(osg::Vec3( targetWidth/2.-(above?0.:0.02),  linearLength/2., 0.));
        osg::ref_ptr<osg::Vec3Array> normals = new osg::Vec3Array;
        normals->push_back(Z_AXIS);
        osg::ref_ptr<osg::Geometry> quad = new osg::Geometry;
        quad->setVertexArray(vertices.get());
        quad->setNormalArray(normals.get());
        quad->setNormalBinding(osg::Geometry::BIND_OVERALL);
        quad->addPrimitiveSet(new osg::DrawArrays(GL_QUADS, 0, 4));
        quad->addPrimitiveSet(new osg::DrawArrays(GL_QUADS, 4, 4));
        outerGeode->addDrawable(quad.get());
    }
    //Inner geode
    osg::ref_ptr<osg::Geode> innerGeode = NULL;
    if(!solid){
        osg::ref_ptr<osg::Material> mat2 = new osg::Material;
        mat2->setDiffuse(osg::Material::FRONT, white);
        innerGeode = new osg::Geode;
        innerGeode->getOrCreateStateSet()->setAttribute(mat2.get(), osg::StateAttribute::ON);
        {
            osg::ref_ptr<osg::Vec3Array> vertices = new osg::Vec3Array;
            vertices->push_back(osg::Vec3(-targetWidth/2.-(above?0.02:0.)+0.002, -linearLength/2.+0.002, 0.0005));
            vertices->push_back(osg::Vec3(-targetWidth/2.+(above?0.:0.02)-0.002, -linearLength/2.+0.002, 0.0005));
            vertices->push_back(osg::Vec3(-targetWidth/2.+(above?0.:0.02)-0.002,  linearLength/2.-0.002, 0.0005));
            vertices->push_back(osg::Vec3(-targetWidth/2.-(above?0.02:0.)+0.002,  linearLength/2.-0.002, 0.0005));
            vertices->push_back(osg::Vec3( targetWidth/2.-(above?0.:0.02)+0.002, -linearLength/2.+0.002, 0.0005));
            vertices->push_back(osg::Vec3( targetWidth/2.+(above?0.02:0.)-0.002, -linearLength/2.+0.002, 0.0005));
            vertices->push_back(osg::Vec3( targetWidth/2.+(above?0.02:0.)-0.002,  linearLength/2.-0.002, 0.0005));
            vertices->push_back(osg::Vec3( targetWidth/2.-(above?0.:0.02)+0.002,  linearLength/2.-0.002, 0.0005));
            osg::ref_ptr<osg::Vec3Array> normals = new osg::Vec3Array;
            normals->push_back(Z_AXIS);
            osg::ref_ptr<osg::Geometry> quad = new osg::Geometry;
            quad->setVertexArray(vertices.get());
            quad->setNormalArray(normals.get());
            quad->setNormalBinding(osg::Geometry::BIND_OVERALL);
            quad->addPrimitiveSet(new osg::DrawArrays(GL_QUADS, 0, 4));
            quad->addPrimitiveSet(new osg::DrawArrays(GL_QUADS, 4, 4));
            innerGeode->addDrawable(quad.get());
        }
    }

    //Add geodes
    osg::ref_ptr<osgwTools::AbsoluteModelTransform> target = new osgwTools::AbsoluteModelTransform;
    target->setDataVariance(osg::Object::STATIC);
    target->setReferenceFrame(osg::Transform::ABSOLUTE_RF_INHERIT_VIEWPOINT);
    osg::Quat rot; rot.makeRotate(Z_AXIS, normal);
    if(normal == X_AXIS || normal == -Y_AXIS) rot = osg::Quat(M_PI/2., osg::Vec3(0., 0., 1.))*rot;
    target->setMatrix(osg::Matrix::rotate(rot)
                      * osg::Matrix::translate(pos));
    target->addChild(outerGeode.get());
    if(!solid) target->addChild(innerGeode.get());
    graphicsWorld->addChild(target.get());

    return new G2LinearTarget(difficulty, dof, target.get(), normal, pos, length, linearLength);
}

G2DotTarget* VirtualWorld::createDotTarget(int difficulty,
										   osg::Vec3 normal,
                                   		   osg::Vec3 pos,
                                   		   double dotRadius,
                                   		   osg::Vec4 color,
								   		   bool solid)
{
    //Outer geode
    osg::ref_ptr<osg::Material> mat = new osg::Material;
    mat->setDiffuse(osg::Material::FRONT, color);
    osg::ref_ptr<osg::Geode> outerGeode = new osg::Geode;
    outerGeode->getOrCreateStateSet()->setAttribute(mat.get(), osg::StateAttribute::ON);
    outerGeode->addDrawable(osgwTools::makeCircle(osg::Matrix::translate(0., 0., 0.0005), dotRadius));
    //Inner geode
    osg::ref_ptr<osg::Geode> innerGeode = NULL;
    if(!solid){
        osg::ref_ptr<osg::Material> mat2 = new osg::Material;
        mat2->setDiffuse(osg::Material::FRONT, white);
        innerGeode = new osg::Geode;
        innerGeode->getOrCreateStateSet()->setAttribute(mat2.get(), osg::StateAttribute::ON);
        innerGeode->addDrawable(osgwTools::makeCircle(osg::Matrix::translate(0., 0., 0.001), dotRadius-0.002));
    }

    //Add geodes
    osg::ref_ptr<osgwTools::AbsoluteModelTransform> target = new osgwTools::AbsoluteModelTransform;
    target->setDataVariance(osg::Object::STATIC);
    target->setReferenceFrame(osg::Transform::ABSOLUTE_RF_INHERIT_VIEWPOINT);
    osg::Quat rot; rot.makeRotate(Z_AXIS, normal);
    target->setMatrix(osg::Matrix::rotate(rot)
                      * osg::Matrix::translate(pos));
    target->addChild(outerGeode.get());
    if(!solid) target->addChild(innerGeode.get());
    graphicsWorld->addChild(target.get());

    return new G2DotTarget(difficulty, target.get(), normal, pos, dotRadius);
}

G2ShadowTarget* VirtualWorld::createShadowTarget(int difficulty,
												 osg::Vec3 normal,
                                                 osg::Vec3 pos,
                                                 G2Object* obj,
                                                 double linearLength,
                                                 osg::Vec4 color,
												 bool solid,
                                                 double offset)
{
    osg::ref_ptr<osgwTools::AbsoluteModelTransform> target = new osgwTools::AbsoluteModelTransform;
    target->setDataVariance(osg::Object::STATIC);
    target->setReferenceFrame(osg::Transform::ABSOLUTE_RF_INHERIT_VIEWPOINT);
    std::vector<std::vector<osg::Vec3> > outerContour;
    std::vector<std::vector<osg::Vec3> > innerContour;
    osg::ref_ptr<osg::Geode> outerGeode = computeShadowContour(pos, osg::Quat(0.,0.,0.,1.), obj, linearLength, color, outerContour, innerContour, offset);
    target->addChild(outerGeode.get());
    if(!solid){
        std::vector<std::vector<osg::Vec3> > outerTmp;
        std::vector<std::vector<osg::Vec3> > innerTmp;
        osg::ref_ptr<osg::Geode> innerGeode = computeShadowContour(pos, osg::Quat(0.,0.,0.,1.), obj, linearLength-0.002, white, outerTmp, innerTmp, offset+0.0005);
        target->addChild(innerGeode.get());
    }
    graphicsWorld->addChild(target.get());
    return new G2ShadowTarget(difficulty, target.get(), normal, pos, linearLength, outerContour, innerContour);
}

G2CombinedTarget* VirtualWorld::createCombinedTarget(int difficulty,
													 osg::Vec3 normal,
                                                     osg::Vec3 pos,
                                                     osg::Quat objOrientation,
                                                     G2Object *obj,
                                                     double linearLength,
                                                     double dotRadius,
                                                     osg::Vec4 color,
													 bool solid,
                                                     double offset)
{
    osg::ref_ptr<osgwTools::AbsoluteModelTransform> target = new osgwTools::AbsoluteModelTransform;
    target->setDataVariance(osg::Object::STATIC);
    target->setReferenceFrame(osg::Transform::ABSOLUTE_RF_INHERIT_VIEWPOINT);
    //Add dot target
    {
        osg::ref_ptr<osg::Material> mat = new osg::Material;
        mat->setDiffuse(osg::Material::FRONT, color);
        osg::Vec3 dotPos;
        osg::Vec3 dirVec = objOrientation * Y_AXIS; dirVec.normalize();
        double length;
        if(fabs(normal*dirVec) > 0.0001){
            length = (pos.z()-0.7695-offset)/(-normal*dirVec);
            dotPos = pos + dirVec*length;
        }
        else dotPos = osg::Vec3(0., 0., -1.);
        //Outer dot
        osg::ref_ptr<osg::Geode> outerDotGeode = new osg::Geode;
        outerDotGeode->getOrCreateStateSet()->setAttribute(mat.get(), osg::StateAttribute::ON);
        outerDotGeode->addDrawable(osgwTools::makeCircle(osg::Matrix::translate(dotPos), dotRadius));
        target->addChild(outerDotGeode.get());
        //Inner dot
        if(!solid){
            osg::ref_ptr<osg::Material> mat2 = new osg::Material;
            mat2->setDiffuse(osg::Material::FRONT, white);
            osg::ref_ptr<osg::Geode> innerDotGeode = new osg::Geode;
            innerDotGeode->getOrCreateStateSet()->setAttribute(mat2.get(), osg::StateAttribute::ON);
            innerDotGeode->addDrawable(osgwTools::makeCircle(osg::Matrix::translate(dotPos+osg::Vec3(0., 0., 0.0005)), dotRadius-0.002));
            target->addChild(innerDotGeode.get());
        }
    }

    //Add shadow target
    std::vector<std::vector<osg::Vec3> > outerContour;
    std::vector<std::vector<osg::Vec3> > innerContour;
    {
        osg::ref_ptr<osg::Geode> outerShadowGeode = computeShadowContour(pos, objOrientation, obj, linearLength, color, outerContour, innerContour, offset);
        target->addChild(outerShadowGeode.get());
        if(!solid){
            std::vector<std::vector<osg::Vec3> > outerTmp;
            std::vector<std::vector<osg::Vec3> > innerTmp;
            osg::ref_ptr<osg::Geode> innerShadowGeode = computeShadowContour(pos, objOrientation, obj, linearLength-0.002, white, outerTmp, innerTmp, offset+0.0005);
            target->addChild(innerShadowGeode.get());
        }
    }

    graphicsWorld->addChild(target.get());

    return new G2CombinedTarget(difficulty, target.get(), normal, pos, linearLength, outerContour, innerContour, dotRadius, objOrientation);
}

void VirtualWorld::createScene(int _st)
{
    task = _st;

    //Clear existing objects
    for(int i = 0; i < objects.size(); i++){
        graphicsWorld->removeChild(objects[i]->representation);
        objects[i]->representation.release();
        if(objects[i]->body){
            physicsWorld->removeRigidBody(objects[i]->body);
            delete objects[i]->body;
        }
    }
    objects.clear();
    for(int i = 0; i < targets.size(); i++){
        graphicsWorld->removeChild(targets.at(i).first->target);
        graphicsWorld->removeChild(targets.at(i).second->target);
    }
    targets.clear();
    for(int i = 0; i < neutralPositions.size(); i++){
        graphicsWorld->removeChild(neutralPositions.at(i)->target);
    }
    neutralPositions.clear();
    physicsWorld->setGravity(btVector3(0., 0., 0.));
    light->setPosition(osg::Vec4(0.5, -0.3, 1.5, 1.));
    light->setDirection(osg::Vec3(0., 0., -1.));
    light->setSpotCutoff(85);

    //Recreate scene

    createObject("ControlledGestures/floor.osg.(0., -0.4, 0.).trans"
                 , STATIC_OBJECT
                 , NO_PHYSICS);
    createObject("ControlledGestures/table.osg.(0., 0.25, 0.).trans"
                 , STATIC_OBJECT
                 , NO_PHYSICS);
    createObject("ControlledGestures/footprint.obj.(0., -0.4, 0.002).trans"
                 , STATIC_OBJECT
                 , NO_PHYSICS);
    if(task == ROT1){
		//Objects
        createObject("ControlledGestures/rot1_carter.obj.(0., 0., -90.).rot.(0., 0., 0.75).trans"
                     , STATIC_OBJECT
                     , NO_PHYSICS);
        createObject("ControlledGestures/rot1_c.obj.(90., 0., -90.).rot"
                     , DYNAMIC_OBJECT
                     , PHYSICS
                     , ROT_Y
                     , objects[objects.size()-1]->representation.get()
                     , osg::Vec3(0., 0.1876 , 0.97)
                     , osg::Vec3(0., 0.1876 , 0.97)
                     , osg::Vec3(0., 0.1876 , 0.97));
        createObject("ControlledGestures/rot1_b.obj.(0., 90., -90.).rot"
                     , DYNAMIC_OBJECT
                     , PHYSICS
                     , ROT_X
                     , objects[objects.size()-2]->representation.get()
                     , osg::Vec3(-0.1876, 0., 0.97)
                     , osg::Vec3(-0.1876, 0., 0.97)
                     , osg::Vec3(-0.1876, 0., 0.97));
        createObject("ControlledGestures/rot1_a.obj.(0., 0., 0.).rot"
                     , DYNAMIC_OBJECT
                     , PHYSICS
                     , ROT_Z
                     , objects[objects.size()-3]->representation.get()
                     , osg::Vec3(0., 0., 0.7824)
                     , osg::Vec3(0., 0., 0.7824)
                     , osg::Vec3(0., 0., 0.7824));
		//Targets
        double radius = 0.10;
        double angle1 = M_PI/5., angle2 = M_PI*2.5/4.;
        double width1 = M_PI/4.5, width2 = M_PI/18.;

        targets.push_back(std::make_pair(
                              createAngularTarget(ES, ROT_X, X_AXIS, osg::Vec3(-0.1995, radius*cos(M_PI/2.+angle1), radius*sin(M_PI/2.+angle1)+0.97), M_PI/2.+angle1, width1, 0.02, radius, blue, true, false),
                              createAngularTarget(ES, ROT_X, X_AXIS, osg::Vec3(-0.1995, radius*cos(M_PI/2.-angle1), radius*sin(M_PI/2.-angle1)+0.97), M_PI/2.-angle1, width1, 0.02, radius, blue, false, false)));
        targets.push_back(std::make_pair(
                              createAngularTarget(ES, ROT_Y, -Y_AXIS, osg::Vec3(radius*cos(M_PI/2.-angle1), 0.1995, radius*sin(M_PI/2.-angle1)+0.97), M_PI/2.-angle1, width1, 0.02, radius, blue, false, false),
                              createAngularTarget(ES, ROT_Y, -Y_AXIS, osg::Vec3(radius*cos(M_PI/2.+angle1), 0.1995, radius*sin(M_PI/2.+angle1)+0.97), M_PI/2.+angle1, width1, 0.02, radius, blue, true, false)));
        targets.push_back(std::make_pair(
                              createAngularTarget(ES, ROT_Z, Z_AXIS, osg::Vec3(radius*cos(M_PI/2.+angle1), radius*sin(M_PI/2.+angle1), 0.7705), M_PI/2.+angle1, width1, 0.02, radius, blue, true, false),
                              createAngularTarget(ES, ROT_Z, Z_AXIS, osg::Vec3(radius*cos(M_PI/2.-angle1), radius*sin(M_PI/2.-angle1), 0.7705), M_PI/2.-angle1, width1, 0.02, radius, blue, false, false)));

        targets.push_back(std::make_pair(
                              createAngularTarget(EL, ROT_X, X_AXIS, osg::Vec3(-0.1995, radius*cos(M_PI/2.+angle2), radius*sin(M_PI/2.+angle2)+0.97), M_PI/2.+angle2, width1, 0.02, radius, green, true, false),
                              createAngularTarget(EL, ROT_X, X_AXIS, osg::Vec3(-0.1995, radius*cos(M_PI/2.-angle2), radius*sin(M_PI/2.-angle2)+0.97), M_PI/2.-angle2, width1, 0.02, radius, green, false, false)));
        targets.push_back(std::make_pair(
                              createAngularTarget(EL, ROT_Y, -Y_AXIS, osg::Vec3(radius*cos(M_PI/2.-angle2), 0.1995, radius*sin(M_PI/2.-angle2)+0.97), M_PI/2.-angle2, width1, 0.02, radius, green, false, false),
                              createAngularTarget(EL, ROT_Y, -Y_AXIS, osg::Vec3(radius*cos(M_PI/2.+angle2), 0.1995, radius*sin(M_PI/2.+angle2)+0.97), M_PI/2.+angle2, width1, 0.02, radius, green, true, false)));
        targets.push_back(std::make_pair(
                              createAngularTarget(EL, ROT_Z, Z_AXIS, osg::Vec3(radius*cos(M_PI/2.+angle2), radius*sin(M_PI/2.+angle2), 0.7705), M_PI/2.+angle2, width1, 0.02, radius, green, true, false),
                              createAngularTarget(EL, ROT_Z, Z_AXIS, osg::Vec3(radius*cos(M_PI/2.-angle2), radius*sin(M_PI/2.-angle2), 0.7705), M_PI/2.-angle2, width1, 0.02, radius, green, false, false)));

        targets.push_back(std::make_pair(
                              createAngularTarget(HS, ROT_X, X_AXIS, osg::Vec3(-0.1995, radius*cos(M_PI/2.+angle1), radius*sin(M_PI/2.+angle1)+0.97), M_PI/2.+angle1, width2, 0.02, radius, yellow, true, true),
                              createAngularTarget(HS, ROT_X, X_AXIS, osg::Vec3(-0.1995, radius*cos(M_PI/2.-angle1), radius*sin(M_PI/2.-angle1)+0.97), M_PI/2.-angle1, width2, 0.02, radius, yellow, false, true)));
        targets.push_back(std::make_pair(
                              createAngularTarget(HS, ROT_Y, -Y_AXIS, osg::Vec3(radius*cos(M_PI/2.-angle1), 0.1995, radius*sin(M_PI/2.-angle1)+0.97), M_PI/2.-angle1, width2, 0.02, radius, yellow, false, true),
                              createAngularTarget(HS, ROT_Y, -Y_AXIS, osg::Vec3(radius*cos(M_PI/2.+angle1), 0.1995, radius*sin(M_PI/2.+angle1)+0.97), M_PI/2.+angle1, width2, 0.02, radius, yellow, true, true)));
        targets.push_back(std::make_pair(
                              createAngularTarget(HS, ROT_Z, Z_AXIS, osg::Vec3(radius*cos(M_PI/2.+angle1), radius*sin(M_PI/2.+angle1), 0.7705), M_PI/2.+angle1, width2, 0.02, radius, yellow, true, true),
                              createAngularTarget(HS, ROT_Z, Z_AXIS, osg::Vec3(radius*cos(M_PI/2.-angle1), radius*sin(M_PI/2.-angle1), 0.7705), M_PI/2.-angle1, width2, 0.02, radius, yellow, false, true)));

        targets.push_back(std::make_pair(
                              createAngularTarget(HL, ROT_X, X_AXIS, osg::Vec3(-0.1995, radius*cos(M_PI/2.+angle2), radius*sin(M_PI/2.+angle2)+0.97), M_PI/2.+angle2, width2, 0.02, radius, red, true, true),
                              createAngularTarget(HL, ROT_X, X_AXIS, osg::Vec3(-0.1995, radius*cos(M_PI/2.-angle2), radius*sin(M_PI/2.-angle2)+0.97), M_PI/2.-angle2, width2, 0.02, radius, red, false, true)));
        targets.push_back(std::make_pair(
                              createAngularTarget(HL, ROT_Y, -Y_AXIS, osg::Vec3(radius*cos(M_PI/2.-angle2), 0.1995, radius*sin(M_PI/2.-angle2)+0.97), M_PI/2.-angle2, width2, 0.02, radius, red, false, true),
                              createAngularTarget(HL, ROT_Y, -Y_AXIS, osg::Vec3(radius*cos(M_PI/2.+angle2), 0.1995, radius*sin(M_PI/2.+angle2)+0.97), M_PI/2.+angle2, width2, 0.02, radius, red, true, true)));
        targets.push_back(std::make_pair(
                              createAngularTarget(HL, ROT_Z, Z_AXIS, osg::Vec3(radius*cos(M_PI/2.+angle2), radius*sin(M_PI/2.+angle2), 0.7705), M_PI/2.+angle2, width2, 0.02, radius, red, true, true),
                              createAngularTarget(HL, ROT_Z, Z_AXIS, osg::Vec3(radius*cos(M_PI/2.-angle2), radius*sin(M_PI/2.-angle2), 0.7705), M_PI/2.-angle2, width2, 0.02, radius, red, false, true)));
		//Neutral positions
        neutralPositions.push_back(createAngularTarget(ES, ROT_X, X_AXIS, osg::Vec3(-0.1995, 0., radius+0.97), M_PI/2., M_PI/90., 0.02, radius, black, true, true));
        neutralPositions.push_back(createAngularTarget(ES, ROT_X, X_AXIS, osg::Vec3(-0.1995, 0., radius+0.97), M_PI/2., M_PI/90., 0.02, radius, black, true, false));
        neutralPositions.push_back(createAngularTarget(ES, ROT_Y, -Y_AXIS, osg::Vec3(0., 0.1995, radius+0.97), M_PI/2., M_PI/90., 0.02, radius, black, true, true));
        neutralPositions.push_back(createAngularTarget(ES, ROT_Y, -Y_AXIS, osg::Vec3(0., 0.1995, radius+0.97), M_PI/2., M_PI/90., 0.02, radius, black, true, false));
        neutralPositions.push_back(createAngularTarget(ES, ROT_Z, Z_AXIS, osg::Vec3(0., radius, 0.7705), M_PI/2., M_PI/90., 0.02, radius, black, true, true));
        neutralPositions.push_back(createAngularTarget(ES, ROT_Z, Z_AXIS, osg::Vec3(0., radius, 0.7705), M_PI/2., M_PI/90., 0.02, radius, black, true, false));
    }
    else if(task == TR1){
		//Objects
        createObject("ControlledGestures/tr1_carter.obj.(0., 0., -90.).rot.(0., 0., 0.75).trans"
                     , STATIC_OBJECT
                     , NO_PHYSICS);
        createObject("ControlledGestures/tr1_c.obj.(0., 0., -90.).rot"
                     , DYNAMIC_OBJECT
                     , PHYSICS
                     , TR_X
                     , objects[objects.size()-1]->representation.get()
                     , osg::Vec3(-0.14, 0.182, 0.97)
                     , osg::Vec3(-0.14, 0.182, 0.97)
                     , osg::Vec3(0.14, 0.182, 0.97));
        createObject("ControlledGestures/tr1_b.obj.(0., 0., -90.).rot"
                     , DYNAMIC_OBJECT
                     , PHYSICS
                     , TR_Z
                     , objects[objects.size()-2]->representation.get()
                     , osg::Vec3(-0.182, 0., 0.83)
                     , osg::Vec3(-0.182, 0., 0.83)
                     , osg::Vec3(-0.182, 0., 1.11));
        createObject("ControlledGestures/tr1_a.obj.(0., 0., -90.).rot"
                     , DYNAMIC_OBJECT
                     , PHYSICS
                     , TR_Y
                     , objects[objects.size()-3]->representation.get()
                     , osg::Vec3(0., -0.14, 0.788)
                     , osg::Vec3(0., -0.14, 0.788)
                     , osg::Vec3(0., 0.14, 0.788));
		//Targets
        double length1 = 0.12, length2 = 0.025;
        double width1 = 0.04, width2 = 0.015;
        double height = 0.19;

        targets.push_back(std::make_pair(
                              createLinearTarget(ES, TR_Z, X_AXIS, osg::Vec3(-0.1995, 0., 0.97+length2),  length2, width1, height, blue, true, false),
                              createLinearTarget(ES, TR_Z, X_AXIS, osg::Vec3(-0.1995, 0., 0.97-length2), -length2, width1, height, blue, false, false)));
        targets.push_back(std::make_pair(
                              createLinearTarget(ES, TR_X, -Y_AXIS, osg::Vec3( length2, 0.1995, 0.97),  length2, width1, height, blue, true, false),
                              createLinearTarget(ES, TR_X, -Y_AXIS, osg::Vec3(-length2, 0.1995, 0.97), -length2, width1, height, blue, false, false)));
        targets.push_back(std::make_pair(
                              createLinearTarget(ES, TR_Y, Z_AXIS, osg::Vec3(0.,  length2, 0.7705),  length2, width1, height, blue, true, false),
                              createLinearTarget(ES, TR_Y, Z_AXIS, osg::Vec3(0., -length2, 0.7705), -length2, width1, height, blue, false, false)));

        targets.push_back(std::make_pair(
                              createLinearTarget(EL, TR_Z, X_AXIS, osg::Vec3(-0.1995, 0., 0.97+length1),  length1, width1, height, green, true, false),
                              createLinearTarget(EL, TR_Z, X_AXIS, osg::Vec3(-0.1995, 0., 0.97-length1), -length1, width1, height, green, false, false)));
        targets.push_back(std::make_pair(
                              createLinearTarget(EL, TR_X, -Y_AXIS, osg::Vec3( length1, 0.1995, 0.97),  length1, width1, height, green, true, false),
                              createLinearTarget(EL, TR_X, -Y_AXIS, osg::Vec3(-length1, 0.1995, 0.97), -length1, width1, height, green, false, false)));
        targets.push_back(std::make_pair(
                              createLinearTarget(EL, TR_Y, Z_AXIS, osg::Vec3(0.,  length1, 0.7705),  length1, width1, height, green, true, false),
                              createLinearTarget(EL, TR_Y, Z_AXIS, osg::Vec3(0., -length1, 0.7705), -length1, width1, height, green, false, false)));

        targets.push_back(std::make_pair(
                              createLinearTarget(HS, TR_Z, X_AXIS, osg::Vec3(-0.1995, 0., 0.97+length2),  length2, width2, height, yellow, true, true),
                              createLinearTarget(HS, TR_Z, X_AXIS, osg::Vec3(-0.1995, 0., 0.97-length2), -length2, width2, height, yellow, false, true)));
        targets.push_back(std::make_pair(
                              createLinearTarget(HS, TR_X, -Y_AXIS, osg::Vec3( length2, 0.1995, 0.97),  length2, width2, height, yellow, true, true),
                              createLinearTarget(HS, TR_X, -Y_AXIS, osg::Vec3(-length2, 0.1995, 0.97), -length2, width2, height, yellow, false, true)));
        targets.push_back(std::make_pair(
                              createLinearTarget(HS, TR_Y, Z_AXIS, osg::Vec3(0.,  length2, 0.7705),  length2, width2, height, yellow, true, true),
                              createLinearTarget(HS, TR_Y, Z_AXIS, osg::Vec3(0., -length2, 0.7705), -length2, width2, height, yellow, false, true)));

        targets.push_back(std::make_pair(
                              createLinearTarget(HL, TR_Z, X_AXIS, osg::Vec3(-0.1995, 0., 0.97+length1),  length1, width2, height, red, true, true),
                              createLinearTarget(HL, TR_Z, X_AXIS, osg::Vec3(-0.1995, 0., 0.97-length1), -length1, width2, height, red, false, true)));
        targets.push_back(std::make_pair(
                              createLinearTarget(HL, TR_X, -Y_AXIS, osg::Vec3( length1, 0.1995, 0.97),  length1, width2, height, red, true, true),
                              createLinearTarget(HL, TR_X, -Y_AXIS, osg::Vec3(-length1, 0.1995, 0.97), -length1, width2, height, red, false, true)));
        targets.push_back(std::make_pair(
                              createLinearTarget(HL, TR_Y, Z_AXIS, osg::Vec3(0.,  length1, 0.7705),  length1, width2, height, red, true, true),
                              createLinearTarget(HL, TR_Y, Z_AXIS, osg::Vec3(0., -length1, 0.7705), -length1, width2, height, red, false, true)));
		//Neutral positions
        neutralPositions.push_back(createLinearTarget(ES, TR_Z, X_AXIS, osg::Vec3(-0.1995, 0., 0.97), 0., 0.002, height, black, true, true));
        neutralPositions.push_back(createLinearTarget(ES, TR_Z, X_AXIS, osg::Vec3(-0.1995, 0., 0.97), 0., 0.002, height, black, true, false));
        neutralPositions.push_back(createLinearTarget(ES, TR_X, -Y_AXIS, osg::Vec3(0., 0.1995, 0.97), 0., 0.002, height, black, true, true));
        neutralPositions.push_back(createLinearTarget(ES, TR_X, -Y_AXIS, osg::Vec3(0., 0.1995, 0.97), 0., 0.002, height, black, true, false));
        neutralPositions.push_back(createLinearTarget(ES, TR_Y, Z_AXIS, osg::Vec3(0., 0., 0.7705), 0., 0.002, height, black, true, true));
        neutralPositions.push_back(createLinearTarget(ES, TR_Y, Z_AXIS, osg::Vec3(0., 0., 0.7705), 0., 0.002, height, black, true, false));
    }
    else if(task == ROT3){
//        light->setPosition(osg::Vec4(.1, -0.1, 1.5, 1.));
//        light->setDirection(osg::Vec3(-1., 1., -.5));
		//Objects
        createObject("ControlledGestures/rot3_carter.obj.(0., 0., -90.).rot.(0., 0., 0.75).trans"
                     , STATIC_OBJECT
                     , NO_PHYSICS);
        createObject("ControlledGestures/rot3_c.obj.(0., 0., -90.).rot"
                     , DYNAMIC_OBJECT
                     , NO_PHYSICS
                     , ROT_Z
                     , objects[objects.size()-1]->representation.get()
                     , osg::Vec3(0., 0., 0.8494)
                     , osg::Vec3(0., 0., 0.8494)
                     , osg::Vec3(0., 0., 0.8494));
        createObject("ControlledGestures/rot3_b.obj.(0., 0., -90.).rot"
                     , DYNAMIC_OBJECT
                     , NO_PHYSICS
                     , ROT_X
                     , objects[objects.size()-1]->representation.get()
                     , osg::Vec3(0., 0., 0.226)
                     , osg::Vec3(0., 0., 0.226)
                     , osg::Vec3(0., 0., 0.226));
        createObject("ControlledGestures/rot3_a.obj.(0., 0., -90.).rot"
                     , DYNAMIC_OBJECT
                     , PHYSICS
                     , ROT_Y
                     , objects[objects.size()-1]->representation.get());
		//Targets
        double radius1 = 0.04, radius2 = 0.02;
        targets.push_back(std::make_pair(
                              createDotTarget(ES, -Y_AXIS, osg::Vec3(-0.25, 0.382, 0.95), radius1, blue, true),
                              createDotTarget(ES, X_AXIS, osg::Vec3(-0.382, 0.25, 1.05), radius1, blue, false)));
        targets.push_back(std::make_pair(
                              createDotTarget(EL, X_AXIS, osg::Vec3(-0.382, 0., 1.2), radius1, green, true),
                              createDotTarget(EL, Z_AXIS, osg::Vec3(0., 0.3, 0.77), radius1, green, false)));
        targets.push_back(std::make_pair(
                              createDotTarget(HS, X_AXIS, osg::Vec3(-0.382, 0.25, 0.95), radius2, yellow, true),
                              createDotTarget(HS, -Y_AXIS, osg::Vec3(-0.25, 0.382, 1.05), radius2, yellow, false)));
        targets.push_back(std::make_pair(
                              createDotTarget(HL, -Y_AXIS, osg::Vec3(0., 0.382, 1.2), radius2, red, true),
                              createDotTarget(HL, Z_AXIS, osg::Vec3(-0.3, -0., 0.77), radius2, red, false)));
		//Neutral position
        neutralPositions.push_back(createDotTarget(ES, Z_AXIS, osg::Vec3(-0.362, 0.362, 0.77), 0.02, black, true));
    }
    else if(task == TR3){
        //Place light
        light->setPosition(osg::Vec4(0.24, 0., 1.15, 1.));
        light->setDirection(osg::Vec3(-1., 0., 0.));
        //Move table
        objects[objects.size()-1]->representation->setMatrix(osg::Matrix::translate(0., -0.2, 0.));

		//Objects
        createObject("ControlledGestures/tr3_carter.obj.(-90., 0., -90.).rot.(-0.258, 0., 1.15).trans"
                     , STATIC_OBJECT
                     , NO_PHYSICS
                     , NONE
                     , NULL);
        createObject("ControlledGestures/tr3_c.obj.(0., -90., 0.).rot.(0., 0., 1.15).trans"
                     , DYNAMIC_OBJECT
                     , NO_PHYSICS
                     , TR_X
                     , objects[objects.size()-1]->representation.get()
                     , osg::Vec3(-0.188, 0., 0.)
                     , osg::Vec3(-0.188, 0., 0.)
                     , osg::Vec3(0.188, 0., 0.));
        createObject("ControlledGestures/tr3_b.obj.(0., -90., 0.).rot.(0., 0., 1.15).trans"
                     , DYNAMIC_OBJECT
                     , NO_PHYSICS
                     , TR_Y
                     , objects[objects.size()-1]->representation.get()
                     , osg::Vec3(0., -0.188, 0.)
                     , osg::Vec3(0., -0.188, 0.)
                     , osg::Vec3(0., 0.188, 0.));
        createObject("ControlledGestures/tr3_a.obj.(0., -90., 0.).rot"
                     , DYNAMIC_OBJECT
                     , PHYSICS
                     , TR_Z
                     , objects[objects.size()-1]->representation.get()
                     , osg::Vec3(0., 0., -0.188+1.15)
                     , osg::Vec3(0., 0., -0.188+1.15)
                     , osg::Vec3(0., 0., 0.188+1.15));
        //Add dots on movable object to place fingers
        {
            osg::ref_ptr<osg::Material> mat = new osg::Material;
            mat->setDiffuse(osg::Material::FRONT, osg::Vec4(1., 1., 1., 1.));
            osg::ref_ptr<osg::Geode> finger1 = new osg::Geode;
            finger1->getOrCreateStateSet()->setAttribute(mat.get(), osg::StateAttribute::ON);
            finger1->addDrawable(osgwTools::makeCircle(osg::Matrix::rotate(osg::Quat(M_PI/2., osg::Vec3(1., 0., 0.))) * osg::Matrix::translate(0.0265, -0.0345, -0.0265), 0.0075));
            objects[objects.size()-1]->representation.get()->addChild(finger1.get());
            osg::ref_ptr<osg::Geode> finger2 = new osg::Geode;
            finger2->getOrCreateStateSet()->setAttribute(mat.get(), osg::StateAttribute::ON);
            finger2->addDrawable(osgwTools::makeCircle(osg::Matrix::rotate(osg::Quat(-M_PI/2., osg::Vec3(1., 0., 0.))) * osg::Matrix::translate(0.0265, 0.0345, -0.0265), 0.0075));
            objects[objects.size()-1]->representation.get()->addChild(finger2.get());
        }

		//Targets
        double error1 = 0.02, error2 = 0.05;
        targets.push_back(std::make_pair(
                              createShadowTarget(ES, X_AXIS, osg::Vec3(-0.17, 0.045, 1.15-0.075), objects[objects.size()-1], error2, blue, true),
                              createShadowTarget(ES, X_AXIS, osg::Vec3(-0.14, -0.035, 1.15+0.025), objects[objects.size()-1], error2, blue, false)));
        targets.push_back(std::make_pair(
                              createShadowTarget(EL, X_AXIS, osg::Vec3(-0.15, 0.10, 1.15+0.10), objects[objects.size()-1], error2, green, true),
                              createShadowTarget(EL, X_AXIS, osg::Vec3(-0.07, -0.10, 1.15-0.10), objects[objects.size()-1], error2, green, false)));
        targets.push_back(std::make_pair(
                              createShadowTarget(HS, X_AXIS, osg::Vec3(-0.14, -0.035, 1.15-0.075), objects[objects.size()-1], error1, yellow, true, 0.0015),
                              createShadowTarget(HS, X_AXIS, osg::Vec3(-0.17, 0.045, 1.15+0.025), objects[objects.size()-1], error1, yellow, false, 0.0015)));
        targets.push_back(std::make_pair(
                              createShadowTarget(HL, X_AXIS, osg::Vec3(-0.15, -0.10, 1.15+0.10), objects[objects.size()-1], error1, red, true, 0.0015),
                              createShadowTarget(HL, X_AXIS, osg::Vec3(-0.07, 0.10, 1.15-0.10), objects[objects.size()-1], error1, red, false, 0.0015)));
		//Neutral position
        neutralPositions.push_back(createShadowTarget(ES, X_AXIS, osg::Vec3(-0.188, 0., 1.15), objects[objects.size()-1], 0.001, black, true, 0.0025));
    }
    else if(task == DOF6){
        light->setPosition(osg::Vec4(0., 0., 1.145, 1.));
		//Object
        createObject("ControlledGestures/plank.obj.(0., 0., 0.75).trans.(0., 0., 180.).rot"
                     , STATIC_OBJECT
                     , NO_PHYSICS);
        createObject("ControlledGestures/arrow.obj.(0., 180., 180.).rot"
                     , DYNAMIC_OBJECT
                     , PHYSICS
                     , ALL_DOF
                     , NULL
                     , osg::Vec3(-0.2, 0., 0.7932)
                     , osg::Vec3(-0.2, -0.14, 0.793)
                     , osg::Vec3(0.2, 0.14, 1.107));

		//Targets
        double shadowError1 = 0.015, shadowError2 = 0.04;
        double dotError1 = 0.02, dotError2 = 0.04;
        targets.push_back(std::make_pair(
                              createCombinedTarget(HS, Z_AXIS, osg::Vec3(-0.15, 0.08, 0.84),
                                                   osg::Quat(-M_PI/4., osg::Vec3(1., 0., 0.)),
                                                   objects[objects.size()-1], shadowError2, dotError2, red, true),
                              createCombinedTarget(HS, Z_AXIS, osg::Vec3(0.16, -0.1, 0.82),
                                                   osg::Quat(-M_PI/2., osg::Vec3(1., 0., 0.)),
                                                   objects[objects.size()-1], shadowError2, dotError2, red, false)));
        targets.push_back(std::make_pair(
                              createCombinedTarget(ES, Z_AXIS, osg::Vec3(-0.12, -0.08, 0.87),
                                                   osg::Quat(-M_PI/2., osg::Vec3(0., 0., 1.))*osg::Quat(M_PI/3., osg::Vec3(0., 1., 0.)),
                                                   objects[objects.size()-1], shadowError1, dotError1, yellow, true),
                              createCombinedTarget(ES, Z_AXIS, osg::Vec3(0.12, 0.08, 0.87),
                                                   osg::Quat(M_PI/2., osg::Vec3(0., 0., 1.))*osg::Quat(-M_PI/3., osg::Vec3(0., 1., 0.)),
                                                   objects[objects.size()-1], shadowError1, dotError1, yellow, false)));
//        targets.push_back(std::make_pair(
//                              createCombinedTarget(Z_AXIS, osg::Vec3(-0.05, 0.05, 0.95), osg::Quat(-M_PI/4., osg::Vec3(1., 0.5, 0.)), objects[objects.size()-1], shadowError1, dotError1, osg::Vec4(0., 0., 1., 1.), true),
//                              createCombinedTarget(Z_AXIS, osg::Vec3(0.15, -0.10, 0.85), osg::Quat(-M_PI/4., osg::Vec3(1., 0.5, 0.)), objects[objects.size()-1], shadowError1, dotError1, osg::Vec4(0., 0., 1., 1.), false)));
//        targets.push_back(std::make_pair(
//                              createCombinedTarget(Z_AXIS, osg::Vec3(-0.05, 0.05, 0.95), osg::Quat(-M_PI/4., osg::Vec3(1., 0.5, 0.)), objects[objects.size()-1], shadowError2, dotError2, osg::Vec4(1., 1., 0., 1.), true),
//                              createCombinedTarget(Z_AXIS, osg::Vec3(0.15, -0.10, 0.85), osg::Quat(-M_PI/4., osg::Vec3(1., 0.5, 0.)), objects[objects.size()-1], shadowError2, dotError2, osg::Vec4(1., 1., 0., 1.), false)));
        //Neutral position
        neutralPositions.push_back(createShadowTarget(ES, Z_AXIS, osg::Vec3(-0.2, 0., 0.7932), objects[objects.size()-1], 0.001, black, true, 0.0025));
    }
}

//Valid for OBJ files only!!!
osg::Geode* VirtualWorld::computeShadowContour(osg::Vec3 pos,
                                               osg::Quat dir,
                                               G2Object* obj,
                                               double linearLength,
                                               osg::Vec4 color,
                                               std::vector<std::vector<osg::Vec3> > &outerContour,
                                               std::vector<std::vector<osg::Vec3> > &innerContour,
                                               double offset)
{
    std::string filename("/user/echapoul/home/sync/OpenSceneGraph-Data/");
    filename.append(obj->name);
    filename.resize(filename.find(".("));
    std::ifstream file(filename);
    if(!file.is_open()){
        std::cout << "Couldn't open file " << filename << std::endl;
        return NULL;
    }
    //Get object transformations
    osg::Matrix transform; transform.makeIdentity();
    size_t transPos = obj->name.find(").trans");
    size_t rotPos = obj->name.find(").rot");
    if(transPos != std::string::npos){ //The object is translated
        size_t charPos;
        if(rotPos < transPos) charPos = obj->name.find(".(", rotPos);
        else charPos = obj->name.find(".(");
        std::stringstream val(obj->name.substr(charPos+2));
        double x, y, z; char c;
        val >> x >> c >> y >> c >> z;
        transform.makeTranslate(x, y, z);
    }
    if(rotPos != std::string::npos){ //The object is rotated
        size_t charPos;
        if(transPos < rotPos) charPos = obj->name.find(".(", transPos);
        else charPos = obj->name.find(".(");
        std::stringstream val(obj->name.substr(charPos+2));
        double x, y, z; char c;
        val >> x >> c >> y >> c >> z;
        transform = osg::Matrix::rotate(osg::Quat(x*M_PI/180., X_AXIS, y*M_PI/180., Y_AXIS, z*M_PI/180., Z_AXIS)) * transform;
    }
    //Read vertices, normals, and faces
    std::vector<osg::Vec3> vertices;
    std::vector<osg::Vec3> normals;
    std::vector<osg::Vec4> facesV;
    std::vector<osg::Vec4> facesN;
    while(!file.eof()){
        std::string line;
        std::getline(file, line);
        if(line.find("v ") != std::string::npos){//found vertex
            std::stringstream stream(line.substr(2));
            double x, y, z;
            stream >> x >> y >> z;
            vertices.push_back(transform * osg::Vec3(x, -z, y));
        }
        if(line.find("vn ") != std::string::npos){//found normal
            std::stringstream stream(line.substr(3));
            double x, y, z;
            stream >> x >> y >> z;
            normals.push_back(transform * osg::Vec3(x, -z, y));
        }
        if(line.find("f ") != std::string::npos){//found face
            std::stringstream stream(line.substr(2));
            int xv = -1, yv = -1, zv = -1, wv = -1;
            int xn = -1, yn = -1, zn = -1, wn = -1;
            char c; int i;
            stream >> xv >> c >> i >> c >> xn;
            stream >> yv >> c >> i >> c >> yn;
            stream >> zv >> c >> i >> c >> zn;
            if(!stream.eof()) stream >> wv >> c >> i >> c >> wn;
            facesV.push_back(osg::Vec4(xv, yv, zv, wv));
            facesN.push_back(osg::Vec4(xn, yn, zn, wn));
        }
    }
    file.close();

    //Compute shadow contours

    //Transform vertices and normals
    std::vector<osg::Vec3> tempVertices;
    std::vector<osg::Vec3> tempNormals;
    for(int i = 0; i < vertices.size(); i++)
        tempVertices.push_back(pos + osg::Matrix::inverse(osg::Matrix::rotate(dir)) * vertices.at(i));
    for(int i = 0; i < normals.size(); i++)
        tempNormals.push_back(osg::Matrix::inverse(osg::Matrix::rotate(dir)) * normals.at(i));
    //Identify visible faces from the light
    osg::Vec3 lightPos(light->getPosition().x(), light->getPosition().y(), light->getPosition().z());
    std::vector<osg::Vec4> visibleFaces;
    for(int i = 0; i < facesV.size(); i++){
        //Compute average face position and normal
        osg::Vec3 p = (tempVertices.at(facesV.at(i).x()-1) +
                tempVertices.at(facesV.at(i).y()-1) +
                tempVertices.at(facesV.at(i).z()-1) +
                ((facesV.at(i).w()>-1)?tempVertices.at(facesV.at(i).w()-1):osg::Vec3(0., 0., 0.)))/
                (facesV.at(i).w()>-1?4.:3.);
        osg::Vec3 n = (tempNormals.at(facesN.at(i).x()-1) +
                tempNormals.at(facesN.at(i).y()-1) +
                tempNormals.at(facesN.at(i).z()-1) +
                ((facesV.at(i).w()>-1)?tempNormals.at(facesN.at(i).w()-1):osg::Vec3(0., 0., 0.)))/
                (facesV.at(i).w()>-1?4.:3.);
        //Compute incident lightdir
        osg::Vec3 incidentLightDir = p - lightPos;
        //Store face if visible
        if(incidentLightDir*n < 0.){
            visibleFaces.push_back(facesV.at(i));
        }
    }
    /*std::cout << "Visible faces: " << visibleFaces.size() << " (out of " << facesV.size() << " faces)" << std::endl;*/

    //Get projected edges
    std::vector<std::vector<osg::Vec3> > projectedEdges;
    for(int i = 0; i < visibleFaces.size(); i++){
        //Compute projected vertices
        float alpha = 1.;
        //Vertex 1
        osg::Vec3 vertex1 = tempVertices.at(visibleFaces.at(i).x()-1);
        if(task == TR3) alpha = (-0.24+offset-lightPos.x())/(vertex1.x()-lightPos.x());
        else alpha = (0.768+offset-lightPos.z())/(vertex1.z()-lightPos.z());
        vertex1 = lightPos+(vertex1-lightPos)*alpha;
        //Vertex 2
        osg::Vec3 vertex2 = tempVertices.at(visibleFaces.at(i).y()-1);
        if(task == TR3) alpha = (-0.24+offset-lightPos.x())/(vertex2.x()-lightPos.x());
        else alpha = (0.768+offset-lightPos.z())/(vertex2.z()-lightPos.z());
        vertex2 = lightPos+(vertex2-lightPos)*alpha;
        //Vertex 3
        osg::Vec3 vertex3 = tempVertices.at(visibleFaces.at(i).z()-1);
        if(task == TR3) alpha = (-0.24+offset-lightPos.x())/(vertex3.x()-lightPos.x());
        else alpha = (0.768+offset-lightPos.z())/(vertex3.z()-lightPos.z());
        vertex3 = lightPos+(vertex3-lightPos)*alpha;
        //Vertex 4 if exists
        osg::Vec3 vertex4;
        if(visibleFaces.at(i).w() > -1){
            vertex4 = tempVertices.at(visibleFaces.at(i).w()-1);
            if(task == TR3) alpha = (-0.24+offset-lightPos.x())/(vertex4.x()-lightPos.x());
            else alpha = (0.768+offset-lightPos.z())/(vertex4.z()-lightPos.z());
            vertex4 = lightPos+(vertex4-lightPos)*alpha;
        }
        //Get edges
        //An edge will be defined as a start point, an end point, and a middle point
        //Edge 1
        {
            std::vector<osg::Vec3> edge;
            edge.push_back(vertex1);
            edge.push_back(vertex2);
            edge.push_back((vertex1+vertex2)/2.);
            projectedEdges.push_back(edge);
        }
        //Edge 2
        {
            std::vector<osg::Vec3> edge;
            edge.push_back(vertex2);
            edge.push_back(vertex3);
            edge.push_back((vertex2+vertex3)/2.);
            projectedEdges.push_back(edge);
        }
        //Edge 3
        {
            std::vector<osg::Vec3> edge;
            edge.push_back(vertex3);
            if(visibleFaces.at(i).w() > -1){
                edge.push_back(vertex4);
                edge.push_back((vertex3+vertex4)/2.);
            }
            else{
                edge.push_back(vertex1);
                edge.push_back((vertex3+vertex1)/2.);
            }
            projectedEdges.push_back(edge);
        }
        //Edge 4 if exists
        {
            if(visibleFaces.at(i).w() > -1){
                std::vector<osg::Vec3> edge;
                edge.push_back(vertex4);
                edge.push_back(vertex1);
                edge.push_back((vertex4+vertex1)/2.);
                projectedEdges.push_back(edge);
            }
        }
    }
    /*std::cout << "Projected edges: " << projectedEdges.size() << std::endl;*/

    //Get contour edges (their middle point belongs to only 1 projected face)
    std::vector<std::vector<osg::Vec3> > contourEdges;
    std::set<osg::Vec3> multipleMiddles;
    for(int i = 0; i < projectedEdges.size(); i++){
        osg::Vec3 middle = (projectedEdges.at(i)).at(2);
        if(multipleMiddles.find(middle) == multipleMiddles.end() ||
                !multipleMiddles.size()){ //First encounter with middle
            bool multiple = false;
            for(int j = 0; j < projectedEdges.size(); j++){
                if(i != j){
                    osg::Vec3 middle2 = (projectedEdges.at(j)).at(2);
                    if((middle2-middle).length() < 0.000001){
                        multiple = true;
                        multipleMiddles.insert(middle2);
                        break;
                    }
                }
            }
            if(!multiple) contourEdges.push_back(projectedEdges.at(i));
        }
    }
    /*std::cout << "Contour edges: " << contourEdges.size() << std::endl;*/

    //Get reduced contour edges list
    std::vector<std::vector<osg::Vec3> > reducedContourEdges;
    int i = 0;
    while(contourEdges.size() && i > -1){
        osg::Vec3 stop = (contourEdges.at(i)).at(1);
        reducedContourEdges.push_back(contourEdges.at(i));
        contourEdges.erase(contourEdges.begin()+i);
		i = -1;
        for(int j = 0; j < contourEdges.size(); j++){
            if(((contourEdges.at(j)).at(0)-stop).length() < 0.000001){
                i = j;
                break;
            }
        }
    }
	/*std::cout << "Unused contour edges: " << contourEdges.size() << std::endl;
    std::cout << "Used contour edges: " << reducedContourEdges.size() << std::endl;
	for(int i = 0; i < reducedContourEdges.size(); i++){
		std::cout << "\t"
				  << reducedContourEdges.at(i).at(0).x() << " "
				  << reducedContourEdges.at(i).at(0).y() << " "
				  << reducedContourEdges.at(i).at(0).z() << std::endl;
		std::cout << "\t"
				  << reducedContourEdges.at(i).at(1).x() << " "
				  << reducedContourEdges.at(i).at(1).y() << " "
				  << reducedContourEdges.at(i).at(1).z() << std::endl;
		std::cout << "\t---------------------------------------------------" << std::endl;
	}*/

    //Draw extended contour
    osg::Geode* geode = new osg::Geode();
    osg::Vec3Array* normalsArray = new osg::Vec3Array;
    normalsArray->push_back(osg::Vec3((task==TR3)?1.:0., 0., (task!=TR3)?1.:0.));
    osg::Vec4Array* colorsArray = new osg::Vec4Array;
    colorsArray->push_back(color);
	//DEBUG: draw contour as lines
//	{
//    	int j = 0;
//    	for(int i = 0; i < contourEdges.size(); i++){
//    	    osg::ref_ptr<osg::Vec3Array> vertexArray = new osg::Vec3Array;
//    	    vertexArray->push_back(contourEdges.at(i).at(0)/*+osg::Vec3(0.0005*j, 0., 0.)*/);
//    	    vertexArray->push_back(contourEdges.at(i).at(1)/*+osg::Vec3(0.0005*j, 0., 0.)*/);
//    	    osg::ref_ptr<osg::DrawElementsUInt> face = new osg::DrawElementsUInt(osg::PrimitiveSet::LINES, 0);
//    	    face->push_back(0);
//    	    face->push_back(1);
//    	    osg::ref_ptr<osg::Geometry> geom = new osg::Geometry;
//    	    geom->setVertexArray(vertexArray.get());
//    	    geom->addPrimitiveSet(face.get());
//    	    geom->setColorArray(colorsArray);
//    	    geom->setColorBinding(osg::Geometry::BIND_OVERALL);
//    	    geom->setNormalArray(normalsArray);
//    	    geom->setNormalBinding(osg::Geometry::BIND_OVERALL);
//	        geode->addDrawable(geom.get());
//    	}
//	}
    for(int i = 0; i < reducedContourEdges.size(); i++){
        //Edge + vertices before and after
        osg::Vec3 v1, v2, v3, v4;
        v1 = reducedContourEdges.at((i==0)?reducedContourEdges.size()-1:i-1).at(0);
        v2 = reducedContourEdges.at(i).at(0);
        v3 = reducedContourEdges.at(i).at(1);
        v4 = reducedContourEdges.at((i+1==reducedContourEdges.size())?0:i+1).at(1);
		/*std::cout << "Edge " << i << " uses: " << std::endl;
		std::cout << "\t" << v1.x() << " " << v1.y() << " " << v1.z() << std::endl;
		std::cout << "\t" << v2.x() << " " << v2.y() << " " << v2.z() << std::endl;
		std::cout << "\t" << v3.x() << " " << v3.y() << " " << v3.z() << std::endl;
		std::cout << "\t" << v4.x() << " " << v4.y() << " " << v4.z() << std::endl;
		std::cout << "\t---------------------------------------------------" << std::endl;*/
        //Projected vertices at distance linearLength/2.
        osg::Vec3 maxv1_1, maxv2_1, maxv2_2, maxv3_1, maxv3_2, maxv4_1;
        osg::Vec3 minv1_1, minv2_1, minv2_2, minv3_1, minv3_2, minv4_1;
        osg::Vec3 tmp;
        tmp = osg::Quat(M_PI/2., osg::Vec3((task==TR3)?1.:0., 0., (task!=TR3)?1.:0.)) * (v2-v1); tmp.normalize();
        maxv1_1 = v1+tmp*linearLength/2.;
        minv1_1 = v1-tmp*linearLength/2.;
        maxv2_1 = v2+tmp*linearLength/2.;
        minv2_1 = v2-tmp*linearLength/2.;
        tmp = osg::Quat(M_PI/2., osg::Vec3((task==TR3)?1.:0., 0., (task!=TR3)?1.:0.)) * (v3-v2); tmp.normalize();
        maxv2_2 = v2+tmp*linearLength/2.;
        minv2_2 = v2-tmp*linearLength/2.;
        maxv3_1 = v3+tmp*linearLength/2.;
        minv3_1 = v3-tmp*linearLength/2.;
        tmp = osg::Quat(M_PI/2., osg::Vec3((task==TR3)?1.:0., 0., (task!=TR3)?1.:0.)) * (v4-v3); tmp.normalize();
        maxv3_2 = v3+tmp*linearLength/2.;
        minv3_2 = v3-tmp*linearLength/2.;
        maxv4_1 = v4+tmp*linearLength/2.;
        minv4_1 = v4-tmp*linearLength/2.;
        //Intersection of projected lines
        osg::Vec3 minv2, maxv2, minv3, maxv3;
        double t; osg::Vec3 A, B, C, D;
        {
            A = maxv1_1;
            B = maxv2_1;
            C = maxv2_2;
            D = maxv3_1;
			if((B-C).length() > 0.000001){
            	if(task==TR3)
                	t = ((C.y()-A.y())*(D.z()-C.z())+(D.y()-C.y())*(A.z()-C.z()))/
                	        ((B.y()-A.y())*(D.z()-C.z())-(D.y()-C.y())*(B.z()-A.z()));
            	else
                	t = ((C.x()-A.x())*(D.y()-C.y())+(D.x()-C.x())*(A.y()-C.y()))/
                	        ((B.x()-A.x())*(D.y()-C.y())-(D.x()-C.x())*(B.y()-A.y()));
	            maxv2 = A + (B-A) * t;
			}
			else{
				maxv2 = B;
			}
        }
        {
            A = minv1_1;
            B = minv2_1;
            C = minv2_2;
            D = minv3_1;
			if((B-C).length() > 0.000001){
            	if(task==TR3)
                	t = ((C.y()-A.y())*(D.z()-C.z())+(D.y()-C.y())*(A.z()-C.z()))/
                	        ((B.y()-A.y())*(D.z()-C.z())-(D.y()-C.y())*(B.z()-A.z()));
            	else
                	t = ((C.x()-A.x())*(D.y()-C.y())+(D.x()-C.x())*(A.y()-C.y()))/
                	        ((B.x()-A.x())*(D.y()-C.y())-(D.x()-C.x())*(B.y()-A.y()));
            minv2 = A + (B-A) * t;
			}
			else{
				minv2 = B;
			}
        }
        {
            A = maxv2_2;
            B = maxv3_1;
            C = maxv3_2;
            D = maxv4_1;
			if((B-C).length() > 0.000001){
	            if(task==TR3)
    	            t = ((C.y()-A.y())*(D.z()-C.z())+(D.y()-C.y())*(A.z()-C.z()))/
    	                    ((B.y()-A.y())*(D.z()-C.z())-(D.y()-C.y())*(B.z()-A.z()));
    	        else
    	            t = ((C.x()-A.x())*(D.y()-C.y())+(D.x()-C.x())*(A.y()-C.y()))/
    	                    ((B.x()-A.x())*(D.y()-C.y())-(D.x()-C.x())*(B.y()-A.y()));
    	        maxv3 = A + (B-A) * t;
			}
			else{
				maxv3 = B;
			}
        }
        {
            A = minv2_2;
            B = minv3_1;
            C = minv3_2;
            D = minv4_1;
			if((B-C).length() > 0.000001){
	            if(task==TR3)
    	            t = ((C.y()-A.y())*(D.z()-C.z())+(D.y()-C.y())*(A.z()-C.z()))/
    	                    ((B.y()-A.y())*(D.z()-C.z())-(D.y()-C.y())*(B.z()-A.z()));
    	        else
    	            t = ((C.x()-A.x())*(D.y()-C.y())+(D.x()-C.x())*(A.y()-C.y()))/
    	                    ((B.x()-A.x())*(D.y()-C.y())-(D.x()-C.x())*(B.y()-A.y()));
    	        minv3 = A + (B-A) * t;
			}
			else{
				minv3 = B;
			}
        }
        //Resulting face
        osg::ref_ptr<osg::Vec3Array> vertexArray = new osg::Vec3Array;
        vertexArray->push_back(maxv3);
        vertexArray->push_back(maxv2);
        vertexArray->push_back(minv2);
        vertexArray->push_back(minv3);
        osg::ref_ptr<osg::DrawElementsUInt> face = new osg::DrawElementsUInt(osg::PrimitiveSet::QUADS, 0);
        face->push_back(0);
        face->push_back(1);
        face->push_back(2);
        face->push_back(3);
        osg::ref_ptr<osg::Geometry> geom = new osg::Geometry;
        geom->setVertexArray(vertexArray.get());
        geom->addPrimitiveSet(face.get());
        geom->setColorArray(colorsArray);
        geom->setColorBinding(osg::Geometry::BIND_OVERALL);
        geom->setNormalArray(normalsArray);
        geom->setNormalBinding(osg::Geometry::BIND_OVERALL);
        geode->addDrawable(geom.get());
        //Store contours
        std::vector<osg::Vec3> outerEdge, innerEdge;
        outerEdge.push_back(minv3);
        outerEdge.push_back(minv2);
        innerEdge.push_back(maxv3);
        innerEdge.push_back(maxv2);
        outerContour.push_back(outerEdge);
        innerContour.push_back(innerEdge);
    }

	/*std::cout << "Outer contour:" << std::endl;
	for(int i = 0; i < outerContour.size(); i++){
		std::cout << "\t"
				  << outerContour.at(i).at(0).x() << " "
				  << outerContour.at(i).at(0).y() << " "
				  << outerContour.at(i).at(0).z() << std::endl;
		std::cout << "\t"
				  << outerContour.at(i).at(1).x() << " "
				  << outerContour.at(i).at(1).y() << " "
				  << outerContour.at(i).at(1).z() << std::endl;
		std::cout << "--------------------------------------------------------" << std::endl;
	}
	std::cout << "Inner contour:" << std::endl;
	for(int i = 0; i < innerContour.size(); i++){
		std::cout << "\t"
				  << innerContour.at(i).at(0).x() << " "
				  << innerContour.at(i).at(0).y() << " "
				  << innerContour.at(i).at(0).z() << std::endl;
		std::cout << "\t"
				  << innerContour.at(i).at(1).x() << " "
				  << innerContour.at(i).at(1).y() << " "
				  << innerContour.at(i).at(1).z() << std::endl;
		std::cout << "--------------------------------------------------------" << std::endl;
	}*/

    return geode;
}

void VirtualWorld::reloadScene()
{
    for(int i = 0; i < objects.size(); i++){
        btTransform transform;
        transform.setIdentity();
        osg::Matrix translate = osg::Matrix::translate(osgbCollision::asOsgVec3(objects[i]->resetPos));
        if(objects[i]->parent) translate = objects[i]->parent->getMatrix() * translate;
        transform = osgbCollision::asBtTransform(objects[i]->osgToBullet * translate);
        if(objects[i]->body){
            objects[i]->body->m_oldKinematicContacts.clear();
            objects[i]->body->m_newKinematicContacts.clear();
            objects[i]->body->m_oldOtherContacts.clear();
            objects[i]->body->m_newOtherContacts.clear();
            objects[i]->body->m_preserveKinematicContact = false;
            objects[i]->body->m_justReleased = 0;
            objects[i]->body->m_kinematicContactType = BT_NO_CONTACT;
            objects[i]->body->m_handled = false;
            objects[i]->body->clearForces();
            objects[i]->body->setLinearVelocity(btVector3(0., 0., 0.));
            objects[i]->body->setAngularVelocity(btVector3(0., 0., 0.));
            objects[i]->body->m_pseudoLinearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
            objects[i]->body->m_pseudoAngularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
            objects[i]->body->setCenterOfMassTransform(transform);
            objects[i]->body->setWorldTransform(transform);
            objects[i]->body->setOldTransform(transform);
        }
        objects[i]->representation->setMatrix(translate);
    }
}

G2Object* VirtualWorld::findObject(osgwTools::AbsoluteModelTransform *amt)
{
    int i = 0;
    while(i < objects.size()){
        if(objects[i]->representation.get() == amt)
            return objects[i];
        i++;
    }
    return NULL;
}

G2Object* VirtualWorld::findObject(const char* name)
{
    int i = 0;
    while(i < objects.size()){
        if(strstr(objects[i]->name.c_str(), name) != NULL)
            return objects[i];
        i++;
    }
    return NULL;
}

std::pair<G2Target*, G2Target*> VirtualWorld::findTarget(int difficulty, int enabledDOF)
{
	std::pair<G2Target*, G2Target*> target;
	for(int i = 0; i < targets.size(); i++){
		if(targets.at(i).first->difficulty == difficulty &&
		   targets.at(i).first->enabledDOF == enabledDOF)
			target = targets.at(i);
	}
	return target;
}
