presque réparé 2.5

include/env_map.h

@@ -33,7 +33,7 @@ class ArticulatedObstacle;
 #include "pfp.h"
 
 //#define EXPORTING3
-//#define TWO_AND_HALF_DIM
+#define TWO_AND_HALF_DIM
 
 #ifdef EXPORTING3
 	#include "Utils/Shaders/shaderPhongTexture.h"
@@ -73,6 +73,7 @@ public:
 
 #ifndef SPATIAL_HASHING
 	Dart getBelongingCell(const VEC3& pos) ;
+	Dart getBelongingCellOnSurface(const VEC3& pos) ;
 
 	void subdivideAllToMaxLevel() ;
 	void subdivideToProperLevel() ;

include/moving_obstacle.h

@@ -27,7 +27,8 @@
 
 
 using namespace std;
-PFP::VEC3 rotate (PFP::VEC3 pos1, PFP::VEC3 center, float angle);
+PFP::VEC3 rotate2D (PFP::VEC3 pos1, PFP::VEC3 center, float angle);
+PFP::VEC3 rotate3D (PFP::VEC3 pos1,PFP::VEC3 pos2, PFP::VEC3 center, float angle);
 float get_angle (PFP::VEC3 v1, PFP::VEC3 v2);
 PFP::VEC3 get_center (ArticulatedObstacle * art, int index);
 class Simulator ;
@@ -144,7 +145,7 @@ public:
 	bool rigid_;
 	bool spinning;
 	ArticulatedObstacle * parent;
-
+	float hight;
 	MovingMesh* mm_;
 	Agent* ag_;
 	int index_parent;

include/simulator.h

@@ -100,7 +100,7 @@ public:
 
 	void setupPlanetScenario(unsigned int nbAgents, unsigned int nbObstacles);
 
-	void addMovingObstacles(unsigned int nb);
+	void addMovingObstacles(unsigned int nb, unsigned int type);
 	void addMovingObstacle(Dart d, unsigned int obstType=0);
 
 	void addAgent(const VEC3& start,const VEC3& goals);

include/viewer.h

@@ -289,6 +289,8 @@ public slots:
 		{
 			for(unsigned int i=0 ; i < simulator.movingObstacles_.size() ; ++i)
 			{
+//				for(unsigned int j=0 ; j < simulator.movingObstacles_[i]->nbVertices ; ++j)
+//				{
 					m_triObst_render.push_back(new Algo::Render::GL2::MapRender());
 
 					Utils::ShaderSimpleColor * shader = new Utils::ShaderSimpleColor();
@@ -304,6 +306,7 @@ public slots:
 					m_triObst_VBO.push_back(vbo);
 					m_triObst_Shader.push_back(shader);
 					registerShader(shader);
+//				}
 			}
 		}
 

src/env_map.cpp

@@ -593,14 +593,36 @@ Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
 		if (!m.isMarked(d))
 		{
 			m.mark(d) ;
-			if (!buildingMark.isMarked(d)
-			    && Algo::Surface::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true)) return d ;
+			if (!buildingMark.isMarked(d) && Algo::Surface::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true))
+				return d ;
 		}
 	}
 
 	std::cout << "ERROR : pos not in map for getBelongingCell " << pos << std::endl ;
 	return map.begin() ;
 }
+
+Dart EnvMap::getBelongingCellOnSurface(const PFP::VEC3& pos)
+{
+//	assert(map.getCurrentLevel() == map.getMaxLevel()) ;
+
+	CellMarkerStore<FACE> m(map) ;
+	for (Dart d = map.begin(); d != map.end(); map.next(d))
+	{
+		if (!m.isMarked(d))
+		{
+			m.mark(d) ;
+			if (!buildingMark.isMarked(d) && Algo::Surface::Geometry::isPointInConvexFace<PFP>(map, d, position, pos, true))
+			{
+					return d ;
+			}
+		}
+	}
+
+	std::cout << "ERROR : pos not in map for getBelongingCellonSurface " << pos << std::endl ;
+	return map.begin() ;
+}
+
 #endif
 
 #ifndef SPATIAL_HASHING

