Adaptation of "3D manipulation techniques for multitouch displays" to the iPad

manip1App.cpp

#include "cinder/app/AppCocoaTouch.h"
#include "cinder/app/Renderer.h"
#include "cinder/Camera.h"
#include "cinder/CinderResources.h"
#include "cinder/ImageIo.h"
#include "cinder/gl/Texture.h"

#include <vector>
#include <map>
#include <list>

#include "TriMesh16.h"
#include "levmar.h"

//#define RES_DUCKY_MESH CINDER_RESOURCE( ../resources/,	ducky.msh, 128, MESH )
//#define RES_DUCKY_TEX CINDER_RESOURCE( ../resources/, ducky.png, 128, IMAGE )

/*
 * STRAIGHT PORT OF THE SCREEN-SPACE AND DS3 METHODS TO CINDER
 * SEE: http://www.youtube.com/watch?v=DnHvpyjgYik
 *
 * BASED ON: https://gist.github.com/764989
 * BY ANTHONY MARTINET: http://www.lifl.fr/~martinea
 *
 * DEPENDENCIES:
 *
 * 1) LEVMAR: http://www.ics.forth.gr/~lourakis/levmar/
 *    - REQUIRES THE Accelerate FRAMEWORK ON iOS
 *    - UNDER THE GPL LICENSE
 *
 * 2) Trimesh16: https://gist.github.com/1004350
 *
 * CONTAINS CODE FROM THE OGRE ENGINE: http://www.ogre3d.org
 */

using namespace ci;
using namespace ci::app;
using namespace std;

/******************************************************************************************************************************
 Very important, those function are needed for the constraint solver (levmar) and have to be global
 *******************************************************************************************************************************/
void diff2D_3DOF(double *p, double *hx, int m, int n, void *adata);
void diff2D_4DOFz(double *p, double *hx, int m, int n, void *adata);
void diff2D_6DOF(double *p, double *hx, int m, int n, void *adata);
bool get2DCoord	(Vec3f pos3D, Vec2f &pos2D);

std::map<int, Vec3f> mapPoint3DToMatch;
std::map<int, Vec2f> mapPoint2D;
Vec3f vCentreObject;
CameraPersp	mCam;

class Node
{
public:
	int id;
	TriMesh16 *mesh;
	gl::Texture *texture;
	ColorAf color;
	Quatf mOrientation;
	Vec3f mPosition;
	Vec3f mScale;
	
	Node(int id) : id(id)
	{
		mPosition = Vec3f::zero();
		mScale = Vec3f(1, 1, 1);

		mesh = NULL;
		texture = NULL;
	}
	
	const Quatf& getOrientation(void) const
    {
        return mOrientation;
    }

	const Vec3f& getPosition(void) const
    {
        return mPosition;
    }
	
	void rotate(const Quatf& q)
    {
		mOrientation *= q;
	}
	
	void translate(const Vec3f& d)
    {
		mPosition += d;
    }
	
	void scale(const Vec3f& s)
	{
		mScale *= s;
	}
	
	/*
	 * BASED ON OgreMatrix4.cpp
	 */
	void getFullTransform(Matrix44f& mat)
    {
		// Ordering:
        // 1. Scale
        // 2. Rotate
        // 3. Translate
		
		float x = mOrientation.v.x;
		float y = mOrientation.v.y;
		float z = mOrientation.v.z;
		float w = mOrientation.w;

		float fTx  = x+x;
        float fTy  = y+y;
        float fTz  = z+z;
        float fTwx = fTx*w;
        float fTwy = fTy*w;
        float fTwz = fTz*w;
        float fTxx = fTx*x;
        float fTxy = fTy*x;
        float fTxz = fTz*x;
        float fTyy = fTy*y;
        float fTyz = fTz*y;
        float fTzz = fTz*z;
		
		float rot3x3[3][3];
        rot3x3[0][0] = 1.0f-(fTyy+fTzz);
        rot3x3[0][1] = fTxy-fTwz;
        rot3x3[0][2] = fTxz+fTwy;
        rot3x3[1][0] = fTxy+fTwz;
        rot3x3[1][1] = 1.0f-(fTxx+fTzz);
        rot3x3[1][2] = fTyz-fTwx;
        rot3x3[2][0] = fTxz-fTwy;
        rot3x3[2][1] = fTyz+fTwx;
        rot3x3[2][2] = 1.0f-(fTxx+fTyy);
		
        // Set up final matrix with scale, rotation and translation
		float *m = (float *)mat.m;
        m[ 0] = mScale.x * rot3x3[0][0]; m[ 4] = mScale.y * rot3x3[0][1]; m[ 8] = mScale.z * rot3x3[0][2]; m[12] = mPosition.x;
        m[ 1] = mScale.x * rot3x3[1][0]; m[ 5] = mScale.y * rot3x3[1][1]; m[ 9] = mScale.z * rot3x3[1][2]; m[13] = mPosition.y;
        m[ 2] = mScale.x * rot3x3[2][0]; m[ 6] = mScale.y * rot3x3[2][1]; m[10] = mScale.z * rot3x3[2][2]; m[14] = mPosition.z;
		
        // No projection term
        m[3] = 0; m[7] = 0; m[11] = 0; m[15] = 1;
	}
	
	/*
	 * BASED ON OgreNode.cpp
	 */
	Vec3f convertLocalToWorldPosition(const Vec3f &localPos) const
	{
		return (localPos * mScale * mOrientation) + mPosition;
	}
};

typedef boost::shared_ptr<class Node> NodeRef;
typedef boost::shared_ptr<class TriMesh16> MeshRef;
typedef boost::shared_ptr<class gl::Texture> TextureRef;

class Finger
{
public:
	uint32_t id;
	Vec2f cvec;

	Finger(uint32_t id) : id(id)
	{
		cvec = Vec2f::zero();
	}

	Finger(Finger *f)
	{
		id = f->id;
		cvec.x = f->cvec.x;
		cvec.y = f->cvec.y;
	}
	
	Finger(uint32_t id, float x, float y) : id(id)
	{
		cvec.x = x;
		cvec.y = y;
	}
	
	void update(const Finger &f)
	{
		id = f.id;
		cvec.x = f.cvec.x;
		cvec.y = f.cvec.y;
	}
};

/*
 * BASED ON OgrePlane.cpp AND OgreMath.cpp
 */
class  Plane
{
public:
	Vec3f normal;
	float d;
	
	Plane(const Vec3f &rkNormal, const Vec3f &rkPoint)
	{
		normal = rkNormal;
		d = -rkNormal.dot(rkPoint);
	}
	
	pair<bool, float> intersects(const Ray& ray)
	{
		float denom = normal.dot(ray.getDirection());
		if (fabs(denom) < std::numeric_limits<float>::epsilon())
		{
			return std::pair<bool, float>(false, 0); // Parallel
		}
		else
		{
			float nom = normal.dot(ray.getOrigin()) + d;
			float t = -(nom/denom);
			return std::pair<bool, float>(t >= 0, t);
		}
	}
};

enum
{
	PRESS,
	MOVE,
	RELEASE
};

enum Mode
{
	INIT_STATE,
	NOTHING,
	TRANSLATION,
	TOUCH_2,
	TOUCH_3,
	Z_TRANSLATION,
	COMBINED,
	ROTATION,
};

enum Interaction
{
	INTERACTION_DS3,
	INTERACTION_SCREENSPACE,
};

class manip1App : public AppCocoaTouch
{
public:
	void	setup();
	void	prepareSettings(Settings *settings);
	void	resize(ResizeEvent event);
	void	update();
	void	draw();
	
	void drawMesh(TriMesh16 *mesh, bool useTexture, bool useNormals);
	TriMesh16* createCubeMesh(const Vec3f &center, const Vec3f &size );
	Node *findNode(int id);

	void touchesBegan(TouchEvent event);
	void touchesMoved(TouchEvent event);
	void touchesEnded(TouchEvent event);
	
	bool pickNode(Ray ray, Node **pickedNode, float *pickedDistance, Vec3f *pickedPoint, Vec3f *pickedNormal);
	Ray getCameraToViewportRay(float u, float v);
	
