Skip to content

GitLab

S
SocialAgents3D
Commit 60f3bfbe authored by pitiot's avatar pitiot
Browse files
Options

presque réparé 2.5

parents b413112c 05ba3385
Hide whitespace changes
Inline Side-by-side
Showing with 127 additions and 36 deletions
include/env_map.h
View file @ 60f3bfbe
......@@ -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"
......@@ -73,6 +73,7 @@ public:
#ifndef SPATIAL_HASHING
Dart getBelongingCell(const VEC3& pos) ;
Dart getBelongingCellOnSurface(const VEC3& pos) ;
void subdivideAllToMaxLevel() ;
void subdivideToProperLevel() ;
......
include/moving_obstacle.h
View file @ 60f3bfbe
......@@ -27,7 +27,8 @@
using namespace std;
PFP::VEC3 rotate (PFP::VEC3 pos1, PFP::VEC3 center, float angle);
PFP::VEC3 rotate2D (PFP::VEC3 pos1, PFP::VEC3 center, float angle);
PFP::VEC3 rotate3D (PFP::VEC3 pos1,PFP::VEC3 pos2, PFP::VEC3 center, float angle);
float get_angle (PFP::VEC3 v1, PFP::VEC3 v2);
PFP::VEC3 get_center (ArticulatedObstacle * art, int index);
class Simulator ;
......@@ -144,7 +145,7 @@ public:
bool rigid_;
bool spinning;
ArticulatedObstacle * parent;
float hight;
MovingMesh* mm_;
Agent* ag_;
int index_parent;
......
include/simulator.h
View file @ 60f3bfbe
......@@ -100,7 +100,7 @@ public:
void setupPlanetScenario(unsigned int nbAgents, unsigned int nbObstacles);
void addMovingObstacles(unsigned int nb);
void addMovingObstacles(unsigned int nb, unsigned int type);
void addMovingObstacle(Dart d, unsigned int obstType=0);
void addAgent(const VEC3& start,const VEC3& goals);
......
include/viewer.h
View file @ 60f3bfbe
......@@ -289,6 +289,8 @@ public slots:
{
for(unsigned int i=0 ; i < simulator.movingObstacles_.size() ; ++i)
{
// for(unsigned int j=0 ; j < simulator.movingObstacles_[i]->nbVertices ; ++j)
// {
m_triObst_render.push_back(new Algo::Render::GL2::MapRender());
Utils::ShaderSimpleColor * shader = new Utils::ShaderSimpleColor();
......@@ -304,6 +306,7 @@ public slots:
m_triObst_VBO.push_back(vbo);
m_triObst_Shader.push_back(shader);
registerShader(shader);
// }
}
}
......
src/env_map.cpp
View file @ 60f3bfbe
......@@ -593,14 +593,36 @@ Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
if (!m.isMarked(d))
{
m.mark(d) ;
if (!buildingMark.isMarked(d)
&& Algo::Surface::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true)) return d ;
if (!buildingMark.isMarked(d) && Algo::Surface::Geometry::isPointInConvexFace2D<PFP>(map, d, position, pos, true))
return d ;
}
}
std::cout << "ERROR : pos not in map for getBelongingCell " << pos << std::endl ;
return map.begin() ;
}
Dart EnvMap::getBelongingCellOnSurface(const PFP::VEC3& pos)
{
// assert(map.getCurrentLevel() == map.getMaxLevel()) ;
CellMarkerStore<FACE> m(map) ;
for (Dart d = map.begin(); d != map.end(); map.next(d))
{
if (!m.isMarked(d))
{
m.mark(d) ;
if (!buildingMark.isMarked(d) && Algo::Surface::Geometry::isPointInConvexFace<PFP>(map, d, position, pos, true))
{
return d ;
}
}
}
std::cout << "ERROR : pos not in map for getBelongingCellonSurface " << pos << std::endl ;
return map.begin() ;
}
#endif
#ifndef SPATIAL_HASHING
......
src/moving_obstacle.cpp
View file @ 60f3bfbe
......@@ -110,7 +110,7 @@ float get_angle(VEC3 v1, VEC3 v2) //renvoie l'angle entre [- pi ; Pi] du v2 à v
return flo;
}
VEC3 rotate(VEC3 pos1, VEC3 center, float angle) // renvoie le déplacement necessaire depuis pos1 pour effectuer la rotation centrée en center d'angle angle
VEC3 rotate2D(VEC3 pos1, VEC3 center, float angle) // renvoie le déplacement necessaire depuis pos1 pour effectuer la rotation centrée en center d'angle angle
{
VEC3 pos2;
PFP::REAL x = pos1[0] - center[0];
......@@ -120,6 +120,15 @@ VEC3 rotate(VEC3 pos1, VEC3 center, float angle) // renvoie le déplacement nece
return pos2;
}
VEC3 rotate3D(VEC3 pos1,VEC3 pos2, VEC3 center, float angle) // renvoie le déplacement necessaire depuis pos1 pour effectuer la rotation centrée en center d'angle angle sur le plan pos1 pos2 center
{
VEC3 pos3;
/// rotation dans la base (center,pos1,pos2)
// PFP::VEC3 inBase =rotate2D(VEC3(1,0,0),VEC3(0,0,0), angle);
///// changer de base ///
return pos3;
}
MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool rigid, bool spin,int curGoal, Dart dInside, ArticulatedObstacle * art, int indParent) :
nbVertices(pos.size()),
center(0),
......@@ -170,11 +179,12 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
for (unsigned int i = 0; i < nbVertices; ++i)
{
Dart d = sim_->envMap_.getBelongingCell(pos[i]);
#ifdef TWO_AND_HALF_DIM
Dart d = sim_->envMap_.getBelongingCellOnSurface(pos[i]);
CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>* part = 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);
#endif
parts_[i] = part;
......@@ -237,7 +247,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
//extrude face to build a cage
// compute edgeLength for mass-spring
Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, -10.0f) ;
hight=10.0f;
Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, -hight) ;
// Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, 0.0f) ;
map.fillHole(groundFace);
groundFace = map.phi2(groundFace);
......@@ -259,7 +270,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
if(!map.isBoundaryMarked2(d) && !treated.isMarked(d))
{
treated.mark(d);
vertexAngle[d] = Algo::Surface::Geometry::angle<PFP>(map,map.phi_1(d),map.phi1(d),position);
vertexAngle[d] = Algo::Surface::Geometry::angle<PFP>(map,map.phi_1(d),map.phi2(map.phi1(d)),position);
}
}
......@@ -351,8 +362,11 @@ void MovingObstacle::draw(bool showPath)
VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{
Dart d(ind); //WARNING : works only for one face created at start !
#ifndef TWO_AND_HALF_DIM
return position[d]+deformation[d];
// return position[d];
#else
return position[d];
#endif
}
VEC3 MovingObstacle::getPosition(unsigned int ind)
......@@ -802,6 +816,7 @@ void MovingObstacle::updateForces()
Dart d;
velocity_[0] = newVelocity_[0] * velocity_factor;
velocity_[1] = newVelocity_[1] * velocity_factor;
velocity_[2] = newVelocity_[2] * velocity_factor;
// velocity_[0] = 0.0;
// velocity_[1] = 0.0;
......@@ -826,7 +841,7 @@ void MovingObstacle::updateForces()
for (unsigned int i = 0; i < nbVertices; ++i)
{
//initialisation of forces with viscosity
forces[dd] += -0.2f*velocity[dd];
forces[dd] += -0.9f*velocity[dd];
// forces[dd] = VEC3(0.0);
map.next(dd);
}
......@@ -852,7 +867,7 @@ void MovingObstacle::updateForces()
//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
float norm = v1.norm();
float rigidity = 70.0f;
float rigidity = 30.0f;
float stretch = rigidity*(edgeLength[dd]-v1.norm());
if(norm>0.0f)
......@@ -886,7 +901,16 @@ void MovingObstacle::updateForces()
{
float angularRig = 70.0f;
float curAngle = Algo::Surface::Geometry::angle<PFP>(map, map.phi_1(dd),map.phi1(dd),position);
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);
VEC3 v3 = v1 ^ v2;
VEC3 vPl = Algo::Surface::Geometry::faceNormal<PFP>(map, dd, position);
if(v3 * vPl < 0.0f)
curAngle *= -1.0f;
float angularStretch = angularRig*(restAngle-curAngle);
......@@ -978,14 +1002,22 @@ void MovingObstacle::updateForces()
Dart d = groundFace;
for (unsigned int i = 0; i < nbVertices; ++i)
{
position[d] += rotate(position[d], center, abs_angle*rotor);
#ifdef TWO_AND_HALF_DIM
// position[d] += rotate3D(position[d],position[map.phi1(d)], center, abs_angle*rotor);
#else
position[d] += rotate2D(position[d], center, abs_angle*rotor);
#endif
position[d] += (velocity_ * sim_->timeStep_);
#ifdef EXPORTING_BOXES
#ifndef TWO_AND_HALF_DIM
position[map.phi<211>(d)] += rotate(position[map.phi<211>(d)], center, abs_angle*rotor);
position[map.phi<211>(d)] += (velocity_ * sim_->timeStep_);
#endif
#endif
// position[map.phi<211>(d)] += (velocity_ * sim_->timeStep_);
bary += position[d];
......@@ -1044,13 +1076,23 @@ void MovingObstacle::applyForces()
for (unsigned int i = 0; i < nbVertices; ++i)
{
#ifdef TWO_AND_HALF_DIM
std::cout << " dist supp " << (getDilatedPosition(i)-parts_[i]->getPosition()).norm() << std::endl;
// 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;
// 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[d] = parts_[i]->getPosition();
position[map.phi<112>(d)] = position[d]+normal;
#endif
// CGoGNout << "avant une etape : Obstacle "<< i << CGoGNendl;
......@@ -1058,10 +1100,10 @@ void MovingObstacle::applyForces()
o->p1 = getDilatedPosition(i);
o->p2 = getDilatedPosition((i+1) % nbVertices);
#ifndef POTENTIAL
o->prevP = getDilatedPosition((i - 1 + nbVertices) % nbVertices);
o->nextP = getDilatedPosition((i + 2 + nbVertices) % nbVertices);
#endif
Dart d1 = parts_[i]->d;
Dart d2 = parts_[(i+1)%nbVertices]->d;
......
src/simulator.cpp
View file @ 60f3bfbe
......@@ -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.05f),
timeStep_(0.025f),
// timeStep_(0.2f),
globalTime_(0.0f),
nbSteps_(0),
......@@ -67,7 +67,7 @@ void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst
case 3 :
envMap_.init(config, 1000.0f, 1000.0f, minSize, 100.0f) ; //grosses cases
setupScenario(nbAgent, false) ;
addMovingObstacles(nbObst);
addMovingObstacles(nbObst, 1);
addPathToObstacles(envMap_.buildingMark, true);
//// setupCityScenario(-1.0f * (12 * (70.0f / 2.0f) - 10),
//// -1.0f * (12 * (70.0f / 2.0f) - 10), 20, 20);
......@@ -79,7 +79,7 @@ void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst
envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ; //svg import
setupScenario(nbAgent, true) ;
// SelectorCellNotMarked<PFP::MAP> scnm(envMap_.pedWayMark);
addMovingObstacles(nbObst);
addMovingObstacles(nbObst, 1);
// addPathToObstacles(envMap_.pedWayMark, false);
addPathsToAgents();
break;
......@@ -87,7 +87,7 @@ void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst
#ifdef TWO_AND_HALF_DIM
envMap_.init(config,200.0,200.0, minSize, 400.0f);
setupPlanetScenario(nbAgent,nbObst);
addMovingObstacles(nbObst);
addMovingObstacles(nbObst, 0);
addPathToObstacles(envMap_.buildingMark, true);
#else
std::cout << "Agents not in 2.5D mode" << std::endl;
......@@ -761,7 +761,7 @@ void Simulator::setupPlanetScenario(unsigned int nbAgents, unsigned int nbObstac
swapAgentsGoals();
}
void Simulator::addMovingObstacles(unsigned int nb)
void Simulator::addMovingObstacles(unsigned int nb, unsigned int type)
{
TraversorF<PFP::MAP> tF(envMap_.map);
Dart d = tF.begin() ;
......@@ -786,7 +786,7 @@ void Simulator::addMovingObstacles(unsigned int nb)
}
if (found)
addMovingObstacle(dCell,1) ;
addMovingObstacle(dCell,type) ;
}
}
......@@ -807,23 +807,41 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
VEC3 ySide (0.0f,5.0f,0.0f);
#ifdef TWO_AND_HALF_DIM
Geom::Plane3D<PFP::REAL> pl(Algo::Surface::Geometry::faceNormal<PFP>(envMap_.map, d, envMap_.position), 0.0f);
Geom::Plane3D<PFP::REAL> pl(Algo::Surface::Geometry::facePlane<PFP>(envMap_.map, d, envMap_.position));
VEC3 np;
VEC3 displ = -xSide-ySide;
Agent::rotate(Agent::xyPlane, pl.normal(), displ);
vPos.push_back(start + displ);
np = start+displ;
pl.project(np); //<- without this line, the point is not on the plane
pl.project(np); //<- without this line, the point is not on the plane (no doublon)
vPos.push_back(np);
displ = xSide-ySide;
Agent::rotate(Agent::xyPlane, pl.normal(), displ);
vPos.push_back(start + displ);
np = start+displ;
pl.project(np);
vPos.push_back(np);
displ = xSide+ySide;
Agent::rotate(Agent::xyPlane, pl.normal(), displ);
vPos.push_back(start + displ);
np = start+displ;
pl.project(np);
vPos.push_back(np);
displ = ySide-xSide;
Agent::rotate(Agent::xyPlane, pl.normal(), displ);
vPos.push_back(start + displ);
np = start+displ;
pl.project(np);
vPos.push_back(np);
#else
vPos.push_back(start-xSide-ySide);
vPos.push_back(start+xSide-ySide);
......@@ -879,11 +897,11 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
TraversorV<PFP2::MAP> tv(mm->map);
for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
{
mm->position[d] += rotate(mm->position[d], bary, angle);
mm->position[d] += rotate2D(mm->position[d], bary, angle);
}
for(std::vector<VEC3>::iterator it = vPos.begin() ; it != vPos.end() ; ++it)
*it += rotate(*it , bary, angle);
// for(std::vector<VEC3>::iterator it = vPos.begin() ; it != vPos.end() ; ++it)
// *it += rotate2D(*it , bary, angle);
}
std::vector<VEC3> goals;
......
src/viewer.cpp
View file @ 60f3bfbe
......@@ -457,8 +457,10 @@ void SocialAgents::updateObstacleVBO()
void SocialAgents::updateObstaclePredTriVBO()
{
// CGoGNout<<"update"<<CGoGNendl;
for(unsigned int i=0 ; i < simulator.movingObstacles_.size() ; ++i)
{
Utils::VBO * vbo = m_triObst_VBO[i];
PFP::VEC3* data = static_cast<PFP::VEC3*>(vbo->lockPtr());
for(unsigned int j =0; j<simulator.movingObstacles_[i]->nbVertices; j++ )
......@@ -472,8 +474,9 @@ void SocialAgents::updateObstaclePredTriVBO()
p = simulator.envMap_.position[simulator.envMap_.map.phi1(simulator.movingObstacles_[i]->parts_[j]->d)];
data[j*4+2] = p;
}
}
vbo->releasePtr();
}
}
......@@ -788,7 +791,7 @@ void SocialAgents::cb_redraw()
{
updateObstaclePredTriVBO();
for(unsigned int i = 0 ; i < simulator.movingObstacles_.size() ; ++i)
for(unsigned int i = 0 ; i < m_triObst_Shader.size() ; ++i)
{
Utils::ShaderSimpleColor* moShader = m_triObst_Shader[i];
MovingObstacle * mo =simulator.movingObstacles_[i];
......@@ -1976,6 +1979,7 @@ int main(int argc, char** argv)
SocialAgents sa(config, minSize, nbAgent, nbObst,multires, iterations) ;
sa.setGeometry(0, 0, 1800, 1200) ;
sa.initGUI() ;
glewInit();
sa.show() ;
// if (argc > 1)
// sa.timer->start() ;
......
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