up

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"
@@ -67,6 +67,7 @@ public:
 
 	void scale(float val);
 	bool is_insideConvexCell2D(VEC3 p, Dart d);
+	bool isInsideFace3D(VEC3 p, Dart d);
 	void markPedWay() ;
 
 	unsigned int mapMemoryCost() ;

include/moving_obstacle.h

@@ -28,7 +28,7 @@
 
 using namespace std;
 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 ;

src/env_map.cpp

@@ -612,7 +612,8 @@ Dart EnvMap::getBelongingCellOnSurface(const PFP::VEC3& pos)
 		if (!m.isMarked(d))
 		{
 			m.mark(d) ;
-			if (!buildingMark.isMarked(d) && Algo::Surface::Geometry::isPointInConvexFace<PFP>(map, d, position, pos, true))
+//			if (!buildingMark.isMarked(d) && Algo::Surface::Geometry::isPointInConvexFace<PFP>(map, d, position, pos, true))
+			if (!buildingMark.isMarked(d) && isInsideFace3D(pos,d) )
 			{
 					return d ;
 			}
@@ -1427,6 +1428,34 @@ void EnvMap::resetAgentInFace(Agent* agent)
 }
 #endif
 
+bool EnvMap::isInsideFace3D(VEC3 p, Dart d)
+{
+	VEC3 vec,vec2, norm=Algo::Surface::Geometry::faceNormal<PFP>(map, d, position), p1,p2 = Algo::Surface::Geometry::faceCentroid<PFP>(map, d, position) ;
+		Dart dd = d,ddd;
+
+		do
+		{
+			ddd = map.phi1(dd) ;
+			 p1 = position[dd];
+
+
+			vec = VEC3(p2 - p1);
+			vec2 = VEC3(p -p1);
+			if (vec*vec2 < 0){
+	//			CGoGNout<<"not inside !"<<CGoGNendl;
+				return false;
+
+			}
+
+			dd = ddd ;
+		} while (dd != d) ;
+		float dist = (p2-p)*norm ;
+		if(dist*dist>10.0f) return false;
+	//	CGoGNout<<"c'est dedans !"<<CGoGNendl;
+		return true;
+}
+
+
 #ifdef SPATIAL_HASHING
 Geom::Vec2ui EnvMap::agentPositionCell(Agent* a)
 {

src/moving_obstacle.cpp

@@ -63,20 +63,19 @@ bool  MovingObstacle::removeGeneralCell (Dart d)
 bool MovingObstacle::is_inside(VEC3 p)
 {
 //	return false;
-	VEC3 vec, norm, p1,p2;
+	VEC3 vec, norm, p1,vec2,p2;
 
 
 	for (unsigned int i = 0; i < nbVertices; i++)
 	{
 		 p1 = getPosition(i);
-		 p2 = getPosition((i+1)%nbVertices);
+		 p2 = center;
 
 		vec = VEC3(p2 - p1);
-		norm[0] = vec[1];
-		norm[1] = -vec[0];
 
-		vec = VEC3(p2 -p);
-		if (vec*norm < 0)
+
+		vec2 = VEC3(p -p1);
+		if (vec*vec2 < 0)
 			return false;
 	}
 
@@ -120,14 +119,6 @@ VEC3 rotate2D(VEC3 pos1, VEC3 center, float angle) // renvoie le déplacement ne
 	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()),
@@ -348,10 +339,10 @@ void MovingObstacle::draw(bool showPath)
 
 		VEC3 col = Utils::color_map_BCGYR(float(index)/float(sim_->movingObstacles_.size()));
 		m_ds->color3f(col[0],col[1],col[2]);
-
-		for(std::vector<VEC3>::iterator it = goals_.begin() ; it != goals_.end() ; ++it)
+		m_ds->vertex(center);
+		for(unsigned int i = 0 ; i < goals_.size() ; i++)
 		{
-			m_ds->vertex(*it);
+			m_ds->vertex(goals_[(curGoal_+i)%(goals_.size())]);
 		}
 
 		m_ds->end();
@@ -363,7 +354,12 @@ 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]+deformation[d];
+	if(!rigid_)
+		{
+			return position[d];
+		}
+	else return position[d]+deformation[d];
 #else
 	return position[d];
 #endif