	void convertFinger(const Vec2f &in, Vec2f &out);
	void updateInteraction(int iLastTouch, int iLastFinger, Finger *f);
	void updateSelected();
	
	void ds3_mode(int iLastTouch, int iLastFinger, Finger *f);
	void ds3_gesture(int iLastTouch, int iLastFinger, Finger *f);
	void ds3_interaction();
	
	void nx_gesture(int iLastTouch, int iLastFinger, Finger *f);
	void nx_mode(int iLastTouch, int iLastFinger, Finger *f);
	void nx_interaction();
	
	void init_offset();
	void translate();
	void z_translate();
	float interpolate(float value);
	
	bool basic_picking(Finger *fPick, Vec3f &Result);	
	bool mini_picking(Vec2f pos, Vec3f &Result);
	bool plane_picking(Vec2f pos, Plane plan, Vec3f &Result);
	
	void moveObject();
	
	void init_direct3D();
	void direct3D_rotation();
	void direct3D_rotation_z();
	void direct3D_full();
	
	list<NodeRef> nodes;
	list<MeshRef> meshes;
	list<TextureRef> textures;
	
	Node *selectedNode;
	Node *lastSelectedNode;
	
	int iCurrentMode;
	int iCurrentTechnique;
	Finger *fZ;	// To store indirect finger of z technique
	map<int,Finger> ObjectFingers;	// To store other fingers
	
	Vec2f vMovePrev;
	Vec2f vMoveCur;	
	Vec2f vZCur;
	Vec2f vZPrev;
	std::list<Vec3f> vRotationPrev;
	std::list<Vec3f> vRotationCur;

	Vec3f vOffset; // Offset Pyramide
	int iNbFingerPicked; // gestion Screenspace

	Vec3f vectorTranslation;	// will store the translation to apply every frame
	Quatf quatRotation;		// will store the rotation to apply every frame
};

void manip1App::prepareSettings( Settings *settings )
{
	settings->enableMultiTouch();
}

void manip1App::setup()
{
	iCurrentMode = INIT_STATE;
	iCurrentTechnique = INTERACTION_DS3/*INTERACTION_SCREENSPACE*/;
	selectedNode = NULL;
	lastSelectedNode = NULL;
	
	fZ = NULL;
	vMovePrev = Vec2f::zero();
	vZPrev = Vec2f::zero();

	vectorTranslation = Vec3f::zero();
	quatRotation = Quatf::identity();
	
	vCentreObject = Vec3f::zero();
	
	// ---
	
	TriMesh16 *cubeMesh = createCubeMesh(Vec3f::zero(), Vec3f(1.0, 1.0, 1.0));
	meshes.push_back(MeshRef(cubeMesh));

	Node *node1 = new Node(1);
	node1->mesh = cubeMesh;
	node1->color = ColorAf(1, 0.5, 0, 1);
	node1->scale(Vec3f(5.0, 5.0, 5.0));
	nodes.push_back(NodeRef(node1));

#ifdef RES_DUCKY_MESH
	TriMesh16 *duckMesh = new TriMesh16;
	duckMesh->read(loadResource(RES_DUCKY_MESH));
	meshes.push_back(MeshRef(duckMesh));
	
	gl::Texture::Format format;
	format.enableMipmapping(true);			
	ImageSourceRef img = loadImage(loadResource(RES_DUCKY_TEX));
	gl::Texture *duckTexture = new gl::Texture(img, format);
	textures.push_back(TextureRef(duckTexture));
	
	Node *node2 = new Node(3);
	node2->mesh = duckMesh;
	node2->texture = duckTexture;
	node2->translate(Vec3f(-5, 0, -4));
	nodes.push_back(NodeRef(node2));
#else
	Node *node2 = new Node(2);
	node2->mesh = cubeMesh;
	node2->color = ColorAf(0.9, 0.9, 0, 1);
	node2->translate(Vec3f(-5, 0, -4));
	node2->scale(Vec3f(4.0, 4.0, 4.0));
	nodes.push_back(NodeRef(node2));
#endif

	// ---
	
	glEnable(GL_DEPTH_TEST);
	glDepthMask(true);
	
	glEnable(GL_BLEND);
	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	
	glShadeModel(GL_SMOOTH);
	glEnable(GL_LIGHT0);
	glEnable(GL_NORMALIZE);
	glEnable(GL_COLOR_MATERIAL);
}

void manip1App::resize( ResizeEvent event )
{
	mCam.lookAt( Vec3f( 20, 0, 0 ), Vec3f::zero() );
	mCam.setPerspective( 60, event.getAspectRatio(), 1, 1000 );
}

void manip1App::update()
{
	if (iCurrentTechnique == INTERACTION_DS3)
	{
		ds3_interaction();
	}
	else if (iCurrentTechnique == INTERACTION_SCREENSPACE)
	{
		nx_interaction();
	}
}

void manip1App::draw()
{
	glClearColor(0.25, 0.0, 0.0, 1);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	
	gl::setMatrices(mCam);

	const GLfloat light_Position[] = {20, 0, 0, 0};
	glLightfv(GL_LIGHT0, GL_POSITION, light_Position);
	glEnable(GL_LIGHTING);

	// ---

	Matrix44f mat; // KEEPING THE DECLARATION OUTSIDE OF THE LOOP, TO AVOID AN USELESS setToIdentity() TO BE INVOKED BY THE CONSTRUCTOR AT EACH ITERATION

	for(list<NodeRef>::const_iterator itr = nodes.begin(); itr != nodes.end(); ++itr)
	{
		gl::Texture *texture = (*itr)->texture;
		bool hasTexture = (texture != NULL);

		if (hasTexture)
		{
			glColor4f(1, 1, 1, 1);
			texture->enableAndBind();
		}
		else
		{
			gl::color((*itr)->color);
		}

		glPushMatrix();
		(*itr)->getFullTransform(mat);
		gl::multModelView(mat);
		drawMesh((*itr)->mesh, hasTexture, true);
		glPopMatrix();

		if (hasTexture)
		{
			texture->unbind();
		}
	}

	glDisable(GL_LIGHTING);
}

void manip1App::drawMesh(TriMesh16 *mesh, bool useTexture, bool useNormals)
{
	if (useTexture && mesh->hasTexCoords())
	{
		glEnableClientState(GL_TEXTURE_COORD_ARRAY);
		glTexCoordPointer(2, GL_FLOAT, 0, &(mesh->getTexCoords()[0]));
	}

	if (useNormals && mesh->hasNormals())
	{
		glEnableClientState( GL_NORMAL_ARRAY );
		glNormalPointer(GL_FLOAT, 0, &(mesh->getNormals()[0]) );
	}

	glEnableClientState( GL_VERTEX_ARRAY );
	glVertexPointer(3, GL_FLOAT, 0, &(mesh->getVertices()[0]));

	glDrawElements(GL_TRIANGLES, mesh->getNumIndices(), GL_UNSIGNED_SHORT, &(mesh->getIndices()[0]));

	glDisableClientState(GL_VERTEX_ARRAY);
	glDisableClientState(GL_NORMAL_ARRAY);
	glDisableClientState(GL_TEXTURE_COORD_ARRAY);
}

TriMesh16* manip1App::createCubeMesh(const Vec3f &c, const Vec3f &size )
{
	TriMesh16 *mesh = new TriMesh16;
	
	GLfloat sx = size.x * 0.5f;
	GLfloat sy = size.y * 0.5f;
	GLfloat sz = size.z * 0.5f;
	
	// ---
	
	mesh->appendVertex(Vec3f(c.x + sx, c.y + sy, c.z + sz));
	mesh->appendVertex(Vec3f(c.x + sx, c.y - sy, c.z + sz));
	mesh->appendVertex(Vec3f(c.x + sx, c.y - sy, c.z - sz));
	mesh->appendVertex(Vec3f(c.x + sx, c.y + sy, c.z - sz));
	
	mesh->appendVertex(Vec3f(c.x + sx, c.y + sy, c.z + sz));
	mesh->appendVertex(Vec3f(c.x + sx, c.y + sy, c.z - sz));
	mesh->appendVertex(Vec3f(c.x - sx, c.y + sy, c.z - sz));
	mesh->appendVertex(Vec3f(c.x - sx, c.y + sy, c.z + sz));
	
	mesh->appendVertex(Vec3f(c.x + sx, c.y + sy, c.z + sz));
	mesh->appendVertex(Vec3f(c.x - sx, c.y + sy, c.z + sz));
	mesh->appendVertex(Vec3f(c.x - sx, c.y - sy, c.z + sz));
	mesh->appendVertex(Vec3f(c.x + sx, c.y - sy, c.z + sz));
	
	mesh->appendVertex(Vec3f(c.x - sx, c.y + sy, c.z + sz));
	mesh->appendVertex(Vec3f(c.x - sx, c.y + sy, c.z - sz));
	mesh->appendVertex(Vec3f(c.x - sx, c.y - sy, c.z - sz));
	mesh->appendVertex(Vec3f(c.x - sx, c.y - sy, c.z + sz));
	
	mesh->appendVertex(Vec3f(c.x - sx, c.y - sy, c.z - sz));
	mesh->appendVertex(Vec3f(c.x + sx, c.y - sy, c.z - sz));
	mesh->appendVertex(Vec3f(c.x + sx, c.y - sy, c.z + sz));
	mesh->appendVertex(Vec3f(c.x - sx, c.y - sy, c.z + sz));
	
	mesh->appendVertex(Vec3f(c.x + sx, c.y - sy, c.z - sz));
	mesh->appendVertex(Vec3f(c.x - sx, c.y - sy, c.z - sz));
	mesh->appendVertex(Vec3f(c.x - sx, c.y + sy, c.z - sz));
	mesh->appendVertex(Vec3f(c.x + sx, c.y + sy, c.z - sz));
	
	// ---
	
	mesh->appendNormal(Vec3f(1,0,0));
	mesh->appendNormal(Vec3f(1,0,0));
	mesh->appendNormal(Vec3f(1,0,0));
	mesh->appendNormal(Vec3f(1,0,0));
	
	mesh->appendNormal(Vec3f(0,1,0));
	mesh->appendNormal(Vec3f(0,1,0));
	mesh->appendNormal(Vec3f(0,1,0));
	mesh->appendNormal(Vec3f(0,1,0));
	
	mesh->appendNormal(Vec3f(0,0,1));
	mesh->appendNormal(Vec3f(0,0,1));
	mesh->appendNormal(Vec3f(0,0,1));
	mesh->appendNormal(Vec3f(0,0,1));
	
	mesh->appendNormal(Vec3f(-1,0,0));
	mesh->appendNormal(Vec3f(-1,0,0));
	mesh->appendNormal(Vec3f(-1,0,0));
	mesh->appendNormal(Vec3f(-1,0,0));
	
	mesh->appendNormal(Vec3f(0,-1,0));
	mesh->appendNormal(Vec3f(0,-1,0));
	mesh->appendNormal(Vec3f(0,-1,0));
	mesh->appendNormal(Vec3f(0,-1,0));
	
	mesh->appendNormal(Vec3f(0,0,-1));
	mesh->appendNormal(Vec3f(0,0,-1));
	mesh->appendNormal(Vec3f(0,0,-1));
	mesh->appendNormal(Vec3f(0,0,-1));
	
	// ---
	
	mesh->appendIndex(0);
	mesh->appendIndex(1);
	mesh->appendIndex(2);
	mesh->appendIndex(0);
	mesh->appendIndex(2);
	mesh->appendIndex(3);
	mesh->appendIndex(4);
	mesh->appendIndex(5);
	mesh->appendIndex(6);
	mesh->appendIndex(4);
	mesh->appendIndex(6);
	mesh->appendIndex(7);
	mesh->appendIndex(8);
	mesh->appendIndex(9);
	mesh->appendIndex(10);
	mesh->appendIndex(8);
	mesh->appendIndex(10);
	mesh->appendIndex(11);
	mesh->appendIndex(12);
	mesh->appendIndex(13);
	mesh->appendIndex(14);
	mesh->appendIndex(12);
	mesh->appendIndex(14);
	mesh->appendIndex(15);
	mesh->appendIndex(16);
	mesh->appendIndex(17);
	mesh->appendIndex(18);
	mesh->appendIndex(16);
	mesh->appendIndex(18);
	mesh->appendIndex(19);
	mesh->appendIndex(20);
	mesh->appendIndex(21);
	mesh->appendIndex(22);
	mesh->appendIndex(20);
	mesh->appendIndex(22);
	mesh->appendIndex(23);
	
	return mesh;
}

Node *manip1App::findNode(int id)
{
	for (list<NodeRef>::iterator itr = nodes.begin(); itr != nodes.end(); ++itr)
	{
		if (id == (*itr)->id)
		{
			return &(**itr);
		}
	}

	return NULL;
}

/*
 * BASED ON Picking3D CINDER SAMPLE
 */
bool manip1App::pickNode(Ray ray, Node **pickedNode, float *pickedDistance, Vec3f *pickedPoint, Vec3f *pickedNormal)
{
	float distance = 0;
	float result = MAXFLOAT;

	int pickedId = -1;
	int pickedTriangleIndex = -1;
	
	for(list<NodeRef>::const_iterator itr = nodes.begin(); itr != nodes.end(); ++itr)
	{
		TriMesh16 *mesh = (*itr)->mesh;
		int id = (*itr)->id;
		Matrix44f transform; (*itr)->getFullTransform(transform); 
		
		// the coordinates of the bounding box are in object space, not world space,
		// so if the model was translated, rotated or scaled, the bounding box would not
		// reflect that
		AxisAlignedBox3f worldBounds = mesh->calcBoundingBox(transform); // TODO: COULD BE CACHED AT THE Node LEVEL (SINCE THE OPERATION IS RELATIVELY EXPENSIVE)
		if (worldBounds.intersects(ray))
		{
			size_t polycount = mesh->getNumTriangles();
			for (size_t j = 0; j <polycount; ++j)
			{
				Vec3f v0, v1, v2;
				mesh->getTriangleVertices(j, &v0, &v1, &v2);
				
				// transform triangle to world space
				v0 = transform.transformPointAffine(v0);
				v1 = transform.transformPointAffine(v1);
				v2 = transform.transformPointAffine(v2);
				
				if (ray.calcTriangleIntersection(v0, v1, v2, &distance))
				{
					if (distance < result)
					{
						result = distance;
						pickedId = id;
						pickedTriangleIndex = j;
					}
				}
			}
		}
	}
	
	if (distance > 0)
	{
		Node *node = findNode(pickedId);

		Vec3f v0, v1, v2;
		node->mesh->getTriangleVertices(pickedTriangleIndex, &v0, &v1, &v2);
		v0 = node->convertLocalToWorldPosition(v0);
		v1 = node->convertLocalToWorldPosition(v1);
		v2 = node->convertLocalToWorldPosition(v2);

		*pickedNormal = (v1 - v0).cross(v2 - v0).normalized();
		*pickedPoint = ray.calcPosition(result);
		*pickedDistance = result;
		*pickedNode = node;

		return true;
	}
	
	return false;
}

Ray manip1App::getCameraToViewportRay(float u, float v)
{
	return mCam.generateRay(u , 1.0f - v, mCam.getAspectRatio());
}

void manip1App::touchesBegan( TouchEvent event )
{
	for (vector<TouchEvent::Touch>::const_iterator touchIt = event.getTouches().begin(); touchIt != event.getTouches().end(); ++touchIt)
	{
		Finger *temp = new Finger(touchIt->getId());
		convertFinger(touchIt->getPos(), temp->cvec);
		updateInteraction(PRESS, touchIt->getId(), temp);
		delete temp;
	}
}

void manip1App::touchesMoved( TouchEvent event )
{
	for (vector<TouchEvent::Touch>::const_iterator touchIt = event.getTouches().begin(); touchIt != event.getTouches().end(); ++touchIt)
	{
		Finger *temp = new Finger(touchIt->getId());
		convertFinger(touchIt->getPos(), temp->cvec);
		updateInteraction(MOVE, touchIt->getId(), temp);
		delete temp;
	}
}

