diff --git a/CMakeLists.txt b/CMakeLists.txt index 7e66547a7d746c153a6fe5689cee17f9c2cfb9f9..b22a707d27dd45f2d74925800677dd8cccb57a67 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,7 +12,7 @@ SET (CGoGN_EXT_LIBS ${CGoGN_EXT_LIBS} ${QT_LIBRARIES} ${QGLVIEWER_LIBRARIES}) SET(CGoGN_ROOT_DIR ${CMAKE_SOURCE_DIR}/../../CGoGN CACHE STRING "CGoGN root dir") -SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g") +SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0") include(${CGoGN_ROOT_DIR}/apps_cmake.txt) add_subdirectory(${CMAKE_SOURCE_DIR}/Release Release) diff --git a/include/env_generator.hpp b/include/env_generator.hpp index a097c50cfaa22e870891ecfe4a619e683ad487df..f2a6fbc30d342ccdcbf9dabac8470fac0eb76923 100644 --- a/include/env_generator.hpp +++ b/include/env_generator.hpp @@ -389,9 +389,8 @@ Dart generateBuilding(EnvMap& envMap, Dart d, float height, unsigned int buildin obstacleMark.mark(dd) ; Dart next = map.phi1(dd) ; - Dart previous = map.phi_1(dd) ; - Obstacle* o = new Obstacle(position[dd], position[next], position[previous], - position[map.phi1(next)], NIL, NIL, NULL, 0); + + Obstacle* o = new Obstacle(position[dd], position[next], NULL, 0); #ifdef SPATIAL_HASHING VEC3 ov = o->p2 - o->p1 ; diff --git a/include/env_map.h b/include/env_map.h index 9edf14430f1c27f98e36057e9cb96bd39ccba66f..eff05fd55fb0c51d5ddd686ebadfacf66f37703a 100644 --- a/include/env_map.h +++ b/include/env_map.h @@ -34,7 +34,7 @@ class ArticulatedObstacle; //#define EXPORTING3 -#define TWO_AND_HALF_DIM +//#define TWO_AND_HALF_DIM #ifdef EXPORTING3 @@ -377,20 +377,7 @@ inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector& b for (std::vector::const_iterator it =belonging_cells.begin();itmo; -// int n = o->index; -// int m = (n+1)%mo->nbVertices; -//#ifdef TWO_AND_HALF_DIM -// CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalfMemo * registering_part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalfMemo(mo->sim_->envMap_.map, mo->parts_[n]->d,o->p1,mo->sim_->envMap_.position); -//#else -// CGoGN::Algo::Surface::MovingObjects::ParticleCell2DMemo * registering_part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DMemo(mo->sim_->envMap_.map, mo->position[n],o->p1,mo->sim_->envMap_.position); -//#endif -// registering_part->move(mo->velocity_[n]*mo->velocityAvoidanceFactor+mo->position[n],memo_mark_speed); -// registering_part->move(mo->velocity_[m]*mo->velocityAvoidanceFactor+mo->position[m],memo_mark_speed); -// std::vector result =(); -// d2=registering_part->d; -// CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest< -// #define LINEAR + #define LINEAR // #define SECURED #ifdef LINEAR #include "ShapeMatching/shapeMatchingLinear.h" @@ -78,6 +78,7 @@ public: void attachAgent(Agent* ag); unsigned int nbVertices; + unsigned int nbParticles; #ifdef SECURED CGoGN::Algo::Surface::MovingObjects::ParticleCell2DSecured * * parts_ ; @@ -124,7 +125,7 @@ public: int max_x_ind; int min_x_ind; - Obstacle* * obstacles_; + std::vector obstacles_; std::vector * belonging_cells; std::vector * neighbor_cells; diff --git a/include/obstacle.h b/include/obstacle.h index 79cd737e334274bcf34a73b7e4500445e0e4e5c6..0321d2dcbd682fd4c3d39c3d9e96aa9591d5fc91 100644 --- a/include/obstacle.h +++ b/include/obstacle.h @@ -6,10 +6,10 @@ class Obstacle { public: - Obstacle(const VEC3 point1, const VEC3 point2, const VEC3 prevPoint, const VEC3 nextPoint, Dart d_1, Dart d_2, + Obstacle(const VEC3 point1, const VEC3 point2, MovingObstacle * moving1=NULL, unsigned int ind=0) : - d1(d_1), d2(d_2), p1(point1), p2(point2), prevP(prevPoint), nextP(nextPoint), - mo(moving1), index(ind),p3(0,0,0) + p1(point1), p2(point2), + mo(moving1), index(ind),p3(0,0,0),obst_stiffness_agent(20),obst_stiffness_obst(20) { // p1[2] = 0 ; // p2[2] = 0 ; @@ -17,16 +17,18 @@ public: // nextP[2] = 0 ; } - Dart d1; - Dart d2; VEC3 p1 ; VEC3 p2 ; - VEC3 prevP ; - VEC3 nextP ; + MovingObstacle * mo ; unsigned int index ; + + VEC3 p3; //pour les ellipses + + double obst_stiffness_agent; + double obst_stiffness_obst; } ; #endif diff --git a/src/agent.cpp b/src/agent.cpp index fb1b21911c122896f165d5af8bf32f0cb8da4e68..f049c2f4f45fe175ab696533fb878be0819bcdf4 100644 --- a/src/agent.cpp +++ b/src/agent.cpp @@ -15,7 +15,7 @@ VEC3 Agent::xyPlane = VEC3(0,0,1); unsigned int Agent::maxNeighbors_ = 10 ; unsigned int Agent::maxMovingObstacles_ = 20; -float Agent::averageMaxSpeed_ = 2.0f ; +float Agent::averageMaxSpeed_ = 0.5f ; // float Agent::averageMaxSpeed_ = 20.0f ; float Agent::neighborDist_ = 10.0f ; //float Agent::neighborDist_ = 20.0f ; @@ -25,7 +25,7 @@ float Agent::neighborDistSq_ = neighborDist_ * neighborDist_ ; float Agent::radius_ = 1.5f ; float Agent::timeHorizon_ = 10.0f ; //float Agent::timeHorizon_ = 100.0f ; -float Agent::timeHorizonObst_ = 10.0f ; +float Agent::timeHorizonObst_ = 20.0f ; float Agent::range_ = 2*timeHorizonObst_ * averageMaxSpeed_ + radius_ ; float Agent::rangeSq_ = range_ * range_ ; @@ -809,12 +809,12 @@ void Agent::computeNewVelocity() //----- forces dues à la répulsion des obstacles en mouvement ---------- VEC3 norm; - double obst_stiffness = 100.0; // agent-obstacle interaction stiffness - int obst_power = 1 ; // the power to which elevate the agent-obstacle distance + + int obst_power = 2 ; // the power to which elevate the agent-obstacle distance double obst_radius_infl; if(sim_->config==0) - obst_radius_infl = 100.; // scenario 0 + obst_radius_infl = 1000.; // scenario 0 else obst_radius_infl = 40.; // scenario 1 et 3 float force_value; @@ -842,7 +842,7 @@ void Agent::computeNewVelocity() 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); + force_value = obst->obst_stiffness_agent*collision_softening_factor*(rest_sum_of_dists - sum_of_dists); VEC3 v_obst = p2 - p1; VEC3 normal = normFace ^v_obst; // Ajouter une composante tangentielle diff --git a/src/agent.miseenpage.cpp b/src/agent.miseenpage.cpp index dc6e52f77ba80a7b6792f2b5c9aaabf2872ecba5..d5520ce7ed783c263d4172452e91e78066ff43d3 100644 --- a/src/agent.miseenpage.cpp +++ b/src/agent.miseenpage.cpp @@ -1,939 +1,939 @@ -/*=====================================================================*\ - Friday December 7th 2012 - Arash HABIBI - agent.cpp - New behaviour algorithm based on a dynamic model -\*=====================================================================*/ - - -#include "agent.h" -#include "simulator.h" - -unsigned int Agent::maxNeighbors_ = 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::rangeSq_ = range_ * range_ ; - -unsigned int Agent::cptAgent = 0 ; - -#ifdef SPATIAL_HASHING -Agent::Agent(Simulator* sim, const VEC3& position) : - pos(position), - curGoal_(-1), - velocity_(0), - newVelocity_(0), - prefVelocity_(0), - meanSpeed_(0), - sim_(sim), - alive(true) -{ - agentNeighbors_.reserve(maxNeighbors_* 2) ; - obstacleNeighbors_.reserve(maxNeighbors_* 2) ; - agentNo = cptAgent++ ; -} -#else -Agent::Agent(Simulator* sim, const VEC3& position, Dart d) : - part_(sim->envMap_.map, d, position, sim->envMap_.position), - curGoal_(-1), - velocity_(0), - newVelocity_(0), - prefVelocity_(0), - meanSpeed_(0), - sim_(sim), - alive(true) -{ - color1 = 1.0f ; - color2 = 0 ; - color3 = 0 ; - agentNeighbors_.reserve(maxNeighbors_ * 2) ; - obstacleNeighbors_.reserve(maxNeighbors_ * 2) ; - movingObstacleNeighbors_.reserve(maxNeighbors_ * 2) ; - agentNo = cptAgent++ ; -} -#endif - -//----------------------------------------------------------------- - -VEC3 Agent::getPosition() -{ -#ifdef SPATIAL_HASHING - return pos ; -#else - return part_.getPosition() ; -#endif -} - -//----------------------------------------------------------------- -//----------------------------------------------------------------- - -bool agentSort(const std::pair& a1, const std::pair& a2) -{ - return a1.first < a2.first ; -} - -//----------------------------------------------------------------- - -bool obstacleSort(const std::pair& o1, const std::pair& o2) -{ - return o1.first < o2.first ; -} - -//----------------------------------------------------------------- - -void Agent::updateAgentNeighbors() -{ - agentNeighbors_.clear() ; - -#ifdef SPATIAL_HASHING - const std::vector& agents = sim_->envMap_.getNeighbors(this) ; - const std::vector& neighborAgents ; - sim_->envMap_.getOneRingNeighbors(this,neighborAgents) ; -#else - const std::vector& agents = sim_->envMap_.agentvect[part_.d] ; - const std::vector& neighborAgents = sim_->envMap_.neighborAgentvect[part_.d] ; -#endif - - float maxDist = 0.0f ; - - for (std::vector::const_iterator it = agents.begin(); it != agents.end(); ++it) - { - if ((*it)->alive) - { - if (*it != this) - { - float distSq = (getPosition() - (*it)->getPosition()).norm2() ; - if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist) - && distSq < neighborDistSq_) - { - if (distSq > maxDist) maxDist = distSq ; - agentNeighbors_.push_back(std::make_pair(distSq, *it)) ; - } - } - } - } - - for (std::vector::const_iterator it = neighborAgents.begin(); it != neighborAgents.end(); ++it) - { - if ((*it)->alive) - { - float distSq = (getPosition() - (*it)->getPosition()).norm2() ; - if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist) - && distSq < neighborDistSq_) - { - if (distSq > maxDist) maxDist = distSq ; - agentNeighbors_.push_back(std::make_pair(distSq, *it)) ; - } - } - - sim_->nbUpdates++ ; - sim_->nearNeighbors += agentNeighbors_.size() ; - sim_->totalNeighbors += agents.size() + neighborAgents.size() ; - - - } - if (agentNeighbors_.size() > maxNeighbors_) - { - sim_->nbSorts++ ; - std::sort(agentNeighbors_.begin(), agentNeighbors_.end(), agentSort) ; - } -} - -//----------------------------------------------------------------- -//----------------------------------------------------------------- - -void Agent::updateObstacleNeighbors() -{ - obstacleNeighbors_.clear() ; - movingObstacleNeighbors_.clear() ; - -#ifdef SPATIAL_HASHING - Geom::Vec2ui c = sim_->envMap_.obstaclePositionCell(pos) ; - if(sim_->envMap_.ht_obstacles.hasData(c)) - { - const std::vector& obst = sim_->envMap_.ht_obstacles[c] ; - for(std::vector::const_iterator it = obst.begin() ; it != obst.end() ; ++it) - { - float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, pos) ; - if(distSq < rangeSq_) - { - if(Geom::testOrientation2D(pos, (*it)->p1, (*it)->p2) == Geom::RIGHT) - obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ; - } - } - } -#else - std::vector& obst = sim_->envMap_.obstvect[part_.d] ; - std::vector& neighborObst = sim_->envMap_.neighborObstvect[part_.d] ; - for(std::vector::const_iterator it = obst.begin() ; it != obst.end() ; ++it) - { - float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ; - if (distSq < rangeSq_) - { - 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)) ; - } - } - - } - } - for(std::vector::const_iterator it = neighborObst.begin() ; it != neighborObst.end() ; ++it) - { - float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ; - if(distSq < rangeSq_) - { - 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)) ; - } - } - } -#endif - -// if (obstacleNeighbors_.size() > maxNeighbors_) -// std::sort(obstacleNeighbors_.begin(), obstacleNeighbors_.end(), obstacleSort) ; -} - -//----------------------------------------------------------------- -//----------------------------------------------------------------- - -void Agent::update() -{ - if (alive) - { - finalGoal = goals_[curGoal_] ; - velocity_[0] = newVelocity_[0] ; - velocity_[1] = newVelocity_[1] ; -#ifdef SPATIAL_HASHING - pos = pos + (velocity_ * sim->timeStep_) ; -#else - VEC3 target = part_.getPosition() + (velocity_ * sim_->timeStep_) ; - -#endif - - meanSpeed_ *= 3.0f ; - meanSpeed_ += velocity_ ; - meanSpeed_ /= 4.0f ; - -// meanPos_ *= 9.0f; -// meanPos_ += part_.m_position; -// meanPos_ /= 10.0f; - -#ifndef SPATIAL_HASHING - part_.move(target) ; -#endif - } -} - -//----------------------------------------------------------------- -//----------------------------------------------------------------- - -void Agent::computeNewVelocity2() -{ - VEC3 centroid ; - VEC3 c ; - VEC3 vel ; - - float cohesion = 1.0f ; - - centroid.zero() ; - c.zero() ; - vel.zero() ; - - for (std::vector >::iterator it = obstacleNeighbors_.begin() ;it != obstacleNeighbors_.end() ; ++it) - { - } - - unsigned int nbA = 0 ; - for (std::vector >::iterator it = agentNeighbors_.begin() ; - it != agentNeighbors_.end() && nbA < maxNeighbors_ ; ++it, ++nbA) - { - VEC3 mPos = getPosition() ; - VEC3 oPos = (*it).second->getPosition() ; - centroid += oPos ; - - VEC3 pOp(oPos - mPos) ; - if (pOp.norm2() <= radius_ * radius_ * maxSpeed_ * maxSpeed_ * 2.0f) - { - c -= pOp ; -// prefVelocity_.zero(); - } - - vel += (*it).second->prefVelocity_ ; - } - - if (nbA > 0) - { - centroid /= nbA ; - centroid -= getPosition() ; - centroid /= 10 ; - - vel /= nbA ; - vel -= prefVelocity_ ; - vel /= 8 ; - } - - newVelocity_ = prefVelocity_ + cohesion * centroid + c + vel ; -} - - - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -//////comportement à modifier/////////////////////////////////////////////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - -//----------------------------------------------------------------- -// fonction d'évitement d'obstacles par les agents (calcul effectué -// avant de lancer RVO2 pour garantir la non-collision entre agents) -// En gros, on représente un obstacle : -// soit par un ensemble de segments orientés dans le sens direct (sim_->avoidance==0), -// soit par son centre (sim_->avoidance==1) -// Cette fonction parcourt les obstacles et calcule la norme de la force globale de -// répulsion. Ensuite, elle ajoute cette somme à l'objectif de façon -// à ce que l'agent change sa trajectoire. - -void Agent::obstacle_priority(PFP::VEC3 * goalVector) -{ - float factor ; - Obstacle* obst ; - PFP::VEC3 x,y,vec ; - PFP::VEC3 norm,sumNorm ; - sumNorm.zero() ; - - // On parcourt les obstacles - for(std::vector >::iterator it = movingObstacleNeighbors_.begin() ; - it != movingObstacleNeighbors_.end() ; - ++it) - { - // rangeSq = range * range (portee au carre) - // les elements de l'iteration sont des obstacles. - // chaque obstacle est une paire : float et *obstacle. - // le float est la distance à l'obstacle et *obstacle - // est un pointeur vers le segment. - - factor =(1.0f-(it->first/rangeSq_)) ; - // En particulier ici, (*it).first (ou it->first) - // represente la distance à l'agent (?) - // factor est quelque chose qui faut 0 - // quand l'agent commence à interagir avec l'obstacle - // et qui faut 1 quand l'agent se trouve colle sur l'obstacle. - -// CGoGNout<second ; - x=obst->p1 ; - y=obst->p2 ; - vec=y-x ; - vec.normalize() ; - norm.zero() ; - - if (sim_->avoidance==0)// avoids with normal of obstacle side - { - norm[0]=vec[1] ; - norm[1]=-vec[0] ; - } - else if (sim_->avoidance==1) // avoids with direction from center of the obstacle - { - MovingObstacle * mo = obst->mo; - norm = this->part_.getPosition()-mo->center; - } -// else if (sim_->avoidance==2) // avoids with opposite direction of obstacle +///*=====================================================================*\ +// Friday December 7th 2012 +// Arash HABIBI +// agent.cpp +// New behaviour algorithm based on a dynamic model +//\*=====================================================================*/ +// +// +//#include "agent.h" +//#include "simulator.h" +// +//unsigned int Agent::maxNeighbors_ = 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::rangeSq_ = range_ * range_ ; +// +//unsigned int Agent::cptAgent = 0 ; +// +//#ifdef SPATIAL_HASHING +//Agent::Agent(Simulator* sim, const VEC3& position) : +// pos(position), +// curGoal_(-1), +// velocity_(0), +// newVelocity_(0), +// prefVelocity_(0), +// meanSpeed_(0), +// sim_(sim), +// alive(true) +//{ +// agentNeighbors_.reserve(maxNeighbors_* 2) ; +// obstacleNeighbors_.reserve(maxNeighbors_* 2) ; +// agentNo = cptAgent++ ; +//} +//#else +//Agent::Agent(Simulator* sim, const VEC3& position, Dart d) : +// part_(sim->envMap_.map, d, position, sim->envMap_.position), +// curGoal_(-1), +// velocity_(0), +// newVelocity_(0), +// prefVelocity_(0), +// meanSpeed_(0), +// sim_(sim), +// alive(true) +//{ +// color1 = 1.0f ; +// color2 = 0 ; +// color3 = 0 ; +// agentNeighbors_.reserve(maxNeighbors_ * 2) ; +// obstacleNeighbors_.reserve(maxNeighbors_ * 2) ; +// movingObstacleNeighbors_.reserve(maxNeighbors_ * 2) ; +// agentNo = cptAgent++ ; +//} +//#endif +// +////----------------------------------------------------------------- +// +//VEC3 Agent::getPosition() +//{ +//#ifdef SPATIAL_HASHING +// return pos ; +//#else +// return part_.getPosition() ; +//#endif +//} +// +////----------------------------------------------------------------- +////----------------------------------------------------------------- +// +//bool agentSort(const std::pair& a1, const std::pair& a2) +//{ +// return a1.first < a2.first ; +//} +// +////----------------------------------------------------------------- +// +//bool obstacleSort(const std::pair& o1, const std::pair& o2) +//{ +// return o1.first < o2.first ; +//} +// +////----------------------------------------------------------------- +// +//void Agent::updateAgentNeighbors() +//{ +// agentNeighbors_.clear() ; +// +//#ifdef SPATIAL_HASHING +// const std::vector& agents = sim_->envMap_.getNeighbors(this) ; +// const std::vector& neighborAgents ; +// sim_->envMap_.getOneRingNeighbors(this,neighborAgents) ; +//#else +// const std::vector& agents = sim_->envMap_.agentvect[part_.d] ; +// const std::vector& neighborAgents = sim_->envMap_.neighborAgentvect[part_.d] ; +//#endif +// +// float maxDist = 0.0f ; +// +// for (std::vector::const_iterator it = agents.begin(); it != agents.end(); ++it) +// { +// if ((*it)->alive) +// { +// if (*it != this) +// { +// float distSq = (getPosition() - (*it)->getPosition()).norm2() ; +// if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist) +// && distSq < neighborDistSq_) +// { +// if (distSq > maxDist) maxDist = distSq ; +// agentNeighbors_.push_back(std::make_pair(distSq, *it)) ; +// } +// } +// } +// } +// +// for (std::vector::const_iterator it = neighborAgents.begin(); it != neighborAgents.end(); ++it) +// { +// if ((*it)->alive) +// { +// float distSq = (getPosition() - (*it)->getPosition()).norm2() ; +// if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist) +// && distSq < neighborDistSq_) +// { +// if (distSq > maxDist) maxDist = distSq ; +// agentNeighbors_.push_back(std::make_pair(distSq, *it)) ; +// } +// } +// +// sim_->nbUpdates++ ; +// sim_->nearNeighbors += agentNeighbors_.size() ; +// sim_->totalNeighbors += agents.size() + neighborAgents.size() ; +// +// +// } +// if (agentNeighbors_.size() > maxNeighbors_) +// { +// sim_->nbSorts++ ; +// std::sort(agentNeighbors_.begin(), agentNeighbors_.end(), agentSort) ; +// } +//} +// +////----------------------------------------------------------------- +////----------------------------------------------------------------- +// +//void Agent::updateObstacleNeighbors() +//{ +// obstacleNeighbors_.clear() ; +// movingObstacleNeighbors_.clear() ; +// +//#ifdef SPATIAL_HASHING +// Geom::Vec2ui c = sim_->envMap_.obstaclePositionCell(pos) ; +// if(sim_->envMap_.ht_obstacles.hasData(c)) +// { +// const std::vector& obst = sim_->envMap_.ht_obstacles[c] ; +// for(std::vector::const_iterator it = obst.begin() ; it != obst.end() ; ++it) +// { +// float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, pos) ; +// if(distSq < rangeSq_) +// { +// if(Geom::testOrientation2D(pos, (*it)->p1, (*it)->p2) == Geom::RIGHT) +// obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ; +// } +// } +// } +//#else +// std::vector& obst = sim_->envMap_.obstvect[part_.d] ; +// std::vector& neighborObst = sim_->envMap_.neighborObstvect[part_.d] ; +// for(std::vector::const_iterator it = obst.begin() ; it != obst.end() ; ++it) +// { +// float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ; +// if (distSq < rangeSq_) +// { +// 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)) ; +// } +// } +// +// } +// } +// for(std::vector::const_iterator it = neighborObst.begin() ; it != neighborObst.end() ; ++it) +// { +// float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ; +// if(distSq < rangeSq_) +// { +// 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)) ; +// } +// } +// } +//#endif +// +//// if (obstacleNeighbors_.size() > maxNeighbors_) +//// std::sort(obstacleNeighbors_.begin(), obstacleNeighbors_.end(), obstacleSort) ; +//} +// +////----------------------------------------------------------------- +////----------------------------------------------------------------- +// +//void Agent::update() +//{ +// if (alive) +// { +// finalGoal = goals_[curGoal_] ; +// velocity_[0] = newVelocity_[0] ; +// velocity_[1] = newVelocity_[1] ; +//#ifdef SPATIAL_HASHING +// pos = pos + (velocity_ * sim->timeStep_) ; +//#else +// VEC3 target = part_.getPosition() + (velocity_ * sim_->timeStep_) ; +// +//#endif +// +// meanSpeed_ *= 3.0f ; +// meanSpeed_ += velocity_ ; +// meanSpeed_ /= 4.0f ; +// +//// meanPos_ *= 9.0f; +//// meanPos_ += part_.m_position; +//// meanPos_ /= 10.0f; +// +//#ifndef SPATIAL_HASHING +// part_.move(target) ; +//#endif +// } +//} +// +////----------------------------------------------------------------- +////----------------------------------------------------------------- +// +//void Agent::computeNewVelocity2() +//{ +// VEC3 centroid ; +// VEC3 c ; +// VEC3 vel ; +// +// float cohesion = 1.0f ; +// +// centroid.zero() ; +// c.zero() ; +// vel.zero() ; +// +// for (std::vector >::iterator it = obstacleNeighbors_.begin() ;it != obstacleNeighbors_.end() ; ++it) +// { +// } +// +// unsigned int nbA = 0 ; +// for (std::vector >::iterator it = agentNeighbors_.begin() ; +// it != agentNeighbors_.end() && nbA < maxNeighbors_ ; ++it, ++nbA) +// { +// VEC3 mPos = getPosition() ; +// VEC3 oPos = (*it).second->getPosition() ; +// centroid += oPos ; +// +// VEC3 pOp(oPos - mPos) ; +// if (pOp.norm2() <= radius_ * radius_ * maxSpeed_ * maxSpeed_ * 2.0f) +// { +// c -= pOp ; +//// prefVelocity_.zero(); +// } +// +// vel += (*it).second->prefVelocity_ ; +// } +// +// if (nbA > 0) +// { +// centroid /= nbA ; +// centroid -= getPosition() ; +// centroid /= 10 ; +// +// vel /= nbA ; +// vel -= prefVelocity_ ; +// vel /= 8 ; +// } +// +// newVelocity_ = prefVelocity_ + cohesion * centroid + c + vel ; +//} +// +// +// +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +////////comportement à modifier/////////////////////////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// +// +////----------------------------------------------------------------- +//// fonction d'évitement d'obstacles par les agents (calcul effectué +//// avant de lancer RVO2 pour garantir la non-collision entre agents) +//// En gros, on représente un obstacle : +//// soit par un ensemble de segments orientés dans le sens direct (sim_->avoidance==0), +//// soit par son centre (sim_->avoidance==1) +//// Cette fonction parcourt les obstacles et calcule la norme de la force globale de +//// répulsion. Ensuite, elle ajoute cette somme à l'objectif de façon +//// à ce que l'agent change sa trajectoire. +// +//void Agent::obstacle_priority(PFP::VEC3 * goalVector) +//{ +// float factor ; +// Obstacle* obst ; +// PFP::VEC3 x,y,vec ; +// PFP::VEC3 norm,sumNorm ; +// sumNorm.zero() ; +// +// // On parcourt les obstacles +// for(std::vector >::iterator it = movingObstacleNeighbors_.begin() ; +// it != movingObstacleNeighbors_.end() ; +// ++it) +// { +// // rangeSq = range * range (portee au carre) +// // les elements de l'iteration sont des obstacles. +// // chaque obstacle est une paire : float et *obstacle. +// // le float est la distance à l'obstacle et *obstacle +// // est un pointeur vers le segment. +// +// factor =(1.0f-(it->first/rangeSq_)) ; +// // En particulier ici, (*it).first (ou it->first) +// // represente la distance à l'agent (?) +// // factor est quelque chose qui faut 0 +// // quand l'agent commence à interagir avec l'obstacle +// // et qui faut 1 quand l'agent se trouve colle sur l'obstacle. +// +//// CGoGNout<second ; +// x=obst->p1 ; +// y=obst->p2 ; +// vec=y-x ; +// vec.normalize() ; +// norm.zero() ; +// +// if (sim_->avoidance==0)// avoids with normal of obstacle side +// { +// norm[0]=vec[1] ; +// norm[1]=-vec[0] ; +// } +// else if (sim_->avoidance==1) // avoids with direction from center of the obstacle // { // MovingObstacle * mo = obst->mo; // norm = this->part_.getPosition()-mo->center; // } - norm.normalize() ; - norm*=5.0f*factor*factor*factor*factor*factor*factor*factor*factor ; - // On met factor à la puissance 8 pour adoucir l'impact. - sumNorm+=norm ; - } - -// if (sumNorm.norm2()>1.0f) sumNorm.normalize(); -// if(sumNorm!=0)CGoGNout< maxSpeed_) - { - goalVector.normalize() ; - goalVector *= maxSpeed_; - } - - prefVelocity_ = goalVector ; -} - -//----------------------------------------------------------------- -//----------------------------------------------------------------- -//----------------------------------------------------------------- -// Search for the best new velocity. - -void Agent::computeNewVelocity() // RVO2 : évitement agents entres eux et avec les obstacles fixes + ajout obstacles mobiles -{ - obstacle_priority(&prefVelocity_);//prise en compte obstacles mobiles - - std::vector orcaLines_ ; - orcaLines_.reserve(obstacleNeighbors_.size() + agentNeighbors_.size()) ; - - float invTimeHorizonObst = 1.0f / timeHorizonObst_ ; - - // Create obstacle ORCA lines. - // Parcourir les obstacles - - for (std::vector >::iterator it = obstacleNeighbors_.begin() ; - it != obstacleNeighbors_.end() ; - ++it) - { - const Obstacle* obst = it->second ; - //?// WHATIS it->second ??? - - const VEC3 relativePosition1(obst->p1 - getPosition()) ; - const VEC3 relativePosition2(obst->p2 - getPosition()) ; - // Je devine que l'obstacle est projeté sur le sol en un - // ensemble de segments et que obst->p1 et obst->p2 sont - // les deux extremités de ce segment. - - // Check if velocity obstacle of obstacle is already taken care of by - // previously constructed obstacle ORCA lines. - - bool alreadyCovered = false ; - - for (unsigned int j = 0 ; j < orcaLines_.size() ; ++j) - { - if ( (det2D(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point, orcaLines_[j].direction) - - invTimeHorizonObst * radius_ >= -RVO_EPSILON) - && - (det2D(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point,orcaLines_[j].direction) - - invTimeHorizonObst * radius_ >= -RVO_EPSILON)) - { - alreadyCovered = true ; - break ; - } - } - - if (alreadyCovered) continue ; - - // Not yet covered. Check for collisions. - - const float distSq1 = relativePosition1.norm2() ; - const float distSq2 = relativePosition2.norm2() ; - - const float radiusSq = radius_ * radius_ ; - - const VEC3 obstacleVector(obst->p2 - obst->p1) ; - const VEC3 nextObstacleVector(obst->nextP - obst->p2) ; - const VEC3 previousObstacleVector(obst->p1 - obst->prevP) ; - - const float s = (-1.0f * relativePosition1 * obstacleVector) / obstacleVector.norm2() ; - const float distSqLine = (-1.0f * relativePosition1 - s * obstacleVector).norm2() ; - - Line line ; - - if (s < 0 && distSq1 <= radiusSq) - { - // Collision with left vertex. Ignore if non-convex. -// if (obstacle1->isConvex_) +//// 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 ; +// // On met factor à la puissance 8 pour adoucir l'impact. +// sumNorm+=norm ; +// } +// +//// if (sumNorm.norm2()>1.0f) sumNorm.normalize(); +//// if(sumNorm!=0)CGoGNout< maxSpeed_) +// { +// goalVector.normalize() ; +// goalVector *= maxSpeed_; +// } +// +// prefVelocity_ = goalVector ; +//} +// +////----------------------------------------------------------------- +////----------------------------------------------------------------- +////----------------------------------------------------------------- +//// Search for the best new velocity. +// +//void Agent::computeNewVelocity() // RVO2 : évitement agents entres eux et avec les obstacles fixes + ajout obstacles mobiles +//{ +// obstacle_priority(&prefVelocity_);//prise en compte obstacles mobiles +// +// std::vector orcaLines_ ; +// orcaLines_.reserve(obstacleNeighbors_.size() + agentNeighbors_.size()) ; +// +// float invTimeHorizonObst = 1.0f / timeHorizonObst_ ; +// +// // Create obstacle ORCA lines. +// // Parcourir les obstacles +// +// for (std::vector >::iterator it = obstacleNeighbors_.begin() ; +// it != obstacleNeighbors_.end() ; +// ++it) +// { +// const Obstacle* obst = it->second ; +// //?// WHATIS it->second ??? +// +// const VEC3 relativePosition1(obst->p1 - getPosition()) ; +// const VEC3 relativePosition2(obst->p2 - getPosition()) ; +// // Je devine que l'obstacle est projeté sur le sol en un +// // ensemble de segments et que obst->p1 et obst->p2 sont +// // les deux extremités de ce segment. +// +// // Check if velocity obstacle of obstacle is already taken care of by +// // previously constructed obstacle ORCA lines. +// +// bool alreadyCovered = false ; +// +// for (unsigned int j = 0 ; j < orcaLines_.size() ; ++j) +// { +// if ( (det2D(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point, orcaLines_[j].direction) +// - invTimeHorizonObst * radius_ >= -RVO_EPSILON) +// && +// (det2D(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point,orcaLines_[j].direction) +// - invTimeHorizonObst * radius_ >= -RVO_EPSILON)) +// { +// alreadyCovered = true ; +// break ; +// } +// } +// +// if (alreadyCovered) continue ; +// +// // Not yet covered. Check for collisions. +// +// const float distSq1 = relativePosition1.norm2() ; +// const float distSq2 = relativePosition2.norm2() ; +// +// const float radiusSq = radius_ * radius_ ; +// +// const VEC3 obstacleVector(obst->p2 - obst->p1) ; +// const VEC3 nextObstacleVector(obst->nextP - obst->p2) ; +// const VEC3 previousObstacleVector(obst->p1 - obst->prevP) ; +// +// const float s = (-1.0f * relativePosition1 * obstacleVector) / obstacleVector.norm2() ; +// const float distSqLine = (-1.0f * relativePosition1 - s * obstacleVector).norm2() ; +// +// Line line ; +// +// if (s < 0 && distSq1 <= radiusSq) +// { +// // Collision with left vertex. Ignore if non-convex. +//// if (obstacle1->isConvex_) +//// { +// line.point = VEC3(0) ; +// line.direction = VEC3(-relativePosition1[1], relativePosition1[0], 0) ; +// line.direction.normalize() ; +// orcaLines_.push_back(line) ; +//// } +// continue ; +// } +// else if (s > 1 && distSq2 <= radiusSq) +// { +// // Collision with right vertex. Ignore if non-convex +// // * or if it will be taken care of by neighboring obstacle +//// VEC3 nextObstacleVectorN(nextObstacleVector); +//// nextObstacleVectorN.normalize(); +//// if (//obstacle2->isConvex_ && +//// det2D(relativePosition2, nextObstacleVectorN +//// //obstacle2->unitDir_ +//// ) >= 0) +//// { +// line.point = VEC3(0) ; +// line.direction = VEC3(-relativePosition2[1], relativePosition2[0], 0) ; +// line.direction.normalize() ; +// orcaLines_.push_back(line) ; +//// } +// continue ; +// } +// else if (s >= 0 && s < 1 && distSqLine <= radiusSq) +// { +// // Collision with obstacle segment. +// line.point = VEC3(0) ; +// line.direction = VEC3(-1.0f * obstacleVector) ; +// line.direction.normalize() ; +// orcaLines_.push_back(line) ; +// continue ; +// } +// +// // No collision. +// // Compute legs. When obliquely viewed, both legs can come from a single +// // vertex. Legs extend cut-off line when nonconvex vertex. +// +// VEC3 leftLegDirection, rightLegDirection ; +// bool obst1EQobst2 = false ; +// bool obst2EQobst1 = false ; +// +// if (s < 0 && distSqLine <= radiusSq) +// { +// // Obstacle viewed obliquely so that left vertex +// // defines velocity obstacle. +// /// +//// if (!obstacle1->isConvex_) +//// { +//// // Ignore obstacle. +//// continue; +//// } +//// obst2 = obst1; +// obst2EQobst1 = true ; +// +// const float leg1 = std::sqrt(distSq1 - radiusSq) ; +// leftLegDirection = VEC3(relativePosition1[0] * leg1 - relativePosition1[1] * radius_, +// relativePosition1[0] * radius_ + relativePosition1[1] * leg1, 0) / distSq1 ; +// rightLegDirection = VEC3(relativePosition1[0] * leg1 + relativePosition1[1] * radius_, +// -relativePosition1[0] * radius_ + relativePosition1[1] * leg1, 0) / distSq1 ; +// } +// else if (s > 1 && distSqLine <= radiusSq) +// { +// // Obstacle viewed obliquely so that right vertex +// // defines velocity obstacle. +// // +//// if (!obstacle2->isConvex_) +//// { +//// // Ignore obstacle. +//// continue; +//// } +//// obst1 = obst2; +// obst1EQobst2 = true ; +// +// const float leg2 = std::sqrt(distSq2 - radiusSq) ; +// leftLegDirection = VEC3(relativePosition2[0] * leg2 - relativePosition2[1] * radius_, +// relativePosition2[0] * radius_ + relativePosition2[1] * leg2, 0) / distSq2 ; +// rightLegDirection = VEC3(relativePosition2[0] * leg2 + relativePosition2[1] * radius_, +// -relativePosition2[0] * radius_ + relativePosition2[1] * leg2, 0) / distSq2 ; +// } +// else +// { +// // Usual situation. +//// if (obstacle1->isConvex_) +//// { +// const float leg1 = std::sqrt(distSq1 - radiusSq) ; +// leftLegDirection = VEC3(relativePosition1[0] * leg1 - relativePosition1[1] * radius_, +// relativePosition1[0] * radius_ + relativePosition1[1] * leg1, 0) / distSq1 ; +//// } +//// else +//// { +//// // Left vertex non-convex; left leg extends cut-off line. +//// leftLegDirection = -obstacle1->unitDir_; +//// } +// +//// if (obstacle2->isConvex_) +//// { +// const float leg2 = std::sqrt(distSq2 - radiusSq) ; +// rightLegDirection = VEC3(relativePosition2[0] * leg2 + relativePosition2[1] * radius_, +// -relativePosition2[0] * radius_ + relativePosition2[1] * leg2, 0) / distSq2 ; +//// } +//// else +//// { +//// // Right vertex non-convex; right leg extends cut-off line. +//// rightLegDirection = obstacle1->unitDir_; +//// } +// } +// +// // Legs can never point into neighboring edge when convex vertex, +// // take cutoff-line of neighboring edge instead. If velocity projected on +// // "foreign" leg, no constraint is added. +// +// bool isLeftLegForeign = false ; +// bool isRightLegForeign = false ; +// +// VEC3 leftObstacle = obst1EQobst2 ? obstacleVector : previousObstacleVector ; +// leftObstacle.normalize() ; +// +// if (// obstacle1->isConvex_ && +// det2D(leftLegDirection, -1.0f * leftObstacle) > 0.0f) +// { +// // Left leg points into obstacle. +// leftLegDirection = -1.0f * leftObstacle ; +// isLeftLegForeign = true ; +// } +// +// VEC3 rightObstacle = obst2EQobst1 ? obstacleVector : nextObstacleVector ; +// rightObstacle.normalize() ; +// +// if ( +// det2D(rightLegDirection, rightObstacle) < 0.0f) +// { +// // Right leg points into obstacle. +// rightLegDirection = rightObstacle ; +// isRightLegForeign = true ; +// } +// +// // Compute cut-off centers. +// const VEC3 leftCutoff = invTimeHorizonObst +// * ((obst1EQobst2 ? obst->p2 : obst->p1) - getPosition()) ; +// const VEC3 rightCutoff = invTimeHorizonObst +// * ((obst2EQobst1 ? obst->p1 : obst->p2) - getPosition()) ; +// const VEC3 cutoffVec = rightCutoff - leftCutoff ; +// +// // Project current velocity on velocity obstacle. +// +// // Check if current velocity is projected on cutoff circles. +// const float t = (// obst1 == obst2 +// obst1EQobst2 ? 0.5f : ((velocity_ - leftCutoff) * cutoffVec) / cutoffVec.norm2()) ; +// const float tLeft = ((velocity_ - leftCutoff) * leftLegDirection) ; +// const float tRight = ((velocity_ - rightCutoff) * rightLegDirection) ; +// +// if ((t < 0.0f && tLeft < 0.0f) +// || (//obst1 == obst2 +// obst1EQobst2 && tLeft < 0.0f && tRight < 0.0f)) +// { +// // Project on left cut-off circle. +// VEC3 unitW = velocity_ - leftCutoff ; +// unitW.normalize() ; +// +// line.direction = VEC3(unitW[1], -unitW[0], 0) ; +// line.point = leftCutoff + radius_ * invTimeHorizonObst * unitW ; +// orcaLines_.push_back(line) ; +// continue ; +// } +// else if (t > 1.0f && tRight < 0.0f) +// { +// // Project on right cut-off circle. +// VEC3 unitW = velocity_ - rightCutoff ; +// unitW.normalize() ; +// +// line.direction = VEC3(unitW[1], -unitW[0], 0) ; +// line.point = rightCutoff + radius_ * invTimeHorizonObst * unitW ; +// orcaLines_.push_back(line) ; +// continue ; +// } +// +// // Project on left leg, right leg, or cut-off line, whichever is closest +// // to velocity. +// +// VEC3 v1 = velocity_ - (leftCutoff + t * cutoffVec) ; +// VEC3 v2 = velocity_ - (leftCutoff + tLeft * leftLegDirection) ; +// VEC3 v3 = velocity_ - (rightCutoff + tRight * rightLegDirection) ; +// const float distSqCutoff = +// (t < 0.0f || t > 1.0f || //obst1 == obst2 +// obst1EQobst2) ? std::numeric_limits::infinity() : v1.norm2() ; +// const float distSqLeft = +// (tLeft < 0.0f) ? std::numeric_limits::infinity() : v2.norm2() ; +// const float distSqRight = +// (tRight < 0.0f) ? std::numeric_limits::infinity() : v3.norm2() ; +// +// if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) +// { +// // Project on cut-off line. +// VEC3 dir = obst1EQobst2 ? nextObstacleVector : obstacleVector ; +// dir.normalize() ; +// line.direction = -1.0f * dir ; +// line.direction.normalize() ; +// line.point = leftCutoff +// + radius_ * invTimeHorizonObst * VEC3(-line.direction[1], line.direction[0], 0) ; +// orcaLines_.push_back(line) ; +// continue ; +// } +// else if (distSqLeft <= distSqRight) +// { +// // Project on left leg. +// if (isLeftLegForeign) continue ; +// +// line.direction = leftLegDirection ; +// line.point = leftCutoff +// + radius_ * invTimeHorizonObst * VEC3(-line.direction[1], line.direction[0], 0) ; +// orcaLines_.push_back(line) ; +// continue ; +// } +// else +// { +// // Project on right leg. +// if (isRightLegForeign) continue ; +// line.direction = -1.0f * rightLegDirection ; +// line.point = rightCutoff +// + radius_ * invTimeHorizonObst * VEC3(-line.direction[1], line.direction[0], 0) ; +// orcaLines_.push_back(line) ; +// continue ; +// } +// } +// +// const unsigned int numObstLines = orcaLines_.size() ; +// +// const float invTimeHorizon = 1.0f / timeHorizon_ ; +// +// // Create agent ORCA lines. +// unsigned int nbA = 0 ; +// for (std::vector >::iterator it = agentNeighbors_.begin() ; +// it != agentNeighbors_.end() && nbA < maxNeighbors_ ; ++it, ++nbA) +// { +// Agent* other = it->second ; +// +// const VEC3 relativePosition(other->getPosition() - getPosition()) ; +// const VEC3 relativeVelocity(velocity_ - other->velocity_) ; +// const float distSq = relativePosition.norm2() ; +// const float combinedRadius = radius_ + other->radius_ ; +// const float combinedRadiusSq = combinedRadius * combinedRadius ; +// +// Line line ; +// VEC3 u ; +// +// // No collision. +// if (distSq > combinedRadiusSq) +// { +// const VEC3 w(relativeVelocity - invTimeHorizon * relativePosition) ; +// const float wLengthSq = w.norm2() ; +// +// const float dotProduct1 = w * relativePosition ; +// +// // Project on cut-off circle +// if (dotProduct1 < 0.0f && dotProduct1 * dotProduct1 > combinedRadiusSq * wLengthSq) // { - line.point = VEC3(0) ; - line.direction = VEC3(-relativePosition1[1], relativePosition1[0], 0) ; - line.direction.normalize() ; - orcaLines_.push_back(line) ; +// const float wLength = std::sqrt(wLengthSq) ; +// const VEC3 unitW = w / wLength ; +// +// line.direction = VEC3(unitW[1], -unitW[0], 0) ; +// u = (combinedRadius * invTimeHorizon - wLength) * unitW ; // } - continue ; - } - else if (s > 1 && distSq2 <= radiusSq) - { - // Collision with right vertex. Ignore if non-convex - // * or if it will be taken care of by neighboring obstacle -// VEC3 nextObstacleVectorN(nextObstacleVector); -// nextObstacleVectorN.normalize(); -// if (//obstacle2->isConvex_ && -// det2D(relativePosition2, nextObstacleVectorN -// //obstacle2->unitDir_ -// ) >= 0) -// { - line.point = VEC3(0) ; - line.direction = VEC3(-relativePosition2[1], relativePosition2[0], 0) ; - line.direction.normalize() ; - orcaLines_.push_back(line) ; -// } - continue ; - } - else if (s >= 0 && s < 1 && distSqLine <= radiusSq) - { - // Collision with obstacle segment. - line.point = VEC3(0) ; - line.direction = VEC3(-1.0f * obstacleVector) ; - line.direction.normalize() ; - orcaLines_.push_back(line) ; - continue ; - } - - // No collision. - // Compute legs. When obliquely viewed, both legs can come from a single - // vertex. Legs extend cut-off line when nonconvex vertex. - - VEC3 leftLegDirection, rightLegDirection ; - bool obst1EQobst2 = false ; - bool obst2EQobst1 = false ; - - if (s < 0 && distSqLine <= radiusSq) - { - // Obstacle viewed obliquely so that left vertex - // defines velocity obstacle. - /// -// if (!obstacle1->isConvex_) -// { -// // Ignore obstacle. -// continue; -// } -// obst2 = obst1; - obst2EQobst1 = true ; - - const float leg1 = std::sqrt(distSq1 - radiusSq) ; - leftLegDirection = VEC3(relativePosition1[0] * leg1 - relativePosition1[1] * radius_, - relativePosition1[0] * radius_ + relativePosition1[1] * leg1, 0) / distSq1 ; - rightLegDirection = VEC3(relativePosition1[0] * leg1 + relativePosition1[1] * radius_, - -relativePosition1[0] * radius_ + relativePosition1[1] * leg1, 0) / distSq1 ; - } - else if (s > 1 && distSqLine <= radiusSq) - { - // Obstacle viewed obliquely so that right vertex - // defines velocity obstacle. - // -// if (!obstacle2->isConvex_) -// { -// // Ignore obstacle. -// continue; -// } -// obst1 = obst2; - obst1EQobst2 = true ; - - const float leg2 = std::sqrt(distSq2 - radiusSq) ; - leftLegDirection = VEC3(relativePosition2[0] * leg2 - relativePosition2[1] * radius_, - relativePosition2[0] * radius_ + relativePosition2[1] * leg2, 0) / distSq2 ; - rightLegDirection = VEC3(relativePosition2[0] * leg2 + relativePosition2[1] * radius_, - -relativePosition2[0] * radius_ + relativePosition2[1] * leg2, 0) / distSq2 ; - } - else - { - // Usual situation. -// if (obstacle1->isConvex_) -// { - const float leg1 = std::sqrt(distSq1 - radiusSq) ; - leftLegDirection = VEC3(relativePosition1[0] * leg1 - relativePosition1[1] * radius_, - relativePosition1[0] * radius_ + relativePosition1[1] * leg1, 0) / distSq1 ; -// } -// else -// { -// // Left vertex non-convex; left leg extends cut-off line. -// leftLegDirection = -obstacle1->unitDir_; -// } - -// if (obstacle2->isConvex_) -// { - const float leg2 = std::sqrt(distSq2 - radiusSq) ; - rightLegDirection = VEC3(relativePosition2[0] * leg2 + relativePosition2[1] * radius_, - -relativePosition2[0] * radius_ + relativePosition2[1] * leg2, 0) / distSq2 ; -// } -// else -// { -// // Right vertex non-convex; right leg extends cut-off line. -// rightLegDirection = obstacle1->unitDir_; -// } - } - - // Legs can never point into neighboring edge when convex vertex, - // take cutoff-line of neighboring edge instead. If velocity projected on - // "foreign" leg, no constraint is added. - - bool isLeftLegForeign = false ; - bool isRightLegForeign = false ; - - VEC3 leftObstacle = obst1EQobst2 ? obstacleVector : previousObstacleVector ; - leftObstacle.normalize() ; - - if (// obstacle1->isConvex_ && - det2D(leftLegDirection, -1.0f * leftObstacle) > 0.0f) - { - // Left leg points into obstacle. - leftLegDirection = -1.0f * leftObstacle ; - isLeftLegForeign = true ; - } - - VEC3 rightObstacle = obst2EQobst1 ? obstacleVector : nextObstacleVector ; - rightObstacle.normalize() ; - - if ( - det2D(rightLegDirection, rightObstacle) < 0.0f) - { - // Right leg points into obstacle. - rightLegDirection = rightObstacle ; - isRightLegForeign = true ; - } - - // Compute cut-off centers. - const VEC3 leftCutoff = invTimeHorizonObst - * ((obst1EQobst2 ? obst->p2 : obst->p1) - getPosition()) ; - const VEC3 rightCutoff = invTimeHorizonObst - * ((obst2EQobst1 ? obst->p1 : obst->p2) - getPosition()) ; - const VEC3 cutoffVec = rightCutoff - leftCutoff ; - - // Project current velocity on velocity obstacle. - - // Check if current velocity is projected on cutoff circles. - const float t = (// obst1 == obst2 - obst1EQobst2 ? 0.5f : ((velocity_ - leftCutoff) * cutoffVec) / cutoffVec.norm2()) ; - const float tLeft = ((velocity_ - leftCutoff) * leftLegDirection) ; - const float tRight = ((velocity_ - rightCutoff) * rightLegDirection) ; - - if ((t < 0.0f && tLeft < 0.0f) - || (//obst1 == obst2 - obst1EQobst2 && tLeft < 0.0f && tRight < 0.0f)) - { - // Project on left cut-off circle. - VEC3 unitW = velocity_ - leftCutoff ; - unitW.normalize() ; - - line.direction = VEC3(unitW[1], -unitW[0], 0) ; - line.point = leftCutoff + radius_ * invTimeHorizonObst * unitW ; - orcaLines_.push_back(line) ; - continue ; - } - else if (t > 1.0f && tRight < 0.0f) - { - // Project on right cut-off circle. - VEC3 unitW = velocity_ - rightCutoff ; - unitW.normalize() ; - - line.direction = VEC3(unitW[1], -unitW[0], 0) ; - line.point = rightCutoff + radius_ * invTimeHorizonObst * unitW ; - orcaLines_.push_back(line) ; - continue ; - } - - // Project on left leg, right leg, or cut-off line, whichever is closest - // to velocity. - - VEC3 v1 = velocity_ - (leftCutoff + t * cutoffVec) ; - VEC3 v2 = velocity_ - (leftCutoff + tLeft * leftLegDirection) ; - VEC3 v3 = velocity_ - (rightCutoff + tRight * rightLegDirection) ; - const float distSqCutoff = - (t < 0.0f || t > 1.0f || //obst1 == obst2 - obst1EQobst2) ? std::numeric_limits::infinity() : v1.norm2() ; - const float distSqLeft = - (tLeft < 0.0f) ? std::numeric_limits::infinity() : v2.norm2() ; - const float distSqRight = - (tRight < 0.0f) ? std::numeric_limits::infinity() : v3.norm2() ; - - if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) - { - // Project on cut-off line. - VEC3 dir = obst1EQobst2 ? nextObstacleVector : obstacleVector ; - dir.normalize() ; - line.direction = -1.0f * dir ; - line.direction.normalize() ; - line.point = leftCutoff - + radius_ * invTimeHorizonObst * VEC3(-line.direction[1], line.direction[0], 0) ; - orcaLines_.push_back(line) ; - continue ; - } - else if (distSqLeft <= distSqRight) - { - // Project on left leg. - if (isLeftLegForeign) continue ; - - line.direction = leftLegDirection ; - line.point = leftCutoff - + radius_ * invTimeHorizonObst * VEC3(-line.direction[1], line.direction[0], 0) ; - orcaLines_.push_back(line) ; - continue ; - } - else - { - // Project on right leg. - if (isRightLegForeign) continue ; - line.direction = -1.0f * rightLegDirection ; - line.point = rightCutoff - + radius_ * invTimeHorizonObst * VEC3(-line.direction[1], line.direction[0], 0) ; - orcaLines_.push_back(line) ; - continue ; - } - } - - const unsigned int numObstLines = orcaLines_.size() ; - - const float invTimeHorizon = 1.0f / timeHorizon_ ; - - // Create agent ORCA lines. - unsigned int nbA = 0 ; - for (std::vector >::iterator it = agentNeighbors_.begin() ; - it != agentNeighbors_.end() && nbA < maxNeighbors_ ; ++it, ++nbA) - { - Agent* other = it->second ; - - const VEC3 relativePosition(other->getPosition() - getPosition()) ; - const VEC3 relativeVelocity(velocity_ - other->velocity_) ; - const float distSq = relativePosition.norm2() ; - const float combinedRadius = radius_ + other->radius_ ; - const float combinedRadiusSq = combinedRadius * combinedRadius ; - - Line line ; - VEC3 u ; - - // No collision. - if (distSq > combinedRadiusSq) - { - const VEC3 w(relativeVelocity - invTimeHorizon * relativePosition) ; - const float wLengthSq = w.norm2() ; - - const float dotProduct1 = w * relativePosition ; - - // Project on cut-off circle - if (dotProduct1 < 0.0f && dotProduct1 * dotProduct1 > combinedRadiusSq * wLengthSq) - { - const float wLength = std::sqrt(wLengthSq) ; - const VEC3 unitW = w / wLength ; - - line.direction = VEC3(unitW[1], -unitW[0], 0) ; - u = (combinedRadius * invTimeHorizon - wLength) * unitW ; - } - // Project on legs - else - { - const float leg = std::sqrt(distSq - combinedRadiusSq) ; - - if (det2D(relativePosition, w) > 0.0f) // Project on left leg - line.direction = VEC3( - relativePosition[0] * leg - relativePosition[1] * combinedRadius, - relativePosition[0] * combinedRadius + relativePosition[1] * leg, 0) - / distSq ; - else - // Project on right leg - line.direction = -1.0f - * VEC3(relativePosition[0] * leg + relativePosition[1] * combinedRadius, - -relativePosition[0] * combinedRadius + relativePosition[1] * leg, 0) - / distSq ; - - const float dotProduct2 = relativeVelocity * line.direction ; - - u = dotProduct2 * line.direction - relativeVelocity ; - } - } - // Collision. - else - { - // Project on cut-off circle of time timeStep. - const float invTimeStep = 1.0f / sim_->timeStep_ ; - - // Vector from cutoff center to relative velocity. - const VEC3 w = relativeVelocity - invTimeStep * relativePosition ; - - const float wLength = w.norm() ; - const VEC3 unitW = w / wLength ; - - line.direction = VEC3(unitW[1], -unitW[0], 0) ; - u = (combinedRadius * invTimeStep - wLength) * unitW ; - } - - line.point = velocity_ + 0.5f * u ; - orcaLines_.push_back(line) ; - } - - unsigned int lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, newVelocity_) ; - - if (lineFail < orcaLines_.size()) linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_,newVelocity_) ; -} - -//----------------------------------------------------------------- - -bool Agent::linearProgram1(const std::vector& lines, unsigned int lineNo, float radius, const VEC3& optVelocity, bool directionOpt, VEC3& result) -{ - const float dotProduct = lines[lineNo].point * lines[lineNo].direction ; - const float discriminant = dotProduct * dotProduct + radius * radius - - lines[lineNo].point.norm2() ; - - // Max speed circle fully invalidates line lineNo. - if (discriminant < 0.0f) return false ; - - const float sqrtDiscriminant = std::sqrt(discriminant) ; - float tLeft = -dotProduct - sqrtDiscriminant ; - float tRight = -dotProduct + sqrtDiscriminant ; - - for (unsigned int i = 0 ; i < lineNo ; ++i) - { - const float denominator = det2D(lines[lineNo].direction, lines[i].direction) ; - const float numerator = det2D(lines[i].direction, lines[lineNo].point - lines[i].point) ; - - if (std::fabs(denominator) <= RVO_EPSILON) // Lines lineNo and i are (almost) parallel. - { - if (numerator < 0.0f) - return false ; - else - continue ; - } - - const float t = numerator / denominator ; - - if (denominator >= 0.0f) // Line i bounds line lineNo on the right - tRight = std::min(tRight, t) ; - else - // Line i bounds line lineNo on the left - tLeft = std::max(tLeft, t) ; - - if (tLeft > tRight) return false ; - } - - // Optimize direction. - if (directionOpt) - { - if (optVelocity * lines[lineNo].direction > 0.0f) // Take right extreme - result = lines[lineNo].point + tRight * lines[lineNo].direction ; - else - // Take left extreme - result = lines[lineNo].point + tLeft * lines[lineNo].direction ; - } - // Optimize closest point. - else - { - const float t = lines[lineNo].direction * (optVelocity - lines[lineNo].point) ; - - if (t < tLeft) - result = lines[lineNo].point + tLeft * lines[lineNo].direction ; - else if (t > tRight) - result = lines[lineNo].point + tRight * lines[lineNo].direction ; - else - result = lines[lineNo].point + t * lines[lineNo].direction ; - } - - return true ; -} - -//----------------------------------------------------------------- - -unsigned int Agent::linearProgram2(const std::vector& lines, float radius, const VEC3& optVelocity, bool directionOpt, VEC3& result) -{ - // Optimize direction. Note that the optimization velocity is of unit - // length in this case. - - if (directionOpt) - result = optVelocity * radius ; - // Optimize closest point and outside circle. - else if (optVelocity.norm2() > radius * radius) - { - VEC3 optVelocityN(optVelocity) ; - optVelocityN.normalize() ; - result = optVelocityN * radius ; - } - // Optimize closest point and inside circle. - else - result = optVelocity ; - - for (unsigned int i = 0 ; i < lines.size() ; ++i) - { - if (det2D(lines[i].direction, lines[i].point - result) > 0.0f) - { - // Result does not satisfy constraint i. Compute new optimal result. - const VEC3 tempResult(result) ; - if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) - { - result = tempResult ; - return i ; - } - } - } - - return lines.size() ; -} - -//----------------------------------------------------------------- - -void Agent::linearProgram3(const std::vector& lines, unsigned int numObstLines, - unsigned int beginLine, float radius, VEC3& result) -{ - float distance = 0.0f ; - - for (unsigned int i = beginLine ; i < lines.size() ; ++i) - { - if (det2D(lines[i].direction, lines[i].point - result) > distance) - { - // Result does not satisfy constraint of line i. - std::vector projLines(lines.begin(), lines.begin() + numObstLines) ; - - for (unsigned int j = numObstLines ; j < i ; ++j) - { - Line line ; - - float determinant = det2D(lines[i].direction, lines[j].direction) ; - - if (std::fabs(determinant) <= RVO_EPSILON) // Line i and line j are (almost) parallel. - { - if (lines[i].direction * lines[j].direction > 0.0f) - continue ; // Line i and line j point in the same direction. - else - line.point = 0.5f * (lines[i].point + lines[j].point) ;// Line i and line j point in opposite direction. - } - else - line.point = lines[i].point - + (det2D(lines[j].direction, lines[i].point - lines[j].point) / determinant) - * lines[i].direction ; - - line.direction = lines[j].direction - lines[i].direction ; - line.direction.normalize() ; - projLines.push_back(line) ; - } - - const VEC3 tempResult = result ; - - // This should in principle not happen. The result is by definition - // already in the feasible region of this linear program. If it fails, - // it is due to small floating point error, and the current result is - // kept. - - if (linearProgram2(projLines, radius, - VEC3(-lines[i].direction[1], lines[i].direction[0], 0.0f), true, - result) < projLines.size()) result = tempResult ; - - distance = det2D(lines[i].direction, lines[i].point - result) ; - } - } -} - +// // Project on legs +// else +// { +// const float leg = std::sqrt(distSq - combinedRadiusSq) ; +// +// if (det2D(relativePosition, w) > 0.0f) // Project on left leg +// line.direction = VEC3( +// relativePosition[0] * leg - relativePosition[1] * combinedRadius, +// relativePosition[0] * combinedRadius + relativePosition[1] * leg, 0) +// / distSq ; +// else +// // Project on right leg +// line.direction = -1.0f +// * VEC3(relativePosition[0] * leg + relativePosition[1] * combinedRadius, +// -relativePosition[0] * combinedRadius + relativePosition[1] * leg, 0) +// / distSq ; +// +// const float dotProduct2 = relativeVelocity * line.direction ; +// +// u = dotProduct2 * line.direction - relativeVelocity ; +// } +// } +// // Collision. +// else +// { +// // Project on cut-off circle of time timeStep. +// const float invTimeStep = 1.0f / sim_->timeStep_ ; +// +// // Vector from cutoff center to relative velocity. +// const VEC3 w = relativeVelocity - invTimeStep * relativePosition ; +// +// const float wLength = w.norm() ; +// const VEC3 unitW = w / wLength ; +// +// line.direction = VEC3(unitW[1], -unitW[0], 0) ; +// u = (combinedRadius * invTimeStep - wLength) * unitW ; +// } +// +// line.point = velocity_ + 0.5f * u ; +// orcaLines_.push_back(line) ; +// } +// +// unsigned int lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, newVelocity_) ; +// +// if (lineFail < orcaLines_.size()) linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_,newVelocity_) ; +//} +// +////----------------------------------------------------------------- +// +//bool Agent::linearProgram1(const std::vector& lines, unsigned int lineNo, float radius, const VEC3& optVelocity, bool directionOpt, VEC3& result) +//{ +// const float dotProduct = lines[lineNo].point * lines[lineNo].direction ; +// const float discriminant = dotProduct * dotProduct + radius * radius +// - lines[lineNo].point.norm2() ; +// +// // Max speed circle fully invalidates line lineNo. +// if (discriminant < 0.0f) return false ; +// +// const float sqrtDiscriminant = std::sqrt(discriminant) ; +// float tLeft = -dotProduct - sqrtDiscriminant ; +// float tRight = -dotProduct + sqrtDiscriminant ; +// +// for (unsigned int i = 0 ; i < lineNo ; ++i) +// { +// const float denominator = det2D(lines[lineNo].direction, lines[i].direction) ; +// const float numerator = det2D(lines[i].direction, lines[lineNo].point - lines[i].point) ; +// +// if (std::fabs(denominator) <= RVO_EPSILON) // Lines lineNo and i are (almost) parallel. +// { +// if (numerator < 0.0f) +// return false ; +// else +// continue ; +// } +// +// const float t = numerator / denominator ; +// +// if (denominator >= 0.0f) // Line i bounds line lineNo on the right +// tRight = std::min(tRight, t) ; +// else +// // Line i bounds line lineNo on the left +// tLeft = std::max(tLeft, t) ; +// +// if (tLeft > tRight) return false ; +// } +// +// // Optimize direction. +// if (directionOpt) +// { +// if (optVelocity * lines[lineNo].direction > 0.0f) // Take right extreme +// result = lines[lineNo].point + tRight * lines[lineNo].direction ; +// else +// // Take left extreme +// result = lines[lineNo].point + tLeft * lines[lineNo].direction ; +// } +// // Optimize closest point. +// else +// { +// const float t = lines[lineNo].direction * (optVelocity - lines[lineNo].point) ; +// +// if (t < tLeft) +// result = lines[lineNo].point + tLeft * lines[lineNo].direction ; +// else if (t > tRight) +// result = lines[lineNo].point + tRight * lines[lineNo].direction ; +// else +// result = lines[lineNo].point + t * lines[lineNo].direction ; +// } +// +// return true ; +//} +// +////----------------------------------------------------------------- +// +//unsigned int Agent::linearProgram2(const std::vector& lines, float radius, const VEC3& optVelocity, bool directionOpt, VEC3& result) +//{ +// // Optimize direction. Note that the optimization velocity is of unit +// // length in this case. +// +// if (directionOpt) +// result = optVelocity * radius ; +// // Optimize closest point and outside circle. +// else if (optVelocity.norm2() > radius * radius) +// { +// VEC3 optVelocityN(optVelocity) ; +// optVelocityN.normalize() ; +// result = optVelocityN * radius ; +// } +// // Optimize closest point and inside circle. +// else +// result = optVelocity ; +// +// for (unsigned int i = 0 ; i < lines.size() ; ++i) +// { +// if (det2D(lines[i].direction, lines[i].point - result) > 0.0f) +// { +// // Result does not satisfy constraint i. Compute new optimal result. +// const VEC3 tempResult(result) ; +// if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) +// { +// result = tempResult ; +// return i ; +// } +// } +// } +// +// return lines.size() ; +//} +// +////----------------------------------------------------------------- +// +//void Agent::linearProgram3(const std::vector& lines, unsigned int numObstLines, +// unsigned int beginLine, float radius, VEC3& result) +//{ +// float distance = 0.0f ; +// +// for (unsigned int i = beginLine ; i < lines.size() ; ++i) +// { +// if (det2D(lines[i].direction, lines[i].point - result) > distance) +// { +// // Result does not satisfy constraint of line i. +// std::vector projLines(lines.begin(), lines.begin() + numObstLines) ; +// +// for (unsigned int j = numObstLines ; j < i ; ++j) +// { +// Line line ; +// +// float determinant = det2D(lines[i].direction, lines[j].direction) ; +// +// if (std::fabs(determinant) <= RVO_EPSILON) // Line i and line j are (almost) parallel. +// { +// if (lines[i].direction * lines[j].direction > 0.0f) +// continue ; // Line i and line j point in the same direction. +// else +// line.point = 0.5f * (lines[i].point + lines[j].point) ;// Line i and line j point in opposite direction. +// } +// else +// line.point = lines[i].point +// + (det2D(lines[j].direction, lines[i].point - lines[j].point) / determinant) +// * lines[i].direction ; +// +// line.direction = lines[j].direction - lines[i].direction ; +// line.direction.normalize() ; +// projLines.push_back(line) ; +// } +// +// const VEC3 tempResult = result ; +// +// // This should in principle not happen. The result is by definition +// // already in the feasible region of this linear program. If it fails, +// // it is due to small floating point error, and the current result is +// // kept. +// +// if (linearProgram2(projLines, radius, +// VEC3(-lines[i].direction[1], lines[i].direction[0], 0.0f), true, +// result) < projLines.size()) result = tempResult ; +// +// distance = det2D(lines[i].direction, lines[i].point - result) ; +// } +// } +//} +// diff --git a/src/agent.orig.cpp b/src/agent.orig.cpp index f09f8d26da41fc0f829f680aff529e6ed31a5739..06d8fb79684dbe776577fed5394f448a8da2dcab 100644 --- a/src/agent.orig.cpp +++ b/src/agent.orig.cpp @@ -1,866 +1,866 @@ -#include "agent.h" -#include "simulator.h" - -unsigned int Agent::maxNeighbors_ = 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::rangeSq_ = range_ * range_ ; - -unsigned int Agent::cptAgent = 0 ; - -#ifdef SPATIAL_HASHING -Agent::Agent(Simulator* sim, const VEC3& position) : - pos(position), - curGoal_(-1), - velocity_(0), - newVelocity_(0), - prefVelocity_(0), - meanSpeed_(0), - sim_(sim), - alive(true) -{ - agentNeighbors_.reserve(maxNeighbors_* 2) ; - obstacleNeighbors_.reserve(maxNeighbors_* 2) ; - agentNo = cptAgent++ ; -} -#else -Agent::Agent(Simulator* sim, const VEC3& position, Dart d) : - part_(sim->envMap_.map, d, position, sim->envMap_.position), - curGoal_(-1), - velocity_(0), - newVelocity_(0), - prefVelocity_(0), - meanSpeed_(0), - sim_(sim), - alive(true) -{ - color1 = 1.0f ; - color2 = 0 ; - color3 = 0 ; - agentNeighbors_.reserve(maxNeighbors_ * 2) ; - obstacleNeighbors_.reserve(maxNeighbors_ * 2) ; - movingObstacleNeighbors_.reserve(maxNeighbors_ * 2) ; - agentNo = cptAgent++ ; -} -#endif - -VEC3 Agent::getPosition() -{ -#ifdef SPATIAL_HASHING - return pos ; -#else - return part_.getPosition() ; -#endif -} - -bool agentSort(const std::pair& a1, const std::pair& a2) -{ - return a1.first < a2.first ; -} - -bool obstacleSort(const std::pair& o1, const std::pair& o2) -{ - return o1.first < o2.first ; -} - -void Agent::updateAgentNeighbors() -{ - agentNeighbors_.clear() ; - -#ifdef SPATIAL_HASHING - const std::vector& agents = sim_->envMap_.getNeighbors(this) ; - const std::vector& neighborAgents ; - sim_->envMap_.getOneRingNeighbors(this,neighborAgents) ; -#else - const std::vector& agents = sim_->envMap_.agentvect[part_.d] ; - const std::vector& neighborAgents = sim_->envMap_.neighborAgentvect[part_.d] ; -#endif - - float maxDist = 0.0f ; - - for (std::vector::const_iterator it = agents.begin(); it != agents.end(); ++it) - { - if ((*it)->alive) - { - if (*it != this) - { - float distSq = (getPosition() - (*it)->getPosition()).norm2() ; - if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist) - && distSq < neighborDistSq_) - { - if (distSq > maxDist) maxDist = distSq ; - agentNeighbors_.push_back(std::make_pair(distSq, *it)) ; - } - } - } - } - - for (std::vector::const_iterator it = neighborAgents.begin(); it != neighborAgents.end(); ++it) - { - if ((*it)->alive) - { - float distSq = (getPosition() - (*it)->getPosition()).norm2() ; - if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist) - && distSq < neighborDistSq_) - { - if (distSq > maxDist) maxDist = distSq ; - agentNeighbors_.push_back(std::make_pair(distSq, *it)) ; - } - } - - sim_->nbUpdates++ ; - sim_->nearNeighbors += agentNeighbors_.size() ; - sim_->totalNeighbors += agents.size() + neighborAgents.size() ; - - - } - if (agentNeighbors_.size() > maxNeighbors_) - { - sim_->nbSorts++ ; - std::sort(agentNeighbors_.begin(), agentNeighbors_.end(), agentSort) ; - } -} - -void Agent::updateObstacleNeighbors() -{ - obstacleNeighbors_.clear() ; - movingObstacleNeighbors_.clear() ; - -#ifdef SPATIAL_HASHING - Geom::Vec2ui c = sim_->envMap_.obstaclePositionCell(pos) ; - if(sim_->envMap_.ht_obstacles.hasData(c)) - { - const std::vector& obst = sim_->envMap_.ht_obstacles[c] ; - for(std::vector::const_iterator it = obst.begin() ; it != obst.end() ; ++it) - { - float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, pos) ; - if(distSq < rangeSq_) - { - if(Geom::testOrientation2D(pos, (*it)->p1, (*it)->p2) == Geom::RIGHT) - obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ; - } - } - } -#else - std::vector& obst = sim_->envMap_.obstvect[part_.d] ; - std::vector& neighborObst = sim_->envMap_.neighborObstvect[part_.d] ; - for(std::vector::const_iterator it = obst.begin() ; it != obst.end() ; ++it) - { - float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ; - if (distSq < rangeSq_) - { - 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)) ; - } - } - - } - } - for(std::vector::const_iterator it = neighborObst.begin() ; it != neighborObst.end() ; ++it) - { - float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ; - if(distSq < rangeSq_) - { - 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)) ; - } - } - } -#endif - -// if (obstacleNeighbors_.size() > maxNeighbors_) -// std::sort(obstacleNeighbors_.begin(), obstacleNeighbors_.end(), obstacleSort) ; -} - -void Agent::update() -{ - if (alive) - { - finalGoal = goals_[curGoal_] ; - velocity_[0] = newVelocity_[0] ; - velocity_[1] = newVelocity_[1] ; -#ifdef SPATIAL_HASHING - pos = pos + (velocity_ * sim->timeStep_) ; -#else - VEC3 target = part_.getPosition() + (velocity_ * sim_->timeStep_) ; - -#endif - - meanSpeed_ *= 3.0f ; - meanSpeed_ += velocity_ ; - meanSpeed_ /= 4.0f ; - -// meanPos_ *= 9.0f; -// meanPos_ += part_.m_position; -// meanPos_ /= 10.0f; - -#ifndef SPATIAL_HASHING - part_.move(target) ; -#endif - } -} - -void Agent::computeNewVelocity2() -{ - VEC3 centroid ; - VEC3 c ; - VEC3 vel ; - - float cohesion = 1.0f ; - - centroid.zero() ; - c.zero() ; - vel.zero() ; - - for (std::vector >::iterator it = obstacleNeighbors_.begin() ;it != obstacleNeighbors_.end() ; ++it) - { - } - - unsigned int nbA = 0 ; - for (std::vector >::iterator it = agentNeighbors_.begin() ; - it != agentNeighbors_.end() && nbA < maxNeighbors_ ; ++it, ++nbA) - { - VEC3 mPos = getPosition() ; - VEC3 oPos = (*it).second->getPosition() ; - centroid += oPos ; - - VEC3 pOp(oPos - mPos) ; - if (pOp.norm2() <= radius_ * radius_ * maxSpeed_ * maxSpeed_ * 2.0f) - { - c -= pOp ; -// prefVelocity_.zero(); - } - - vel += (*it).second->prefVelocity_ ; - } - - if (nbA > 0) - { - centroid /= nbA ; - centroid -= getPosition() ; - centroid /= 10 ; - - vel /= nbA ; - vel -= prefVelocity_ ; - vel /= 8 ; - } - - newVelocity_ = prefVelocity_ + cohesion * centroid + c + vel ; -} - - - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -//////comportement à modifier/////////////////////////////////////////////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - -void Agent::obstacle_priority(PFP::VEC3 * goalVector) //fonction d'évitement d'obstacles par les agents (calcul effectué avant de lancer RVO2 pour garantir la non-collision entre agents) -{ - float factor ; - Obstacle* obst ; - PFP::VEC3 x,y,vec ; - PFP::VEC3 norm,sumNorm ; - sumNorm.zero() ; - - for(std::vector >::iterator it = movingObstacleNeighbors_.begin() ; - it != movingObstacleNeighbors_.end() ; - ++it) - { - factor =(1.0f-((*it).first/rangeSq_)) ; -// CGoGNout<p1 ; - y=obst->p2 ; - vec=y-x ; - vec.normalize() ; - norm.zero() ; - if (sim_->avoidance==0)// avoids with normal of obstacle side - { - norm[0]=vec[1] ; - norm[1]=-vec[0] ; - } - else if (sim_->avoidance==1) // avoids with direction from center of the obstacle - { - MovingObstacle * mo = obst->mo; - norm = this->part_.getPosition()-mo->center; - } -// else if (sim_->avoidance==2) // avoids with opposite direction of obstacle +//#include "agent.h" +//#include "simulator.h" +// +//unsigned int Agent::maxNeighbors_ = 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::rangeSq_ = range_ * range_ ; +// +//unsigned int Agent::cptAgent = 0 ; +// +//#ifdef SPATIAL_HASHING +//Agent::Agent(Simulator* sim, const VEC3& position) : +// pos(position), +// curGoal_(-1), +// velocity_(0), +// newVelocity_(0), +// prefVelocity_(0), +// meanSpeed_(0), +// sim_(sim), +// alive(true) +//{ +// agentNeighbors_.reserve(maxNeighbors_* 2) ; +// obstacleNeighbors_.reserve(maxNeighbors_* 2) ; +// agentNo = cptAgent++ ; +//} +//#else +//Agent::Agent(Simulator* sim, const VEC3& position, Dart d) : +// part_(sim->envMap_.map, d, position, sim->envMap_.position), +// curGoal_(-1), +// velocity_(0), +// newVelocity_(0), +// prefVelocity_(0), +// meanSpeed_(0), +// sim_(sim), +// alive(true) +//{ +// color1 = 1.0f ; +// color2 = 0 ; +// color3 = 0 ; +// agentNeighbors_.reserve(maxNeighbors_ * 2) ; +// obstacleNeighbors_.reserve(maxNeighbors_ * 2) ; +// movingObstacleNeighbors_.reserve(maxNeighbors_ * 2) ; +// agentNo = cptAgent++ ; +//} +//#endif +// +//VEC3 Agent::getPosition() +//{ +//#ifdef SPATIAL_HASHING +// return pos ; +//#else +// return part_.getPosition() ; +//#endif +//} +// +//bool agentSort(const std::pair& a1, const std::pair& a2) +//{ +// return a1.first < a2.first ; +//} +// +//bool obstacleSort(const std::pair& o1, const std::pair& o2) +//{ +// return o1.first < o2.first ; +//} +// +//void Agent::updateAgentNeighbors() +//{ +// agentNeighbors_.clear() ; +// +//#ifdef SPATIAL_HASHING +// const std::vector& agents = sim_->envMap_.getNeighbors(this) ; +// const std::vector& neighborAgents ; +// sim_->envMap_.getOneRingNeighbors(this,neighborAgents) ; +//#else +// const std::vector& agents = sim_->envMap_.agentvect[part_.d] ; +// const std::vector& neighborAgents = sim_->envMap_.neighborAgentvect[part_.d] ; +//#endif +// +// float maxDist = 0.0f ; +// +// for (std::vector::const_iterator it = agents.begin(); it != agents.end(); ++it) +// { +// if ((*it)->alive) +// { +// if (*it != this) +// { +// float distSq = (getPosition() - (*it)->getPosition()).norm2() ; +// if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist) +// && distSq < neighborDistSq_) +// { +// if (distSq > maxDist) maxDist = distSq ; +// agentNeighbors_.push_back(std::make_pair(distSq, *it)) ; +// } +// } +// } +// } +// +// for (std::vector::const_iterator it = neighborAgents.begin(); it != neighborAgents.end(); ++it) +// { +// if ((*it)->alive) +// { +// float distSq = (getPosition() - (*it)->getPosition()).norm2() ; +// if ((agentNeighbors_.size() < maxNeighbors_ || distSq < maxDist) +// && distSq < neighborDistSq_) +// { +// if (distSq > maxDist) maxDist = distSq ; +// agentNeighbors_.push_back(std::make_pair(distSq, *it)) ; +// } +// } +// +// sim_->nbUpdates++ ; +// sim_->nearNeighbors += agentNeighbors_.size() ; +// sim_->totalNeighbors += agents.size() + neighborAgents.size() ; +// +// +// } +// if (agentNeighbors_.size() > maxNeighbors_) +// { +// sim_->nbSorts++ ; +// std::sort(agentNeighbors_.begin(), agentNeighbors_.end(), agentSort) ; +// } +//} +// +//void Agent::updateObstacleNeighbors() +//{ +// obstacleNeighbors_.clear() ; +// movingObstacleNeighbors_.clear() ; +// +//#ifdef SPATIAL_HASHING +// Geom::Vec2ui c = sim_->envMap_.obstaclePositionCell(pos) ; +// if(sim_->envMap_.ht_obstacles.hasData(c)) +// { +// const std::vector& obst = sim_->envMap_.ht_obstacles[c] ; +// for(std::vector::const_iterator it = obst.begin() ; it != obst.end() ; ++it) +// { +// float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, pos) ; +// if(distSq < rangeSq_) +// { +// if(Geom::testOrientation2D(pos, (*it)->p1, (*it)->p2) == Geom::RIGHT) +// obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ; +// } +// } +// } +//#else +// std::vector& obst = sim_->envMap_.obstvect[part_.d] ; +// std::vector& neighborObst = sim_->envMap_.neighborObstvect[part_.d] ; +// for(std::vector::const_iterator it = obst.begin() ; it != obst.end() ; ++it) +// { +// float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ; +// if (distSq < rangeSq_) +// { +// 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)) ; +// } +// } +// +// } +// } +// for(std::vector::const_iterator it = neighborObst.begin() ; it != neighborObst.end() ; ++it) +// { +// float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ; +// if(distSq < rangeSq_) +// { +// 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)) ; +// } +// } +// } +//#endif +// +//// if (obstacleNeighbors_.size() > maxNeighbors_) +//// std::sort(obstacleNeighbors_.begin(), obstacleNeighbors_.end(), obstacleSort) ; +//} +// +//void Agent::update() +//{ +// if (alive) +// { +// finalGoal = goals_[curGoal_] ; +// velocity_[0] = newVelocity_[0] ; +// velocity_[1] = newVelocity_[1] ; +//#ifdef SPATIAL_HASHING +// pos = pos + (velocity_ * sim->timeStep_) ; +//#else +// VEC3 target = part_.getPosition() + (velocity_ * sim_->timeStep_) ; +// +//#endif +// +// meanSpeed_ *= 3.0f ; +// meanSpeed_ += velocity_ ; +// meanSpeed_ /= 4.0f ; +// +//// meanPos_ *= 9.0f; +//// meanPos_ += part_.m_position; +//// meanPos_ /= 10.0f; +// +//#ifndef SPATIAL_HASHING +// part_.move(target) ; +//#endif +// } +//} +// +//void Agent::computeNewVelocity2() +//{ +// VEC3 centroid ; +// VEC3 c ; +// VEC3 vel ; +// +// float cohesion = 1.0f ; +// +// centroid.zero() ; +// c.zero() ; +// vel.zero() ; +// +// for (std::vector >::iterator it = obstacleNeighbors_.begin() ;it != obstacleNeighbors_.end() ; ++it) +// { +// } +// +// unsigned int nbA = 0 ; +// for (std::vector >::iterator it = agentNeighbors_.begin() ; +// it != agentNeighbors_.end() && nbA < maxNeighbors_ ; ++it, ++nbA) +// { +// VEC3 mPos = getPosition() ; +// VEC3 oPos = (*it).second->getPosition() ; +// centroid += oPos ; +// +// VEC3 pOp(oPos - mPos) ; +// if (pOp.norm2() <= radius_ * radius_ * maxSpeed_ * maxSpeed_ * 2.0f) +// { +// c -= pOp ; +//// prefVelocity_.zero(); +// } +// +// vel += (*it).second->prefVelocity_ ; +// } +// +// if (nbA > 0) +// { +// centroid /= nbA ; +// centroid -= getPosition() ; +// centroid /= 10 ; +// +// vel /= nbA ; +// vel -= prefVelocity_ ; +// vel /= 8 ; +// } +// +// newVelocity_ = prefVelocity_ + cohesion * centroid + c + vel ; +//} +// +// +// +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +////////comportement à modifier/////////////////////////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// +//void Agent::obstacle_priority(PFP::VEC3 * goalVector) //fonction d'évitement d'obstacles par les agents (calcul effectué avant de lancer RVO2 pour garantir la non-collision entre agents) +//{ +// float factor ; +// Obstacle* obst ; +// PFP::VEC3 x,y,vec ; +// PFP::VEC3 norm,sumNorm ; +// sumNorm.zero() ; +// +// for(std::vector >::iterator it = movingObstacleNeighbors_.begin() ; +// it != movingObstacleNeighbors_.end() ; +// ++it) +// { +// factor =(1.0f-((*it).first/rangeSq_)) ; +//// CGoGNout<p1 ; +// y=obst->p2 ; +// vec=y-x ; +// vec.normalize() ; +// norm.zero() ; +// if (sim_->avoidance==0)// avoids with normal of obstacle side +// { +// norm[0]=vec[1] ; +// norm[1]=-vec[0] ; +// } +// else if (sim_->avoidance==1) // avoids with direction from center of the 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 ; - - } - -// if (sumNorm.norm2()>1.0f) sumNorm.normalize(); -// if(sumNorm!=0)CGoGNout< maxSpeed_) - { - goalVector.normalize() ; - goalVector *= maxSpeed_; - } - - prefVelocity_ = goalVector ; - -} - -/* Search for the best new velocity. */ -void Agent::computeNewVelocity() // RVO2 : évitement agents entres eux et avec les obstacles fixes + ajout obstacles mobiles -{ - obstacle_priority(&prefVelocity_);//prise en compte obstacles mobiles - - std::vector orcaLines_ ; - orcaLines_.reserve(obstacleNeighbors_.size() + agentNeighbors_.size()) ; - - float invTimeHorizonObst = 1.0f / timeHorizonObst_ ; - - /* Create obstacle ORCA lines. */ - for (std::vector >::iterator it = obstacleNeighbors_.begin() ; - it != obstacleNeighbors_.end() ; ++it) - { - - const Obstacle* obst = it->second ; - - const VEC3 relativePosition1(obst->p1 - getPosition()) ; - const VEC3 relativePosition2(obst->p2 - getPosition()) ; - - /* - * Check if velocity obstacle of obstacle is already taken care of by - * previously constructed obstacle ORCA lines. - */ - bool alreadyCovered = false ; - - for (unsigned int j = 0 ; j < orcaLines_.size() ; ++j) - { - if (det2D(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point, - orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON - && det2D(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point, - orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON) - { - alreadyCovered = true ; - break ; - } - } - - if (alreadyCovered) - continue ; - - /* Not yet covered. Check for collisions. */ - - const float distSq1 = relativePosition1.norm2() ; - const float distSq2 = relativePosition2.norm2() ; - - const float radiusSq = radius_ * radius_ ; - - const VEC3 obstacleVector(obst->p2 - obst->p1) ; - const VEC3 nextObstacleVector(obst->nextP - obst->p2) ; - const VEC3 previousObstacleVector(obst->p1 - obst->prevP) ; - - const float s = (-1.0f * relativePosition1 * obstacleVector) / obstacleVector.norm2() ; - const float distSqLine = (-1.0f * relativePosition1 - s * obstacleVector).norm2() ; - - Line line ; - - if (s < 0 && distSq1 <= radiusSq) - { - /* Collision with left vertex. Ignore if non-convex. */ -// if (obstacle1->isConvex_) +//// 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 ; +// +// } +// +//// if (sumNorm.norm2()>1.0f) sumNorm.normalize(); +//// if(sumNorm!=0)CGoGNout< maxSpeed_) +// { +// goalVector.normalize() ; +// goalVector *= maxSpeed_; +// } +// +// prefVelocity_ = goalVector ; +// +//} +// +///* Search for the best new velocity. */ +//void Agent::computeNewVelocity() // RVO2 : évitement agents entres eux et avec les obstacles fixes + ajout obstacles mobiles +//{ +// obstacle_priority(&prefVelocity_);//prise en compte obstacles mobiles +// +// std::vector orcaLines_ ; +// orcaLines_.reserve(obstacleNeighbors_.size() + agentNeighbors_.size()) ; +// +// float invTimeHorizonObst = 1.0f / timeHorizonObst_ ; +// +// /* Create obstacle ORCA lines. */ +// for (std::vector >::iterator it = obstacleNeighbors_.begin() ; +// it != obstacleNeighbors_.end() ; ++it) +// { +// +// const Obstacle* obst = it->second ; +// +// const VEC3 relativePosition1(obst->p1 - getPosition()) ; +// const VEC3 relativePosition2(obst->p2 - getPosition()) ; +// +// /* +// * Check if velocity obstacle of obstacle is already taken care of by +// * previously constructed obstacle ORCA lines. +// */ +// bool alreadyCovered = false ; +// +// for (unsigned int j = 0 ; j < orcaLines_.size() ; ++j) +// { +// if (det2D(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point, +// orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON +// && det2D(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point, +// orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON) +// { +// alreadyCovered = true ; +// break ; +// } +// } +// +// if (alreadyCovered) +// continue ; +// +// /* Not yet covered. Check for collisions. */ +// +// const float distSq1 = relativePosition1.norm2() ; +// const float distSq2 = relativePosition2.norm2() ; +// +// const float radiusSq = radius_ * radius_ ; +// +// const VEC3 obstacleVector(obst->p2 - obst->p1) ; +// const VEC3 nextObstacleVector(obst->nextP - obst->p2) ; +// const VEC3 previousObstacleVector(obst->p1 - obst->prevP) ; +// +// const float s = (-1.0f * relativePosition1 * obstacleVector) / obstacleVector.norm2() ; +// const float distSqLine = (-1.0f * relativePosition1 - s * obstacleVector).norm2() ; +// +// Line line ; +// +// if (s < 0 && distSq1 <= radiusSq) +// { +// /* Collision with left vertex. Ignore if non-convex. */ +//// if (obstacle1->isConvex_) +//// { +// line.point = VEC3(0) ; +// line.direction = VEC3(-relativePosition1[1], relativePosition1[0], 0) ; +// line.direction.normalize() ; +// orcaLines_.push_back(line) ; +//// } +// continue ; +// } +// else if (s > 1 && distSq2 <= radiusSq) +// { +// /* Collision with right vertex. Ignore if non-convex +// * or if it will be taken care of by neighboring obstacle */ +//// VEC3 nextObstacleVectorN(nextObstacleVector); +//// nextObstacleVectorN.normalize(); +//// if (/*obstacle2->isConvex_ && */det2D(relativePosition2, nextObstacleVectorN/*obstacle2->unitDir_*/) >= 0) +//// { +// line.point = VEC3(0) ; +// line.direction = VEC3(-relativePosition2[1], relativePosition2[0], 0) ; +// line.direction.normalize() ; +// orcaLines_.push_back(line) ; +//// } +// continue ; +// } +// else if (s >= 0 && s < 1 && distSqLine <= radiusSq) +// { +// /* Collision with obstacle segment. */ +// line.point = VEC3(0) ; +// line.direction = VEC3(-1.0f * obstacleVector) ; +// line.direction.normalize() ; +// orcaLines_.push_back(line) ; +// continue ; +// } +// +// /* +// * No collision. +// * Compute legs. When obliquely viewed, both legs can come from a single +// * vertex. Legs extend cut-off line when nonconvex vertex. +// */ +// +// VEC3 leftLegDirection, rightLegDirection ; +// bool obst1EQobst2 = false ; +// bool obst2EQobst1 = false ; +// +// if (s < 0 && distSqLine <= radiusSq) +// { +// /* +// * Obstacle viewed obliquely so that left vertex +// * defines velocity obstacle. +// */ +//// if (!obstacle1->isConvex_) +//// { +//// /* Ignore obstacle. */ +//// continue; +//// } +//// obst2 = obst1; +// obst2EQobst1 = true ; +// +// const float leg1 = std::sqrt(distSq1 - radiusSq) ; +// leftLegDirection = VEC3(relativePosition1[0] * leg1 - relativePosition1[1] * radius_, +// relativePosition1[0] * radius_ + relativePosition1[1] * leg1, 0) / distSq1 ; +// rightLegDirection = VEC3(relativePosition1[0] * leg1 + relativePosition1[1] * radius_, +// -relativePosition1[0] * radius_ + relativePosition1[1] * leg1, 0) / distSq1 ; +// } +// else if (s > 1 && distSqLine <= radiusSq) +// { +// /* +// * Obstacle viewed obliquely so that right vertex +// * defines velocity obstacle. +// */ +//// if (!obstacle2->isConvex_) +//// { +//// /* Ignore obstacle. */ +//// continue; +//// } +//// obst1 = obst2; +// obst1EQobst2 = true ; +// +// const float leg2 = std::sqrt(distSq2 - radiusSq) ; +// leftLegDirection = VEC3(relativePosition2[0] * leg2 - relativePosition2[1] * radius_, +// relativePosition2[0] * radius_ + relativePosition2[1] * leg2, 0) / distSq2 ; +// rightLegDirection = VEC3(relativePosition2[0] * leg2 + relativePosition2[1] * radius_, +// -relativePosition2[0] * radius_ + relativePosition2[1] * leg2, 0) / distSq2 ; +// } +// else +// { +// /* Usual situation. */ +//// if (obstacle1->isConvex_) +//// { +// const float leg1 = std::sqrt(distSq1 - radiusSq) ; +// leftLegDirection = VEC3(relativePosition1[0] * leg1 - relativePosition1[1] * radius_, +// relativePosition1[0] * radius_ + relativePosition1[1] * leg1, 0) / distSq1 ; +//// } +//// else +//// { +//// /* Left vertex non-convex; left leg extends cut-off line. */ +//// leftLegDirection = -obstacle1->unitDir_; +//// } +// +//// if (obstacle2->isConvex_) +//// { +// const float leg2 = std::sqrt(distSq2 - radiusSq) ; +// rightLegDirection = VEC3(relativePosition2[0] * leg2 + relativePosition2[1] * radius_, +// -relativePosition2[0] * radius_ + relativePosition2[1] * leg2, 0) / distSq2 ; +//// } +//// else +//// { +//// /* Right vertex non-convex; right leg extends cut-off line. */ +//// rightLegDirection = obstacle1->unitDir_; +//// } +// } +// +// /* +// * Legs can never point into neighboring edge when convex vertex, +// * take cutoff-line of neighboring edge instead. If velocity projected on +// * "foreign" leg, no constraint is added. +// */ +// +// bool isLeftLegForeign = false ; +// bool isRightLegForeign = false ; +// +// VEC3 leftObstacle = obst1EQobst2 ? obstacleVector : previousObstacleVector ; +// leftObstacle.normalize() ; +// +// if (/*obstacle1->isConvex_ && */det2D(leftLegDirection, -1.0f * leftObstacle) > 0.0f) +// { +// /* Left leg points into obstacle. */ +// leftLegDirection = -1.0f * leftObstacle ; +// isLeftLegForeign = true ; +// } +// +// VEC3 rightObstacle = obst2EQobst1 ? obstacleVector : nextObstacleVector ; +// rightObstacle.normalize() ; +// +// if (/*obstacle2->isConvex_ && */det2D(rightLegDirection, rightObstacle) < 0.0f) +// { +// /* Right leg points into obstacle. */ +// rightLegDirection = rightObstacle ; +// isRightLegForeign = true ; +// } +// +// /* Compute cut-off centers. */ +// const VEC3 leftCutoff = invTimeHorizonObst +// * ((obst1EQobst2 ? obst->p2 : obst->p1) - getPosition()) ; +// const VEC3 rightCutoff = invTimeHorizonObst +// * ((obst2EQobst1 ? obst->p1 : obst->p2) - getPosition()) ; +// const VEC3 cutoffVec = rightCutoff - leftCutoff ; +// +// /* Project current velocity on velocity obstacle. */ +// +// /* Check if current velocity is projected on cutoff circles. */ +// const float t = (/*obst1 == obst2*/ +// obst1EQobst2 ? 0.5f : ((velocity_ - leftCutoff) * cutoffVec) / cutoffVec.norm2()) ; +// const float tLeft = ((velocity_ - leftCutoff) * leftLegDirection) ; +// const float tRight = ((velocity_ - rightCutoff) * rightLegDirection) ; +// +// if ((t < 0.0f && tLeft < 0.0f) +// || (/*obst1 == obst2*/obst1EQobst2 && tLeft < 0.0f && tRight < 0.0f)) +// { +// /* Project on left cut-off circle. */ +// VEC3 unitW = velocity_ - leftCutoff ; +// unitW.normalize() ; +// +// line.direction = VEC3(unitW[1], -unitW[0], 0) ; +// line.point = leftCutoff + radius_ * invTimeHorizonObst * unitW ; +// orcaLines_.push_back(line) ; +// continue ; +// } +// else if (t > 1.0f && tRight < 0.0f) +// { +// /* Project on right cut-off circle. */ +// VEC3 unitW = velocity_ - rightCutoff ; +// unitW.normalize() ; +// +// line.direction = VEC3(unitW[1], -unitW[0], 0) ; +// line.point = rightCutoff + radius_ * invTimeHorizonObst * unitW ; +// orcaLines_.push_back(line) ; +// continue ; +// } +// +// /* +// * Project on left leg, right leg, or cut-off line, whichever is closest +// * to velocity. +// */ +// VEC3 v1 = velocity_ - (leftCutoff + t * cutoffVec) ; +// VEC3 v2 = velocity_ - (leftCutoff + tLeft * leftLegDirection) ; +// VEC3 v3 = velocity_ - (rightCutoff + tRight * rightLegDirection) ; +// const float distSqCutoff = +// (t < 0.0f || t > 1.0f || /*obst1 == obst2*/obst1EQobst2) ? std::numeric_limits::infinity() : v1.norm2() ; +// const float distSqLeft = +// (tLeft < 0.0f) ? std::numeric_limits::infinity() : v2.norm2() ; +// const float distSqRight = +// (tRight < 0.0f) ? std::numeric_limits::infinity() : v3.norm2() ; +// +// if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) +// { +// /* Project on cut-off line. */ +// VEC3 dir = obst1EQobst2 ? nextObstacleVector : obstacleVector ; +// dir.normalize() ; +// line.direction = -1.0f * dir ; +// line.direction.normalize() ; +// line.point = leftCutoff +// + radius_ * invTimeHorizonObst * VEC3(-line.direction[1], line.direction[0], 0) ; +// orcaLines_.push_back(line) ; +// continue ; +// } +// else if (distSqLeft <= distSqRight) +// { +// /* Project on left leg. */ +// if (isLeftLegForeign) continue ; +// +// line.direction = leftLegDirection ; +// line.point = leftCutoff +// + radius_ * invTimeHorizonObst * VEC3(-line.direction[1], line.direction[0], 0) ; +// orcaLines_.push_back(line) ; +// continue ; +// } +// else +// { +// /* Project on right leg. */ +// if (isRightLegForeign) continue ; +// line.direction = -1.0f * rightLegDirection ; +// line.point = rightCutoff +// + radius_ * invTimeHorizonObst * VEC3(-line.direction[1], line.direction[0], 0) ; +// orcaLines_.push_back(line) ; +// continue ; +// } +// } +// +// const unsigned int numObstLines = orcaLines_.size() ; +// +// const float invTimeHorizon = 1.0f / timeHorizon_ ; +// +// /* Create agent ORCA lines. */ +// unsigned int nbA = 0 ; +// for (std::vector >::iterator it = agentNeighbors_.begin() ; +// it != agentNeighbors_.end() && nbA < maxNeighbors_ ; ++it, ++nbA) +// { +// Agent* other = it->second ; +// +// const VEC3 relativePosition(other->getPosition() - getPosition()) ; +// const VEC3 relativeVelocity(velocity_ - other->velocity_) ; +// const float distSq = relativePosition.norm2() ; +// const float combinedRadius = radius_ + other->radius_ ; +// const float combinedRadiusSq = combinedRadius * combinedRadius ; +// +// Line line ; +// VEC3 u ; +// +// /* No collision. */ +// if (distSq > combinedRadiusSq) +// { +// const VEC3 w(relativeVelocity - invTimeHorizon * relativePosition) ; +// const float wLengthSq = w.norm2() ; +// +// const float dotProduct1 = w * relativePosition ; +// +// /* Project on cut-off circle */ +// if (dotProduct1 < 0.0f && dotProduct1 * dotProduct1 > combinedRadiusSq * wLengthSq) // { - line.point = VEC3(0) ; - line.direction = VEC3(-relativePosition1[1], relativePosition1[0], 0) ; - line.direction.normalize() ; - orcaLines_.push_back(line) ; +// const float wLength = std::sqrt(wLengthSq) ; +// const VEC3 unitW = w / wLength ; +// +// line.direction = VEC3(unitW[1], -unitW[0], 0) ; +// u = (combinedRadius * invTimeHorizon - wLength) * unitW ; // } - continue ; - } - else if (s > 1 && distSq2 <= radiusSq) - { - /* Collision with right vertex. Ignore if non-convex - * or if it will be taken care of by neighboring obstacle */ -// VEC3 nextObstacleVectorN(nextObstacleVector); -// nextObstacleVectorN.normalize(); -// if (/*obstacle2->isConvex_ && */det2D(relativePosition2, nextObstacleVectorN/*obstacle2->unitDir_*/) >= 0) -// { - line.point = VEC3(0) ; - line.direction = VEC3(-relativePosition2[1], relativePosition2[0], 0) ; - line.direction.normalize() ; - orcaLines_.push_back(line) ; -// } - continue ; - } - else if (s >= 0 && s < 1 && distSqLine <= radiusSq) - { - /* Collision with obstacle segment. */ - line.point = VEC3(0) ; - line.direction = VEC3(-1.0f * obstacleVector) ; - line.direction.normalize() ; - orcaLines_.push_back(line) ; - continue ; - } - - /* - * No collision. - * Compute legs. When obliquely viewed, both legs can come from a single - * vertex. Legs extend cut-off line when nonconvex vertex. - */ - - VEC3 leftLegDirection, rightLegDirection ; - bool obst1EQobst2 = false ; - bool obst2EQobst1 = false ; - - if (s < 0 && distSqLine <= radiusSq) - { - /* - * Obstacle viewed obliquely so that left vertex - * defines velocity obstacle. - */ -// if (!obstacle1->isConvex_) -// { -// /* Ignore obstacle. */ -// continue; -// } -// obst2 = obst1; - obst2EQobst1 = true ; - - const float leg1 = std::sqrt(distSq1 - radiusSq) ; - leftLegDirection = VEC3(relativePosition1[0] * leg1 - relativePosition1[1] * radius_, - relativePosition1[0] * radius_ + relativePosition1[1] * leg1, 0) / distSq1 ; - rightLegDirection = VEC3(relativePosition1[0] * leg1 + relativePosition1[1] * radius_, - -relativePosition1[0] * radius_ + relativePosition1[1] * leg1, 0) / distSq1 ; - } - else if (s > 1 && distSqLine <= radiusSq) - { - /* - * Obstacle viewed obliquely so that right vertex - * defines velocity obstacle. - */ -// if (!obstacle2->isConvex_) -// { -// /* Ignore obstacle. */ -// continue; -// } -// obst1 = obst2; - obst1EQobst2 = true ; - - const float leg2 = std::sqrt(distSq2 - radiusSq) ; - leftLegDirection = VEC3(relativePosition2[0] * leg2 - relativePosition2[1] * radius_, - relativePosition2[0] * radius_ + relativePosition2[1] * leg2, 0) / distSq2 ; - rightLegDirection = VEC3(relativePosition2[0] * leg2 + relativePosition2[1] * radius_, - -relativePosition2[0] * radius_ + relativePosition2[1] * leg2, 0) / distSq2 ; - } - else - { - /* Usual situation. */ -// if (obstacle1->isConvex_) -// { - const float leg1 = std::sqrt(distSq1 - radiusSq) ; - leftLegDirection = VEC3(relativePosition1[0] * leg1 - relativePosition1[1] * radius_, - relativePosition1[0] * radius_ + relativePosition1[1] * leg1, 0) / distSq1 ; -// } -// else -// { -// /* Left vertex non-convex; left leg extends cut-off line. */ -// leftLegDirection = -obstacle1->unitDir_; -// } - -// if (obstacle2->isConvex_) -// { - const float leg2 = std::sqrt(distSq2 - radiusSq) ; - rightLegDirection = VEC3(relativePosition2[0] * leg2 + relativePosition2[1] * radius_, - -relativePosition2[0] * radius_ + relativePosition2[1] * leg2, 0) / distSq2 ; -// } -// else -// { -// /* Right vertex non-convex; right leg extends cut-off line. */ -// rightLegDirection = obstacle1->unitDir_; -// } - } - - /* - * Legs can never point into neighboring edge when convex vertex, - * take cutoff-line of neighboring edge instead. If velocity projected on - * "foreign" leg, no constraint is added. - */ - - bool isLeftLegForeign = false ; - bool isRightLegForeign = false ; - - VEC3 leftObstacle = obst1EQobst2 ? obstacleVector : previousObstacleVector ; - leftObstacle.normalize() ; - - if (/*obstacle1->isConvex_ && */det2D(leftLegDirection, -1.0f * leftObstacle) > 0.0f) - { - /* Left leg points into obstacle. */ - leftLegDirection = -1.0f * leftObstacle ; - isLeftLegForeign = true ; - } - - VEC3 rightObstacle = obst2EQobst1 ? obstacleVector : nextObstacleVector ; - rightObstacle.normalize() ; - - if (/*obstacle2->isConvex_ && */det2D(rightLegDirection, rightObstacle) < 0.0f) - { - /* Right leg points into obstacle. */ - rightLegDirection = rightObstacle ; - isRightLegForeign = true ; - } - - /* Compute cut-off centers. */ - const VEC3 leftCutoff = invTimeHorizonObst - * ((obst1EQobst2 ? obst->p2 : obst->p1) - getPosition()) ; - const VEC3 rightCutoff = invTimeHorizonObst - * ((obst2EQobst1 ? obst->p1 : obst->p2) - getPosition()) ; - const VEC3 cutoffVec = rightCutoff - leftCutoff ; - - /* Project current velocity on velocity obstacle. */ - - /* Check if current velocity is projected on cutoff circles. */ - const float t = (/*obst1 == obst2*/ - obst1EQobst2 ? 0.5f : ((velocity_ - leftCutoff) * cutoffVec) / cutoffVec.norm2()) ; - const float tLeft = ((velocity_ - leftCutoff) * leftLegDirection) ; - const float tRight = ((velocity_ - rightCutoff) * rightLegDirection) ; - - if ((t < 0.0f && tLeft < 0.0f) - || (/*obst1 == obst2*/obst1EQobst2 && tLeft < 0.0f && tRight < 0.0f)) - { - /* Project on left cut-off circle. */ - VEC3 unitW = velocity_ - leftCutoff ; - unitW.normalize() ; - - line.direction = VEC3(unitW[1], -unitW[0], 0) ; - line.point = leftCutoff + radius_ * invTimeHorizonObst * unitW ; - orcaLines_.push_back(line) ; - continue ; - } - else if (t > 1.0f && tRight < 0.0f) - { - /* Project on right cut-off circle. */ - VEC3 unitW = velocity_ - rightCutoff ; - unitW.normalize() ; - - line.direction = VEC3(unitW[1], -unitW[0], 0) ; - line.point = rightCutoff + radius_ * invTimeHorizonObst * unitW ; - orcaLines_.push_back(line) ; - continue ; - } - - /* - * Project on left leg, right leg, or cut-off line, whichever is closest - * to velocity. - */ - VEC3 v1 = velocity_ - (leftCutoff + t * cutoffVec) ; - VEC3 v2 = velocity_ - (leftCutoff + tLeft * leftLegDirection) ; - VEC3 v3 = velocity_ - (rightCutoff + tRight * rightLegDirection) ; - const float distSqCutoff = - (t < 0.0f || t > 1.0f || /*obst1 == obst2*/obst1EQobst2) ? std::numeric_limits::infinity() : v1.norm2() ; - const float distSqLeft = - (tLeft < 0.0f) ? std::numeric_limits::infinity() : v2.norm2() ; - const float distSqRight = - (tRight < 0.0f) ? std::numeric_limits::infinity() : v3.norm2() ; - - if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) - { - /* Project on cut-off line. */ - VEC3 dir = obst1EQobst2 ? nextObstacleVector : obstacleVector ; - dir.normalize() ; - line.direction = -1.0f * dir ; - line.direction.normalize() ; - line.point = leftCutoff - + radius_ * invTimeHorizonObst * VEC3(-line.direction[1], line.direction[0], 0) ; - orcaLines_.push_back(line) ; - continue ; - } - else if (distSqLeft <= distSqRight) - { - /* Project on left leg. */ - if (isLeftLegForeign) continue ; - - line.direction = leftLegDirection ; - line.point = leftCutoff - + radius_ * invTimeHorizonObst * VEC3(-line.direction[1], line.direction[0], 0) ; - orcaLines_.push_back(line) ; - continue ; - } - else - { - /* Project on right leg. */ - if (isRightLegForeign) continue ; - line.direction = -1.0f * rightLegDirection ; - line.point = rightCutoff - + radius_ * invTimeHorizonObst * VEC3(-line.direction[1], line.direction[0], 0) ; - orcaLines_.push_back(line) ; - continue ; - } - } - - const unsigned int numObstLines = orcaLines_.size() ; - - const float invTimeHorizon = 1.0f / timeHorizon_ ; - - /* Create agent ORCA lines. */ - unsigned int nbA = 0 ; - for (std::vector >::iterator it = agentNeighbors_.begin() ; - it != agentNeighbors_.end() && nbA < maxNeighbors_ ; ++it, ++nbA) - { - Agent* other = it->second ; - - const VEC3 relativePosition(other->getPosition() - getPosition()) ; - const VEC3 relativeVelocity(velocity_ - other->velocity_) ; - const float distSq = relativePosition.norm2() ; - const float combinedRadius = radius_ + other->radius_ ; - const float combinedRadiusSq = combinedRadius * combinedRadius ; - - Line line ; - VEC3 u ; - - /* No collision. */ - if (distSq > combinedRadiusSq) - { - const VEC3 w(relativeVelocity - invTimeHorizon * relativePosition) ; - const float wLengthSq = w.norm2() ; - - const float dotProduct1 = w * relativePosition ; - - /* Project on cut-off circle */ - if (dotProduct1 < 0.0f && dotProduct1 * dotProduct1 > combinedRadiusSq * wLengthSq) - { - const float wLength = std::sqrt(wLengthSq) ; - const VEC3 unitW = w / wLength ; - - line.direction = VEC3(unitW[1], -unitW[0], 0) ; - u = (combinedRadius * invTimeHorizon - wLength) * unitW ; - } - /* Project on legs */ - else - { - const float leg = std::sqrt(distSq - combinedRadiusSq) ; - - if (det2D(relativePosition, w) > 0.0f) // Project on left leg - line.direction = VEC3( - relativePosition[0] * leg - relativePosition[1] * combinedRadius, - relativePosition[0] * combinedRadius + relativePosition[1] * leg, 0) - / distSq ; - else - // Project on right leg - line.direction = -1.0f - * VEC3(relativePosition[0] * leg + relativePosition[1] * combinedRadius, - -relativePosition[0] * combinedRadius + relativePosition[1] * leg, 0) - / distSq ; - - const float dotProduct2 = relativeVelocity * line.direction ; - - u = dotProduct2 * line.direction - relativeVelocity ; - } - } - /* Collision. */ - else - { - /* Project on cut-off circle of time timeStep. */ - const float invTimeStep = 1.0f / sim_->timeStep_ ; - - /* Vector from cutoff center to relative velocity. */ - const VEC3 w = relativeVelocity - invTimeStep * relativePosition ; - - const float wLength = w.norm() ; - const VEC3 unitW = w / wLength ; - - line.direction = VEC3(unitW[1], -unitW[0], 0) ; - u = (combinedRadius * invTimeStep - wLength) * unitW ; - } - - line.point = velocity_ + 0.5f * u ; - orcaLines_.push_back(line) ; - } - - unsigned int lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, - newVelocity_) ; - - if (lineFail < orcaLines_.size()) linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, - newVelocity_) ; -} - -bool Agent::linearProgram1(const std::vector& lines, unsigned int lineNo, float radius, - const VEC3& optVelocity, bool directionOpt, VEC3& result) -{ - const float dotProduct = lines[lineNo].point * lines[lineNo].direction ; - const float discriminant = dotProduct * dotProduct + radius * radius - - lines[lineNo].point.norm2() ; - - /* Max speed circle fully invalidates line lineNo. */ - if (discriminant < 0.0f) return false ; - - const float sqrtDiscriminant = std::sqrt(discriminant) ; - float tLeft = -dotProduct - sqrtDiscriminant ; - float tRight = -dotProduct + sqrtDiscriminant ; - - for (unsigned int i = 0 ; i < lineNo ; ++i) - { - const float denominator = det2D(lines[lineNo].direction, lines[i].direction) ; - const float numerator = det2D(lines[i].direction, lines[lineNo].point - lines[i].point) ; - - if (std::fabs(denominator) <= RVO_EPSILON) // Lines lineNo and i are (almost) parallel. - { - if (numerator < 0.0f) - return false ; - else - continue ; - } - - const float t = numerator / denominator ; - - if (denominator >= 0.0f) // Line i bounds line lineNo on the right - tRight = std::min(tRight, t) ; - else - // Line i bounds line lineNo on the left - tLeft = std::max(tLeft, t) ; - - if (tLeft > tRight) return false ; - } - - /* Optimize direction. */ - if (directionOpt) - { - if (optVelocity * lines[lineNo].direction > 0.0f) // Take right extreme - result = lines[lineNo].point + tRight * lines[lineNo].direction ; - else - // Take left extreme - result = lines[lineNo].point + tLeft * lines[lineNo].direction ; - } - /* Optimize closest point. */ - else - { - const float t = lines[lineNo].direction * (optVelocity - lines[lineNo].point) ; - - if (t < tLeft) - result = lines[lineNo].point + tLeft * lines[lineNo].direction ; - else if (t > tRight) - result = lines[lineNo].point + tRight * lines[lineNo].direction ; - else - result = lines[lineNo].point + t * lines[lineNo].direction ; - } - - return true ; -} - -unsigned int Agent::linearProgram2(const std::vector& lines, float radius, - const VEC3& optVelocity, bool directionOpt, VEC3& result) -{ - /* - * Optimize direction. Note that the optimization velocity is of unit - * length in this case. - */ - if (directionOpt) - result = optVelocity * radius ; - /* Optimize closest point and outside circle. */ - else if (optVelocity.norm2() > radius * radius) - { - VEC3 optVelocityN(optVelocity) ; - optVelocityN.normalize() ; - result = optVelocityN * radius ; - } - /* Optimize closest point and inside circle. */ - else - result = optVelocity ; - - for (unsigned int i = 0 ; i < lines.size() ; ++i) - { - if (det2D(lines[i].direction, lines[i].point - result) > 0.0f) - { - /* Result does not satisfy constraint i. Compute new optimal result. */ - const VEC3 tempResult(result) ; - if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) - { - result = tempResult ; - return i ; - } - } - } - - return lines.size() ; -} - -void Agent::linearProgram3(const std::vector& lines, unsigned int numObstLines, - unsigned int beginLine, float radius, VEC3& result) -{ - float distance = 0.0f ; - - for (unsigned int i = beginLine ; i < lines.size() ; ++i) - { - if (det2D(lines[i].direction, lines[i].point - result) > distance) - { - /* Result does not satisfy constraint of line i. */ - std::vector projLines(lines.begin(), lines.begin() + numObstLines) ; - - for (unsigned int j = numObstLines ; j < i ; ++j) - { - Line line ; - - float determinant = det2D(lines[i].direction, lines[j].direction) ; - - if (std::fabs(determinant) <= RVO_EPSILON) // Line i and line j are (almost) parallel. - { - if (lines[i].direction * lines[j].direction > 0.0f) - continue ; // Line i and line j point in the same direction. - else - line.point = 0.5f * (lines[i].point + lines[j].point) ;// Line i and line j point in opposite direction. - } - else - line.point = lines[i].point - + (det2D(lines[j].direction, lines[i].point - lines[j].point) / determinant) - * lines[i].direction ; - - line.direction = lines[j].direction - lines[i].direction ; - line.direction.normalize() ; - projLines.push_back(line) ; - } - - const VEC3 tempResult = result ; - - /* This should in principle not happen. The result is by definition - * already in the feasible region of this linear program. If it fails, - * it is due to small floating point error, and the current result is - * kept. - */ - if (linearProgram2(projLines, radius, - VEC3(-lines[i].direction[1], lines[i].direction[0], 0.0f), true, - result) < projLines.size()) result = tempResult ; - - distance = det2D(lines[i].direction, lines[i].point - result) ; - } - } -} - +// /* Project on legs */ +// else +// { +// const float leg = std::sqrt(distSq - combinedRadiusSq) ; +// +// if (det2D(relativePosition, w) > 0.0f) // Project on left leg +// line.direction = VEC3( +// relativePosition[0] * leg - relativePosition[1] * combinedRadius, +// relativePosition[0] * combinedRadius + relativePosition[1] * leg, 0) +// / distSq ; +// else +// // Project on right leg +// line.direction = -1.0f +// * VEC3(relativePosition[0] * leg + relativePosition[1] * combinedRadius, +// -relativePosition[0] * combinedRadius + relativePosition[1] * leg, 0) +// / distSq ; +// +// const float dotProduct2 = relativeVelocity * line.direction ; +// +// u = dotProduct2 * line.direction - relativeVelocity ; +// } +// } +// /* Collision. */ +// else +// { +// /* Project on cut-off circle of time timeStep. */ +// const float invTimeStep = 1.0f / sim_->timeStep_ ; +// +// /* Vector from cutoff center to relative velocity. */ +// const VEC3 w = relativeVelocity - invTimeStep * relativePosition ; +// +// const float wLength = w.norm() ; +// const VEC3 unitW = w / wLength ; +// +// line.direction = VEC3(unitW[1], -unitW[0], 0) ; +// u = (combinedRadius * invTimeStep - wLength) * unitW ; +// } +// +// line.point = velocity_ + 0.5f * u ; +// orcaLines_.push_back(line) ; +// } +// +// unsigned int lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, +// newVelocity_) ; +// +// if (lineFail < orcaLines_.size()) linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, +// newVelocity_) ; +//} +// +//bool Agent::linearProgram1(const std::vector& lines, unsigned int lineNo, float radius, +// const VEC3& optVelocity, bool directionOpt, VEC3& result) +//{ +// const float dotProduct = lines[lineNo].point * lines[lineNo].direction ; +// const float discriminant = dotProduct * dotProduct + radius * radius +// - lines[lineNo].point.norm2() ; +// +// /* Max speed circle fully invalidates line lineNo. */ +// if (discriminant < 0.0f) return false ; +// +// const float sqrtDiscriminant = std::sqrt(discriminant) ; +// float tLeft = -dotProduct - sqrtDiscriminant ; +// float tRight = -dotProduct + sqrtDiscriminant ; +// +// for (unsigned int i = 0 ; i < lineNo ; ++i) +// { +// const float denominator = det2D(lines[lineNo].direction, lines[i].direction) ; +// const float numerator = det2D(lines[i].direction, lines[lineNo].point - lines[i].point) ; +// +// if (std::fabs(denominator) <= RVO_EPSILON) // Lines lineNo and i are (almost) parallel. +// { +// if (numerator < 0.0f) +// return false ; +// else +// continue ; +// } +// +// const float t = numerator / denominator ; +// +// if (denominator >= 0.0f) // Line i bounds line lineNo on the right +// tRight = std::min(tRight, t) ; +// else +// // Line i bounds line lineNo on the left +// tLeft = std::max(tLeft, t) ; +// +// if (tLeft > tRight) return false ; +// } +// +// /* Optimize direction. */ +// if (directionOpt) +// { +// if (optVelocity * lines[lineNo].direction > 0.0f) // Take right extreme +// result = lines[lineNo].point + tRight * lines[lineNo].direction ; +// else +// // Take left extreme +// result = lines[lineNo].point + tLeft * lines[lineNo].direction ; +// } +// /* Optimize closest point. */ +// else +// { +// const float t = lines[lineNo].direction * (optVelocity - lines[lineNo].point) ; +// +// if (t < tLeft) +// result = lines[lineNo].point + tLeft * lines[lineNo].direction ; +// else if (t > tRight) +// result = lines[lineNo].point + tRight * lines[lineNo].direction ; +// else +// result = lines[lineNo].point + t * lines[lineNo].direction ; +// } +// +// return true ; +//} +// +//unsigned int Agent::linearProgram2(const std::vector& lines, float radius, +// const VEC3& optVelocity, bool directionOpt, VEC3& result) +//{ +// /* +// * Optimize direction. Note that the optimization velocity is of unit +// * length in this case. +// */ +// if (directionOpt) +// result = optVelocity * radius ; +// /* Optimize closest point and outside circle. */ +// else if (optVelocity.norm2() > radius * radius) +// { +// VEC3 optVelocityN(optVelocity) ; +// optVelocityN.normalize() ; +// result = optVelocityN * radius ; +// } +// /* Optimize closest point and inside circle. */ +// else +// result = optVelocity ; +// +// for (unsigned int i = 0 ; i < lines.size() ; ++i) +// { +// if (det2D(lines[i].direction, lines[i].point - result) > 0.0f) +// { +// /* Result does not satisfy constraint i. Compute new optimal result. */ +// const VEC3 tempResult(result) ; +// if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) +// { +// result = tempResult ; +// return i ; +// } +// } +// } +// +// return lines.size() ; +//} +// +//void Agent::linearProgram3(const std::vector& lines, unsigned int numObstLines, +// unsigned int beginLine, float radius, VEC3& result) +//{ +// float distance = 0.0f ; +// +// for (unsigned int i = beginLine ; i < lines.size() ; ++i) +// { +// if (det2D(lines[i].direction, lines[i].point - result) > distance) +// { +// /* Result does not satisfy constraint of line i. */ +// std::vector projLines(lines.begin(), lines.begin() + numObstLines) ; +// +// for (unsigned int j = numObstLines ; j < i ; ++j) +// { +// Line line ; +// +// float determinant = det2D(lines[i].direction, lines[j].direction) ; +// +// if (std::fabs(determinant) <= RVO_EPSILON) // Line i and line j are (almost) parallel. +// { +// if (lines[i].direction * lines[j].direction > 0.0f) +// continue ; // Line i and line j point in the same direction. +// else +// line.point = 0.5f * (lines[i].point + lines[j].point) ;// Line i and line j point in opposite direction. +// } +// else +// line.point = lines[i].point +// + (det2D(lines[j].direction, lines[i].point - lines[j].point) / determinant) +// * lines[i].direction ; +// +// line.direction = lines[j].direction - lines[i].direction ; +// line.direction.normalize() ; +// projLines.push_back(line) ; +// } +// +// const VEC3 tempResult = result ; +// +// /* This should in principle not happen. The result is by definition +// * already in the feasible region of this linear program. If it fails, +// * it is due to small floating point error, and the current result is +// * kept. +// */ +// if (linearProgram2(projLines, radius, +// VEC3(-lines[i].direction[1], lines[i].direction[0], 0.0f), true, +// result) < projLines.size()) result = tempResult ; +// +// distance = det2D(lines[i].direction, lines[i].point - result) ; +// } +// } +//} +// diff --git a/src/env_map.cpp b/src/env_map.cpp index 6debc74226fcc8a33f642a2a68d0a27f4dfc2453..fdb24ffd6818bae57eeb91e85d31a04d899054cf 100644 --- a/src/env_map.cpp +++ b/src/env_map.cpp @@ -699,9 +699,8 @@ void EnvMap::registerWallInFaces() { Dart dd2 = map.phi2(dd) ; Dart next = map.phi1(dd2) ; - Dart previous = map.phi_1(dd2) ; - Obstacle* o = new Obstacle(position[next], position[dd2], position[previous], - position[map.phi1(next)], next, dd2, NULL, 0) ; + + Obstacle* o = new Obstacle(position[next], position[dd2], NULL, 0) ; obstvect[dd2].push_back(o) ; } dd = map.phi1(dd) ; @@ -745,10 +744,8 @@ void EnvMap::registerObstaclesInFaces() { Dart dd2 = map.phi2(dd) ; Dart next = map.phi1(dd2) ; - Dart previous = map.phi_1(dd2) ; + Obstacle* o = new Obstacle(position[dd2], position[next], - position[previous], position[map.phi1(next)], - next, dd2, NULL, 0) ; obstvect[d].push_back(o) ; } @@ -798,9 +795,8 @@ void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbo // { Dart dd2 = map.phi2(dd) ; Dart next = map.phi1(dd2) ; - Dart previous = map.phi_1(dd2) ; - Obstacle* o = new Obstacle(position[dd2], position[next], position[previous], - position[map.phi1(next)], next, dd2, NULL, 0) ; + + Obstacle* o = new Obstacle(position[dd2], position[next], NULL, 0) ; obst.push_back(o) ; // } } diff --git a/src/moving_obstacle.cpp b/src/moving_obstacle.cpp index c96a8e4e8e8d9545e2c1c6fcd36531a0ee29272d..cc490f7b817d5dc577f48943f9113578fab4f330 100644 --- a/src/moving_obstacle.cpp +++ b/src/moving_obstacle.cpp @@ -9,7 +9,7 @@ //float MovingObstacle::neighborDistSq_ = 5.0f * 5.0f; -float MovingObstacle::maxSpeed_ = 2.0f; +float MovingObstacle::maxSpeed_ = 1.0f; float MovingObstacle::neighborDist_ = 10.0f ; float MovingObstacle::neighborDistSq_ = neighborDist_ * neighborDist_ ; float MovingObstacle::timeHorizonObst_ = 10.0f; @@ -144,6 +144,7 @@ VEC3 rotate2D(VEC3 pos1, VEC3 center, float angle) MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector pos, std::vector goals, bool rigid, bool spin,int curGoal, Dart dInside, ArticulatedObstacle * art, int indParent) : nbVertices(pos.size()), + nbParticles(pos.size()), center(0), index(ind), goals_(goals), @@ -166,12 +167,22 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector pos, s alpha(0.99) { + + for (unsigned int i = 0; i < nbVertices; ++i) + { + center += pos[i]; + } + center /= nbVertices; assert(pos.size() > 2); if(dInside==NIL) dInside = sim_->envMap_.getBelongingCell(pos[0]); - - unsigned int nbParticles = nbVertices; + if(rigid_) + { + pos.push_back(center); + pos.push_back(center); + nbParticles=nbVertices+2; + } // if(!rigid); // nbVertices +=1; //a center particle for the mass-spring @@ -186,9 +197,9 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector pos, s #endif - obstacles_ = new Obstacle*[nbVertices]; - belonging_cells = new std::vector[nbVertices]; - neighbor_cells = new std::vector[nbVertices]; + + belonging_cells = new std::vector[nbParticles]; + neighbor_cells = new std::vector[nbParticles]; position = map.addAttribute("position") ; normal = map.addAttribute("normal") ; @@ -202,12 +213,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector pos, s edgeLength = map.addAttribute("edgeLength") ; vertexAngle = map.addAttribute("vertexAngle") ; } - for (unsigned int i = 0; i < nbVertices; ++i) - { - center += pos[i]; - } - center /= nbVertices; - for (unsigned int i = 0; i < nbVertices; ++i) + + for (unsigned int i = 0; i < nbParticles; ++i) { #ifdef TWO_AND_HALF_DIM Dart d = dInside; @@ -226,21 +233,10 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector pos, s if(i==0) dDir = d; } - front=(parts_[0]->getPosition() + parts_[1]->getPosition()) / 2; -//////////////////////particule centrale pour masse ressort -// if(!rigid_) -// { -//#ifdef TWO_AND_HALF_DIM -// parts_[nbVertices] = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf(sim_->envMap_.map, dInside, center, sim_->envMap_.position); -//#else -//#ifdef SECURED -// parts_[nbVertices] = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DSecured(sim_->envMap_.map, dInside, center, sim_->envMap_.position); -//#else -// parts_[nbVertices] = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D(sim_->envMap_.map, dInside, center, sim_->envMap_.position); -//#endif -//#endif -// } + + + // M appartient à l'ellipse ssi MF1 + MF2 = sum_dist_foci est une constante // où F1 et F2 sont les deux foyers. @@ -370,21 +366,23 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector pos, s map.enableQuickTraversal(); dDir=dInside; - } #ifdef LINEAR shape_= new ShapeMatchingLinear(map,position,bord,beta); #else shape_= new ShapeMatchingQuadratic(map,position,bord,beta); #endif shape_->initialize(); + } + for (unsigned int i = 0; i < nbVertices; ++i) { Obstacle* o = new Obstacle(parts_[i]->getPosition(), parts_[(i + 1) % nbVertices]->getPosition(), - parts_[(i - 1 + nbVertices) % nbVertices]->getPosition(), - parts_[(i + 2) % nbVertices]->getPosition(), i, (i+1)% nbVertices, this, i); - obstacles_[i] = o; + this, i); + o->obst_stiffness_obst=0.5; + o->obst_stiffness_agent=100; + obstacles_.push_back(o); /////definition du troisieme point if (rigid_) o->p3=parts_[(i + 2) % nbVertices]->getPosition(); @@ -392,6 +390,32 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector pos, s sim_->envMap_.pushObstacleInCells(o); } + /////obstacles lointain + if(rigid_) + { + Obstacle* o = new Obstacle(parts_[nbVertices]->getPosition(), + parts_[nbVertices+1]->getPosition(), + this, nbVertices); + o->obst_stiffness_obst=100; + o->obst_stiffness_agent=1000; + obstacles_.push_back(o); + + // CGoGNout<<" obstacle :"<< i << " num : "<< o<envMap_.pushObstacleInCells(o); + + Obstacle* o2 = new Obstacle(parts_[nbVertices+1]->getPosition(), + parts_[nbVertices]->getPosition(), + this, nbVertices+1); + o2->obst_stiffness_obst=100; + o2->obst_stiffness_agent=1000; + obstacles_.push_back(o2); + + // CGoGNout<<" obstacle :"<< i << " num : "<< o<envMap_.pushObstacleInCells(o2); + + + /// fin ajout + } } @@ -480,6 +504,26 @@ void MovingObstacle::draw(bool showPath) m_ds->end(); m_ds->endList(); } + if(true)//show particles + { + + m_ds->newList(GL_COMPILE_AND_EXECUTE); + m_ds->begin(GL_LINE_STRIP); + + VEC3 col = Utils::color_map_BCGYR(float(index)/float(sim_->movingObstacles_.size())); + m_ds->color3f(col[0],col[1],col[2]); + + + for(unsigned int i = nbVertices ; i < nbParticles ; i++) + { + + m_ds->vertex(parts_[i]->getPosition()); + + } + + m_ds->end(); + m_ds->endList(); +} } VEC3 MovingObstacle::getDilatedPosition(unsigned int ind) @@ -1096,7 +1140,7 @@ void MovingObstacle::updateForces() //------------------------------------------------------------------------- // ARASH : A présent on calcule les interactions avec les autres obstacles. VEC3 norm; - double obst_stiffness = 0.5; // agent-obstacle interaction stiffness + int obst_power = 2 ; // the power to which elevate the agent-obstacle distance double obst_radius_infl, obst_radius_infl_buildings; @@ -1122,7 +1166,7 @@ void MovingObstacle::updateForces() VEC3 p1=obst->p1 ; VEC3 p2=obst->p2 ; - forces[dd] += computeForce(p,p1,p2,obst_radius_infl,obst_power,obst_stiffness,normFace); + forces[dd] += computeForce(p,p1,p2,obst_radius_infl,obst_power,obst->obst_stiffness_obst,normFace); } // Evitement d'obstacles fixes @@ -1134,7 +1178,7 @@ void MovingObstacle::updateForces() VEC3 p1=obst->p2 ; VEC3 p2=obst->p1 ; - forces[dd] += computeForce(p,p1,p2,obst_radius_infl_buildings,fixed_obst_factor*obst_power,fixed_obst_factor*obst_stiffness,normFace); + forces[dd] += computeForce(p,p1,p2,obst_radius_infl_buildings,fixed_obst_factor*obst_power,fixed_obst_factor*obst->obst_stiffness_obst,normFace); } d = map.phi_1(d); @@ -1250,7 +1294,11 @@ void MovingObstacle::applyForces() { velocity[d] += (forces[d]-0.01*velocity[d]) * sim_->timeStep_ ; - +// if (velocity[d].norm2() > maxSpeed_) +// { +// velocity[d].normalize() ; +// velocity[d] *= maxSpeed_; +// } position[d] += (velocity[d] * sim_->timeStep_); position [map.phi_1(map.phi2(d))]+=(0.5*velocity[d] * sim_->timeStep_); // PFP::VEC3 normal = CGoGN::Algo::Surface::Geometry::faceNormal(sim_->envMap_.map, parts_[i]->d, sim_->envMap_.position); @@ -1323,17 +1371,20 @@ void MovingObstacle::updateRegistration() position[map.phi_1(d)] = position[d]+normal; } } + + //////ajouts evitement lointain + parts_[nbVertices]->move(center); + parts_[nbVertices+1]->move(center+100*velocity_); + + + for (unsigned int i = 0; i < nbVertices; ++i) { // CGoGNout << "avant une etape : Obstacle "<< i << CGoGNendl; Obstacle* o = obstacles_[i]; - o->p1 = getDilatedPosition(i); - o->p2 = getDilatedPosition((i+1) % nbVertices); - - - o->prevP = getDilatedPosition((i - 1 ) % nbVertices); - o->nextP = getDilatedPosition((i + 2 ) % nbVertices); - if (rigid_) o->p3=getDilatedPosition((i + 2 ) % nbVertices); + o->p1 = parts_[i]->getPosition(); + o->p2 = parts_[(i + 1 ) % nbVertices]->getPosition(); + if (rigid_) o->p3=parts_[(i + 2 ) % nbVertices]->getPosition(); Dart d1 = parts_[i]->d; Dart d2 = parts_[(i+1)%nbVertices]->d; @@ -1354,6 +1405,28 @@ void MovingObstacle::updateRegistration() // CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl; } + Obstacle* o = obstacles_[nbVertices]; + o->p1 = parts_[nbVertices]->getPosition(); + o->p2 = parts_[nbVertices+1]->getPosition(); + Dart d1 = parts_[nbVertices]->d; + Dart d2 = parts_[nbVertices+1]->d; + + if(!((sim_->envMap_.map.sameFace(d1,d2))&& (parts_[nbVertices]->crossCell==CGoGN::Algo::Surface::MovingObjects::NO_CROSS && parts_[nbVertices+1]->crossCell==CGoGN::Algo::Surface::MovingObjects::NO_CROSS))) + { + sim_->envMap_.popAndPushObstacleInCells(o,nbVertices); + } + Obstacle* o2 = obstacles_[nbVertices+1]; + o2->p1 = parts_[nbVertices+1]->getPosition(); + o2->p2 = parts_[nbVertices]->getPosition(); + d1 = parts_[nbVertices+1]->d; + d2 = parts_[nbVertices]->d; + + if(!((sim_->envMap_.map.sameFace(d1,d2))&& (parts_[nbVertices+1]->crossCell==CGoGN::Algo::Surface::MovingObjects::NO_CROSS && parts_[nbVertices]->crossCell==CGoGN::Algo::Surface::MovingObjects::NO_CROSS))) + { + sim_->envMap_.popAndPushObstacleInCells(o2,nbVertices+1); + } + + /////affichage du general_belonging // CGoGNout<< CGoGNendl; // CGoGNout << "General : "; @@ -1408,8 +1481,15 @@ void resetPartSubdiv(Obstacle* o) if (mo != NULL) { unsigned int n =o->index; - unsigned int n2 =(n+1)%(mo->nbVertices); - + /// ajout obstacles lointains + unsigned int n2; + switch (n-(mo->nbVertices)) + { + case 0 : n2=n+1; break; + case 1 : n2=n-1;break; + default : n2 =(n+1)%(mo->nbVertices); break; + } + //////// VEC3 pos = mo->parts_[n]->getPosition(); VEC3 pos2 = mo->parts_[n2]->getPosition(); mo->parts_[n]->CGoGN::Algo::MovingObjects::ParticleBase::move(Algo::Surface::Geometry::faceCentroid(mo->sim_->envMap_.map, mo->parts_[n]->d, mo->sim_->envMap_.position)) ; @@ -1417,7 +1497,7 @@ void resetPartSubdiv(Obstacle* o) mo->parts_[n]->setState(FACE) ; mo->parts_[n]->move(pos) ; - mo->parts_[n2]->CGoGN::Algo::MovingObjects::ParticleBase::move(Algo::Surface::Geometry::faceCentroid(mo->sim_->envMap_.map, mo->parts_[n]->d, mo->sim_->envMap_.position)) ; + mo->parts_[n2]->CGoGN::Algo::MovingObjects::ParticleBase::move(Algo::Surface::Geometry::faceCentroid(mo->sim_->envMap_.map, mo->parts_[n2]->d, mo->sim_->envMap_.position)) ; mo->parts_[n2]->setState(FACE) ; mo->parts_[n2]->move(pos2) ; @@ -1433,7 +1513,15 @@ void resetObstPartInFace(Obstacle* o, Dart d1) if (mo != NULL) { unsigned int n =o->index; - unsigned int n2 =(n+1)%(mo->nbVertices); + /// ajout obstacles lointains + unsigned int n2; + switch (n-(mo->nbVertices)) + { + case 0 : n2=n+1; break; + case 1 : n2=n-1;break; + default : n2 =(n+1)%(mo->nbVertices); break; + } + //////// VEC3 pos1 = mo->parts_[n]->getPosition(); VEC3 pos2 = mo->parts_[n2]->getPosition(); // if (Algo::Surface::Geometry::isPointInConvexFace2D (mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos1, true)) @@ -1478,11 +1566,20 @@ void resetPart(Obstacle * o, Dart d1) { int n =o->index; + /// ajout obstacles lointains + unsigned int n2; + switch (n-(mo->nbVertices)) + { + case 0 : n2=n+1; break; + case 1 : n2=n-1;break; + default : n2 =(n+1)%(mo->nbVertices); break; + } + //////// if (mo->parts_[n]->d == mo->sim_->envMap_.map.phi1(d1)) mo->parts_[n]->d = d1; - if (mo->parts_[(n+1)%mo->nbVertices]->d == mo->sim_->envMap_.map.phi1(d1)) - mo->parts_[(n+1)%mo->nbVertices]->d = d1; + if (mo->parts_[n2]->d == mo->sim_->envMap_.map.phi1(d1)) + mo->parts_[n2]->d = d1; if(n==0) mo->dDir = mo->parts_[n]->d; diff --git a/src/simulator.cpp b/src/simulator.cpp index 045385735310074fadd3c4a275432a9d27cc2655..8c67048f7cc6b44cfdfe6cc094d778dd61e230c6 100644 --- a/src/simulator.cpp +++ b/src/simulator.cpp @@ -20,7 +20,7 @@ Simulator::Simulator(unsigned int config, unsigned int minS, unsigned int nbAgen timeStep_(0.5f), #else // timeStep_(config > 2 ? 0.01f : 0.25f), - timeStep_(0.01f), + timeStep_(0.25f), #endif globalTime_(0.0f), diff --git a/src/viewer.cpp b/src/viewer.cpp index c0cf7b1fdb123c37b7d3c4fbd19940a292272fd9..c18c7602f48b4fda51d2960eac4765c28b98c025 100644 --- a/src/viewer.cpp +++ b/src/viewer.cpp @@ -803,8 +803,8 @@ void SocialAgents::cb_redraw() MovingObstacle * mo =simulator.movingObstacles_[i]; if (drawEnvTopo) { - unsigned int n = 0; - if(nnbVertices) + unsigned int n = mo->nbVertices; + if(nnbParticles) { for (std::vector::iterator it = mo->belonging_cells[n].begin(); it != mo->belonging_cells[n].end(); ++it) {