Commit 3797df7a authored by Jund Thomas's avatar Jund Thomas
Browse files

ajout qq fonctionnalité obst et affichage

parent adea2157
......@@ -26,6 +26,7 @@ inline void renderFace(EnvMap& m, Dart d)
} while (dd != d) ;
}
inline void renderPredictionTriangle(EnvMap& m, Dart d, PFP::VEC3 p)
{
Geom::Plane3D<float> pl = Algo::Geometry::facePlane<PFP>(m.map, d, m.position) ;
......@@ -42,6 +43,32 @@ inline void renderPredictionTriangle(EnvMap& m, Dart d, PFP::VEC3 p)
glEnd() ;
}
inline void renderObstacle(EnvMap& m, MovingObstacle * obst, bool showBelonging=false, bool renderPath = false)
{
glBegin(GL_POLYGON) ;
for (unsigned int i = 0 ; i < (obst->nbVertices) ; ++i)
{
const VEC3& p = obst->vertices[i] ;
glVertex3fv(p.data()) ;
}
glEnd() ;
if (renderPath)
{
glLineWidth(3.0f) ;
glBegin(GL_LINE_STRIP) ;
for (std::vector<VEC3>::iterator it = ++(obst->goals_.begin()) ; it != obst->goals_.end() ;
++it)
{
glVertex3f((*it)[0], (*it)[1], (*it)[2]) ;
}
glEnd() ;
glLineWidth(1.0f) ;
}
}
static const float cosT[5] = { 1, 0.309017, -0.809017, -0.809017, 0.309017 } ;
static const float sinT[5] = { 0, 0.951057, 0.587785, -0.587785, -0.951057 } ;
......@@ -61,9 +88,9 @@ inline void renderAgent(EnvMap& m, Agent* agent, bool showNeighborDist = false,
glLineWidth(1.0f) ;
// VEC3 col = Utils::color_map_BCGYR(float(agent->agentNo)/float(agent->sim_->agents_.size()));
// glColor3fv(col.data());
glColor3f(c1,c2,c3) ;
VEC3 col = Utils::color_map_BCGYR(float(agent->agentNo)/float(agent->sim_->agents_.size()));
glColor3fv(col.data());
// glColor3f(c1,c2,c3) ;
glBegin(GL_POLYGON) ;
for(unsigned int i = 0 ; i < 5 ; ++i)
glVertex3f(pos[0] + (cosT[i] * radius), pos[1] + (sinT[i] * radius), pos[2]+0.01f) ;
......
......@@ -96,10 +96,14 @@ public:
void setupCityScenario(int nbLines, int nbRank) ;
void setupScenario(unsigned int nbMaxAgent) ;
void addMovingObstacles(unsigned int nb);
void addMovingObstacle(Dart d, unsigned int obstType=0);
void addAgent(const VEC3& start,const VEC3& goals);
void addAgent(const VEC3& start,const VEC3& goals, Dart d);
#ifndef SPATIAL_HASHING
void addPathToObstacles();
void addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal);
void addPathToCorner() ;
void addPathsToAgents() ;
......
......@@ -111,6 +111,13 @@
<string>draw agents path</string>
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="check_drawObstPath">
<property name="text">
<string>draw obst path</string>
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="check_dart">
......
......@@ -101,6 +101,7 @@ public:
bool drawAgentsPredictionTri ;
bool drawAgentsPath ;
bool drawObstPredictionTri ;
bool drawObstPath ;
bool draw_dart;
unsigned int dartSlider;
......@@ -166,6 +167,11 @@ public slots:
drawMovingObstacles = b ;
updateGL() ;
}
void slot_drawObstPath(bool b)
{
drawObstPath= b ;
updateGL() ;
}
void slot_dart(bool b)
{
draw_dart = b ;
......
......@@ -125,7 +125,8 @@ void Agent::updateAgentNeighbors()
if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist)
&& distSq < neighborDistSq_)
{
if (distSq > maxDist) maxDist = distSq ;
if (distSq > maxDist)
maxDist = distSq ;
agentNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
}
......
......@@ -58,8 +58,10 @@ void Simulator::init(unsigned int config, float dimension, bool enablePathFindin
// break ;
case 5:
envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ; //svg import
setupScenario(10000) ;
setupScenario(30) ;
addPathsToAgents();
// addMovingObstacles(5);
// addPathToObstacles();
break;
default:
std::cout << "Unknown scenario !" << std::endl ;
......@@ -76,7 +78,8 @@ void Simulator::init(unsigned int config, float dimension, bool enablePathFindin
{
if (dimension == 2.0f)
addPathsToAgents() ;
else if (dimension == 2.5f) addPathsToAgents_height() ;
else if (dimension == 2.5f)
addPathsToAgents_height() ;
}
#endif
......@@ -143,7 +146,8 @@ void Simulator::doStep()
}
}
#else
if (multires) envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
if (multires)
envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
nb_dead = 0 ;
for (unsigned int i = 0 ; i < agents_.size() ; ++i)
{
......@@ -521,8 +525,8 @@ void Simulator::setupScenario(unsigned int nbMaxAgent)
TraversorF<PFP::MAP> tF(envMap_.map);
Dart d = tF.begin() ;
unsigned int nbx = 1 ;
unsigned int nby = 1 ;
unsigned int nbx = 2 ;
unsigned int nby = 2 ;
unsigned int bMax = nbx * nby > 0 ? nbMaxAgent / (nbx * nby) : nbMaxAgent ;
......@@ -534,7 +538,7 @@ void Simulator::setupScenario(unsigned int nbMaxAgent)
while (!found && d != tF.end())
{
if (!envMap_.buildingMark.isMarked(d)
// && envMap_.pedWayMark.isMarked(d)
&& envMap_.pedWayMark.isMarked(d)
)
{
pos = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
......@@ -547,7 +551,7 @@ void Simulator::setupScenario(unsigned int nbMaxAgent)
if (found)
{
float ecart = 0.0f ;
float ecart = 3.0f ;
VEC3 posinit = VEC3(pos[0] - (float(nbx) / 2.0f * ecart),
pos[1] - (float(nby) / 2.0f * ecart), pos[2]) ;
for (unsigned int curx = 0 ; curx < nbx ; ++curx)
......@@ -564,6 +568,86 @@ void Simulator::setupScenario(unsigned int nbMaxAgent)
swapAgentsGoals() ;
}
void Simulator::addMovingObstacles(unsigned int nb)
{
TraversorF<PFP::MAP> tF(envMap_.map);
Dart d = tF.begin() ;
for (unsigned int i = 0 ; i < nb && d != envMap_.map.end() ; ++i)
{
bool found = false ;
Dart dCell ;
while (!found && d != tF.end())
{
if (!envMap_.buildingMark.isMarked(d)
&& !envMap_.pedWayMark.isMarked(d)
)
{
dCell = d ;
found = true ;
}
d = tF.next() ;
}
if (found)
addMovingObstacle(dCell) ;
}
}
void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
{
std::vector<VEC3> vPos;
VEC3 start;
switch(obstType)
{
case 0 :
{
start = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position) ;
VEC3 xSide (2.0f,0.0f,0.0f);
VEC3 ySide (0.0f,5.0f,0.0f);
vPos.push_back(start-xSide+ySide);
vPos.push_back(start-xSide-ySide);
vPos.push_back(start+xSide-ySide);
vPos.push_back(start+xSide+ySide);
}
break;
default:
std::cout << "Unknown obstacle type" << std::endl;
return;
}
std::vector<VEC3> goals;
goals.push_back(start);
MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals,false);
movingObstacles_.push_back(mo);
}
void Simulator::addPathToObstacles()
{
unsigned int dartDistForPath = 50 ;
for (std::vector<MovingObstacle*>::iterator it = movingObstacles_.begin() ; it != movingObstacles_.end() ; ++it)
{
Dart dStart = (*it)->registering_part->d;
Dart dStop = dStart;
for (unsigned int j = 0 ; envMap_.buildingMark.isMarked(dStop)
|| j < dartDistForPath + rand() * 20
|| envMap_.map.sameFace(dStop, dStart)
|| !envMap_.pedWayMark.isMarked(dStop)
; ++j)
{
envMap_.map.next(dStop) ;
if (dStop == envMap_.map.end())
dStop = envMap_.map.begin() ;
}
addPathToObstacle(*it, dStart, dStop);
}
}
void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
{
std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
......@@ -616,6 +700,8 @@ void Simulator::addPathsToAgents()
{
agents_[i]->goals_.clear() ;
TraversorF<PFP::MAP> tF(envMap_.map);
Dart dStart = agents_[i]->part_.d ;
Dart dStop = dStart ;
for (unsigned int j = 0 ;
......@@ -627,7 +713,8 @@ void Simulator::addPathsToAgents()
; ++j)
{
envMap_.map.next(dStop) ;
if (dStop == envMap_.map.end()) dStop = envMap_.map.begin() ;
if (dStop == envMap_.map.end())
dStop = envMap_.map.begin() ;
}
// std::cout << "dest1" << Algo::Geometry::faceCentroid<PFP>(envMap_.map, dStop, envMap_.position) << std::endl;
......@@ -657,7 +744,8 @@ void Simulator::addPathsToAgents()
; ++j)
{
envMap_.map.next(dStop2) ;
if (dStop2 == envMap_.map.end()) dStop2 = envMap_.map.begin() ;
if (dStop2 == envMap_.map.end())
dStop2 = envMap_.map.begin() ;
}
// std::cout << "dest2" << Algo::Geometry::faceCentroid<PFP>(envMap_.map, dStop2, envMap_.position) << std::endl;
......@@ -685,6 +773,7 @@ void Simulator::addPathsToAgents()
// VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
dest /= 2.0f ;
dest[2] = 0 ;
agents_[i]->goals_.push_back(dest) ;
......
......@@ -41,9 +41,10 @@ SocialAgents::SocialAgents(unsigned int config, unsigned int minSize, unsigned i
drawAgents(true),
drawAgentsNeighborDist(false),
drawAgentsObstacleDist(false),
drawAgentsPath(false),
drawAgentsPredictionTri(false),
drawAgentsPath(false),
drawObstPredictionTri(false),
drawObstPath(false),
draw_dart(false)
{
timer = new QTimer(this) ;
......@@ -76,6 +77,7 @@ void SocialAgents::initGUI()
setCallBack(dock.check_drawAgentsNeighborDist, SIGNAL(toggled(bool)), SLOT(slot_drawAgentsNeighborDist(bool))) ;
setCallBack(dock.check_drawAgentsObstacleDist, SIGNAL(toggled(bool)), SLOT(slot_drawAgentsObstacleDist(bool))) ;
setCallBack(dock.check_drawAgentsPath, SIGNAL(toggled(bool)), SLOT(slot_drawAgentsPath(bool))) ;
setCallBack(dock.check_drawObstPath, SIGNAL(toggled(bool)), SLOT(slot_drawObstPath(bool))) ;
setCallBack(dock.check_dart, SIGNAL(toggled(bool)), SLOT(slot_dart(bool))) ;
setCallBack(dock.check_slide, SIGNAL(valueChanged(int)), SLOT(slot_slide(int))) ;
}
......@@ -213,21 +215,16 @@ void SocialAgents::cb_redraw()
if (drawMovingObstacles)
{
glColor3f(1.0f, 1.0f, 1.0f) ;
for (std::vector<MovingObstacle*>::iterator it = simulator.movingObstacles_.begin() ; it != simulator.movingObstacles_.end() ; ++it)
{
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL) ;
glColor3f(1.0f, 1.0f, 1.0f) ;
glLineWidth(1.0f) ;
glBegin(GL_POLYGON) ;
for (unsigned int i = 0 ; i < ((*it)->nbVertices) ; ++i)
{
const VEC3& p = (*it)->vertices[i] ;
glVertex3fv(p.data()) ;
}
glEnd() ;
//////Affiche l'objectif actuel
renderObstacle(simulator.envMap_, *it,false,drawObstPath);
//////Affiche l'objectif actuel
// glColor3f(1.0f, 0.0f, 0.0f) ;
// glLineWidth(3.0f) ;
// glBegin(GL_LINES);
......
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