void manip1App::touchesEnded( TouchEvent event )
{
	for (vector<TouchEvent::Touch>::const_iterator touchIt = event.getTouches().begin(); touchIt != event.getTouches().end(); ++touchIt)
	{
		Finger *temp = new Finger(touchIt->getId());
		convertFinger(touchIt->getPos(), temp->cvec);
		updateInteraction(RELEASE, touchIt->getId(), temp);
		delete temp;
	}
}

void manip1App::convertFinger(const Vec2f &in, Vec2f &out)
{
	out.x = (in.x) / getWindowWidth();
	out.y = (in.y) / getWindowHeight();
}

void manip1App::updateInteraction(int iLastTouch, int iLastFinger, Finger *f)
{
	if (iCurrentTechnique == INTERACTION_DS3)
	{
		ds3_mode(iLastTouch, iLastFinger, f);
		ds3_gesture(iLastTouch, iLastFinger, f);
	}
	else if (iCurrentTechnique==INTERACTION_SCREENSPACE)
	{
		nx_mode(iLastTouch, iLastFinger, f);
		nx_gesture(iLastTouch, iLastFinger, f);
	}
}

void manip1App::updateSelected()
{
	lastSelectedNode = selectedNode;
	selectedNode = NULL;
}

// ------------------------------
// DS3 FUNCTION
// ------------------------------

void manip1App::ds3_mode(int iLastTouch, int iLastFinger, Finger *f)
{
	// According to the mode and picking we switch mode, build the objectfingers map !
	switch(iLastTouch)
	{
		case PRESS:
			if(iCurrentMode == INIT_STATE)
			{
				Vec3f vPicking;
				if(basic_picking(f,vPicking))
				{
					ObjectFingers.insert(pair<int,Finger> (f->id,Finger(f)));
					iCurrentMode = TRANSLATION;
					break;
				}
				break;
			}
			else if(iCurrentMode == TRANSLATION)
			{
				Vec3f vPicking;
				if(basic_picking(f,vPicking))
				{
					ObjectFingers.insert(pair<int,Finger> (f->id,Finger(f)));
					iCurrentMode = ROTATION;
					break;
				}
				else
				{
					fZ = new Finger(f);
					iCurrentMode = Z_TRANSLATION;
					break;
				}
			}
			else if(iCurrentMode == Z_TRANSLATION)
			{
				Vec3f vPicking;
				if(basic_picking(f,vPicking))
				{
					ObjectFingers.insert(pair<int,Finger> (f->id,Finger(f)));
					iCurrentMode = COMBINED;
					break;
				}
				break;
			}
			else if(iCurrentMode == ROTATION)
			{
				Vec3f vPicking;
				if(!basic_picking(f,vPicking))
				{
					fZ = new Finger(f);
					iCurrentMode = COMBINED;
					break;
				}
				else
				{
					ObjectFingers.insert(pair<int,Finger> (f->id,Finger(f)));
					break;
				}
			}
			break;	
			
		case MOVE:	// Simple update of the finger
			if(fZ!=NULL && iLastFinger == fZ->id)
			{
				fZ->update(f);
				break;
			}
			else if(ObjectFingers.find(f->id)!=ObjectFingers.end())
			{
				ObjectFingers.find(f->id)->second.update(f);
				break;
			}
			break;
			
		case RELEASE:	// A bit more tricky
			if(iCurrentMode == TRANSLATION)
			{
				if(ObjectFingers.find(f->id)!=ObjectFingers.end())
				{
					ObjectFingers.erase(f->id);
					iCurrentMode = INIT_STATE;
					updateSelected();
					break;
				}
				break;
			}
			else if(iCurrentMode == ROTATION)
			{
				if(ObjectFingers.find(f->id)!=ObjectFingers.end())
				{
					ObjectFingers.erase(f->id);
					if(ObjectFingers.size()==1)
					{
						Vec2f temp = ObjectFingers.begin()->second.cvec;
						Vec3f temp2;
						// with two fingers, sometimes, when releasing, we are not on the object anymore ...
						if(mini_picking(temp,temp2))
						{
							iCurrentMode = TRANSLATION;
						}
						else
						{
							iCurrentMode = NOTHING;
						}
					}
					break;
				}
				break;
			}
			else if(iCurrentMode == Z_TRANSLATION)
			{
				if(fZ!=NULL && iLastFinger == fZ->id)
				{
					delete fZ;
					fZ = NULL;
					iCurrentMode = TRANSLATION;
					break;
					
				}
				else if(ObjectFingers.find(f->id)!=ObjectFingers.end())
				{
					ObjectFingers.erase(f->id);
					if(ObjectFingers.size()==0)
					{
						iCurrentMode = NOTHING;
						updateSelected();
					}
					break;
				}
				break;
			}
			else if(iCurrentMode == COMBINED)
			{
				if(ObjectFingers.find(f->id)!=ObjectFingers.end())
				{
					ObjectFingers.erase(f->id);
					int size = ObjectFingers.size();
					if(size==1)
					{
						Vec2f temp = ObjectFingers.begin()->second.cvec;
						Vec3f temp2;
						if(mini_picking(temp,temp2))
						{
							iCurrentMode = Z_TRANSLATION;
							vMovePrev = Vec2f::zero();
						}
						else
						{
							iCurrentMode = NOTHING;
							updateSelected();
						}
					}
					else if(size==0)
					{
						iCurrentMode = NOTHING;
						updateSelected();
					}
					break;
				}
				else if(fZ!=NULL && iLastFinger == fZ->id)
				{
					delete fZ;
					fZ = NULL;
					iCurrentMode = ROTATION;
					break;
				}
				break;
			}
			else if(iCurrentMode == NOTHING)
			{
				if(fZ!=NULL && iLastFinger == fZ->id)
				{
					delete fZ;
					fZ = NULL;
					
				}
				else if(ObjectFingers.find(f->id)!=ObjectFingers.end())
				{
					ObjectFingers.erase(f->id);
				}
				
				if(ObjectFingers.empty() && fZ==NULL)
				{
					iCurrentMode = INIT_STATE;
				}
				break;
			}				
	}
}

void manip1App::ds3_gesture(int iLastTouch, int iLastFinger, Finger *f)
{
	if(iCurrentMode == INIT_STATE)
	{
	}
	else if(iCurrentMode == TRANSLATION)
	{
		// Only one finger, we use its position to move the object
		if(ObjectFingers.begin()!=ObjectFingers.end())
		{
			vMoveCur = Vec2f(ObjectFingers.begin()->second.cvec.x,ObjectFingers.begin()->second.cvec.y);
		}
	}
	else if(iCurrentMode == Z_TRANSLATION)
	{
		// two fingers : direct and indirect
		// tz
		vZCur = Vec2f(fZ->cvec.x,fZ->cvec.y);
		
		//tx,ty
		if(ObjectFingers.begin()!=ObjectFingers.end())
		{
			vMoveCur = Vec2f(ObjectFingers.begin()->second.cvec.x,ObjectFingers.begin()->second.cvec.y);
		}
	}
	else if(iCurrentMode == ROTATION)
	{
		// We create a map with Vec3f :
		//	.x & .y = position
		//	.z = id
		std::map<int,Finger>::iterator it = ObjectFingers.begin();
		vRotationCur.clear();
		for(it; it!=ObjectFingers.end();it++)
		{
			vRotationCur.push_back(Vec3f(it->second.cvec.x,it->second.cvec.y,it->second.id));
		}
		
	}
	else if(iCurrentMode == COMBINED)
	{
		// a mix of above
		std::map<int,Finger>::iterator it = ObjectFingers.begin();
		vRotationCur.clear();
		for(it; it!=ObjectFingers.end();it++)
		{
			vRotationCur.push_back(Vec3f(it->second.cvec.x,it->second.cvec.y,it->second.id));
		}
		vZCur = Vec2f(fZ->cvec.x,fZ->cvec.y);		
	}	
}

void manip1App::ds3_interaction()
{
	if(iCurrentMode==INIT_STATE)
	{
		// reset everything
		vRotationCur.clear();
		vZCur = Vec2f::zero();
		vMoveCur = Vec2f::zero();
		
		vMovePrev = Vec2f::zero();
		vZPrev = Vec2f::zero();
		vRotationPrev.clear();
	}
	else if(iCurrentMode==TRANSLATION)
	{
		// tx, ty avec vMoveCur
		translate();
		
		//update
		vMovePrev = vMoveCur;
		
		// reset other
		vZPrev = Vec2f::zero();
		vRotationPrev.clear();
	}
	else if(iCurrentMode==Z_TRANSLATION)
	{
		// tx, ty avec vMoveCur
		translate();
		
		// tz avec vZCur
		z_translate();
		
		// update
		vMovePrev = vMoveCur;
		vZPrev = vZCur;
		
		// reset other
		vRotationPrev.clear();
	}
	else if(iCurrentMode==ROTATION)
	{
		// Handle rx,ry,rz
		if(vRotationPrev.size() == vRotationCur.size())
		{
			
			direct3D_rotation();
		}
		else
		{
			init_direct3D();
		}
		
		// update
		vRotationPrev = vRotationCur;
		
		//reset other
		vZPrev = Vec2f::zero();
		vMovePrev = Vec2f::zero();	
	}
	else if(iCurrentMode==COMBINED)
	{
		// Handle rx,ry,rz
		if(vRotationPrev.size() == vRotationCur.size())
		{
			direct3D_rotation();
		}
		else
		{
			init_direct3D();
		}
		
		// tz
		z_translate();
		
		// update
		vZPrev = vZCur;
		vRotationPrev = vRotationCur;
		
		// reset other
		vMovePrev = Vec2f::zero();
	}
	
	moveObject();
}

// ------------------------------
// SCREENSPACE FUNCTION
// ------------------------------

void manip1App::nx_mode(int iLastTouch, int iLastFinger, Finger *f)
{
	switch(iLastTouch)
	{
		case PRESS:
			if (iCurrentMode == INIT_STATE)
			{
				Vec3f vPicking;
				if (basic_picking(f,vPicking))
				{
					ObjectFingers.insert(pair<int,Finger> (f->id,Finger(f)));
					iCurrentMode = TRANSLATION;
					break;
				}
				break;
			}
			else if (iCurrentMode == TRANSLATION)
			{
				Vec3f vPicking;
				if (basic_picking(f,vPicking))
				{
					ObjectFingers.insert(pair<int,Finger> (f->id,Finger(f)));
					iCurrentMode = TOUCH_2;
					break;
				}
			}
			else if (iCurrentMode == TOUCH_2)
			{
				Vec3f vPicking;
				if (basic_picking(f,vPicking))
				{
					ObjectFingers.insert(pair<int,Finger> (f->id,Finger(f)));
					iCurrentMode = TOUCH_3;
					break;
				}
			}
			else if (iCurrentMode == TOUCH_3)
			{
				Vec3f vPicking;
				if (basic_picking(f,vPicking))
				{
					ObjectFingers.insert(pair<int,Finger> (f->id,Finger(f)));
					break;
				}
			}
			break;
			
		case MOVE:
			if (ObjectFingers.find(f->id)!=ObjectFingers.end())
			{
				ObjectFingers.find(f->id)->second.update(f);
				break;
			}
			break;
			
		case RELEASE:
			if (iCurrentMode == TRANSLATION)
			{
				if (ObjectFingers.find(f->id)!=ObjectFingers.end())
				{
					ObjectFingers.erase(f->id);
					iCurrentMode = INIT_STATE;
					updateSelected();
					break;
				}
				break;
			}
			else if (iCurrentMode == TOUCH_2)
			{
				if (ObjectFingers.find(f->id)!=ObjectFingers.end())
				{
					ObjectFingers.erase(f->id);
					Vec2f temp = ObjectFingers.begin()->second.cvec;
					Vec3f temp2;
					if(mini_picking(temp, temp2))
					{
						iCurrentMode = TRANSLATION;
					}
					else
					{
						iCurrentMode = NOTHING;
					}
					break;
				}
				break;
			}
			else if (iCurrentMode == TOUCH_3)
			{
				if (ObjectFingers.find(f->id)!=ObjectFingers.end())
				{
					ObjectFingers.erase(f->id);
					int size = ObjectFingers.size();
					if(size == 2)
					{
						iCurrentMode = TOUCH_2;
						break;
					}
				}
				break;
			}
			else if (iCurrentMode == NOTHING)
			{
				if (ObjectFingers.find(f->id)!=ObjectFingers.end())
				{
					ObjectFingers.erase(f->id);
					
				}
				
				if (ObjectFingers.empty())
				{
					iCurrentMode = INIT_STATE;
				}
				break;
			}
	}
}

void manip1App::nx_gesture(int iLastTouch, int iLastFinger, Finger *f)
{
	if (iCurrentMode == INIT_STATE)
	{
	}
	else if (iCurrentMode == TRANSLATION)
	{
		if(ObjectFingers.begin()!=ObjectFingers.end())
		{
			vMoveCur = Vec2f(ObjectFingers.begin()->second.cvec.x,ObjectFingers.begin()->second.cvec.y);
		}
	}
	else if (iCurrentMode == TOUCH_2 || iCurrentMode == TOUCH_3)
	{
		std::map<int,Finger>::iterator it = ObjectFingers.begin();
		vRotationCur.clear();
		for(it; it!=ObjectFingers.end();it++)
		{
			vRotationCur.push_back(Vec3f(it->second.cvec.x, it->second.cvec.y, it->second.id));
		}
	}
}

void manip1App::nx_interaction()
{
	if (iCurrentMode==INIT_STATE)
	{
		vRotationCur.clear();
		vMoveCur = Vec2f::zero();
		
		vMovePrev = Vec2f::zero();
		vRotationPrev.clear();
	}
	else if (iCurrentMode == TRANSLATION)
	{
		// tx, ty avec vMoveCur
		translate();
		
		// update
		vMovePrev = vMoveCur;
		
		// reset
		vRotationPrev.clear();
	}
	else if (iCurrentMode == TOUCH_2)
	{
		// tz, rx, ry, rz
		if (vRotationPrev.size() == vRotationCur.size())
		{
			direct3D_rotation_z();
		}
		else{
			init_direct3D();
		}
		
		// update
		vRotationPrev = vRotationCur;
		
		// reset
		vMovePrev = Vec2f::zero();
	}
	else if (iCurrentMode == TOUCH_3)
	{
		// tx, ty, tz, rx, ry, rz
		if (vRotationPrev.size() == vRotationCur.size())
		{
			direct3D_full();
		}
		else
		{
			init_direct3D();
		}
		
		// update
		vRotationPrev = vRotationCur;
		
		// reset
		vMovePrev = Vec2f::zero();
	}

	moveObject();
}

void manip1App::init_offset()
{
	Vec3f vectorResult;
	if (mini_picking(vMoveCur,vectorResult))
	{
		vOffset = vectorResult;	// Store the exact point picked !
	}
}

// handle 2DOF translation
void manip1App::translate()
{
	if (vMovePrev == Vec2f::zero())
	{
		init_offset(); // if we just start interaction, init !
	}
	else if (vMoveCur != vMovePrev)
	{
		// I used a planed defined with a Vec3f
		Quatf cameraOrientation = Quatf(mCam.getModelViewMatrix());
		Vec3f vPlanCamera = cameraOrientation*Vec3f(0,0,-1);
		
		// and a point (where we touched the object in init_offset)
		Plane plan(vPlanCamera,vOffset);
		
		Vec3f vCur;
		if(plane_picking(vMoveCur,plan,vCur))
		{
			// translation with 2DOF
			vectorTranslation = (vCur-vOffset);
			
			// updating the offset
			vOffset = vCur;
		}
	}
}

// handle translation among the third dimension
void manip1App::z_translate()
{
	if (vZPrev != Vec2f::zero() && vZCur != vZPrev)
	{
		Vec2f vDiff = vZCur - vZPrev;
		float interpolate_z = interpolate(vDiff.y) * vDiff.y;
		
		// We want the object to stay under the finger, we thus will move it among the ray defined by vMoveCur (position of the translation finger)
		Ray mouseRay = getCameraToViewportRay(vMoveCur.x, vMoveCur.y);
		Node *pickedNode;
		float impact;
		Vec3f oldpos, pickedNormal;
		if (pickNode(mouseRay, &pickedNode, &impact, &oldpos, &pickedNormal))
		{
			// To have our translation vector, we proceed to a difference
			Vec3f newpos = mouseRay.calcPosition(impact + interpolate_z);	// Current point + z_translation
			Vec3f diff = oldpos-newpos;	// we obtain a distance among the ray
			vectorTranslation += diff;	// that we add to vectorTranslation
		}
	}
}

