3D presque réparé

include/env_generator.hpp

@@ -81,7 +81,7 @@ void generateCity(EnvMap& envMap, unsigned int nbBuildings)
 #ifndef TWO_AND_HALF_DIM
  			generateBuilding<PFP>(envMap, d, (1 + (rand() % 3)) * 50.0f, rand() % 4) ;
 #else
- 			generateBuilding<PFP>(envMap, d, (1 + (rand() % 3)) * 10.0f, rand() % 3) ;
+ 			generateBuilding<PFP>(envMap, d, (1 + (rand() % 3)) * 40.0f, rand() % 3) ;
 
 #endif
 			--nbBuilding ;
@@ -379,7 +379,8 @@ Dart generateBuilding(EnvMap& envMap, Dart d, float height, unsigned int buildin
 
 		Dart next = map.phi1(dd) ;
 		Dart previous = map.phi_1(dd) ;
-		Obstacle* o = new Obstacle(position[next], position[dd],position[map.phi1(next)] , position[previous] , NIL, NIL, NULL, 0);
+		Obstacle* o = new Obstacle(position[dd], position[next], position[previous],
+		                           position[map.phi1(next)], NIL, NIL, NULL, 0);
 
 #ifdef SPATIAL_HASHING
 		VEC3 ov = o->p2 - o->p1 ;

include/env_map.h

@@ -104,6 +104,7 @@ public:
 	void refine() ;
 	void coarse() ;
 	void updateMap() ;
+	int testOrientation(VEC3 p, VEC3 p1, VEC3 p2, Dart d);
 
 	void resetAgentInFace(Agent* agent) ;
 #endif

include/moving_obstacle.h

@@ -14,7 +14,7 @@
 	#include "Algo/MovingObjects/particle_cell_2D_memo.h"
 #endif
 
-//#define EXPORTING_BOXES
+#define EXPORTING_BOXES
 
 #ifdef EXPORTING_BOXES
 	#include "Algo/Render/GL2/mapRender.h"
@@ -28,7 +28,7 @@
 
 using namespace std;
 PFP::VEC3 rotate2D (PFP::VEC3 pos1, PFP::VEC3 center, float angle);
-
+VEC3 computeForce(VEC3 p, VEC3 p1, VEC3 p2, float obst_radius_infl, float obst_power, float obst_stiffness, VEC3 normFace);
 float get_angle (PFP::VEC3 v1, PFP::VEC3 v2);
 PFP::VEC3 get_center (ArticulatedObstacle * art, int index);
 class Simulator ;

src/agent.cpp

@@ -356,7 +356,7 @@ void Agent::updateObstacleNeighbors()
 			if ((obstacleNeighbors_.size() < maxMovingObstacles_|| distSq < maxDistObst)
 					&& distSq < rangeSq_)
 			{
-//				if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
+//				if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1)
 				{
 
 					if (distSq > maxDistObst)
@@ -376,7 +376,7 @@ void Agent::updateObstacleNeighbors()
 				if ((movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst)
 						&& distSq < rangeSq_)
 				{
-//					 if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
+//					if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1)
 					{
 
 						if (distSq > maxDistMovingObst)
@@ -398,7 +398,7 @@ void Agent::updateObstacleNeighbors()
 			if ((obstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistObst)
 					&&distSq < rangeSq_)
 			{
-//				if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
+//				if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1)
 				{
 
 					if (distSq > maxDistObst) maxDistObst = distSq ;
@@ -414,7 +414,7 @@ void Agent::updateObstacleNeighbors()
 			if ((movingObstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistMovingObst)
 					&&distSq < rangeSq_)
 			{
-//				 if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
+//				if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1)
 				{
 
 					if (distSq > maxDistMovingObst) maxDistMovingObst = distSq ;
@@ -684,7 +684,7 @@ void Agent::computePrefVelocity()
 	// Si l'agent arrive à proximité de l'objectif,
 	// alors il passe à l'objectif suivant
 
-	if (goalDist2 < radius_*radius_*100.0f)
+	if (goalDist2 < radius_*radius_*30.0f)
 	{
 		curGoal_ = (curGoal_ + 1) % goals_.size() ;
 		goalVector = goals_[curGoal_] - getPosition() ;
@@ -745,6 +745,7 @@ static int moAppend(MovingObstacle **moving_obstacles, int nb_mos, int max, Movi
 void Agent::computeNewVelocity()
 {
 	// The objective is to compute the sum of forces exerted on the agent.
+	VEC3 normFace = Algo::Surface::Geometry::faceNormal<PFP>(sim_->envMap_.map, part_.d, sim_->envMap_.position);
 	double collision_softening_factor;
 	float ag_ambient_damping = 10.0;
 
@@ -826,7 +827,7 @@ void Agent::computeNewVelocity()
 				collision_softening_factor = pow(1-sum_of_dists/rest_sum_of_dists,obst_power);
 				force_value = obst_stiffness*collision_softening_factor*(rest_sum_of_dists - sum_of_dists);
 				VEC3 v_obst = p2 - p1;
-				VEC3 normal = VEC3(v_obst[1],-v_obst[0],0);
+				VEC3 normal = normFace ^v_obst;
 				// Ajouter une composante tangentielle
 				normal += v_obst * ((d1-d2)/(5*sum_of_dists));
 				// Le facteur 5 est là seulement pour diminuer l'influence de la composante tangentielle
@@ -873,7 +874,7 @@ void Agent::computeNewVelocity()
 
 	nobst=0;
 	// double fixed_obst_stiffness = 50000.0; // agent-obstacle interaction stiffness
-	double fixed_obst_stiffness = 50000.0; // agent-obstacle interaction stiffness
+	double fixed_obst_stiffness = 5000.0; // agent-obstacle interaction stiffness
 	// double fixed_obst_damping = 1.0;  // agent-obstacle interaction damping
 	int fixed_obst_power = 1;           // the power to which elevate the agent-obstacle distance
 	Obstacle* fixed_obst ;
@@ -885,7 +886,7 @@ void Agent::computeNewVelocity()
 		double dist = it->first;
 		// cerr << "nobst=" << nobst << "dist=" << dist << endl;
 		// double effective_range = 50*range_;
-		double effective_range = 10*range_;
+		double effective_range = 1*range_;
 		float force_value=0.0;
 		if(dist < effective_range)
 		{
@@ -900,11 +901,8 @@ void Agent::computeNewVelocity()
 		VEC3 p2=fixed_obst->p2 ;
 		vec=p2-p1;
 		vec.normalize();
-		norm.zero();
-
-		norm[0]=vec[1] ;
-		norm[1]=-vec[0] ;
-		forces += force_value * norm;
+		norm= normFace ^vec;
+		forces -= force_value * norm;
 		// cerr << "obstacles fixes : nobst=" << nobst << " dist=" << dist << " force=" << force_value*norm << endl;
 
 		nobst++;

src/env_map.cpp

@@ -1459,6 +1459,23 @@ bool EnvMap::isInsideFace3D(VEC3 p, Dart d)
 		return true;
 }
 
+int EnvMap::testOrientation(VEC3 p, VEC3 p1, VEC3 p2, Dart d)
+{
+	VEC3 normal = Algo::Surface::Geometry::faceNormal<PFP>(map, d, position);
+	VEC3 vObst = p2 - p1;
+
+	VEC3 dir = normal ^vObst;
+
+	VEC3 test = p - p1;
+
+	float res = test * dir;
+
+	if(res <0) return 1;
+	if(res>0) return 0;
+	return -1;
+
+
+}
 
 #ifdef SPATIAL_HASHING
 Geom::Vec2ui EnvMap::agentPositionCell(Agent* a)

src/moving_obstacle.cpp

@@ -615,10 +615,12 @@ void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance p
 					if (/*(movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst) &&*/
 							 distSq < distance_detectionSq)
 					{
-
-//						if (distSq > maxDistMovingObst)
-//							maxDistMovingObst = distSq ;
-						movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
+//						if (sim_->envMap_.testOrientation(parts_[nbVertices]->getPosition(), (*it)->p1, (*it)->p2, parts_[nbVertices]->d) == 1)
+						{
+	//						if (distSq > maxDistMovingObst)
+	//							maxDistMovingObst = distSq ;
+							movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
+						}
 
 
 
@@ -654,10 +656,12 @@ void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance p
 					if (/*(movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst) &&*/
 							 distSq < distance_detectionSq)
 					{
-
-//						if (distSq > maxDistMovingObst)
-//							maxDistMovingObst = distSq ;
-						movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
+//						if (sim_->envMap_.testOrientation(parts_[nbVertices]->getPosition(), (*it)->p1, (*it)->p2, parts_[nbVertices]->d) == 1)
+						{
+	//						if (distSq > maxDistMovingObst)
+	//							maxDistMovingObst = distSq ;
+							movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
+						}
 					}
 				}
 			}
@@ -784,7 +788,7 @@ void MovingObstacle::initForces()
 
 //-------------------------------------------------------------
 
-VEC3 computeForce(VEC3 p, VEC3 p1, VEC3 p2, float obst_radius_infl, float obst_power, float obst_stiffness)
+VEC3 computeForce(VEC3 p, VEC3 p1, VEC3 p2, float obst_radius_infl, float obst_power, float obst_stiffness, VEC3 normFace)
 {
 	double force_value=0.0;
 	double longueur2 = (p1-p2).norm2();
@@ -798,7 +802,7 @@ VEC3 computeForce(VEC3 p, VEC3 p1, VEC3 p2, float obst_radius_infl, float obst_p
 		double collision_softening_factor = pow(1-sum_of_dists/rest_sum_of_dists,obst_power);
 		force_value = obst_stiffness*collision_softening_factor*(rest_sum_of_dists - sum_of_dists);
 		VEC3 v_obst = p2 - p1;
-		VEC3 normal = VEC3(v_obst[1],-v_obst[0],0);
+		VEC3 normal = normFace^v_obst;
 		// Ajouter une composante tangentielle
 
 		normal += v_obst * ((d1-d2)/sum_of_dists);
@@ -925,8 +929,10 @@ void MovingObstacle::updateForces()
 		if(sim_->config==0)
 			obst_radius_infl = 100.; // scenario 0
 		else
-			obst_radius_infl = 30.; // scenario 1 et 3
+			obst_radius_infl = 10.; // scenario 1 et 3
+
 
+		PFP::VEC3 normFace = CGoGN::Algo::Surface::Geometry::faceNormal<PFP>(sim_->envMap_.map, parts_[nbVertices]->d, sim_->envMap_.position);
 			do {
 				Dart dd = map.phi1(d);
 					VEC3 p = position[dd]+(velocity[dd] * sim_->timeStep_);
@@ -940,19 +946,19 @@ void MovingObstacle::updateForces()
 						VEC3 p1=obst->p1 ;
 						VEC3 p2=obst->p2 ;
 
-						forces[dd] += computeForce(p,p1,p2,obst_radius_infl,obst_power,obst_stiffness);
+						forces[dd] += computeForce(p,p1,p2,obst_radius_infl,obst_power,obst_stiffness,normFace);
 					}
 
-					// Evitement d'obstacles fixes
+					//	Evitement d'obstacles fixes
 
 					for(std::vector<std::pair<float, Obstacle*> >::iterator it = obstacleNeighbors_.begin() ;
 						it != obstacleNeighbors_.end() ; ++it)
 					{
 						Obstacle * obst = it->second;
-						VEC3 p1=obst->p1 ;
-						VEC3 p2=obst->p2 ;
+						VEC3 p1=obst->p2 ;
+						VEC3 p2=obst->p1 ;
 
-						forces[dd] += computeForce(p,p1,p2,obst_radius_infl,obst_power,10*obst_stiffness);
+						forces[dd] += computeForce(p,p1,p2,obst_radius_infl,obst_power,1*obst_stiffness,normFace);
 					}
 
 				d = map.phi<21>(d);
@@ -1337,7 +1343,7 @@ void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atte
 
 			float goalDist2 = goalVector.norm2() ;
 
-			if (goalDist2 < 1000.0f)
+			if (goalDist2 < (gravity_dist*gravity_dist) )
 			{
 				curGoal_ = (curGoal_ + 1) % goals_.size() ;
 				goalVector = goals_[curGoal_] - center ;

src/simulator.cpp

@@ -1111,12 +1111,12 @@ void Simulator::addPathsToAgents()
 				d = envMap_.map.phi1(d);
 			}
 
-//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
+			VEC3 dest = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
 
-			VEC3 dest = envMap_.position[d] + envMap_.position[envMap_.map.phi1(d)] ;
-			dest /= 2.0f ;
-			dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0);
-			dest[2] = 0 ;
+//			VEC3 dest = envMap_.position[d] + envMap_.position[envMap_.map.phi1(d)] ;
+//			dest /= 2.0f ;
+//			dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0);
+//			dest[2] = 0 ;
 
 			agents_[i]->goals_.push_back(dest) ;
 		}
@@ -1148,11 +1148,11 @@ void Simulator::addPathsToAgents()
 				d = envMap_.map.phi1(d);
 			}
 
-//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
-			VEC3 dest = envMap_.position[d] + envMap_.position[envMap_.map.phi1(d)] ;
-			dest /= 2.0f ;
-			dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0);
-			dest[2] = 0 ;
+			VEC3 dest = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
+//			VEC3 dest = envMap_.position[d] + envMap_.position[envMap_.map.phi1(d)] ;
+//			dest /= 2.0f ;
+//			dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0);
+//			dest[2] = 0 ;
 
 			agents_[i]->goals_.push_back(dest) ;
 		}
@@ -1164,11 +1164,11 @@ void Simulator::addPathsToAgents()
 
 		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
 		{
-//			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
-			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
-			dest /= 2.0f ;
-			dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0);
-			dest[2] = 0 ;
+			VEC3 dest = Algo::Surface::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
+//			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
+//			dest /= 2.0f ;
+//			dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0);
+//			dest[2] = 0 ;
 
 			agents_[i]->goals_.push_back(dest) ;
 		}

src/viewer.cpp

@@ -664,6 +664,28 @@ void SocialAgents::cb_redraw()
 
 	if (drawAgents)
 	{
+		if(drawAgentsPath)
+		{
+			for (unsigned int i=0; i< simulator.agents_.size(); ++i)
+			{
+				Agent * ag = simulator.agents_[i];
+				m_ds->newList(GL_COMPILE_AND_EXECUTE);
+				m_ds->begin(GL_LINE_STRIP);
+
+				VEC3 col = Utils::color_map_BCGYR(float(ag->agentNo)/float(simulator.agents_.size()));
+				m_ds->color3f(col[0],col[1],col[2]);
+				m_ds->vertex(ag->getPosition());
+				for(unsigned int i = 0 ; i < ag->goals_.size() ; i++)
+				{
+					m_ds->vertex(ag->goals_[(ag->curGoal_+i)%(ag->goals_.size())]);
+				}
+
+				m_ds->end();
+				m_ds->endList();
+			}
+
+		}
+
 #ifdef EXPORTING_OBJ
 		for (unsigned int i=0; i< simulator.agents_.size(); ++i)
 		{
@@ -767,8 +789,8 @@ void SocialAgents::cb_redraw()
 #endif
 		}
 		// Commente par Arash
-		 for (std::vector<MovingMesh*>::iterator it = simulator.movingMeshes_.begin() ; it != simulator.movingMeshes_.end() ; ++it)
-		 (*it)->draw();
+//		 for (std::vector<MovingMesh*>::iterator it = simulator.movingMeshes_.begin() ; it != simulator.movingMeshes_.end() ; ++it)
+//		 (*it)->draw();
 
 	}