Commit 5de581be authored by pitiot's avatar pitiot
Browse files

p3 obstacles + shapematching debut

parent bb48b7b5
......@@ -54,7 +54,7 @@ void ShapeMatchingLinear<PFP>::animate()
//initialize(nfe,frame_prec);
unsigned int n = m_darts.size();
unsigned int n = this->m_darts.size();
VEC3 xcm = this->computeBarycenterDarts();
......@@ -107,7 +107,7 @@ void ShapeMatchingLinear<PFP>::animate()
//once the transformation computed : move the vertices of the map
//compute barycenter
VEC3 b = this->computeBarycenterDarts();
// VEC3 b = this->computeBarycenterDarts();
//move the vertices
for(std::vector<Dart>::iterator it = this->m_darts.begin() ; it != this->m_darts.end() ; ++it)
......
......@@ -11,7 +11,7 @@
// #define SECURED
//#define EXPORTING_AGENT
#define EXPORTING_OBJ
//#define EXPORTING_OBJ
#define ARASH
......
......@@ -32,9 +32,9 @@ class ArticulatedObstacle;
#include "pfp.h"
#define EXPORTING3
//#define EXPORTING3
#define TWO_AND_HALF_DIM
//#define TWO_AND_HALF_DIM
#ifdef EXPORTING3
......@@ -371,11 +371,30 @@ inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& b
assert(map.getCurrentLevel() == map.getMaxLevel());
assert(!belonging_cells.empty());
neighbor_cells->clear();
CellMarkerMemo<FACE> memo_mark(map);
CellMarkerMemo<FACE> OneRingMark(map);
// CellMarkerMemo<FACE> memo_mark_speed(map);//marqueur additionnel (vitesse)
CellMarkerMemo<FACE> memo_mark(map); //marqueur des cellules "présentes"
CellMarkerMemo<FACE> OneRingMark(map); // marquer des cellules voisines
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;
std::vector<Dart>::const_iterator it=belonging_cells.begin();
Dart beg = NIL;
......
......@@ -18,7 +18,7 @@
using namespace std;
#define EXPORTING2
//#define EXPORTING2
float get_angle3D(VEC3 v1, VEC3 v2);
......
......@@ -2,7 +2,7 @@
#define M_MOVING_OBSTACLE_H
#include <iostream>
#include "ShapeMatching/shapeMatchingLinear.h"
#include "utils.h"
#include "env_map.h"
#include <set>
......@@ -19,7 +19,7 @@
#endif
#endif
// #define EXPORTING_BOXES
#define EXPORTING_BOXES
#ifdef EXPORTING_BOXES
#include "Algo/Render/GL2/mapRender.h"
......@@ -122,6 +122,7 @@ public:
Obstacle* * obstacles_;
std::vector<Dart> * belonging_cells;
std::vector<Dart> * neighbor_cells;
std::vector<std::pair<Dart, int> > general_belonging;
VEC3 front;
......@@ -163,6 +164,14 @@ public:
int index_parent;
float gravity_dist; /// distance entre le centre du MO et son sommet le plus éloigné
VertexAttribute<NoMathIONameAttribute<std::vector<PFP::REAL> > > mvc_;
/////// ShapeMatching
ShapeMatchingLinear<PFP> * shape_;
PFP::REAL beta;
float alpha;
};
#endif
......@@ -9,7 +9,7 @@ public:
Obstacle(const VEC3 point1, const VEC3 point2, const VEC3 prevPoint, const VEC3 nextPoint, Dart d_1, Dart d_2,
MovingObstacle * moving1=NULL, unsigned int ind=0) :
d1(d_1), d2(d_2), p1(point1), p2(point2), prevP(prevPoint), nextP(nextPoint),
mo(moving1), index(ind)
mo(moving1), index(ind),p3(0,0,0)
{
// p1[2] = 0 ;
// p2[2] = 0 ;
......@@ -23,8 +23,10 @@ public:
VEC3 p2 ;
VEC3 prevP ;
VEC3 nextP ;
MovingObstacle * mo ;
unsigned int index ;
VEC3 p3; //pour les ellipses
} ;
#endif
......@@ -100,7 +100,7 @@ public:
void setupPlanetScenario(unsigned int nbAgents, unsigned int nbObstacles, unsigned int nbx = 2,unsigned int nby= 2, float areaMin = 100.0f);
void addMovingObstacles(unsigned int nb, unsigned int type, float areaMin = 1400, int randLimace=12);
void addMovingObstacles(unsigned int nb, unsigned int type, float areaMin = 500, int randLimace=1);
void addMovingObstacle(Dart d, unsigned int obstType=0);
void addAgent(const VEC3& start,const VEC3& goals);
......
......@@ -56,8 +56,8 @@
#include "shaderCustom.h"
#include "shaderPhongTexCust.h"
#define AFFICHE_MESH
#define DRAW_REGISTRATION //montre l'enregistrement des obstacles mobiles
//#define AFFICHE_MESH
//#define EXPORTING
//#define ONERING
//#define SHADOWSHELL
......@@ -82,6 +82,7 @@ public:
void updateObstacleVBO();
void updateAgentPredTriVBO();
void updateObstaclePredTriVBO();
void drawCell(Dart d, float width, float r, float g, float b);
void moveCameraTo(VEC3 newPos);
void cb_redraw() ;
......
......@@ -169,7 +169,7 @@ void RigidComputation::jacrot(Geom::Matrix33f &a, Geom::Matrix33f &r)
q=0;
p=0;
float max = maxoffdiag(a,p,q);
if(max>0.0000001)
if(max>0.0000001) // eventuellement changer pour les perfs
{
rotate(a,r,p,q);
if((++iter)==10)
......@@ -212,8 +212,10 @@ int RigidComputation::jacobiDiagonalization( Geom::Matrix33f& A, Geom::Matrix33f
}
if( sm == 0.0f ) // The normal return, which relies on quadratic convergence to machine underflow.
return nrot;
{
// std::cout<<"OK"<<std::endl;
return nrot;
}
if( i < 3 )
thresh = 0.2f*sm/(n*n); // ... on the first three sweeps.
else
......@@ -293,6 +295,6 @@ int RigidComputation::jacobiDiagonalization( Geom::Matrix33f& A, Geom::Matrix33f
}
}
// std::cout<<"K O"<<std::endl;
return -1; // In case of error, returns minus one.
}
......@@ -93,7 +93,7 @@ void EnvMap::init(unsigned int config, REAL width, REAL height, REAL minSize, RE
break ;
case 3 :
CityGenerator::generateGrid<PFP>(*this) ;
CityGenerator::generateCity<PFP>(*this,10,500.0f) ;
// CityGenerator::generateCity<PFP>(*this,10,500.0f) ;
break ;
case 4 :
CityGenerator::generatePlanet<PFP>(*this) ;
......
......@@ -87,6 +87,7 @@ bool MovingObstacle::is_inside(VEC3 p)
vec2 = VEC3(p -p1);
if (vec*vec2 < 0)
return false;
}
return true;
......@@ -160,7 +161,10 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
mm_(NULL),
ag_(NULL),
index_parent(indParent),
gravity_dist(0)
gravity_dist(0),
beta(0.9),
alpha(0.1)
{
assert(pos.size() > 2);
......@@ -168,8 +172,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
dInside = sim_->envMap_.getBelongingCell(pos[0]);
unsigned int nbParticles = nbVertices;
if(!rigid);
nbParticles +=1; //a center particle for the mass-spring
// if(!rigid);
// nbVertices +=1; //a center particle for the mass-spring
#ifdef TWO_AND_HALF_DIM
parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>*[nbParticles];
......@@ -181,17 +185,19 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
#endif
#endif
obstacles_ = new Obstacle*[nbVertices];
belonging_cells = new std::vector<Dart>[nbVertices];
neighbor_cells = new std::vector<Dart>[nbVertices];
position = map.addAttribute<VEC3, VERTEX>("position") ;
normal = map.addAttribute<VEC3, VERTEX>("normal") ;
velocity = map.addAttribute<VEC3, VERTEX>("velocity") ;
deformation = map.addAttribute<VEC3, VERTEX>("deformation") ;
if(!rigid_)
{
velocity = map.addAttribute<VEC3, VERTEX>("velocity") ;
forces = map.addAttribute<VEC3, VERTEX>("force") ;
edgeLength = map.addAttribute<float, EDGE>("edgeLength") ;
vertexAngle = map.addAttribute<float, DART>("vertexAngle") ;
......@@ -222,19 +228,19 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
}
front=(parts_[0]->getPosition() + parts_[1]->getPosition()) / 2;
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
}
//////////////////////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
// où F1 et F2 sont les deux foyers.
......@@ -292,10 +298,11 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
Dart d = i;
position[d] = pos[i];
deformation[d] = VEC3(0);
velocity[d] = VEC3(0);
if(!rigid_)
{
velocity[d] = VEC3(0);
forces[d] = VEC3(0);
}
}
......@@ -308,47 +315,65 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
map.fillHole(groundFace);
groundFace = map.phi2(groundFace);
std::vector<Dart> bord;
if(!rigid_)
{
Dart d = Algo::Surface::Modelisation::trianguleFace<PFP>(map,groundFace);
position[d]=parts_[nbVertices]->getPosition();
//////////////////Mass SPRING
// Dart d = Algo::Surface::Modelisation::trianguleFace<PFP>(map,groundFace);
// position[d]=parts_[nbVertices]->getPosition();
Dart d=groundFace;
for (unsigned int i = 0; i < nbVertices; ++i)
{
Dart e =map.phi<12>(d);
Dart e =map.phi<2111>(d);
map.splitFace(map.phi_1(e),map.phi1(e));
d=map.phi<21>(d);
d=map.phi_1(d);
}
Algo::Surface::Modelisation::EarTriangulation<PFP> et(map);
et.triangule();
// Algo::Surface::Modelisation::EarTriangulation<PFP> et(map);
// et.triangule();
//
// TraversorE<PFP::MAP> tE(map);
// for(Dart d = tE.begin() ; d != tE.end() ; d = tE.next())
// {
// edgeLength[d] = VEC3(position[map.phi1(d)]-position[d]).norm();
// }
//
// Dart centerDart =map.phi<11>(groundFace);
// d = centerDart;
//
// do
// {
// Dart e = map.phi1(d);
// vertexAngle[e] = Algo::Surface::Geometry::angle<PFP>(map,d,map.phi2(map.phi_1(d)),position);
// vertexAngle[d] = Algo::Surface::Geometry::angle<PFP>(map,map.phi_1(d),map.phi2(e),position);
// vertexAngle[map.phi_1(d)]= Algo::Surface::Geometry::angle<PFP>(map,e,map.phi2(d),position);
// d=map.phi<21>(d);
// }
// while(d!=centerDart);
///////////////////:
////////////////////SHAPE
TraversorE<PFP::MAP> tE(map);
for(Dart d = tE.begin() ; d != tE.end() ; d = tE.next())
d = groundFace;
do
{
edgeLength[d] = VEC3(position[map.phi1(d)]-position[d]).norm();
}
bord.push_back(d);
bord.push_back(map.phi_1(map.phi2(d)));
d=map.phi_1(d);
}while ( d !=groundFace);
Dart centerDart =map.phi<11>(groundFace);
d = centerDart;
do
{
Dart e = map.phi1(d);
vertexAngle[e] = Algo::Surface::Geometry::angle<PFP>(map,d,map.phi2(map.phi_1(d)),position);
vertexAngle[d] = Algo::Surface::Geometry::angle<PFP>(map,map.phi_1(d),map.phi2(e),position);
vertexAngle[map.phi_1(d)]= Algo::Surface::Geometry::angle<PFP>(map,e,map.phi2(d),position);
d=map.phi<21>(d);
}
while(d!=centerDart);
///////////////////
map.enableQuickTraversal<VERTEX>();
dDir=dInside;
}
shape_= new ShapeMatchingLinear<PFP>(map,position,bord,beta);
shape_->initialize();
for (unsigned int i = 0; i < nbVertices; ++i)
{
......@@ -357,9 +382,14 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
parts_[(i - 1 + nbVertices) % nbVertices]->getPosition(),
parts_[(i + 2) % nbVertices]->getPosition(), i, (i+1)% nbVertices, this, i);
obstacles_[i] = o;
/////definition du troisieme point
if (rigid_) o->p3=parts_[(i + 2) % nbVertices]->getPosition();
// CGoGNout<<" obstacle :"<< i << " num : "<< o<<CGoGNendl;
sim_->envMap_.pushObstacleInCells(o);
}
}
void MovingObstacle::initGL()
......@@ -423,6 +453,27 @@ void MovingObstacle::draw(bool showPath)
m_ds->vertex(goals_[(curGoal_+i)%(goals_.size())]);
}
m_ds->end();
m_ds->endList();
}
if(false)//show vertices velocity
{
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]);
m_ds->vertex(center);
Dart d =groundFace;
for(unsigned int i = 0 ; i < nbVertices ; i++)
{
m_ds->vertex(position[d]);
VEC3 speed = velocity[d];
m_ds->vertex(speed+position[d]);
m_ds->vertex(position[d]);
map.next(d);
}
m_ds->end();
m_ds->endList();
}
......@@ -437,7 +488,7 @@ VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{
return position[d];
}
else return position[d]+deformation[d];
else return position[d];
#else
return position[d];
#endif
......@@ -824,17 +875,17 @@ void MovingObstacle::initForces()
if(!rigid_)
{
Dart centerDart =map.phi<11>(groundFace);
Dart d = centerDart;
Dart d = groundFace;
do
{
Dart e = map.phi1(d);
//initialisation of forces
forces[e] = VEC3(0.0);
d=map.phi<21>(d);
forces[d] = VEC3(0.0);
d=map.phi_1(d);;
}
while(d!=centerDart);
forces[d] = VEC3(0.0);
while(d!=groundFace);
}
}
......@@ -910,134 +961,134 @@ void MovingObstacle::updateForces()
rotor = abs_angle*angle > 0.04f ? 0.04f : abs_angle*angle ;
else
rotor = abs_angle*angle ;
// masse ressort pour la limace
if(!rigid_)
{
Dart centerDart =map.phi<11>(groundFace);
Dart d = centerDart;
forces[d] += -0.9f*velocity[d];
float rigidity = 10.0f;
float bend_factor=1.0f;// limace 1 ||
float angular_factor=2.0f;// limace 2
do
{
Dart e = map.phi1(d);
//initialisation of forces with viscosity
forces[e] += -0.9f*velocity[e];
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.
VEC3 v1 = (p1Next-p2Next);
//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
float norm = v1.norm();
float stretch = bend_factor*rigidity*(edgeLength[e]-norm);//4
float angularStretch = 0, angularStretch2 = 0;
float restAngle = vertexAngle[e];
if(restAngle!=0.0f)
{
float angularRig = angular_factor*rigidity;//2
float curAngle = Algo::Surface::Geometry::angle<PFP>(map, d,map.phi2(map.phi_1(d)),position);
VEC3 v = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, d, position);
VEC3 v2 = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, map.phi2(map.phi_1(d)), position);
VEC3 v3 = v ^ v2;
VEC3 vPl = Algo::Surface::Geometry::faceNormal<PFP>(map, d, position);
if(v3 * vPl < 0.0f)
curAngle *= -1.0f;
angularStretch = angularRig*(restAngle-curAngle);
}
if(norm>0.0f)
{
VEC3 f = (stretch+angularStretch)*(v1/norm);
//apply force symmetrically
forces[e] -= f;
forces[map.phi1(e)] += f;
}
p1Next = position[e]+(velocity[e] * sim_->timeStep_); /// ressorts vers le centre de la face
p2Next = position[d]+(velocity[d] * sim_->timeStep_);
// p1Next et p2Next sont la position des extremites de l'arete.
v1 = (p1Next-p2Next);
//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
norm = v1.norm();
restAngle = vertexAngle[d];
if(restAngle!=0.0f)
{
float angularRig = angular_factor*rigidity;//2
float curAngle = Algo::Surface::Geometry::angle<PFP>(map, map.phi_1(d),map.phi2(map.phi1(d)),position);
VEC3 v = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, map.phi_1(d), position);
VEC3 v2 = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, map.phi2(map.phi1(d)), position);
VEC3 v3 = v ^ v2;
VEC3 vPl = Algo::Surface::Geometry::faceNormal<PFP>(map, d, position);
if(v3 * vPl < 0.0f)
curAngle *= -1.0f;
angularStretch = angularRig*(restAngle-curAngle);
}
stretch = bend_factor*rigidity*(edgeLength[d]-norm);//4
if(norm>0.0f)
{
VEC3 f = (stretch+angularStretch)*(v1/norm);
//apply force symmetrically
forces[d] -= f;
forces[e] += f;
}
p1Next = position[d]+(velocity[d] * sim_->timeStep_); // ressorts sur le bord
p2Next = position[map.phi1(e)]+(velocity[map.phi1(e)] * sim_->timeStep_);
// p1Next et p2Next sont la position des extremites de l'arete.
v1 = (p1Next-p2Next);
norm = v1.norm();
restAngle = vertexAngle[map.phi_1(d)];
if(restAngle!=0.0f)
{
float angularRig = angular_factor*rigidity;//2
float curAngle = Algo::Surface::Geometry::angle<PFP>(map, e,map.phi2(d),position);
VEC3 v = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, e, position);
VEC3 v2 = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, map.phi2(d), position);
VEC3 v3 = v ^ v2;
VEC3 vPl = Algo::Surface::Geometry::faceNormal<PFP>(map, d, position);
if(v3 * vPl < 0.0f)
curAngle *= -1.0f;
angularStretch2 = angularRig*(restAngle-curAngle);
}
if(norm>0.0f)
{
VEC3 f = (angularStretch2)*(v1/norm);
//apply force symmetrically
forces[map.phi1(e)] -= f;
forces[d] += f;
}
// // masse ressort pour la limace
//
d=map.phi<21>(d);
}
while(d!=centerDart);
// Dart centerDart =map.phi<11>(groundFace);
// Dart d = centerDart;
// forces[d] += -0.9f*velocity[d];
// float rigidity = 10.0f;
// float bend_factor=1.0f;// limace 1 ||
// float angular_factor=2.0f;// limace 2
// do