Commit ff4a4d6a authored by pitiot's avatar pitiot
Browse files

maj cellmarkermemo => 1 unique dans envmap + maj simplif encore un bug

parent 6fef9060
......@@ -119,13 +119,13 @@ public:
CellMarker<EDGE> obstacleMark ;
CellMarker<FACE> buildingMark ;
CellMarker<FACE> pedWayMark ;
CellMarkerMemo<FACE> memo_mark;
// ajout moving obst
void addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& belonging_cells, std::vector<Dart> * nieghbor_cells);
// void addMovingObstAsNeighbor (MovingObstacle * mo,std::vector<Dart> belonging_cells, std::vector<Dart> *neighbor_cells);
void pushObstNeighborInCells(Obstacle* o, Dart d);
void popObstNeighborInCells(Obstacle* o, Dart d);
void find_next(Obstacle* o,Dart * d, CellMarkerStore<FACE>& cms);
void find_next(Obstacle* o,Dart * d, CellMarkerMemo<FACE>& cms);
std::vector<Dart> newBuildings ;
......@@ -299,16 +299,8 @@ inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& b
assert(!belonging_cells.empty());
neighbor_cells->clear();
CellMarkerStore<FACE> MovingObstMark(map);
CellMarkerStore<FACE> OneRingMark(map);
for(std::vector<Dart>::const_iterator it= belonging_cells.begin();it!=belonging_cells.end();++it)
{
assert(!buildingMark.isMarked(*it)) ;
MovingObstMark.mark(*it);
}
std::vector<Dart>::const_iterator it=belonging_cells.begin();
Dart beg = NIL;
Dart first =NIL;
......@@ -324,7 +316,7 @@ inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& b
do {
dd=map.alpha1(map.alpha1(d));
do {
if (!MovingObstMark.isMarked(dd))
if (!memo_mark.isMarked(dd))
{
first = dd;
}
......@@ -348,14 +340,14 @@ inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& b
(*neighbor_cells).push_back(d);
}
find_next(o,&d, MovingObstMark);
find_next(o,&d, memo_mark);
// CGoGNout<<"d : "<<d<<CGoGNendl;
}while(!map.sameFace(d,first));
}
// find_next cherche la prochaine case "voisine" d'un obstacle faisant parti d'un movingobstacle (algo de parcours du one-ring )
inline void EnvMap::find_next(Obstacle* o,Dart * ddd, CellMarkerStore<FACE>& cms)
inline void EnvMap::find_next(Obstacle* o,Dart * ddd, CellMarkerMemo<FACE>& cms)
{
Dart d=*ddd;
Dart first = NIL;
......
......@@ -23,7 +23,7 @@ public:
bool is_inside (VEC3 p);
void computePrefVelocity();
std::vector<Dart> getMemoCross(const VEC3& pos, const VEC3& dest, Dart * d1);
std::vector<Dart> getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1);
void update();
......@@ -47,6 +47,7 @@ public:
std::vector<Dart> * neighbor_cells;
std::vector<Dart> general_belonging;
VEC3 front;
CellMarkerMemo<FACE> * memo_mark;
//difference entre 2 listes utilisé pour update
// std::list<Dart> * res_diff;
......
......@@ -21,6 +21,7 @@ EnvMap::EnvMap() :
obstacleMark(map),
buildingMark(map),
pedWayMark(map),
memo_mark(map),
#ifndef SPATIAL_HASHING
refineMark(map),
coarsenMark(map)
......@@ -109,6 +110,7 @@ void EnvMap::init(unsigned int config, REAL width, REAL height, REAL minSize, RE
for (unsigned int i = subdivisableFace.begin(); i < subdivisableFace.end(); subdivisableFace.next(i))
subdivisableFace[i].first = false ;
#endif
}
void EnvMap::markPedWay()
......@@ -610,214 +612,264 @@ void EnvMap::updateMap()
}
refineCandidate.clear() ;
// // On recrée une liste des faces à simplifier en empêchant les doublons
// // On en profite pour vérifier les conditions de simplifications
// std::vector<Dart> checkCoarsenCandidate ;
// checkCoarsenCandidate.reserve(coarsenCandidate.size()) ;
//
// for (unsigned int it = 0; it < coarsenCandidate.size(); ++it)
// {
// Dart old = coarsenCandidate[it] ;
// bool oldIsMarked = coarsenMark.isMarked(old) ;
// coarsenMark.unmark(old) ;
//
// unsigned int fLevel = map.faceLevel(old) ;
//
// if (oldIsMarked && fLevel > 0 && map.getDartLevel(old) < fLevel)
// {
// unsigned int cur = map.getCurrentLevel() ;
// map.setCurrentLevel(fLevel - 1) ;
//
// if (map.faceIsSubdividedOnce(old))
// {
// // on compte le nombre d'agents dans les sous-faces
// // on en profite pour compter le degré de la face grossière
// unsigned int degree = 0 ;
// unsigned int nbAgents = 0 ;
// Dart fit = old ;
// do
// {
// nbAgents += agentvect[fit].size() ;
// ++degree ;
// coarsenMark.unmark(fit) ;
//// TODO Optimisation déjà faite
//// unsigned int start = it + 1;
//// unsigned int fEmb = map.getEmbedding<FACE>(fit) ;
//// while(start < coarsenCandidate.size())
//// {
//// if(map.getEmbedding<FACE>(coarsenCandidate[start]) == fEmb)
//// {
//// coarsenCandidate[start] = coarsenCandidate.back() ;
//// coarsenCandidate.pop_back() ;
//// }
//// else
//// ++start ;
//// }
//
// fit = map.phi1(fit) ;
// } while (fit != old) ;
//
//
// //Loop subdivision
// if (degree == 3)
// {
// map.setCurrentLevel(fLevel) ;
// Dart centerFace = map.phi2(map.phi1(old)) ;
// nbAgents += agentvect[centerFace].size() ;
// coarsenMark.unmark(centerFace) ;
//// TODO Optimisation déjà faite
//// unsigned int start = it + 1;
//// unsigned int fEmb = map.getEmbedding<FACE>(centerFace) ;
//// while(start < coarsenCandidate.size())
//// {
//// if(map.getEmbedding<FACE>(coarsenCandidate[start]) == fEmb)
//// {
//// coarsenCandidate[start] = coarsenCandidate.back() ;
//// coarsenCandidate.pop_back() ;
//// }
//// else
//// ++start ;
//// }
// map.setCurrentLevel(fLevel - 1) ;
// }
// if (nbAgents < nbAgentsToSimplify) checkCoarsenCandidate.push_back(old) ;
// }
// map.setCurrentLevel(cur) ;
// }
// }
// coarsenCandidate.clear() ;
//
// // On réalise la simplification (les conditions ont déjà été vérifiées)
// for (unsigned int it = 0; it < checkCoarsenCandidate.size(); ++it)
// {
// Dart old = checkCoarsenCandidate[it] ;
//
// unsigned int fLevel = map.faceLevel(old) ;
// unsigned int cur = map.getCurrentLevel() ;
// map.setCurrentLevel(fLevel - 1) ;
//
// // on compte le degré de la face grossière
// unsigned int degree = 0 ;
// Dart fit = old ;
// do
// {
// ++degree ;
// fit = map.phi1(fit) ;
// } while (fit != old) ;
//
// PFP::AGENTS agents ;
// PFP::OBSTACLEVECT obst ;
// PFP::OBSTACLEVECT neighborObst ;
//
// fit = old ;
// do
// {
// PFP::AGENTS a(agentvect[fit]) ;
// PFP::OBSTACLEVECT ob(obstvect[fit]) ;
// PFP::OBSTACLEVECT nob(neighborObstvect[fit]) ;
//
// agents.insert(agents.end(), a.begin(), a.end()) ;
// obst.insert(obst.end(), ob.begin(), ob.end()) ;
// neighborObst.insert(neighborObst.end(), nob.begin(), nob.end()) ;
// map.setCurrentLevel(map.getMaxLevel()) ;
// 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))
// {
// map.setCurrentLevel(fLevel) ;
// PFP::AGENTS& an = agentvect[nf] ;
// PFP::OBSTACLEVECT resetob = obstvect[nf] ;
// for (PFP::AGENTS::iterator ait = an.begin(); ait != an.end(); ++ait)
// {
// if ((*ait)->part_.d == map.phi1(nf)) (*ait)->part_.d = nf ;
// }
// for (PFP::OBSTACLEVECT::iterator ait = resetob.begin(); ait != resetob.end(); ++ait)
// {
// MovingObstacle* mo = (*ait)->mo ;
// if (mo != NULL)
// On recrée une liste des faces à simplifier en empêchant les doublons
// On en profite pour vérifier les conditions de simplifications
std::vector<Dart> checkCoarsenCandidate ;
checkCoarsenCandidate.reserve(coarsenCandidate.size()) ;
for (unsigned int it = 0; it < coarsenCandidate.size(); ++it)
{
Dart old = coarsenCandidate[it] ;
bool oldIsMarked = coarsenMark.isMarked(old) ;
coarsenMark.unmark(old) ;
unsigned int fLevel = map.faceLevel(old) ;
if (oldIsMarked && fLevel > 0 && map.getDartLevel(old) < fLevel)
{
unsigned int cur = map.getCurrentLevel() ;
map.setCurrentLevel(fLevel - 1) ;
if (map.faceIsSubdividedOnce(old))
{
// on compte le nombre d'agents dans les sous-faces
// on en profite pour compter le degré de la face grossière
unsigned int degree = 0 ;
unsigned int nbAgents = 0 ;
Dart fit = old ;
do
{
nbAgents += agentvect[fit].size() ;
++degree ;
coarsenMark.unmark(fit) ;
// TODO Optimisation déjà faite
// unsigned int start = it + 1;
// unsigned int fEmb = map.getEmbedding<FACE>(fit) ;
// while(start < coarsenCandidate.size())
// {
// resetPart(mo,nf) ;
// if(map.getEmbedding<FACE>(coarsenCandidate[start]) == fEmb)
// {
// coarsenCandidate[start] = coarsenCandidate.back() ;
// coarsenCandidate.pop_back() ;
// }
// else
// ++start ;
// }
// }
// map.setCurrentLevel(fLevel - 1) ;
// }
// fit = map.phi1(fit) ;
// } while (fit != old) ;
//
// if (degree == 3)
// {
// map.setCurrentLevel(fLevel) ;
// Dart centerFace = map.phi2(map.phi1(old)) ;
// PFP::AGENTS a(agentvect[centerFace]) ;
// PFP::OBSTACLEVECT ob(obstvect[centerFace]) ;
// PFP::OBSTACLEVECT nob(neighborObstvect[centerFace]) ;
// agents.insert(agents.end(), a.begin(), a.end()) ;
// obst.insert(obst.end(), ob.begin(), ob.end()) ;
// neighborObst.insert(neighborObst.end(), nob.begin(), nob.end()) ;
// map.setCurrentLevel(map.getMaxLevel()) ;
// for (PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
// popAgentInCells(*ait, centerFace) ;
// map.setCurrentLevel(fLevel - 1) ;
// }
// neighborAgentvect[old].clear() ;
//
// for (PFP::OBSTACLEVECT::iterator ait = obst.begin(); ait != obst.end(); ++ait)
// removeElementFromVector<Obstacle* >(neighborObst, (*ait));
// // TODO Check with optimisation
// map.setCurrentLevel(map.getMaxLevel()) ;
// for (PFP::OBSTACLEVECT::iterator ait = obst.begin(); ait != obst.end(); ++ait)
// register_pop(*ait, (*ait)->index) ;
// for (PFP::OBSTACLEVECT::iterator ait = neighborObst.begin(); ait != neighborObst.end(); ++ait)
// register_pop(*ait, (*ait)->index) ;
// map.setCurrentLevel(fLevel - 1) ;
//
// Algo::IHM::coarsenFace<PFP>(map, old, position) ;
//
// std::pair<bool, bool>& sf = subdivisableFace[old] ;
// sf.first = true ;
// sf.second = true ;
//
// map.setCurrentLevel(map.getMaxLevel()) ;
fit = map.phi1(fit) ;
} while (fit != old) ;
//Loop subdivision
if (degree == 3)
{
map.setCurrentLevel(fLevel) ;
Dart centerFace = map.phi2(map.phi1(old)) ;
nbAgents += agentvect[centerFace].size() ;
coarsenMark.unmark(centerFace) ;
// TODO Optimisation déjà faite
// unsigned int start = it + 1;
// unsigned int fEmb = map.getEmbedding<FACE>(centerFace) ;
// while(start < coarsenCandidate.size())
// {
// if(map.getEmbedding<FACE>(coarsenCandidate[start]) == fEmb)
// {
// coarsenCandidate[start] = coarsenCandidate.back() ;
// coarsenCandidate.pop_back() ;
// }
// else
// ++start ;
// }
map.setCurrentLevel(fLevel - 1) ;
}
if (nbAgents < nbAgentsToSimplify) checkCoarsenCandidate.push_back(old) ;
}
map.setCurrentLevel(cur) ;
}
}
coarsenCandidate.clear() ;
// On réalise la simplification (les conditions ont déjà été vérifiées)
for (unsigned int it = 0; it < checkCoarsenCandidate.size(); ++it)
{
Dart old = checkCoarsenCandidate[it] ;
unsigned int fLevel = map.faceLevel(old) ;
unsigned int cur = map.getCurrentLevel() ;
map.setCurrentLevel(fLevel - 1) ;
// on compte le degré de la face grossière
unsigned int degree = 0 ;
Dart fit = old ;
do
{
++degree ;
fit = map.phi1(fit) ;
} while (fit != old) ;
PFP::AGENTS agents ;
PFP::OBSTACLEVECT obst ;
PFP::OBSTACLEVECT neighborObst ;
//premier tour pour les présents
fit = old ;
do
{
PFP::AGENTS a(agentvect[fit]) ;
PFP::OBSTACLEVECT ob(obstvect[fit]) ;
agents.insert(agents.end(), a.begin(), a.end()) ;
map.setCurrentLevel(map.getMaxLevel()) ;
for(PFP::OBSTACLEVECT::iterator ait = ob.begin(); ait != ob.end(); ++ait)
{
register_pop(*ait, (*ait)->index) ;
obst.push_back(*ait);
}
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))
{
map.setCurrentLevel(fLevel) ;
PFP::AGENTS& an = agentvect[nf] ;
PFP::OBSTACLEVECT resetob = obstvect[nf] ;
for (PFP::AGENTS::iterator ait = an.begin(); ait != an.end(); ++ait)
{
if ((*ait)->part_.d == map.phi1(nf)) (*ait)->part_.d = nf ;
}
for (PFP::OBSTACLEVECT::iterator ait = resetob.begin(); ait != resetob.end(); ++ait)
{
MovingObstacle* mo = (*ait)->mo ;
if (mo != NULL)
{
resetPart(mo,nf) ;
}
}
map.setCurrentLevel(fLevel - 1) ;
}
fit = map.phi1(fit) ;
} while (fit != old) ;
if (degree == 3)
{
map.setCurrentLevel(fLevel) ;
Dart centerFace = map.phi2(map.phi1(old)) ;
PFP::AGENTS a(agentvect[centerFace]) ;
PFP::OBSTACLEVECT ob(obstvect[centerFace]) ;
agents.insert(agents.end(), a.begin(), a.end()) ;
map.setCurrentLevel(map.getMaxLevel()) ;
for(PFP::OBSTACLEVECT::iterator ait = ob.begin(); ait != ob.end(); ++ait)
{
register_pop(*ait, (*ait)->index) ;
obst.push_back(*ait);
}
for (PFP::AGENTS::iterator ait = a.begin(); ait != a.end(); ++ait)
popAgentInCells(*ait, centerFace) ;
map.setCurrentLevel(fLevel - 1) ;
}
//deuxieme tour concernant les voisins
fit = old ;
do
{
PFP::OBSTACLEVECT nob(neighborObstvect[fit]) ;
map.setCurrentLevel(map.getMaxLevel()) ;
for(PFP::OBSTACLEVECT::iterator ait = nob.begin(); ait != nob.end(); ++ait)
{
register_pop(*ait, (*ait)->index) ;
neighborObst.push_back(*ait);
}
map.setCurrentLevel(fLevel - 1) ;
fit = map.phi1(fit) ;
} while (fit != old) ;
if (degree == 3)
{
map.setCurrentLevel(fLevel) ;
Dart centerFace = map.phi2(map.phi1(old)) ;
PFP::OBSTACLEVECT nob(neighborObstvect[centerFace]) ;
map.setCurrentLevel(map.getMaxLevel()) ;
for(PFP::OBSTACLEVECT::iterator ait = nob.begin(); ait != nob.end(); ++ait)
{
register_pop(*ait, (*ait)->index) ;
neighborObst.push_back(*ait);
}
map.setCurrentLevel(fLevel - 1) ;
}
neighborAgentvect[old].clear() ;
// TODO Check with optimisation
//
// for (PFP::AGENTS::iterator itA = agents.begin(); itA != agents.end(); ++itA)
// {
// (*itA)->part_.d = old ;
// pushAgentInCells(*itA, old) ;
// }
// CGoGNout<<"Avant pop"<<CGoGNendl;
// CGoGNout<<"Obst dans la case : ";
// for (PFP::OBSTACLEVECT::iterator ait = obst.begin(); ait != obst.end(); ++ait)
// {
// resetObstPartInFace(*ait, old) ;
// update_registration(*ait) ;
// }
// CGoGNout<<(*ait)->index<<" ; ";
// CGoGNout<<CGoGNendl;
// CGoGNout<<"Obst voisins : ";
// for (PFP::OBSTACLEVECT::iterator ait = neighborObst.begin(); ait != neighborObst.end(); ++ait)
// {
// update_registration(*ait) ;
// }
//
// Dart dd = old ;
// do
// {
// Dart ddd = map.alpha1(map.alpha1(dd)) ;
// while (ddd != dd)
// {
// neighborAgentvect[old].insert(neighborAgentvect[old].end(), agentvect[ddd].begin(), agentvect[ddd].end()) ;
// ddd = map.alpha1(ddd) ;
// }
// dd = map.phi1(dd) ;
// } while (dd != old) ;
//
// if (fLevel > 1 && !coarsenMark.isMarked(old) && agentvect[old].size() < nbAgentsToSimplify)
// {
// coarsenMark.mark(old) ;
// coarsenCandidate.push_back(map.faceOldestDart(old)) ;
// }
//
// map.setCurrentLevel(cur) ;
// }
// map.setCurrentLevel(map.getMaxLevel()) ;
// if (coarsenCandidate.size() > 0) updateMap() ;
// CGoGNout<<(*ait)->index<<" ; ";
// CGoGNout<<CGoGNendl;
map.setCurrentLevel(fLevel - 1) ;
Algo::IHM::coarsenFace<PFP>(map, old, position) ;
std::pair<bool, bool>& sf = subdivisableFace[old] ;
sf.first = true ;
sf.second = true ;
map.setCurrentLevel(map.getMaxLevel()) ;
for (PFP::AGENTS::iterator itA = agents.begin(); itA != agents.end(); ++itA)
{
(*itA)->part_.d = old ;
pushAgentInCells(*itA, old) ;
}
for (PFP::OBSTACLEVECT::iterator ait = obst.begin(); ait != obst.end(); ++ait)
{
resetObstPartInFace(*ait, old) ;
update_registration(*ait) ;
}
for (PFP::OBSTACLEVECT::iterator ait = neighborObst.begin(); ait != neighborObst.end(); ++ait)
{
update_registration(*ait) ;
}
Dart dd = old ;
do
{
Dart ddd = map.alpha1(map.alpha1(dd)) ;
while (ddd != dd)
{
neighborAgentvect[old].insert(neighborAgentvect[old].end(), agentvect[ddd].begin(), agentvect[ddd].end()) ;
ddd = map.alpha1(ddd) ;
}
dd = map.phi1(dd) ;
} while (dd != old) ;
if (fLevel > 1 && !coarsenMark.isMarked(old) && agentvect[old].size() < nbAgentsToSimplify)
{
coarsenMark.mark(old) ;
coarsenCandidate.push_back(map.faceOldestDart(old)) ;
}
map.setCurrentLevel(cur) ;
}
map.setCurrentLevel(map.getMaxLevel()) ;
if (coarsenCandidate.size() > 0) updateMap() ;
}
void EnvMap::resetAgentInFace(Agent* agent)
......
......@@ -140,7 +140,7 @@ void MovingObstacle::updateFixedObstNeighbors()
for(int k =0;k<2;k++)
{
registering_part->get_memo(vertices[k]);
registering_part->get_memo(vertices[k],sim_->envMap_.memo_mark);
std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[registering_part->d];
//search all obstacles around
......@@ -388,12 +388,12 @@ void register_add(Obstacle* o, int n, const std::vector<Dart>& memo_cross)
}
}
std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest, Dart * d1)
std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1)
{
registering_part->get_memo(pos);
*d1=registering_part->d;
registering_part->get_memo(pos,sim_->envMap_.memo_mark);
d1=registering_part->d;
// CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest<<CGoGNendl;
return (registering_part->get_memo(dest));
return (registering_part->get_memo(dest,sim_->envMap_.memo_mark));
}
void MovingObstacle::register_update(Obstacle* o, int n)// maj de l'enregistrement
......@@ -410,7 +410,7 @@ void MovingObstacle::register_update(Obstacle* o, int n)// maj de l'enregistreme
// if(p1->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS
// || p2->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
// {
memo = getMemoCross(p1,p2,&d1);
memo = getMemoCross(p1,p2,d1);
d2=registering_part->d;
// memo.sort();
// modif=true;
......@@ -450,7 +450,7 @@ void update_registration(Obstacle * o)// réenregistre l'obstacle en question
Dart d1=NIL;
Dart d2=NIL;
std::vector<Dart> memo;
memo = mo->getMemoCross(p1,p2,&d1);
memo = mo->getMemoCross(p1,p2,d1);
d2=mo->registering_part->d;
if(mo->sim_->envMap_.map.sameFace(d1,d2))
......@@ -474,7 +474,7 @@ void resetPartSubdiv(Obstacle* o)
VEC3 pos =mo->registering_part->getPosition();
mo->registering_part->ParticleBase<PFP>::move(Algo::Geometry::faceCentroid<PFP>(mo->sim_->envMap_.map, mo->registering_part->d, mo->sim_->envMap_.position)) ;
mo->registering_part->setState(FACE) ;
mo->registering_part->get_memo(pos) ;
mo->registering_part->get_memo(pos,mo->sim_->envMap_.memo_mark) ;
}
}
......
......@@ -35,7 +35,7 @@ void Simulator::init(unsigned int config, int minSize, float dimension, bool ena
setupCircleScenario(100) ;
break ;
case 1 :
setupCorridorScenario(500, 40) ;
setupCorridorScenario(500,100) ;
break ;
case 2 :
setupScenario(1000) ;
......@@ -285,7 +285,7 @@ void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObst
for (unsigned int i = 0 ; i < nbObstacles ; i++)
{
float x = xStartMin + ((int)i*30) % xStartDelta;
std::cout << "x ? " << x << " xStartDelta " << xStartDelta << std::endl;