Skip to content

GitLab

S
SocialAgents3D
Commit 31b1668a authored by pitiot's avatar pitiot
Browse files
Options

fini ? corrigé limaces 3D

parent bb6a8e0e
Hide whitespace changes
Inline Side-by-side
Showing with 319 additions and 169 deletions
cams/cam.out~
View file @ 31b1668a
2 282.553 -549.754 -431.112 243.297 -467.981 -389.015 0.278767 0.541963 -0.792821
2 380.527 224.803 -813.318 330.469 214.097 -727.414 -0.172437 0.984769 0.0222493
0 2892
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
include/articulated_obstacle.h
View file @ 31b1668a
......@@ -7,7 +7,7 @@ using namespace std;
class ArticulatedObstacle
{
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;
int index;
......
include/env_map.h
View file @ 31b1668a
......@@ -34,7 +34,7 @@ class ArticulatedObstacle;
//#define EXPORTING3
//#define TWO_AND_HALF_DIM
#define TWO_AND_HALF_DIM
#ifdef EXPORTING3
......
include/moving_obstacle.h
View file @ 31b1668a
......@@ -6,8 +6,12 @@
#include "utils.h"
#include "env_map.h"
#include <set>
#define LINEAR
// #define SECURED
#ifdef LINEAR
#include "ShapeMatching/shapeMatchingLinear.h"
#else
......@@ -179,7 +183,7 @@ public:
#endif
PFP::REAL beta;
float alpha;
float speed_detection_factor;
};
......
include/obstacle.h
View file @ 31b1668a
......@@ -7,9 +7,9 @@ class Obstacle
{
public:
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),
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 ;
// p2[2] = 0 ;
......@@ -29,6 +29,8 @@ public:
double obst_stiffness_agent;
double obst_stiffness_obst;
double obst_radius_infl_agent;
double obst_radius_infl_obst;
} ;
#endif
include/simulator.h
View file @ 31b1668a
......@@ -93,6 +93,7 @@ public:
bool reachedGoal() ;
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 setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize) ;
void setupCityScenario(int nbLines, int nbRank) ;
......
src/agent.cpp
View file @ 31b1668a
......@@ -347,14 +347,14 @@ void Agent::updateObstacleNeighbors()
std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[part_.d] ;
float maxDistObst = 0.0f ;
float maxDistMovingObst = 0.0f ;
float range =1000;
for(std::vector<Obstacle*>::const_iterator it = obst.begin() ; it != obst.end() ; ++it)
{
if ((*it)->mo==NULL)
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((obstacleNeighbors_.size() < maxMovingObstacles_|| distSq < maxDistObst)
&& distSq < rangeSq_)
&& distSq < range)
{
// if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1)
{
......@@ -374,7 +374,7 @@ void Agent::updateObstacleNeighbors()
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst)
&& distSq < rangeSq_)
&& distSq < range)
{
// if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1)
{
......@@ -396,7 +396,7 @@ void Agent::updateObstacleNeighbors()
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((obstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistObst)
&&distSq < rangeSq_)
&&distSq < range)
{
// if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1)
{
......@@ -412,7 +412,7 @@ void Agent::updateObstacleNeighbors()
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((movingObstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistMovingObst)
&&distSq < rangeSq_)
&&distSq < range)
{
// if (sim_->envMap_.testOrientation(part_.getPosition(), (*it)->p1, (*it)->p2, part_.d) == 1)
{
......@@ -812,11 +812,7 @@ void Agent::computeNewVelocity()
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;
int nobst=0;
......@@ -833,7 +829,7 @@ void Agent::computeNewVelocity()
VEC3 p1=obst->p1 ;
VEC3 p2=obst->p2 ;
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();
double d1 = (p-p1).norm();
......@@ -844,10 +840,17 @@ void Agent::computeNewVelocity()
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);
VEC3 v_obst = p2 - p1;
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
VEC3 normal;
///new vector tangente à l'ellipse
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();
forces += force_value * normal;
}
......
src/articulated_obstacle.cpp
View file @ 31b1668a
#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;
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);
for(int i =1; i<nbBodyPart; i++)
{
......
src/env_map.cpp
View file @ 31b1668a
......@@ -96,8 +96,7 @@ void EnvMap::init(unsigned int config, REAL width, REAL height, REAL minSize, RE
// CityGenerator::generateCity<PFP>(*this,10,500.0f) ;
break ;
case 4 :
CityGenerator::generatePlanet<PFP>(*this) ;
CityGenerator::generateCity<PFP>(*this,10,500.0f) ;
CityGenerator::generateGrid<PFP>(*this) ;
break ;
case 5 :
{
......
src/moving_mesh.cpp
View file @ 31b1668a
......@@ -24,8 +24,8 @@ constrainedV(map)
#endif
position = map.getAttribute<VEC3, VERTEX>(attrNames[0]) ;
float area = Algo::Surface::Geometry::convexFaceArea<PFP>(envMap.map, d, envMap.position);
scaleValue = std::max(area/1400.0f,2.0f);
// float area = Algo::Surface::Geometry::convexFaceArea<PFP>(envMap.map, d, envMap.position);
scaleValue = 2.0f;
// std::cout << "scaleVal " << scaleValue << std::endl;
// scale(scaleValue/1.8f);
......@@ -232,7 +232,7 @@ std::vector<VEC3> MovingMesh::computeProjectedPointSet(float maxHeight, Dart d)
std::vector<bool> active;
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;
VEC3 center =Algo::Surface::Geometry::faceCentroid<PFP>(motherMap, d, motherPosition);
......
src/moving_obstacle.cpp
View file @ 31b1668a
......@@ -9,7 +9,7 @@
//float MovingObstacle::neighborDistSq_ = 5.0f * 5.0f;
float MovingObstacle::maxSpeed_ = 1.0f;
float MovingObstacle::maxSpeed_ = 0.9f;
float MovingObstacle::neighborDist_ = 10.0f ;
float MovingObstacle::neighborDistSq_ = neighborDist_ * neighborDist_ ;
float MovingObstacle::timeHorizonObst_ = 10.0f;
......@@ -164,7 +164,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
index_parent(indParent),
gravity_dist(0),
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
if(dInside==NIL)
dInside = sim_->envMap_.getBelongingCell(pos[0]);
if(rigid_)
if(rigid_&&index_parent<1)
{
pos.push_back(center);
pos.push_back(center);
......@@ -197,10 +198,16 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
#endif
belonging_cells = new std::vector<Dart>[nbParticles];
neighbor_cells = new std::vector<Dart>[nbParticles];
if(rigid_&&index_parent<1)
{
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") ;
normal = map.addAttribute<VEC3, VERTEX>("normal") ;
velocity = map.addAttribute<VEC3, VERTEX>("velocity") ;
......@@ -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
{
// VEC3 axeZ=VEC3 (0,0,1);
//#ifdef TWO_AND_HALF_DIM
//
// VEC3 normale = Algo::Surface::Geometry::faceNormal<PFP>(sim->envMap_.map, parts_[nbVertices]->d, sim->envMap_.position);
//#else
// VEC3 normale =axeZ;
//#endif
// Geom::Matrix44f rot ;
// rot.identity() ;
// angle = Geom::angle(goals_[curGoal_] - center,front - center);
// Geom::rotate(axeZ,angle,rot);
//
// float angle1 = Geom::angle(normale, VEC3 (0,0,1) ) ;
// // CGoGNout<<"angle : "<<angle<<CGoGNendl;
// VEC3 axis = VEC3(0,0,1) ^ normale ;
//
// // Geom::translate(center[0],center[1],center[2],rot);
// Geom::rotate(axis, angle1, rot) ;
//
//
//// std::cout<<" angle : "<< angle;
//
// for (unsigned int i = 0; i < nbVertices; ++i)
// {
//// std::cout<<" || pos[i] avant : "<< pos [i];
// Geom::transform(pos[i],rot);
//// std::cout<<" || pos[i] APRES : "<< pos [i]<<std::endl;
//
// parts_[i]->move(pos[i]);
//
//
// }
// angle=0;
// front=(pos[0] + pos[1]) / 2;
VEC3 axeZ=VEC3 (0,0,1);
#ifdef TWO_AND_HALF_DIM
VEC3 normale = Algo::Surface::Geometry::faceNormal<PFP>(sim->envMap_.map, parts_[0]->d, sim->envMap_.position);
#else
VEC3 normale =axeZ;
#endif
Geom::Matrix44f rot ;
rot.identity() ;
angle = Geom::angle(goals_[curGoal_] - center,front - center);
Geom::rotate(axeZ,angle,rot);
float angle1 = Geom::angle(normale, VEC3 (0,0,1) ) ;
// CGoGNout<<"angle : "<<angle<<CGoGNendl;
VEC3 axis = VEC3(0,0,1) ^ normale ;
// Geom::translate(center[0],center[1],center[2],rot);
Geom::rotate(axis, angle1, rot) ;
// std::cout<<" angle : "<< angle;
for (unsigned int i = 0; i < nbVertices; ++i)
{
// std::cout<<" || pos[i] avant : "<< pos [i];
Geom::transform(pos[i],rot);
// std::cout<<" || pos[i] APRES : "<< pos [i]<<std::endl;
parts_[i]->move(pos[i]);
}
angle=0;
front=(pos[0] + pos[1]) / 2;
}
groundFace = map.newFace(nbVertices);
......@@ -379,9 +386,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
{
Obstacle* o = new Obstacle(parts_[i]->getPosition(),
parts_[(i + 1) % nbVertices]->getPosition(),
this, i);
o->obst_stiffness_obst=0.5;
o->obst_stiffness_agent=100;
this, i,1000,0.01,100,12);
obstacles_.push_back(o);
/////definition du troisieme point
if (rigid_) o->p3=parts_[(i + 2) % nbVertices]->getPosition();
......@@ -391,27 +396,25 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
}
/////obstacles lointain
if(rigid_)
if(rigid_&&index_parent<1)
{
Obstacle* o = new Obstacle(parts_[nbVertices]->getPosition(),
parts_[nbVertices+1]->getPosition(),
this, nbVertices);
o->obst_stiffness_obst=100;
o->obst_stiffness_agent=1000;
this, nbVertices,1000,100,500,20);
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);
// Obstacle* o2 = new Obstacle(parts_[nbVertices+1]->getPosition(),
// parts_[nbVertices]->getPosition(),
// this, nbVertices+1,1000,100,2500,20);
//
// obstacles_.push_back(o2);
//
// // CGoGNout<<" obstacle :"<< i << " num : "<< o<<CGoGNendl;
// sim_->envMap_.pushObstacleInCells(o2);
/// fin ajout
......@@ -766,10 +769,9 @@ void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance p
if (/*(movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst) &&*/
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)) ;
}
......@@ -807,10 +809,9 @@ void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance p
if (/*(movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst) &&*/
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)) ;
}
}
......@@ -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);
force_value = obst_stiffness*collision_softening_factor*(rest_sum_of_dists - sum_of_dists);
VEC3 v_obst = p2 - p1;
VEC3 normal = normFace^v_obst;
// Ajouter une composante tangentielle
VEC3 normal;
///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));
// Le facteur 5 est là seulement pour diminuer l'influence de la composante tangentielle
normal.normalize();
......@@ -1000,14 +1007,7 @@ void MovingObstacle::updateForces()
velocity_ = newVelocity_* velocity_factor;
// 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_)
{
// // masse ressort pour la limace
......@@ -1143,10 +1143,7 @@ void MovingObstacle::updateForces()
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;
d=groundFace;
......@@ -1166,7 +1163,7 @@ void MovingObstacle::updateForces()
VEC3 p1=obst->p1 ;
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
......@@ -1178,7 +1175,7 @@ void MovingObstacle::updateForces()
VEC3 p1=obst->p2 ;
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);
......@@ -1205,6 +1202,14 @@ void MovingObstacle::updateForces()
}
else
{
float abs_angle= angle > 0 ? 1 : -1;
float rotor=0;
if (index_parent==0)
rotor = abs_angle*angle > 0.01f ? 0.01f : abs_angle*angle ;
else
rotor = abs_angle*angle ;
// CGoGNout << "Obstacle "<< index << CGoGNendl;
// CGoGNout << "vitesse : "<< velocity_ << CGoGNendl;
// on fait tourner l'obstacle
......@@ -1300,7 +1305,7 @@ void MovingObstacle::applyForces()
// velocity[d] *= maxSpeed_;
// }
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.8*velocity[d] * sim_->timeStep_);
// PFP::VEC3 normal = CGoGN::Algo::Surface::Geometry::faceNormal<PFP>(sim_->envMap_.map, parts_[i]->d, sim_->envMap_.position);
// normal *= height;
// position[map.phi_1(map.phi2(d))] = position[d]+normal;
......@@ -1313,6 +1318,9 @@ void MovingObstacle::applyForces()
for (unsigned int i = 0; i < nbVertices; ++i)
{
position[d] += alpha*(shape_->goal[d]-position[d]);
#ifndef TWO_AND_HALF_DIM
position[d][2]=0;
#endif
position [map.phi_1(map.phi2(d))]+=alpha*(shape_->goal[map.phi_1(map.phi2(d))]-position[map.phi_1(map.phi2(d))]);
// PFP::VEC3 normal = CGoGN::Algo::Surface::Geometry::faceNormal<PFP>(sim_->envMap_.map, parts_[i]->d, sim_->envMap_.position);
// normal *= height;
......@@ -1337,23 +1345,15 @@ void MovingObstacle::updateRegistration()
Dart d = groundFace;
for (unsigned int i = 0; i < nbVertices; ++i)
{
parts_[i]->move(position[d]);
#ifdef TWO_AND_HALF_DIM
// CGoGNout<<"avant : "<<position[d];
position[d] = parts_[i]->getPosition(); //recalage de l'obstacle sur ses particules (qui elles ont bien suivi la carte au sol)
// CGoGNout<<" || apres : "<<position[d]<<CGoGNendl;
#endif
bary += position[d];
d=map.phi_1(d);
}
center = bary / nbVertices;
}
else
......@@ -1370,11 +1370,15 @@ void MovingObstacle::updateRegistration()
normal *= height;
position[map.phi_1(d)] = position[d]+normal;
}
//////ajouts evitement lointain
if(index_parent<1)
{
parts_[nbVertices]->move(center);
parts_[nbVertices+1]->move(center+speed_detection_factor*velocity_);
}
}
//////ajouts evitement lointain
parts_[nbVertices]->move(center);
parts_[nbVertices+1]->move(center+100*velocity_);
......@@ -1404,26 +1408,28 @@ void MovingObstacle::updateRegistration()
// CGoGNout<< CGoGNendl;
// CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl;
}
Obstacle* o = obstacles_[nbVertices];
o->p1 = parts_[nbVertices]->getPosition();
o->p2 = parts_[nbVertices+1]->getPosition();
Dart d1 = parts_[nbVertices]->d;
Dart d2 = parts_[nbVertices+1]->d;
if(!((sim_->envMap_.map.sameFace(d1,d2))&& (parts_[nbVertices]->crossCell==CGoGN::Algo::Surface::MovingObjects::NO_CROSS && parts_[nbVertices+1]->crossCell==CGoGN::Algo::Surface::MovingObjects::NO_CROSS)))
if(rigid_ &&index_parent<1)
{
sim_->envMap_.popAndPushObstacleInCells(o,nbVertices);
}
Obstacle* o2 = obstacles_[nbVertices+1];
o2->p1 = parts_[nbVertices+1]->getPosition();
o2->p2 = parts_[nbVertices]->getPosition();
d1 = parts_[nbVertices+1]->d;
d2 = parts_[nbVertices]->d;
Obstacle* o = obstacles_[nbVertices];
o->p1 = parts_[nbVertices]->getPosition();
o->p2 = parts_[nbVertices+1]->getPosition();
Dart d1 = parts_[nbVertices]->d;
Dart d2 = parts_[nbVertices+1]->d;
if(!((sim_->envMap_.map.sameFace(d1,d2))&& (parts_[nbVertices+1]->crossCell==CGoGN::Algo::Surface::MovingObjects::NO_CROSS && parts_[nbVertices]->crossCell==CGoGN::Algo::Surface::MovingObjects::NO_CROSS)))
{
sim_->envMap_.popAndPushObstacleInCells(o2,nbVertices+1);
if(!((sim_->envMap_.map.sameFace(d1,d2))&& (parts_[nbVertices]->crossCell==CGoGN::Algo::Surface::MovingObjects::NO_CROSS && parts_[nbVertices+1]->crossCell==CGoGN::Algo::Surface::MovingObjects::NO_CROSS)))
{
sim_->envMap_.popAndPushObstacleInCells(o,nbVertices);
}
// Obstacle* o2 = obstacles_[nbVertices+1];
// o2->p1 = parts_[nbVertices+1]->getPosition();
// o2->p2 = parts_[nbVertices]->getPosition();
// d1 = parts_[nbVertices+1]->d;
// d2 = parts_[nbVertices]->d;
//
// if(!((sim_->envMap_.map.sameFace(d1,d2))&& (parts_[nbVertices+1]->crossCell==CGoGN::Algo::Surface::MovingObjects::NO_CROSS && parts_[nbVertices]->crossCell==CGoGN::Algo::Surface::MovingObjects::NO_CROSS)))
// {
// sim_->envMap_.popAndPushObstacleInCells(o2,nbVertices+1);
// }
}
......@@ -1618,7 +1624,7 @@ void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atte
goalDist2 = goalVector.norm2() ;
}
if (goalDist2 > maxSpeed_)
if (goalDist2 > maxSpeed_*maxSpeed_)
{
goalVector.normalize() ;
goalVector *= maxSpeed_;
......@@ -1669,42 +1675,46 @@ void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atte
// std::cout << "dDir " << dDir << " goalVec" << goalVector << std::endl;
// float goalDist2 = goalVector.norm2() ;
// if (goalDist2 > maxSpeed_)
float goalDist2 = goalVector.norm2() ;
if (goalDist2 > maxSpeed_*maxSpeed_)
{
goalVector.normalize() ;
goalVector *= maxSpeed_*maxSpeed_;
goalVector *= maxSpeed_;
}
}
}
}
else //articulated