petite amelioration path finding

src/simulator.cpp

@@ -853,8 +853,15 @@ void Simulator::addPathsToAgents()
 
 		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
 		{
+			Dart d = *it;
+			while(envMap_.map.isBoundaryEdge(d))
+			{
+				d = envMap_.map.phi1(d);
+			}
+
 //			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
-			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
+
+			VEC3 dest = envMap_.position[d] + envMap_.position[envMap_.map.phi1(d)] ;
 			dest /= 2.0f ;
 			dest[2] = 0 ;
 
@@ -882,8 +889,14 @@ void Simulator::addPathsToAgents()
 
 		for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
 		{
+			Dart d = *it;
+			while(envMap_.map.isBoundaryEdge(d))
+			{
+				d = envMap_.map.phi1(d);
+			}
+
 //			VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
-			VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
+			VEC3 dest = envMap_.position[d] + envMap_.position[envMap_.map.phi1(d)] ;
 			dest /= 2.0f ;
 			dest[2] = 0 ;