src/moving_obstacle.cpp

@@ -110,7 +110,7 @@ float get_angle(VEC3 v1, VEC3 v2) //renvoie l'angle entre [- pi ; Pi] du v2 à v
 	return flo;
 }
 
-VEC3 rotate(VEC3 pos1, VEC3 center, float angle) // renvoie le déplacement necessaire depuis pos1 pour effectuer la rotation centrée en center d'angle angle
+VEC3 rotate2D(VEC3 pos1, VEC3 center, float angle) // renvoie le déplacement necessaire depuis pos1 pour effectuer la rotation centrée en center d'angle angle
 {
 	VEC3 pos2;
 	PFP::REAL x = pos1[0] - center[0];
@@ -120,6 +120,15 @@ VEC3 rotate(VEC3 pos1, VEC3 center, float angle) // renvoie le déplacement nece
 	return pos2;
 }
 
+VEC3 rotate3D(VEC3 pos1,VEC3 pos2, VEC3 center, float angle) // renvoie le déplacement necessaire depuis pos1 pour effectuer la rotation centrée en center d'angle angle sur le plan pos1 pos2 center
+{
+	VEC3 pos3;
+	/// rotation dans la base (center,pos1,pos2)
+//	PFP::VEC3 inBase =rotate2D(VEC3(1,0,0),VEC3(0,0,0), angle);
+	///// changer de base ///
+	return pos3;
+}
+
 MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool rigid, bool spin,int curGoal, Dart dInside, ArticulatedObstacle * art, int indParent) :
 		nbVertices(pos.size()),
 		center(0),
@@ -170,11 +179,12 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
 
 	for (unsigned int i = 0; i < nbVertices; ++i)
 	{
-		Dart d = sim_->envMap_.getBelongingCell(pos[i]);
 #ifdef TWO_AND_HALF_DIM
+		Dart d = sim_->envMap_.getBelongingCellOnSurface(pos[i]);
 		CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>* part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>(sim_->envMap_.map, d, pos[i], sim_->envMap_.position);
 
 #else
+		Dart d = sim_->envMap_.getBelongingCell(pos[i]);
 		CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>* part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>(sim_->envMap_.map, d, pos[i], sim_->envMap_.position);
 #endif
 		parts_[i] = part;
@@ -237,7 +247,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
 
 	//extrude face to build a cage
 	// compute edgeLength for mass-spring
-	Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, -10.0f) ;
+	hight=10.0f;
+	Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, -hight) ;
 //	Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, 0.0f) ;
 	map.fillHole(groundFace);
 	groundFace = map.phi2(groundFace);
@@ -259,7 +270,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
 			if(!map.isBoundaryMarked2(d) && !treated.isMarked(d))
 			{
 				treated.mark(d);
-				vertexAngle[d] = Algo::Surface::Geometry::angle<PFP>(map,map.phi_1(d),map.phi1(d),position);
+				vertexAngle[d] = Algo::Surface::Geometry::angle<PFP>(map,map.phi_1(d),map.phi2(map.phi1(d)),position);
 			}
 		}
 
@@ -351,8 +362,11 @@ void MovingObstacle::draw(bool showPath)
 VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
 {
 	Dart d(ind); //WARNING : works only for one face created at start !
+#ifndef TWO_AND_HALF_DIM
 	return position[d]+deformation[d];
-//	return position[d];
+#else
+	return position[d];
+#endif
 }
 
 VEC3 MovingObstacle::getPosition(unsigned int ind)
@@ -802,6 +816,7 @@ void MovingObstacle::updateForces()
 	Dart d;
 	velocity_[0] = newVelocity_[0] * velocity_factor;
 	velocity_[1] = newVelocity_[1] * velocity_factor;
