Commit 927cd7f8 authored by Pierre Kraemer's avatar Pierre Kraemer
Browse files

simplify moving obstacle polygon & associate moving mesh with barycentric coordinates

parent d44c4a4f
......@@ -29,6 +29,7 @@ public:
void draw();
std::vector<VEC3> computeProjectedPointSet(float maxHeight);
void simplifyCurve(std::vector<VEC3>& pointSet, std::vector<bool>& active, int start, int end, float epsilon);
std::vector<VEC3> jarvisConvexHull(const std::vector<VEC3>& projectedPointSet);
std::vector<VEC3> computeSkeleton(std::vector<VEC3> projectedPointSet, unsigned int nodeNumber);
......
......@@ -7,11 +7,13 @@
#include "env_map.h"
#include <set>
#include "Algo/MovingObjects/particle_cell_2D_memo.h"
using namespace std;
PFP::VEC3 rotate (PFP::VEC3 pos1, PFP::VEC3 center, float angle);
float get_angle (PFP::VEC3 v1, PFP::VEC3 v2);
PFP::VEC3 get_center (ArticulatedObstacle * art, int index);
class Simulator ;
class MovingMesh;
class MovingObstacle
{
......@@ -31,6 +33,11 @@ public:
void update();
PFP::REAL computeMVC(PFP::VEC3 p, Dart vertex);
void computePointMVC(PFP::VEC3 point, std::vector<PFP::REAL>& coordinates);
void attachMesh(MovingMesh* mm);
void updateMesh();
unsigned int nbVertices;
CGoGN::Algo::MovingObjects::ParticleCell2DMemo<PFP> *registering_part;
......@@ -97,6 +104,9 @@ public:
bool spinning;
ArticulatedObstacle * parent;
int index_parent;
MovingMesh* mm_;
VertexAttribute<NoMathIONameAttribute<std::vector<PFP::REAL> > > mvc_;
};
#endif
......@@ -35,6 +35,8 @@ constrainedV(map)
//
// smg = new ShapeMatchingQuadratic<PFP>(map,position,obstDarts,0.5f);
// smg->initialize();
map.enableQuickTraversal<VERTEX>();
}
void MovingMesh::linkWithObstacle(MovingObstacle * mo)
......@@ -131,34 +133,81 @@ void MovingMesh::animate()
void MovingMesh::draw()
{
// glColor3f(0,1,0);
// Algo::Render::GL1::renderTriQuadPoly<PFP>(map, Algo::Render::GL1::LINE,
// 1.0, position,
// normal) ;
glColor3f(0,1,0);
Algo::Render::GL1::renderTriQuadPoly<PFP>(map, Algo::Render::GL1::LINE,
1.0, position,
normal) ;
}
std::vector<VEC3> MovingMesh::computeProjectedPointSet(float maxHeight)
{
std::vector<VEC3> res;
TraversorV<PFP::MAP> tv(map);
for(Dart dd = tv.begin() ; dd != tv.end() ; dd = tv.next())
{
if(position[dd][2]<maxHeight)
res.push_back(position[dd]);
}
res = jarvisConvexHull(res);
res.pop_back();
Geom::Plane3D<float> pl = Algo::Geometry::facePlane<PFP>(motherMap, motherMap.begin(), motherPosition);
for(unsigned int i = 0; i < res.size() ; ++i)
{
VEC3& v = res[i];
pl.project(v);
}
return res;
std::vector<VEC3> points;
TraversorV<PFP::MAP> tv(map);
for(Dart dd = tv.begin() ; dd != tv.end() ; dd = tv.next())
{
if(position[dd][2]<maxHeight)
points.push_back(position[dd]);
}
points = jarvisConvexHull(points);
points.pop_back();
Geom::Plane3D<float> pl = Algo::Geometry::facePlane<PFP>(motherMap, motherMap.begin(), motherPosition);
for(unsigned int i = 0; i < points.size() ; ++i)
{
VEC3& v = points[i];
pl.project(v);
}
std::vector<bool> active;
active.assign(points.size(), true);
simplifyCurve(points, active, 0, points.size()-1, 0.1f);
std::vector<VEC3> res;
for(unsigned int i = 0; i < points.size() ; ++i)
{
if(active[i])
res.push_back(points[i]);
}
std::cout << "nb vertices before simplification -> " << points.size() << std::endl;
std::cout << "nb vertices after simplification -> " << res.size() << std::endl;
return res;
}
void MovingMesh::simplifyCurve(std::vector<VEC3>& pointSet, std::vector<bool>& active, int start, int end, float epsilon)
{
float distMax = 0;
int index = start;
VEC3 first = pointSet[start];
VEC3 last = pointSet[end];
for (int i = start + 1; active[i] && i < end; ++i)
{
float d = distSqPointLineSegment(first, last, pointSet[i]);
if (d > distMax)
{
index = i;
distMax = d;
}
}
std::vector<VEC3> resultList;
// If max distance is greater than epsilon, recursively simplify
if (distMax >= epsilon)
{
// Recursive call
simplifyCurve(pointSet, active, start, index, epsilon);
simplifyCurve(pointSet, active, index, end, epsilon);
}
else
{
for (int i = start + 1; i < end; ++i)
active[i] = false;
}
}
std::vector<VEC3> MovingMesh::jarvisConvexHull(const std::vector<VEC3>& pointSet)
......
......@@ -2,6 +2,7 @@
#include "obstacle.h"
#include "agent.h"
#include "simulator.h"
#include "moving_mesh.h"
#include "Algo/Modelisation/triangulation.h"
......@@ -132,7 +133,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
{
//extrude face to build a cage
// compute edgeLength for mass-spring
Algo::Modelisation::extrudeFace<PFP>(map, position, d, 5.0f) ;
Algo::Modelisation::extrudeFace<PFP>(map, position, d, 10.0f) ;
map.fillHole(groundFace);
groundFace = map.phi2(groundFace);
......@@ -144,6 +145,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
{
edgeLength[d] = VEC3(position[map.phi1(d)]-position[d]).norm();
}
map.enableQuickTraversal<VERTEX>();
}
center /= nbVertices;
......@@ -197,8 +200,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{
Dart d(ind); //WARNING : works only for one face created at start !
return position[d]+deformation[d];
// return position[d];
// return position[d]+deformation[d];
return position[d];
}
VEC3 MovingObstacle::getPosition(unsigned int ind)
......@@ -422,11 +425,11 @@ void MovingObstacle::update()
VEC3 v1 = (position[map.phi1(dd)]-position[dd]);
//inertie
velocity[dd] = velocity[dd]*0.9f;
velocity[dd] = velocity[dd]*0.995f;
//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
float norm = v1.norm();
float rigidity = 500.0f;
float rigidity = 10.0f;
float stretch = rigidity*(edgeLength[dd]-v1.norm());
VEC3 f = stretch*(v1/norm)*sim_->timeStep_;
......@@ -681,3 +684,92 @@ void MovingObstacle::computeNewVelocity() //comportement des obstacles en tenan
newVelocity_=prefVelocity_;
}
}
PFP::REAL MovingObstacle::computeMVC(PFP::VEC3 p, Dart vertex)
{
PFP::REAL r = (position[vertex]-p).norm();
PFP::REAL sumU(0.);
Dart it = vertex;
do
{
PFP::VEC3 vi = position[it];
PFP::VEC3 vj = position[map.phi1(it)];
PFP::VEC3 vk = position[map.phi_1(it)];
PFP::REAL Bjk = Geom::angle(vj - p, vk - p);
PFP::REAL Bij = Geom::angle(vi - p, vj - p);
PFP::REAL Bki = Geom::angle(vk - p, vi - p);
PFP::VEC3 ei = (vi - p) / ((vi - p).norm());
PFP::VEC3 ej = (vj - p) / ((vj - p).norm());
PFP::VEC3 ek = (vk - p) / ((vk - p).norm());
PFP::VEC3 nij = (ei ^ ej) / ((ei ^ ej).norm());
PFP::VEC3 njk = (ej ^ ek) / ((ej ^ ek).norm());
PFP::VEC3 nki = (ek ^ ei) / ((ek ^ ei).norm());
PFP::REAL ui= (Bjk + (Bij*(nij*njk)) + (Bki*(nki*njk))) / (2.0f*ei*njk);
sumU += ui;
it = map.phi<21>(it);
}
while(it != vertex);
return (1.0f / r) * sumU;
}
void MovingObstacle::computePointMVC(PFP::VEC3 point, std::vector<PFP::REAL>& coordinates)
{
DartMarker mark_vertex(map);
coordinates.clear();
PFP::REAL sumMVC(0);
unsigned int j = 0;
TraversorV<PFP::MAP> t(map) ;
for(Dart it = t.begin(); it != t.end(); it = t.next())
{
PFP::REAL c = computeMVC(point, it);
coordinates.push_back(c);
sumMVC += c;
j++;
}
for(unsigned int i = 0; i < j; i++)
coordinates[i] /= sumMVC;
}
void MovingObstacle::attachMesh(MovingMesh* mm)
{
mm_ = mm;
mvc_ = mm->map.getAttribute<NoMathIONameAttribute<std::vector<PFP::REAL> >, VERTEX>("mvc");
if(!mvc_.isValid())
mvc_ = mm->map.addAttribute<NoMathIONameAttribute<std::vector<PFP::REAL> >, VERTEX>("mvc");
TraversorV<PFP::MAP> t(mm_->map);
for(Dart it = t.begin(); it != t.end(); it = t.next())
computePointMVC(mm->position[it], mvc_[it]);
}
void MovingObstacle::updateMesh()
{
if(mm_)
{
TraversorV<PFP::MAP> t(mm_->map);
for(Dart it = t.begin(); it != t.end(); it = t.next())
{
PFP::VEC3 newPos;
unsigned int j = 0;
TraversorV<PFP::MAP> tt(map) ;
for(Dart itt = tt.begin(); itt != tt.end(); itt = tt.next())
{
newPos += mvc_[it][j] * position[itt];
++j;
}
mm_->position[it] = newPos;
}
}
}
#include "simulator.h"
Simulator::Simulator(unsigned int config, unsigned int minS, unsigned int nbAgent, unsigned int nbObst) :
timeStep_(0.02f),
timeStep_(0.2f),
globalTime_(0.0f),
nbSteps_(0),
nbUpdates(0),
......@@ -36,7 +36,7 @@ void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst
{
case 0 :
setupCircleScenario(nbAgent,nbObst) ;
addPathsToAgents();
// addPathsToAgents();
break ;
case 1 :
setupCorridorScenario(nbAgent,nbObst) ;
......@@ -110,7 +110,7 @@ void Simulator::doStep()
#endif
movingObstacles_[i]->update() ;
//PIERRE
movingObstacles_[i]->updateMesh() ;
}
for (unsigned int i = 0 ; i < agents_.size() ; ++i)
......@@ -175,8 +175,8 @@ void Simulator::doStep()
nbRefineCandidate += envMap_.refineCandidate.size() ;
nbCoarsenCandidate += envMap_.coarsenCandidate.size() ;
// if (multires)
// envMap_.updateMap() ;
if (multires)
envMap_.updateMap() ;
#endif
globalTime_ += timeStep_ ;
......@@ -642,6 +642,8 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
std::vector<VEC3> vPos;
VEC3 start;
MovingMesh* mm = NULL;
switch(obstType)
{
case 0 :
......@@ -658,7 +660,7 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
break;
case 1 :
{
MovingMesh * mm = new MovingMesh(envMap_, d, "meshRessources/Limace.ply");
mm = new MovingMesh(envMap_, d, "meshRessources/Limace.ply");
movingMeshes_.push_back(mm);
float maxHeight=10.0f;
vPos = mm->computeProjectedPointSet(maxHeight);
......@@ -675,9 +677,9 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
movingObstacles_.push_back(mo);
if(obstType==1)
if(mm != NULL)
{
//PIERRE : calcul coord bary d som du movMesh en fction de la cage movingObstacle
mo->attachMesh(mm);
}
}
......
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