Création d'un compte pour un collaborateur extérieur au laboratoire depuis l'intranet ICube : https://intranet.icube.unistra.fr/fr/labs/member/profile

Commit 298506ab authored by pitiot's avatar pitiot
Browse files

circle scenario +spinning obstacles

parent 84006d76
......@@ -17,7 +17,7 @@ class Simulator ;
class MovingObstacle
{
public:
MovingObstacle(Simulator* sim, int index, std::vector<PFP::VEC3> pos, std::vector<VEC3> goals,float rota);
MovingObstacle(Simulator* sim, int index, std::vector<PFP::VEC3> pos, std::vector<VEC3> goals,bool spin);
bool test_opposition(VEC3 o, VEC3 p1, VEC3 p2);
// void contournerBatiment();
void updateAgentNeighbors() ;
......@@ -56,9 +56,8 @@ public:
// std::set<std::pair<float, Obstacle*> > obstacleNeighbors_ ;
// std::set<std::pair<float, Obstacle*> > movingObstNeighbors_;
float rot;
VEC3 finalGoal;
float make_half_turn;
float angle;
// Dart finalDart;
std::vector<VEC3> goals_ ;
......@@ -77,6 +76,7 @@ public:
VEC3 newVelocity_;
VEC3 prefVelocity_;
Simulator* sim_;
bool spinning;
};
#endif
......@@ -33,7 +33,7 @@ bool MovingObstacle::is_inside(VEC3 p)
return true;
}
float get_angle(VEC3 v1, VEC3 v2)
float get_angle(VEC3 v1, VEC3 v2) //renvoie l'angle entre [- pi ; Pi] du v2 à v1
{
float flo = 0.0f;
float nb = std::sqrt(v1.norm2() * v2.norm2());
......@@ -50,11 +50,17 @@ float get_angle(VEC3 v1, VEC3 v2)
}
if ((v1[0] * v2[1] - v2[0] * v1[1]) > 0)
flo = 2.0f * M_PI - flo;
flo = - flo;
if(flo >M_PI)
flo= flo-2*M_PI;
if (flo<(-M_PI))
flo=flo+2*M_PI;
return flo;
}
VEC3 rotate(VEC3 pos1, VEC3 center, float angle)
VEC3 rotate(VEC3 pos1, VEC3 center, float angle) // renvoie le déplacement necessaire depuis pos1 pour effectuer la rotation centrée en center d'angle angle
{
VEC3 pos2;
PFP::REAL x = pos1[0] - center[0];
......@@ -64,7 +70,7 @@ VEC3 rotate(VEC3 pos1, VEC3 center, float angle)
return pos2;
}
MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, float rota) :
MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool spin) :
index(ind),
newVelocity_(0),
sim_(sim)
......@@ -73,7 +79,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
// movingObstNeighbors_.reserve(maxNeighbors_);
rot = rota;
goals_=goals;
curGoal_ = 1;
VEC3 sum = 0;
nb_agents_voisins = 0;
nb_register_cells = 0;
......@@ -91,10 +98,24 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
for (unsigned int i = 0; i < nbVertices; ++i)
{
vertices[i]=pos [i];
sum += vertices[i];
}
center = sum / nbVertices;
front=(vertices[1] + vertices[2]) / 2;
if (spinning) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
{
angle = get_angle(goals_[curGoal_] - center,front - center);
for (unsigned int i = 0; i < nbVertices; ++i)
{
vertices[i]+=rotate(vertices[i], center, angle);
}
angle=0;
front=(vertices[1] + vertices[2]) / 2;
}
for (unsigned int i = 0; i < nbVertices; ++i)
{
sum += vertices[i];
Obstacle* o = new Obstacle(vertices[i],
vertices[(i + 1) % nbVertices],
vertices[(i - 1 + nbVertices) % nbVertices],
......@@ -105,7 +126,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
}
center = sum / nbVertices;
for (unsigned int i = 0; i < nbVertices; ++i)
{
float it = (center - pos[i]).norm();
......@@ -115,12 +136,11 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
gravity_dist = pos_max + 5.0f;
obstacle_range = 15.0f * 15.0f;
front=(vertices[0] + vertices[1]) / 2;
make_half_turn = get_angle(finalGoal - center,front - center) * nbVertices;
goals_=goals;
curGoal_ = 0;
}
bool MovingObstacle::test_opposition(VEC3 o, VEC3 p1, VEC3 p2)
......@@ -306,37 +326,33 @@ void MovingObstacle::update()
velocity_[1] = newVelocity_[1] * velocity_factor;
// MAJ des particules
make_half_turn = 0;
float rotor = make_half_turn > 0.01f * nbVertices ? 0.01f : make_half_turn / nbVertices;
// CGoGNout << "début ++ update "<< CGoGNendl;
float abs_angle= angle > 0 ? 1 : -1;
float rotor = abs_angle*angle > 0.01f ? 0.01f : abs_angle*angle ;
// CGoGNout << "Obstacle "<< index << CGoGNendl;
// CGoGNout << "vitesse : "<< velocity_ << CGoGNendl;
// on fait tourner l'obstacle
for (unsigned int i = 0; i < nbVertices; ++i)
{
// CGoGNout << "update particule : "<< i << CGoGNendl;
VEC3 target =vertices[i];
// Dart oldFace = parts_[i]->d;
if (make_half_turn == 0)
{
if (rot != 0)
{
target += rotate(vertices[i], center, rot);
}
target += (velocity_ * sim_->timeStep_);
VEC3 target =vertices[i];
}
else
{
target += rotate(vertices[i], center, rotor);
}
target += rotate(vertices[i], center, abs_angle*rotor);
target += (velocity_ * sim_->timeStep_);
vertices[i]=target;
bary += target;
}
make_half_turn -= rotor * nbVertices;
front=(vertices[1] + vertices[2]) / 2;
if(angle >0)
{
angle -= rotor;
}
else
{
angle += rotor;
}
if (make_half_turn == 0)
center = bary / nbVertices;
center = bary / nbVertices;
// MAJ des obstacles
for (unsigned int i = 0; i < nbVertices; ++i)
......@@ -587,12 +603,12 @@ void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atte
{
VEC3 goalVector = goals_[curGoal_] - center ;
float goalDist2 = goalVector.norm2() ;
if (goalDist2 < 2.0f)
{
curGoal_ = (curGoal_ + 1) % goals_.size() ;
goalVector = goals_[curGoal_] - center ;
goalDist2 = goalVector.norm2() ;
}
if (goalDist2 > maxSpeed_)
......@@ -600,12 +616,26 @@ void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atte
goalVector.normalize() ;
goalVector *= maxSpeed_;
}
if (spinning) angle =get_angle(goalVector,front-center);
prefVelocity_ = goalVector ;
}
void MovingObstacle::computeNewVelocity() //comportement des obstacles en tenant compte de l'environnement.
{
newVelocity_=prefVelocity_;
if (spinning)
{
VEC3 forward = front-center;
float goalDist2 = forward.norm2() ;
float objective = prefVelocity_.norm2();
if (goalDist2 > objective)
{
forward.normalize() ;
forward *= objective;
}
newVelocity_=forward;
}
else
{
newVelocity_=prefVelocity_;
}
}
......@@ -18,7 +18,7 @@ Simulator::Simulator(int minS) :
detect_agent_collision=false;
srand(10) ;
nbStepsPerUnit_ = 1 / timeStep_ ;
config=0;
config=1;
init( minSize, 2.0f) ;
}
......@@ -238,12 +238,20 @@ void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObsta
std::vector<VEC3> vPos;
MovingObstacle* mo4;
std::vector<VEC3> positions;
for (unsigned int i = 0 ; i < nbObstacles/2 ; i++)
for (unsigned int i = 0 ; i < nbObstacles ; i++)
{
double angle = i * 2.0f * pi / float(nbObstacles/2) ;
VEC3 v(std::cos(angle) * (2*radius/3), std::sin(angle) * (2*radius/3), 0) ;
VEC3 start = center + v ;
positions.push_back(start);
double angle = i * 2.0f * pi / float(nbObstacles/2) ;
VEC3 v,start;
if(i<nbObstacles/2)
{
v=VEC3 (std::cos(angle) * (2*radius/3), std::sin(angle) * (2*radius/3), 0) ;
}
else
{
v=VEC3 (std::cos(angle) * (radius/3), std::sin(angle) * (radius/3), 0) ;
}
start = center + v ;
positions.push_back(start);
}
for (unsigned int i = 0 ; i < nbObstacles/2 ; i++)
{
......@@ -261,27 +269,31 @@ void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObsta
vPos.push_back(start-xSide+ySide);
vPos.push_back(start-xSide-ySide);
mo4= new MovingObstacle(this, i,vPos,goals,0);
mo4= new MovingObstacle(this, i,vPos,goals,true);
movingObstacles_.push_back(mo4);
}
//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);
for (unsigned int i = nbObstacles/2 ; i < nbObstacles ; i++)
{
VEC3 start = positions[i];
std::vector<VEC3> goals;
for(unsigned int k = (nbObstacles/2) ; k > 0 ; k--)
{
goals.push_back(positions[((i-(nbObstacles/2)+k)%(nbObstacles/2))+nbObstacles/2]);
}
vPos.clear();
// Un obstacle sur deux va vers le haut
vPos.push_back(start+xSide-ySide);
vPos.push_back(start+xSide+ySide);
vPos.push_back(start-xSide+ySide);
vPos.push_back(start-xSide-ySide);
mo4= new MovingObstacle(this, i,vPos,goals,true);
movingObstacles_.push_back(mo4);
}
#ifndef SPATIAL_HASHING
envMap_.clearUpdateCandidates() ;
envMap_.map.setCurrentLevel(0) ;
......@@ -386,7 +398,7 @@ void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObst
std::vector<VEC3> goals;
goals.push_back(start);
goals.push_back(goal);
mo4= new MovingObstacle(this, i,vPos,goals,0);
mo4= new MovingObstacle(this, i,vPos,goals,false);
//for generating a random path
......
......@@ -249,33 +249,25 @@ void SocialAgents::cb_redraw()
glVertex3fv(p.data()) ;
}
glEnd() ;
//display collision avoidance circle
// glColor3f(1.0f, 0.8f, 0.0f);
// VEC3 pos =mo->center;
// float radius = mo->gravity_dist;
// glBegin(GL_POLYGON);
// for(unsigned int i = 0; i < 5; ++i)
// glVertex3f(pos[0] + (cosT[i] * radius), pos[1] + (sinT[i] * radius), pos[2]+0.01f);
// glEnd();
//
//////
// for(unsigned int i = 0; i < ((*it)->nbVertices); ++i)
// {
//
//
//
// glColor3f(1.0f, 0.0f, 0.0f);
// glLineWidth(5.0f);
// std::list<Dart> cells=(*it)->neighbor_cells[i];
// for (std::list<Dart>::iterator it2 = cells.begin(); it2 != cells.end(); ++ it2)
// {
// renderFace(sim.envMap_, (*it2).index);
// }
//
//
// }
//////Affiche l'objectif actuel
// glColor3f(1.0f, 0.0f, 0.0f) ;
// glLineWidth(3.0f) ;
// glBegin(GL_LINES);
// const VEC3& p = (*it)->front ;
// glVertex3fv(p.data());
// const VEC3& p2 = (*it)->goals_[(*it)->curGoal_] ;
// glVertex3fv(p2.data());
// glEnd();
//////Affiche la direction
// glColor3f(0.0f, 1.0f, 1.0f) ;
// glLineWidth(3.0f) ;
// glBegin(GL_LINES);
// const VEC3& p3 = (*it)->center ;
// glVertex3fv(p3.data());
// const VEC3& p4 = (*it)->front ;
// glVertex3fv(p4.data());
// glEnd();
//affiche les cases ou se sont enregistrés les obstacles
//
// glColor3f(1.0f, 0.5f, 0.5f);
......
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