embouteillage

src/moving_obstacle.cpp

@@ -312,7 +312,9 @@ void MovingObstacle::draw()
 	m_positionVBO->updateData(position);
 //	m_shader->setColor(Geom::Vec4f(movingObstacleNeighbors_.size()==0 ? 1.0f : 0,0.,0.,0.));
 	VEC3 col = Utils::color_map_BCGYR(float(index)/float(sim_->movingObstacles_.size()));
+
 	if(movingObstacleNeighbors_.size()==0)
+	// if(obstacleNeighbors_.size()==0)
 		m_shader->setColor(Geom::Vec4f(col[0],col[1],col[2],0));
 	else
 		m_shader->setColor(Geom::Vec4f(0.5,0.5,0.5,0));
@@ -713,6 +715,47 @@ void MovingObstacle::initForces()
 		}
 }
 
+
+//-------------------------------------------------------------
+
+VEC3 computeForce(VEC3 p, VEC3 p1, VEC3 p2, float obst_radius_infl, float obst_power, float obst_stiffness)
+{
+	double force_value=0.0;
+	double longueur2 = (p1-p2).norm2();
+	double rest_sum_of_dists = 2 * sqrt(obst_radius_infl*obst_radius_infl + longueur2/4);
+
+	double d1 = (p-p1).norm();
+	double d2 = (p-p2).norm();
+	double sum_of_dists = d1+d2;
+	if(sum_of_dists < rest_sum_of_dists)
+	{
+		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);
+		// Ajouter une composante tangentielle
+
+		normal += v_obst * ((d1-d2)/sum_of_dists);
+		// normal += v_obst * ((d1-d2)/(5*sum_of_dists));
+		// Le facteur 5 est là seulement pour diminuer l'influence de la composante tangentielle
+		normal.normalize();
+
+		// force_value *= 10;
+		/*
+		VEC3 force_vector1, force_vector2;
+		getResponse1(force_vector,p,p1,p2,&force_vector1,&force_vector2);
+		Dart d1 = obst->d1;
+		Dart d2 = obst->d2;
+		obst->mo->forces[d1] += force_vector1;
+		obst->mo->forces[d2] += force_vector2;
+		*/
+		return(force_value * normal);
+	}
+	else
+		return(VEC3(0));
+}
+
+
 //-------------------------------------------------------------
 
 void MovingObstacle::updateForces()
@@ -853,10 +896,11 @@ void MovingObstacle::updateForces()
 						obst_radius_infl = 100.; // scenario 0
 					else
 						obst_radius_infl = 30.; // scenario 1 et 3
-					float force_value;
 
 					VEC3 p = position[dd]+(velocity[dd] * sim_->timeStep_);
 
+					// Evitement d'obstacles mobiles
+
 					for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
 						it != movingObstacleNeighbors_.end() ; ++it)
 					{
@@ -864,55 +908,19 @@ void MovingObstacle::updateForces()
 						VEC3 p1=obst->p1 ;
 						VEC3 p2=obst->p2 ;
 
-						MovingObstacle *mo = obst->mo;
-						if(mo!=NULL)
-						{
-							// int N = mo->nbVertices;
-							// int ind1 = obst->index;
-							// int ind2 = (obst->index+1)%N;
-							// fprintf(stderr,"%d %d [%d]\n",ind1,ind2,N);
-							// cout << "-------------------" << mo->forces[ind1] << endl;
-						}
-						else
-							fprintf(stderr,"NULL\n");
+						forces[dd] += computeForce(p,p1,p2,obst_radius_infl,obst_power,obst_stiffness);
+					}
 
-						double longueur2 = (p1-p2).norm2();
-						double rest_sum_of_dists = 2 * sqrt(obst_radius_infl*obst_radius_infl + longueur2/4);
+					// Evitement d'obstacles fixes
 
-						double d1 = (p-p1).norm();
-						double d2 = (p-p2).norm();
-						double sum_of_dists = d1+d2;
-						if(sum_of_dists < rest_sum_of_dists)
-						{
-							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);
-							// 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
-							normal.normalize();
-
-							// force_value *= 10;
-							VEC3 force_vector = force_value * normal;
-							forces[dd] += force_vector;
-
-							// obst->index ??? indice local de l'obstacle dans le mo
-							// obst->mo : le moving_obstacle
-							// obst->mo->nbVertices
-							// obst->mo->forces[obst->index]
-							// obst->mo->forces[obst->index+1%obst->mo->nbVertices]
-							VEC3 force_vector1, force_vector2;
-							getResponse1(force_vector,p,p1,p2,&force_vector1,&force_vector2);
-							Dart d1 = obst->d1;
-							Dart d2 = obst->d2;
-							// obst->mo->forces[d1] += force_vector1;
-							// obst->mo->forces[d2] += force_vector2;
-
-							// VEC3 p11 = obst->mo->position[d1];
-							// VEC3 p22 = obst->mo->position[d2];
-							// cerr << "p1=" << p1 << " p11=" << p11 << " p2=" << p2 << " p22=" << p22 << endl;
-						}
+					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 ;
+
+						forces[dd] += computeForce(p,p1,p2,obst_radius_infl,obst_power,10*obst_stiffness);
 					}
 				}
 				dd = map.phi1(dd);