Skip to content

GitLab

Création d'un compte pour un collaborateur extérieur au laboratoire depuis l'intranet ICube : https://intranet.icube.unistra.fr/fr/labs/member/profile

Commit e6def94a authored by pitiot's avatar pitiot
Browse files

update shapematching

parent 5de581be
Hide whitespace changes
Inline Side-by-side
include/env_map.h
View file @ e6def94a
......@@ -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 @ e6def94a
......@@ -2,12 +2,17 @@
#define M_MOVING_OBSTACLE_H
#include <iostream>
#include "ShapeMatching/shapeMatchingLinear.h"
#include "utils.h"
#include "env_map.h"
#include <set>
#define LINEAR
// #define SECURED
#ifdef LINEAR
#include "ShapeMatching/shapeMatchingLinear.h"
#else
#include "ShapeMatching/shapeMatchingQuadratic.h"
#endif
#ifdef TWO_AND_HALF_DIM
#include "Algo/MovingObjects/particle_cell_2DandHalf_memo.h"
......@@ -166,8 +171,11 @@ public:
VertexAttribute<NoMathIONameAttribute<std::vector<PFP::REAL> > > mvc_;
/////// ShapeMatching
#ifdef LINEAR
ShapeMatchingLinear<PFP> * shape_;
#else
ShapeMatchingQuadratic<PFP> * shape_;
#endif
PFP::REAL beta;
float alpha;
......
src/moving_obstacle.cpp
View file @ e6def94a
......@@ -162,8 +162,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
ag_(NULL),
index_parent(indParent),
gravity_dist(0),
beta(0.9),
alpha(0.1)
beta(0.95),
alpha(0.2)
{
assert(pos.size() > 2);
......@@ -371,8 +371,11 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
dDir=dInside;
}
#ifdef LINEAR
shape_= new ShapeMatchingLinear<PFP>(map,position,bord,beta);
#else
shape_= new ShapeMatchingQuadratic<PFP>(map,position,bord,beta);
#endif
shape_->initialize();
for (unsigned int i = 0; i < nbVertices; ++i)
......@@ -447,7 +450,7 @@ void MovingObstacle::draw(bool showPath)
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);
m_ds->vertex(position[groundFace]);
for(unsigned int i = 0 ; i < goals_.size() ; i++)
{
m_ds->vertex(goals_[(curGoal_+i)%(goals_.size())]);
......@@ -1141,7 +1144,7 @@ void MovingObstacle::updateForces()
//guiding vertex = first vertex (set the displacement)
// forces[groundFace] = VEC3(0);
velocity[groundFace] = velocity_;
velocity[map.phi_1(groundFace)] = velocity_;
//apply force to each vertex
/*
d = groundFace;
......@@ -1238,11 +1241,17 @@ void MovingObstacle::applyForces()
if(!rigid_)
{
Dart d = groundFace;
shape_->animate();
for (unsigned int i = 0; i < nbVertices; ++i)
{
velocity[d] += forces[d] * sim_->timeStep_;
velocity[d] += forces[d] * sim_->timeStep_ +(alpha/sim_->timeStep_)*(shape_->goal[d]-position[d]);
velocity[map.phi_1(map.phi2(d))] += forces[d] * sim_->timeStep_ +(alpha/sim_->timeStep_)*(shape_->goal[map.phi_1(map.phi2(d))]-position[map.phi_1(map.phi2(d))]);;
position[d] += (velocity[d] * sim_->timeStep_);
position [map.phi_1(map.phi2(d))]+=(velocity[map.phi_1(map.phi2(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;
......@@ -1250,16 +1259,10 @@ void MovingObstacle::applyForces()
}
// velocity[centerDart] +=forces[centerDart] * sim_->timeStep_;
// position[centerDart] += (velocity[centerDart] * sim_->timeStep_);
shape_->animate();
do
{
// CGoGNout<<"dart : "<<d<<" || pos : " <<position[d] <<" || obj : "<< shape_->goal[d]<<CGoGNendl;
position[d]+=alpha*(shape_->goal[d]-position[d]);
// position[d][2]=0;
position [map.phi_1(map.phi2(d))]=shape_->goal[map.phi_1(map.phi2(d))];
d=map.phi_1(d);
}while (d!=groundFace);
}
......@@ -1282,8 +1285,9 @@ void MovingObstacle::updateRegistration()
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
......@@ -1528,29 +1532,32 @@ void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atte
{
// if(sim_->envMap_.pedWayMark.isMarked(sim_->envMap_.map.phi2(dDir)))
// dDir = sim_->envMap_.map.phi1(sim_->envMap_.map.phi1(dDir));
goalVector = goals_[curGoal_] - position[groundFace] ;
VEC3 mid = (sim_->envMap_.position[dDir]+sim_->envMap_.position[sim_->envMap_.map.phi1(dDir)])*0.5f;
goalVector = mid - position[0] ;
Dart dStart=dDir;
VEC3 dir = center - position[0];
while(goalVector.norm2()==0 || sim_->envMap_.pedWayMark.isMarked(sim_->envMap_.map.phi2(dDir)) || ((dir*goalVector)>0))
{
// std::cout << "d " << dDir << std::endl;
// std::cout << "dir*goalVector " << dir*goalVector << std::endl;
// std::cout << "goal vec " << goalVector << std::endl;
dDir = sim_->envMap_.map.phi_1(dDir);
mid = (sim_->envMap_.position[dDir]+sim_->envMap_.position[sim_->envMap_.map.phi1(dDir)])*0.5f;
goalVector = mid - position[0] ;
if(dDir==dStart)
break;
}
if(sim_->envMap_.map.isBoundaryEdge(sim_->envMap_.map.phi2(dDir)))
goalVector = VEC3(0);
///////////////////////////////////////////////WTH
// VEC3 mid = (sim_->envMap_.position[dDir]+sim_->envMap_.position[sim_->envMap_.map.phi1(dDir)])*0.5f;
// goalVector = mid - position[0] ;
//
// Dart dStart=dDir;
// VEC3 dir = center - position[0];
// while(goalVector.norm2()==0 || sim_->envMap_.pedWayMark.isMarked(sim_->envMap_.map.phi2(dDir)) || ((dir*goalVector)>0))
// {
// // std::cout << "d " << dDir << std::endl;
// // std::cout << "dir*goalVector " << dir*goalVector << std::endl;
// // std::cout << "goal vec " << goalVector << std::endl;
//
// dDir = sim_->envMap_.map.phi_1(dDir);
// mid = (sim_->envMap_.position[dDir]+sim_->envMap_.position[sim_->envMap_.map.phi1(dDir)])*0.5f;
// goalVector = mid - position[0] ;
//
// if(dDir==dStart)
// break;
// }
//
// if(sim_->envMap_.map.isBoundaryEdge(sim_->envMap_.map.phi2(dDir)))
// goalVector = VEC3(0);
/////////////////////////////////////////////////////////////////
// goalVector = mid - center ;
// std::cout << "dDir " << dDir << " goalVec" << goalVector << std::endl;
......
src/simulator.cpp
View file @ e6def94a
......@@ -17,7 +17,7 @@ timespec timespec_delta(const timespec& t1, const timespec& t2) {
Simulator::Simulator(unsigned int config, unsigned int minS, unsigned int nbAgent, unsigned int nbObst, bool resolution) :
#ifdef TWO_AND_HALF_DIM
timeStep_(0.25f),
timeStep_(0.05f),
#else
// timeStep_(config > 2 ? 0.01f : 0.25f),
timeStep_(0.01f),
......
src/viewer.cpp
View file @ e6def94a
......@@ -773,6 +773,8 @@ void SocialAgents::cb_redraw()
m_ds->end();
m_ds->endList();
}
if (drawMovingObstacles)
{
#ifdef SHADOWSHELL
......@@ -873,6 +875,22 @@ void SocialAgents::cb_redraw()
if (draw_dart && dartSlider < (simulator.envMap_.map.end().index))
m_renderTopo->overdrawDart(dartSlider,5.0f,0,1.0f,0.5f);
if(drawObstacles)
{
CGoGN::CellMarkerStore<FACE> m(simulator.envMap_.map) ;
for (Dart d = simulator.envMap_.map.begin(); d != simulator.envMap_.map.end(); simulator.envMap_.map.next(d))
{
if (!m.isMarked(d))
{
m.mark(d) ;
if(simulator.envMap_.obstvect[d].size()!=0)
{
drawCell(d,5.0f,0,1.0f,0.5f);
}
}
}
}
}
if (drawEnvFaces)
......@@ -1199,7 +1217,7 @@ void SocialAgents::animate()
}
struct timespec begTime ;
clock_gettime(CLOCK_MONOTONIC, &begTime) ;
for(int n =1; n<30; n++)
for(int n =1; n<2; n++)
simulator.doStep() ;
struct timespec endTime ;
clock_gettime(CLOCK_MONOTONIC, &endTime) ;
......
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