Skip to content

GitLab

S
SocialAgents3D
Commit 88f271e5 authored by pitiot's avatar pitiot
Browse files
Options

3D presque réparé

parent 60a61619
Hide whitespace changes
Inline Side-by-side
Showing with 96 additions and 51 deletions
include/env_generator.hpp
View file @ 88f271e5
......@@ -81,7 +81,7 @@ void generateCity(EnvMap& envMap, unsigned int nbBuildings)
#ifndef TWO_AND_HALF_DIM
generateBuilding<PFP>(envMap, d, (1 + (rand() % 3)) * 50.0f, rand() % 4) ;
#else
generateBuilding<PFP>(envMap, d, (1 + (rand() % 3)) * 10.0f, rand() % 3) ;
generateBuilding<PFP>(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 ;
......
include/env_map.h
View file @ 88f271e5
......@@ -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
......
include/moving_obstacle.h
View file @ 88f271e5
......@@ -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 ;
......
src/agent.cpp
View file @ 88f271e5
......@@ -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<PFP>(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++;
......
src/env_map.cpp
View file @ 88f271e5
......@@ -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<PFP>(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)
......
src/moving_obstacle.cpp
View file @ 88f271e5
......@@ -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<PFP>(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<std::pair<float, Obstacle*> >::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 ;
......
src/simulator.cpp
View file @ 88f271e5
......@@ -1111,12 +1111,12 @@ void Simulator::addPathsToAgents()
d = envMap_.map.phi1(d);
}
// VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position);
VEC3 dest = Algo::Surface::Geometry::faceCentroid<PFP>(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<PFP>(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<PFP>(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<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
{
// VEC3 dest = Algo::Geometry::faceCentroid<PFP>(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<PFP>(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) ;
}
......
src/viewer.cpp
View file @ 88f271e5
......@@ -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<MovingMesh*>::iterator it = simulator.movingMeshes_.begin() ; it != simulator.movingMeshes_.end() ; ++it)
(*it)->draw();
// for (std::vector<MovingMesh*>::iterator it = simulator.movingMeshes_.begin() ; it != simulator.movingMeshes_.end() ; ++it)
// (*it)->draw();
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment