Skip to content

GitLab

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

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