diff --git a/include/env_map.h b/include/env_map.h index 0557804c1165d2f8199e356904ab805694bac231..c82b82f086d9eba01e6c2c7b0a92df1208344601 100644 --- a/include/env_map.h +++ b/include/env_map.h @@ -66,7 +66,7 @@ public: void draw(); void scale(float val); - + bool is_insideConvexCell2D(VEC3 p, Dart d); void markPedWay() ; unsigned int mapMemoryCost() ; @@ -221,6 +221,7 @@ public: void update_registration(Obstacle * o); void register_pop(Obstacle* o, int n); void resetPartSubdiv(Obstacle* o); +//void resetObstPartInFace(Obstacle* o, Dart d, unsigned int fLevel);// empeche de viser une dart ayant disparu void resetObstPartInFace(Obstacle* o, Dart d);// empeche de viser une dart ayant disparu void resetPart(Obstacle * mo, Dart d); // empeche de viser une dart ayant disparu pour les voisins void displayMO(Obstacle * o); diff --git a/include/viewer.h b/include/viewer.h index c8cb071b3119ec06cec5b50f6256859e4f01dc12..97e94449f0dbcb3f36850afb7668fbb891d789bf 100644 --- a/include/viewer.h +++ b/include/viewer.h @@ -298,7 +298,7 @@ public slots: Utils::VBO * vbo = new Utils::VBO(); vbo->setDataSize(3); - vbo->allocate(3); + vbo->allocate(4*simulator.movingObstacles_[i]->nbVertices); shader->setAttributePosition(vbo); m_triObst_VBO.push_back(vbo); diff --git a/src/agent.cpp b/src/agent.cpp index d182af9b525fd2fee9964515625e9cad6b58a4f5..a3ee700dc222f5af9c3d23eba228211f7db258a2 100644 --- a/src/agent.cpp +++ b/src/agent.cpp @@ -5,7 +5,7 @@ New behaviour algorithm based on a dynamic model \*=====================================================================*/ -#define POTENTIAL +//#define POTENTIAL #include "agent.h" #include "simulator.h" #include "Geometry/frame.h" @@ -17,14 +17,14 @@ unsigned int Agent::maxNeighbors_ = 10 ; unsigned int Agent::maxMovingObstacles_ = 20; float Agent::averageMaxSpeed_ = 2.0f ; // float Agent::averageMaxSpeed_ = 20.0f ; -// float Agent::neighborDist_ = 10.0f ; -float Agent::neighborDist_ = 20.0f ; + float Agent::neighborDist_ = 10.0f ; +//float Agent::neighborDist_ = 20.0f ; float Agent::neighborDistSq_ = neighborDist_ * neighborDist_ ; //float Agent::radius_ = 8.0f ; -float Agent::radius_ = 3.0f ; -//float Agent::radius_ = 1.5f ; -//float Agent::timeHorizon_ = 10.0f ; -float Agent::timeHorizon_ = 100.0f ; +//float Agent::radius_ = 3.0f ; +float Agent::radius_ = 1.5f ; +float Agent::timeHorizon_ = 10.0f ; +//float Agent::timeHorizon_ = 100.0f ; float Agent::timeHorizonObst_ = 10.0f ; float Agent::range_ = 2*timeHorizonObst_ * averageMaxSpeed_ + radius_ ; float Agent::rangeSq_ = range_ * range_ ; diff --git a/src/env_map.cpp b/src/env_map.cpp index 088678eff3730975ffec3b7617f21b5f1434c42e..45c6a395e088b12b74aed2733e6723ff87a62b6e 100644 --- a/src/env_map.cpp +++ b/src/env_map.cpp @@ -404,6 +404,35 @@ void EnvMap::scale(float val) } +bool EnvMap::is_insideConvexCell2D(VEC3 p, Dart d) +{ +// return false; + VEC3 vec, norm, p1,p2; + Dart dd = d,ddd; + + do + { + ddd = map.phi1(dd) ; + p1 = position[dd]; + p2 = position[ddd]; + + vec = VEC3(p2 - p1); + norm[0] = vec[1]; + norm[1] = -vec[0]; + + vec = VEC3(p2 -p); + if (vec*norm < 0){ +// CGoGNout<<"not inside !"<index==12 && o->index ==1) CGoGNout<<"popAndPush :"<belonging_cells[n].push_back(d); mo->addGeneralCell (d); // mo->neighbor_cells[n].clear(); -// if(o->index==1 && mo->index==279) CGoGNout <<"obstacle"<< o->index << " : "<< d << CGoGNendl; +// if(o->index==1 && mo->index==12) CGoGNout <<"push : "<< d << CGoGNendl; Dart dd = d; do { @@ -836,16 +865,16 @@ void EnvMap::pushObstacleInCells(Obstacle* o, int n, const std::vector& me // mo->belonging_cells[n].clear(); // mo->neighbor_cells[n].clear(); mo->belonging_cells[n] = memo_cross; -// if(o->index==1 && mo->index==279) CGoGNout<<"obstacle"<< o->index << " : "; +// if (mo->index==12 && o->index ==1) CGoGNout<<"push : "<::iterator it = mo->belonging_cells[n].begin(); it != mo->belonging_cells[n].end(); ++it) { -// if(o->index==1 && mo->index==279) CGoGNout << *it << " ; "; +// if(o->index==1 && mo->index==12) CGoGNout << *it << " ; "; addElementToVector(obstvect[*it],o); mo->addGeneralCell (*it); } -// if(o->index==1 && mo->index==279) CGoGNout <index==1 && mo->index==12) CGoGNout <belonging_cells[n], &(mo->neighbor_cells[n])); for (std::vector::iterator it = mo->neighbor_cells[n].begin(); it != mo->neighbor_cells[n].end(); ++it) @@ -867,12 +896,14 @@ void EnvMap::popObstacleInCells(Obstacle* o, int n) if (mo != NULL) { +// if (mo->index==12 && o->index ==1) CGoGNout<<"pop : "<::iterator it = mo->belonging_cells[n].begin(); it != mo->belonging_cells[n].end(); ++it) { +// if (mo->index==12 && o->index ==1) CGoGNout<<*it<< " ; "; removeElementFromVector(obstvect[*it], o) ; mo->removeGeneralCell (*it); } - +// if (mo->index==12 && o->index ==1) CGoGNout<::iterator it = mo->neighbor_cells[n].begin(); it != mo->neighbor_cells[n].end(); ++it) { if (!map.isBoundaryMarked2(*it)) @@ -912,6 +943,7 @@ void EnvMap::agentChangeFace(Agent* agent, Dart oldFace) void EnvMap::refine() { +// CGoGNout<<"refine"<::iterator it = refineCandidate.begin(); it != refineCandidate.end(); ++it) { Dart d = (*it) ; @@ -1051,7 +1083,7 @@ void EnvMap::refine() //agents contained in the subdivided cell are pushed correctly for (PFP::AGENTS::iterator ait = oldAgents.begin(); ait != oldAgents.end(); ++ait) { - +// CGoGNout<<"reseting Agents"<part_.d) ; } @@ -1059,6 +1091,7 @@ void EnvMap::refine() //same for obstacles contained for (PFP::OBSTACLEVECT::iterator ait = oldObst.begin(); ait != oldObst.end(); ++ait) { +// CGoGNout<<"reseting Obstacles"< checkCoarsenCandidate ; @@ -1218,6 +1252,7 @@ void EnvMap::coarse() map.setCurrentLevel(map.getMaxLevel()) ; for(PFP::OBSTACLEVECT::iterator ait = ob.begin(); ait != ob.end(); ++ait) { +// resetObstPartInFace(*ait, fit,fLevel); this->popObstacleInCells(*ait, (*ait)->index) ; obst.push_back(*ait); } @@ -1242,6 +1277,7 @@ void EnvMap::coarse() map.setCurrentLevel(map.getMaxLevel()) ; for(PFP::OBSTACLEVECT::iterator ait = ob.begin(); ait != ob.end(); ++ait) { +// resetObstPartInFace(*ait, fit,Level); this->popObstacleInCells(*ait, (*ait)->index) ; obst.push_back(*ait); } @@ -1317,7 +1353,7 @@ void EnvMap::coarse() } for (PFP::OBSTACLEVECT::iterator ait = obst.begin(); ait != obst.end(); ++ait) { - resetObstPartInFace(*ait, old) ; + resetObstPartInFace(*ait, old); pushObstacleInCells(*ait) ; } for (PFP::OBSTACLEVECT::iterator ait = neighborObst.begin(); ait != neighborObst.end(); ++ait) diff --git a/src/moving_obstacle.cpp b/src/moving_obstacle.cpp index 4c2ed386c40b7966133ac70bd4ae32a13f6beacb..2970871e72ec280620cfaf5fe7d1a554a3d9baa1 100644 --- a/src/moving_obstacle.cpp +++ b/src/moving_obstacle.cpp @@ -58,6 +58,8 @@ bool MovingObstacle::removeGeneralCell (Dart d) return false; } + + bool MovingObstacle::is_inside(VEC3 p) { // return false; @@ -316,7 +318,8 @@ void MovingObstacle::draw(bool showPath) VEC3 col = Utils::color_map_BCGYR(float(index)/float(sim_->movingObstacles_.size())); - if(movingObstacleNeighbors_.size()==0) +// if(movingObstacleNeighbors_.size()==0) + if(index==12) // if(obstacleNeighbors_.size()==0) m_shader->setColor(Geom::Vec4f(col[0],col[1],col[2],0)); else @@ -348,8 +351,8 @@ void MovingObstacle::draw(bool showPath) VEC3 MovingObstacle::getDilatedPosition(unsigned int ind) { Dart d(ind); //WARNING : works only for one face created at start ! -// return position[d]+deformation[d]; - return position[d]; + return position[d]+deformation[d]; +// return position[d]; } VEC3 MovingObstacle::getPosition(unsigned int ind) @@ -1131,18 +1134,25 @@ void resetPartSubdiv(Obstacle* o) if (mo != NULL) { - int n =o->index; + unsigned int n =o->index; + unsigned int n2 =(n+1)%(mo->nbVertices); - VEC3 pos =mo->parts_[n]->getPosition(); + VEC3 pos = mo->parts_[n]->getPosition(); + VEC3 pos2 = mo->parts_[n2]->getPosition(); mo->parts_[n]->CGoGN::Algo::MovingObjects::ParticleBase::move(Algo::Surface::Geometry::faceCentroid(mo->sim_->envMap_.map, mo->parts_[n]->d, mo->sim_->envMap_.position)) ; mo->parts_[n]->setState(FACE) ; mo->parts_[n]->move(pos) ; + mo->parts_[n2]->CGoGN::Algo::MovingObjects::ParticleBase::move(Algo::Surface::Geometry::faceCentroid(mo->sim_->envMap_.map, mo->parts_[n]->d, mo->sim_->envMap_.position)) ; + + mo->parts_[n2]->setState(FACE) ; + mo->parts_[n2]->move(pos2) ; + mo->dDir = mo->parts_[0]->d; } } - +//, unsigned int fLevel void resetObstPartInFace(Obstacle* o, Dart d1) { MovingObstacle * mo = o->mo; @@ -1150,14 +1160,42 @@ void resetObstPartInFace(Obstacle* o, Dart d1) if (mo != NULL) { unsigned int n =o->index; + unsigned int n2 =(n+1)%(mo->nbVertices); VEC3 pos1 = mo->parts_[n]->getPosition(); - - if (Algo::Surface::Geometry::isPointInConvexFace2D(mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos1, true)) + VEC3 pos2 = mo->parts_[n2]->getPosition(); +// if (Algo::Surface::Geometry::isPointInConvexFace2D (mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos1, true)) + if ( mo->sim_->envMap_.is_insideConvexCell2D(pos1, d1)) mo->parts_[n]->d = d1; - +// if (Algo::Surface::Geometry::isPointInConvexFace2D (mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos2, true)) + if ( mo->sim_->envMap_.is_insideConvexCell2D(pos2, d1)) + mo->parts_[n2]->d = d1; if(n==0) mo->dDir = mo->parts_[0]->d; } +// +// MovingObstacle * mo = o->mo; +// +// if (mo != NULL) +// { +// +// unsigned int n =o->index; +// unsigned int n2 =(n+1)%mo->nbVertices; +// Dart d2 =mo->sim_->envMap_.map.faceOldestDart(mo->parts_[n]->d); +// Dart d3 =mo->sim_->envMap_.map.faceOldestDart(mo->parts_[n2]->d); +// mo->sim_->envMap_.map.setCurrentLevel(fLevel - 1) ; +// if ( mo->sim_->envMap_.map.sameFace(d1,d2)) +// { +// CGoGNout<<"particule : "<< n<<" du MovingObstacle "<index<< " changée de " << mo->parts_[n]->d <<" en "<parts_[n]->d = d2; +// +// } +// if ( mo->sim_->envMap_.map.sameFace(d1,d3)) +// { +// CGoGNout<<"particule : "<< n2<<" du MovingObstacle "<index<< " changée de " << mo->parts_[n2]->d <<" en "<parts_[n2]->d = d3; +// } +// mo->sim_->envMap_.map.setCurrentLevel(mo->sim_->envMap_.map.getMaxLevel()) ; +// } } void resetPart(Obstacle * o, Dart d1) @@ -1170,6 +1208,9 @@ void resetPart(Obstacle * o, Dart d1) if (mo->parts_[n]->d == mo->sim_->envMap_.map.phi1(d1)) mo->parts_[n]->d = d1; + if (mo->parts_[(n+1)%mo->nbVertices]->d == mo->sim_->envMap_.map.phi1(d1)) + mo->parts_[(n+1)%mo->nbVertices]->d = d1; + if(n==0) mo->dDir = mo->parts_[n]->d; } diff --git a/src/simulator.cpp b/src/simulator.cpp index a168a65fc6381f50e7d14e714749a4b250ba6a20..7cdab5e456319a309911b9bafe054eda728eeeff 100644 --- a/src/simulator.cpp +++ b/src/simulator.cpp @@ -135,7 +135,9 @@ void Simulator::doStep() struct timespec begTime, endTime ; for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i) + { +// unsigned int i =12; clock_gettime(CLOCK_MONOTONIC, &begTime) ; movingObstacles_[i]->computePrefVelocity() ; movingObstacles_[i]->computeNewVelocity() ; @@ -149,6 +151,7 @@ void Simulator::doStep() clock_gettime(CLOCK_MONOTONIC, &begTime) ; for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i) { +// unsigned int i =12; movingObstacles_[i]->updateAgentNeighbors(); movingObstacles_[i]->updateObstacleNeighbors(); movingObstacles_[i]->updateForces() ; @@ -156,16 +159,17 @@ void Simulator::doStep() } clock_gettime(CLOCK_MONOTONIC, &endTime) ; time_obstacle+= timespec_delta(begTime,endTime).tv_nsec; - +// CGoGNout<<"deplacement obstacles"<applyForces(); clock_gettime(CLOCK_MONOTONIC, &endTime) ; time_obstacle+= timespec_delta(begTime,endTime).tv_nsec; movingObstacles_[i]->updateMesh() ; } - +// CGoGNout<<"deplacement obstacles fin"< 10 ? ((nbObstacles*20*xSide[0])/6) : 500.0f , 960.0f, minSize, 320.0f) ; //grosses cases + envMap_.init(config, nbObstacles > 60 ? ((nbObstacles*20*xSide[0])/6) : 1000.0f , 960.0f, minSize, 320.0f) ; //grosses cases } else { diff --git a/src/viewer.cpp b/src/viewer.cpp index 3e0262cf989ae44c44af67ab0e78d36f2d67d385..a09b0ccbf99dd17a77cbc20b740f8de0c1259df5 100644 --- a/src/viewer.cpp +++ b/src/viewer.cpp @@ -461,15 +461,18 @@ void SocialAgents::updateObstaclePredTriVBO() { Utils::VBO * vbo = m_triObst_VBO[i]; PFP::VEC3* data = static_cast(vbo->lockPtr()); + for(unsigned int j =0; jnbVertices; j++ ) + { + VEC3 p = simulator.movingObstacles_[i]->getDilatedPosition(j); + data[j*4+0] = p; + data[j*4+3] = p; - VEC3 p = simulator.movingObstacles_[i]->getDilatedPosition(0); - data[0] = p; + p = simulator.envMap_.position[simulator.movingObstacles_[i]->parts_[j]->d]; + data[j*4+1] = p; - p = simulator.envMap_.position[simulator.movingObstacles_[i]->parts_[0]->d]; - data[1] = p; - - p = simulator.envMap_.position[simulator.envMap_.map.phi1(simulator.movingObstacles_[i]->parts_[0]->d)]; - data[2] = p; + p = simulator.envMap_.position[simulator.envMap_.map.phi1(simulator.movingObstacles_[i]->parts_[j]->d)]; + data[j*4+2] = p; + } vbo->releasePtr(); } @@ -714,7 +717,26 @@ void SocialAgents::cb_redraw() m_ds->endList(); } } + if (draw_posX) + { + glDisable(GL_LIGHTING); + VEC3 pos =VEC3 (posXSlider,posYSlider,0); + m_ds->newList(GL_COMPILE_AND_EXECUTE); + m_ds->lineWidth(5.0f); + m_ds->pointSize(10.0f); + m_ds->begin(GL_POINTS); + // fait varier la couleur du plus pres au plus loin + + m_ds->color3f(0.0f , 1.0f, 0.9f); + + m_ds->vertex(pos); + m_ds->vertex(pos+VEC3(0,0,50)); + + m_ds->vertex(pos+VEC3(0,10,50)); + m_ds->end(); + m_ds->endList(); + } if (drawMovingObstacles) { #ifdef SHADOWSHELL @@ -724,7 +746,7 @@ void SocialAgents::cb_redraw() for(unsigned int i = 0 ; i < simulator.movingObstacles_.size() ; ++i) { #ifdef SHADOWSHELL - if(simulator.movingObstacles_[i]->index==279){ + if(simulator.movingObstacles_[i]->index==12){ Utils::ShaderFlat* moShader = m_obstShader[i]; // moShader->setAmbiant(Geom::Vec4f(0.43137254902,0.76862745098,0.8862745098,0.)); @@ -769,9 +791,9 @@ void SocialAgents::cb_redraw() for(unsigned int i = 0 ; i < simulator.movingObstacles_.size() ; ++i) { Utils::ShaderSimpleColor* moShader = m_triObst_Shader[i]; - + MovingObstacle * mo =simulator.movingObstacles_[i]; moShader->enableVertexAttribs(); - glDrawArrays(GL_LINE_LOOP, 0, 3); + glDrawArrays(GL_LINE_LOOP, 0, 4*mo->nbVertices); moShader->disableVertexAttribs(); } } @@ -836,7 +858,7 @@ void SocialAgents::cb_redraw() display_times= false; } - if (elapsedTime/1000000000.0f >= nextUpdate) + if (elapsedTime/100000000.0f >= nextUpdate) { // Sortie des stats pour analyse externe // std::cout << elapsedTime << ";" << frames << ";" << sim.nbUpdates << ";" @@ -870,7 +892,7 @@ void SocialAgents::cb_redraw() simulator.nbRefineCandidate = 0 ; simulator.nbCoarsenCandidate = 0 ; frames = 0 ; - nextUpdate = elapsedTime/1000000000.0f + 1 ; + nextUpdate = elapsedTime/100000000.0f + 1 ; }