+	velocity_[2] = newVelocity_[2] * velocity_factor;
 	// velocity_[0] = 0.0;
 	// velocity_[1] = 0.0;
 
@@ -826,7 +841,7 @@ void MovingObstacle::updateForces()
 		for (unsigned int i = 0; i < nbVertices; ++i)
 		{
 			//initialisation of forces with viscosity
-			forces[dd] += -0.2f*velocity[dd];
+			forces[dd] += -0.9f*velocity[dd];
 			// forces[dd] = VEC3(0.0);
 			map.next(dd);
 		}
@@ -852,7 +867,7 @@ void MovingObstacle::updateForces()
 
 					//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
 					float norm = v1.norm();
-					float rigidity = 70.0f;
+					float rigidity = 30.0f;
 
 					float stretch = rigidity*(edgeLength[dd]-v1.norm());
 					if(norm>0.0f)
@@ -886,7 +901,16 @@ void MovingObstacle::updateForces()
 					{
 						float angularRig = 70.0f;
 
-						float curAngle = Algo::Surface::Geometry::angle<PFP>(map, map.phi_1(dd),map.phi1(dd),position);
+						float curAngle = Algo::Surface::Geometry::angle<PFP>(map, map.phi_1(dd),map.phi2(map.phi1(dd)),position);
+
+						VEC3 v1 = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, map.phi_1(dd), position);
+						VEC3 v2 = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, map.phi2(map.phi1(dd)), position);
+
+						VEC3 v3 = v1 ^ v2;
+						VEC3 vPl = Algo::Surface::Geometry::faceNormal<PFP>(map, dd, position);
+
+						if(v3 * vPl < 0.0f)
+							curAngle *= -1.0f;
 
 						float angularStretch = angularRig*(restAngle-curAngle);
 
