Commit e69d1be0 authored by pitiot's avatar pitiot
Browse files

correction particules jund

parent 70a6de52
......@@ -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() ;
......
......@@ -84,6 +84,13 @@
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="check_drawObstPredictionTri">
<property name="text">
<string>draw obstacles prediction tri</string>
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="check_drawAgentsNeighborDist">
<property name="text">
......
......@@ -95,6 +95,7 @@ public:
bool drawAgentsNeighborDist ;
bool drawAgentsObstacleDist ;
bool drawAgentsPredictionTri ;
bool drawObstPredictionTri ;
bool draw_dart;
unsigned int dartSlider;
......@@ -135,6 +136,11 @@ public slots:
drawAgentsPredictionTri = b ;
updateGL() ;
}
void slot_drawObstPredictionTri(bool b)
{
drawObstPredictionTri = b ;
updateGL() ;
}
void slot_drawAgentsNeighborDist(bool b)
{
drawAgentsNeighborDist = b ;
......
......@@ -215,43 +215,43 @@ void Agent::update()
void Agent::obstacle_priority(PFP::VEC3 * goalVector)
{
float factor ;
Obstacle* obst ;
PFP::VEC3 x,y,vec ;
PFP::VEC3 norm,sumNorm ;
sumNorm.zero() ;
for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
it != movingObstacleNeighbors_.end() ;
++it)
Obstacle* obst ;
PFP::VEC3 x,y,vec ;
PFP::VEC3 norm,sumNorm ;
sumNorm.zero() ;
for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
it != movingObstacleNeighbors_.end() ;
++it)
{
factor =(1.0f-((*it).first/rangeSq_)) ;
// CGoGNout<<factor<<" par rapport à "<<(*it).first<< " ayant un range de "<< rangeSq_<<CGoGNendl;
obst=(*it).second ;
x=obst->p1 ;
y=obst->p2 ;
vec=y-x ;
vec.normalize() ;
norm.zero() ;
if (sim_->avoidance==0)// avoids with normal of obstacle side
{
factor =(1.0f-((*it).first/rangeSq_)) ;
// CGoGNout<<factor<<" par rapport à "<<(*it).first<< " ayant un range de "<< rangeSq_<<CGoGNendl;
obst=(*it).second ;
x=obst->p1 ;
y=obst->p2 ;
vec=y-x ;
vec.normalize() ;
norm.zero() ;
if (sim_->avoidance==0)// avoids with normal of obstacle side
{
norm[0]=vec[1] ;
norm[1]=-vec[0] ;
}
else if (sim_->avoidance==1) // avoids with direction from center of the obstacle
{
MovingObstacle * mo = obst->mo;
norm = this->part_.getPosition()-mo->center;
}
// else if (sim_->avoidance==2) // avoids with opposite direction of obstacle
// {
// MovingObstacle * mo = obst->mo;
// norm = this->part_.getPosition()-mo->center;
// }
norm.normalize() ;
norm*=5.0f*factor*factor*factor*factor*factor*factor*factor*factor ;
sumNorm+=norm ;
norm[0]=vec[1] ;
norm[1]=-vec[0] ;
}
else if (sim_->avoidance==1) // avoids with direction from center of the obstacle
{
MovingObstacle * mo = obst->mo;
norm = this->part_.getPosition()-mo->center;
}
// else if (sim_->avoidance==2) // avoids with opposite direction of obstacle
// {
// MovingObstacle * mo = obst->mo;
// norm = this->part_.getPosition()-mo->center;
// }
norm.normalize() ;
norm*=5.0f*factor*factor*factor*factor*factor*factor*factor*factor ;
sumNorm+=norm ;
}
// if (sumNorm.norm2()>1.0f) sumNorm.normalize();
// if(sumNorm!=0)CGoGNout<<sumNorm<<"goal vector before :" <<(*goalVector)<< " goal vector after : "<<((*goalVector)+sumNorm)<<CGoGNendl;
......
......@@ -610,8 +610,8 @@ 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
// // 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()) ;
//
......@@ -761,10 +761,6 @@ void EnvMap::updateMap()
// popAgentInCells(*ait, centerFace) ;
// map.setCurrentLevel(fLevel - 1) ;
// }
// for (PFP::OBSTACLEVECT::iterator ait = obst.begin(); ait != obst.end(); ++ait)
// {
// removeElementFromVector<Obstacle*>(neighborObst, (*ait)) ;
// }
// neighborAgentvect[old].clear() ;
// // TODO Check with optimisation
// map.setCurrentLevel(map.getMaxLevel()) ;
......
......@@ -73,7 +73,6 @@ 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 +120,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,37 +195,55 @@ 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;
if(co==1)
finalGoal[0]*=-1;
else
finalGoal[1]*=-1;
VEC3 goalVector = goals_[curGoal_] - center ;
float goalDist2 = goalVector.norm2() ;
goalVector = finalGoal - center;
angle =get_angle(finalGoal-center,(parts_[0]->getPosition())-center);
make_half_turn=angle*nbVertices;
if (goalDist2 < 2.0f)
{
curGoal_ = (curGoal_ + 1) % goals_.size() ;
goalVector = goals_[curGoal_] - center ;
goalDist2 = goalVector.norm2() ;
}
if (goalDist2 > 1.0f)
goalVector.normalize();
if (goalDist2 > maxSpeed_)
{
goalVector.normalize() ;
goalVector *= maxSpeed_;
}
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;
}
// TODO Check position
void MovingObstacle::update()
{
assert(sim_->envMap_.map.getCurrentLevel() == sim_->envMap_.map.getMaxLevel()) ;
// general_belonging.clear();
PFP::VEC3 bary = 0;
......@@ -501,12 +522,13 @@ void resetObstPartInFace(Obstacle* o, Dart d1)
}
}
void resetPart(MovingObstacle * mo,int index, Dart d1)
{
int i = index;
if (mo->parts_[i]->d == mo->sim_->envMap_.map.phi1(d1))
mo->parts_[i]->d = d1;
if (mo->parts_[i]->d == mo->sim_->envMap_.map.phi1(d1))
mo->parts_[i]->d = d1;
}
......
......@@ -82,7 +82,7 @@ void Simulator::doStep()
{
#ifndef SPATIAL_HASHING
envMap_.clearUpdateCandidates() ;
envMap_.map.setCurrentLevel(0) ;
envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
......@@ -309,6 +309,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);
}
}
......@@ -437,6 +454,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()
{
......
......@@ -42,6 +42,7 @@ SocialAgents::SocialAgents(int minSize, int iterations) :
drawAgentsNeighborDist(false),
drawAgentsObstacleDist(false),
drawAgentsPredictionTri(false),
drawObstPredictionTri(false),
draw_dart(false)
{
timer = new QTimer(this) ;
......@@ -70,6 +71,7 @@ void SocialAgents::initGUI()
setCallBack(dock.check_drawMovingObstacles, SIGNAL(toggled(bool)), SLOT(slot_drawMovingObstacles(bool))) ;
setCallBack(dock.check_drawAgents, SIGNAL(toggled(bool)), SLOT(slot_drawAgents(bool))) ;
setCallBack(dock.check_drawAgentsPredictionTri, SIGNAL(toggled(bool)), SLOT(slot_drawAgentsPredictionTri(bool))) ;
setCallBack(dock.check_drawObstPredictionTri, SIGNAL(toggled(bool)), SLOT(slot_drawObstPredictionTri(bool))) ;
setCallBack(dock.check_drawAgentsNeighborDist, SIGNAL(toggled(bool)), SLOT(slot_drawAgentsNeighborDist(bool))) ;
setCallBack(dock.check_drawAgentsObstacleDist, SIGNAL(toggled(bool)), SLOT(slot_drawAgentsObstacleDist(bool))) ;
setCallBack(dock.check_dart, SIGNAL(toggled(bool)), SLOT(slot_dart(bool))) ;
......@@ -276,23 +278,61 @@ void SocialAgents::cb_redraw()
// glLineWidth(1.0f);
}
// affiche les cases de la map qui ont des obstacles
glColor3f(1.0f, 0.5f, 0.5f);
glLineWidth(5.0f);
int nb =0;
for(Dart d = sim.envMap_.map.begin(); d != sim.envMap_.map.end(); sim.envMap_.map.next(d))
{
for(std::vector<Obstacle*>::const_iterator it =sim.envMap_.obstvect[d].begin();it!=sim.envMap_.obstvect[d].end();++it)
{
if((*it)->mo!=NULL){
renderDart(sim.envMap_, d);
// CGoGNout << "Dart "<< d.index << " coloriée" << CGoGNendl;
nb ++;
break;
}
}
}
// // affiche les cases de la map qui ont des obstacles
// glColor3f(1.0f, 0.5f, 0.5f);
// glLineWidth(5.0f);
// int nb =0;
// for(Dart d = sim.envMap_.map.begin(); d != sim.envMap_.map.end(); sim.envMap_.map.next(d))
// {
// for(std::vector<Obstacle*>::const_iterator it =sim.envMap_.obstvect[d].begin();it!=sim.envMap_.obstvect[d].end();++it)
// {
// if((*it)->mo!=NULL){
//
// renderDart(sim.envMap_, d);
//// CGoGNout << "Dart "<< d.index << " coloriée" << CGoGNendl;
// nb ++;
// break;
// }
// }
// }
CellMarker<FACE> m(sim.envMap_.map) ;
for (Dart d = sim.envMap_.map.begin(); d != sim.envMap_.map.end(); sim.envMap_.map.next(d))
{
if (!m.isMarked(d) && !sim.envMap_.buildingMark.isMarked(d))
{
m.mark(d) ;
Dart dd = d ;
VEC3 cornerLeftBottom = sim.envMap_.position[d] ;
do
{
VEC3 p = sim.envMap_.position[dd] ;
if (cornerLeftBottom[0] > p[0]) cornerLeftBottom[0] = p[0] ;
if (cornerLeftBottom[1] > p[1]) cornerLeftBottom[1] = p[1] ;
if (cornerLeftBottom[2] > p[2]) cornerLeftBottom[2] = p[2] ;
dd = sim.envMap_.map.phi1(dd) ;
} while (dd != d) ;
//draw vectors
//contains
for (unsigned int i = 0; i < sim.envMap_.obstvect[d].size(); ++i)
{
Obstacle* ag = sim.envMap_.obstvect[d][i] ;
if (ag->mo!=NULL)
{
VEC3 col = Utils::color_map_BCGYR(float(ag->mo->index) / float(sim.movingObstacles_.size())) ;
glColor3fv(col.data()) ;
glBegin(GL_POINTS) ;
if (i < 5)
glVertex3f(cornerLeftBottom[0] + 5 + 1 + i * 2, cornerLeftBottom[1] + 7 + 1.5, cornerLeftBottom[2] + 2) ;
else
glVertex3f(cornerLeftBottom[0] + 5 + 1 + (i - 5) * 2, cornerLeftBottom[1] + 7 + 1.5 + 3, cornerLeftBottom[2] + 2) ;
glEnd() ;
}
}
}
}
}
......@@ -306,6 +346,25 @@ void SocialAgents::cb_redraw()
renderPredictionTriangle(sim.envMap_, (*it)->part_.d, (*it)->getPosition()) ;
}
}
if (drawObstPredictionTri)
{
for (std::vector<MovingObstacle*>::iterator it = sim.movingObstacles_.begin() ; it != sim.movingObstacles_.end() ; ++it)
{
for(unsigned int i = 0 ; i < (*it)->nbVertices ; ++i)
{
CGoGN::Algo::MovingObjects::ParticleCell2D<PFP> p = *(*it)->parts_[i];
glColor3f(p.getState() / 3.0f, p.getState() % 2, p.getState()) ;
glLineWidth(1.0f) ;
renderPredictionTriangle(sim.envMap_, p.d, p.getPosition()) ;
if(p.getState()==VERTEX)
glBegin(GL_POINTS);
glVertex3fv(p.getPosition().data());
glEnd();
}
}
}
#endif
//draw the environment
......@@ -1086,7 +1145,8 @@ 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 +1154,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