Commit fb0ef620 authored by Jund Thomas's avatar Jund Thomas
Browse files

gestion de but multiples pour les obstacles

parent f5d9d943
......@@ -55,9 +55,10 @@ public:
VEC3 finalGoal;
float make_half_turn;
// Dart finalDart;
//
// unsigned int curGoal_;
//
std::vector<VEC3> goals_ ;
unsigned int curGoal_;
// static float neighborDistSq_;
static unsigned int maxNeighbors_;
static float detectionFixedObst;
......
......@@ -106,6 +106,7 @@ public:
void setupMovingObstacle(unsigned int nbObstacles) ;
#ifndef SPATIAL_HASHING
void addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal);
void addPathToCorner() ;
void addPathsToAgents() ;
void addPathsToAgents_height() ;
......
......@@ -73,7 +73,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, V
movingObstNeighbors_.reserve(maxNeighbors_);
rot = rota;
finalGoal = goal;
VEC3 sum = 0;
nb_agents_voisins = 0;
nb_register_cells = 0;
......@@ -121,6 +121,10 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, V
obstacle_range = 15.0f * 15.0f;
make_half_turn = get_angle(finalGoal - center, (parts_[0]->getPosition() + parts_[1]->getPosition()) / 2 - center) * nbVertices;
goals_.push_back(center);
goals_.push_back(goal);
curGoal_ = 0;
}
bool MovingObstacle::test_opposition(VEC3 o, VEC3 p1, VEC3 p2)
......@@ -192,31 +196,49 @@ void MovingObstacle::updateFixedObstNeighbors()
// TODO Check position
void MovingObstacle::computePrefVelocity()
{
VEC3 goalVector = finalGoal - center;
int co=2;
float angle;
float goalDist2 = goalVector.norm2();
// CGoGNout << "finalGoal "<< finalGoal << CGoGNendl;
// CGoGNout << "dist 2 "<< goalDist2 << CGoGNendl;
if(goalDist2 < 2.0f)
{
// co=rand() % 2 + 1;
VEC3 goalVector = goals_[curGoal_] - center ;
float goalDist2 = goalVector.norm2() ;
if(co==1)
finalGoal[0]*=-1;
else
finalGoal[1]*=-1;
if (goalDist2 < 2.0f)
{
curGoal_ = (curGoal_ + 1) % goals_.size() ;
goalVector = goals_[curGoal_] - center ;
goalDist2 = goalVector.norm2() ;
}
goalVector = finalGoal - center;
angle =get_angle(finalGoal-center,(parts_[0]->getPosition())-center);
make_half_turn=angle*nbVertices;
if (goalDist2 > maxSpeed_)
{
goalVector.normalize() ;
goalVector *= maxSpeed_;
}
if (goalDist2 > 1.0f)
goalVector.normalize();
prefVelocity_ = goalVector ;
prefVelocity_=goalVector;
// VEC3 goalVector = finalGoal - center;
// int co=2;
// float angle;
// float goalDist2 = goalVector.norm2();
// // CGoGNout << "finalGoal "<< finalGoal << CGoGNendl;
// // CGoGNout << "dist 2 "<< goalDist2 << CGoGNendl;
// if(goalDist2 < 2.0f)
// {
//// co=rand() % 2 + 1;
//
//
// if(co==1)
// finalGoal[0]*=-1;
// else
// finalGoal[1]*=-1;
//
// goalVector = finalGoal - center;
// angle =get_angle(finalGoal-center,(parts_[0]->getPosition())-center);
// make_half_turn=angle*nbVertices;
// }
//
// if (goalDist2 > 1.0f)
// goalVector.normalize();
//
// prefVelocity_=goalVector;
}
......
......@@ -304,6 +304,23 @@ void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObst
vPos.push_back(start-xSide-ySide);
}
mo4= new MovingObstacle(this, i,vPos,goal,0);
//for generating a random path
unsigned int dartDistForPath = 50 ;
mo4->goals_.clear() ;
Dart dStart = mo4->parts_[0]->d;
Dart dStop = dStart ;
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())
dStop = envMap_.map.begin() ;
}
addPathToObstacle(mo4, dStart, dStop);
addPathToObstacle(mo4, dStop, dStart);
movingObstacles_.push_back(mo4);
}
}
......@@ -432,6 +449,21 @@ void Simulator::setupScenario(unsigned int nbMaxAgent)
swapAgentsGoals() ;
}
void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
{
std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
envMap_.position, dStart,
dGoal,
envMap_.buildingMark) ;
for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
{
VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
mo->goals_.push_back(dest) ;
}
}
#ifndef SPATIAL_HASHING
void Simulator::addPathToCorner()
{
......
......@@ -634,7 +634,7 @@ void SocialAgents::animate()
std::ostringstream oss ;
std::ostringstream tmpNb ;
tmpNb << std::setfill('0') << std::setw(4) << nbIterations ;
oss << "./corridor/corrida" << tmpNb.str() << ".pov" ;
oss << "./corridorTop/torrida" << tmpNb.str() << ".pov" ;
std::string chaine = oss.str() ;
// VEC3 agPos = sim.agents_[0]->meanPos_ ;
// agPos[2] = agPos[1] ;
......@@ -690,8 +690,12 @@ void SocialAgents::animate()
// VEC3 camLook(200, 0, 0) ;
//corridor side
VEC3 camPos(-700, 100, 0) ;
VEC3 camLook(200, 0, 0) ;
// VEC3 camPos(-700, 100, 0) ;
// VEC3 camLook(200, 0, 0) ;
//corridor top
VEC3 camPos(0, 300, 0) ;
VEC3 camLook(0, 0, 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);
......@@ -1086,7 +1090,7 @@ bool SocialAgents::exportScenePov(PFP::MAP& map, PFP::TVEC3& position, const std
for (std::vector<MovingObstacle*>::iterator it = sim.movingObstacles_.begin() ; it != sim.movingObstacles_.end() ; ++it)
{
float bottom = 0.0f;
float height = 15.0f;
float height = 10.0f;
unsigned int nbPoints = (*it)->nbVertices;
out << "prism {" << std::endl;
......@@ -1094,7 +1098,7 @@ bool SocialAgents::exportScenePov(PFP::MAP& map, PFP::TVEC3& position, const std
out << "linear_spline" << std::endl;
out << bottom << std::endl; // sweep the following shape from here ...
out << bottom+height << std::endl; // ... up through here
out << nbPoints << std::endl; // the number of points making up the shape ...
out << nbPoints+1 << std::endl; // the number of points making up the shape ...
for(unsigned int i=0; i <= nbPoints ; ++i)
{
......
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