Commit dbb974dc authored by Thomas's avatar Thomas
Browse files

ca marche pas c'est nul

parent f0adcdd9
......@@ -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<5;++i)
for(unsigned int i=0;i<1;++i)
{
float floorHeight = 100;
typename PFP::VEC3 transl(0,0,floorHeight);
......
......@@ -18,54 +18,65 @@ float pathCostSqr(typename PFP::MAP& map,const typename PFP::TVEC3& position, Da
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< Dart > tablePred(map,FACE_ORBIT);
std::vector<std::pair<float,Dart> > vToDev;
std::map<float,Dart> vToDev;
CellMarker m(map,FACE_ORBIT);
Dart toDev = stopPos;
vToDev.push_back(std::make_pair(0, toDev));
vToDev.insert(std::make_pair(0, toDev));
tablePred[toDev] = toDev;
do {
//dev cell
do
{
vToDev.erase(vToDev.begin());
//get all vertex-adjacent cells
Dart dd = toDev;
do {
Dart ddd = map.alpha1(dd);
do {
ddd = map.alpha1(ddd);
if(ddd!=dd) {
if(!bad.isMarked(ddd) && tablePred[ddd]==EMBNULL) {
//evaluate their cost and push them in the vector to dev
if(tablePred[ddd]==EMBNULL)
tablePred[ddd]=toDev;
std::vector<std::pair<float,Dart> >::iterator it=vToDev.begin();
float costDDD=pathCostSqr<PFP>(map,position,startPos,ddd)+(vToDev.begin())->first;
while(it!=vToDev.end() && (*it).first<costDDD)
++it;
vToDev.insert(it, std::make_pair(costDDD, ddd));
}
do
{
Dart ddd = map.alpha1(map.alpha1(dd));
while(ddd != dd)
{
//evaluate their cost and push them in the vector to dev
if(!m.isMarked(ddd) && !bad.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;
}
} while(ddd!=dd);
ddd = map.alpha1(ddd);
}
dd = map.phi1(dd);
} while(dd!=toDev);
} while(dd != toDev);
//remove developped cell and get new cell to dev
if(vToDev.size()>0) {
//get new cell to dev
if(vToDev.size()>0)
toDev = (vToDev.begin())->second;
vToDev.erase(vToDev.begin());
}
} while(vToDev.size()>0 && !map.sameOrientedFace(startPos,toDev));
//if path found : from start to stop -> push all predecessors
if(map.sameOrientedFace(startPos,toDev))
{
Dart toPush=startPos;
do {
do
{
path.push_back(toPush);
toPush = tablePred[toPush];
} while(!map.sameOrientedFace(toPush,stopPos));
}
else
{
std::cout << "fichtre" << std::endl;
}
return path;
}
......
......@@ -125,15 +125,14 @@ void Simulator::setupScenario(unsigned int nbMaxAgent)
void Simulator::addPathsToAgents()
{
//city
// unsigned int dartDistForPath=50;
unsigned int dartDistForPath=4;
unsigned int dartDistForPath=50;
for(unsigned int i = 0; i < agents_.size(); ++i)
{
agents_[i]->goals_.clear();
Dart dStart = agents_[i]->part_.d;
Dart dStop = dStart;
for(unsigned int j = 0; envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath+rand()*20; ++j)
for(unsigned int j = 0; envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath+rand()*20 || envMap_.map.sameOrientedFace(dStop,dStart); ++j)
{
envMap_.map.next(dStop);
if(dStop == envMap_.map.end())
......@@ -146,7 +145,7 @@ void Simulator::addPathsToAgents()
agents_[i]->goals_.push_back(Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position));
Dart dStop2 = dStop;
for(unsigned int j = 0; envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath+rand()*20; ++j)
for(unsigned int j = 0; envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath+rand()*20 || envMap_.map.sameOrientedFace(dStop,dStop2) || envMap_.map.sameOrientedFace(dStop2,dStart); ++j)
{
envMap_.map.next(dStop2);
if(dStop2 == envMap_.map.end())
......
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