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

Merge cgogn:~habibi/CGoGN_Apps/SocialAgents2

parents 8d755aaa 52c29bed
......@@ -10,7 +10,8 @@ ENDIF (WIN32)
include_directories(
${CMAKE_CURRENT_BINARY_DIR}
${CMAKE_CURRENT_SOURCE_DIR}
${CMAKE_CURRENT_SOURCE_DIR}/Debug
# ${CMAKE_CURRENT_SOURCE_DIR}
../include
${CGoGN_ROOT_DIR}/include
${COMMON_INCLUDES}
......
......@@ -10,7 +10,8 @@ ENDIF (WIN32)
include_directories(
${CMAKE_CURRENT_BINARY_DIR}
${CMAKE_CURRENT_SOURCE_DIR}
${CMAKE_CURRENT_SOURCE_DIR}/Release
# ${CMAKE_CURRENT_SOURCE_DIR}
../include
${CGoGN_ROOT_DIR}/include
${COMMON_INCLUDES}
......
......@@ -221,30 +221,30 @@ inline void renderAgent(EnvMap& m, Agent* agent, bool showNeighborDist = false,
//show goals
if (renderPath)
{
// glLineWidth(3.0f) ;
// glBegin(GL_LINE_STRIP) ;
// for (std::vector<VEC3>::iterator it = (agent->goals_.begin()) ; it != agent->goals_.end() ;
// ++it)
// {
// glVertex3f((*it)[0], (*it)[1], agent->agentNo) ;
// }
//
// glVertex3f((*(agent->goals_.begin()))[0], (*(agent->goals_.begin()))[1], (*(agent->goals_.begin()))[2]) ;
// glEnd() ;
// glLineWidth(1.0f) ;
//
// glPointSize(5.0f) ;
// glBegin(GL_POINTS) ;
// VEC3 g = agent->goals_[agent->curGoal_];
// glVertex3fv(g.data()) ;
// glEnd() ;
// glPointSize(1.0f) ;
//
// glBegin(GL_LINES);
// VEC3 p = agent->getPosition();
// glVertex3fv(p.data()) ;
// glVertex3fv(g.data()) ;
// glEnd();
glLineWidth(3.0f) ;
glBegin(GL_LINE_STRIP) ;
for (std::vector<VEC3>::iterator it = (agent->goals_.begin()) ; it != agent->goals_.end() ;
++it)
{
glVertex3f((*it)[0], (*it)[1], agent->agentNo) ;
}
glVertex3f((*(agent->goals_.begin()))[0], (*(agent->goals_.begin()))[1], (*(agent->goals_.begin()))[2]) ;
glEnd() ;
glLineWidth(1.0f) ;
glPointSize(5.0f) ;
glBegin(GL_POINTS) ;
VEC3 g = agent->goals_[agent->curGoal_];
glVertex3fv(g.data()) ;
glEnd() ;
glPointSize(1.0f) ;
glBegin(GL_LINES);
VEC3 p = agent->getPosition();
glVertex3fv(p.data()) ;
glVertex3fv(g.data()) ;
glEnd();
}
if (showNeighborDist)
......
......@@ -78,8 +78,6 @@ 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;
......
......@@ -229,7 +229,7 @@ void Agent::updateObstacleNeighbors()
if ((movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst)
&& distSq < rangeSq_)
{
if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
// if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
{
if (distSq > maxDistMovingObst)
......@@ -267,7 +267,7 @@ void Agent::updateObstacleNeighbors()
if ((movingObstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistMovingObst)
&&distSq < rangeSq_)
{
if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
// if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
{
if (distSq > maxDistMovingObst) maxDistMovingObst = distSq ;
......@@ -329,6 +329,7 @@ void Agent::update()
pos = pos + (velocity_ * sim_->timeStep_) ;
#else
VEC3 target = part_.getPosition() + (velocity_ * sim_->timeStep_) ;
// cerr << agentNo << endl;
part_.move(target) ;
#ifdef SECURED
......@@ -489,7 +490,7 @@ void Agent::computePrefVelocity()
// Si l'agent arrive à proximité de l'objectif,
// alors il passe à l'objectif suivant
if (goalDist2 < radius_*radius_*100.0f)
if (goalDist2 < 100*radius_*radius_)
{
curGoal_ = (curGoal_ + 1) % goals_.size() ;
goalVector = goals_[curGoal_] - getPosition() ;
......@@ -512,7 +513,7 @@ void Agent::computePrefVelocity()
}
//-----------------------------------------------------------------
/*
static int moAppend(MovingObstacle **moving_obstacles, int nb_mos, int max, MovingObstacle *mo)
{
unsigned char already_processed=false;
......@@ -540,7 +541,7 @@ static int moAppend(MovingObstacle **moving_obstacles, int nb_mos, int max, Movi
}
return(nb_mos);
}
*/
//-----------------------------------------------------------------
//-----------------------------------------------------------------
//-----------------------------------------------------------------
......@@ -550,9 +551,11 @@ void Agent::computeNewVelocity()
{
// The objective is to compute the sum of forces exerted on the agent.
double collision_softening_factor;
float ag_ambient_damping = 1.0;
double mass_var = 0.95;
double average_mass = 1.0;
float ag_ambient_damping = 10.0;
// double mass_var = 0.95;
// double average_mass = 1.0;
// double average_radius = 20.f;
// double radius_var = 0.9;
// range_ = 2*timeHorizonObst_ * averageMaxSpeed_ + radius_ ;
......@@ -595,53 +598,51 @@ void Agent::computeNewVelocity()
//----- forces dues à la répulsion des obstacles en mouvement ----------
VEC3 norm;
// double obst_stiffness = 10000.0; // agent-obstacle interaction stiffness
double obst_stiffness = 1000.0; // agent-obstacle interaction stiffness
// double obst_damping = 1.0; // agent-obstacle interaction damping
int obst_power = 2 ; // the power to which elevate the agent-obstacle distance
Obstacle* obst ;
double obst_stiffness = 100.0; // agent-obstacle interaction stiffness
int obst_power = 1 ; // the power to which elevate the agent-obstacle distance
double obst_radius_infl;
if(sim_->config==0)
obst_radius_infl = 100.; // scenario 0
else
obst_radius_infl = 40.; // scenario 1 et 3
float force_value;
int nobst=0;
#define ARASH
#ifdef ARASH
nb_mos = 0;
for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
it != movingObstacleNeighbors_.end() ;
++it)
{
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++)
for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
it != movingObstacleNeighbors_.end() ; ++it)
{
mo = movingObstacles_[mo_count];
float dist = (getPosition()-mo->focus2).norm() + (getPosition()-mo->focus1).norm();
double effective_range = mo->sum_dist_foci;
if(dist <= effective_range)
Obstacle * obst = it->second;
if(obst->mo!=NULL)
{
collision_softening_factor = pow(1 - dist/effective_range,obst_power);
force_value = obst_stiffness*collision_softening_factor*(effective_range-dist);
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;
VEC3 p1=obst->p1 ;
VEC3 p2=obst->p2 ;
double longueur2 = (p1-p2).norm2();
double rest_sum_of_dists = 2 * sqrt(obst_radius_infl*obst_radius_infl + longueur2/4);
VEC3 p = getPosition();
double d1 = (p-p1).norm();
double d2 = (p-p2).norm();
double sum_of_dists = d1+d2;
if(sum_of_dists < rest_sum_of_dists)
{
collision_softening_factor = pow(1-sum_of_dists/rest_sum_of_dists,obst_power);
force_value = obst_stiffness*collision_softening_factor*(rest_sum_of_dists - sum_of_dists);
VEC3 v_obst = p2 - p1;
VEC3 normal = VEC3(v_obst[1],-v_obst[0],0);
// Ajouter une composante tangentielle
normal += v_obst * ((d1-d2)/(5*sum_of_dists));
// Le facteur 5 est là seulement pour diminuer l'influence de la composante tangentielle
normal.normalize();
forces += force_value * normal;
}
}
}
#else
float force_value;
for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
it != movingObstacleNeighbors_.end() ; ++it)
{
......@@ -668,7 +669,7 @@ void Agent::computeNewVelocity()
force_value = obst_stiffness*pow(1/dist,obst_power);
std::cout << "ag " << agentNo << "angle " << a << " force val " << force_value << " " << norm2 << " dist " << dist << std::endl;
// std::cout << "ag " << agentNo << "angle " << a << " force val " << force_value << " " << norm2 << " dist " << dist << std::endl;
forces += force_value * norm2;
}
......@@ -676,14 +677,13 @@ void Agent::computeNewVelocity()
//----- forces dues à la répulsion des obstacles fixes ----------
nobst=0;
// double fixed_obst_stiffness = 50000.0; // agent-obstacle interaction stiffness
double fixed_obst_stiffness = 50000.0; // agent-obstacle interaction stiffness
// double fixed_obst_damping = 1.0; // agent-obstacle interaction damping
int fixed_obst_power = 1; // the power to which elevate the agent-obstacle distance
Obstacle* fixed_obst ;
int nobst=0;
for(std::vector<std::pair<float, Obstacle*> >::iterator it = obstacleNeighbors_.begin() ;
it != obstacleNeighbors_.end() ;
++it)
......@@ -695,11 +695,13 @@ void Agent::computeNewVelocity()
float force_value=0.0;
if(dist < effective_range)
{
collision_softening_factor = pow(1 - dist/effective_range,fixed_obst_power);
collision_softening_factor = pow(1-dist/effective_range,fixed_obst_power);
force_value = fixed_obst_stiffness*collision_softening_factor*(effective_range-dist);
}
fixed_obst=it->second ;
if(fixed_obst->mo!=NULL)
cerr << "Collision avec limace !!!" << endl;
VEC3 p1=fixed_obst->p1 ;
VEC3 p2=fixed_obst->p2 ;
vec=p2-p1;
......@@ -721,8 +723,8 @@ void Agent::computeNewVelocity()
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.8;
double ag_phys_radius_coef = 8.0*(1 + rand*radius_var); // valeurs uniformement réparties entre min et max
double radius_var = 0.5;
double ag_phys_radius_coef = 4.0*(1 + rand*radius_var); // valeurs uniformement réparties entre min et max
unsigned int nbA = 0 ;
......@@ -751,7 +753,8 @@ void Agent::computeNewVelocity()
if(dist < combinedRadius)
{
collision_softening_factor = pow(1 - dist/combinedRadius,ag_power);
// collision_softening_factor = pow(1-dist/combinedRadius,ag_power);
collision_softening_factor = 1;
float force_value = - ag_stiffness*collision_softening_factor*(combinedRadius-dist)
- ag_damping * (dist - previous_dist) / sim_->timeStep_;
......@@ -763,7 +766,10 @@ void Agent::computeNewVelocity()
//------- calcul de la trainee --------------------------------------
// forces -= ag_ambient_damping * velocity_;
//------- calcul de la trainee --------------------------------------
forces -= ag_ambient_damping * velocity_;
//------- calcul de la nouvelle valeur de la vitesse ----------------
......
......@@ -77,7 +77,7 @@ void EnvMap::init(unsigned int config, REAL width, REAL height, REAL minSize, RE
{
case 0 :
// CityGenerator::generateGrid<PFP>(*this) ;
CityGenerator::generateCity<PFP>(*this,0) ;
CityGenerator::generateCity<PFP>(*this,10) ;
break ;
case 1 :
// CityGenerator::generateGrid<PFP>(*this) ;
......@@ -1366,7 +1366,8 @@ void EnvMap::updateMap()
void EnvMap::resetAgentInFace(Agent* agent)
{
VEC3 pos = agent->part_.getPosition() ;
agent->part_.ParticleBase<PFP>::move(Algo::Surface::Geometry::faceCentroid<PFP>(map, agent->part_.d, position)) ;
agent->part_.Algo::MovingObjects::ParticleBase<PFP>::move(Algo::Surface::Geometry::faceCentroid<PFP>(map, agent->part_.d, position)) ;
agent->part_.setState(FACE) ;
agent->part_.move(pos) ;
}
......
......@@ -167,16 +167,6 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
center /= nbVertices;
front=(pos[1] + pos[2]) / 2;
//-------- code ajoute par Arash pour les obstacles rectangulaires --------------
length = (pos[0]-pos[1]).norm();
width = (pos[1]-pos[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 && parent==NULL) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
{
angle = get_angle(goals_[curGoal_] - center,front - center);
......@@ -609,25 +599,6 @@ void MovingObstacle::update()
// if(!rigid_)
// center = position[groundFace];
//-------- code ajoute par Arash pour les obstacles rectangulaires --------------
VEC3 P0_P1 = position[1] - position[0];
float velocity_coef = 10.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)
{
......
......@@ -115,7 +115,8 @@ void Simulator::doStep()
#endif
movingObstacles_[i]->update() ;
movingObstacles_[i]->updateMesh() ;
// commente par Arash
// movingObstacles_[i]->updateMesh() ;
}
for (unsigned int i = 0 ; i < agents_.size() ; ++i)
......@@ -283,7 +284,7 @@ void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObsta
}
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);
......@@ -589,8 +590,8 @@ void Simulator::setupScenario(unsigned int nbMaxAgent, bool pedWay)
TraversorF<PFP::MAP> tF(envMap_.map);
Dart d = tF.begin() ;
unsigned int nbx = 2 ;
unsigned int nby = 2 ;
unsigned int nbx = 4 ;
unsigned int nby = 4 ;
unsigned int bMax = nbx * nby > 0 ? nbMaxAgent / (nbx * nby) : nbMaxAgent ;
......
......@@ -571,11 +571,11 @@ void SocialAgents::cb_redraw()
moShader->disableVertexAttribs();
}
for (std::vector<MovingMesh*>::iterator it = simulator.movingMeshes_.begin() ; it != simulator.movingMeshes_.end() ; ++it)
// Commente par Arash
/*for (std::vector<MovingMesh*>::iterator it = simulator.movingMeshes_.begin() ; it != simulator.movingMeshes_.end() ; ++it)
{
(*it)->draw();
}
}*/
glColor3f(1.0f, 1.0f, 1.0f) ;
for (std::vector<MovingObstacle*>::iterator it = simulator.movingObstacles_.begin() ; it != simulator.movingObstacles_.end() ; ++it)
......@@ -841,7 +841,7 @@ void SocialAgents::animate()
// if (CityGenerator::animateCity<PFP>(&sim.envMap_)) sim.addPathToCorner() ;
// sim.addPathsToAgents() ;
//
std::cout << "t : " << simulator.globalTime_ << std::endl ;
// std::cout << "t : " << simulator.globalTime_ << std::endl ;
//
// timeval startTime ;
// gettimeofday(&startTime, NULL) ;
......
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