Skip to content

GitLab

Commit c5ea16b4 authored by Thomas Jund's avatar Thomas Jund
Browse files

ajout de particule secured pour les moving obst

parent ed23a69b
Hide whitespace changes
Inline Side-by-side
Showing with 90 additions and 159 deletions
include/moving_obstacle.h
View file @ c5ea16b4
......@@ -7,11 +7,16 @@
#include "env_map.h"
#include <set>
// #define SECURED
#ifdef TWO_AND_HALF_DIM
#include "Algo/MovingObjects/particle_cell_2DandHalf_memo.h"
#else
#ifdef SECURED
#include "Algo/MovingObjects/particle_cell_2D_secured.h"
#else
#include "Algo/MovingObjects/particle_cell_2D_memo.h"
#endif
#endif
#define EXPORTING_BOXES
......@@ -69,10 +74,15 @@ public:
unsigned int nbVertices;
#ifdef TWO_AND_HALF_DIM
CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>* * parts_;
#else
CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>* * parts_;
#ifdef SECURED
CGoGN::Algo::Surface::MovingObjects::ParticleCell2DSecured<PFP> * * parts_ ;
#else
#ifdef TWO_AND_HALF_DIM
CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP> * * parts_ ;
#else
CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>* * parts_ ;
#endif
#endif
PFP::MAP map;
......@@ -147,7 +157,7 @@ public:
bool rigid_;
bool spinning;
ArticulatedObstacle * parent;
float hight;
float height;
MovingMesh* mm_;
Agent* ag_;
int index_parent;
......
src/moving_obstacle.cpp
View file @ c5ea16b4
......@@ -146,25 +146,18 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
if(dInside==NIL)
dInside = sim_->envMap_.getBelongingCell(pos[0]);
unsigned int nbParticles = nbVertices;
if(!rigid);
nbParticles +=1; //a center particle for the mass-spring
#ifdef TWO_AND_HALF_DIM
if(!rigid_)
{
parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>*[nbVertices+1];
}
else
{
parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>*[nbVertices];
}
parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>*[nbParticles];
#else
if(!rigid_)
{
parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>*[nbVertices+1];
}
else
{
parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>*[nbVertices];
}
#ifdef SECURED
parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DSecured<PFP>*[nbParticles];
#else
parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>*[nbParticles];
#endif
#endif
obstacles_ = new Obstacle*[nbVertices];
......@@ -186,82 +179,40 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
for (unsigned int i = 0; i < nbVertices; ++i)
{
#ifdef TWO_AND_HALF_DIM
Dart d = dInside;/*sim_->envMap_.getBelongingCellOnSurface(pos[i]); TO CHECK*/
// CGoGNout<<" d trouvée :"<< d <<CGoGNendl;
CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>* part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>(sim_->envMap_.map, d, pos[i], sim_->envMap_.position);
Dart d = dInside;
parts_[i] = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>(sim_->envMap_.map, d, pos[i], sim_->envMap_.position);
#else
Dart d = sim_->envMap_.getBelongingCell(pos[i]);
CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>* part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>(sim_->envMap_.map, d, pos[i], sim_->envMap_.position);
#ifdef SECURED
parts_[i] = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DSecured<PFP>(sim_->envMap_.map, d, pos[i], sim_->envMap_.position);
#else
parts_[i] = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>(sim_->envMap_.map, d, pos[i], sim_->envMap_.position);
#endif
#endif
parts_[i] = part;
center += pos[i];
if(i==0)
dDir = d;
}
center /= nbVertices;
front=(pos[0] + pos[1]) / 2;
center /= nbVertices;
front=(pos[0] + pos[1]) / 2;
if(!rigid_)
{
#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;
}
parts_[nbVertices] = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>(sim_->envMap_.map, dInside, center, sim_->envMap_.position);
#else
if(!rigid_)
{
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;
}
#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
// M appartient à l'ellipse ssi MF1 + MF2 = sum_dist_foci est une constante
// où F1 et F2 sont les deux foyers.
// length = (pos[0]-pos[1]).norm();
// width = (pos[1]-pos[2]).norm();
// sum_dist_foci_rest = 2*(length + width*(sqrt(2)-0.5));
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;
}
#endif
}
groundFace = map.newFace(nbVertices);
for (unsigned int i = 0; i < nbVertices; ++i)
......@@ -280,24 +231,16 @@ if(!rigid_)
velocity[d] = VEC3(0);
forces[d] = VEC3(0);
}
}
//extrude face to build a cage
// compute edgeLength for mass-spring
hight=rigid_ ? 10.0f : -10.0f;
Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, hight) ;
// Dart dT = map.phi1(map.phi1(map.phi2(Dart(0))));
// std::cout << __FUNCTION__ << " val " << dT << std::endl;
// position[dT]= VEC3(0);
// std::cout << __FUNCTION__ << " pos " << position[dT] << std::endl;
// Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, 0.0f) ;
height=rigid_ ? 10.0f : 10.0f;
Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, height) ;
map.fillHole(groundFace);
groundFace = map.phi2(groundFace);
if(!rigid_)
{
Dart d = Algo::Surface::Modelisation::trianguleFace<PFP>(map,groundFace);
......@@ -891,9 +834,6 @@ void MovingObstacle::updateForces()
Dart d;
velocity_ = newVelocity_* velocity_factor;
// velocity_[0] = 0.0;
// velocity_[1] = 0.0;
// MAJ des particules
float abs_angle= angle > 0 ? 1 : -1;
......@@ -916,7 +856,6 @@ void MovingObstacle::updateForces()
Dart e = map.phi1(d);
//initialisation of forces with viscosity
forces[e] += -0.9f*velocity[e];
// forces[dd] = VEC3(0.0);
VEC3 p1Next = position[map.phi1(e)]+(velocity[map.phi1(e)] * sim_->timeStep_); // ressorts sur le bord
VEC3 p2Next = position[e]+(velocity[e] * sim_->timeStep_);
// p1Next et p2Next sont la position des extremites de l'arete.
......@@ -924,7 +863,7 @@ void MovingObstacle::updateForces()
//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
float norm = v1.norm();
float rigidity = 50.0f;
float rigidity = 10.0f;
float stretch = 4*rigidity*(edgeLength[e]-norm);
float angularStretch = 0, angularStretch2 = 0;
......@@ -1178,96 +1117,77 @@ void MovingObstacle::applyForces()
velocity[e] += forces[e] * sim_->timeStep_;
position[e] += (velocity[e] * sim_->timeStep_);
d=map.phi<21>(d);
}
velocity[centerDart] +=forces[centerDart] * sim_->timeStep_;
position[centerDart] += (velocity[centerDart] * sim_->timeStep_);
// MAJ des obstacles
}
}
void MovingObstacle::updateRegistration()
{
PFP::VEC3 bary(0);
Dart centerDart =map.phi<11>(groundFace);
Dart centerDart =map.phi<11>(groundFace);
if(!rigid_)
if(!rigid_)
{
Dart d = centerDart;
for (unsigned int i = 0; i < nbVertices; ++i)
{
Dart d = centerDart;
for (unsigned int i = 0; i < nbVertices; ++i)
{
Dart e = map.phi_1(d);
Dart e = map.phi_1(d);
parts_[i]->move(position[e]);
#ifdef TWO_AND_HALF_DIM
parts_[i]->move(position[e]);
position[e] = parts_[i]->getPosition(); //recalage de l'obstacle sur ses particules (qui elles ont bien suivi la carte au sol)
#ifdef TWO_AND_HALF_DIM
PFP::VEC3 normal = CGoGN::Algo::Surface::Geometry::faceNormal<PFP>(sim_->envMap_.map, parts_[i]->d, sim_->envMap_.position);
normal *= hight;
// std::cout << " phi11 d : " << map.phi<112>(d) <<" || d : "<<d<< std::endl;
position[map.phi_1(map.phi<12>(d))] = position[e]+normal;
position[e] = parts_[i]->getPosition(); //recalage de l'obstacle sur ses particules (qui elles ont bien suivi la carte au sol)
#endif
bary += position[e];
d=map.phi<21>(d);
}
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.phi<12>(d))] = position[e]+normal;
center = bary / nbVertices;
parts_[nbVertices]->move(position[centerDart]);
#endif
bary += position[e];
d=map.phi<21>(d);
}
#ifdef TWO_AND_HALF_DIM
position[centerDart] = parts_[nbVertices]->getPosition(); //recalage de l'obstacle sur ses particules (qui elles ont bien suivi la carte au sol)
#endif
// MAJ des obstacles
center = bary / nbVertices;
parts_[nbVertices]->move(position[centerDart]);
#ifdef TWO_AND_HALF_DIM
position[centerDart] = parts_[nbVertices]->getPosition(); //recalage de l'obstacle sur ses particules (qui elles ont bien suivi la carte au sol)
#endif
}
}
else
{
for (unsigned int i = 0; i < nbVertices; ++i)
{
for (unsigned int i = 0; i < nbVertices; ++i)
{
VEC3 pos = getDilatedPosition(i);
parts_[i]->move(pos);
VEC3 pos = getDilatedPosition(i);
parts_[i]->move(pos);
#ifdef TWO_AND_HALF_DIM
#ifdef TWO_AND_HALF_DIM
Dart d(i);
position[d] = parts_[i]->getPosition(); //recalage de l'obstacle sur ses particules (qui elles ont bien suivi la carte au sol)
Dart d(i);
position[d] = parts_[i]->getPosition(); //recalage de l'obstacle sur ses particules (qui elles ont bien suivi la carte au sol)
PFP::VEC3 normal = CGoGN::Algo::Surface::Geometry::faceNormal<PFP>(sim_->envMap_.map, parts_[i]->d, sim_->envMap_.position);
normal *= hight;
// std::cout << " phi11 d : " << map.phi<112>(d) <<" || d : "<<d<< std::endl;
position[map.phi_1(d)] = position[d]+normal;
PFP::VEC3 normal = CGoGN::Algo::Surface::Geometry::faceNormal<PFP>(sim_->envMap_.map, parts_[i]->d, sim_->envMap_.position);
normal *= hight;
// std::cout << " phi11 d : " << map.phi<112>(d) <<" || d : "<<d<< std::endl;
position[map.phi_1(d)] = position[d]+normal;
#endif
}
#endif
}
}
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];
o->p1 = getDilatedPosition(i);
o->p2 = getDilatedPosition((i+1) % nbVertices);
......@@ -1285,7 +1205,7 @@ void MovingObstacle::updateRegistration()
sim_->envMap_.popAndPushObstacleInCells(o,i);
}
/////affichage des belonging cells
/////affichage des belonging cells
// CGoGNout<< CGoGNendl;
// CGoGNout << "Obstacle "<< i << ": ";
// for(std::vector<Dart>::iterator ite = belonging_cells[i].begin();ite!=belonging_cells[i].end(); ++ite)
......@@ -1293,8 +1213,9 @@ void MovingObstacle::updateRegistration()
// CGoGNout<<(*ite).index<<" ; ";
// }
// CGoGNout<< CGoGNendl;
// CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl;
// CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl;
}
/////affichage du general_belonging
// CGoGNout<< CGoGNendl;
// CGoGNout << "General : ";
......
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