@@ -978,14 +1002,22 @@ void MovingObstacle::updateForces()
 		Dart d = groundFace;
 		for (unsigned int i = 0; i < nbVertices; ++i)
 		{
-			position[d] += rotate(position[d], center, abs_angle*rotor);
+#ifdef TWO_AND_HALF_DIM
+//			position[d] += rotate3D(position[d],position[map.phi1(d)], center, abs_angle*rotor);
+#else
+			position[d] += rotate2D(position[d], center, abs_angle*rotor);
+#endif
 			position[d] += (velocity_ * sim_->timeStep_);
 
 #ifdef EXPORTING_BOXES
+
+#ifndef TWO_AND_HALF_DIM
 			position[map.phi<211>(d)] += rotate(position[map.phi<211>(d)], center, abs_angle*rotor);
 			position[map.phi<211>(d)] += (velocity_ * sim_->timeStep_);
 #endif
 
+#endif
+
 //			position[map.phi<211>(d)] += (velocity_ * sim_->timeStep_);
 
 			bary += position[d];
@@ -1044,13 +1076,23 @@ void MovingObstacle::applyForces()
 	for (unsigned int i = 0; i < nbVertices; ++i)
 	{
 #ifdef TWO_AND_HALF_DIM
-			std::cout << " dist supp " << (getDilatedPosition(i)-parts_[i]->getPosition()).norm() << std::endl;
+
+
+//			std::cout << " dist prevu " << (getDilatedPosition(i) - parts_[i]->getPosition()).norm() << std::endl;
+
 #endif
+
 			parts_[i]->move(getDilatedPosition(i));
+
 #ifdef TWO_AND_HALF_DIM
-			std::cout << " dist " << parts_[i]->getDistance() << std::endl;
+//			std::cout << " dist " << parts_[i]->getDistance() << std::endl;
+
+			Dart d(i);
+			position[d] = parts_[i]->getPosition(); //recalage de l'obstacle sur ses particules (qui elles ont bien suivi la carte au sol)
+			PFP::VEC3 normal = CGoGN::Algo::Surface::Geometry::faceNormal<PFP>(sim_->envMap_.map, parts_[i]->d, sim_->envMap_.position);
+				normal *= hight;
 
-			position[d] = parts_[i]->getPosition();
+			position[map.phi<112>(d)] = position[d]+normal;
 #endif
 
 		//		CGoGNout << "avant une etape : Obstacle "<< i << CGoGNendl;
@@ -1058,10 +1100,10 @@ void MovingObstacle::applyForces()
 		o->p1 = getDilatedPosition(i);
 		o->p2 = getDilatedPosition((i+1) % nbVertices);
 
-#ifndef POTENTIAL
+
 		o->prevP = getDilatedPosition((i - 1 + nbVertices) % nbVertices);
 		o->nextP = getDilatedPosition((i + 2 + nbVertices) % nbVertices);
-#endif
+
 
 		Dart d1 = parts_[i]->d;
 		Dart d2 = parts_[(i+1)%nbVertices]->d;

src/simulator.cpp

@@ -16,7 +16,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) :
-	timeStep_(0.05f),
+	timeStep_(0.025f),
 //	timeStep_(0.2f),
 	globalTime_(0.0f),
 	nbSteps_(0),
@@ -67,7 +67,7 @@ void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst
 		case 3 :
 			envMap_.init(config, 1000.0f, 1000.0f, minSize, 100.0f) ; //grosses cases
 			setupScenario(nbAgent, false) ;
-			addMovingObstacles(nbObst);
+			addMovingObstacles(nbObst, 1);
 			addPathToObstacles(envMap_.buildingMark, true);
 ////			setupCityScenario(-1.0f * (12 * (70.0f / 2.0f) - 10),
 ////					-1.0f * (12 * (70.0f / 2.0f) - 10), 20, 20);
@@ -79,7 +79,7 @@ void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst
 			envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ; //svg import
 			setupScenario(nbAgent, true) ;
 //			SelectorCellNotMarked<PFP::MAP> scnm(envMap_.pedWayMark);
-			addMovingObstacles(nbObst);
+			addMovingObstacles(nbObst, 1);
 //			addPathToObstacles(envMap_.pedWayMark, false);
 			addPathsToAgents();
 			break;
@@ -87,7 +87,7 @@ void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst
 #ifdef TWO_AND_HALF_DIM
 			envMap_.init(config,200.0,200.0, minSize, 400.0f);
 			setupPlanetScenario(nbAgent,nbObst);
-			addMovingObstacles(nbObst);
+			addMovingObstacles(nbObst, 0);
 			addPathToObstacles(envMap_.buildingMark, true);
 #else
 			std::cout << "Agents not in 2.5D mode" << std::endl;
@@ -761,7 +761,7 @@ void Simulator::setupPlanetScenario(unsigned int nbAgents, unsigned int nbObstac
 	swapAgentsGoals();
 }
 
-void Simulator::addMovingObstacles(unsigned int nb)
+void Simulator::addMovingObstacles(unsigned int nb, unsigned int type)
 {
 	TraversorF<PFP::MAP> tF(envMap_.map);
 	Dart d = tF.begin() ;
@@ -786,7 +786,7 @@ void Simulator::addMovingObstacles(unsigned int nb)
 		}
 
 		if (found)
-			addMovingObstacle(dCell,1) ;
+			addMovingObstacle(dCell,type) ;
 	}
 }
 
@@ -807,23 +807,41 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
 			VEC3 ySide (0.0f,5.0f,0.0f);
 
 #ifdef TWO_AND_HALF_DIM
-			Geom::Plane3D<PFP::REAL> pl(Algo::Surface::Geometry::faceNormal<PFP>(envMap_.map, d, envMap_.position), 0.0f);
+			Geom::Plane3D<PFP::REAL> pl(Algo::Surface::Geometry::facePlane<PFP>(envMap_.map, d, envMap_.position));
+
+			VEC3 np;
 
 			VEC3 displ = -xSide-ySide;
 			Agent::rotate(Agent::xyPlane, pl.normal(), displ);
