Commit c8122245 authored by pitiot's avatar pitiot
Browse files

detection obstacles + rotation mexicain

parent 2e9afbf1
......@@ -69,7 +69,7 @@ public:
std::vector<std::pair<float, Obstacle*> > movingObstacleNeighbors_;
#ifdef EXPORTING_AGENT
static const int m_ghost_nb = 500;
static const unsigned int m_ghost_nb = 500;
std::list<VEC3> m_ghost_previousPos;
Utils::VBO * m_ghost_VBO;
Utils::ShaderSimpleColor * m_ghost_shader;
......
......@@ -125,8 +125,10 @@ public:
MovingMesh* mm_;
Agent* ag_;
int index_parent;
float gravity_dist; /// distance entre le centre du MO et son sommet le plus éloigné
VertexAttribute<NoMathIONameAttribute<std::vector<PFP::REAL> > > mvc_;
std::vector<std::pair<float, Obstacle*> > obstacleNeighbors_ ;
std::vector<std::pair<float, Obstacle*> > movingObstacleNeighbors_;
};
#endif
......@@ -496,7 +496,7 @@ void Agent::update()
int sign = axisRot[2] > 0 ? 1 : -1 ;
//57,2957795 : conversion from radian to degree
float myRot = acos(-dir[1]);
float myRot =sign * acos(-dir[1]);
const VEC3 displ = getPosition();
// TraversorV<PFP2::MAP> tV(map);
......@@ -887,7 +887,7 @@ void Agent::computeNewVelocity()
double ag_stiffness = 200.0; // agent-agent interaction stiffness
double ag_damping = 1.0; // agent-agent interaction damping
double ag_power = 1; // the power to which elevate the agent-agent distance
// double ag_power = 1; // the power to which elevate the agent-agent distance
rand = 2.0*drand48()-1.0; // compris entre -1 et 1
double radius_var = 0.5;
......
......@@ -17,25 +17,25 @@ float MovingObstacle::detectionFixedObst = 50;
bool MovingObstacle::is_inside(VEC3 p)
{
return false;
// VEC3 vec, norm, p1,p2;
//
//
// for (unsigned int i = 0; i < nbVertices; i++)
// {
// p1 = getPosition(i);
// p2 = getPosition((i+1)%nbVertices);
//
// vec = VEC3(p2 - p1);
// norm[0] = vec[1];
// norm[1] = -vec[0];
//
// vec = VEC3(p2 -p);
// if (vec*norm < 0)
// return false;
// }
//
// return true;
// return false;
VEC3 vec, norm, p1,p2;
for (unsigned int i = 0; i < nbVertices; i++)
{
p1 = getPosition(i);
p2 = getPosition((i+1)%nbVertices);
vec = VEC3(p2 - p1);
norm[0] = vec[1];
norm[1] = -vec[0];
vec = VEC3(p2 -p);
if (vec*norm < 0)
return false;
}
return true;
}
float get_angle(VEC3 v1, VEC3 v2) //renvoie l'angle entre [- pi ; Pi] du v2 à v1
......@@ -93,7 +93,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
parent(art),
mm_(NULL),
ag_(NULL),
index_parent(indParent)
index_parent(indParent),
gravity_dist(0)
{
assert(pos.size() > 2);
......@@ -117,12 +118,55 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
vertexAngle = map.addAttribute<float, DART>("vertexAngle") ;
}
for (unsigned int i = 0; i < nbVertices; ++i)
{
Dart d = sim_->envMap_.getBelongingCell(pos[i]);
CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>* part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>(sim_->envMap_.map, d, pos[i], sim_->envMap_.position);
parts_[i] = part;
center += pos[i];
if(i==0)
dDir = d;
}
center /= nbVertices;
front=(pos[0] + pos[1]) / 2;
// M appartient à l'ellipse ssi MF1 + MF2 = sum_dist_foci est une constante
// où F1 et F2 sont les deux foyers.
// length = (pos[0]-pos[1]).norm();
// width = (pos[1]-pos[2]).norm();
// sum_dist_foci_rest = 2*(length + width*(sqrt(2)-0.5));
// if (spinning && parent==NULL) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
// {
//
// angle = get_angle(goals_[curGoal_] - center,front - center);
//// std::cout<<" angle : "<< angle;
//
// for (unsigned int i = 0; i < nbVertices; ++i)
// {
//// std::cout<<" || pos[i] avant : "<< pos [i];
// pos[i]+=rotate(pos[i], center, angle);
//// std::cout<<" || pos[i] APRES : "<< pos [i]<<std::endl;
//
// parts_[i]->move(pos[i]);
//
//
// }
// angle=0;
// front=(pos[0] + pos[1]) / 2;
// }
groundFace = map.newFace(nbVertices);
for (unsigned int i = 0; i < nbVertices; ++i)
{
float distance=(center-pos[i]).norm();
if(distance>gravity_dist)
{
gravity_dist=distance;
}
Dart d = i;
position[d] = pos[i];
deformation[d] = VEC3(0);
......@@ -133,7 +177,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
forces[d] = VEC3(0);
}
center += pos[i];
}
//extrude face to build a cage
......@@ -168,43 +212,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
dDir=dInside;
}
for (unsigned int i = 0; i < nbVertices; ++i)
{
Dart d = sim_->envMap_.getBelongingCell(pos[i]);
CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>* part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>(sim_->envMap_.map, d, pos[i], sim_->envMap_.position);
parts_[i] = part;
if(i==0)
dDir = d;
}
center /= nbVertices;
front=(position[0] + position[1]) / 2;
// M appartient à l'ellipse ssi MF1 + MF2 = sum_dist_foci est une constante
// où F1 et F2 sont les deux foyers.
// length = (pos[0]-pos[1]).norm();
// width = (pos[1]-pos[2]).norm();
// sum_dist_foci_rest = 2*(length + width*(sqrt(2)-0.5));
// if (spinning && parent==NULL) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
// {
//
// angle = get_angle(goals_[curGoal_] - center,front - center);
// Dart d = groundFace;
// for (unsigned int i = 0; i < nbVertices; ++i)
// {
// position[d]+=rotate(position[d], center, angle);
//
// Dart d(i);
// position[map.phi<211>(d)] += rotate(position[map.phi<211>(d)], center, -angle);
//
// parts_[i]->move(position[d]);
// d = map.phi1(d);
//
// }
// angle=0;
// front=(position[1] + position[2]) / 2;
// }
for (unsigned int i = 0; i < nbVertices; ++i)
{
......@@ -248,7 +256,7 @@ void MovingObstacle::draw()
// m_render->initPrimitives<PFP>(map, Algo::Render::GL2::LINES,false) ;
// m_render->initPrimitives<PFP>(map, Algo::Render::GL2::TRIANGLES,false) ;
m_positionVBO->updateData(position);
m_shader->setColor(Geom::Vec4f(1.,0.,0.,0.));
m_shader->setColor(Geom::Vec4f(movingObstacleNeighbors_.size()>1 ? 1.0f : 0,0.,0.,0.));
m_render->draw(m_shader, Algo::Render::GL2::TRIANGLES);
m_shader->setColor(Geom::Vec4f(0.,0.,0.,0.));
m_render->draw(m_shader, Algo::Render::GL2::LINES);
......@@ -258,8 +266,8 @@ void MovingObstacle::draw()
VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{
Dart d(ind); //WARNING : works only for one face created at start !
// return position[d]+deformation[d];
return position[d];
return position[d]+deformation[d];
// return position[d];
}
VEC3 MovingObstacle::getPosition(unsigned int ind)
......@@ -451,49 +459,100 @@ void MovingObstacle::updateAgentNeighbors() // agents voisins avec distance au b
void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance par rapport aux centres des segments// a mettre en place si besoin
{
// obstacleNeighbors_.clear() ;
// movingObstacleNeighbors_.clear() ;
//
// std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[part_.d] ;
// std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[part_.d] ;
// for(std::vector<Obstacle*>::const_iterator it = obst.begin() ; it != obst.end() ; ++it)
// {
// float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
// if (distSq < rangeSq_)
// {
// if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
// {
// if ((*it)->mo==NULL)
// obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
// else
// {
// movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
// }
// }
//
// }
// }
// for(std::vector<Obstacle*>::const_iterator it = neighborObst.begin() ; it != neighborObst.end() ; ++it)
// {
// float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
// if(distSq < rangeSq_)
// {
// if(Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
// {
// if ((*it)->mo==NULL)
// obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
// else
// movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
// }
// }
// }
obstacleNeighbors_.clear() ;
movingObstacleNeighbors_.clear() ;
for (std::set<Dart>::iterator it2 = general_belonging.begin();it2 != general_belonging.end(); ++it2)
{
std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[*it2] ;
std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[*it2] ;
// float maxDistObst = 0.0f ;
// float maxDistMovingObst = 0.0f ;
float distance_detection=2.5* gravity_dist;
float distance_detectionSq=distance_detection*distance_detection;
for(std::vector<Obstacle*>::const_iterator it = obst.begin() ; it != obst.end() ; ++it)
{
if ((*it)->mo==NULL)
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, center) ;
if (/*(obstacleNeighbors_.size() < maxMovingObstacles_|| distSq < maxDistObst)
&&*/ distSq < distance_detectionSq)
{
// if (distSq > maxDistObst)
// maxDistObst = distSq ;
obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
}
else
{
if((*it)->mo->index!=index)
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, center) ;
if (/*(movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst) &&*/
distSq < distance_detectionSq)
{
// if (distSq > maxDistMovingObst)
// maxDistMovingObst = distSq ;
movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
}
}
}
for(std::vector<Obstacle*>::const_iterator it = neighborObst.begin() ; it != neighborObst.end() ; ++it)
{
if ((*it)->mo==NULL)
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, center) ;
if (/*(obstacleNeighbors_.size() < maxMovingObstacles_|| distSq < maxDistObst)
&&*/ distSq < distance_detectionSq)
{
// if (distSq > maxDistObst)
// maxDistObst = distSq ;
obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
}
else
{
if((*it)->mo->index!=index)
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, center) ;
if (/*(movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst) &&*/
distSq < distance_detectionSq)
{
// if (distSq > maxDistMovingObst)
// maxDistMovingObst = distSq ;
movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
}
}
}
}
}
void MovingObstacle::update()
{
assert(sim_->envMap_.map.getCurrentLevel() == sim_->envMap_.map.getMaxLevel()) ;
///////remise a zero
if(sim_->detect_agent_collision)
// if(sim_->detect_agent_collision)
general_belonging.clear();
//pour les tests de détection///////////////
if(!seen)
......@@ -686,13 +745,13 @@ void MovingObstacle::update()
}
if(sim_->detect_agent_collision)
{
// if(sim_->detect_agent_collision)
// {
for (std::vector<Dart>::iterator it = belonging_cells[i].begin(); it != belonging_cells[i].end(); ++it)
{
general_belonging.insert(*it);
}
}
// }
// CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl;
}
......
......@@ -111,6 +111,7 @@ void Simulator::doStep()
movingObstacles_[i]->computePrefVelocity() ;
movingObstacles_[i]->computeNewVelocity() ;
movingObstacles_[i]->updateAgentNeighbors();
movingObstacles_[i]->updateObstacleNeighbors();
#ifndef SPATIAL_HASHING
envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
......@@ -289,10 +290,10 @@ void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObsta
vPos.push_back(start-xSide+ySide);
vPos.push_back(start-xSide-ySide);
curGoal = ((int)(i*float(nbGoals)/float(nbObstacles/2.0f))+1)%nbGoals;
// mo4= new MovingObstacle(this, i,vPos,goals, true, true,curGoal);
mo4= new MovingObstacle(this, i,vPos,goals, true, true,curGoal);
mo4= new MovingObstacle(this, i,vPos,goals, true, true);
mo4->curGoal_ = i*float(nbGoals)/float(nbObstacles/2.0f);
// mo4= new MovingObstacle(this, i,vPos,goals, true, true);
// mo4->curGoal_ = i*float(nbGoals)/float(nbObstacles/2.0f);
movingObstacles_.push_back(mo4);
}
......@@ -319,11 +320,11 @@ void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObsta
vPos.push_back(start+xSide+ySide);
vPos.push_back(start-xSide+ySide);
vPos.push_back(start-xSide-ySide);
curGoal = ((int)(i*float(nbGoals)/float(nbObstacles/2.0f))+1)%nbGoals;
// mo4= new MovingObstacle(this, i,vPos,goals, true, true,curGoal);
curGoal = ((int)(i*float(nbGoals)/float(nbObstacles/2.0f))+5)%nbGoals;
mo4= new MovingObstacle(this, i,vPos,goals, true, true,curGoal);
mo4= new MovingObstacle(this, i,vPos,goals, true, true);
mo4->curGoal_ = (i-nbObstacles/2)*float(nbGoals)/float(nbObstacles/2.0f);
// mo4= new MovingObstacle(this, i,vPos,goals, true, true);
// mo4->curGoal_ = (i-nbObstacles/2)*float(nbGoals)/float(nbObstacles/2.0f);
movingObstacles_.push_back(mo4);
}
......
......@@ -30,6 +30,9 @@
SocialAgents::SocialAgents(unsigned int config, unsigned int minSize, unsigned int nbAgent, unsigned int nbObst,bool resolution, unsigned int iterations) :
#ifdef EXPORTING_OBJ
m_objAgent(mapAgent),
#endif
m_renderStyle(0),
nbIterations(0),
maxIterations(iterations),
......@@ -37,9 +40,6 @@ SocialAgents::SocialAgents(unsigned int config, unsigned int minSize, unsigned i
frames(0),
nextUpdate(0),
simulator(config, minSize, nbAgent, nbObst, resolution),
#ifdef EXPORTING_OBJ
m_objAgent(mapAgent),
#endif
render_anim(false),
drawEnvLines(false),
drawEnvFaces(true),
......
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