Commit a9135140 authored by Thomas's avatar Thomas
Browse files

Merge branch 'master' of cgogn:~kraemer/CGoGN_Apps/SocialAgents2

Conflicts:
	include/env_map.h
	src/agent.cpp
	src/env_map.cpp
	src/simulator.cpp
parents 7dea319f faef5f07
......@@ -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))
{
......@@ -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>
......@@ -68,11 +68,9 @@ public :
void pushAgentInCells(Agent* agent, Dart d);
void popAgentInCells(Agent* agent, Dart d);
void removeAgentFromVector(PFP::AGENTS& a, Agent * ag);
void addRefineCandidate(Dart d);
void addCoarsenCandidate(Dart d);
void clearUpdateCandidates();
void updateMap();
void resetAgentInFace(Agent* agent);
......@@ -93,10 +91,12 @@ public :
CellMarker buildingMark;
static const unsigned int nbAgentsToSubdivide = 7;
static const unsigned int nbAgentsToSimplify = 7;
static const unsigned int nbAgentsToSimplify = 5;
std::map<unsigned int, Dart> refineCandidate;
std::map<unsigned int, Dart> coarsenCandidate;
CellMarker refineMark;
std::vector<Dart> refineCandidate;
CellMarker coarsenMark;
std::vector<Dart> coarsenCandidate;
};
/**************************************
......@@ -123,17 +123,16 @@ inline void EnvMap::pushAgentInCells(Agent* agent, Dart d)
} while(dd != d);
}
inline void removeAgentFromVector(PFP::AGENTS& a, Agent * ag)
inline void EnvMap::removeAgentFromVector(PFP::AGENTS& a, Agent* ag)
{
PFP::AGENTS::iterator end = a.end();
bool found = false;
for(PFP::AGENTS::iterator it = a.begin(); it != end && !found; ++it)
for(PFP::AGENTS::iterator it = a.begin(); it != end; ++it)
{
if(*it == ag)
{
*it = a.back();
a.pop_back();
found = true;
return;
}
}
}
......@@ -143,9 +142,7 @@ inline void EnvMap::popAgentInCells(Agent* agent, Dart d)
assert(map.getCurrentLevel() == map.getMaxLevel());
assert(std::find(agentvect[d].begin(), agentvect[d].end(), agent) != agentvect[d].end());
PFP::AGENTS& a = agentvect[d];
removeAgentFromVector(a,agent);
removeAgentFromVector(agentvect[d],agent);
Dart dd = d;
do
......@@ -153,34 +150,17 @@ inline void EnvMap::popAgentInCells(Agent* agent, Dart d)
Dart ddd = map.alpha1(map.alpha1(dd));
while(ddd != dd)
{
PFP::AGENTS& na = neighborAgentvect[ddd];
removeAgentFromVector(na,agent);
removeAgentFromVector(neighborAgentvect[ddd], agent);
ddd = map.alpha1(ddd);
}
dd = map.phi1(dd);
} while(dd != d);
}
inline void EnvMap::addRefineCandidate(Dart d)
{
if(agentvect[d].size() > nbAgentsToSubdivide)
{
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)) ;
}
}
inline void EnvMap::addCoarsenCandidate(Dart d)
{
coarsenCandidate.insert(std::pair<unsigned int, Dart>(map.getEmbedding(d, FACE_ORBIT), d)) ;
}
inline void EnvMap::clearUpdateCandidates()
{
refineCandidate.clear();
// coarsenCandidate.clear();
coarsenCandidate.clear();
}
#endif
......@@ -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);
......
......@@ -12,6 +12,64 @@
#include <fstream>
#include <sstream>
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();
}
}
};
class Simulator
{
public:
......@@ -40,6 +98,11 @@ public:
float globalTime_;
float timeStep_;
ThreadUpdateInfo* tc1;
ThreadUpdateInfo* tc2;
ThreadUpdateInfo* tc3;
ThreadUpdateInfo* tc4;
};
#endif
......@@ -39,16 +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(*it!=this && (agentNeighbors_.size()<maxNeighbors_ || distSq<maxDist) && distSq<neighborDistSq_)
if(*it != this && (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));
}
}
......@@ -57,17 +57,16 @@ void Agent::updateAgentNeighbors()
{
VEC3 pop = part_.m_position - (*it)->part_.m_position;
float distSq = pop.norm2();
if((agentNeighbors_.size()<maxNeighbors_ || distSq<maxDist) && distSq<neighborDistSq_)
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));
}
}
if(agentNeighbors_.size()>maxNeighbors_) {
if(agentNeighbors_.size() > maxNeighbors_)
std::sort(agentNeighbors_.begin(), agentNeighbors_.end(), agentSort);
}
}
void Agent::updateObstacleNeighbors()
......@@ -79,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));
}
......@@ -133,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)
......@@ -145,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);
......@@ -362,9 +360,8 @@ 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 != agentNeighbors_.end() && nbA < maxNeighbors_;
++it, ++nbA)
{
Agent* other = it->second;
......
......@@ -9,7 +9,11 @@
using namespace CGoGN;
EnvMap::EnvMap() : obstacleMark(map, EDGE_CELL), buildingMark(map, FACE_CELL)
EnvMap::EnvMap() :
obstacleMark(map, EDGE_CELL),
buildingMark(map, FACE_CELL),
refineMark(map, FACE_CELL),
coarsenMark(map, FACE_CELL)
{
position = map.addAttribute<PFP::VEC3>(VERTEX_ORBIT, "position");
normal = map.addAttribute<PFP::VEC3>(VERTEX_ORBIT, "normal");
......@@ -29,10 +33,11 @@ unsigned int EnvMap::mapMemoryCost()
Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
{
CellMarkerStore m(map,FACE_ORBIT);
CellMarkerStore m(map, FACE_ORBIT);
for(Dart d = map.begin(); d != map.end(); map.next(d))
{
if(!m.isMarked(d)) {
if(!m.isMarked(d))
{
m.mark(d);
if(!buildingMark.isMarked(d) && Algo::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true))
return d;
......@@ -46,7 +51,7 @@ Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
void EnvMap::init()
{
float sideSize = 70.0f;
unsigned int nbSquares = 15;
unsigned int nbSquares = 3;
CityGenerator::generateCity<PFP>(map, position, obstacleMark, buildingMark, sideSize, nbSquares);
// CityGenerator::generateMall<PFP>(map, position, obstacleMark, buildingMark, sideSize);
// CityGenerator::simplifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
......@@ -54,9 +59,6 @@ void EnvMap::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)
......@@ -109,9 +111,8 @@ void EnvMap::registerObstaclesInFaces()
for(Dart d = map.begin(); d != map.end(); map.next(d))
{
if(m.isMarked(d) && !buildingMark.isMarked(d)) {
if(m.isMarked(d) && !buildingMark.isMarked(d))
m.unmark(d);
}
}
}
......@@ -217,20 +218,40 @@ void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbo
void EnvMap::agentChangeFace(Agent* agent, Dart oldFace)
{
Dart newFace = agent->part_.d;
popAgentInCells(agent, oldFace);
// addCoarsenCandidate(oldFace);
pushAgentInCells(agent, newFace);
if(!coarsenMark.isMarked(oldFace) && agentvect[oldFace].size() < nbAgentsToSimplify / 2)
{
coarsenMark.mark(oldFace);
coarsenCandidate.push_back(oldFace);
}
pushAgentInCells(agent, agent->part_.d);
addRefineCandidate(agent->part_.d);
if(!refineMark.isMarked(newFace) && !coarsenMark.isMarked(newFace) && agentvect[newFace].size() > nbAgentsToSubdivide)
{
std::pair<bool,bool>& sf = subdivisableFace[newFace];
if(sf.first == false || (sf.first == true && sf.second))
{
refineMark.mark(newFace);
refineCandidate.push_back(newFace);
}
}
}
void EnvMap::updateMap()
{
assert(map.getCurrentLevel() == map.getMaxLevel()) ;
for(std::map<unsigned int, Dart>::iterator it = refineCandidate.begin(); it != refineCandidate.end(); ++it)
for(std::vector<Dart>::iterator it = refineCandidate.begin(); it != refineCandidate.end(); ++it)
{
Dart d = (*it).second ;
Dart d = (*it) ;
if(!map.isDartValid(d))
{
std::cout << "refine INVALID" << std::endl ;
}
refineMark.unmark(d);
if(agentvect[d].size() > nbAgentsToSubdivide)
{
......@@ -256,7 +277,7 @@ void EnvMap::updateMap()
PFP::VEC3 edge = Algo::Geometry::vectorOutOfDart<PFP>(map, fd, position) ;
PFP::VEC3 proj = fCenter - (p + (edge * (fCenter - p) / edge.norm2()) * edge) ;
if(proj.norm2() < maxEdgeSize)
// if(Algo::Geometry::vectorOutOfDart<PFP>(map, fd, position).norm2() < maxEdgeSize)
// if(Algo::Geometry::vectorOutOfDart<PFP>(map, fd, position).norm2() < maxEdgeSize)
{
subdivisable = false ;
break ;
......@@ -344,11 +365,16 @@ void EnvMap::updateMap()
}
}
}
/*
for(std::map<unsigned int, Dart>::iterator it = coarsenCandidate.begin(); it != coarsenCandidate.end(); ++it)
for(std::vector<Dart>::iterator it = coarsenCandidate.begin(); it != coarsenCandidate.end(); ++it)
{
unsigned int face = (*it).first ;
Dart d = (*it).second ;
Dart d = (*it) ;
if(!map.isDartValid(d))
{
std::cout << "coarsen INVALID" << std::endl ;
continue ;
}
coarsenMark.unmark(d) ;
unsigned int fLevel = map.faceLevel(d) ;
Dart old = map.faceOldestDart(d) ;
......@@ -384,14 +410,24 @@ void EnvMap::updateMap()
do
{
map.setCurrentLevel(fLevel) ;
unsigned int fEmb = map.getEmbedding(fit, FACE_ORBIT) ;
if(fEmb != face)
coarsenCandidate.erase(fEmb) ;
coarsenMark.unmark(fit) ;
std::vector<Dart>::iterator start = it + 1 ;
if(start != coarsenCandidate.end())
{
unsigned int fEmb = map.getEmbedding(FACE_ORBIT, fit) ;
while(start != coarsenCandidate.end())
{
if(map.getEmbedding(FACE_ORBIT, *start) == fEmb)
start = coarsenCandidate.erase(start) ;
else
++start ;
}
}
PFP::AGENTS a(agentvect[fit]) ;
agents.insert(agents.end(), a.begin(), a.end()) ;
map.setCurrentLevel(map.getMaxLevel()) ;
for(PFP::AGENTS::iterator it = a.begin(); it != a.end(); ++it)
popAgentInCells(*it, fit);
for(PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
popAgentInCells(*ait, fit);
map.setCurrentLevel(fLevel - 1) ;
Dart nf = map.phi2(fit) ;
if(!map.faceIsSubdivided(nf))
......@@ -400,8 +436,8 @@ void EnvMap::updateMap()
PFP::AGENTS& an = agentvect[nf] ;
for(PFP::AGENTS::iterator ait = an.begin(); ait != an.end(); ++ait)
{
if((*ait)->part_.d == nf)
(*ait)->part_.d = map.phi1(nf) ;
if((*ait)->part_.d == map.phi1(nf))
(*ait)->part_.d = nf ;
}
map.setCurrentLevel(fLevel - 1) ;
}
......@@ -411,20 +447,32 @@ void EnvMap::updateMap()
{
map.setCurrentLevel(fLevel) ;
Dart centerFace = map.phi2(map.phi1(old)) ;
unsigned int fEmb = map.getEmbedding(centerFace, FACE_ORBIT) ;
if(fEmb != face)
coarsenCandidate.erase(fEmb) ;
coarsenMark.unmark(centerFace) ;
std::vector<Dart>::iterator start = it + 1 ;
if(start != coarsenCandidate.end())
{
unsigned int fEmb = map.getEmbedding(FACE_ORBIT, centerFace) ;
while(start != coarsenCandidate.end())
{
if(map.getEmbedding(FACE_ORBIT, *start) == fEmb)
start = coarsenCandidate.erase(start) ;
else
++start ;
}
}
PFP::AGENTS a(agentvect[centerFace]) ;
agents.insert(agents.end(), a.begin(), a.end()) ;
map.setCurrentLevel(map.getMaxLevel());
for(PFP::AGENTS::iterator it = a.begin(); it != a.end(); ++it)
popAgentInCells(*it, centerFace);
for(PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
popAgentInCells(*ait, centerFace);
map.setCurrentLevel(fLevel - 1) ;
}
Algo::IHM::coarsenFace<PFP>(map, old, position) ;
// gestion des obstacles !!
std::pair<bool,bool>& sf = subdivisableFace[old] ;
sf.first = true;
sf.second = true;
map.setCurrentLevel(map.getMaxLevel());
......@@ -446,12 +494,13 @@ void EnvMap::updateMap()
}
dd = map.phi1(dd);
} while(dd != old);
}
}
map.setCurrentLevel(cur) ;
}
}
*/
map.setCurrentLevel(map.getMaxLevel()) ;
}
......
......@@ -8,11 +8,19 @@ Simulator::Simulator() : globalTime_(0.0f), timeStep_(0.2f)
// importAgents("myAgents.pos");
setupScenario(15000);
// addPathsToAgents();
for(unsigned int i = 0; i < agents_.size(); ++i)
unsigned nbAgents = agents_.size();
for(unsigned int i = 0; i < nbAgents; ++i)
{
agents_[i]->updateObstacleNeighbors();
agents_[i]->updateAgentNeighbors();
}
// tc1 = new ThreadUpdateInfo(agents_, 0, nbAgents / 4);
// tc2 = new ThreadUpdateInfo(agents_, nbAgents / 4, nbAgents / 2);
// tc3 = new ThreadUpdateInfo(agents_, nbAgents / 2, nbAgents * 3 / 4);
// tc4 = new ThreadUpdateInfo(agents_, nbAgents * 3 / 4, nbAgents);
}
Simulator::~Simulator()
......@@ -21,60 +29,12 @@ 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]->updateObstacleNeighbors();
......@@ -100,14 +60,6 @@ void Simulator::doStep()
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;
......@@ -135,6 +87,7 @@ void Simulator::doStep()