Commit 31b1668a authored by pitiot's avatar pitiot

fini ? corrigé limaces 3D

parent bb6a8e0e
2 282.553 -549.754 -431.112 243.297 -467.981 -389.015 0.278767 0.541963 -0.792821 0 2892
2 380.527 224.803 -813.318 330.469 214.097 -727.414 -0.172437 0.984769 0.0222493 0 13.2112 -343.6 594.385 10.9265 -307.115 501.306 0.0322209 0.930812 0.364074
1637 13.2112 -343.6 594.385 10.9265 -307.115 501.306 0.0322209 0.930812 0.364074
2263 -225.649 -65.4916 186.836 -153.811 -25.9154 129.625 0.511978 0.255997 0.819966
2892 154.528 244.386 80.6884 119.103 153.875 57.1753 0.0753229 -0.278238 0.957554
...@@ -7,7 +7,7 @@ using namespace std; ...@@ -7,7 +7,7 @@ using namespace std;
class ArticulatedObstacle class ArticulatedObstacle
{ {
public: public:
ArticulatedObstacle(Simulator* sim, int index, int currentIndex, std::vector<PFP::VEC3> * pos, int nbParts, std::vector<VEC3> goals); ArticulatedObstacle(Simulator* sim, int index, int currentIndex, std::vector<PFP::VEC3> * pos, int nbParts, std::vector<VEC3> goals, int curGoal =0);
std::vector<MovingObstacle *> members; std::vector<MovingObstacle *> members;
int index; int index;
......
...@@ -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
......
...@@ -6,8 +6,12 @@ ...@@ -6,8 +6,12 @@
#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"
#else #else
...@@ -179,7 +183,7 @@ public: ...@@ -179,7 +183,7 @@ public:
#endif #endif
PFP::REAL beta; PFP::REAL beta;
float alpha; float alpha;
float speed_detection_factor;
}; };
......
...@@ -7,9 +7,9 @@ class Obstacle ...@@ -7,9 +7,9 @@ class Obstacle
{ {
public: public:
Obstacle(const VEC3 point1, const VEC3 point2, Obstacle(const VEC3 point1, const VEC3 point2,
MovingObstacle * moving1=NULL, unsigned int ind=0) : MovingObstacle * moving1=NULL, unsigned int ind=0,double stiff_agent=20,double stiff_obst=5,double radius_agent=500,double radius_obst=10) :
p1(point1), p2(point2), p1(point1), p2(point2),
mo(moving1), index(ind),p3(0,0,0),obst_stiffness_agent(20),obst_stiffness_obst(20) mo(moving1), index(ind),p3(0,0,0),obst_stiffness_agent(stiff_agent),obst_stiffness_obst(stiff_obst),obst_radius_infl_agent(radius_agent),obst_radius_infl_obst(radius_obst)
{ {
// p1[2] = 0 ; // p1[2] = 0 ;
// p2[2] = 0 ; // p2[2] = 0 ;
...@@ -29,6 +29,8 @@ public: ...@@ -29,6 +29,8 @@ public:
double obst_stiffness_agent; double obst_stiffness_agent;
double obst_stiffness_obst; double obst_stiffness_obst;
double obst_radius_infl_agent;
double obst_radius_infl_obst;
} ; } ;
#endif #endif
...@@ -93,6 +93,7 @@ public: ...@@ -93,6 +93,7 @@ public:
bool reachedGoal() ; bool reachedGoal() ;
void setupCircleScenario(unsigned int nbAgents, unsigned int nbObstacles) ; void setupCircleScenario(unsigned int nbAgents, unsigned int nbObstacles) ;
void setupCrowdedScenario(unsigned int nbAgents , unsigned int nbObstacles, int snake);
void setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles) ; void setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles) ;
void setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize) ; void setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize) ;
void setupCityScenario(int nbLines, int nbRank) ; void setupCityScenario(int nbLines, int nbRank) ;
......
...@@ -347,14 +347,14 @@ void Agent::updateObstacleNeighbors() ...@@ -347,14 +347,14 @@ void Agent::updateObstacleNeighbors()
std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[part_.d] ; std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[part_.d] ;
float maxDistObst = 0.0f ; float maxDistObst = 0.0f ;
float maxDistMovingObst = 0.0f ; float maxDistMovingObst = 0.0f ;
float range =1000;
for(std::vector<Obstacle*>::const_iterator it = obst.begin() ; it != obst.end() ; ++it) for(std::vector<Obstacle*>::const_iterator it = obst.begin() ; it != obst.end() ; ++it)
{ {
if ((*it)->mo==NULL) if ((*it)->mo==NULL)
{ {
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ; float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((obstacleNeighbors_.size() < maxMovingObstacles_|| distSq < maxDistObst) if ((obstacleNeighbors_.size() < maxMovingObstacles_|| distSq < maxDistObst)
&& distSq < rangeSq_) && distSq < range)
{ {
// if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1) // if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1)
{ {
...@@ -374,7 +374,7 @@ void Agent::updateObstacleNeighbors() ...@@ -374,7 +374,7 @@ void Agent::updateObstacleNeighbors()
{ {
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ; float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst) if ((movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst)
&& distSq < rangeSq_) && distSq < range)
{ {
// if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1) // if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1)
{ {
...@@ -396,7 +396,7 @@ void Agent::updateObstacleNeighbors() ...@@ -396,7 +396,7 @@ void Agent::updateObstacleNeighbors()
{ {
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ; float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((obstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistObst) if ((obstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistObst)
&&distSq < rangeSq_) &&distSq < range)
{ {
// if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1) // if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1)
{ {
...@@ -412,7 +412,7 @@ void Agent::updateObstacleNeighbors() ...@@ -412,7 +412,7 @@ void Agent::updateObstacleNeighbors()
{ {
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ; float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((movingObstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistMovingObst) if ((movingObstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistMovingObst)
&&distSq < rangeSq_) &&distSq < range)
{ {
// if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1) // if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1)
{ {
...@@ -812,11 +812,7 @@ void Agent::computeNewVelocity() ...@@ -812,11 +812,7 @@ void Agent::computeNewVelocity()
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;
if(sim_->config==0)
obst_radius_infl = 1000.; // scenario 0
else
obst_radius_infl = 40.; // scenario 1 et 3
float force_value; float force_value;
int nobst=0; int nobst=0;
...@@ -833,7 +829,7 @@ void Agent::computeNewVelocity() ...@@ -833,7 +829,7 @@ void Agent::computeNewVelocity()
VEC3 p1=obst->p1 ; VEC3 p1=obst->p1 ;
VEC3 p2=obst->p2 ; VEC3 p2=obst->p2 ;
double longueur2 = (p1-p2).norm2(); double longueur2 = (p1-p2).norm2();
double rest_sum_of_dists = 2 * sqrt(obst_radius_infl*obst_radius_infl + longueur2/4); double rest_sum_of_dists = 2 * sqrt(obst->obst_radius_infl_agent*obst->obst_radius_infl_agent + longueur2/4); // constante de l'ellipse
VEC3 p = getPosition(); VEC3 p = getPosition();
double d1 = (p-p1).norm(); double d1 = (p-p1).norm();
...@@ -844,10 +840,17 @@ void Agent::computeNewVelocity() ...@@ -844,10 +840,17 @@ void Agent::computeNewVelocity()
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->obst_stiffness_agent*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;
// Ajouter une composante tangentielle VEC3 normal;
normal += v_obst * ((d1-d2)/(5*sum_of_dists)); ///new vector tangente à l'ellipse
// Le facteur 5 est là seulement pour diminuer l'influence de la composante tangentielle VEC3 bc = p-p2;
normal =((bc.norm())/sum_of_dists)*v_obst+bc;
// previous vector
// 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
//
normal.normalize(); normal.normalize();
forces += force_value * normal; forces += force_value * normal;
} }
......
#include "articulated_obstacle.h" #include "articulated_obstacle.h"
ArticulatedObstacle::ArticulatedObstacle(Simulator* sim, int index, int currentIndex, std::vector<PFP::VEC3> * pos, int size , std::vector<VEC3> goals) ArticulatedObstacle::ArticulatedObstacle(Simulator* sim, int index, int currentIndex, std::vector<PFP::VEC3> * pos, int size , std::vector<VEC3> goals,int curGoal)
{ {
this->index=index; this->index=index;
nbBodyPart = size; nbBodyPart = size;
MovingObstacle * mo4= new MovingObstacle(sim,currentIndex+1 ,pos[0],goals,true,true,1, NIL,this,0); MovingObstacle * mo4= new MovingObstacle(sim,currentIndex+1 ,pos[0],goals,true,true,curGoal, NIL,this,0);
members.push_back(mo4); members.push_back(mo4);
for(int i =1; i<nbBodyPart; i++) for(int i =1; i<nbBodyPart; i++)
{ {
......
...@@ -96,8 +96,7 @@ void EnvMap::init(unsigned int config, REAL width, REAL height, REAL minSize, RE ...@@ -96,8 +96,7 @@ void EnvMap::init(unsigned int config, REAL width, REAL height, REAL minSize, RE
// CityGenerator::generateCity<PFP>(*this,10,500.0f) ; // CityGenerator::generateCity<PFP>(*this,10,500.0f) ;
break ; break ;
case 4 : case 4 :
CityGenerator::generatePlanet<PFP>(*this) ; CityGenerator::generateGrid<PFP>(*this) ;
CityGenerator::generateCity<PFP>(*this,10,500.0f) ;
break ; break ;
case 5 : case 5 :
{ {
......
...@@ -24,8 +24,8 @@ constrainedV(map) ...@@ -24,8 +24,8 @@ constrainedV(map)
#endif #endif
position = map.getAttribute<VEC3, VERTEX>(attrNames[0]) ; position = map.getAttribute<VEC3, VERTEX>(attrNames[0]) ;
float area = Algo::Surface::Geometry::convexFaceArea<PFP>(envMap.map, d, envMap.position); // float area = Algo::Surface::Geometry::convexFaceArea<PFP>(envMap.map, d, envMap.position);
scaleValue = std::max(area/1400.0f,2.0f); scaleValue = 2.0f;
// std::cout << "scaleVal " << scaleValue << std::endl; // std::cout << "scaleVal " << scaleValue << std::endl;
// scale(scaleValue/1.8f); // scale(scaleValue/1.8f);
...@@ -232,7 +232,7 @@ std::vector<VEC3> MovingMesh::computeProjectedPointSet(float maxHeight, Dart d) ...@@ -232,7 +232,7 @@ std::vector<VEC3> MovingMesh::computeProjectedPointSet(float maxHeight, Dart d)
std::vector<bool> active; std::vector<bool> active;
active.assign(points.size(), true); active.assign(points.size(), true);
simplifyCurve(points, active, 0, points.size()-1, 0.5f); simplifyCurve(points, active, 0, points.size()-1, 0.1f);
std::vector<VEC3> res; std::vector<VEC3> res;
VEC3 center =Algo::Surface::Geometry::faceCentroid<PFP>(motherMap, d, motherPosition); VEC3 center =Algo::Surface::Geometry::faceCentroid<PFP>(motherMap, d, motherPosition);
......
...@@ -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_ = 1.0f; float MovingObstacle::maxSpeed_ = 0.9f;
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;
...@@ -164,7 +164,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s ...@@ -164,7 +164,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
index_parent(indParent), index_parent(indParent),
gravity_dist(0), gravity_dist(0),
beta(0.95), beta(0.95),
alpha(0.99) alpha(0.99),
speed_detection_factor(100)
{ {
...@@ -177,7 +178,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s ...@@ -177,7 +178,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
if(dInside==NIL) if(dInside==NIL)
dInside = sim_->envMap_.getBelongingCell(pos[0]); dInside = sim_->envMap_.getBelongingCell(pos[0]);
if(rigid_) if(rigid_&&index_parent<1)
{ {
pos.push_back(center); pos.push_back(center);
pos.push_back(center); pos.push_back(center);
...@@ -197,10 +198,16 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s ...@@ -197,10 +198,16 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
#endif #endif
if(rigid_&&index_parent<1)
belonging_cells = new std::vector<Dart>[nbParticles]; {
neighbor_cells = new std::vector<Dart>[nbParticles]; belonging_cells = new std::vector<Dart>[nbParticles-1];
neighbor_cells = new std::vector<Dart>[nbParticles-1];
}
else
{
belonging_cells = new std::vector<Dart>[nbParticles];
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") ;
velocity = map.addAttribute<VEC3, VERTEX>("velocity") ; velocity = map.addAttribute<VEC3, VERTEX>("velocity") ;
...@@ -246,40 +253,40 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s ...@@ -246,40 +253,40 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
if ( parent==NULL) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations if ( parent==NULL) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
{ {
// VEC3 axeZ=VEC3 (0,0,1); VEC3 axeZ=VEC3 (0,0,1);
//#ifdef TWO_AND_HALF_DIM #ifdef TWO_AND_HALF_DIM
//
// VEC3 normale = Algo::Surface::Geometry::faceNormal<PFP>(sim->envMap_.map, parts_[nbVertices]->d, sim->envMap_.position); VEC3 normale = Algo::Surface::Geometry::faceNormal<PFP>(sim->envMap_.map, parts_[0]->d, sim->envMap_.position);
//#else #else
// VEC3 normale =axeZ; VEC3 normale =axeZ;
//#endif #endif
// Geom::Matrix44f rot ; Geom::Matrix44f rot ;
// rot.identity() ; rot.identity() ;
// angle = Geom::angle(goals_[curGoal_] - center,front - center); angle = Geom::angle(goals_[curGoal_] - center,front - center);
// Geom::rotate(axeZ,angle,rot); Geom::rotate(axeZ,angle,rot);
//
// float angle1 = Geom::angle(normale, VEC3 (0,0,1) ) ; float angle1 = Geom::angle(normale, VEC3 (0,0,1) ) ;
// // CGoGNout<<"angle : "<<angle<<CGoGNendl; // CGoGNout<<"angle : "<<angle<<CGoGNendl;
// 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);
// Geom::rotate(axis, angle1, rot) ; Geom::rotate(axis, angle1, rot) ;
//
//
//// std::cout<<" angle : "<< angle; // std::cout<<" angle : "<< angle;
//
// for (unsigned int i = 0; i < nbVertices; ++i) for (unsigned int i = 0; i < nbVertices; ++i)
// { {
//// std::cout<<" || pos[i] avant : "<< pos [i]; // std::cout<<" || pos[i] avant : "<< pos [i];
// Geom::transform(pos[i],rot); Geom::transform(pos[i],rot);
//// std::cout<<" || pos[i] APRES : "<< pos [i]<<std::endl; // std::cout<<" || pos[i] APRES : "<< pos [i]<<std::endl;
//
// parts_[i]->move(pos[i]); parts_[i]->move(pos[i]);
//
//
// } }
// angle=0; angle=0;
// front=(pos[0] + pos[1]) / 2; front=(pos[0] + pos[1]) / 2;
} }
groundFace = map.newFace(nbVertices); groundFace = map.newFace(nbVertices);
...@@ -379,9 +386,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s ...@@ -379,9 +386,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
{ {
Obstacle* o = new Obstacle(parts_[i]->getPosition(), Obstacle* o = new Obstacle(parts_[i]->getPosition(),
parts_[(i + 1) % nbVertices]->getPosition(), parts_[(i + 1) % nbVertices]->getPosition(),
this, i); this, i,1000,0.01,100,12);
o->obst_stiffness_obst=0.5;
o->obst_stiffness_agent=100;
obstacles_.push_back(o); 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();
...@@ -391,27 +396,25 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s ...@@ -391,27 +396,25 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
} }
/////obstacles lointain /////obstacles lointain
if(rigid_) if(rigid_&&index_parent<1)
{ {
Obstacle* o = new Obstacle(parts_[nbVertices]->getPosition(), Obstacle* o = new Obstacle(parts_[nbVertices]->getPosition(),
parts_[nbVertices+1]->getPosition(), parts_[nbVertices+1]->getPosition(),
this, nbVertices); this, nbVertices,1000,100,500,20);
o->obst_stiffness_obst=100;
o->obst_stiffness_agent=1000;
obstacles_.push_back(o); obstacles_.push_back(o);
// CGoGNout<<" obstacle :"<< i << " num : "<< o<<CGoGNendl; // CGoGNout<<" obstacle :"<< i << " num : "<< o<<CGoGNendl;
sim_->envMap_.pushObstacleInCells(o); sim_->envMap_.pushObstacleInCells(o);
Obstacle* o2 = new Obstacle(parts_[nbVertices+1]->getPosition(), // Obstacle* o2 = new Obstacle(parts_[nbVertices+1]->getPosition(),
parts_[nbVertices]->getPosition(), // parts_[nbVertices]->getPosition(),
this, nbVertices+1); // this, nbVertices+1,1000,100,2500,20);
o2->obst_stiffness_obst=100; //
o2->obst_stiffness_agent=1000; // obstacles_.push_back(o2);
obstacles_.push_back(o2); //
// // CGoGNout<<" obstacle :"<< i << " num : "<< o<<CGoGNendl;
// CGoGNout<<" obstacle :"<< i << " num : "<< o<<CGoGNendl; // sim_->envMap_.pushObstacleInCells(o2);
sim_->envMap_.pushObstacleInCells(o2);
/// fin ajout /// fin ajout
...@@ -766,10 +769,9 @@ void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance p ...@@ -766,10 +769,9 @@ void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance p
if (/*(movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst) &&*/ if (/*(movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst) &&*/
distSq < distance_detectionSq) distSq < distance_detectionSq)
{ {
// 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)) ; movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
} }
...@@ -807,10 +809,9 @@ void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance p ...@@ -807,10 +809,9 @@ void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance p
if (/*(movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst) &&*/ if (/*(movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst) &&*/
distSq < distance_detectionSq) distSq < distance_detectionSq)
{ {
// 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)) ; movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
} }
} }
...@@ -953,10 +954,16 @@ VEC3 computeForce(VEC3 p, VEC3 p1, VEC3 p2, float obst_radius_infl, float obst_p ...@@ -953,10 +954,16 @@ 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); 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); force_value = obst_stiffness*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;
// Ajouter une composante tangentielle ///new vector tangente à l'ellipse
VEC3 bc = p-p2;
normal =((bc.norm())/sum_of_dists)*v_obst+bc;
normal += v_obst * ((d1-d2)/sum_of_dists); // //vector arash
// normal= normFace^v_obst;
// // Ajouter une composante tangentielle
//
// normal += v_obst * ((d1-d2)/sum_of_dists);
// normal += v_obst * ((d1-d2)/(5*sum_of_dists)); // normal += v_obst * ((d1-d2)/(5*sum_of_dists));
// Le facteur 5 est là seulement pour diminuer l'influence de la composante tangentielle // Le facteur 5 est là seulement pour diminuer l'influence de la composante tangentielle
normal.normalize(); normal.normalize();
...@@ -1000,14 +1007,7 @@ void MovingObstacle::updateForces() ...@@ -1000,14 +1007,7 @@ void MovingObstacle::updateForces()
velocity_ = newVelocity_* velocity_factor; velocity_ = newVelocity_* velocity_factor;
// MAJ des particules // MAJ des particules
float abs_angle= angle > 0 ? 1 : -1;
float rotor=0;
if (index_parent==0)
rotor = abs_angle*angle > 0.04f ? 0.04f : abs_angle*angle ;
else
rotor = abs_angle*angle ;
if(!rigid_) if(!rigid_)
{ {
// // masse ressort pour la limace // // masse ressort pour la limace
...@@ -1143,10 +1143,7 @@ void MovingObstacle::updateForces() ...@@ -1143,10 +1143,7 @@ void MovingObstacle::updateForces()
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;
obst_radius_infl = 30.; // scenario 1 et 3
obst_radius_infl_buildings= 50.0f;
float fixed_obst_factor = 5.0f; float fixed_obst_factor = 5.0f;
d=groundFace; d=groundFace;
...@@ -1166,7 +1163,7 @@ void MovingObstacle::updateForces() ...@@ -1166,7 +1163,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->obst_stiffness_obst,normFace); forces[dd] += computeForce(p,p1,p2,obst->obst_radius_infl_obst,obst_power,obst->obst_stiffness_obst,normFace);
} }
// Evitement d'obstacles fixes // Evitement d'obstacles fixes
...@@ -1178,7 +1175,7 @@ void MovingObstacle::updateForces() ...@@ -1178,7 +1175,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->obst_stiffness_obst,normFace); forces[dd] += computeForce(p,p1,p2,obst->obst_radius_infl_obst,fixed_obst_factor*obst_power,fixed_obst_factor*obst->obst_stiffness_obst,normFace);
} }
d = map.phi_1(d); d = map.phi_1(d);
...@@ -1205,6 +1202,14 @@ void MovingObstacle::updateForces() ...@@ -1205,6 +1202,14 @@ void MovingObstacle::updateForces()
} }
else