update shapematching

include/env_map.h

@@ -34,7 +34,7 @@ class ArticulatedObstacle;
 
 //#define EXPORTING3
 
-//#define TWO_AND_HALF_DIM
+#define TWO_AND_HALF_DIM
 
 
 #ifdef EXPORTING3

include/moving_obstacle.h

@@ -2,12 +2,17 @@
 #define M_MOVING_OBSTACLE_H
 
 #include <iostream>
-#include "ShapeMatching/shapeMatchingLinear.h"
+
 #include "utils.h"
 #include "env_map.h"
 #include <set>
-
+ #define LINEAR
 // #define SECURED
+#ifdef LINEAR
+	#include "ShapeMatching/shapeMatchingLinear.h"
+#else
+	#include "ShapeMatching/shapeMatchingQuadratic.h"
+#endif
 
 #ifdef TWO_AND_HALF_DIM
 	#include "Algo/MovingObjects/particle_cell_2DandHalf_memo.h"
@@ -166,8 +171,11 @@ public:
 	VertexAttribute<NoMathIONameAttribute<std::vector<PFP::REAL> > > mvc_;
 
 	/////// ShapeMatching
-
+#ifdef LINEAR
 	ShapeMatchingLinear<PFP> * shape_;
+#else
+	ShapeMatchingQuadratic<PFP> * shape_;
+#endif
 	PFP::REAL beta;
 	float alpha;
 

src/moving_obstacle.cpp

@@ -162,8 +162,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
 		ag_(NULL),
 		index_parent(indParent),
 		gravity_dist(0),
-		beta(0.9),
-		alpha(0.1)
+		beta(0.95),
+		alpha(0.2)
 
 {
 	assert(pos.size() > 2);
@@ -371,8 +371,11 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
 
 		dDir=dInside;
 	}
-
+#ifdef LINEAR
 	shape_= new ShapeMatchingLinear<PFP>(map,position,bord,beta);
+#else
+	shape_= new ShapeMatchingQuadratic<PFP>(map,position,bord,beta);
+#endif
 	shape_->initialize();
 
 	for (unsigned int i = 0; i < nbVertices; ++i)
@@ -447,7 +450,7 @@ void MovingObstacle::draw(bool showPath)
 
 		VEC3 col = Utils::color_map_BCGYR(float(index)/float(sim_->movingObstacles_.size()));
 		m_ds->color3f(col[0],col[1],col[2]);