@@ -1003,7 +999,16 @@ void MovingObstacle::updateForces()
 		for (unsigned int i = 0; i < nbVertices; ++i)
 		{
 #ifdef TWO_AND_HALF_DIM
-//			position[d] += rotate3D(position[d],position[map.phi1(d)], center, abs_angle*rotor);
+
+			//// rotation pour les obstacles rectangulaires
+			VEC3 x = parts_[i]->getPosition()-center;
+			VEC3 normale = (parts_[(i+1)%nbVertices]->getPosition()-center)^(x);
+			normale.normalize();
+			VEC3 y = normale ^x;
+			y.normalize();
+			VEC3 nouvpos = rotate2D(VEC3 (1,0,0),(0,0,0),angle);
+
+			position[d] += nouvpos[0]*x+nouvpos[1]*y;
 #else
 			position[d] += rotate2D(position[d], center, abs_angle*rotor);
 #endif
@@ -1012,7 +1017,7 @@ void MovingObstacle::updateForces()
 #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)] += rotate2D(position[map.phi<211>(d)], center, abs_angle*rotor);
 			position[map.phi<211>(d)] += (velocity_ * sim_->timeStep_);
 #endif
 

src/simulator.cpp

@@ -144,7 +144,13 @@ void Simulator::doStep()
 		movingObstacles_[i]->initForces();
 		clock_gettime(CLOCK_MONOTONIC, &endTime) ;
 		time_behave+= timespec_delta(begTime,endTime).tv_nsec;
-
+//		CGoGNout<<" Obstacle "<<i<<" : ";
+//		for(unsigned int j=0;j<movingObstacles_[i]->nbVertices;j++)
+//		{
+//			VEC3 dist =movingObstacles_[i]->parts_[j]->getPosition()-movingObstacles_[i]->parts_[(j+1)%movingObstacles_[i]->nbVertices]->getPosition();
+//			CGoGNout<<dist.norm()<<" || ";
+//		}
+//		CGoGNout<<CGoGNendl;
 
 	}
 
@@ -807,41 +813,19 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
 			VEC3 ySide (0.0f,5.0f,0.0f);
 
 #ifdef TWO_AND_HALF_DIM
+			float xSideF=2.0f;
+			float ySideF=5.0f;
 			Geom::Plane3D<PFP::REAL> pl(Algo::Surface::Geometry::facePlane<PFP>(envMap_.map, d, envMap_.position));
+			VEC3 norm =Algo::Surface::Geometry::faceNormal<PFP>(envMap_.map, d, envMap_.position) ;
+			VEC3 dir1 =VEC3(norm[1]+norm[2],norm[2]-norm[0],-norm[0]-norm[1]);
 
-			VEC3 np;
-
-			VEC3 displ = -xSide-ySide;
-			Agent::rotate(Agent::xyPlane, pl.normal(), 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);
-
-			np = start+displ;
-			pl.project(np);
-
-			vPos.push_back(np);
-
-			displ = xSide+ySide;
-			Agent::rotate(Agent::xyPlane, pl.normal(), displ);
-
-			np = start+displ;
-			pl.project(np);
-
-			vPos.push_back(np);
-
-			displ = ySide-xSide;
-			Agent::rotate(Agent::xyPlane, pl.normal(), displ);
-
-			np = start+displ;
-			pl.project(np);
+			dir1.normalize();
+			VEC3 dir2 = norm ^dir1;
 
-			vPos.push_back(np);
+			vPos.push_back(start-xSideF*dir1-ySideF*dir2);
+			vPos.push_back(start+xSideF*dir1-ySideF*dir2);
+			vPos.push_back(start+xSideF*dir1+ySideF*dir2);
+			vPos.push_back(start-xSideF*dir1+ySideF*dir2);
 #else
 			vPos.push_back(start-xSide-ySide);
 			vPos.push_back(start+xSide-ySide);