Commit 76e8789f authored by pitiot's avatar pitiot
Browse files

maj behaviour

parent de3a6e76
......@@ -15,7 +15,7 @@ bool notDiagonalAdjacentToAnObstacle(typename PFP::MAP& map, Dart d, CellMarker<
template <typename PFP> Algo::Modelisation::Polyhedron<PFP> generateGrid(EnvMap& envMap) ;
template <typename PFP> void generateCity(EnvMap& envMap) ;
template <typename PFP> void generateCity(EnvMap& envMap, unsigned int nbBuildings) ;
template <typename PFP>
Algo::Modelisation::Polyhedron<PFP> generateTrianGrid(typename PFP::MAP& map,
......
......@@ -63,9 +63,9 @@ Algo::Modelisation::Polyhedron<PFP> generateGrid(EnvMap& envMap)
}
template <typename PFP>
void generateCity(EnvMap& envMap)
void generateCity(EnvMap& envMap, unsigned int nbBuildings)
{
unsigned int nbBuilding = 1000 ;
unsigned int nbBuilding = nbBuildings ;
std::cout << " - Generate City : " << nbBuilding << " buildings" << std::endl ;
generateGrid<PFP>(envMap) ;
......@@ -76,7 +76,7 @@ void generateCity(EnvMap& envMap)
if (!envMap.buildingMark.isMarked(d) && (rand() % 12 == 0)
&& notDiagonalAdjacentToAnObstacle<PFP>(envMap.map, d, envMap.buildingMark))
{
generateBuilding<PFP>(envMap, d, (1 + (rand() % 3)) * 2.0f, rand() % 4) ;
generateBuilding<PFP>(envMap, d, (1 + (rand() % 3)) * 50.0f, rand() % 4) ;
--nbBuilding ;
}
}
......
......@@ -9,15 +9,15 @@
#include "agent.h"
#include "simulator.h"
unsigned int Agent::maxNeighbors_ = 10 ;
unsigned int Agent::maxMovingObstacles_ = 10;
unsigned int Agent::maxNeighbors_ = 25 ;
unsigned int Agent::maxMovingObstacles_ = 5;
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_ = 5*timeHorizonObst_ * maxSpeed_ + radius_ ;
float Agent::range_ = 2*timeHorizonObst_ * maxSpeed_ + radius_ ;
// float Agent::range_ = timeHorizonObst_ * maxSpeed_ + radius_ ;
float Agent::rangeSq_ = range_ * range_ ;
......@@ -175,54 +175,113 @@ void Agent::updateObstacleNeighbors()
#else
std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[part_.d] ;
std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[part_.d] ;
float maxDistObst = 0.0f ;
float maxDistMovingObst = 0.0f ;
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 ((*it)->mo==NULL)
{
if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((obstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistObst)
&&distSq < rangeSq_)
{
if ((*it)->mo==NULL)
obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
else
if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
{
movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
if (distSq > maxDistObst) maxDistObst = distSq ;
obstacleNeighbors_.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_)
else
{
if(Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((movingObstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistMovingObst)
&&distSq < rangeSq_)
{
if ((*it)->mo==NULL)
obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
else
if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
{
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, part_.getPosition()) ;
if ((obstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistObst)
&&distSq < rangeSq_)
{
if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
{
if (distSq > maxDistObst) maxDistObst = distSq ;
obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
}
}
else
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((movingObstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistMovingObst)
&&distSq < rangeSq_)
{
if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
{
if (distSq > maxDistMovingObst) maxDistMovingObst = distSq ;
movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
}
}
}
if (obstacleNeighbors_.size() > maxNeighbors_)
{
sim_->nbSorts++ ;
std::sort(obstacleNeighbors_.begin(), obstacleNeighbors_.end(), obstacleSort) ;
}
if (movingObstacleNeighbors_.size() > maxNeighbors_)
{
sim_->nbSorts++ ;
std::sort(movingObstacleNeighbors_.begin(), movingObstacleNeighbors_.end(), obstacleSort) ;
}
#endif
// if (obstacleNeighbors_.size() > maxNeighbors_)
// std::sort(obstacleNeighbors_.begin(), obstacleNeighbors_.end(), obstacleSort) ;
// CGoGNout<<"obstacles perçus : "<<CGoGNendl;
// for (std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
// it != movingObstacleNeighbors_.end() ;
// ++it)
// {
// (*it).second->mo->color1=0.5f;
// (*it).second->mo->color1=0.0f;
// (*it).second->mo->color2=0.0f;
// (*it).second->mo->seen=true;
// CGoGNout<<"obstacle : "<< (*it).second->mo->index<<CGoGNendl;
//
// }
for (int it= 0; it < nb_mos; it ++)
{
movingObstacles_[it]->color1=0.0f;
movingObstacles_[it]->color2=0.0f;
movingObstacles_[it]->seen =true;
}
// for (int it= 0; it < nb_mos; it ++)
// {
// movingObstacles_[it]->color1=0.0f;
// movingObstacles_[it]->color2=0.0f;
// movingObstacles_[it]->seen =true;
// }
}
//-----------------------------------------------------------------
......@@ -429,7 +488,7 @@ static int moAppend(MovingObstacle **moving_obstacles, int nb_mos, int max, Movi
unsigned char already_processed=false;
int mo_count;
if(nb_mos>=max-1)
if(nb_mos>=max)
{
// cerr << "Agent::moPush : the maximum number of moving obstacles is assumed to be " << max << endl;
// cerr << "There is visibly a need for more." << endl;
......@@ -585,31 +644,31 @@ void Agent::computeNewVelocity() // de arash
}
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;
// //------- 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;
}
///* Search for the best new velocity. */
......
......@@ -69,17 +69,18 @@ void EnvMap::init(unsigned int config, REAL width, REAL height, REAL minSize, RE
switch (config)
{
case 0 :
CityGenerator::generateGrid<PFP>(*this) ;
// CityGenerator::generateGrid<PFP>(*this) ;
CityGenerator::generateCity<PFP>(*this,1) ;
break ;
case 1 :
CityGenerator::generateGrid<PFP>(*this) ;
break ;
case 2 :
CityGenerator::generateCity<PFP>(*this) ;
CityGenerator::generateGrid<PFP>(*this) ;
// CityGenerator::generateMall<PFP>(map, position, obstacleMark, buildingMark, sideSize);
break ;
case 3 :
CityGenerator::generateCity<PFP>(*this) ;
CityGenerator::generateCity<PFP>(*this,10) ;
break ;
case 4 :
CityGenerator::generatePlanet<PFP>(*this) ;
......
......@@ -376,7 +376,7 @@ void MovingObstacle::update()
//-------- code ajoute par Arash pour les obstacles rectangulaires --------------
VEC3 P0_P1 = vertices[1] - vertices[0];
float velocity_coef = 50.0;
float velocity_coef = 10.0;
if(P0_P1 * velocity_ > 0) // P0_P1 est dans le sens de la vitesse
{
......
......@@ -14,8 +14,8 @@ Simulator::Simulator(int minS) :
nb_dead(0)
{
minSize=minS;
multires=false;
detect_agent_collision=false;
multires=true;
detect_agent_collision=true;
srand(10) ;
nbStepsPerUnit_ = 1 / timeStep_ ;
config=0;
......@@ -41,7 +41,7 @@ void Simulator::init( float dimension, bool enablePathFinding)
switch (config)
{
case 0 :
setupCircleScenario(5,40) ;
setupCircleScenario(1000,20) ;
break ;
case 1 :
setupCorridorScenario(1000,40) ;
......@@ -257,7 +257,7 @@ void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObsta
}
else
{
v=VEC3 (std::cos(angle) * (radius/3), std::sin(angle) * (radius/3), 0) ;
v=VEC3 (std::cos(angle) * (radius/2), std::sin(angle) * (radius/2), 0) ;
}
start = center + v ;
positions.push_back(start);
......@@ -434,12 +434,12 @@ void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int n
{
if (multires)
{
envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
envMap_.init(config, 1600.0f, 960.0f, minSize, 160.0f) ; //grosses cases
}
else
{
envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //cases fines
envMap_.init(config, 1600.0f, 960.0f, minSize, 160.0f) ; //cases fines
}
......
......@@ -1460,7 +1460,7 @@ int main(int argc, char** argv)
{
QApplication app(argc, argv) ;
int minSize = Agent::neighborDist_ ;
int minSize = Agent::range_ ;
int iterations = 0 ;
if (argc > 2) iterations = atoi(argv[2]) ;
......
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