Commit ec750a6b authored by Jund Thomas's avatar Jund Thomas

amelioration register_update et debut ajout moving_mesh

parent 8159c58a
......@@ -26,7 +26,9 @@ add_executable( socialAgentsD
../src/agent.cpp
../src/moving_obstacle.cpp
../src/simulator.cpp
../src/moving_mesh.cpp
../src/gl2ps.c
../src/ShapeMatching/rigidXfComputation.cpp
${socialAgents_moc}
${socialAgents_ui}
)
......
......@@ -24,7 +24,9 @@ add_executable( socialAgents
../src/agent.cpp
../src/moving_obstacle.cpp
../src/simulator.cpp
../src/moving_mesh.cpp
../src/gl2ps.c
../src/ShapeMatching/rigidXfComputation.cpp
${socialAgents_moc}
${socialAgents_ui}
)
......
#ifndef __RIGIDXFCOMPUTATION_09122008__
#define __RIGIDXFCOMPUTATION_09122008__
#include "Geometry/matrix.h"
#include "Geometry/vector_gen.h"
using namespace CGoGN;
/** two ways to compute the minimal linear transformation between 2 sets of points*/
class RigidComputation
{
public :
/** SVD decomposition */
//Thanks to Frederic Larue
// extern gmtl::Matrix44f getRigidTransform( std::vector<Geom::Vec3f> from,std::vector<Geom::Vec3f> to, float time);
/** Jacobian rotation : to use with the covariance matrix*/
// http://books.google.com/books?id=weYj75E_t6MC&pg=PA263&lpg=PA263&dq=diagonalize+matrix+Jacobi+rotation+method&source=web&ots=LPY0V-udrx&sig=0mBoM5L9nH3KAdiunJKuGeVDW0s&hl=en&sa=X&oi=book_result&resnum=5&ct=result#PPA266,M1
// rotate a throught beta in pq-plane to set a[p][q]=0
// rotation stored in r whose columns are eigenvectors of a
void rotate(Geom::Matrix33f &a,Geom::Matrix33f &r, int p, int q);
float maxoffdiag(Geom::Matrix33f &a,int &p, int &q);
//final a is diagonal matrix of eigenvalues
//final r has eigenvectors in rows
void jacrot(Geom::Matrix33f &a, Geom::Matrix33f &r);
static int jacobiDiagonalization( Geom::Matrix33f& A, Geom::Matrix33f& V );
};
#endif
#ifndef SHAPEMATCHING_H
#define SHAPEMATCHING_H
#include "Geometry/matrix.h"
#include "Geometry/vector_gen.h"
#include "env_map.h"
using namespace CGoGN;
template <typename PFP>
class ShapeMatchingGen
{
public:
ShapeMatchingGen(typename PFP::MAP& map, VertexAttribute<VEC3>& position,std::vector<Dart> v_darts, typename PFP::REAL beta):
m_map(map),
m_position(position),
m_darts(v_darts),
m_beta(beta)
{
this->goal = this->m_map.template addAttribute<VEC3, VERTEX>("");
}
~ShapeMatchingGen()
{
if(goal.isValid())
this->m_map.template removeAttribute<VEC3, VERTEX>(this->goal);
}
virtual void initialize()=0;
virtual void animate()=0;
void computeAqqMatrix()
{
this->aqq.zero();
for(unsigned int i = 0 ; i < this->q.size() ; i++)
{
this->aqq(0,0)+=this->q[i][0]*this->q[i][0]; this->aqq(1,0)+=this->q[i][0]*this->q[i][1]; this->aqq(2,0)+=this->q[i][0]*this->q[i][2];
this->aqq(0,1)+=this->q[i][1]*this->q[i][0]; this->aqq(1,1)+=this->q[i][1]*this->q[i][1]; this->aqq(2,1)+=this->q[i][1]*this->q[i][2];
this->aqq(0,2)+=this->q[i][2]*this->q[i][0]; this->aqq(1,2)+=this->q[i][2]*this->q[i][1]; this->aqq(2,2)+=this->q[i][2]*this->q[i][2];
}
this->aqq.invert(this->aqq);
}
VEC3 computeBarycenterDarts()
{
VEC3 b(0);
for(std::vector<Dart>::iterator it = this->m_darts.begin() ; it != this->m_darts.end() ; ++it)
b += this->m_position[*it];
b /= this->m_darts.size();
return b;
}
typename PFP::REAL computeDeterminant(const Geom::Matrix33f& a)
{
return a(0,0)*a(1,1)*a(2,2)+a(0,2)*a(1,0)*a(2,1)+a(0,1)*a(1,2)*a(2,0)-a(0,2)*a(1,1)*a(2,0)-a(0,0)*a(1,2)*a(2,1)-a(0,1)*a(1,0)*a(2,2);
}
// LIST_MAT m_lm;
//
typename PFP::MAP& m_map;
VertexAttribute<VEC3>& m_position;
std::vector<Dart> m_darts;
VertexAttribute<VEC3> goal;
Geom::Matrix33f aqq;
std::vector<Geom::Vec3f> q;
typename PFP::REAL m_beta;
};
#endif
#ifndef SHAPEMATCHING_LINEAR_H
#define SHAPEMATCHING_LINEAR_H
#include "shapeMatching.h"
using namespace CGoGN;
template <typename PFP>
class ShapeMatchingLinear : public ShapeMatchingGen<PFP>
{
public:
ShapeMatchingLinear(typename PFP::MAP& map, VertexAttribute<VEC3>& position,std::vector<Dart> v_darts, typename PFP::REAL beta);
~ShapeMatchingLinear();
void initialize();
void animate();
protected:
VertexAttribute<VEC3> vecToBary;
};
#include "shapeMatchingLinear.hpp"
#endif
#include "rigidXfComputation.h"
//******************************************************************************
template <typename PFP>
ShapeMatchingLinear<PFP>::ShapeMatchingLinear(typename PFP::MAP& map, VertexAttribute<VEC3>& position,std::vector<Dart> v_darts, typename PFP::REAL beta) :
ShapeMatchingGen<PFP>(map,position,v_darts,beta)
{
this->vecToBary = this->m_map.template addAttribute<VEC3, VERTEX>("");
}
//******************************************************************************
template <typename PFP>
ShapeMatchingLinear<PFP>::~ShapeMatchingLinear()
{
std::cout << "delete SM" << std::endl;
if(this->vecToBary.isValid())
this->m_map.template removeAttribute<VEC3, VERTEX>(this->vecToBary);
}
template <typename PFP>
void ShapeMatchingLinear<PFP>::initialize()
{
assert(this->m_darts.size()>0);
typedef typename PFP::VEC3 VEC3;
typedef typename PFP::REAL REAL;
//********************* first compute the data for the MATS***********************************************************
//barycenter
VEC3 x0cm = this->computeBarycenterDarts();
this->q.reserve(this->m_darts.size());
for (std::vector<Dart>::iterator it = this->m_darts.begin(); it != this->m_darts.end(); ++it)
{
VEC3 myQ(this->m_position[*it]-x0cm);
this->q.push_back(myQ);
}
this->computeAqqMatrix();
//********************* then compute the vector to barycenter for the map***********************************************************
//compute barycenter
// VEC3 b = this->computeBarycenterDarts();
for(std::vector<Dart>::iterator it = this->m_darts.begin() ; it != this->m_darts.end() ; ++it)
this->vecToBary[*it] = this->m_position[*it] - x0cm;
}
template <typename PFP>
void ShapeMatchingLinear<PFP>::animate()
{
typedef typename PFP::VEC3 VEC3;
//initialize(nfe,frame_prec);
unsigned int n = m_darts.size();
VEC3 xcm = this->computeBarycenterDarts();
std::vector<VEC3> p;
p.reserve( n );
for(std::vector<Dart>::iterator it = this->m_darts.begin(); it != this->m_darts.end(); ++it)
p.push_back(this->m_position[*it]-xcm);
Geom::Matrix33f apq;
apq.zero();
for(unsigned int i=0;i<p.size();++i)
for(unsigned int x=0;x<3;++x)
for(unsigned int y=0;y<3;++y)
apq(x,y) += p[i][x]*this->q[i][y];
Geom::Matrix33f s = apq.transposed()*apq;
Geom::Matrix33f sr;
RigidComputation::jacobiDiagonalization(s,sr);
s(0,0) = 1.0f/sqrt(s(0,0));
s(1,1) = 1.0f/sqrt(s(1,1));
s(2,2) = 1.0f/sqrt(s(2,2));
s = sr*s*sr.transposed();
Geom::Matrix33f r = apq*s;
Geom::Matrix33f a = apq*this->aqq;
typename PFP::REAL det = this->computeDeterminant(a);
typename PFP::REAL detAInv = 1.0f/powf(det,1.0f/3.0f);
if(std::isfinite(detAInv))
{
for(unsigned int x=0;x<3;++x)
for(unsigned int y=0;y<3;++y)
r(x,y) = this->m_beta*a(x,y)*detAInv+(1.0f-this->m_beta)*r(x,y);
}
else
{
std::cout << "no linear deformation (planar config ?), applying identity" << std::endl;
r.identity();
}
//once the transformation computed : move the vertices of the map
//compute barycenter
VEC3 b = this->computeBarycenterDarts();
//move the vertices
for(std::vector<Dart>::iterator it = this->m_darts.begin() ; it != this->m_darts.end() ; ++it)
{
this->goal[*it] = r*this->vecToBary[*it]+xcm;
}
}
#ifndef SHAPEMATCHING_QUAD_H
#define SHAPEMATCHING_QUAD_H
#include "shapeMatching.h"
#include "Geometry/vector_gen.h"
using namespace CGoGN;
template <typename PFP>
class ShapeMatchingQuadratic : public ShapeMatchingGen<PFP>
{
public:
ShapeMatchingQuadratic(typename PFP::MAP& map, VertexAttribute<VEC3>& position,std::vector<Dart> v_darts, typename PFP::REAL beta);
~ShapeMatchingQuadratic();
void initialize();
void animate();
protected:
Geom::Matrix<9,9,typename PFP::REAL> aqqQuad;
//vectors to barycenter
std::vector<Geom::Vector<9,typename PFP::REAL> > qQuad;
VertexAttribute<Geom::Vector<9,typename PFP::REAL> > vecQuad ;
};
#include "shapeMatchingQuadratic.hpp"
#endif
#include "rigidXfComputation.h"
using namespace CGoGN;
//******************************************************************************
template <typename PFP>
ShapeMatchingQuadratic<PFP>::ShapeMatchingQuadratic(typename PFP::MAP& map, VertexAttribute<VEC3>& position,std::vector<Dart> v_darts, typename PFP::REAL beta) :
ShapeMatchingGen<PFP>(map,position,v_darts,beta)
{
this->vecQuad = this->m_map.template addAttribute<Geom::Vector<9,typename PFP::REAL>,VERTEX >("");
}
//******************************************************************************
template <typename PFP>
ShapeMatchingQuadratic<PFP>::~ShapeMatchingQuadratic()
{
if(this->vecQuad.isValid())
this->m_map.template removeAttribute<Geom::Vector<9,typename PFP::REAL> >(this->vecQuad);
}
template <typename PFP>
void ShapeMatchingQuadratic<PFP>::initialize()
{
assert(this->m_darts.size()>0);
typedef typename PFP::VEC3 VEC3;
typedef typename PFP::REAL REAL;
//********************* first compute the data for the MATS***********************************************************
//barycenter
VEC3 x0cm = this->computeBarycenterDarts();
this->q.reserve(this->m_darts.size());
for (std::vector<Dart>::iterator it = this->m_darts.begin(); it != this->m_darts.end(); ++it)
{
VEC3 myQ(this->m_position[*it]-x0cm);
this->q.push_back(myQ);
Geom::Vector<9,typename PFP::REAL> myQQuad;
myQQuad[0] = myQ[0];
myQQuad[1] = myQ[1];
myQQuad[2] = myQ[2];
myQQuad[3] = myQ[0]*myQ[0];
myQQuad[4] = myQ[1]*myQ[1];
myQQuad[5] = myQ[2]*myQ[2];
myQQuad[6] = myQ[0]*myQ[1];
myQQuad[7] = myQ[1]*myQ[2];
myQQuad[8] = myQ[2]*myQ[0];
qQuad.push_back(myQQuad);
}
this->computeAqqMatrix();
aqqQuad.zero();
for(unsigned int i = 0; i < this->q.size(); ++i)
for(int x=0;x<9;++x)
for(int y=0;y<9;++y)
aqqQuad(x,y)+= qQuad[i][x]*qQuad[i][y];
aqqQuad.invert(aqqQuad);
//********************* then compute the vector to barycenter for the map***********************************************************
//compute barycenter
// VEC3 b = this->computeBarycenterDarts();
for(std::vector<Dart>::iterator it = this->m_darts.begin() ; it != this->m_darts.end() ; ++it)
{
VEC3 toBary = this->m_position[*it] - x0cm;
Geom::Vector<9,typename PFP::REAL> myQQuad;
myQQuad[0] = toBary[0];
myQQuad[1] = toBary[1];
myQQuad[2] = toBary[2];
myQQuad[3] = toBary[0]*toBary[0];
myQQuad[4] = toBary[1]*toBary[1];
myQQuad[5] = toBary[2]*toBary[2];
myQQuad[6] = toBary[0]*toBary[1];
myQQuad[7] = toBary[1]*toBary[2];
myQQuad[8] = toBary[2]*toBary[0];
this->vecQuad[*it] = myQQuad;
}
}
template <typename PFP>
void ShapeMatchingQuadratic<PFP>::animate()
{
typedef typename PFP::VEC3 VEC3;
unsigned int n = this->m_darts.size();
VEC3 xcm = this->computeBarycenterDarts();
std::vector<VEC3> p;
p.reserve( n );
Geom::Matrix<3,9,typename PFP::REAL> apqQuad;
apqQuad.zero();
Geom::Matrix33f apq;
apq.zero();
unsigned int i=0;
for(std::vector<Dart>::iterator it = this->m_darts.begin(); it != this->m_darts.end(); ++it, ++i)
{
p.push_back(this->m_position[*it]-xcm);
for(unsigned int x=0; x<3; x++ )
for(unsigned int y=0; y<9; y++ )
{
if(y<3)
apq(x,y) += p[i][x]*this->q[i][y];
apqQuad(x,y) += p[i][x] * qQuad[i][y];
}
}
Geom::Matrix33f s = apq.transposed()*apq;
Geom::Matrix33f sr;
RigidComputation::jacobiDiagonalization(s,sr);
s(0,0) = 1.0f/sqrt(s(0,0));
s(1,1) = 1.0f/sqrt(s(1,1));
s(2,2) = 1.0f/sqrt(s(2,2));
s = sr*s*sr.transposed();
Geom::Matrix33f r = apq*s;
//compute R tilde
Geom::Matrix<3,9,typename PFP::REAL> r_big;
r_big.zero();
for(unsigned int x=0; x<3 ; ++x)
for(unsigned int y=0; y<3 ; ++y)
r_big(x,y) = r(x,y);
//compute quadratic deformation
//#define BETA 0.5f
Geom::Matrix<3,9,typename PFP::REAL> a = apqQuad*this->aqqQuad;
Geom::Matrix<9,9,typename PFP::REAL> a_square;
a_square.identity();
for(unsigned int x=0; x<3; ++x)
for(unsigned int y=0; y<9; ++y)
a_square(x,y) = a(x,y);
typename PFP::REAL det = a_square.invert(a_square);
det = 1.0f/pow(fabs(det),1.0f/9.0f);
if(det<0.0f)
det = -det;
a *= det;
//Geom::Matrix<3,9,typename PFP::REAL> t = BETA*a+(1.0f-BETA)*r_big;
Geom::Matrix<3,9,typename PFP::REAL> t = this->m_beta*a+(1.0f-this->m_beta)*r_big;
//once the transformation computed : move the vertices of the map
//compute barycenter
// VEC3 b = this->computeBarycenterDarts();
// std::cout << "b " << b << std::endl;
//move the vertices
for(std::vector<Dart>::iterator it = this->m_darts.begin() ; it != this->m_darts.end() ; ++it)
{
Geom::Vector<3,typename PFP::REAL> q_d = t*this->vecQuad[*it];
typename PFP::VEC3 v(q_d[0], q_d[1], q_d[2]);
this->goal[*it] = v+xcm;
}
}
......@@ -168,7 +168,6 @@ public :
void update_registration(Obstacle * o);
void register_pop(Obstacle* o, int n);
void register_add(Obstacle* o, int n);
void resetObstInFace(Obstacle* o);
void resetObstPartInFace(Obstacle* o, Dart d);
void resetPart(MovingObstacle * mo, Dart d);
......
#ifndef M_MOVING_MESH_H
#define M_MOVING_MESH_H
#include <iostream>
#include "utils.h"
#include "env_map.h"
#include "moving_obstacle.h"
#include "Algo/Import/import.h"
#include "ShapeMatching/shapeMatchingQuadratic.h"
using namespace std;
class MovingMesh
{
public:
MovingMesh(EnvMap * envMap, Dart d, std::string filename);
void linkWithObstacle(MovingObstacle * mo);
void scale(float val);
void moveInFace(typename PFP::MAP& motherMap, Dart d, VertexAttribute<VEC3> pos);
void move();
void animate();
std::vector<VEC3> computeProjectedPointSet();
std::vector<VEC3> jarvisConvexHull(std::vector<VEC3> projectedPointSet);
std::vector<VEC3> computeSkeleton(std::vector<VEC3> projectedPointSet, unsigned int nodeNumber);
typename PFP::MAP map;
VertexAttribute<VEC3> position;
VertexAttribute<VEC3> normal;
ShapeMatchingQuadratic<PFP> * smg;
std::vector<VEC3> skeleton;
MovingObstacle * mo;
EnvMap* envMap_;
CellMarker<VERTEX> constrainedV;
};
#endif
......@@ -10,6 +10,7 @@
using namespace std;
PFP::VEC3 rotate (PFP::VEC3 pos1, PFP::VEC3 center, float angle);
float get_angle (PFP::VEC3 v1, PFP::VEC3 v2);
void register_add(Obstacle* o, int n, const std::list<Dart>& memo);
class MovingObstacle
{
......@@ -20,6 +21,8 @@ public:
bool is_inside (VEC3 p);
void computePrefVelocity();
std::list<Dart> getMemoCross(const Algo::MovingObjects::ParticleCell2D<PFP>* p1, const Algo::MovingObjects::ParticleCell2D<PFP>* p2);
void update();
void register_ (Obstacle* o,Dart d, int n );
......
......@@ -7,6 +7,7 @@
#include "agent.h"
#include "obstacle.h"
#include "moving_obstacle.h"
#include "moving_mesh.h"
#include "path_finder.h"
#include <iostream>
......
#include "ShapeMatching/rigidXfComputation.h"
using namespace CGoGN;
// gmtl::Matrix44f getRigidTransform( std::vector<Geom::Vec3f> from, std::vector<Geom::Vec3f> to,float time)
// {
// const int nb_c = from.size();
//
// /* Compute the centroids of both datasets */
// Geom::Vec3f mu1( 0.0f, 0.0f, 0.0f );
// Geom::Vec3f mu2( 0.0f, 0.0f, 0.0f );