Commit 27e8ba8b authored by Thomas's avatar Thomas

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

parent 796d8ff2
......@@ -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);
......
......@@ -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);
......
......@@ -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;
}
......
......@@ -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_;
......
......@@ -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)
......
#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);
}
}
}
......
......@@ -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;
......
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