diff --git a/include/env_generator.hpp b/include/env_generator.hpp index e35339b86a8baeb8555074f358ea2b0dabb24eba..b7f88b8431428ae31c234f7a7548e70f366415c8 100644 --- a/include/env_generator.hpp +++ b/include/env_generator.hpp @@ -81,7 +81,7 @@ void generateCity(EnvMap& envMap, unsigned int nbBuildings) #ifndef TWO_AND_HALF_DIM generateBuilding(envMap, d, (1 + (rand() % 3)) * 50.0f, rand() % 4) ; #else - generateBuilding(envMap, d, (1 + (rand() % 3)) * 10.0f, rand() % 3) ; + generateBuilding(envMap, d, (1 + (rand() % 3)) * 40.0f, rand() % 3) ; #endif --nbBuilding ; @@ -379,7 +379,8 @@ Dart generateBuilding(EnvMap& envMap, Dart d, float height, unsigned int buildin Dart next = map.phi1(dd) ; Dart previous = map.phi_1(dd) ; - Obstacle* o = new Obstacle(position[next], position[dd],position[map.phi1(next)] , position[previous] , NIL, NIL, NULL, 0); + Obstacle* o = new Obstacle(position[dd], position[next], position[previous], + position[map.phi1(next)], NIL, NIL, NULL, 0); #ifdef SPATIAL_HASHING VEC3 ov = o->p2 - o->p1 ; diff --git a/include/env_map.h b/include/env_map.h index a630381cbc0450be86dd07dd24c3a2d65947a3ec..4a40c8414f55f0e30248370f3527e4014753410e 100644 --- a/include/env_map.h +++ b/include/env_map.h @@ -104,6 +104,7 @@ public: void refine() ; void coarse() ; void updateMap() ; + int testOrientation(VEC3 p, VEC3 p1, VEC3 p2, Dart d); void resetAgentInFace(Agent* agent) ; #endif diff --git a/include/moving_obstacle.h b/include/moving_obstacle.h index 4c04ff57f846d4bd5b74bab9c994a6889f03fbc3..20b33810bd9ffdbe29ab3eecf74c2d496d372683 100644 --- a/include/moving_obstacle.h +++ b/include/moving_obstacle.h @@ -14,7 +14,7 @@ #include "Algo/MovingObjects/particle_cell_2D_memo.h" #endif -//#define EXPORTING_BOXES +#define EXPORTING_BOXES #ifdef EXPORTING_BOXES #include "Algo/Render/GL2/mapRender.h" @@ -28,7 +28,7 @@ using namespace std; PFP::VEC3 rotate2D (PFP::VEC3 pos1, PFP::VEC3 center, float angle); - +VEC3 computeForce(VEC3 p, VEC3 p1, VEC3 p2, float obst_radius_infl, float obst_power, float obst_stiffness, VEC3 normFace); float get_angle (PFP::VEC3 v1, PFP::VEC3 v2); PFP::VEC3 get_center (ArticulatedObstacle * art, int index); class Simulator ; diff --git a/src/agent.cpp b/src/agent.cpp index 50928b3a6a23b110b15466d9e71d8d26acdd8622..bb4620efbe8c71b6be8bc993dd05abc802ae9135 100644 --- a/src/agent.cpp +++ b/src/agent.cpp @@ -356,7 +356,7 @@ void Agent::updateObstacleNeighbors() if ((obstacleNeighbors_.size() < maxMovingObstacles_|| distSq < maxDistObst) && distSq < rangeSq_) { -// if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT) +// if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1) { if (distSq > maxDistObst) @@ -376,7 +376,7 @@ void Agent::updateObstacleNeighbors() if ((movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst) && distSq < rangeSq_) { -// if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT) +// if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1) { if (distSq > maxDistMovingObst) @@ -398,7 +398,7 @@ void Agent::updateObstacleNeighbors() if ((obstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistObst) &&distSq < rangeSq_) { -// if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT) +// if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1) { if (distSq > maxDistObst) maxDistObst = distSq ; @@ -414,7 +414,7 @@ void Agent::updateObstacleNeighbors() if ((movingObstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistMovingObst) &&distSq < rangeSq_) { -// if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT) +// if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1) { if (distSq > maxDistMovingObst) maxDistMovingObst = distSq ; @@ -684,7 +684,7 @@ void Agent::computePrefVelocity() // Si l'agent arrive à proximité de l'objectif, // alors il passe à l'objectif suivant - if (goalDist2 < radius_*radius_*100.0f) + if (goalDist2 < radius_*radius_*30.0f) { curGoal_ = (curGoal_ + 1) % goals_.size() ; goalVector = goals_[curGoal_] - getPosition() ; @@ -745,6 +745,7 @@ static int moAppend(MovingObstacle **moving_obstacles, int nb_mos, int max, Movi void Agent::computeNewVelocity() { // The objective is to compute the sum of forces exerted on the agent. + VEC3 normFace = Algo::Surface::Geometry::faceNormal(sim_->envMap_.map, part_.d, sim_->envMap_.position); double collision_softening_factor; float ag_ambient_damping = 10.0; @@ -826,7 +827,7 @@ void Agent::computeNewVelocity() collision_softening_factor = pow(1-sum_of_dists/rest_sum_of_dists,obst_power); force_value = obst_stiffness*collision_softening_factor*(rest_sum_of_dists - sum_of_dists); VEC3 v_obst = p2 - p1; - VEC3 normal = VEC3(v_obst[1],-v_obst[0],0); + VEC3 normal = normFace ^v_obst; // Ajouter une composante tangentielle normal += v_obst * ((d1-d2)/(5*sum_of_dists)); // Le facteur 5 est là seulement pour diminuer l'influence de la composante tangentielle @@ -873,7 +874,7 @@ void Agent::computeNewVelocity() nobst=0; // double fixed_obst_stiffness = 50000.0; // agent-obstacle interaction stiffness - double fixed_obst_stiffness = 50000.0; // agent-obstacle interaction stiffness + double fixed_obst_stiffness = 5000.0; // agent-obstacle interaction stiffness // double fixed_obst_damping = 1.0; // agent-obstacle interaction damping int fixed_obst_power = 1; // the power to which elevate the agent-obstacle distance Obstacle* fixed_obst ; @@ -885,7 +886,7 @@ void Agent::computeNewVelocity() double dist = it->first; // cerr << "nobst=" << nobst << "dist=" << dist << endl; // double effective_range = 50*range_; - double effective_range = 10*range_; + double effective_range = 1*range_; float force_value=0.0; if(dist < effective_range) { @@ -900,11 +901,8 @@ void Agent::computeNewVelocity() VEC3 p2=fixed_obst->p2 ; vec=p2-p1; vec.normalize(); - norm.zero(); - - norm[0]=vec[1] ; - norm[1]=-vec[0] ; - forces += force_value * norm; + norm= normFace ^vec; + forces -= force_value * norm; // cerr << "obstacles fixes : nobst=" << nobst << " dist=" << dist << " force=" << force_value*norm << endl; nobst++; diff --git a/src/env_map.cpp b/src/env_map.cpp index ff0e568671b4e0f08446636e9eb6186a7a3629da..9100cf9129e987d7b85831bffe02e27673ab7d22 100644 --- a/src/env_map.cpp +++ b/src/env_map.cpp @@ -1459,6 +1459,23 @@ bool EnvMap::isInsideFace3D(VEC3 p, Dart d) return true; } +int EnvMap::testOrientation(VEC3 p, VEC3 p1, VEC3 p2, Dart d) +{ + VEC3 normal = Algo::Surface::Geometry::faceNormal(map, d, position); + VEC3 vObst = p2 - p1; + + VEC3 dir = normal ^vObst; + + VEC3 test = p - p1; + + float res = test * dir; + + if(res <0) return 1; + if(res>0) return 0; + return -1; + + +} #ifdef SPATIAL_HASHING Geom::Vec2ui EnvMap::agentPositionCell(Agent* a) diff --git a/src/moving_obstacle.cpp b/src/moving_obstacle.cpp index c801b269fe66377c83cb8ea3a929552df887a49d..4552f39179b18b9c7662c13cb024b8fffbd23caa 100644 --- a/src/moving_obstacle.cpp +++ b/src/moving_obstacle.cpp @@ -615,10 +615,12 @@ void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance p if (/*(movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst) &&*/ distSq < distance_detectionSq) { - -// if (distSq > maxDistMovingObst) -// maxDistMovingObst = distSq ; - movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ; +// if (sim_->envMap_.testOrientation(parts_[nbVertices]->getPosition(), (*it)->p1, (*it)->p2, parts_[nbVertices]->d) == 1) + { + // if (distSq > maxDistMovingObst) + // maxDistMovingObst = distSq ; + movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ; + } @@ -654,10 +656,12 @@ void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance p if (/*(movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst) &&*/ distSq < distance_detectionSq) { - -// if (distSq > maxDistMovingObst) -// maxDistMovingObst = distSq ; - movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ; +// if (sim_->envMap_.testOrientation(parts_[nbVertices]->getPosition(), (*it)->p1, (*it)->p2, parts_[nbVertices]->d) == 1) + { + // if (distSq > maxDistMovingObst) + // maxDistMovingObst = distSq ; + movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ; + } } } } @@ -784,7 +788,7 @@ void MovingObstacle::initForces() //------------------------------------------------------------- -VEC3 computeForce(VEC3 p, VEC3 p1, VEC3 p2, float obst_radius_infl, float obst_power, float obst_stiffness) +VEC3 computeForce(VEC3 p, VEC3 p1, VEC3 p2, float obst_radius_infl, float obst_power, float obst_stiffness, VEC3 normFace) { double force_value=0.0; double longueur2 = (p1-p2).norm2(); @@ -798,7 +802,7 @@ VEC3 computeForce(VEC3 p, VEC3 p1, VEC3 p2, float obst_radius_infl, float obst_p double collision_softening_factor = pow(1-sum_of_dists/rest_sum_of_dists,obst_power); force_value = obst_stiffness*collision_softening_factor*(rest_sum_of_dists - sum_of_dists); VEC3 v_obst = p2 - p1; - VEC3 normal = VEC3(v_obst[1],-v_obst[0],0); + VEC3 normal = normFace^v_obst; // Ajouter une composante tangentielle normal += v_obst * ((d1-d2)/sum_of_dists); @@ -925,8 +929,10 @@ void MovingObstacle::updateForces() if(sim_->config==0) obst_radius_infl = 100.; // scenario 0 else - obst_radius_infl = 30.; // scenario 1 et 3 + obst_radius_infl = 10.; // scenario 1 et 3 + + PFP::VEC3 normFace = CGoGN::Algo::Surface::Geometry::faceNormal(sim_->envMap_.map, parts_[nbVertices]->d, sim_->envMap_.position); do { Dart dd = map.phi1(d); VEC3 p = position[dd]+(velocity[dd] * sim_->timeStep_); @@ -940,19 +946,19 @@ void MovingObstacle::updateForces() VEC3 p1=obst->p1 ; VEC3 p2=obst->p2 ; - forces[dd] += computeForce(p,p1,p2,obst_radius_infl,obst_power,obst_stiffness); + forces[dd] += computeForce(p,p1,p2,obst_radius_infl,obst_power,obst_stiffness,normFace); } - // Evitement d'obstacles fixes + // Evitement d'obstacles fixes for(std::vector >::iterator it = obstacleNeighbors_.begin() ; it != obstacleNeighbors_.end() ; ++it) { Obstacle * obst = it->second; - VEC3 p1=obst->p1 ; - VEC3 p2=obst->p2 ; + VEC3 p1=obst->p2 ; + VEC3 p2=obst->p1 ; - forces[dd] += computeForce(p,p1,p2,obst_radius_infl,obst_power,10*obst_stiffness); + forces[dd] += computeForce(p,p1,p2,obst_radius_infl,obst_power,1*obst_stiffness,normFace); } d = map.phi<21>(d); @@ -1337,7 +1343,7 @@ void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atte float goalDist2 = goalVector.norm2() ; - if (goalDist2 < 1000.0f) + if (goalDist2 < (gravity_dist*gravity_dist) ) { curGoal_ = (curGoal_ + 1) % goals_.size() ; goalVector = goals_[curGoal_] - center ; diff --git a/src/simulator.cpp b/src/simulator.cpp index f0f5f1c41a25c7868c70edf8fa380c97f455787b..66150f7ee088a6d13d52ae7478af353d2cb9059d 100644 --- a/src/simulator.cpp +++ b/src/simulator.cpp @@ -1111,12 +1111,12 @@ void Simulator::addPathsToAgents() d = envMap_.map.phi1(d); } -// VEC3 dest = Algo::Geometry::faceCentroid(envMap_.map, *it, envMap_.position); + VEC3 dest = Algo::Surface::Geometry::faceCentroid(envMap_.map, *it, envMap_.position); - VEC3 dest = envMap_.position[d] + envMap_.position[envMap_.map.phi1(d)] ; - dest /= 2.0f ; - dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0); - dest[2] = 0 ; +// VEC3 dest = envMap_.position[d] + envMap_.position[envMap_.map.phi1(d)] ; +// dest /= 2.0f ; +// dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0); +// dest[2] = 0 ; agents_[i]->goals_.push_back(dest) ; } @@ -1148,11 +1148,11 @@ void Simulator::addPathsToAgents() d = envMap_.map.phi1(d); } -// VEC3 dest = Algo::Geometry::faceCentroid(envMap_.map, *it, envMap_.position); - VEC3 dest = envMap_.position[d] + envMap_.position[envMap_.map.phi1(d)] ; - dest /= 2.0f ; - dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0); - dest[2] = 0 ; + VEC3 dest = Algo::Surface::Geometry::faceCentroid(envMap_.map, *it, envMap_.position); +// VEC3 dest = envMap_.position[d] + envMap_.position[envMap_.map.phi1(d)] ; +// dest /= 2.0f ; +// dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0); +// dest[2] = 0 ; agents_[i]->goals_.push_back(dest) ; } @@ -1164,11 +1164,11 @@ void Simulator::addPathsToAgents() for (std::vector::iterator it = path.begin() ; it != path.end() ; ++it) { -// VEC3 dest = Algo::Geometry::faceCentroid(envMap_.map, *it, envMap_.position); - VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ; - dest /= 2.0f ; - dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0); - dest[2] = 0 ; + VEC3 dest = Algo::Surface::Geometry::faceCentroid(envMap_.map, *it, envMap_.position); +// VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ; +// dest /= 2.0f ; +// dest += VEC3(5.0f*rand()/float(RAND_MAX),5.0f*rand()/float(RAND_MAX),0); +// dest[2] = 0 ; agents_[i]->goals_.push_back(dest) ; } diff --git a/src/viewer.cpp b/src/viewer.cpp index dc271eff6ee248172ef13b2c418f6ba667e768a3..2873fe31a01f46b3ca08194e92c0d1ee2bff7201 100644 --- a/src/viewer.cpp +++ b/src/viewer.cpp @@ -664,6 +664,28 @@ void SocialAgents::cb_redraw() if (drawAgents) { + if(drawAgentsPath) + { + for (unsigned int i=0; i< simulator.agents_.size(); ++i) + { + Agent * ag = simulator.agents_[i]; + m_ds->newList(GL_COMPILE_AND_EXECUTE); + m_ds->begin(GL_LINE_STRIP); + + VEC3 col = Utils::color_map_BCGYR(float(ag->agentNo)/float(simulator.agents_.size())); + m_ds->color3f(col[0],col[1],col[2]); + m_ds->vertex(ag->getPosition()); + for(unsigned int i = 0 ; i < ag->goals_.size() ; i++) + { + m_ds->vertex(ag->goals_[(ag->curGoal_+i)%(ag->goals_.size())]); + } + + m_ds->end(); + m_ds->endList(); + } + + } + #ifdef EXPORTING_OBJ for (unsigned int i=0; i< simulator.agents_.size(); ++i) { @@ -767,8 +789,8 @@ void SocialAgents::cb_redraw() #endif } // Commente par Arash - for (std::vector::iterator it = simulator.movingMeshes_.begin() ; it != simulator.movingMeshes_.end() ; ++it) - (*it)->draw(); +// for (std::vector::iterator it = simulator.movingMeshes_.begin() ; it != simulator.movingMeshes_.end() ; ++it) +// (*it)->draw(); }