amelioration register_update et debut ajout moving_mesh

Debug/CMakeLists.txt

@@ -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}
 )

Release/CMakeLists.txt

@@ -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}
 )

include/ShapeMatching/rigidXfComputation.h

+#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

include/ShapeMatching/shapeMatching.h

+#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

include/ShapeMatching/shapeMatchingLinear.h

+#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/ShapeMatching/shapeMatchingLinear.hpp

+#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;
+    }
+}
+

include/ShapeMatching/shapeMatchingQuadratic.h

+#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/ShapeMatching/shapeMatchingQuadratic.hpp

+#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;
+	}
+}

include/env_map.h

@@ -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);

include/moving_mesh.h

+#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

include/moving_obstacle.h

@@ -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 );

include/simulator.h

@@ -7,6 +7,7 @@
 #include "agent.h"
 #include "obstacle.h"
 #include "moving_obstacle.h"
+#include "moving_mesh.h"
 #include "path_finder.h"
 
 #include <iostream>

src/ShapeMatching/rigidXfComputation.cpp

+#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 );