float manip1App::interpolate(float value)
{
	return 100; // FIXME
}

bool manip1App::basic_picking(Finger *fPick, Vec3f &result)
{
	Ray mouseRay = getCameraToViewportRay(fPick->cvec.x, fPick->cvec.y);
	Node *pickedNode;
	float pickedDistance;
	Vec3f pickedPoint, pickedNormal;
	if (pickNode(mouseRay, &pickedNode, &pickedDistance, &pickedPoint, &pickedNormal))
	{
		if (selectedNode == NULL)
		{
			selectedNode = pickedNode;
			result = pickedPoint;
			return true;
		}
		else if (selectedNode->id == pickedNode->id)
		{
			result = pickedPoint;
			return true;
		}
	}
	
	return false;
}

bool manip1App::mini_picking(Vec2f pos, Vec3f &result)
{
	Ray mouseRay = getCameraToViewportRay(pos.x, pos.y);
	Node *pickedNode;
	float pickedDistance;
	Vec3f pickedPoint, pickedNormal;
	if (pickNode(mouseRay, &pickedNode, &pickedDistance, &pickedPoint, &pickedNormal))
	{
		result = pickedPoint;
		return true;
	}

	return false;
}

bool manip1App::plane_picking(Vec2f pos, Plane plan, Vec3f &result)
{
	Ray mouseRay = getCameraToViewportRay(pos.x,pos.y);
	
	std::pair<bool, float> res = plan.intersects(mouseRay);
	if(res.first)
	{
		result = mouseRay.calcPosition(res.second);
		return true;
	}

	return false;
}

void manip1App::moveObject()
{
	if (selectedNode != NULL)
	{
		if (quatRotation != Quatf::identity())
		{
			selectedNode->rotate(quatRotation);
			quatRotation = Quatf::identity();
		}
		
		if (vectorTranslation != Vec3f::zero())
		{
			selectedNode->translate(vectorTranslation);

			if (vectorTranslation.z != 0.0) // if we change the depth, re init offset !
			{
				init_offset();
			}

			vectorTranslation = Vec3f::zero();			
		}
	}
}

// init the solver
void manip1App::init_direct3D()
{
	mapPoint3DToMatch.clear();
	mapPoint2D.clear();
	
	// We will pick every finger so as to match 2D point with 3D points
	int nbFingerOK = 0;
	
	Vec3f vPicked;
	Vec3f vFingerCur;
	
	std::list<Vec3f>::iterator itcur = vRotationCur.begin();
	for(itcur; itcur!=vRotationCur.end();itcur++)
	{
		vFingerCur = (*itcur);
		if(mini_picking(Vec2f(vFingerCur.x,vFingerCur.y),vPicked))
		{
			mapPoint3DToMatch.insert(pair<int,Vec3f>(vFingerCur.z,vPicked));	// id of the finger, with 3D position of the object in the 3D scene
			mapPoint2D.insert(pair<int,Vec2f>(vFingerCur.z,Vec2f(vFingerCur.x,vFingerCur.y)));	// id of the finger, with 2D position on the screen
			nbFingerOK++;
		}
	}

	iNbFingerPicked = nbFingerOK;
}

void manip1App::direct3D_rotation()
{
	if(vRotationCur != vRotationPrev)
	{
		// On va mettre à jour les positions 2D si nécessaire et calculer le déplacement avec prev !
		Vec3f vPicked;
		
		Vec2f vFingerCur;
		Vec2f vFingerPrev;
		Vec2f vSommeFingerRotationPrev = Vec2f::zero();
		
		std::list<Vec3f>::iterator itprev = vRotationPrev.begin();
		std::list<Vec3f>::iterator itcur = vRotationCur.begin();
		
		for(itprev; itprev!=vRotationPrev.end();itprev++)
		{
			vFingerPrev = Vec2f((*itprev).x,(*itprev).y);
			vFingerCur =  Vec2f((*itcur).x,(*itcur).y);
			int fID = (*itprev).z;
			
			// Calcul prev
			vSommeFingerRotationPrev = vSommeFingerRotationPrev + vFingerPrev;
			
			// Mise à jour coordonnée 2D et si picking hop on met à jour
			std::map<int,Vec2f>::iterator it2D = mapPoint2D.find(fID);
			std::map<int,Vec3f>::iterator it3D = mapPoint3DToMatch.find(fID);
			
			// Si la fonction find a bien finder et
			if(it2D != mapPoint2D.end())
			{
				// Si picking ok
				if(it3D != mapPoint3DToMatch.end() && mini_picking(it2D->second,vPicked))
				{
					// alors on met à jour les point3D associé avec le point2D prev!
					it3D->second = vPicked;
				}
				
				// On met à jour le point 2D avec la valeur courante
				it2D->second = vFingerCur;
			}			
			itcur++;
		}		
		
		// Si OK
		if (mapPoint2D.size()>1)
		{
			vCentreObject = selectedNode->getPosition();
			
			// taille du problème : ici nombre de doigts
			int n = mapPoint2D.size();
			
			// 1 - Compute the center of the previous fingers.
			Vec2f vPrevCentre2D = (vSommeFingerRotationPrev)/n;
			
			
			// 2 - Compute the sum of the displacements from the center.
			Vec2f displacement = Vec2f::zero();
			std::map<int,Finger>::iterator it = ObjectFingers.begin();
			for(it; it!=ObjectFingers.end();it++)
			{
				Finger *temp = &(it->second);
				displacement = displacement  + (Vec2f(temp->cvec.x,temp->cvec.y)-vPrevCentre2D);
			}
			
			// Tableau pour ce qu'on va calculer
			double *x = (double *)malloc(n*2*sizeof(double)); 
			for (int i = 0; i < n*2; i++)
			{
				x[i] = 0.0;
			}
			
			// (ex, ey, ez)
			int m = 3;
			double p[3] = { 0.0, 0.0, 0.0};		
			
			
			if (displacement.x < 0.0)
			{
				p[0] = -0.01;
			}
			else
			{
				p[0] = 0.01;
			}
			
			if (displacement.y < 0.0)
			{
				p[1] = -0.01;
			}
			else
			{
				p[1] = 0.01;
			}
			
			double opts[LM_OPTS_SZ];
			opts[0] = LM_INIT_MU; 
			opts[1] = 1E-17; 
			opts[2] = 1E-17; 
			opts[3] = 1E-17;
			opts[4] = LM_DIFF_DELTA;
			double info[LM_INFO_SZ];
			
			dlevmar_dif(diff2D_3DOF, p, x, m, n*2, 1000, opts, info, NULL, NULL, NULL);
			
			// Si OK
			if ((fabs(info[6] -1.0) < 0.00001) || (fabs(info[6] - 2.0) < 0.00001))
			{
				Quatf qRot;
				qRot.v.x = p[0];
				qRot.v.y = p[1];
				qRot.v.z = p[2];
				qRot.normalize();
				
				quatRotation = qRot;
				
				// Mise à jour des points 3D en tenant compte de la rotation !
				std::map<int,Vec3f>::iterator it3D = mapPoint3DToMatch.begin();
				for(it3D;it3D!=mapPoint3DToMatch.end();it3D++)
				{
					Vec3f vPoint3D = it3D->second;
					it3D->second = qRot*(vPoint3D-vCentreObject) + vCentreObject;
				}
			}
			
			// Cleaning stuff
			free(x);		
		}
	}
}

void manip1App::direct3D_rotation_z()
{
	if (vRotationCur != vRotationPrev)
	{
		// On va mettre à jour les positions 2D si nécessaire et calculer le déplacement avec prev !
		Vec3f vPicked;
		
		Vec2f vFingerCur;
		Vec2f vFingerPrev;
		Vec2f vSommeFingerRotationPrev = Vec2f::zero();

		std::list<Vec3f>::iterator itprev = vRotationPrev.begin();
		std::list<Vec3f>::iterator itcur = vRotationCur.begin();
		
		for (itprev; itprev!=vRotationPrev.end(); itprev++)
		{
			vFingerPrev = Vec2f((*itprev).x, (*itprev).y);
			vFingerCur = Vec2f((*itcur).x, (*itcur).y);
			int fID = (*itprev).z;
			
			// Calcul prev
			vSommeFingerRotationPrev = vSommeFingerRotationPrev + vFingerPrev;
			
			// Mise à jour coordonnée 2D et si picking hop on met à jour
			std::map<int,Vec2f>::iterator it2D = mapPoint2D.find(fID);
			std::map<int,Vec3f>::iterator it3D = mapPoint3DToMatch.find(fID);
			
			// Si la fonction find a bien finder et
			if (it2D != mapPoint2D.end())
			{
				// Si picking ok
				if(it3D != mapPoint3DToMatch.end() && mini_picking(it2D->second, vPicked))
				{
					// alors on met à jour les point3D associé avec le point2D prev!
					it3D->second = vPicked;
				}
				
				// On met à jour le point 2D avec la valeur courante
				it2D->second = vFingerCur;
			}
			itcur++;
		}
		
		// Si OK
		if (mapPoint2D.size()>1)
		{
			// Centre de rotation : centre Objet
			vCentreObject = selectedNode->getPosition();
			
			// taille du problème : ici nombre de doigts
			int n = mapPoint2D.size();
			
			// 1 - Compute the center of the previous fingers.
			Vec2f vPrevCentre2D = (vSommeFingerRotationPrev) / n;
			
			// 2 - Compute the sum of the displacements from the center.
			Vec2f displacement = Vec2f::zero();
			std::map<int,Finger>::iterator it = ObjectFingers.begin();
			for (it; it!=ObjectFingers.end(); it++)
			{
				Finger *temp = &(it->second);
				displacement = displacement + (Vec2f(temp->cvec.x,temp->cvec.y)-vPrevCentre2D);
			}
			
			// Tableau pour ce qu'on va calculer
			double *x = (double *)malloc(n*2*sizeof(double));
			for (int i = 0; i < n*2; i++)
			{
				x[i] = 0.0;
			}
			
			// (tz, ex, ey, ez)
			int m = 3;
			double p[4] = { 0.0, 0.0, 0.0, 0.0};
			
			if (displacement.x < 0.0)
			{
				p[1] = -0.01;
			}
			else
			{
				p[1] = 0.01;
			}
			
			if (displacement.y < 0.0)
			{
				p[2] = -0.01;
			}
			else
			{
				p[2] = 0.01;
			}
			
			double opts[LM_OPTS_SZ];
			opts[0] = LM_INIT_MU;
			opts[1] = 1E-15;
			opts[2] = 1E-15;
			opts[3] = 1E-20;
			opts[4] = LM_DIFF_DELTA;
			double info[LM_INFO_SZ];
			
			dlevmar_dif(diff2D_4DOFz, p, x, m, n*2, 1000, opts, info, NULL, NULL, NULL);
			
			// Si OK
			if ((fabs(info[6] -1.0) < 0.00001) || (fabs(info[6] - 2.0) < 0.00001))
			{
				Vec3f deltaPos = vCentreObject - mCam.getEyePoint();
				vectorTranslation = deltaPos * p[0];
				
				Quatf qRot;
				qRot.v.x = p[1];
				qRot.v.y = p[2];
				qRot.v.z = p[3];
				qRot.normalize();
				
				quatRotation = qRot;
				
				// Mise à jour des points 3D en tenant compte de la rotation !
				std::map<int,Vec3f>::iterator it3D = mapPoint3DToMatch.begin();
				for (it3D;it3D!=mapPoint3DToMatch.end();it3D++)
				{
					Vec3f vPoint3D = it3D->second;
					it3D->second = quatRotation*(vPoint3D-vCentreObject) + vCentreObject + vectorTranslation;
				}
			}
			
			// Cleaning stuff
			free(x);
		}
	}
}

void manip1App::direct3D_full()
{
	if (vRotationCur != vRotationPrev)
	{
		// On va mettre à jour les positions 2D si nécessaire et calculer le déplacement avec prev !
		Vec3f vPicked;
		
		Vec2f vFingerCur;
		Vec2f vFingerPrev;
		Vec2f vSommeFingerRotationPrev = Vec2f::zero();
		
		std::list<Vec3f>::iterator itprev = vRotationPrev.begin();
		std::list<Vec3f>::iterator itcur = vRotationCur.begin();
		
		for (itprev; itprev!=vRotationPrev.end();itprev++)
		{
			vFingerPrev = Vec2f((*itprev).x,(*itprev).y);
			vFingerCur = Vec2f((*itcur).x,(*itcur).y);
			int fID = (*itprev).z;
			
			// Calcul prev
			vSommeFingerRotationPrev = vSommeFingerRotationPrev + vFingerPrev;
			
			// Mise à jour coordonnée 2D et si picking hop on met à jour
			std::map<int,Vec2f>::iterator it2D = mapPoint2D.find(fID);
			std::map<int,Vec3f>::iterator it3D = mapPoint3DToMatch.find(fID);
			
			// Si la fonction find a bien finder et
			if (it2D != mapPoint2D.end())
			{
				// Si picking ok
				if(it3D != mapPoint3DToMatch.end() && mini_picking(it2D->second,vPicked))
				{
					// alors on met à jour les point3D associé avec le point2D prev!
					it3D->second = vPicked;
				}
				
				// On met à jour le point 2D avec la valeur courante
				it2D->second = vFingerCur;
			}
			itcur++;
		}
		
		// Si OK
		if (mapPoint2D.size()>2)
		{
			
			// Centre de rotation : centre Objet
			vCentreObject = selectedNode->getPosition();
			
			// taille du problème : ici nombre de doigts
			int n = mapPoint2D.size();
			
			// 1 - Compute the center of the previous fingers.
			Vec2f vPrevCentre2D = (vSommeFingerRotationPrev)/n;
			
			// 2 - Compute the sum of the displacements from the center.
			Vec2f displacement = Vec2f::zero();
			std::map<int,Finger>::iterator it = ObjectFingers.begin();
			for(it; it!=ObjectFingers.end();it++)
			{
				Finger *temp = &(it->second);
				displacement = displacement + (Vec2f(temp->cvec.x,temp->cvec.y)-vPrevCentre2D);
			}
			
			
			// Tableau pour ce qu'on va calculer
			double *x = (double *)malloc(n*2*sizeof(double));
			for (int i = 0; i < n*2; i++)
			{
				x[i] = 0.0;
			}
			
			// (tx,ty,tz, ex, ey, ez)
			int m = 6;
			double p[6] = { 0.0, 0.0, 0.1, 0.1 , 0.1, 0.1};
			
			if (displacement.x < 0.0)
			{
				p[0] = -0.01;
			}
			else
			{
				p[0] = 0.01;
			}
			
			if (displacement.y < 0.0)
			{
				p[1] = -0.01;
			}
			else
			{
				p[1] = 0.01;
			}
			
			double opts[LM_OPTS_SZ];
			opts[0] = LM_INIT_MU;
			opts[1] = 1E-15;
			opts[2] = 1E-15;
			opts[3] = 1E-20;
			opts[4] = LM_DIFF_DELTA;
			double info[LM_INFO_SZ];
			
			dlevmar_dif(diff2D_6DOF, p, x, m, n*2, 1000, opts, info, NULL, NULL, NULL);
			
			// Si OK
			if ((fabs(info[6] -1.0) < 0.00001) || (fabs(info[6] - 2.0) < 0.00001))
			{
				
				Quatf qRot;
				qRot.v.x = p[3];
				qRot.v.y = p[4];
				qRot.v.z = p[5];
				qRot.normalize();

				quatRotation = qRot;
				vectorTranslation = mCam.getEyePoint() * Vec3f(p[0],p[1],p[2]);
				
				// Mise à jour des points 3D en tenant compte de la rotation !
				std::map<int,Vec3f>::iterator it3D = mapPoint3DToMatch.begin();
				for(it3D;it3D!=mapPoint3DToMatch.end();it3D++)
				{
					Vec3f vPoint3D = it3D->second;
					it3D->second = quatRotation*(vPoint3D-vCentreObject) + vCentreObject + vectorTranslation;
				}
			}
			
			// Cleaning stuff
			free(x);
		}
	}
}

