Skip to content
GitLab
Commit 27e8ba8b authored by Thomas's avatar Thomas
Browse files

ajout parallelisation, correction Astar, test insertion avant tri des voisins et des obstacles

parent 796d8ff2
Hide whitespace changes
Inline Side-by-side
include/env_generator.hpp
View file @ 27e8ba8b
......@@ -154,7 +154,7 @@ void generateMall(typename PFP::MAP& map,typename PFP::TVEC3& position, CellMark
floor2->embedGrid(sideSize*side,sideSize*side);
for(unsigned int i=0;i<1;++i)
for(unsigned int i=0;i<5;++i)
{
float floorHeight = 100;
typename PFP::VEC3 transl(0,0,floorHeight);
......
include/env_render.h
View file @ 27e8ba8b
......@@ -15,7 +15,7 @@ inline void renderDart(EnvMap& m, Dart d)
glEnd();
}
inline void renderFace(EnvMap& m, Dart& d)
inline void renderFace(EnvMap& m, Dart d)
{
Dart dd=d;
do {
......@@ -48,9 +48,9 @@ inline void renderAgent(EnvMap& m, Agent* agent, bool showNeighborDist = false,
VEC3 pos = agent->part_.m_position;
float radius = agent->radius_;
Geom::Plane3D<float> pl = Algo::Geometry::facePlane<PFP>(m.map,agent->part_.d,m.position);
pos[2] -= 1000;
Geom::intersectionPlaneRay(pl,pos,VEC3(0,0,-1),pos);
// Geom::Plane3D<float> pl = Algo::Geometry::facePlane<PFP>(m.map,agent->part_.d,m.position);
// pos[2] -= 1000;
// Geom::intersectionPlaneRay(pl,pos,VEC3(0,0,-1),pos);
glLineWidth(1.0f);
......
include/path_finder.hpp
View file @ 27e8ba8b
......@@ -12,29 +12,36 @@ namespace PathFinder
template <typename PFP>
float pathCostSqr(typename PFP::MAP& map,const typename PFP::TVEC3& position, Dart startPos, Dart stopPos)
{
return VEC3(Algo::Geometry::faceCentroid<PFP>(map,stopPos,position)-Algo::Geometry::faceCentroid<PFP>(map,startPos,position)).norm2();
return VEC3(Algo::Geometry::faceCentroid<PFP>(map,stopPos,position)-Algo::Geometry::faceCentroid<PFP>(map,startPos,position)).norm2()
+(position[stopPos][2]-position[startPos][2])*1000.0f;
}
template <typename PFP>
std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3& position, Dart startPos, Dart stopPos, CellMarker& bad)
{
std::cout << "start" << std::endl;
std::vector<Dart> path;
AutoAttributeHandler< float > tablePathCost(map,FACE_ORBIT);
AutoAttributeHandler< Dart > tablePred(map,FACE_ORBIT);
std::map<float,Dart> vToDev;
std::multimap<float,Dart> vToDev;
CellMarker m(map,FACE_ORBIT);
Dart toDev = stopPos;
vToDev.insert(std::make_pair(0, toDev));
tablePred[toDev] = toDev;
vToDev.insert(std::make_pair(0, stopPos));
m.mark(stopPos);
tablePred[stopPos] = stopPos;
tablePathCost[stopPos] = 0;
Dart toDev;
do
{
//get all vertex-adjacent cells
toDev = (vToDev.begin())->second;
vToDev.erase(vToDev.begin());
//get all vertex-adjacent cells
Dart dd = toDev;
do
{
......@@ -42,14 +49,16 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3
while(ddd != dd)
{
//evaluate their cost and push them in the vector to dev
if(!m.isMarked(ddd) && !bad.isMarked(ddd))
if(!m.isMarked(ddd))
{
m.mark(ddd);
tablePred[ddd]=toDev;
float costDDD=pathCostSqr<PFP>(map,position,startPos,ddd)+(vToDev.begin())->first;
vToDev.insert(std::make_pair(costDDD, ddd));
std::cout << "ins " << ddd << std::endl;
if(!bad.isMarked(ddd) && tablePred[ddd]==EMBNULL)
{
tablePred[ddd]=toDev;
tablePathCost[ddd] = tablePathCost[toDev]+pathCostSqr<PFP>(map,position,toDev,ddd);
float costDDD=pathCostSqr<PFP>(map,position,startPos,ddd)+tablePathCost[toDev];
vToDev.insert(std::make_pair(costDDD, ddd));
}
}
ddd = map.alpha1(ddd);
......@@ -57,26 +66,20 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3
dd = map.phi1(dd);
} while(dd != toDev);
//get new cell to dev
if(vToDev.size()>0)
toDev = (vToDev.begin())->second;
} while(vToDev.size()>0 && !map.sameOrientedFace(startPos,toDev));
} while(vToDev.size()>0 && !map.sameFace(startPos,toDev));
//if path found : from start to stop -> push all predecessors
if(map.sameOrientedFace(startPos,toDev))
if(vToDev.size()>0 && map.sameFace(startPos,toDev))
{
Dart toPush=startPos;
do
{
path.push_back(toPush);
toPush = tablePred[toPush];
} while(!map.sameOrientedFace(toPush,stopPos));
} while(!map.sameFace(toPush,stopPos));
}
else
{
std::cout << "fichtre" << std::endl;
}
return path;
}
......
src/agent.cpp
View file @ 27e8ba8b
......@@ -39,18 +39,28 @@ void Agent::updateAgentNeighbors()
std::vector<Agent*>& neighborAgents = sim_->envMap_.neighborAgentvect[part_.d];
agentNeighbors_.clear();
float maxDist=0;
for(std::vector<Agent*>::iterator it = agents.begin(); it != agents.end(); ++it)
{
VEC3 pop = part_.m_position - (*it)->part_.m_position;
float distSq = pop.norm2();
agentNeighbors_.push_back(std::make_pair(distSq, *it));
if(agentNeighbors_.size()<maxNeighbors_ || distSq<maxDist) {
if(distSq>maxDist)
maxDist=distSq;
agentNeighbors_.push_back(std::make_pair(distSq, *it));
}
}
for(std::vector<Agent*>::iterator it = neighborAgents.begin(); it != neighborAgents.end(); ++it)
{
VEC3 pop = part_.m_position - (*it)->part_.m_position;
float distSq = pop.norm2();
agentNeighbors_.push_back(std::make_pair(distSq, *it));
if(agentNeighbors_.size()<maxNeighbors_ || distSq<maxDist) {
if(distSq>maxDist)
maxDist=distSq;
agentNeighbors_.push_back(std::make_pair(distSq, *it));
}
}
std::sort(agentNeighbors_.begin(), agentNeighbors_.end(), agentSort);
......@@ -65,7 +75,8 @@ void Agent::updateObstacleNeighbors()
for(std::vector<Dart>::iterator it = obst.begin(); it != obst.end(); ++it)
{
float distSq = distSqPointLineSegment(sim_->envMap_.position[*it], sim_->envMap_.position[sim_->envMap_.map.phi1(*it)], part_.m_position);
obstacleNeighbors_.push_back(std::make_pair(distSq, *it));
if(distSq<rangeSq_)
obstacleNeighbors_.push_back(std::make_pair(distSq, *it));
}
std::sort(obstacleNeighbors_.begin(), obstacleNeighbors_.end(), obstacleSort);
......@@ -120,7 +131,7 @@ void Agent::computeNewVelocity()
/* Create obstacle ORCA lines. */
// for(std::multimap<float, Dart>::iterator it = obstacleNeighbors_.begin();
for(std::vector<std::pair<float, Dart> >::iterator it = obstacleNeighbors_.begin();
it != obstacleNeighbors_.end() && it->first < rangeSq_;
it != obstacleNeighbors_.end();
++it)
{
float invTimeHorizonObst = 1.0f / timeHorizonObst_;
......
src/env_map.cpp
View file @ 27e8ba8b
......@@ -29,10 +29,14 @@ unsigned int EnvMap::mapMemoryCost()
Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
{
CellMarkerStore m(map,FACE_ORBIT);
for(Dart d = map.begin(); d != map.end(); map.next(d))
{
if(!buildingMark.isMarked(d) && Algo::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true))
return d;
if(!m.isMarked(d)) {
m.mark(d);
if(!buildingMark.isMarked(d) && Algo::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true))
return d;
}
}
std::cout << "ERROR : pos not in map" << std::endl;
......@@ -42,14 +46,16 @@ Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
void EnvMap::init()
{
float sideSize = 70.0f;
unsigned int nbSquares = 20;
unsigned int nbSquares = 10;
CityGenerator::generateCity<PFP>(map, position, obstacleMark, buildingMark, sideSize, nbSquares);
// CityGenerator::generateMall<PFP>(map, position, obstacleMark, buildingMark, sideSize);
// CityGenerator::simplifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
CityGenerator::simplifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
map.init();
registerObstaclesInFaces();
for(unsigned int i = subdivisableFace.begin(); i < subdivisableFace.end(); subdivisableFace.next(i))
subdivisableFace[i].first = false ;
// Algo::Geometry::computeNormalFaces<PFP>(map,position,normal);
}
void EnvMap::foreach_neighborFace(Dart d, FunctorType& f)
......
src/simulator.cpp
View file @ 27e8ba8b
#include "simulator.h"
Simulator::Simulator() : globalTime_(0.0f), timeStep_(0.1f)
Simulator::Simulator() : globalTime_(0.0f), timeStep_(0.2f)
{
srand(10);
envMap_.init();
......@@ -21,18 +21,92 @@ Simulator::~Simulator()
delete agents_[i];
}
class ThreadUpdateInfo {
public :
std::vector<Agent*> ag_;
unsigned int bMin_;
unsigned int bMax_;
// Constructor
ThreadUpdateInfo(std::vector<Agent*> agents,unsigned int bMin,unsigned int bMax) : bMin_(bMin), bMax_(bMax) {
ag_.insert(ag_.end(),agents.begin()+bMin,agents.begin()+bMax);
}
// Destructor
~ThreadUpdateInfo() { }
void run() {
// Thread execution stuff goes here
for (unsigned int i = 0; i < ag_.size(); ++i)
{
ag_[i]->updateObstacleNeighbors();
ag_[i]->updateAgentNeighbors();
ag_[i]->computePrefVelocity();
ag_[i]->computeNewVelocity();
}
}
};
class ThreadUpdatePos {
public :
std::vector<Agent*> ag_;
unsigned int bMin_;
unsigned int bMax_;
// Constructor
ThreadUpdatePos(std::vector<Agent*> agents,unsigned int bMin,unsigned int bMax) : bMin_(bMin), bMax_(bMax) {
ag_.insert(ag_.end(),agents.begin()+bMin,agents.begin()+bMax);
}
// Destructor
~ThreadUpdatePos() { }
void run() {
// Thread execution stuff goes here
for (unsigned int i = 0; i < ag_.size(); ++i)
{
ag_[i]->update();
}
}
};
void Simulator::doStep()
{
envMap_.clearUpdateCandidates();
envMap_.map.setCurrentLevel(0);
for (unsigned int i = 0; i < agents_.size(); ++i)
{
agents_[i]->computePrefVelocity();
agents_[i]->computeNewVelocity();
}
// for (unsigned int i = 0; i < agents_.size(); ++i)
// {
// agents_[i]->updateObstacleNeighbors();
// agents_[i]->updateAgentNeighbors();
// agents_[i]->computePrefVelocity();
// agents_[i]->computeNewVelocity();
// }
ThreadUpdateInfo tc1(agents_,0,agents_.size()/2);
ThreadUpdateInfo tc2(agents_,agents_.size()/2,agents_.size());
boost::thread thread1(&ThreadUpdateInfo::run, &tc1);
boost::thread thread2(&ThreadUpdateInfo::run, &tc2);
// std::vector<Dart> oldDarts;
// oldDarts.reserve(agents_.size());
// for (unsigned int i = 0; i < agents_.size(); ++i)
// {
// oldDarts.push_back(agents_[i]->part_.d);
// }
thread1.join();
thread2.join();
envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel());
// ThreadUpdatePos tc3(agents_,0,agents_.size()/2);
// ThreadUpdatePos tc4(agents_,agents_.size()/2,agents_.size());
// boost::thread thread3(&ThreadUpdatePos::run, &tc3);
// boost::thread thread4(&ThreadUpdatePos::run, &tc4);
// thread3.join();
// thread4.join();
for (unsigned int i = 0; i < agents_.size(); ++i)
{
Dart oldFace = agents_[i]->part_.d;
......@@ -48,14 +122,10 @@ void Simulator::doStep()
// break;
// case CGoGN::Algo::MovingObjects::CROSS_OTHER :
envMap_.agentChangeFace(agents_[i], oldFace);
// envMap_.agentChangeFace(agents_[i], oldDarts[i]);
// break;
}
envMap_.map.setCurrentLevel(0);
agents_[i]->updateObstacleNeighbors();
envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel());
agents_[i]->updateAgentNeighbors();
}
envMap_.updateMap();
......@@ -77,8 +147,8 @@ void Simulator::setupScenario(unsigned int nbMaxAgent)
Dart d = envMap_.map.begin();
DartMarkerStore filled(envMap_.map);
unsigned int nbx = 3;
unsigned int nby = 3;
unsigned int nbx = 10;
unsigned int nby = 10;
unsigned int bMax = nbMaxAgent / (nbx*nby);
......@@ -135,7 +205,7 @@ void Simulator::addPathsToAgents()
Dart dStart = agents_[i]->part_.d;
Dart dStop = dStart;
for(unsigned int j = 0; envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath+rand()*20 || envMap_.map.sameOrientedFace(dStop,dStart); ++j)
for(unsigned int j = 0; envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath+rand()*20 || envMap_.map.sameFace(dStop,dStart); ++j)
{
envMap_.map.next(dStop);
if(dStop == envMap_.map.end())
......@@ -145,10 +215,14 @@ void Simulator::addPathsToAgents()
std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStart, dStop, envMap_.buildingMark);
for(std::vector<Dart>::iterator it = path.begin(); it != path.end(); ++it)
agents_[i]->goals_.push_back(Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position));
{
VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
dest[2]=0;
agents_[i]->goals_.push_back(dest);
}
Dart dStop2 = dStop;
for(unsigned int j = 0; envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath+rand()*20 || envMap_.map.sameOrientedFace(dStop,dStop2) || envMap_.map.sameOrientedFace(dStop2,dStart); ++j)
for(unsigned int j = 0; envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath+rand()*20 || envMap_.map.sameFace(dStop,dStop2) || envMap_.map.sameFace(dStop2,dStart); ++j)
{
envMap_.map.next(dStop2);
if(dStop2 == envMap_.map.end())
......@@ -158,12 +232,20 @@ void Simulator::addPathsToAgents()
path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop, dStop2, envMap_.buildingMark);
for(std::vector<Dart>::iterator it = path.begin(); it!=path.end();++it)
agents_[i]->goals_.push_back(Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position));
{
VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
dest[2]=0;
agents_[i]->goals_.push_back(dest);
}
path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop2, dStart, envMap_.buildingMark);
for(std::vector<Dart>::iterator it = path.begin(); it!=path.end();++it)
agents_[i]->goals_.push_back(Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position));
{
VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
dest[2]=0;
agents_[i]->goals_.push_back(dest);
}
}
}
......
src/viewer.cpp
View file @ 27e8ba8b
......@@ -168,7 +168,7 @@ void SocialAgents::cb_redraw()
// glPushMatrix();
// glBegin(GL_LINES);
// unsigned int iter = 100;
unsigned int iter = 100;
// for(int i=0;i<700;i=i+iter)
// {
// VEC3 camPos(-800,-800,10);
......@@ -195,6 +195,33 @@ void SocialAgents::cb_redraw()
// glEnd();
// glPopMatrix();
glPushMatrix();
glBegin(GL_LINES);
for(int i=0;i<700;i=i+iter)
{
VEC3 camPos(0,-200,10);
camPos = camPos + VEC3(-10-0.15*i,0,0);
glVertex3fv(&camPos[0]);
camPos = camPos + VEC3(-10-0.15*(i+iter),0,0);
glVertex3fv(&camPos[0]);
}
glEnd();
glPopMatrix();
glPushMatrix();
glBegin(GL_LINES);
for(int i=0;i<700;i=i+iter)
{
VEC3 camLook(0,-200,10);
camLook[1] += 100;
camLook = camLook + VEC3(-10-0.15*i,0,0);
glVertex3fv(&camLook[0]);
camLook = camLook + VEC3(-10-0.15*(i+iter),0,0);
glVertex3fv(&camLook[0]);
}
glEnd();
glPopMatrix();
// glColor3f(0.0f, 1.0f, 0.0f);
// glLineWidth(5.0f);
......@@ -256,6 +283,7 @@ void SocialAgents::animate()
if(sim.globalTime_<300+sim.timeStep_ && sim.globalTime_>=300) {
CGoGNout << " breakpoint for exporting pov" << CGoGNendl;
timer->stop();
dock.check_timer->setChecked(false);
}
if(render_anim)
......@@ -264,7 +292,7 @@ void SocialAgents::animate()
std::ostringstream tmpNb;
tmpNb << std::setfill('0') << std::setw(4) << nbGenerated;
nbGenerated++;
oss << "./planMall/mallScene" << tmpNb.str() << ".pov";
oss << "./travelScene/cityNear" << tmpNb.str() << ".pov";
std::string chaine = oss.str();
// VEC3 agPos = sim.agents_[0]->meanPos_;
// agPos[2] = agPos[1];
......@@ -273,12 +301,19 @@ void SocialAgents::animate()
// VEC3 camLook(agPos);
// camPos = camPos+VEC3(-10-0.05*nbGenerated,30+0.05*nbGenerated,log(1.0f+nbGenerated/700.0f));
VEC3 camPos(-800,10,-800);
camPos = camPos + VEC3(-10-0.3*nbGenerated,30+0.3*nbGenerated,400.0f*log(1.0f+nbGenerated/700.0f));
VEC3 camLook(0,0,0);
camLook = camLook + VEC3(-10-0.1*nbGenerated,30+0.1*nbGenerated,400.0f*log(1.0f+nbGenerated/700.0f));
//travelling city nbMaxAgent 15000, nbSquare 40 (45?)
// VEC3 camPos(-800,10,-800);
// camPos = camPos + VEC3(-10-0.3*nbGenerated,30+0.3*nbGenerated,400.0f*log(1.0f+nbGenerated/700.0f));
// VEC3 camLook(0,0,0);
// camLook = camLook + VEC3(-10-0.1*nbGenerated,30+0.1*nbGenerated,400.0f*log(1.0f+nbGenerated/700.0f));
//travelling horizontal
VEC3 camPos(0,5,-200);
camPos = camPos + VEC3(-10-0.15*nbGenerated,0,0);
VEC3 camLook = camPos;
camLook[0] += 0.15*nbGenerated;
camLook[2] += 100;
camLook[1] = 0;
exportScenePov(sim.envMap_.map,sim.envMap_.position,chaine,camPos,camLook,VEC3(0.0f,0,0),0,0,0);
// exportScenePov(sim.envMap_.map,sim.envMap_.position,chaine,VEC3(43,762,65),VEC3(0,762,0),VEC3(1.0f,0,0),0,0,0);
......@@ -453,8 +488,8 @@ bool SocialAgents::exportScenePov(PFP::MAP& map, PFP::TVEC3& position, const std
Algo::ExportPov::exportMeshPlain<PFP>(out,map,position,"myMesh",sum);
out << "object {facesHighlighted" << std::endl;
out << "translate <" << translate[0] << "," << translate[1] << "," << translate[2] << ">" << std::endl;
out << "rotate <" << angleX << "," << angleY << "," << angleZ << "> " << std::endl;
out << "translate <" << translate[0] << "," << translate[1] << "," << translate[2] << ">" << std::endl;
out << "texture{ pigment{ color rgb<0.4667,0.709,0.996>} finish { ambient rgb 0.35 brilliance 0.5 } } }" << std::endl;
}
else {
......@@ -486,6 +521,14 @@ void SocialAgents::cb_keyPress(int keycode)
{
switch (keycode)
{
case 'a': {
if(timer->isActive())
timer->stop();
else
timer->start();
dock.check_timer->setChecked(timer->isActive());
break;
}
case 'c': {
sim.envMap_.map.check();
break;
......
Supports Markdown
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