Commit 6b7da6c3 authored by David Cazier's avatar David Cazier
Browse files

Merge cgogn:~jund/CGoGN_Apps/SA2

Conflicts:
	src/simulator.cpp

Correction des conflits pour importSVG
parents 7bcdf558 d4992aaf
......@@ -4,7 +4,7 @@ project(SocialAgents)
#add_definitions(-DSPATIAL_HASHING)
SET(CGoGN_ROOT_DIR ${CMAKE_SOURCE_DIR}/../CGoGN CACHE STRING "CGoGN root dir")
SET(CGoGN_ROOT_DIR ${CMAKE_SOURCE_DIR}/../../CGoGN CACHE STRING "CGoGN root dir")
include(${CGoGN_ROOT_DIR}/apps_cmake.txt)
add_subdirectory(${CMAKE_SOURCE_DIR}/Release Release)
......
......@@ -37,10 +37,12 @@ struct PFP : public PFP_STANDARD
// definition des listes d'agent
typedef std::vector<Agent*> AGENTS ;
typedef std::vector<Obstacle*> OBSTACLES ;
typedef std::vector<MovingObstacle*> MOVINGOBSTACLES;
typedef NoMathIONameAttribute<AGENTS> AGENTVECT ;
typedef NoMathIONameAttribute<OBSTACLES> OBSTACLEVECT ;
typedef NoMathIONameAttribute<MOVINGOBSTACLES> MOVINGOBSTACLEVECT;
typedef VertexAttribute<PFP::VEC3> TVEC3;
......@@ -194,7 +196,7 @@ void update_registration(Obstacle * o);
void register_pop(Obstacle* o, int n);
void resetObstInFace(Obstacle* o);
void resetObstPartInFace(Obstacle* o, Dart d);
void resetPart(MovingObstacle * mo, Dart d);
void resetPart(MovingObstacle * mo,int index, Dart d);
void displayMO(Obstacle * o);
/**************************************
......
......@@ -23,7 +23,7 @@ public:
bool is_inside (VEC3 p);
void computePrefVelocity();
std::list<Dart> getMemoCross(const Algo::MovingObjects::ParticleCell2D<PFP>* p1, const Algo::MovingObjects::ParticleCell2D<PFP>* p2);
std::list<Dart> getMemoCross(const Algo::MovingObjects::ParticleCell2D<PFP>* p1, const VEC3& dest);
void update();
......@@ -55,9 +55,10 @@ public:
VEC3 finalGoal;
float make_half_turn;
// Dart finalDart;
//
// unsigned int curGoal_;
//
std::vector<VEC3> goals_ ;
unsigned int curGoal_;
// static float neighborDistSq_;
static unsigned int maxNeighbors_;
static float detectionFixedObst;
......
......@@ -106,6 +106,7 @@ public:
void setupMovingObstacle(unsigned int nbObstacles) ;
#ifndef SPATIAL_HASHING
void addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal);
void addPathToCorner() ;
void addPathsToAgents() ;
void addPathsToAgents_height() ;
......
......@@ -84,6 +84,13 @@
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="check_drawObstPredictionTri">
<property name="text">
<string>draw obstacles prediction tri</string>
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="check_drawAgentsNeighborDist">
<property name="text">
......
......@@ -95,6 +95,7 @@ public:
bool drawAgentsNeighborDist ;
bool drawAgentsObstacleDist ;
bool drawAgentsPredictionTri ;
bool drawObstPredictionTri ;
bool draw_dart;
unsigned int dartSlider;
......@@ -135,6 +136,11 @@ public slots:
drawAgentsPredictionTri = b ;
updateGL() ;
}
void slot_drawObstPredictionTri(bool b)
{
drawObstPredictionTri = b ;
updateGL() ;
}
void slot_drawAgentsNeighborDist(bool b)
{
drawAgentsNeighborDist = b ;
......
This diff is collapsed.
......@@ -141,7 +141,7 @@ void Agent::updateObstacleNeighbors()
if(distSq < rangeSq_)
{
if(Geom::testOrientation2D(pos, (*it)->p1, (*it)->p2) == Geom::RIGHT)
obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
}
}
......@@ -155,8 +155,12 @@ void Agent::updateObstacleNeighbors()
{
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)) ;
if ((*it)->mo==NULL)
obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
else
{
movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
}
}
......@@ -168,16 +172,17 @@ void Agent::updateObstacleNeighbors()
{
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)) ;
if ((*it)->mo==NULL)
obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
else
movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
}
}
#endif
if (obstacleNeighbors_.size() > maxNeighbors_) std::sort(obstacleNeighbors_.begin(),
obstacleNeighbors_.end(),
obstacleSort) ;
// if (obstacleNeighbors_.size() > maxNeighbors_)
// std::sort(obstacleNeighbors_.begin(), obstacleNeighbors_.end(), obstacleSort) ;
}
void Agent::update()
......@@ -191,6 +196,7 @@ void Agent::update()
pos = pos + (velocity_ * sim->timeStep_) ;
#else
VEC3 target = part_.getPosition() + (velocity_ * sim_->timeStep_) ;
obstacle_priority(&target);
#endif
meanSpeed_ *= 3.0f ;
......@@ -231,11 +237,16 @@ void Agent::obstacle_priority(PFP::VEC3 * goalVector)
norm[0]=vec[1] ;
norm[1]=-vec[0] ;
}
else if (sim_->avoidance==1) // avoids along the obstacle side
else if (sim_->avoidance==1) // avoids with direction from center of the obstacle
{
norm[0]=(3.0f*vec[1]+vec[0])/4.0f ;
norm[1]=(vec[1]-3.0f*vec[0])/4.0f ;
MovingObstacle * mo = obst->mo;
norm = this->part_.getPosition()-mo->center;
}
// else if (sim_->avoidance==2) // avoids with opposite direction of obstacle
// {
// MovingObstacle * mo = obst->mo;
// norm = this->part_.getPosition()-mo->center;
// }
norm.normalize() ;
norm*=5.0f*factor*factor*factor*factor*factor*factor*factor*factor ;
sumNorm+=norm ;
......@@ -261,7 +272,8 @@ void Agent::computePrefVelocity()
goalDist2 = goalVector.norm2() ;
}
if (goalDist2 > maxSpeed_) {
if (goalDist2 > maxSpeed_)
{
goalVector.normalize() ;
goalVector *= maxSpeed_;
}
......@@ -329,6 +341,7 @@ void Agent::computeNewVelocity()
for (std::vector<std::pair<float, Obstacle*> >::iterator it = obstacleNeighbors_.begin() ;
it != obstacleNeighbors_.end() ; ++it)
{
const Obstacle* obst = it->second ;
const VEC3 relativePosition1(obst->p1 - getPosition()) ;
......@@ -352,7 +365,8 @@ void Agent::computeNewVelocity()
}
}
if (alreadyCovered) continue ;
if (alreadyCovered)
continue ;
/* Not yet covered. Check for collisions. */
......@@ -433,11 +447,9 @@ void Agent::computeNewVelocity()
const float leg1 = std::sqrt(distSq1 - radiusSq) ;
leftLegDirection = VEC3(relativePosition1[0] * leg1 - relativePosition1[1] * radius_,
relativePosition1[0] * radius_ + relativePosition1[1] * leg1, 0)
/ distSq1 ;
relativePosition1[0] * radius_ + relativePosition1[1] * leg1, 0) / distSq1 ;
rightLegDirection = VEC3(relativePosition1[0] * leg1 + relativePosition1[1] * radius_,
-relativePosition1[0] * radius_ + relativePosition1[1] * leg1,
0) / distSq1 ;
-relativePosition1[0] * radius_ + relativePosition1[1] * leg1, 0) / distSq1 ;
}
else if (s > 1 && distSqLine <= radiusSq)
{
......@@ -455,11 +467,9 @@ void Agent::computeNewVelocity()
const float leg2 = std::sqrt(distSq2 - radiusSq) ;
leftLegDirection = VEC3(relativePosition2[0] * leg2 - relativePosition2[1] * radius_,
relativePosition2[0] * radius_ + relativePosition2[1] * leg2, 0)
/ distSq2 ;
relativePosition2[0] * radius_ + relativePosition2[1] * leg2, 0) / distSq2 ;
rightLegDirection = VEC3(relativePosition2[0] * leg2 + relativePosition2[1] * radius_,
-relativePosition2[0] * radius_ + relativePosition2[1] * leg2,
0) / distSq2 ;
-relativePosition2[0] * radius_ + relativePosition2[1] * leg2, 0) / distSq2 ;
}
else
{
......@@ -468,8 +478,7 @@ void Agent::computeNewVelocity()
// {
const float leg1 = std::sqrt(distSq1 - radiusSq) ;
leftLegDirection = VEC3(relativePosition1[0] * leg1 - relativePosition1[1] * radius_,
relativePosition1[0] * radius_ + relativePosition1[1] * leg1, 0)
/ distSq1 ;
relativePosition1[0] * radius_ + relativePosition1[1] * leg1, 0) / distSq1 ;
// }
// else
// {
......@@ -481,8 +490,7 @@ void Agent::computeNewVelocity()
// {
const float leg2 = std::sqrt(distSq2 - radiusSq) ;
rightLegDirection = VEC3(relativePosition2[0] * leg2 + relativePosition2[1] * radius_,
-relativePosition2[0] * radius_ + relativePosition2[1] * leg2,
0) / distSq2 ;
-relativePosition2[0] * radius_ + relativePosition2[1] * leg2, 0) / distSq2 ;
// }
// else
// {
......
This diff is collapsed.
......@@ -73,7 +73,6 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, V
movingObstNeighbors_.reserve(maxNeighbors_);
rot = rota;
finalGoal = goal;
VEC3 sum = 0;
nb_agents_voisins = 0;
nb_register_cells = 0;
......@@ -121,6 +120,10 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, V
obstacle_range = 15.0f * 15.0f;
make_half_turn = get_angle(finalGoal - center, (parts_[0]->getPosition() + parts_[1]->getPosition()) / 2 - center) * nbVertices;
goals_.push_back(center);
goals_.push_back(goal);
curGoal_ = 0;
}
bool MovingObstacle::test_opposition(VEC3 o, VEC3 p1, VEC3 p2)
......@@ -192,45 +195,64 @@ void MovingObstacle::updateFixedObstNeighbors()
// TODO Check position
void MovingObstacle::computePrefVelocity()
{
VEC3 goalVector = finalGoal - center;
int co=2;
float angle;
float goalDist2 = goalVector.norm2();
// CGoGNout << "finalGoal "<< finalGoal << CGoGNendl;
// CGoGNout << "dist 2 "<< goalDist2 << CGoGNendl;
if(goalDist2 < 2.0f)
{
// co=rand() % 2 + 1;
VEC3 goalVector = goals_[curGoal_] - center ;
float goalDist2 = goalVector.norm2() ;
if(co==1)
finalGoal[0]*=-1;
else
finalGoal[1]*=-1;
goalVector = finalGoal - center;
angle =get_angle(finalGoal-center,(parts_[0]->getPosition())-center);
make_half_turn=angle*nbVertices;
if (goalDist2 < 2.0f)
{
curGoal_ = (curGoal_ + 1) % goals_.size() ;
goalVector = goals_[curGoal_] - center ;
goalDist2 = goalVector.norm2() ;
}
if (goalDist2 > 1.0f)
goalVector.normalize();
if (goalDist2 > maxSpeed_)
{
goalVector.normalize() ;
goalVector *= maxSpeed_;
}
prefVelocity_=goalVector;
prefVelocity_ = goalVector ;
// VEC3 goalVector = finalGoal - center;
// int co=2;
// float angle;
// float goalDist2 = goalVector.norm2();
// // CGoGNout << "finalGoal "<< finalGoal << CGoGNendl;
// // CGoGNout << "dist 2 "<< goalDist2 << CGoGNendl;
// if(goalDist2 < 2.0f)
// {
//// co=rand() % 2 + 1;
//
//
// if(co==1)
// finalGoal[0]*=-1;
// else
// finalGoal[1]*=-1;
//
// goalVector = finalGoal - center;
// angle =get_angle(finalGoal-center,(parts_[0]->getPosition())-center);
// make_half_turn=angle*nbVertices;
// }
//
// if (goalDist2 > 1.0f)
// goalVector.normalize();
//
// prefVelocity_=goalVector;
}
// TODO Check position
void MovingObstacle::update()
{
assert(sim_->envMap_.map.getCurrentLevel() == sim_->envMap_.map.getMaxLevel()) ;
// general_belonging.clear();
PFP::VEC3 sum = 0;
PFP::VEC3 bary = 0;
Dart d;
velocity_[0] = prefVelocity_[0] * velocity_factor;
velocity_[1] = prefVelocity_[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;
......@@ -254,17 +276,14 @@ void MovingObstacle::update()
target += rotate(parts_[i]->getPosition(), center, rotor);
}
// CGoGNout << "avant déplacement particule : "<< i << CGoGNendl;
// CGoGNout << "particule : "<< parts_[i]->state << CGoGNendl;
parts_[i]->move(target);
// CGoGNout << "apres déplacement particule : "<< i << CGoGNendl;
sum += target;
bary += target;
}
make_half_turn -= rotor * nbVertices;
if (make_half_turn == 0)
center = sum / nbVertices;
center = bary / nbVertices;
// MAJ des obstacles
for (unsigned int i = 0; i < nbVertices; ++i)
......@@ -402,14 +421,13 @@ void register_add(Obstacle* o, int n, const std::list<Dart>& memo_cross)
}
}
std::list<Dart> MovingObstacle::getMemoCross(const Algo::MovingObjects::ParticleCell2D<PFP>* p1,const Algo::MovingObjects::ParticleCell2D<PFP>* p2)
std::list<Dart> MovingObstacle::getMemoCross(const Algo::MovingObjects::ParticleCell2D<PFP>* p1, const VEC3& dest)
{
Dart d = p1->d;
const VEC3& start = p1->getPosition();
const VEC3& stop = p2->getPosition();
CGoGN::Algo::MovingObjects::ParticleCell2DMemo<PFP> part(sim_->envMap_.map, d, start, sim_->envMap_.position);
part.move(stop);
part.move(dest);
return part.memo_cross;
}
......@@ -421,24 +439,24 @@ void MovingObstacle::register_update(Obstacle* o, Dart d, int n)
std::list<Dart> memo;
bool modif=false;
// bool modif=false;
if(p1->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS
|| p2->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
{
memo = getMemoCross(p1,p2);
memo.sort();
modif=true;
}
else if(!sim_->envMap_.map.sameFace(p1->d,p2->d))
{
memo = getMemoCross(p1,p2);
memo.sort();
if(belonging_cells[n] != memo)
modif=true;
}
if(modif)
// if(p1->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS
// || p2->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
// {
memo = getMemoCross(p1,p2->getPosition());
// memo.sort();
// modif=true;
// }
// else if(!sim_->envMap_.map.sameFace(p1->d,p2->d))
// {
// memo = getMemoCross(p1,p2->getPosition());
// memo.sort();
// if(belonging_cells[n] != memo)
// modif=true;
// }
//
// if(modif)
{
register_pop(o, n);
......@@ -460,7 +478,7 @@ void update_registration(Obstacle * o)
{
int i = o->index;
std::list<Dart> memo = mo->getMemoCross(mo->parts_[i] , mo->parts_[(i+1)%mo->nbVertices]);
std::list<Dart> memo = mo->getMemoCross(mo->parts_[i] , mo->parts_[(i+1)%mo->nbVertices]->getPosition());
register_add(o, i, memo);
// mo->general_belonging.clear();
......@@ -504,13 +522,13 @@ void resetObstPartInFace(Obstacle* o, Dart d1)
}
}
void resetPart(MovingObstacle * mo, Dart d1)
void resetPart(MovingObstacle * mo,int index, Dart d1)
{
int i = index;
if (mo->parts_[i]->d == mo->sim_->envMap_.map.phi1(d1))
mo->parts_[i]->d = d1;
for (unsigned int i = 0; i < mo->nbVertices; i++) {
if (mo->parts_[i]->d == mo->sim_->envMap_.map.phi1(d1))
mo->parts_[i]->d = d1;
}
}
void displayMO(Obstacle * o)
......
......@@ -10,12 +10,12 @@ Simulator::Simulator(int minSize) :
nbCoarsenCandidate(0),
nearNeighbors(0),
totalNeighbors(0),
avoidance(0),
avoidance(1),
nb_dead(0)
{
srand(10) ;
nbStepsPerUnit_ = 1 / timeStep_ ;
init(1, minSize, 2.0f) ;
init(5, minSize, 2.0f) ;
}
Simulator::~Simulator()
......@@ -35,7 +35,7 @@ void Simulator::init(unsigned int config, int minSize, float dimension, bool ena
setupCircleScenario(100) ;
break ;
case 1 :
setupCorridorScenario(100, 10) ;
setupCorridorScenario(500, 25) ;
break ;
case 2 :
setupScenario(1000) ;
......@@ -72,6 +72,7 @@ void Simulator::init(unsigned int config, int minSize, float dimension, bool ena
#ifndef SPATIAL_HASHING
// envMap_.subdivideToProperLevel() ;
// envMap_.subdivideAllToMaxLevel();
#endif
// setupMovingObstacle(40);
......@@ -81,7 +82,7 @@ void Simulator::doStep()
{
#ifndef SPATIAL_HASHING
envMap_.clearUpdateCandidates() ;
envMap_.map.setCurrentLevel(0) ;
envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
......@@ -90,19 +91,10 @@ void Simulator::doStep()
movingObstacles_[i]->update() ;
}
for (unsigned int i = 0 ; i < agents_.size() ; ++i)
{
agents_[i]->updateObstacleNeighbors() ;
#ifndef SPATIAL_HASHING
envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
agents_[i]->updateAgentNeighbors() ;
#ifndef SPATIAL_HASHING
envMap_.map.setCurrentLevel(0) ;
#endif
agents_[i]->computePrefVelocity() ;
agents_[i]->computeNewVelocity() ;
}
......@@ -148,7 +140,8 @@ void Simulator::doStep()
nbRefineCandidate += envMap_.refineCandidate.size() ;
nbCoarsenCandidate += envMap_.coarsenCandidate.size() ;
// envMap_.updateMap() ;
envMap_.updateMap() ;
#endif
globalTime_ += timeStep_ ;
......@@ -272,14 +265,15 @@ void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObst
yGoalDelta = envMap_.geometry.size(1) / 5 ;
yGoalMin = envMap_.geometry.max()[1] - yBorder - yGoalDelta ;
VEC3 xSide (10.0f,0.0f,0.0f);
VEC3 ySide (0.0f,15.0f,0.0f);
VEC3 xSide (5.0f,0.0f,0.0f);
VEC3 ySide (0.0f,10.0f,0.0f);
std::vector<VEC3> vPos;
MovingObstacle* mo4;
for (unsigned int i = 0 ; i < nbObstacles ; i++)
{
float x = xStartMin + rand() % xStartDelta;
float x = xStartMin + ((int)i*30) % xStartDelta;
std::cout << "x ? " << x << " xStartDelta " << xStartDelta << std::endl;
VEC3 start(x, yStartMin + rand() % yStartDelta, 0) ;
VEC3 goal(x, yGoalMin + rand() % yGoalDelta, 0) ;
......@@ -291,18 +285,35 @@ void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObst
tmp = goal ;
goal = start ;
start = tmp ;
vPos.push_back(start-xSide+ySide);
vPos.push_back(start-xSide-ySide);
vPos.push_back(start+xSide-ySide);
vPos.push_back(start+xSide+ySide);
vPos.push_back(start-xSide+ySide);
}
else {
vPos.push_back(start+xSide-ySide);
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,goal,0);
//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);
movingObstacles_.push_back(mo4);
}
}
......@@ -431,6 +442,21 @@ void Simulator::setupScenario(unsigned int nbMaxAgent)
swapAgentsGoals() ;
}
void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
{
std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map,
envMap_.position, dStart,
dGoal,
envMap_.buildingMark) ;
for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
{
VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
mo->goals_.push_back(dest) ;
}
}
#ifndef SPATIAL_HASHING
void Simulator::addPathToCorner()
{
......
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.