Commit cf21d247 authored by pitiot's avatar pitiot

2.5 D fonctionne \o/

parent 7be9965d
......@@ -11,7 +11,7 @@
//#define SECURED
//#define EXPORTING_AGENT
//#define EXPORTING_OBJ
#define ARASH
#ifdef SECURED
#include "Algo/MovingObjects/particle_cell_2D_secured.h"
......
......@@ -33,7 +33,7 @@ class ArticulatedObstacle;
#include "pfp.h"
//#define EXPORTING3
//#define TWO_AND_HALF_DIM
#define TWO_AND_HALF_DIM
#ifdef EXPORTING3
#include "Utils/Shaders/shaderPhongTexture.h"
......
......@@ -18,7 +18,9 @@
using namespace std;
//#define EXPORTING2
#define EXPORTING2
float get_angle3D(VEC3 v1, VEC3 v2);
class MovingMesh
{
......@@ -38,7 +40,7 @@ public:
void draw();
std::vector<VEC3> computeProjectedPointSet(float maxHeight);
std::vector<VEC3> computeProjectedPointSet(float maxHeight, Dart d);
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);
......
......@@ -14,7 +14,7 @@
#include "Algo/MovingObjects/particle_cell_2D_memo.h"
#endif
#define EXPORTING_BOXES
//#define EXPORTING_BOXES
#ifdef EXPORTING_BOXES
#include "Algo/Render/GL2/mapRender.h"
......@@ -58,6 +58,8 @@ public:
void updateForces();
void applyForces();
void updateRegistration();
PFP::REAL computeMVC(PFP::VEC3 p, Dart vertex);
void computePointMVC(PFP::VEC3 point, std::vector<PFP::REAL>& coordinates);
void attachMesh(MovingMesh* mm);
......
......@@ -802,7 +802,7 @@ void Agent::computeNewVelocity()
float force_value;
int nobst=0;
#define ARASH
#ifdef ARASH
for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
......
......@@ -25,7 +25,7 @@ constrainedV(map)
position = map.getAttribute<VEC3, VERTEX>(attrNames[0]) ;
float area = Algo::Surface::Geometry::convexFaceArea<PFP>(envMap.map, d, envMap.position);
scaleValue = std::max(area/1400.0f,5.0f);
scaleValue = std::max(area/1400.0f,2.0f);
std::cout << "scaleVal " << scaleValue << std::endl;
// scale(scaleValue/1.8f);
......@@ -37,8 +37,7 @@ constrainedV(map)
// Geom::rotate(axis, float(M_PI/2.0f),m);
// transform(m);
VEC3 v = Algo::Surface::Geometry::faceCentroid<PFP>(motherMap, d, motherPosition) ;
translate(v);
map.enableQuickTraversal<VERTEX>();
}
......@@ -183,6 +182,8 @@ void MovingMesh::animate()
}
}
void MovingMesh::draw()
{
#ifdef EXPORTING2
......@@ -204,7 +205,7 @@ void MovingMesh::draw()
#endif
}
std::vector<VEC3> MovingMesh::computeProjectedPointSet(float maxHeight)
std::vector<VEC3> MovingMesh::computeProjectedPointSet(float maxHeight, Dart d)
{
std::vector<VEC3> points;
......@@ -219,8 +220,8 @@ std::vector<VEC3> MovingMesh::computeProjectedPointSet(float maxHeight)
points = jarvisConvexHull(points);
points.pop_back();
Geom::Plane3D<float> pl = Algo::Surface::Geometry::facePlane<PFP>(motherMap, motherMap.begin(), motherPosition);
VEC3 axeZ = VEC3(0,0,1);
Geom::Plane3D<float> pl = Geom::Plane3D<float>(axeZ, 0.0f);
for(unsigned int i = 0; i < points.size() ; ++i)
{
VEC3& v = points[i];
......@@ -232,10 +233,31 @@ std::vector<VEC3> MovingMesh::computeProjectedPointSet(float maxHeight)
simplifyCurve(points, active, 0, points.size()-1, 0.3f);
std::vector<VEC3> res;
VEC3 center =Algo::Surface::Geometry::faceCentroid<PFP>(motherMap, d, motherPosition);
VEC3 normale = Algo::Surface::Geometry::faceNormal<PFP>(motherMap, d, motherPosition);
Geom::Matrix44f rot ;
rot.identity() ;
float angle = 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, angle, rot) ;
for(Dart dd = tv.begin() ; dd != tv.end() ; dd = tv.next())
{
position[dd]=Geom::transform(position[dd],rot);
}
translate(center);
for(unsigned int i = 0; i < points.size() ; ++i)
{
points[i] =Geom::transform(points[i],rot);
if(active[i])
res.push_back(points[i]);
res.push_back(points[i]+ center);
// std::cout << "fin " << points[i] << std::endl;
}
......
......@@ -147,7 +147,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
dInside = sim_->envMap_.getBelongingCell(pos[0]);
#ifdef TWO_AND_HALF_DIM
parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>*[nbVertices];
parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>*[nbVertices+1];
#else
parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>*[nbVertices];
#endif
......@@ -171,7 +171,8 @@ 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 = sim_->envMap_.getBelongingCellOnSurface(pos[i]);
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);
#else
......@@ -188,6 +189,12 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
center /= nbVertices;
front=(pos[0] + pos[1]) / 2;
#ifdef TWO_AND_HALF_DIM
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;
#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();
......@@ -240,12 +247,30 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
// compute edgeLength for mass-spring
hight=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) ;
map.fillHole(groundFace);
groundFace = map.phi2(groundFace);
if(!rigid_)
{
Dart d = Algo::Surface::Modelisation::trianguleFace<PFP>(map,groundFace);
position[d]=parts_[nbVertices]->getPosition();
for (unsigned int i = 0; i < nbVertices; ++i)
{
Dart e =map.phi<12>(d);
map.splitFace(map.phi_1(e),map.phi1(e));
d=map.phi<21>(d);
}
Algo::Surface::Modelisation::EarTriangulation<PFP> et(map);
et.triangule();
......@@ -739,14 +764,21 @@ void getResponse2(VEC3 f, VEC3 p, VEC3 p1, VEC3 p2, VEC3 *f1, VEC3 *f2)
void MovingObstacle::initForces()
{
Dart dd = groundFace;
if(!rigid_)
for (unsigned int i = 0; i < nbVertices; ++i)
{
Dart centerDart =map.phi<11>(groundFace);
Dart d = centerDart;
do
{
Dart e = map.phi1(d);
//initialisation of forces
forces[dd] = VEC3(0.0);
map.next(dd);
forces[e] = VEC3(0.0);
d=map.phi<21>(d);
}
while(d!=centerDart);
forces[d] = VEC3(0.0);
}
}
......@@ -810,9 +842,8 @@ void MovingObstacle::updateForces()
PFP::VEC3 bary(0);
Dart d;
velocity_[0] = newVelocity_[0] * velocity_factor;
velocity_[1] = newVelocity_[1] * velocity_factor;
velocity_[2] = newVelocity_[2] * velocity_factor;
velocity_ = newVelocity_* velocity_factor;
// velocity_[0] = 0.0;
// velocity_[1] = 0.0;
......@@ -830,117 +861,74 @@ void MovingObstacle::updateForces()
if(!rigid_)
{
Dart d = groundFace;
// map.next(d);
Dart dd =d;
for (unsigned int i = 0; i < nbVertices; ++i)
Dart centerDart =map.phi<11>(groundFace);
Dart d = centerDart;
forces[d] += -0.9f*velocity[d];
do
{
Dart e = map.phi1(d);
//initialisation of forces with viscosity
forces[dd] += -0.9f*velocity[dd];
forces[e] += -0.9f*velocity[e];
// forces[dd] = VEC3(0.0);
map.next(dd);
}
CellMarkerStore<EDGE> cm(map);
CellMarkerStore<VERTEX> cmV(map);
DartMarkerStore dm(map);
// ARASH : On parcourt les sommets de la grande face
for (unsigned int i = 0; i < nbVertices; ++i)
{
Dart dd = d;
// ARASH : On parcourt les sous-faces triangulaire de la grande face
do {
if(!cm.isMarked(dd))
{
cm.mark(dd);
VEC3 p1Next = position[map.phi1(dd)]+(velocity[map.phi1(dd)] * sim_->timeStep_);
VEC3 p2Next = position[dd]+(velocity[dd] * 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 rigidity = 30.0f;
float stretch = rigidity*(edgeLength[dd]-v1.norm());
if(norm>0.0f)
{
VEC3 f = 1.5*stretch*(v1/norm);
VEC3 p1Next = position[map.phi1(e)]+(velocity[map.phi1(e)] * sim_->timeStep_);
VEC3 p2Next = position[e]+(velocity[e] * sim_->timeStep_);
// p1Next et p2Next sont la position des extremites de l'arete.
VEC3 v1 = (p1Next-p2Next);
//apply force symmetrically
forces[dd] -= f;
forces[map.phi1(dd)] += f;
}
}
//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
float norm = v1.norm();
float rigidity = 50.0f;
dd = map.phi1(dd);
} while(dd!=d);
float stretch = rigidity*(edgeLength[e]-norm);
if(norm>0.0f)
{
VEC3 f = stretch*(v1/norm);
// ARASH : les ressorts angulaires
do {
if(!dm.isMarked(dd))
{
dm.mark(dd);
// VEC3 v1 = (position[map.phi1(dd)]-position[dd]);
VEC3 p1Next = position[map.phi1(dd)]+(velocity[map.phi1(dd)] * sim_->timeStep_);
VEC3 p2Next = position[dd]+(velocity[dd] * sim_->timeStep_);
VEC3 v1 = (p1Next-p2Next);
//apply force symmetrically
forces[e] -= f;
forces[map.phi1(e)] += f;
}
float norm = v1.norm();
p1Next = position[map.phi1(d)]+(velocity[map.phi1(d)] * sim_->timeStep_);
p2Next = position[d]+(velocity[d] * sim_->timeStep_);
// p1Next et p2Next sont la position des extremites de l'arete.
v1 = (p1Next-p2Next);
//bend spring: /!\ max rigidity relative to the timestep used (unstable otherwise)
float restAngle = vertexAngle[dd];
if(restAngle!=0.0f)
{
float angularRig = 70.0f;
//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
norm = v1.norm();
float curAngle = Algo::Surface::Geometry::angle<PFP>(map, map.phi_1(dd),map.phi2(map.phi1(dd)),position);
VEC3 v1 = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, map.phi_1(dd), position);
VEC3 v2 = Algo::Surface::Geometry::vectorOutOfDart<PFP>(map, map.phi2(map.phi1(dd)), position);
stretch = rigidity*(edgeLength[d]-norm);
if(norm>0.0f)
{
VEC3 f = stretch*(v1/norm);
VEC3 v3 = v1 ^ v2;
VEC3 vPl = Algo::Surface::Geometry::faceNormal<PFP>(map, dd, position);
//apply force symmetrically
forces[d] -= f;
forces[map.phi1(d)] += f;
}
if(v3 * vPl < 0.0f)
curAngle *= -1.0f;
float angularStretch = angularRig*(restAngle-curAngle);
if(norm>0.0f && curAngle==curAngle) //test to avoid NaN
{
VEC3 f = angularStretch*(v1/norm);
forces[dd] += f;
forces[map.phi1(dd)] -= f;
}
}
}
dd = map.phi1(dd);
} while(dd!=d);
d=map.phi<21>(d);
}
while(d!=centerDart);
//-------------------------------------------------------------------------
// ARASH : A présent on calcule les interactions avec les autres obstacles.
VEC3 norm;
double obst_stiffness = 5.0; // agent-obstacle interaction stiffness
int obst_power = 2 ; // the power to which elevate the agent-obstacle distance
do {
if(!cmV.isMarked(dd))
{
cmV.mark(dd);
VEC3 norm;
double obst_stiffness = 5.0; // agent-obstacle interaction stiffness
int obst_power = 2 ; // the power to which elevate the agent-obstacle distance
double obst_radius_infl;
if(sim_->config==0)
obst_radius_infl = 100.; // scenario 0
else
obst_radius_infl = 30.; // scenario 1 et 3
double obst_radius_infl;
if(sim_->config==0)
obst_radius_infl = 100.; // scenario 0
else
obst_radius_infl = 30.; // scenario 1 et 3
do {
Dart dd = map.phi1(d);
VEC3 p = position[dd]+(velocity[dd] * sim_->timeStep_);
// Evitement d'obstacles mobiles
......@@ -966,12 +954,11 @@ void MovingObstacle::updateForces()
forces[dd] += computeForce(p,p1,p2,obst_radius_infl,obst_power,10*obst_stiffness);
}
}
dd = map.phi1(dd);
} while(dd!=d);
map.next(d);
}
d = map.phi<21>(d);
} while(d!=centerDart);
//guiding vertex = first vertex (set the displacement)
// forces[groundFace] = VEC3(0);
......@@ -1062,44 +1049,107 @@ void MovingObstacle::updateForces()
void MovingObstacle::applyForces()
{
PFP::VEC3 bary(0);
Dart centerDart =map.phi<11>(groundFace);
if(!rigid_)
{
Dart d = groundFace;
Dart d = centerDart;
for (unsigned int i = 0; i < nbVertices; ++i)
{
velocity[d] += forces[d] * sim_->timeStep_;
position[d] += (velocity[d] * sim_->timeStep_);
position[map.phi<211>(d)] += (velocity[d] * sim_->timeStep_);
bary += position[d];
map.next(d);
Dart e = map.phi_1(d);
velocity[e] += forces[e] * sim_->timeStep_;
position[e] += (velocity[e] * sim_->timeStep_);
d=map.phi<21>(d);
}
center = bary / nbVertices;
}
velocity[centerDart] +=forces[centerDart] * sim_->timeStep_;
position[centerDart] += (velocity[centerDart] * sim_->timeStep_);
// MAJ des obstacles
for (unsigned int i = 0; i < nbVertices; ++i)
{
#ifdef TWO_AND_HALF_DIM
// std::cout << " dist prevu " << (getDilatedPosition(i) - parts_[i]->getPosition()).norm() << std::endl;
}
#endif
parts_[i]->move(getDilatedPosition(i));
#ifdef TWO_AND_HALF_DIM
// std::cout << " dist " << parts_[i]->getDistance() << std::endl;
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;
position[map.phi<112>(d)] = position[d]+normal;
#endif
}
void MovingObstacle::updateRegistration()
{
PFP::VEC3 bary(0);
Dart centerDart =map.phi<11>(groundFace);
if(!rigid_)
{
Dart d = centerDart;
for (unsigned int i = 0; i < nbVertices; ++i)
{
Dart e = map.phi_1(d);
parts_[i]->move(position[e]);
#ifdef TWO_AND_HALF_DIM
position[e] = 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(map.phi<12>(d))] = position[e]+normal;
#endif
bary += position[e];
d=map.phi<21>(d);
}
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
// MAJ des obstacles
}
else
{
for (unsigned int i = 0; i < nbVertices+1; ++i)
{
parts_[i]->move(getDilatedPosition(i));
#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)
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
}
}
for (unsigned int i = 0; i < nbVertices; ++i)
{
// CGoGNout << "avant une etape : Obstacle "<< i << CGoGNendl;
Obstacle* o = obstacles_[i];
o->p1 = getDilatedPosition(i);
......
......@@ -16,7 +16,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) :
timeStep_(0.025f),
timeStep_(0.02f),
// timeStep_(0.2f),
globalTime_(0.0f),
nbSteps_(0),
......@@ -89,6 +89,7 @@ void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst
setupPlanetScenario(nbAgent,nbObst);
addMovingObstacles(nbObst, 1);
addPathToObstacles(envMap_.buildingMark, true);
addPathsToAgents();
#else
std::cout << "Agents not in 2.5D mode" << std::endl;
#endif
......@@ -171,10 +172,14 @@ void Simulator::doStep()
// unsigned int i =12;
clock_gettime(CLOCK_MONOTONIC, &begTime) ;
movingObstacles_[i]->applyForces();
movingObstacles_[i]->updateRegistration();
clock_gettime(CLOCK_MONOTONIC, &endTime) ;
time_obstacle+= timespec_delta(begTime,endTime).tv_nsec;
movingObstacles_[i]->updateMesh() ;
}
// CGoGNout<<"deplacement obstacles fin"<<CGoGNendl;
for (unsigned int i = 0 ; i < agents_.size() ; ++i)
......@@ -746,10 +751,10 @@ void Simulator::setupPlanetScenario(unsigned int nbAgents, unsigned int nbObstac
d=envMap_.map.begin();
CellMarker<FACE> filled(envMap_.map);
unsigned int nbx = nbAgents==0 ? 1 : (nbAgents / (nb_cells))/5;
unsigned int nbx =1;
unsigned int nby = 5;
unsigned int bMax = nbAgents / (nbx*nby);
unsigned int bMax = nbx * nby > 0 ? nbAgents / (nbx * nby) : nbAgents ;
for (unsigned int i = 0; i < bMax && d != envMap_.map.end(); ++i)
{
......@@ -771,8 +776,10 @@ void Simulator::setupPlanetScenario(unsigned int nbAgents, unsigned int nbObstac
if(found)
{
float ecart = 3.0f;
VEC3 displ2(- (float(nbx)/2.0f * ecart), - (float(nby)/2.0f * ecart), 0);
Geom::Plane3D<PFP::REAL> pl(Algo::Surface::Geometry::faceNormal<PFP>(envMap_.map, dCell, envMap_.position), 0.0f);
VEC3 posinit = VEC3(pos[0] - (float(nbx)/2.0f * ecart), pos[1] - (float(nby)/2.0f * ecart), pos[2]);
Agent::rotate(Agent::xyPlane, pl.normal(), displ2);
VEC3 posinit = pos + displ2;
for(unsigned int curx = 0; curx < nbx; ++curx)
{
for(unsigned int cury = 0; cury < nby; ++cury)
......@@ -820,7 +827,10 @@ void Simulator::addMovingObstacles(unsigned int nb, unsigned int type)
}
if (found)
{
addMovingObstacle(dCell,type) ;
// CGoGNout<<" dCell initiale :"<< d <<CGoGNendl;
}
}
}
......@@ -869,18 +879,20 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
// mm = new MovingMesh(envMap_, d, "./meshRessources/cylinder.obj");
// mm = new MovingMesh(envMap_, d, "./meshRessources/simpleCyl.obj");
movingMeshes_.push_back(mm);
vPos = mm->computeProjectedPointSet(maxHeight);
vPos = mm->computeProjectedPointSet(maxHeight, d);
#ifndef TWO_AND_HALF_DIM
//scale projected pointset
VEC3 bary(0);
for(std::vector<VEC3>::iterator it = vPos.begin() ; it != vPos.end() ; ++it)
bary +