Skip to content

Instantly share code, notes, and snippets.

@arielm

arielm/manip1App.cpp

Created June 11, 2011 17:38
Show Gist options
  • Save arielm/1020782 to your computer and use it in GitHub Desktop.
Save arielm/1020782 to your computer and use it in GitHub Desktop.
Download ZIP
Adaptation of "3D manipulation techniques for multitouch displays" to the iPad
Raw
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 )
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment