diff --git a/include/env_map.h b/include/env_map.h index d44980962d5ec354d4a13759f8c45b66fa66d7a1..1a41025676c84d7cfca0ab7827f8dc2dcaca9de8 100644 --- a/include/env_map.h +++ b/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() ; diff --git a/include/moving_obstacle.h b/include/moving_obstacle.h index 3657d6401fd7edc9fa5169f347e502defc371cf6..24849ea93bf2cc4409836ed92c4c26a0bf608b26 100644 --- a/include/moving_obstacle.h +++ b/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 ; diff --git a/src/env_map.cpp b/src/env_map.cpp index 02eb03f234bf6efdda60dd549828cef7e531d804..84e9ad04f4d8469f8b1b0a96c0c95901ceed369b 100644 --- a/src/env_map.cpp +++ b/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(map, d, position, pos, true)) +// if (!buildingMark.isMarked(d) && Algo::Surface::Geometry::isPointInConvexFace(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(map, d, position), p1,p2 = Algo::Surface::Geometry::faceCentroid(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 !"<10.0f) return false; + // CGoGNout<<"c'est dedans !"< pos, std::vector 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::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 diff --git a/src/simulator.cpp b/src/simulator.cpp index cc350fc8f06ef3b0b00aca36763749be4d06b502..3edd8bc776e0bdf2669bba7ad8852bc2d32c85e3 100644 --- a/src/simulator.cpp +++ b/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 "<nbVertices;j++) +// { +// VEC3 dist =movingObstacles_[i]->parts_[j]->getPosition()-movingObstacles_[i]->parts_[(j+1)%movingObstacles_[i]->nbVertices]->getPosition(); +// CGoGNout< pl(Algo::Surface::Geometry::facePlane(envMap_.map, d, envMap_.position)); + VEC3 norm =Algo::Surface::Geometry::faceNormal(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);