Commit ed23a69b authored by pitiot's avatar pitiot
Browse files

buggé limace 2D

parent 8bd8a582
...@@ -8,7 +8,7 @@ ...@@ -8,7 +8,7 @@
#include "spatialHashing.h" #include "spatialHashing.h"
#define SECURED //#define SECURED
//#define EXPORTING_AGENT //#define EXPORTING_AGENT
//#define EXPORTING_OBJ //#define EXPORTING_OBJ
......
...@@ -32,9 +32,9 @@ class ArticulatedObstacle; ...@@ -32,9 +32,9 @@ class ArticulatedObstacle;
#include "pfp.h" #include "pfp.h"
#define EXPORTING3 //#define EXPORTING3
#define TWO_AND_HALF_DIM //#define TWO_AND_HALF_DIM
#ifdef EXPORTING3 #ifdef EXPORTING3
......
...@@ -18,7 +18,7 @@ ...@@ -18,7 +18,7 @@
using namespace std; using namespace std;
#define EXPORTING2 //#define EXPORTING2
float get_angle3D(VEC3 v1, VEC3 v2); float get_angle3D(VEC3 v1, VEC3 v2);
......
...@@ -14,7 +14,7 @@ ...@@ -14,7 +14,7 @@
#include "Algo/MovingObjects/particle_cell_2D_memo.h" #include "Algo/MovingObjects/particle_cell_2D_memo.h"
#endif #endif
// #define EXPORTING_BOXES #define EXPORTING_BOXES
#ifdef EXPORTING_BOXES #ifdef EXPORTING_BOXES
#include "Algo/Render/GL2/mapRender.h" #include "Algo/Render/GL2/mapRender.h"
......
...@@ -21,12 +21,12 @@ float Agent::averageMaxSpeed_ = 2.0f ; ...@@ -21,12 +21,12 @@ float Agent::averageMaxSpeed_ = 2.0f ;
//float Agent::neighborDist_ = 20.0f ; //float Agent::neighborDist_ = 20.0f ;
float Agent::neighborDistSq_ = neighborDist_ * neighborDist_ ; float Agent::neighborDistSq_ = neighborDist_ * neighborDist_ ;
//float Agent::radius_ = 8.0f ; //float Agent::radius_ = 8.0f ;
float Agent::radius_ = 3.0f ; //float Agent::radius_ = 3.0f ;
// float Agent::radius_ = 1.5f ; float Agent::radius_ = 1.5f ;
float Agent::timeHorizon_ = 10.0f ; float Agent::timeHorizon_ = 10.0f ;
//float Agent::timeHorizon_ = 100.0f ; //float Agent::timeHorizon_ = 100.0f ;
float Agent::timeHorizonObst_ = 10.0f ; float Agent::timeHorizonObst_ = 10.0f ;
float Agent::range_ = 4*timeHorizonObst_ * averageMaxSpeed_ + radius_ ; float Agent::range_ = 2*timeHorizonObst_ * averageMaxSpeed_ + radius_ ;
float Agent::rangeSq_ = range_ * range_ ; float Agent::rangeSq_ = range_ * range_ ;
unsigned int Agent::cptAgent = 0 ; unsigned int Agent::cptAgent = 0 ;
...@@ -506,11 +506,13 @@ void Agent::update() ...@@ -506,11 +506,13 @@ void Agent::update()
#endif #endif
meanDirection_.set(0) ; meanDirection_.set(0) ;
for (unsigned int i=0 ; i<SMOOTHING_BUFFER_SIZE ; ++i) { for (unsigned int i=0 ; i<SMOOTHING_BUFFER_SIZE ; ++i) {
meanDirection_ += meanVelocity_[i] ; meanDirection_ += meanVelocity_[i] ;
meanVelocity_[i] = meanVelocity_[(i+1)%SMOOTHING_BUFFER_SIZE] ; meanVelocity_[i] = meanVelocity_[(i+1)%SMOOTHING_BUFFER_SIZE] ;
} }
meanVelocity_[SMOOTHING_BUFFER_SIZE-1] = velocity_ ; meanVelocity_[SMOOTHING_BUFFER_SIZE-1] = velocity_ ;
meanDirection_.normalize() ; meanDirection_.normalize() ;
} }
...@@ -751,7 +753,7 @@ void Agent::computeNewVelocity() ...@@ -751,7 +753,7 @@ void Agent::computeNewVelocity()
VEC3 normFace = VEC3 (0,0,1); VEC3 normFace = VEC3 (0,0,1);
#endif #endif
double collision_softening_factor; double collision_softening_factor;
float ag_ambient_damping = 50.0; float ag_ambient_damping = 10.0;
// double mass_var = 0.95; // double mass_var = 0.95;
// double average_mass = 1.0; // double average_mass = 1.0;
...@@ -765,7 +767,7 @@ void Agent::computeNewVelocity() ...@@ -765,7 +767,7 @@ void Agent::computeNewVelocity()
double rand = 2.0*drand48()-1.0; // compris entre -1 et 1 double rand = 2.0*drand48()-1.0; // compris entre -1 et 1
// double ag_mass = average_mass*(1 + rand*mass_var); // valeurs uniformement réparties entre min et max // double ag_mass = average_mass*(1 + rand*mass_var); // valeurs uniformement réparties entre min et max
double ag_mass = 200.0; double ag_mass = 50.0;
/* /*
rand = 2.0*drand48()-1.0; // compris entre -1 et 1 rand = 2.0*drand48()-1.0; // compris entre -1 et 1
radius_ = average_radius + rand*radius_var; // valeurs uniformement réparties entre min et max radius_ = average_radius + rand*radius_var; // valeurs uniformement réparties entre min et max
...@@ -805,7 +807,7 @@ void Agent::computeNewVelocity() ...@@ -805,7 +807,7 @@ void Agent::computeNewVelocity()
obst_radius_infl = 100.; // scenario 0 obst_radius_infl = 100.; // scenario 0
else else
obst_radius_infl = 40.; // scenario 1 et 3 obst_radius_infl = 40.; // scenario 1 et 3
float force_value=0.0; float force_value;
int nobst=0; int nobst=0;
...@@ -881,14 +883,13 @@ void Agent::computeNewVelocity() ...@@ -881,14 +883,13 @@ void Agent::computeNewVelocity()
// 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_stiffness = 5000.0; // agent-obstacle interaction stiffness
// double fixed_obst_damping = 1.0; // agent-obstacle interaction damping // double fixed_obst_damping = 1.0; // agent-obstacle interaction damping
int fixed_obst_power = 4.0; // the power to which elevate the agent-obstacle distance int fixed_obst_power = 1; // the power to which elevate the agent-obstacle distance
Obstacle* fixed_obst ; Obstacle* fixed_obst ;
for(std::vector<std::pair<float, Obstacle*> >::iterator it = obstacleNeighbors_.begin() ; for(std::vector<std::pair<float, Obstacle*> >::iterator it = obstacleNeighbors_.begin() ;
it != obstacleNeighbors_.end() ; it != obstacleNeighbors_.end() ;
++it) ++it)
{ {
double dist = it->first; double dist = it->first;
// cerr << "nobst=" << nobst << "dist=" << dist << endl; // cerr << "nobst=" << nobst << "dist=" << dist << endl;
// double effective_range = 50*range_; // double effective_range = 50*range_;
...@@ -901,33 +902,29 @@ void Agent::computeNewVelocity() ...@@ -901,33 +902,29 @@ void Agent::computeNewVelocity()
} }
fixed_obst=it->second ; fixed_obst=it->second ;
if(fixed_obst->mo!=NULL)
cerr << "Collision avec limace !!!" << endl;
VEC3 p1=fixed_obst->p1 ; VEC3 p1=fixed_obst->p1 ;
VEC3 p2=fixed_obst->p2 ; VEC3 p2=fixed_obst->p2 ;
vec=p2-p1; vec=p2-p1;
vec.normalize(); vec.normalize();
norm= normFace ^vec; norm= normFace ^vec;
forces -= force_value * norm; forces -= force_value * norm;
// cerr << "obstacles fixes : nobst=" << nobst << " dist=" << dist << " force=" << force_value*norm << endl; // cerr << "obstacles fixes : nobst=" << nobst << " dist=" << dist << " force=" << force_value*norm << endl;
nobst++; nobst++;
} }
//----- forces dues à la répulsion des autres agents ------- //----- forces dues à la répulsion des autres agents -------
double ag_stiffness = 400.0; // agent-agent interaction stiffness double ag_stiffness = 400.0; // agent-agent interaction stiffness
double ag_damping = 1.0; // agent-agent interaction damping double ag_damping = 1.0; // agent-agent interaction damping
double ag_power = 4.0; // the power to which elevate the agent-agent distance // double ag_power = 1; // the power to which elevate the agent-agent distance
rand = 2.0*drand48()-1.0; // compris entre -1 et 1 rand = 2.0*drand48()-1.0; // compris entre -1 et 1
double radius_var = 0.5; double radius_var = 0.5;
double ag_phys_radius_coef = 2.0*(1 + rand*radius_var); // valeurs uniformement réparties entre min et max double ag_phys_radius_coef = 2.0*(1 + rand*radius_var); // valeurs uniformement réparties entre min et max
unsigned int nbA = 0 ; unsigned int nbA = 0 ;
for (std::vector<std::pair<float, Agent*> >::iterator it = agentNeighbors_.begin() ; for (std::vector<std::pair<float, Agent*> >::iterator it = agentNeighbors_.begin() ;
...@@ -955,7 +952,8 @@ void Agent::computeNewVelocity() ...@@ -955,7 +952,8 @@ void Agent::computeNewVelocity()
if(dist < combinedRadius) if(dist < combinedRadius)
{ {
collision_softening_factor = pow(1-dist/combinedRadius,ag_power); // collision_softening_factor = pow(1-dist/combinedRadius,ag_power);
collision_softening_factor = 1;
float force_value = - ag_stiffness*collision_softening_factor*(combinedRadius-dist) float force_value = - ag_stiffness*collision_softening_factor*(combinedRadius-dist)
- ag_damping * (dist - previous_dist) / sim_->timeStep_; - ag_damping * (dist - previous_dist) / sim_->timeStep_;
......
...@@ -314,7 +314,7 @@ void EnvMap::initGL() ...@@ -314,7 +314,7 @@ void EnvMap::initGL()
position_nmap.push_back(position_Export); position_nmap.push_back(position_Export);
TraversorV<PFP2::MAP> tV(*nmap); // TraversorV<PFP2::MAP> tV(*nmap);
// for(Dart d = tV.begin() ; d != tV.end() ; d = tV.next()) // for(Dart d = tV.begin() ; d != tV.end() ; d = tV.next())
//// { //// {
......
...@@ -246,6 +246,7 @@ std::vector<VEC3> MovingMesh::computeProjectedPointSet(float maxHeight, Dart d) ...@@ -246,6 +246,7 @@ std::vector<VEC3> MovingMesh::computeProjectedPointSet(float maxHeight, Dart d)
VEC3 axis = VEC3(0,0,1) ^ normale ; VEC3 axis = VEC3(0,0,1) ^ normale ;
// Geom::translate(center[0],center[1],center[2],rot); // Geom::translate(center[0],center[1],center[2],rot);
if(angle != 0)
Geom::rotate(axis, angle, rot) ; Geom::rotate(axis, angle, rot) ;
for(Dart dd = tv.begin() ; dd != tv.end() ; dd = tv.next()) for(Dart dd = tv.begin() ; dd != tv.end() ; dd = tv.next())
...@@ -264,6 +265,7 @@ std::vector<VEC3> MovingMesh::computeProjectedPointSet(float maxHeight, Dart d) ...@@ -264,6 +265,7 @@ std::vector<VEC3> MovingMesh::computeProjectedPointSet(float maxHeight, Dart d)
axis = VEC3(0,0,1) ^ normale ; axis = VEC3(0,0,1) ^ normale ;
// Geom::translate(center[0],center[1],center[2],rot); // Geom::translate(center[0],center[1],center[2],rot);
if(angle != 0)
Geom::rotate(axis, angle, rot) ; Geom::rotate(axis, angle, rot) ;
for(unsigned int i = 0; i < points.size() ; ++i) for(unsigned int i = 0; i < points.size() ; ++i)
...@@ -275,8 +277,8 @@ std::vector<VEC3> MovingMesh::computeProjectedPointSet(float maxHeight, Dart d) ...@@ -275,8 +277,8 @@ std::vector<VEC3> MovingMesh::computeProjectedPointSet(float maxHeight, Dart d)
// std::cout << "fin " << points[i] << std::endl; // std::cout << "fin " << points[i] << std::endl;
} }
std::cout << "nb vertices before simplification -> " << points.size() << std::endl; std::cout << "nb vertices before simplification -> " << points.size() <<" ||" << points [0]<< std::endl;
std::cout << "nb vertices after simplification -> " << res.size() << std::endl; std::cout << "nb vertices after simplification -> " << res.size() << " ||" << res [0]<< std::endl;
return res; return res;
} }
......
...@@ -147,9 +147,24 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s ...@@ -147,9 +147,24 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
dInside = sim_->envMap_.getBelongingCell(pos[0]); dInside = sim_->envMap_.getBelongingCell(pos[0]);
#ifdef TWO_AND_HALF_DIM #ifdef TWO_AND_HALF_DIM
parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>*[nbVertices+1];
if(!rigid_)
{
parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>*[nbVertices+1];
}
else
{
parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>*[nbVertices];
}
#else #else
if(!rigid_)
{
parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>*[nbVertices+1];
}
else
{
parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>*[nbVertices]; parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>*[nbVertices];
}
#endif #endif
obstacles_ = new Obstacle*[nbVertices]; obstacles_ = new Obstacle*[nbVertices];
...@@ -188,13 +203,21 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s ...@@ -188,13 +203,21 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
center /= nbVertices; center /= nbVertices;
front=(pos[0] + pos[1]) / 2; front=(pos[0] + pos[1]) / 2;
#ifdef TWO_AND_HALF_DIM
if(!rigid_)
{
CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>* part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>(sim_->envMap_.map, dInside, center, sim_->envMap_.position);
parts_[nbVertices]=part;
}
#else
#ifdef TWO_AND_HALF_DIM if(!rigid_)
CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>* part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>(sim_->envMap_.map, dInside, center, sim_->envMap_.position); {
CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>* part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>(sim_->envMap_.map, dInside, center, sim_->envMap_.position);
parts_[nbVertices]=part; parts_[nbVertices]=part;
}
#endif #endif
// M appartient à l'ellipse ssi MF1 + MF2 = sum_dist_foci est une constante // M appartient à l'ellipse ssi MF1 + MF2 = sum_dist_foci est une constante
// où F1 et F2 sont les deux foyers. // où F1 et F2 sont les deux foyers.
// length = (pos[0]-pos[1]).norm(); // length = (pos[0]-pos[1]).norm();
...@@ -263,7 +286,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s ...@@ -263,7 +286,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
//extrude face to build a cage //extrude face to build a cage
// compute edgeLength for mass-spring // compute edgeLength for mass-spring
hight=rigid_ ? -10.0f : 10.0f; hight=rigid_ ? 10.0f : -10.0f;
Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, hight) ; Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, hight) ;
// Dart dT = map.phi1(map.phi1(map.phi2(Dart(0)))); // Dart dT = map.phi1(map.phi1(map.phi2(Dart(0))));
// std::cout << __FUNCTION__ << " val " << dT << std::endl; // std::cout << __FUNCTION__ << " val " << dT << std::endl;
......
...@@ -71,7 +71,7 @@ void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst ...@@ -71,7 +71,7 @@ void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst
case 3 : case 3 :
envMap_.init(config, 1000.0f, 1000.0f, minSize, 100.0f) ; //grosses cases envMap_.init(config, 1000.0f, 1000.0f, minSize, 100.0f) ; //grosses cases
setupScenario(nbAgent, false) ; setupScenario(nbAgent, false) ;
addMovingObstacles(nbObst, 1); addMovingObstacles(nbObst, 1, 10);
addPathToObstacles(envMap_.buildingMark, true); addPathToObstacles(envMap_.buildingMark, true);
//// setupCityScenario(-1.0f * (12 * (70.0f / 2.0f) - 10), //// setupCityScenario(-1.0f * (12 * (70.0f / 2.0f) - 10),
//// -1.0f * (12 * (70.0f / 2.0f) - 10), 20, 20); //// -1.0f * (12 * (70.0f / 2.0f) - 10), 20, 20);
...@@ -708,7 +708,7 @@ void Simulator::setupScenario(unsigned int nbMaxAgent, bool pedWay) ...@@ -708,7 +708,7 @@ void Simulator::setupScenario(unsigned int nbMaxAgent, bool pedWay)
TraversorF<PFP::MAP> tF(envMap_.map); TraversorF<PFP::MAP> tF(envMap_.map);
d = tF.begin() ; d = tF.begin() ;
unsigned int nbx = nbMaxAgent==0 ? 1 : (nbMaxAgent / (nb_cells))/5; unsigned int nbx = 1;
unsigned int nby = 5; unsigned int nby = 5;
...@@ -888,10 +888,10 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType) ...@@ -888,10 +888,10 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
vPos.push_back(start+xSideF*dir1+ySideF*dir2); vPos.push_back(start+xSideF*dir1+ySideF*dir2);
vPos.push_back(start-xSideF*dir1+ySideF*dir2); vPos.push_back(start-xSideF*dir1+ySideF*dir2);
#else #else
vPos.push_back(start-xSide-ySide);
vPos.push_back(start+xSide-ySide);
vPos.push_back(start+xSide+ySide); vPos.push_back(start+xSide+ySide);
vPos.push_back(start-xSide+ySide); vPos.push_back(start+xSide-ySide);
vPos.push_back(start-xSide-ySide);
vPos.push_back(start-xSide+ySide);
#endif #endif
} }
...@@ -905,17 +905,7 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType) ...@@ -905,17 +905,7 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
movingMeshes_.push_back(mm); movingMeshes_.push_back(mm);
vPos = mm->computeProjectedPointSet(maxHeight, d); vPos = mm->computeProjectedPointSet(maxHeight, d);
std::reverse(vPos.begin(),vPos.end()); std::reverse(vPos.begin(),vPos.end());
#ifndef TWO_AND_HALF_DIM std::cout << "créé limace" << std::endl;
//scale projected pointset
VEC3 bary(0);
for(std::vector<VEC3>::iterator it = vPos.begin() ; it != vPos.end() ; ++it)
bary += *it;
bary /= vPos.size();
float scale =1.2f;
for(std::vector<VEC3>::iterator it = vPos.begin() ; it != vPos.end() ; ++it)
*it = *it*scale - bary*(scale-1.0f);
#endif
} }
break; break;
...@@ -958,7 +948,7 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType) ...@@ -958,7 +948,7 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals, (obstType==0), (obstType==0), 0 , d); MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals, (obstType==0), (obstType==0), 0 , d);
std::cout << "créé obstacle : "<<vPos[0] << std::endl;
movingObstacles_.push_back(mo); movingObstacles_.push_back(mo);
......
...@@ -2138,13 +2138,13 @@ int main(int argc, char** argv) ...@@ -2138,13 +2138,13 @@ int main(int argc, char** argv)
// S'il y a un cam input file, alors lire le premier // S'il y a un cam input file, alors lire le premier
// entier N et faire marcher la simulation N fois. // entier N et faire marcher la simulation N fois.
// sa.readCameraInputFile("src/cam.scn3.spline"); // sa.readCameraInputFile("src/cam.scn3.spline");
// sa.cam_output_file.open("src/cam.out",std::ofstream::out); // sa.cam_output_file.open("src/cam.out",std::ofstream::out);
// sa.cam_output_file("src/cam.out"); //
//
// int i; // unsigned int i;
// for(i=0;i<sa.cif_begin;i++) // for(i=0;i<sa.cif_begin;i++)
// sa.animate(); sa.animate();
return app.exec() ; return app.exec() ;
} }
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