Commit 90d6a95b authored by Pierre Kraemer's avatar Pierre Kraemer
Browse files

optim update agents neighbours

parent 27e8ba8b
......@@ -26,7 +26,7 @@ Algo::Modelisation::Polyhedron<PFP> generateGrid(typename PFP::MAP& map, typenam
{
Algo::Modelisation::Polyhedron<PFP> prim(map, position);
prim.grid_topo(cX, cY);
Dart boundary;
for(Dart d = map.begin(); d != map.end(); map.next(d))
{
......@@ -407,7 +407,7 @@ void simplifyFreeSpace(typename PFP::MAP& map, typename PFP::TVEC3& position, Ce
stillSimplif = false;
}
convexifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
// convexifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
map.check();
}
......@@ -476,7 +476,7 @@ template <typename PFP>
void convexifyFreeSpace(typename PFP::MAP& map, typename PFP::TVEC3& position, CellMarker& obstacleMark, CellMarker& buildingMark)
{
typedef typename PFP::VEC3 VEC3;
float area;
CellMarker m(map, FACE_CELL);
for(Dart d = map.begin(); d != map.end(); map.next(d))
......
#ifndef ENVMAP_H
#define ENVMAP_H
#ifndef ENV_MAP_H
#define ENV_MAP_H
#include <iostream>
#include <algorithm>
......@@ -171,13 +171,13 @@ inline void EnvMap::addRefineCandidate(Dart d)
{
std::pair<bool,bool>& sf = subdivisableFace[d] ;
if(sf.first == false || (sf.first == true && sf.second))
refineCandidate.insert(std::pair<unsigned int, Dart>(map.getEmbedding(d, FACE_ORBIT), d)) ;
refineCandidate.insert(std::pair<unsigned int, Dart>(map.getEmbedding(FACE_ORBIT, d), d)) ;
}
}
inline void EnvMap::addCoarsenCandidate(Dart d)
{
coarsenCandidate.insert(std::pair<unsigned int, Dart>(map.getEmbedding(d, FACE_ORBIT), d)) ;
coarsenCandidate.insert(std::pair<unsigned int, Dart>(map.getEmbedding(FACE_ORBIT, d), d)) ;
}
inline void EnvMap::clearUpdateCandidates()
......
......@@ -63,17 +63,17 @@ inline void renderAgent(EnvMap& m, Agent* agent, bool showNeighborDist = false,
VEC3 dir = agent->velocity_;
dir.normalize();
//draw the speed vector
// //draw the speed vector
// VEC3 base(0,0,1);
// VEC3 myAxisRot = base^dir;
// myAxisRot.normalize();
// float myRot = acos(base*dir);
// float myRot = 57.29f * acos(base*dir);
//
// glPushMatrix();
// glTranslatef(pos[0],pos[1],pos[2]);
// glRotatef(myRot,myAxisRot[0],myAxisRot[1],myAxisRot[2]);
//
// glColor3f(0.0,0.0,0.0);
// glColor3f(0.0f, 1.0f, 0.0f);
// glLineWidth(5.0f);
// glBegin(GL_LINES);
// glVertex3f(0.,0.,0.0);
......
......@@ -39,15 +39,16 @@ void Agent::updateAgentNeighbors()
std::vector<Agent*>& neighborAgents = sim_->envMap_.neighborAgentvect[part_.d];
agentNeighbors_.clear();
float maxDist=0;
float maxDist = 0.0f;
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();
if(agentNeighbors_.size()<maxNeighbors_ || distSq<maxDist) {
if(distSq>maxDist)
maxDist=distSq;
if((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist) && distSq < neighborDistSq_)
{
if(distSq > maxDist)
maxDist = distSq;
agentNeighbors_.push_back(std::make_pair(distSq, *it));
}
}
......@@ -56,14 +57,16 @@ void Agent::updateAgentNeighbors()
{
VEC3 pop = part_.m_position - (*it)->part_.m_position;
float distSq = pop.norm2();
if(agentNeighbors_.size()<maxNeighbors_ || distSq<maxDist) {
if(distSq>maxDist)
maxDist=distSq;
if((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist) && distSq < neighborDistSq_)
{
if(distSq > maxDist)
maxDist = distSq;
agentNeighbors_.push_back(std::make_pair(distSq, *it));
}
}
std::sort(agentNeighbors_.begin(), agentNeighbors_.end(), agentSort);
if(agentNeighbors_.size() > maxNeighbors_)
std::sort(agentNeighbors_.begin(), agentNeighbors_.end(), agentSort);
}
void Agent::updateObstacleNeighbors()
......@@ -75,7 +78,7 @@ 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);
if(distSq<rangeSq_)
if(distSq < rangeSq_)
obstacleNeighbors_.push_back(std::make_pair(distSq, *it));
}
......@@ -129,7 +132,6 @@ void Agent::computeNewVelocity()
std::vector<Line> orcaLines_;
/* 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)
......@@ -141,8 +143,8 @@ void Agent::computeNewVelocity()
VEC3 obst1Pos(sim_->envMap_.position[obst1]);
VEC3 obst2Pos(sim_->envMap_.position[obst2]);
obst1Pos[2]=0;
obst2Pos[2]=0;
obst1Pos[2] = 0;
obst2Pos[2] = 0;
const VEC3 relativePosition1(obst1Pos - part_.m_position);
const VEC3 relativePosition2(obst2Pos - part_.m_position);
......@@ -358,7 +360,6 @@ void Agent::computeNewVelocity()
/* Create agent ORCA lines. */
unsigned int nbA = 0;
// for(std::multimap<float, Agent*>::iterator it = agentNeighbors_.begin();
for(std::vector<std::pair<float, Agent*> >::iterator it = agentNeighbors_.begin();
it != agentNeighbors_.end() && it->first < neighborDistSq_ && nbA < maxNeighbors_;
++it, ++nbA)
......
......@@ -46,16 +46,18 @@ Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
void EnvMap::init()
{
float sideSize = 70.0f;
unsigned int nbSquares = 10;
unsigned int nbSquares = 15;
map.check();
CityGenerator::generateCity<PFP>(map, position, obstacleMark, buildingMark, sideSize, nbSquares);
// CityGenerator::generateMall<PFP>(map, position, obstacleMark, buildingMark, sideSize);
CityGenerator::simplifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
map.check();
// 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);
// Algo::Geometry::computeNormalFaces<PFP>(map, position, normal);
}
void EnvMap::foreach_neighborFace(Dart d, FunctorType& f)
......
......@@ -21,21 +21,26 @@ Simulator::~Simulator()
delete agents_[i];
}
class ThreadUpdateInfo {
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);
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() {
void run()
{
// Thread execution stuff goes here
for (unsigned int i = 0; i < ag_.size(); ++i)
{
......@@ -47,21 +52,26 @@ public :
}
};
class ThreadUpdatePos {
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);
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() {
void run()
{
// Thread execution stuff goes here
for (unsigned int i = 0; i < ag_.size(); ++i)
{
......@@ -75,18 +85,24 @@ void Simulator::doStep()
envMap_.clearUpdateCandidates();
envMap_.map.setCurrentLevel(0);
// 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);
for (unsigned int i = 0; i < agents_.size(); ++i)
{
agents_[i]->updateObstacleNeighbors();
agents_[i]->updateAgentNeighbors();
agents_[i]->computePrefVelocity();
agents_[i]->computeNewVelocity();
}
// unsigned nbAgents = agents_.size();
// ThreadUpdateInfo tc1(agents_, 0, nbAgents / 4);
// ThreadUpdateInfo tc2(agents_, nbAgents / 4, nbAgents / 2);
// ThreadUpdateInfo tc3(agents_, nbAgents / 2, nbAgents * 3 / 4);
// ThreadUpdateInfo tc4(agents_, nbAgents * 3 / 4, nbAgents);
// boost::thread thread1(&ThreadUpdateInfo::run, &tc1);
// boost::thread thread2(&ThreadUpdateInfo::run, &tc2);
// boost::thread thread3(&ThreadUpdateInfo::run, &tc3);
// boost::thread thread4(&ThreadUpdateInfo::run, &tc4);
// std::vector<Dart> oldDarts;
// oldDarts.reserve(agents_.size());
......@@ -95,17 +111,19 @@ void Simulator::doStep()
// oldDarts.push_back(agents_[i]->part_.d);
// }
thread1.join();
thread2.join();
// thread1.join();
// thread2.join();
// thread3.join();
// thread4.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();
// 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)
{
......@@ -147,8 +165,8 @@ void Simulator::setupScenario(unsigned int nbMaxAgent)
Dart d = envMap_.map.begin();
DartMarkerStore filled(envMap_.map);
unsigned int nbx = 10;
unsigned int nby = 10;
unsigned int nbx = 5;
unsigned int nby = 5;
unsigned int bMax = nbMaxAgent / (nbx*nby);
......@@ -249,7 +267,6 @@ void Simulator::addPathsToAgents()
}
}
bool Simulator::importAgents(std::string filename)
{
std::ifstream myfile(filename.c_str(), std::ios::in);
......@@ -296,14 +313,14 @@ bool Simulator::importAgents(std::string filename)
bool Simulator::exportAgents(std::string filename)
{
std::ofstream out(filename.c_str(), std::ios::out);
if (!out.good()) {
if (!out.good())
{
std::cerr << "(export) Unable to open file " << filename << std::endl;
return false;
}
for (unsigned int i = 0; i < agents_.size(); ++i) {
for (unsigned int i = 0; i < agents_.size(); ++i)
out << agents_[i]->part_.m_position << std::endl;
}
out.close();
return true;
......@@ -324,3 +341,4 @@ void Simulator::swapAgentsGoals()
}
}
}
......@@ -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,33 +195,32 @@ 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();
// 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);
......@@ -280,11 +279,11 @@ void SocialAgents::animate()
sim.doStep();
if(sim.globalTime_<300+sim.timeStep_ && sim.globalTime_>=300) {
CGoGNout << " breakpoint for exporting pov" << CGoGNendl;
timer->stop();
dock.check_timer->setChecked(false);
}
// 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)
{
......
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