-		m_ds->vertex(center);
+		m_ds->vertex(position[groundFace]);
 		for(unsigned int i = 0 ; i < goals_.size() ; i++)
 		{
 			m_ds->vertex(goals_[(curGoal_+i)%(goals_.size())]);
@@ -1141,7 +1144,7 @@ void MovingObstacle::updateForces()
 		//guiding vertex = first vertex (set the displacement)
 		// forces[groundFace] = VEC3(0);
 		velocity[groundFace] = velocity_;
-
+		velocity[map.phi_1(groundFace)] = velocity_;
 		//apply force to each vertex
 		/*
 		d = groundFace;
@@ -1238,11 +1241,17 @@ void MovingObstacle::applyForces()
 	if(!rigid_)
 	{
 		Dart d = groundFace;
+		shape_->animate();
+
+
+
 		for (unsigned int i = 0; i < nbVertices; ++i)
 		{
 
-			velocity[d] += forces[d] * sim_->timeStep_;
+			velocity[d] += forces[d] * sim_->timeStep_ +(alpha/sim_->timeStep_)*(shape_->goal[d]-position[d]);
+			velocity[map.phi_1(map.phi2(d))] += forces[d] * sim_->timeStep_ +(alpha/sim_->timeStep_)*(shape_->goal[map.phi_1(map.phi2(d))]-position[map.phi_1(map.phi2(d))]);;
 			position[d] += (velocity[d] * sim_->timeStep_);
+			position [map.phi_1(map.phi2(d))]+=(velocity[map.phi_1(map.phi2(d))] * sim_->timeStep_);
 //			PFP::VEC3 normal = CGoGN::Algo::Surface::Geometry::faceNormal<PFP>(sim_->envMap_.map, parts_[i]->d, sim_->envMap_.position);
 //			normal *= height;
 //			position[map.phi_1(map.phi2(d))] = position[d]+normal;
@@ -1250,16 +1259,10 @@ void MovingObstacle::applyForces()
 		}
 //		velocity[centerDart] +=forces[centerDart] * sim_->timeStep_;
 //		position[centerDart] += (velocity[centerDart] * sim_->timeStep_);
-		shape_->animate();
 
-		do
-		{
-//			CGoGNout<<"dart : "<<d<<" || pos : " <<position[d] <<" ||  obj : "<< shape_->goal[d]<<CGoGNendl;
-			position[d]+=alpha*(shape_->goal[d]-position[d]);
-//			position[d][2]=0;
-			position [map.phi_1(map.phi2(d))]=shape_->goal[map.phi_1(map.phi2(d))];
-			d=map.phi_1(d);
-		}while (d!=groundFace);
+
+
+
 
 	}
 
@@ -1282,8 +1285,9 @@ void MovingObstacle::updateRegistration()
 			parts_[i]->move(position[d]);
 
 #ifdef TWO_AND_HALF_DIM
-
+//			CGoGNout<<"avant : "<<position[d];
 			position[d] = parts_[i]->getPosition(); //recalage de l'obstacle sur ses particules (qui elles ont bien suivi la carte au sol)
+//			CGoGNout<<" || apres : "<<position[d]<<CGoGNendl;
 #endif
 
 
@@ -1528,29 +1532,32 @@ void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atte
 			{
 	//			if(sim_->envMap_.pedWayMark.isMarked(sim_->envMap_.map.phi2(dDir)))
 	//				dDir = sim_->envMap_.map.phi1(sim_->envMap_.map.phi1(dDir));
+				goalVector = goals_[curGoal_] - position[groundFace] ;
 
-				VEC3 mid = (sim_->envMap_.position[dDir]+sim_->envMap_.position[sim_->envMap_.map.phi1(dDir)])*0.5f;
-				goalVector = mid - position[0] ;
-
-				Dart dStart=dDir;
-				VEC3 dir = center - position[0];
-				while(goalVector.norm2()==0 || sim_->envMap_.pedWayMark.isMarked(sim_->envMap_.map.phi2(dDir)) || ((dir*goalVector)>0))
-				{
-	//				std::cout << "d " << dDir << std::endl;
-	//				std::cout << "dir*goalVector " << dir*goalVector << std::endl;
-	//				std::cout << "goal vec " << goalVector << std::endl;
-
-					dDir = sim_->envMap_.map.phi_1(dDir);
-					mid = (sim_->envMap_.position[dDir]+sim_->envMap_.position[sim_->envMap_.map.phi1(dDir)])*0.5f;
-					goalVector = mid - position[0] ;
-
-					if(dDir==dStart)
-						break;
-				}
-
-				if(sim_->envMap_.map.isBoundaryEdge(sim_->envMap_.map.phi2(dDir)))
-					goalVector = VEC3(0);
 
+///////////////////////////////////////////////WTH
+//				VEC3 mid = (sim_->envMap_.position[dDir]+sim_->envMap_.position[sim_->envMap_.map.phi1(dDir)])*0.5f;
+//				goalVector = mid - position[0] ;
+//
+//				Dart dStart=dDir;
+//				VEC3 dir = center - position[0];
+//				while(goalVector.norm2()==0 || sim_->envMap_.pedWayMark.isMarked(sim_->envMap_.map.phi2(dDir)) || ((dir*goalVector)>0))
+//				{
+//	//				std::cout << "d " << dDir << std::endl;
+//	//				std::cout << "dir*goalVector " << dir*goalVector << std::endl;
+//	//				std::cout << "goal vec " << goalVector << std::endl;
+//
+//					dDir = sim_->envMap_.map.phi_1(dDir);
+//					mid = (sim_->envMap_.position[dDir]+sim_->envMap_.position[sim_->envMap_.map.phi1(dDir)])*0.5f;
+//					goalVector = mid - position[0] ;
+//
+//					if(dDir==dStart)
+//						break;
+//				}
+//
+//				if(sim_->envMap_.map.isBoundaryEdge(sim_->envMap_.map.phi2(dDir)))
+//					goalVector = VEC3(0);
+/////////////////////////////////////////////////////////////////
 	//			goalVector = mid - center ;
 
 	//			std::cout << "dDir " << dDir << " goalVec" << goalVector << std::endl;

src/simulator.cpp

@@ -17,7 +17,7 @@ timespec timespec_delta(const timespec& t1, const timespec& t2) {
 
 Simulator::Simulator(unsigned int config, unsigned int minS, unsigned int nbAgent, unsigned int nbObst, bool resolution) :
 #ifdef TWO_AND_HALF_DIM
-	timeStep_(0.25f),
+	timeStep_(0.05f),
 #else
 //	timeStep_(config > 2 ? 0.01f : 0.25f),
 	timeStep_(0.01f),

src/viewer.cpp

@@ -773,6 +773,8 @@ void SocialAgents::cb_redraw()
 		m_ds->end();
 		m_ds->endList();
 	}
+
+
 	if (drawMovingObstacles)
 	{
 #ifdef SHADOWSHELL
@@ -873,6 +875,22 @@ void SocialAgents::cb_redraw()
 
 		if (draw_dart && dartSlider < (simulator.envMap_.map.end().index))
 				m_renderTopo->overdrawDart(dartSlider,5.0f,0,1.0f,0.5f);
+
+		if(drawObstacles)
+		{
+			CGoGN::CellMarkerStore<FACE> m(simulator.envMap_.map) ;
+				for (Dart d = simulator.envMap_.map.begin(); d != simulator.envMap_.map.end(); simulator.envMap_.map.next(d))
+				{
+					if (!m.isMarked(d))
+					{
+						m.mark(d) ;
+						if(simulator.envMap_.obstvect[d].size()!=0)
+						{
+							drawCell(d,5.0f,0,1.0f,0.5f);
+						}
+					}
+				}
+		}
 	}
 
 	if (drawEnvFaces)
@@ -1199,7 +1217,7 @@ void SocialAgents::animate()
 	}
 		struct timespec begTime ;
 		clock_gettime(CLOCK_MONOTONIC, &begTime) ;
-		for(int n =1; n<30; n++)
+		for(int n =1; n<2; n++)
 		simulator.doStep() ;
 		struct timespec endTime ;
 		clock_gettime(CLOCK_MONOTONIC, &endTime) ;