//------------------------------
// SOLVER FUNCTION (EXTERN)
//------------------------------
/* The following function measures the error between the current 3D points and
 the reprojected 2D points considering the p vetor. */

/* p is the std::vector of values we are looking for (translations and rotations).
 hx is the result of the computation done by the function.
 n is the number of values for hx. */

void diff2D_3DOF(double *p, double *hx, int m, int n, void *adata)
{
	/* Retrieve the translation and the rotation from the p std::vector. */
	/* Translation values are the first 3 values in the p std::vector. */
	
	Quatf deltaQuat;
	deltaQuat.v.x = p[0];
	deltaQuat.v.y = p[1];
	deltaQuat.v.z = p[2];
	deltaQuat.normalize();
	
	/*	Save the current transformations, then apply the current transformations from the p std::vector,
	 project and finally restaure the initial transformations. */
	
	/* For each world point, compute the projected point, depending on the input parameters p. */
	std::map<int,Vec3f>::iterator itV3;
	std::map<int,Vec2f>::iterator itV2;
	int i=0;
	
	for (itV3=mapPoint3DToMatch.begin();itV3!=mapPoint3DToMatch.end();itV3++)
	{
		Vec3f v3Prev = (*itV3).second;
		itV2 = mapPoint2D.find((*itV3).first);
		Vec2f v2DCur = Vec2f::zero();
		if(itV2 != mapPoint2D.end())
		{
			v2DCur = (*itV2).second;
		}
		else
		{
			cout << "erreur diff2D_3DOF" << endl;
			return;
		}
		
		Vec3f V3DOK = deltaQuat*(v3Prev-vCentreObject) + vCentreObject;
		
		/* Project the world points to the screen space, using transformations defined by the p std::vector. */
		Vec2f v2DTest;
		get2DCoord(V3DOK,v2DTest);
		
		/*	Compute the result, ie the difference between fingers 2D points and 
		 the computed 2D points considering the p std::vector. */
		hx[i*2] = (v2DTest.x - v2DCur.x);
		hx[i*2 + 1] = (v2DTest.y - v2DCur.y);
		
		/*	Update pointeurs */
		i++;
		itV2++;
	}
}

void diff2D_4DOFz(double *p, double *hx, int m, int n, void *adata)
{
	/* Retrieve the translation and the rotation from the p std::vector. */
	Vec3f deltaPos = vCentreObject - mCam.getEyePoint();
	
	/* Translation values are the first 3 values in the p std::vector. */
	deltaPos = p[0]*deltaPos;
	
	/* Quaternion x, y, z are the 3 last values of the p std::vector. */
	Quatf deltaQuat;
	deltaQuat.v.x = p[1];
	deltaQuat.v.y = p[2];
	deltaQuat.v.z = p[3];
	deltaQuat.normalize();
	
	/* Save the current transformations, then apply the current transformations from the p std::vector,
	 project and finally restaure the initial transformations. */
	
	/* For each world point, compute the projected point, depending on the input parameters p. */
	std::map<int,Vec3f>::iterator itV3;
	std::map<int,Vec2f>::iterator itV2;
	int i=0;
	
	for (itV3=mapPoint3DToMatch.begin();itV3!=mapPoint3DToMatch.end();itV3++)
	{
		Vec3f v3Prev = (*itV3).second;
		itV2 = mapPoint2D.find((*itV3).first);
		Vec2f v2DCur = Vec2f::zero();
		if(itV2 != mapPoint2D.end())
		{
			v2DCur = (*itV2).second;
		}
		else
		{
			cout << "erreur diff2D_4DOFz" << endl;
			return;
		}
		
		/* Appliquer la transformation tr et rotate à V3Prev */
		Vec3f temp = vCentreObject + deltaPos;
		Matrix44f mat = deltaQuat.toMatrix44();
		mat.m[12] = temp.x;
		mat.m[13] = temp.y;
		mat.m[14] = temp.z;
//		Matrix3 temp;
//		deltaQuat.ToRotationMatrix(temp);
//		Matrix4 mat(temp);
//		mat.setTrans(vCentreObject+deltaPos);
		
		Vec3f V3DOK = mat*(v3Prev-vCentreObject);
		
		/* Project the world points to the screen space, using transformations defined by the p std::vector. */
		Vec2f v2DTest;
		get2DCoord(V3DOK, v2DTest);
		
		/* Compute the result, ie the difference between fingers 2D points and
		 the computed 2D points considering the p std::vector. */
		hx[i*2] = (v2DTest.x - v2DCur.x);
		hx[i*2 + 1] = (v2DTest.y - v2DCur.y);
		
		/* Update pointeurs */
		i++;
		itV2++;
	}
}

void diff2D_6DOF(double *p, double *hx, int m, int n, void *adata)
{
	/* Retrieve the translation and the rotation from the p std::vector. */
	
	/* Translation values are the first 3 values in the p std::vector. */
	Vec3f deltaPos;
	deltaPos.x = p[0];
	deltaPos.y = p[1];
	deltaPos.z = p[2];
	deltaPos = mCam.getEyePoint() * deltaPos;
	
	/* Quaternion x, y, z are the 3 last values of the p std::vector. */
	Quatf deltaQuat;
	deltaQuat.v.x = p[3];
	deltaQuat.v.y = p[4];
	deltaQuat.v.z = p[5];
	deltaQuat.normalize();
	
	/* Save the current transformations, then apply the current transformations from the p std::vector,
	 project and finally restaure the initial transformations. */
	
	/* For each world point, compute the projected point, depending on the input parameters p. */
	std::map<int,Vec3f>::iterator itV3;
	std::map<int,Vec2f>::iterator itV2;
	int i=0;
	
	for (itV3=mapPoint3DToMatch.begin();itV3!=mapPoint3DToMatch.end();itV3++)
	{
		Vec3f v3Prev = (*itV3).second;
		itV2 = mapPoint2D.find((*itV3).first);
		Vec2f v2DCur = Vec2f::zero();
		if (itV2 != mapPoint2D.end())
		{
			v2DCur = (*itV2).second;
		}
		else
		{
			cout << "erreur diff2D_6DOF" << endl;
			return;
		}
		
		/* Apply tr & rotate to V3Prev */
		Vec3f temp = vCentreObject + deltaPos;
		Matrix44f mat = deltaQuat.toMatrix44();
		mat.m[12] = temp.x;
		mat.m[13] = temp.y;
		mat.m[14] = temp.z;
//		Matrix3 temp;
//		deltaQuat.ToRotationMatrix(temp);
//		Matrix4 mat(temp);
//		mat.setTrans(vCentreObject+deltaPos);
		
		Vec3f V3DOK = mat*(v3Prev-vCentreObject);
		
		/* Project the world points to the screen space, using transformations defined by the p std::vector. */
		Vec2f v2DTest;
		get2DCoord(V3DOK,v2DTest);
		
		/* Compute the result, ie the difference between fingers 2D points and
		 the computed 2D points considering the p std::vector. */
		hx[i*2] = (v2DTest.x - v2DCur.x);
		hx[i*2 + 1] = (v2DTest.y - v2DCur.y);
		
		/* Update pointeurs */
		i++;
		itV2++;
	}
}

bool get2DCoord(Vec3f pos3D, Vec2f &pos2D)
{
	Vec3f eyeSpacePos = mCam.getModelViewMatrix() * pos3D;
	float x,y;
	
	// z < 0 means in front of cam
	if (eyeSpacePos.z < 0)
	{
		// calculate projected pos
		Vec3f screenSpacePos = mCam.getProjectionMatrix() * eyeSpacePos;
		
		// Coming back to [0,1] of the screen
		x = (screenSpacePos.x + 1.0f) / 2.0f;
		y = (1.0f - screenSpacePos.y) / 2.0f;
		
		pos2D = Vec2f(x,y);
		return true;
	}
	else
	{
		x = (-eyeSpacePos.x > 0) ? -1 : 1;
		y = (-eyeSpacePos.y > 0) ? -1 : 1;
		
		pos2D = Vec2f(x, y);
		return false;
	}
}

CINDER_APP_COCOA_TOUCH( manip1App, RendererGl )