Commit bb6a8e0e authored by pitiot's avatar pitiot

evitement de loin

parent 86512eb7
...@@ -12,7 +12,7 @@ SET (CGoGN_EXT_LIBS ${CGoGN_EXT_LIBS} ${QT_LIBRARIES} ${QGLVIEWER_LIBRARIES}) ...@@ -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(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) include(${CGoGN_ROOT_DIR}/apps_cmake.txt)
add_subdirectory(${CMAKE_SOURCE_DIR}/Release Release) add_subdirectory(${CMAKE_SOURCE_DIR}/Release Release)
......
...@@ -389,9 +389,8 @@ Dart generateBuilding(EnvMap& envMap, Dart d, float height, unsigned int buildin ...@@ -389,9 +389,8 @@ Dart generateBuilding(EnvMap& envMap, Dart d, float height, unsigned int buildin
obstacleMark.mark(dd) ; obstacleMark.mark(dd) ;
Dart next = map.phi1(dd) ; Dart next = map.phi1(dd) ;
Dart previous = map.phi_1(dd) ;
Obstacle* o = new Obstacle(position[dd], position[next], position[previous], Obstacle* o = new Obstacle(position[dd], position[next], NULL, 0);
position[map.phi1(next)], NIL, NIL, NULL, 0);
#ifdef SPATIAL_HASHING #ifdef SPATIAL_HASHING
VEC3 ov = o->p2 - o->p1 ; VEC3 ov = o->p2 - o->p1 ;
......
...@@ -34,7 +34,7 @@ class ArticulatedObstacle; ...@@ -34,7 +34,7 @@ class ArticulatedObstacle;
//#define EXPORTING3 //#define EXPORTING3
#define TWO_AND_HALF_DIM //#define TWO_AND_HALF_DIM
#ifdef EXPORTING3 #ifdef EXPORTING3
...@@ -377,20 +377,7 @@ inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& b ...@@ -377,20 +377,7 @@ inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& b
for (std::vector<Dart>::const_iterator it =belonging_cells.begin();it<belonging_cells.end();++it) for (std::vector<Dart>::const_iterator it =belonging_cells.begin();it<belonging_cells.end();++it)
memo_mark.mark(*it); memo_mark.mark(*it);
///////TENTATIVE D'AJOUT RATE
// MovingObstacle * mo = o->mo;
// int n = o->index;
// int m = (n+1)%mo->nbVertices;
//#ifdef TWO_AND_HALF_DIM
// CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalfMemo<PFP> * registering_part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalfMemo<PFP>(mo->sim_->envMap_.map, mo->parts_[n]->d,o->p1,mo->sim_->envMap_.position);
//#else
// CGoGN::Algo::Surface::MovingObjects::ParticleCell2DMemo<PFP> * registering_part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DMemo<PFP>(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<Dart> result =();
// d2=registering_part->d;
// CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest<<CGoGNendl;
......
...@@ -6,7 +6,7 @@ ...@@ -6,7 +6,7 @@
#include "utils.h" #include "utils.h"
#include "env_map.h" #include "env_map.h"
#include <set> #include <set>
// #define LINEAR #define LINEAR
// #define SECURED // #define SECURED
#ifdef LINEAR #ifdef LINEAR
#include "ShapeMatching/shapeMatchingLinear.h" #include "ShapeMatching/shapeMatchingLinear.h"
...@@ -78,6 +78,7 @@ public: ...@@ -78,6 +78,7 @@ public:
void attachAgent(Agent* ag); void attachAgent(Agent* ag);
unsigned int nbVertices; unsigned int nbVertices;
unsigned int nbParticles;
#ifdef SECURED #ifdef SECURED
CGoGN::Algo::Surface::MovingObjects::ParticleCell2DSecured<PFP> * * parts_ ; CGoGN::Algo::Surface::MovingObjects::ParticleCell2DSecured<PFP> * * parts_ ;
...@@ -124,7 +125,7 @@ public: ...@@ -124,7 +125,7 @@ public:
int max_x_ind; int max_x_ind;
int min_x_ind; int min_x_ind;
Obstacle* * obstacles_; std::vector<Obstacle *> obstacles_;
std::vector<Dart> * belonging_cells; std::vector<Dart> * belonging_cells;
std::vector<Dart> * neighbor_cells; std::vector<Dart> * neighbor_cells;
......
...@@ -6,10 +6,10 @@ ...@@ -6,10 +6,10 @@
class Obstacle class Obstacle
{ {
public: 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) : MovingObstacle * moving1=NULL, unsigned int ind=0) :
d1(d_1), d2(d_2), p1(point1), p2(point2), prevP(prevPoint), nextP(nextPoint), p1(point1), p2(point2),
mo(moving1), index(ind),p3(0,0,0) mo(moving1), index(ind),p3(0,0,0),obst_stiffness_agent(20),obst_stiffness_obst(20)
{ {
// p1[2] = 0 ; // p1[2] = 0 ;
// p2[2] = 0 ; // p2[2] = 0 ;
...@@ -17,16 +17,18 @@ public: ...@@ -17,16 +17,18 @@ public:
// nextP[2] = 0 ; // nextP[2] = 0 ;
} }
Dart d1;
Dart d2;
VEC3 p1 ; VEC3 p1 ;
VEC3 p2 ; VEC3 p2 ;
VEC3 prevP ;
VEC3 nextP ;
MovingObstacle * mo ; MovingObstacle * mo ;
unsigned int index ; unsigned int index ;
VEC3 p3; //pour les ellipses VEC3 p3; //pour les ellipses
double obst_stiffness_agent;
double obst_stiffness_obst;
} ; } ;
#endif #endif
...@@ -15,7 +15,7 @@ VEC3 Agent::xyPlane = VEC3(0,0,1); ...@@ -15,7 +15,7 @@ VEC3 Agent::xyPlane = VEC3(0,0,1);
unsigned int Agent::maxNeighbors_ = 10 ; unsigned int Agent::maxNeighbors_ = 10 ;
unsigned int Agent::maxMovingObstacles_ = 20; unsigned int Agent::maxMovingObstacles_ = 20;
float Agent::averageMaxSpeed_ = 2.0f ; float Agent::averageMaxSpeed_ = 0.5f ;
// float Agent::averageMaxSpeed_ = 20.0f ; // float Agent::averageMaxSpeed_ = 20.0f ;
float Agent::neighborDist_ = 10.0f ; float Agent::neighborDist_ = 10.0f ;
//float Agent::neighborDist_ = 20.0f ; //float Agent::neighborDist_ = 20.0f ;
...@@ -25,7 +25,7 @@ float Agent::neighborDistSq_ = neighborDist_ * neighborDist_ ; ...@@ -25,7 +25,7 @@ float Agent::neighborDistSq_ = neighborDist_ * neighborDist_ ;
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_ = 20.0f ;
float Agent::range_ = 2*timeHorizonObst_ * averageMaxSpeed_ + radius_ ; float Agent::range_ = 2*timeHorizonObst_ * averageMaxSpeed_ + radius_ ;
float Agent::rangeSq_ = range_ * range_ ; float Agent::rangeSq_ = range_ * range_ ;
...@@ -809,12 +809,12 @@ void Agent::computeNewVelocity() ...@@ -809,12 +809,12 @@ void Agent::computeNewVelocity()
//----- forces dues à la répulsion des obstacles en mouvement ---------- //----- forces dues à la répulsion des obstacles en mouvement ----------
VEC3 norm; 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; double obst_radius_infl;
if(sim_->config==0) if(sim_->config==0)
obst_radius_infl = 100.; // scenario 0 obst_radius_infl = 1000.; // scenario 0
else else
obst_radius_infl = 40.; // scenario 1 et 3 obst_radius_infl = 40.; // scenario 1 et 3
float force_value; float force_value;
...@@ -842,7 +842,7 @@ void Agent::computeNewVelocity() ...@@ -842,7 +842,7 @@ void Agent::computeNewVelocity()
if(sum_of_dists < rest_sum_of_dists) if(sum_of_dists < rest_sum_of_dists)
{ {
collision_softening_factor = pow(1-sum_of_dists/rest_sum_of_dists,obst_power); 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 v_obst = p2 - p1;
VEC3 normal = normFace ^v_obst; VEC3 normal = normFace ^v_obst;
// Ajouter une composante tangentielle // Ajouter une composante tangentielle
......
This diff is collapsed.
This diff is collapsed.
...@@ -699,9 +699,8 @@ void EnvMap::registerWallInFaces() ...@@ -699,9 +699,8 @@ void EnvMap::registerWallInFaces()
{ {
Dart dd2 = map.phi2(dd) ; Dart dd2 = map.phi2(dd) ;
Dart next = map.phi1(dd2) ; Dart next = map.phi1(dd2) ;
Dart previous = map.phi_1(dd2) ;
Obstacle* o = new Obstacle(position[next], position[dd2], position[previous], Obstacle* o = new Obstacle(position[next], position[dd2], NULL, 0) ;
position[map.phi1(next)], next, dd2, NULL, 0) ;
obstvect[dd2].push_back(o) ; obstvect[dd2].push_back(o) ;
} }
dd = map.phi1(dd) ; dd = map.phi1(dd) ;
...@@ -745,10 +744,8 @@ void EnvMap::registerObstaclesInFaces() ...@@ -745,10 +744,8 @@ void EnvMap::registerObstaclesInFaces()
{ {
Dart dd2 = map.phi2(dd) ; Dart dd2 = map.phi2(dd) ;
Dart next = map.phi1(dd2) ; Dart next = map.phi1(dd2) ;
Dart previous = map.phi_1(dd2) ;
Obstacle* o = new Obstacle(position[dd2], position[next], Obstacle* o = new Obstacle(position[dd2], position[next],
position[previous], position[map.phi1(next)],
next, dd2,
NULL, 0) ; NULL, 0) ;
obstvect[d].push_back(o) ; obstvect[d].push_back(o) ;
} }
...@@ -798,9 +795,8 @@ void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbo ...@@ -798,9 +795,8 @@ void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbo
// { // {
Dart dd2 = map.phi2(dd) ; Dart dd2 = map.phi2(dd) ;
Dart next = map.phi1(dd2) ; Dart next = map.phi1(dd2) ;
Dart previous = map.phi_1(dd2) ;
Obstacle* o = new Obstacle(position[dd2], position[next], position[previous], Obstacle* o = new Obstacle(position[dd2], position[next], NULL, 0) ;
position[map.phi1(next)], next, dd2, NULL, 0) ;
obst.push_back(o) ; obst.push_back(o) ;
// } // }
} }
......
...@@ -9,7 +9,7 @@ ...@@ -9,7 +9,7 @@
//float MovingObstacle::neighborDistSq_ = 5.0f * 5.0f; //float MovingObstacle::neighborDistSq_ = 5.0f * 5.0f;
float MovingObstacle::maxSpeed_ = 2.0f; float MovingObstacle::maxSpeed_ = 1.0f;
float MovingObstacle::neighborDist_ = 10.0f ; float MovingObstacle::neighborDist_ = 10.0f ;
float MovingObstacle::neighborDistSq_ = neighborDist_ * neighborDist_ ; float MovingObstacle::neighborDistSq_ = neighborDist_ * neighborDist_ ;
float MovingObstacle::timeHorizonObst_ = 10.0f; float MovingObstacle::timeHorizonObst_ = 10.0f;
...@@ -144,6 +144,7 @@ VEC3 rotate2D(VEC3 pos1, VEC3 center, float angle) ...@@ -144,6 +144,7 @@ VEC3 rotate2D(VEC3 pos1, VEC3 center, float angle)
MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool rigid, bool spin,int curGoal, Dart dInside, ArticulatedObstacle * art, int indParent) : MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool rigid, bool spin,int curGoal, Dart dInside, ArticulatedObstacle * art, int indParent) :
nbVertices(pos.size()), nbVertices(pos.size()),
nbParticles(pos.size()),
center(0), center(0),
index(ind), index(ind),
goals_(goals), goals_(goals),
...@@ -166,12 +167,22 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s ...@@ -166,12 +167,22 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
alpha(0.99) alpha(0.99)
{ {
for (unsigned int i = 0; i < nbVertices; ++i)
{
center += pos[i];
}
center /= nbVertices;
assert(pos.size() > 2); assert(pos.size() > 2);
if(dInside==NIL) if(dInside==NIL)
dInside = sim_->envMap_.getBelongingCell(pos[0]); dInside = sim_->envMap_.getBelongingCell(pos[0]);
if(rigid_)
unsigned int nbParticles = nbVertices; {
pos.push_back(center);
pos.push_back(center);
nbParticles=nbVertices+2;
}
// if(!rigid); // if(!rigid);
// nbVertices +=1; //a center particle for the mass-spring // nbVertices +=1; //a center particle for the mass-spring
...@@ -186,9 +197,9 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s ...@@ -186,9 +197,9 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
#endif #endif
obstacles_ = new Obstacle*[nbVertices];
belonging_cells = new std::vector<Dart>[nbVertices]; belonging_cells = new std::vector<Dart>[nbParticles];
neighbor_cells = new std::vector<Dart>[nbVertices]; neighbor_cells = new std::vector<Dart>[nbParticles];
position = map.addAttribute<VEC3, VERTEX>("position") ; position = map.addAttribute<VEC3, VERTEX>("position") ;
normal = map.addAttribute<VEC3, VERTEX>("normal") ; normal = map.addAttribute<VEC3, VERTEX>("normal") ;
...@@ -202,12 +213,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s ...@@ -202,12 +213,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
edgeLength = map.addAttribute<float, EDGE>("edgeLength") ; edgeLength = map.addAttribute<float, EDGE>("edgeLength") ;
vertexAngle = map.addAttribute<float, DART>("vertexAngle") ; vertexAngle = map.addAttribute<float, DART>("vertexAngle") ;
} }
for (unsigned int i = 0; i < nbVertices; ++i)
{ for (unsigned int i = 0; i < nbParticles; ++i)
center += pos[i];
}
center /= nbVertices;
for (unsigned int i = 0; i < nbVertices; ++i)
{ {
#ifdef TWO_AND_HALF_DIM #ifdef TWO_AND_HALF_DIM
Dart d = dInside; Dart d = dInside;
...@@ -226,21 +233,10 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s ...@@ -226,21 +233,10 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
if(i==0) if(i==0)
dDir = d; dDir = d;
} }
front=(parts_[0]->getPosition() + parts_[1]->getPosition()) / 2; 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<PFP>(sim_->envMap_.map, dInside, center, sim_->envMap_.position);
//#else
//#ifdef SECURED
// parts_[nbVertices] = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DSecured<PFP>(sim_->envMap_.map, dInside, center, sim_->envMap_.position);
//#else
// parts_[nbVertices] = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>(sim_->envMap_.map, dInside, center, sim_->envMap_.position);
//#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.
...@@ -370,21 +366,23 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s ...@@ -370,21 +366,23 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
map.enableQuickTraversal<VERTEX>(); map.enableQuickTraversal<VERTEX>();
dDir=dInside; dDir=dInside;
}
#ifdef LINEAR #ifdef LINEAR
shape_= new ShapeMatchingLinear<PFP>(map,position,bord,beta); shape_= new ShapeMatchingLinear<PFP>(map,position,bord,beta);
#else #else
shape_= new ShapeMatchingQuadratic<PFP>(map,position,bord,beta); shape_= new ShapeMatchingQuadratic<PFP>(map,position,bord,beta);
#endif #endif
shape_->initialize(); shape_->initialize();
}
for (unsigned int i = 0; i < nbVertices; ++i) for (unsigned int i = 0; i < nbVertices; ++i)
{ {
Obstacle* o = new Obstacle(parts_[i]->getPosition(), Obstacle* o = new Obstacle(parts_[i]->getPosition(),
parts_[(i + 1) % nbVertices]->getPosition(), parts_[(i + 1) % nbVertices]->getPosition(),
parts_[(i - 1 + nbVertices) % nbVertices]->getPosition(), this, i);
parts_[(i + 2) % nbVertices]->getPosition(), i, (i+1)% nbVertices, this, i); o->obst_stiffness_obst=0.5;
obstacles_[i] = o; o->obst_stiffness_agent=100;
obstacles_.push_back(o);
/////definition du troisieme point /////definition du troisieme point
if (rigid_) o->p3=parts_[(i + 2) % nbVertices]->getPosition(); if (rigid_) o->p3=parts_[(i + 2) % nbVertices]->getPosition();
...@@ -392,6 +390,32 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s ...@@ -392,6 +390,32 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
sim_->envMap_.pushObstacleInCells(o); 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<<CGoGNendl;
sim_->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<<CGoGNendl;
sim_->envMap_.pushObstacleInCells(o2);
/// fin ajout
}
} }
...@@ -480,6 +504,26 @@ void MovingObstacle::draw(bool showPath) ...@@ -480,6 +504,26 @@ void MovingObstacle::draw(bool showPath)
m_ds->end(); m_ds->end();
m_ds->endList(); 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) VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
...@@ -1096,7 +1140,7 @@ void MovingObstacle::updateForces() ...@@ -1096,7 +1140,7 @@ void MovingObstacle::updateForces()
//------------------------------------------------------------------------- //-------------------------------------------------------------------------
// ARASH : A présent on calcule les interactions avec les autres obstacles. // ARASH : A présent on calcule les interactions avec les autres obstacles.
VEC3 norm; VEC3 norm;
double obst_stiffness = 0.5; // agent-obstacle interaction stiffness
int obst_power = 2 ; // 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, obst_radius_infl_buildings; double obst_radius_infl, obst_radius_infl_buildings;
...@@ -1122,7 +1166,7 @@ void MovingObstacle::updateForces() ...@@ -1122,7 +1166,7 @@ void MovingObstacle::updateForces()
VEC3 p1=obst->p1 ; VEC3 p1=obst->p1 ;
VEC3 p2=obst->p2 ; 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 // Evitement d'obstacles fixes
...@@ -1134,7 +1178,7 @@ void MovingObstacle::updateForces() ...@@ -1134,7 +1178,7 @@ void MovingObstacle::updateForces()
VEC3 p1=obst->p2 ; VEC3 p1=obst->p2 ;
VEC3 p2=obst->p1 ; 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); d = map.phi_1(d);
...@@ -1250,7 +1294,11 @@ void MovingObstacle::applyForces() ...@@ -1250,7 +1294,11 @@ void MovingObstacle::applyForces()
{ {
velocity[d] += (forces[d]-0.01*velocity[d]) * sim_->timeStep_ ; 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[d] += (velocity[d] * sim_->timeStep_);
position [map.phi_1(map.phi2(d))]+=(0.5*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<PFP>(sim_->envMap_.map, parts_[i]->d, sim_->envMap_.position); // PFP::VEC3 normal = CGoGN::Algo::Surface::Geometry::faceNormal<PFP>(sim_->envMap_.map, parts_[i]->d, sim_->envMap_.position);
...@@ -1323,17 +1371,20 @@ void MovingObstacle::updateRegistration() ...@@ -1323,17 +1371,20 @@ void MovingObstacle::updateRegistration()
position[map.phi_1(d)] = position[d]+normal; 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) for (unsigned int i = 0; i < nbVertices; ++i)
{ {
// CGoGNout << "avant une etape : Obstacle "<< i << CGoGNendl; // CGoGNout << "avant une etape : Obstacle "<< i << CGoGNendl;
Obstacle* o = obstacles_[i]; Obstacle* o = obstacles_[i];
o->p1 = getDilatedPosition(i); o->p1 = parts_[i]->getPosition();
o->p2 = getDilatedPosition((i+1) % nbVertices); o->p2 = parts_[(i + 1 ) % nbVertices]->getPosition();
if (rigid_) o->p3=parts_[(i + 2 ) % nbVertices]->getPosition();
o->prevP = getDilatedPosition((i - 1 ) % nbVertices);
o->nextP = getDilatedPosition((i + 2 ) % nbVertices);
if (rigid_) o->p3=getDilatedPosition((i + 2 ) % nbVertices);
Dart d1 = parts_[i]->d; Dart d1 = parts_[i]->d;
Dart d2 = parts_[(i+1)%nbVertices]->d; Dart d2 = parts_[(i+1)%nbVertices]->d;
...@@ -1354,6 +1405,28 @@ void MovingObstacle::updateRegistration() ...@@ -1354,6 +1405,28 @@ void MovingObstacle::updateRegistration()
// CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl; // CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl;
} }
Obstacle* o = obstacles_[nbVertices];
o->p1 = parts_[nbVertices]->getPosition();