Skip to content

GitLab

Commit 14ce6b30 authored by pitiot's avatar pitiot
Browse files
Options

up

parent a05111cb
Show whitespace changes
Inline Side-by-side
Showing with 145 additions and 41 deletions
include/env_map.h
View file @ 14ce6b30
......@@ -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);
......
include/viewer.h
View file @ 14ce6b30
......@@ -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);
......
src/agent.cpp
View file @ 14ce6b30
......@@ -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_ ;
......
src/env_map.cpp
View file @ 14ce6b30
......@@ -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 !"<<CGoGNendl;
return false;
}
dd = ddd ;
} while (dd != d) ;
// CGoGNout<<"c'est dedans !"<<CGoGNendl;
return true;
}
void EnvMap::markPedWay()
{
//split all pedway with 3 roads intersection (to avoid spikes)
......@@ -775,7 +804,7 @@ Dart EnvMap::popAndPushObstacleInCells(Obstacle* o, int n)// maj de l'enregistre
// if(belonging_cells[n] != memo)
// modif=true;
// }
//
// if (mo->index==12 && o->index ==1) CGoGNout<<"popAndPush :"<<CGoGNendl;
// if(modif)
{
popObstacleInCells(o, n);
......@@ -805,7 +834,7 @@ void EnvMap::pushObstacleInOneRingCells(Obstacle * o, Dart d, int n)
mo->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<Dart>& 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 : "<<CGoGNendl;
for (std::vector<Dart>::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<Obstacle*>(obstvect[*it],o);
mo->addGeneralCell (*it);
}
// if(o->index==1 && mo->index==279) CGoGNout <<CGoGNendl;
// if(o->index==1 && mo->index==12) CGoGNout <<CGoGNendl;
addObstAsNeighbor(o, mo->belonging_cells[n], &(mo->neighbor_cells[n]));
for (std::vector<Dart>::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 : "<<CGoGNendl;
for (std::vector<Dart>::iterator it = mo->belonging_cells[n].begin(); it != mo->belonging_cells[n].end(); ++it)
{
// if (mo->index==12 && o->index ==1) CGoGNout<<*it<< " ; ";
removeElementFromVector<Obstacle*>(obstvect[*it], o) ;
mo->removeGeneralCell (*it);
}
// if (mo->index==12 && o->index ==1) CGoGNout<<CGoGNendl<<CGoGNendl;
for (std::vector<Dart>::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"<<CGoGNendl;
for (std::vector<Dart>::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"<<CGoGNendl;
resetAgentInFace(*ait) ;
pushAgentInCells(*ait, (*ait)->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"<<CGoGNendl;
resetPartSubdiv(*ait);
pushObstacleInCells(*ait);
......@@ -1083,6 +1116,7 @@ void EnvMap::refine()
void EnvMap::coarse()
{
// CGoGNout<<"coarse"<<CGoGNendl;
// On recrée une liste des faces à simplifier en empêchant les doublons
// On en profite pour vérifier les conditions de simplifications
std::vector<Dart> 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)
......
src/moving_obstacle.cpp
View file @ 14ce6b30
......@@ -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<PFP>::move(Algo::Surface::Geometry::faceCentroid<PFP>(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<PFP>::move(Algo::Surface::Geometry::faceCentroid<PFP>(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<PFP>(mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos1, true))
VEC3 pos2 = mo->parts_[n2]->getPosition();
// if (Algo::Surface::Geometry::isPointInConvexFace2D <PFP> (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 <PFP> (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 "<<mo->index<< " changée de " << mo->parts_[n]->d <<" en "<<d2<<CGoGNendl;
// mo->parts_[n]->d = d2;
//
// }
// if ( mo->sim_->envMap_.map.sameFace(d1,d3))
// {
// CGoGNout<<"particule : "<< n2<<" du MovingObstacle "<<mo->index<< " changée de " << mo->parts_[n2]->d <<" en "<<d3<<CGoGNendl;
// mo->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;
}
......
src/simulator.cpp
View file @ 14ce6b30
......@@ -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"<<CGoGNendl;
for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
{
// unsigned int i =12;
clock_gettime(CLOCK_MONOTONIC, &begTime) ;
movingObstacles_[i]->applyForces();
clock_gettime(CLOCK_MONOTONIC, &endTime) ;
time_obstacle+= timespec_delta(begTime,endTime).tv_nsec;
movingObstacles_[i]->updateMesh() ;
}
// CGoGNout<<"deplacement obstacles fin"<<CGoGNendl;
for (unsigned int i = 0 ; i < agents_.size() ; ++i)
{
......@@ -397,7 +401,7 @@ void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObst
if (multires)
{
// envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
envMap_.init(config, nbObstacles > 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
{
......
src/viewer.cpp
View file @ 14ce6b30
......@@ -461,15 +461,18 @@ void SocialAgents::updateObstaclePredTriVBO()
{
Utils::VBO * vbo = m_triObst_VBO[i];
PFP::VEC3* data = static_cast<PFP::VEC3*>(vbo->lockPtr());
for(unsigned int j =0; j<simulator.movingObstacles_[i]->nbVertices; 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 ;
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment