03_2013

include/moving_obstacle.h

@@ -79,6 +79,11 @@ public:
 	std::set<Dart> general_belonging;
 	VEC3 front;
 
+// voisinage
+
+	std::vector<std::pair<float, Obstacle*> > obstacleNeighbors_ ;
+	std::vector<std::pair<float, Obstacle*> > movingObstacleNeighbors_;
+
 	VEC3 finalGoal;
 	float angle;
 //	Dart finalDart;

src/moving_obstacle.cpp

@@ -398,42 +398,44 @@ void MovingObstacle::updateAgentNeighbors() // agents voisins avec distance au b
 
 void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance par rapport aux centres des segments// a mettre en place si besoin
 {
-//	obstacleNeighbors_.clear() ;
-//	movingObstacleNeighbors_.clear() ;
-//
-//	std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[part_.d] ;
-//	std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[part_.d] ;
-//	for(std::vector<Obstacle*>::const_iterator it = obst.begin() ; it != obst.end() ; ++it)
-//	{
-//		float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
-//		if (distSq < rangeSq_)
-//		{
-//			if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
-//			{
-//				if ((*it)->mo==NULL)
-//					obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
-//				else
-//				{
-//					movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
-//				}
-//			}
-//
-//		}
-//	}
-//	for(std::vector<Obstacle*>::const_iterator it = neighborObst.begin() ; it != neighborObst.end() ; ++it)
-//	{
-//		float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
-//		if(distSq < rangeSq_)
-//		{
-//			if(Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
-//			{
-//				if ((*it)->mo==NULL)
-//					obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
-//				else
-//					movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
-//			}
-//		}
-//	}
+// ARASH  en decommentant ce qui suit, on rend les obstacles perceptibles par la tete de la limace (ou du chariot)
+
+	obstacleNeighbors_.clear() ;
+	movingObstacleNeighbors_.clear() ;
+
+	std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[part_.d] ;
+	std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[part_.d] ;
+	for(std::vector<Obstacle*>::const_iterator it = obst.begin() ; it != obst.end() ; ++it)
+	{
+		float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
+		if (distSq < rangeSq_)
+		{
+			if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
+			{
+				if ((*it)->mo==NULL)
+					obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
+				else
+				{
+					movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
+				}
+			}
+
+		}
+	}
+	for(std::vector<Obstacle*>::const_iterator it = neighborObst.begin() ; it != neighborObst.end() ; ++it)
+	{
+		float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
+		if(distSq < rangeSq_)
+		{
+			if(Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
+			{
+				if ((*it)->mo==NULL)
+					obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
+				else
+					movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
+			}
+		}
+	}
 }
 
 void MovingObstacle::update()
@@ -467,6 +469,8 @@ void MovingObstacle::update()
 	else
 		rotor = abs_angle*angle ;
 
+	// masse ressort pour la limace
+
 	if(!rigid_)
 	{
 		Dart d = groundFace;
@@ -482,15 +486,19 @@ void MovingObstacle::update()
 
 		CellMarkerStore<EDGE> cm(map);
 		DartMarkerStore dm(map);
+		// ARASH : On parcourt les sommets de la grande face
 		for (unsigned int i = 1; i < nbVertices; ++i)
 		{
 			Dart dd = d;
+
+			// ARASH : On parcourt les sous-faces triangulaire de la grande face
 			do {
 				if(!cm.isMarked(dd))
 				{
 					cm.mark(dd);
 					VEC3 p1Next = position[map.phi1(dd)]+(velocity[map.phi1(dd)] * sim_->timeStep_);
 					VEC3 p2Next = position[dd]+(velocity[dd] * sim_->timeStep_);
+					// p1Next et p2Next sont la position des extremites de l'arete.
 					VEC3 v1 = (p1Next-p2Next);
 
 					//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
@@ -508,6 +516,7 @@ void MovingObstacle::update()
 				dd = map.phi1(dd);
 			} while(dd!=d);
 
+			// ARASH : les ressorts angulaires
 			do {
 				if(!dm.isMarked(dd))
 				{
@@ -537,6 +546,48 @@ void MovingObstacle::update()
 				dd = map.phi1(dd);
 			} while(dd!=d);
 
+			//-------------------------------------------------------------------------
+			// ARASH : A présent on calcule les interactions avec les autres obstacles.
+
+			do {
+				if(!cm.isMarked(dd))
+				{
+					cm.mark(dd);
+
+					VEC3 p = position[dd]+(velocity[dd] * sim_->timeStep_);
+
+					for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
+						it != movingObstacleNeighbors_.end() ; ++it)
+					{
+						Obstacle * obst = it->second;
+						VEC3 p1=obst->p1 ;
+						VEC3 p2=obst->p2 ;
+						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)
+						{
+							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();
+							forces[dd] -= force_value * normal;
+							// This force is not symmetrically applied
+							// We assume that a homologous vertex in the other obstacle
+							// is also intersecting the current obstacle
+						}
+					}
+				}
+				dd = map.phi1(dd);
+			} while(dd!=d);
+
 			map.next(d);
 		}