Commit a89ac220 authored by Arash HABIBI's avatar Arash HABIBI
Browse files

Voici donc un commentaire :

parent d380cc70
......@@ -57,7 +57,11 @@ public:
unsigned int curGoal_ ;
MovingObstacle **movingObstacles_;
int nb_mos;
static unsigned int maxNeighbors_ ;
static unsigned int maxMovingObstacles_;
static float maxSpeed_ ;
static float neighborDist_ ;
static float neighborDistSq_ ;
......
......@@ -44,6 +44,8 @@ public:
std::vector<Dart> * neighbor_cells;
std::set<Dart> general_belonging;
VEC3 front;
VEC3 focus1, focus2;
double length, width, sum_dist_foci, sum_dist_foci_rest;
VEC3 finalGoal;
float angle;
......
......@@ -10,13 +10,15 @@
#include "simulator.h"
unsigned int Agent::maxNeighbors_ = 10 ;
unsigned int Agent::maxMovingObstacles_ = 10;
float Agent::maxSpeed_ = 2.0f ;
float Agent::neighborDist_ = 10.0f ;
float Agent::neighborDistSq_ = neighborDist_ * neighborDist_ ;
float Agent::radius_ = 1.5f ;
float Agent::timeHorizon_ = 10.0f ;
float Agent::timeHorizonObst_ = 10.0f ;
float Agent::range_ = (timeHorizonObst_ * maxSpeed_ + radius_) ;
float Agent::range_ = 20*timeHorizonObst_ * maxSpeed_ + radius_ ;
// float Agent::range_ = timeHorizonObst_ * maxSpeed_ + radius_ ;
float Agent::rangeSq_ = range_ * range_ ;
unsigned int Agent::cptAgent = 0 ;
......@@ -34,6 +36,8 @@ Agent::Agent(Simulator* sim, const VEC3& position) :
{
agentNeighbors_.reserve(maxNeighbors_* 2) ;
obstacleNeighbors_.reserve(maxNeighbors_* 2) ;
movingObstacles_ = new MovingObstacle* [maxMovingObstacles_];
nb_mos=0;
agentNo = cptAgent++ ;
}
#else
......@@ -53,6 +57,8 @@ Agent::Agent(Simulator* sim, const VEC3& position, Dart d) :
agentNeighbors_.reserve(maxNeighbors_ * 2) ;
obstacleNeighbors_.reserve(maxNeighbors_ * 2) ;
movingObstacleNeighbors_.reserve(maxNeighbors_ * 2) ;
movingObstacles_ = new MovingObstacle* [maxMovingObstacles_];
nb_mos=0;
agentNo = cptAgent++ ;
}
#endif
......@@ -401,22 +407,56 @@ void Agent::computePrefVelocity()
prefVelocity_ = goalVector ;
}
//-----------------------------------------------------------------
static int moAppend(MovingObstacle **moving_obstacles, int nb_mos, int max, MovingObstacle *mo)
{
unsigned char already_processed=false;
int mo_count;
if(nb_mos>=max-1)
{
// cerr << "Agent::moPush : the maximum number of moving obstacles is assumed to be " << max << endl;
// cerr << "There is visibly a need for more." << endl;
}
else
{
for(mo_count=0;
(mo_count<nb_mos)&&(already_processed==false);
mo_count++)
if(moving_obstacles[mo_count] == mo)
already_processed = true;
if(mo_count==nb_mos)
{
moving_obstacles[mo_count] = mo;
nb_mos++;
}
}
return(nb_mos);
}
//-----------------------------------------------------------------
//-----------------------------------------------------------------
//-----------------------------------------------------------------
// Search for the best new velocity.
void Agent::computeNewVelocity() // RVO2 : évitement agents entres eux et avec les obstacles fixes + ajout obstacles mobiles
void Agent::computeNewVelocity()
{
// The objective is to compute the sum of forces exerted on the agent.
double collision_softening_factor;
double ag_mass = 1.0;
float ag_ambient_damping = 1.0;
double mass_var = 0.9;
double average_mass = 1.0;
srand48(agentNo);
double rand = 2.0*drand48()-1.0; // compris entre -1 et 1
// double ag_mass = average_mass + rand*rand*rand*mass_var; // valeurs plus denses autour de la moyenne
double ag_mass = average_mass + rand*mass_var; // valeurs uniformement réparties entre min et max
//-------------
VEC3 p1, p2, vec, forces, previous_pos;
VEC3 vec, forces, previous_pos;
forces.zero();
previous_pos = getPosition() - velocity_*sim_->timeStep_;
......@@ -433,43 +473,48 @@ void Agent::computeNewVelocity() // RVO2 : évitement agents entres eux et avec
//----- forces dues à la répulsion des obstacles ----------
VEC3 norm;
double obst_stiffness = 50.0; // agent-obstacle interaction stiffness
// double obst_damping = 1.0; // agent-obstacle interaction damping
int obst_power = 4; // the power to which elevate the agent-obstacle distance
double obst_stiffness = 5000.0; // agent-obstacle interaction stiffness
// double obst_damping = 1.0; // agent-obstacle interaction damping
int obst_power = 1; // the power to which elevate the agent-obstacle distance
Obstacle* obst ;
nb_mos = 0;
for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
it != movingObstacleNeighbors_.end() ;
++it)
{
double dist = it->first;
// double effective_range = 50*range_;
double effective_range = 10*range_;
float force_value=0.0;
if(dist < effective_range)
obst=it->second;
nb_mos=moAppend(movingObstacles_,nb_mos,maxMovingObstacles_,obst->mo);
}
MovingObstacle *mo;
float force_value;
int mo_count;
for(mo_count=0;
mo_count<nb_mos;
mo_count++)
{
mo = movingObstacles_[mo_count];
float dist = (getPosition()-mo->focus2).norm() + (getPosition()-mo->focus1).norm();
// double effective_range = 3*mo->sum_dist_foci;
double effective_range = mo->sum_dist_foci;
if(dist <= effective_range)
{
collision_softening_factor = pow(1 - dist/effective_range,obst_power);
force_value = obst_stiffness*collision_softening_factor*(effective_range-dist);
}
obst=it->second ;
p1=obst->p1 ;
p2=obst->p2 ;
vec=p2-p1;
vec.normalize();
norm.zero();
if (sim_->avoidance==0)// avoids with normal of obstacle side
{
norm[0]=vec[1] ;
norm[1]=-vec[0] ;
norm.zero();
// norm = (getPosition()-mo->focus2) + (getPosition()-mo->focus1);
// norm.normalize();
VEC3 vec1 = getPosition()-mo->focus1;
vec1.normalize();
VEC3 vec2 = getPosition()-mo->focus2;
vec2.normalize();
norm = vec1 + vec2;
norm.normalize();
forces += force_value * norm;
}
else if (sim_->avoidance==1) // avoids with direction from center of the obstacle
{
MovingObstacle * mo = obst->mo;
norm = this->part_.getPosition()-mo->center;
}
forces += force_value * norm;
}
//----- forces dues à la répulsion des autres agents -------
......@@ -477,7 +522,7 @@ void Agent::computeNewVelocity() // RVO2 : évitement agents entres eux et avec
double ag_stiffness = 20.0; // agent-agent interaction stiffness
double ag_damping = 1.0; // agent-agent interaction damping
double ag_phys_radius_coef = 20.0;
int ag_power = 8; // the power to which elevate the agent-agent distance
double ag_power = 8; // the power to which elevate the agent-agent distance
unsigned int nbA = 0 ;
......@@ -529,6 +574,32 @@ void Agent::computeNewVelocity() // RVO2 : évitement agents entres eux et avec
velocity_tmp *= maxSpeed_;
}
newVelocity_ = velocity_tmp;
//------- color depending on velocity -------------------------------
double vmax = 0.5*maxSpeed_;
VEC3 min_vx_color(1.0,0.0,0.0); // agents going towards the positive xs are red
VEC3 max_vx_color(0.0,0.1,0.1); // agents going towards the negative xs are cyan
VEC3 min_vy_color(0.0,1.0,0.0);
VEC3 max_vy_color(1.0,0.0,1.1);
double alpha_x = (newVelocity_[0]+vmax) / (2*vmax);
double alpha_y = (newVelocity_[1]+vmax) / (2*vmax);
double tmp_color1 =
abs(alpha_x-0.5)*(alpha_x * max_vx_color[0] + (1-alpha_x)*min_vx_color[0]) +
abs(alpha_y-0.5)*(alpha_y * max_vy_color[0] + (1-alpha_y)*min_vy_color[0]);
double tmp_color2 =
abs(alpha_x-0.5)*(alpha_x * max_vx_color[1] + (1-alpha_x)*min_vx_color[1]) +
abs(alpha_y-0.5)*(alpha_y * max_vy_color[1] + (1-alpha_y)*min_vy_color[1]);
double tmp_color3 =
abs(alpha_x-0.5)*(alpha_x * max_vx_color[2] + (1-alpha_x)*min_vx_color[2]) +
abs(alpha_y-0.5)*(alpha_y * max_vy_color[2] + (1-alpha_y)*min_vy_color[2]);
float blend = 0.1;
color1 = blend*tmp_color1 + (1-blend)*color1;
color2 = blend*tmp_color2 + (1-blend)*color2;
color3 = blend*tmp_color3 + (1-blend)*color3;
}
//-----------------------------------------------------------------
......
......@@ -102,6 +102,17 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
}
center = sum / nbVertices;
front=(vertices[1] + vertices[2]) / 2;
//-------- code ajoute par Arash pour les obstacles rectangulaires --------------
length = (vertices[0]-vertices[1]).norm();
width = (vertices[1]-vertices[2]).norm();
sum_dist_foci_rest = 2*(length + width*(sqrt(2)-0.5));
// M appartient à l'ellipse ssi MF1 + MF2 = sum_dist_foci est une constante
// où F1 et F2 sont les deux foyers.
//-------- fin du code ajoute par Arash pour les obstacles rectangulaires -------
if (spinning) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
{
angle = get_angle(goals_[curGoal_] - center,front - center);
......@@ -136,11 +147,6 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
gravity_dist = pos_max + 5.0f;
obstacle_range = 15.0f * 15.0f;
}
bool MovingObstacle::test_opposition(VEC3 o, VEC3 p1, VEC3 p2)
......@@ -345,6 +351,7 @@ void MovingObstacle::update()
bary += target;
}
front=(vertices[1] + vertices[2]) / 2;
if(angle >0)
{
angle -= rotor;
......@@ -356,6 +363,25 @@ void MovingObstacle::update()
center = bary / nbVertices;
//-------- code ajoute par Arash pour les obstacles rectangulaires --------------
VEC3 P0_P1 = vertices[1] - vertices[0];
float velocity_coef = 50.0;
if(P0_P1 * velocity_ > 0) // P0_P1 est dans le sens de la vitesse
{
focus1 = center - P0_P1*(1-(width/length)*(sqrt(2)-0.5));
focus2 = center + P0_P1*(1-(width/length)*(sqrt(2)-0.5)) + (velocity_coef*velocity_);
}
else
{
focus1 = center - P0_P1*(1-(width/length)*(sqrt(2)-0.5)) + (velocity_coef*velocity_);
focus2 = center + P0_P1*(1-(width/length)*(sqrt(2)-0.5));
}
sum_dist_foci = sum_dist_foci_rest + velocity_coef*velocity_.norm();
//-------- fin du code ajoute par Arash pour les obstacles rectangulaires -------
// MAJ des obstacles
for (unsigned int i = 0; i < nbVertices; ++i)
{
......
......@@ -18,8 +18,8 @@ Simulator::Simulator(int minS) :
detect_agent_collision=false;
srand(10) ;
nbStepsPerUnit_ = 1 / timeStep_ ;
// config=0;
config=1;
config=0;
// config=1;
init( minSize, 2.0f) ;
}
......@@ -168,8 +168,9 @@ void Simulator::doStep()
nbRefineCandidate += envMap_.refineCandidate.size() ;
nbCoarsenCandidate += envMap_.coarsenCandidate.size() ;
if (multires)
envMap_.updateMap() ;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>><
// if (multires)
// envMap_.updateMap() ;
#endif
globalTime_ += timeStep_ ;
......
......@@ -93,6 +93,59 @@ void SocialAgents::cb_initGL()
setParamObject(gWidthObj, gPosObj.data()) ;
}
//-----------------------------------
// An ellipse has two foci placed at
// F1 and F2. The ellipse is the set
// of points M for whom MF1 + MF2 = s
static void drawEllipse(VEC3 F1, VEC3 F2, float s, int N)
{
float a,b;
VEC3 O,u,v;
O = 0.5f*(F1+F2);
u = F1-F2;
float c = (F1-F2).norm();
if(c==0)
{
u[0] = 1.0;
u[1] = 0.0;
v[0] = 0.0;
v[1] = 1.0;
a=s/2;
b=s/2;
}
else // c>0
{
u = (1.0f/c) * u;
v.zero();
v[0] = u[1];
v[1] = -u[0];
c = 0.5 * c;
a = s/2.0;
if(a<c)
{
cerr << "a=" << a << " and c=" << c << endl;
cerr << "No such an ellipse is possible" << endl;
}
else // a >= c
b = sqrt( a*a - c*c );
}
glBegin(GL_LINE_LOOP);
VEC3 M;
float dtheta = 2 * M_PI / N;
float theta;
for(theta=0; theta<2*M_PI; theta+=dtheta)
{
M = O + a*cos(theta)*u + b*sin(theta)*v;
glColor3f(0,0,1);
glVertex2f(M[0],M[1]);
}
glEnd();
}
//-----------------------------------
void SocialAgents::cb_redraw()
{
dock.check_slide->setRange(0, sim.envMap_.map.end().index) ;
......@@ -217,6 +270,16 @@ void SocialAgents::cb_redraw()
{
for (std::vector<MovingObstacle*>::iterator it = sim.movingObstacles_.begin() ; it != sim.movingObstacles_.end() ; ++it)
{
// Dessiner les foyers
glBegin(GL_POINTS);
glColor3f(1.0,0.0,0.0);
glVertex3fv((*it)->focus1.data());
glColor3f(0.0,1.0,0.0);
glVertex3fv((*it)->focus2.data());
glEnd();
// Dessiner l'ellipse
drawEllipse((*it)->focus1,(*it)->focus2,(*it)->sum_dist_foci,20);
// MovingObstacle* mo = *it ;
//affiche les sommets des obstacles
// for (unsigned int i =0; i< (*it)->nbVertices;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