-			vPos.push_back(start + displ);
+
+			np = start+displ;
+			pl.project(np); //<- without this line, the point is not on the plane
+			pl.project(np); //<- without this line, the point is not on the plane (no doublon)
+			vPos.push_back(np);
 
 			displ = xSide-ySide;
 			Agent::rotate(Agent::xyPlane, pl.normal(), displ);
-			vPos.push_back(start + displ);
+
+			np = start+displ;
+			pl.project(np);
+
+			vPos.push_back(np);
 
 			displ = xSide+ySide;
 			Agent::rotate(Agent::xyPlane, pl.normal(), displ);
-			vPos.push_back(start + displ);
+
+			np = start+displ;
+			pl.project(np);
+
+			vPos.push_back(np);
 
 			displ = ySide-xSide;
 			Agent::rotate(Agent::xyPlane, pl.normal(), displ);
-			vPos.push_back(start + displ);
+
+			np = start+displ;
+			pl.project(np);
+
+			vPos.push_back(np);
 #else
 			vPos.push_back(start-xSide-ySide);
 			vPos.push_back(start+xSide-ySide);
@@ -879,11 +897,11 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
 		TraversorV<PFP2::MAP> tv(mm->map);
 		for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
 		{
-			mm->position[d] += rotate(mm->position[d], bary, angle);
+			mm->position[d] += rotate2D(mm->position[d], bary, angle);
 		}
 
-		for(std::vector<VEC3>::iterator it = vPos.begin() ; it != vPos.end() ; ++it)
-			*it += rotate(*it , bary, angle);
+//		for(std::vector<VEC3>::iterator it = vPos.begin() ; it != vPos.end() ; ++it)
+//			*it += rotate2D(*it , bary, angle);
 	}
 
 	std::vector<VEC3> goals;

src/viewer.cpp

@@ -457,8 +457,10 @@ void SocialAgents::updateObstacleVBO()
 
 void SocialAgents::updateObstaclePredTriVBO()
 {
+//	CGoGNout<<"update"<<CGoGNendl;
 	for(unsigned int i=0 ; i < simulator.movingObstacles_.size() ; ++i)
 	{
+
 		Utils::VBO * vbo = m_triObst_VBO[i];
 		PFP::VEC3* data = static_cast<PFP::VEC3*>(vbo->lockPtr());
 		for(unsigned int j =0; j<simulator.movingObstacles_[i]->nbVertices; j++ )
@@ -472,8 +474,9 @@ void SocialAgents::updateObstaclePredTriVBO()
 
 			p = simulator.envMap_.position[simulator.envMap_.map.phi1(simulator.movingObstacles_[i]->parts_[j]->d)];
 			data[j*4+2] = p;
-		}
 
+
+		}
 		vbo->releasePtr();
 	}
 }
@@ -788,7 +791,7 @@ void SocialAgents::cb_redraw()
 	{
 		updateObstaclePredTriVBO();
 
-		for(unsigned int i = 0 ; i < simulator.movingObstacles_.size() ; ++i)
+		for(unsigned int i = 0 ; i < m_triObst_Shader.size() ; ++i)
 		{
 			Utils::ShaderSimpleColor* moShader = m_triObst_Shader[i];
 			MovingObstacle * mo =simulator.movingObstacles_[i];
@@ -1976,6 +1979,7 @@ int main(int argc, char** argv)
 	SocialAgents sa(config, minSize, nbAgent, nbObst,multires, iterations) ;
 	sa.setGeometry(0, 0, 1800, 1200) ;
 	sa.initGUI() ;
+	glewInit();
 	sa.show() ;
 //	if (argc > 1)
 //		sa.timer->start() ;