Skip to content

GitLab

S
SocialAgents3D
Commit 1684fabf authored by pitiot's avatar pitiot
Browse files
Options

Merge

parents 28c99a9b 7f81911e
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 599 additions and 433 deletions
CMakeLists.txt
View file @ 1684fabf
......@@ -2,8 +2,15 @@ cmake_minimum_required(VERSION 2.8)
project(SocialAgents)
#SET ( CMAKE_VERBOSE_MAKEFILE 1 )
#add_definitions(-DSPATIAL_HASHING)
SET(QT_USE_QTXML TRUE )
SET (CGoGN_EXT_INCLUDES ${CGoGN_EXT_INCLUDES} ${QT_INCLUDE_DIR})
SET (CGoGN_EXT_LIBS ${CGoGN_EXT_LIBS} ${QT_LIBRARIES} ${QGLVIEWER_LIBRARIES})
SET(CGoGN_ROOT_DIR ${CMAKE_SOURCE_DIR}/../../CGoGN CACHE STRING "CGoGN root dir")
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g")
include(${CGoGN_ROOT_DIR}/apps_cmake.txt)
......
Debug/CMakeLists.txt
View file @ 1684fabf
......@@ -51,4 +51,4 @@ add_executable( socialAgentsD
add_dependencies(socialAgentsD shader_targetD )
target_link_libraries( socialAgentsD ${CGoGN_LIBS_D} ${COMMON_LIBS} )
target_link_libraries( socialAgentsD ${CGoGN_LIBS_D} ${COMMON_LIBS} ${CGoGN_EXT_LIBS})
Release/CMakeLists.txt
View file @ 1684fabf
......@@ -10,7 +10,6 @@ ENDIF (WIN32)
include_directories(
${CMAKE_CURRENT_BINARY_DIR}
${CMAKE_CURRENT_SOURCE_DIR}/Release
# ${CMAKE_CURRENT_SOURCE_DIR}
../include
......@@ -30,6 +29,7 @@ add_custom_target(shader_target ${CGoGN_ROOT_DIR}/ThirdParty/bin/shader_to_h ${s
WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}
SOURCES ${shaders_src} )
SET(QT_USE_QTXML TRUE )
#SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
QT4_WRAP_UI( socialAgents_ui ../include/socialAgents.ui )
......@@ -52,4 +52,4 @@ add_executable( socialAgents
add_dependencies(socialAgents shader_target )
target_link_libraries( socialAgents ${CGoGN_LIBS_R} ${COMMON_LIBS} )
target_link_libraries( socialAgents ${CGoGN_LIBS_R} ${COMMON_LIBS} ${CGoGN_EXT_LIBS})
include/agent.h
View file @ 1684fabf
......@@ -12,12 +12,19 @@
//#define EXPORTING_AGENT
//#define EXPORTING_OBJ
#ifdef SECURED
#include "Algo/MovingObjects/particle_cell_2D_secured.h"
#else
#ifdef TWO_AND_HALF_DIM
#include "Algo/MovingObjects/particle_cell_2DandHalf.h"
#else
#include "Algo/MovingObjects/particle_cell_2D.h"
#endif
#endif
#ifdef EXPORTING_AGENT
#include "Utils/vbo.h"
#include "Utils/Shaders/shaderSimpleColor.h"
......@@ -39,6 +46,8 @@ public:
Agent(Simulator* sim, const VEC3& position, const VEC3& goals, Dart d) ;
Agent(Simulator* sim, const VEC3& position, const VEC3& goals) ;
static void rotate(const VEC3& planeN1, const VEC3& planeN2, VEC3& project);
void init(const VEC3& start, const VEC3& goal);
void initGL();
void draw();
......@@ -80,8 +89,8 @@ public:
// VertexAttribute<VEC3> position;
// VertexAttribute<VEC3> normal;
VEC3 previousPos;
float previousRot;
// VEC3 previousPos;
// float previousRot;
Geom::Matrix44f m_transfo;
// Algo::Render::GL2::MapRender* m_render;
......@@ -101,7 +110,12 @@ public:
#ifdef SECURED
CGoGN::Algo::Surface::MovingObjects::ParticleCell2DSecured<PFP> part_ ;
#else
#ifdef TWO_AND_HALF_DIM
CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP> part_ ;
#else
CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP> part_ ;
#endif
#endif
#endif
......@@ -114,6 +128,8 @@ public:
MovingObstacle **movingObstacles_;
int nb_mos;
static VEC3 xyPlane;
static unsigned int maxNeighbors_ ;
static unsigned int maxMovingObstacles_;
......
include/env_generator.hpp
View file @ 1684fabf
......@@ -40,8 +40,10 @@ Algo::Surface::Modelisation::Polyhedron<PFP> generateGrid(EnvMap& envMap)
{
unsigned int nx = envMap.geometry.size(0) / envMap.maxCellSize ;
unsigned int ny = envMap.geometry.size(1) / envMap.maxCellSize ;
if (nx < 1) nx = 1 ;
if (ny < 1) ny = 1 ;
if (nx < 1)
nx = 1 ;
if (ny < 1)
ny = 1 ;
std::cout << " - Generate Grid : " << nx << " x " << ny << std::endl ;
......@@ -607,15 +609,26 @@ void generatePlanet(EnvMap& envMap)
unsigned int nx = envMap.geometry.size(0) / envMap.maxCellSize ;
unsigned int ny = envMap.geometry.size(1) / envMap.maxCellSize ;
if (nx < 1)
nx = 1 ;
nx = 20 ;
if (ny < 1)
ny = 1 ;
ny = 20 ;
std::cout << "nx : " << nx << " ny " << ny << std::endl;
Algo::Surface::Modelisation::Polyhedron<PFP> prim(envMap.map, envMap.position) ;
prim.cylinder_topo(nx, ny, true, true) ;
double xRadius = envMap.geometry.size(0) / 2 * M_PI;
double yRadius = envMap.geometry.size(1) / 2 * M_PI ;
unsigned int count = 0;
for(Dart d = envMap.map.begin() ; d != envMap.map.end() ; envMap.map.next(d))
count++;
std::cout << "count : " << count << std::endl;
double xRadius = envMap.geometry.size(0);
double yRadius = envMap.geometry.size(1);
std::cout << "radius : " << xRadius+yRadius << std::endl;
prim.embedSphere((xRadius+yRadius)/2.0f) ;
}
......
include/env_map.h
View file @ 1684fabf
......@@ -33,6 +33,7 @@ class ArticulatedObstacle;
#include "pfp.h"
//#define EXPORTING3
#define TWO_AND_HALF_DIM
#ifdef EXPORTING3
#include "Utils/Shaders/shaderPhongTexture.h"
......@@ -308,9 +309,7 @@ inline void EnvMap::pushAgentInCells(Agent* agent, Dart d)
while (ddd != dd)
{
if (!map.isBoundaryMarked2(ddd))
{
addElementToVector<Agent*>(neighborAgentvect[ddd],agent);
}
// neighborAgentvect[ddd].push_back(agent) ;
// nbAgentsIncrease(ddd);
......@@ -335,9 +334,8 @@ inline void EnvMap::popAgentInCells(Agent* agent, Dart d)
while (ddd != dd)
{
if (!map.isBoundaryMarked2(ddd))
{
removeElementFromVector<Agent*>(neighborAgentvect[ddd], agent) ;
}
// nbAgentsDecrease(ddd) ;
ddd = map.alpha1(ddd) ;
}
......@@ -366,7 +364,7 @@ inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& b
neighbor_cells->clear();
CellMarkerMemo<FACE> memo_mark(map);
CellMarkerMemo<FACE> OneRingMark(map);
memo_mark.unmarkAll();
for (std::vector<Dart>::const_iterator it =belonging_cells.begin();it<belonging_cells.end();++it)
memo_mark.mark(*it);
......
include/moving_obstacle.h
View file @ 1684fabf
......@@ -6,7 +6,13 @@
#include "utils.h"
#include "env_map.h"
#include <set>
#include "Algo/MovingObjects/particle_cell_2D_memo.h"
#ifdef TWO_AND_HALF_DIM
#include "Algo/MovingObjects/particle_cell_2DandHalf_memo.h"
#else
#include "Algo/MovingObjects/particle_cell_2D_memo.h"
#endif
#define EXPORTING_BOXES
......@@ -55,7 +61,11 @@ public:
unsigned int nbVertices;
#ifdef TWO_AND_HALF_DIM
CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>* * parts_;
#else
CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>* * parts_;
#endif
PFP::MAP map;
VertexAttribute<VEC3> position;
......
include/simulator.h
View file @ 1684fabf
......@@ -98,6 +98,8 @@ public:
void setupCityScenario(int nbLines, int nbRank) ;
void setupScenario(unsigned int nbMaxAgent, bool pedWay=false) ;
void setupPlanetScenario(unsigned int nbAgents, unsigned int nbObstacles);
void addMovingObstacles(unsigned int nb);
void addMovingObstacle(Dart d, unsigned int obstType=0);
......
include/viewer.h
View file @ 1684fabf
......@@ -26,7 +26,7 @@
#include <fstream>
#include <sys/time.h>
#include "Utils/Qt/qtSimple.h"
#include "Utils/Qt/qtQGLV.h"
#include "ui_socialAgents.h"
#include "Utils/Qt/qtui.h"
......@@ -55,12 +55,14 @@
#include "shaderCustom.h"
//#define EXPORTING
//#define ONERING
#define SHADOWSHELL
using namespace CGoGN ;
typedef PFP::MAP MAP ;
class SocialAgents : public Utils::QT::SimpleQT
class SocialAgents : public Utils::QT::SimpleQGLV
{
Q_OBJECT
......@@ -87,7 +89,8 @@ public:
float angle_X, float angle_Y, float angle_Z) ;
#endif
void cb_keyPress(int keycode) ;
// void cb_keyPress(int keycode) ;
void keyPressEvent(QKeyEvent *e);
//RENDERING
Geom::Vec4f colDif ;
......@@ -104,6 +107,8 @@ public:
Utils::VBO* m_colorVBO;
Utils::ShaderSimpleColor* m_simpleColorShader;
Algo::Render::GL2::MapRender* m_renderWithin;
//building
Algo::Render::GL2::MapRender* m_render_scenary;
Utils::VBO* m_positionVBO_scenary;
......
src/agent.cpp
View file @ 1684fabf
......@@ -9,6 +9,9 @@
#include "agent.h"
#include "simulator.h"
#include "Geometry/frame.h"
#include "Utils/colorMaps.h"
VEC3 Agent::xyPlane = VEC3(0,0,1);
unsigned int Agent::maxNeighbors_ = 10 ;
unsigned int Agent::maxMovingObstacles_ = 20;
......@@ -18,7 +21,8 @@ float Agent::averageMaxSpeed_ = 2.0f ;
float Agent::neighborDist_ = 20.0f ;
float Agent::neighborDistSq_ = neighborDist_ * neighborDist_ ;
//float Agent::radius_ = 8.0f ;
float Agent::radius_ = 1.5f ;
float Agent::radius_ = 3.0f ;
//float Agent::radius_ = 1.5f ;
//float Agent::timeHorizon_ = 10.0f ;
float Agent::timeHorizon_ = 100.0f ;
float Agent::timeHorizonObst_ = 10.0f ;
......@@ -67,6 +71,17 @@ Agent::Agent(Simulator* sim, const VEC3& start, const VEC3& goal) :
init(start,goal);
}
void Agent::rotate(const VEC3& planeN1, const VEC3& planeN2, VEC3& project)
{
// rotate from face plane to (x,y) plane
float angle = Geom::angle(planeN1, planeN2) ;
if(angle > 0.0f)
{
VEC3 axis = planeN1 ^ planeN2 ;
project = Geom::rotate(axis, angle, project) ;
}
}
void Agent::init(const VEC3& start, const VEC3& goal)
{
......@@ -94,6 +109,7 @@ void Agent::init(const VEC3& start, const VEC3& goal)
goals_.clear() ;
goals_.push_back(goal) ;
goals_.push_back(start) ;
float ratio = (80.0f + rand() % 20) / 100.0f ;
maxSpeed_ = averageMaxSpeed_ * ratio ; // from 80% to 100% of averageMaxSpeed_
// color1 = 1.0f * ratio ; // red = high speed agents
......@@ -107,7 +123,9 @@ void Agent::init(const VEC3& start, const VEC3& goal)
// color1=0;
// color2 = 1.0f;
// }
for (unsigned int i=0 ; i<4 ; ++i) meanVelocity_[i].set(0) ;
for (unsigned int i=0 ; i<4 ; ++i)
meanVelocity_[i].set(0) ;
agentNeighbors_.reserve(maxNeighbors_ * 2) ;
obstacleNeighbors_.reserve(maxNeighbors_ * 2) ;
movingObstacleNeighbors_.reserve(maxNeighbors_ * 2) ;
......@@ -121,7 +139,10 @@ void Agent::initGL()
{
#ifdef EXPORTING_AGENT
m_ghost_shader = new Utils::ShaderSimpleColor();
m_ghost_shader->setColor(Geom::Vec4f(1.,0.,0.,0.));
VEC3 col = Utils::color_map_BCGYR(float(agentNo)/float(sim_->agents_.size()));
m_ghost_shader->setColor(Geom::Vec4f(col[0],col[1],col[2],0.));
// m_ghost_shader->setColor(Geom::Vec4f(1.,0.,0.,0.));
// m_ghost_shader->setAmbiant(Geom::Vec4f(1.,1.,0.,0.)) ;
// m_ghost_shader->setDiffuse(Geom::Vec4f(1.,1.,0.,0.));
......@@ -474,9 +495,12 @@ void Agent::update()
#endif
#ifdef EXPORTING_AGENT
m_ghost_previousPos.push_back(target);
if(m_ghost_previousPos.size()>m_ghost_nb)
m_ghost_previousPos.pop_front();
if(int(sim_->globalTime_ / sim_->timeStep_) % 4 == 0)
{
m_ghost_previousPos.push_back(target);
if(m_ghost_previousPos.size()>m_ghost_nb)
m_ghost_previousPos.pop_front();
}
#endif
#endif
......@@ -511,8 +535,8 @@ void Agent::update()
Geom::rotateZ(myRot,m_transfo);
Geom::translate(displ[0],displ[1],displ[2],m_transfo);
m_transfo.transpose();
previousPos = getPosition();
previousRot = myRot;
// previousPos = getPosition();
// previousRot = myRot;
#endif
}
......
src/env_map.cpp
View file @ 1684fabf
......@@ -85,6 +85,7 @@ void EnvMap::init(unsigned int config, REAL width, REAL height, REAL minSize, RE
break ;
case 2 :
CityGenerator::generateGrid<PFP>(*this) ;
// CityGenerator::generateTrianGrid<PFP>(*this,obstacleMark, buildingMark);
// CityGenerator::generateMall<PFP>(map, position, obstacleMark, buildingMark, sideSize);
break ;
case 3 :
......@@ -194,6 +195,9 @@ void EnvMap::init(unsigned int config, REAL width, REAL height, REAL minSize, RE
scale(20.0f);
}
break;
case 6:
CityGenerator::generatePlanet<PFP>(*this);
break;
}
// CityGenerator::simplifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
......@@ -204,7 +208,7 @@ void EnvMap::init(unsigned int config, REAL width, REAL height, REAL minSize, RE
map.init() ;
// registerObstaclesInFaces();
// TODO Check registerWallInFaces();
registerWallInFaces() ;
// registerWallInFaces() ;
// subdivideAllToMaxLevel();
for (unsigned int i = subdivisableFace.begin(); i < subdivisableFace.end(); subdivisableFace.next(i))
......@@ -349,75 +353,6 @@ void EnvMap::initGL()
}
std::cout << "bb " << bbTest.min() << " & " << bbTest.max() << std::endl;
// unsigned int i =0;
// for(std::vector<PFP::MAP * >::iterator it = m_map_Export.begin() ; it != m_map_Export.end() ; ++it, ++i)
// {
//
// CityGenerator::planetify<PFP>(**it, position_nmap[i], 2000.0f, bbTest);
// }
// m_map_Export = new PFP2::MAP();
//
// std::vector<std::string> attrNames ;
// m_obj_Export = new Algo::Surface::Import::OBJModel<PFP2>(m_map_Export);
// m_obj_Export->import("./meshRessources/cityTex/Building11.obj",attrNames);
//
// position_Export = m_map_Export.getAttribute<VEC3, VERTEX>(attrNames[0]) ;
// TraversorV<PFP2::MAP> tV(m_map_Export);
//
// VEC3 bary(0);
// unsigned int count = 0;
// for(Dart d = tV.begin() ; d != tV.end() ; d = tV.next())
// {
// bary += position_Export[d];
// count++;
// }
// bary /= float(count);
//
// for(Dart d = tV.begin() ; d != tV.end() ; d = tV.next())
// {
//// position_Export[d] -= bary;
// position_Export[d] *= 100.0f;
// std::cout << position_Export[d] << std::endl;
// position_Export[d] += VEC3(2000,0,0);
// std::cout << position_Export[d] << std::endl;
// }
//
// m_texcoordVBO_Export = new Utils::VBO();
// m_positionVBO_Export = new Utils::VBO();
// m_normalVBO_Export = new Utils::VBO();
//
// m_texture_Export = new Utils::Texture<2,Geom::Vec3uc>(GL_UNSIGNED_BYTE);
//
// if (m_texture_Export->load("./meshRessources/cityTex/AO_Buildings11.png"))
// m_texture_Export->update();
// else
// std::cout << "problem : loading texture" << std::endl;
//
// m_texture_Export->setWrapping(GL_CLAMP_TO_EDGE);
//
// m_shaderTex_Export = new ShaderCustomTex();
// m_shaderTex_Export->setAttributePosition(m_positionVBO_Export);
// m_shaderTex_Export->setAttributeTexCoord(m_texcoordVBO_Export);
// m_shaderTex_Export->setTextureUnit(GL_TEXTURE0);
// m_shaderTex_Export->setTexture(m_texture_Export);
//
// glEnable(GL_TEXTURE_2D);
//
// m_map_Export.setBrowser(NULL);
//
// if (!m_obj_Export->hasNormals())
// {
// normal_Export = m_map_Export.getAttribute<VEC3, VERTEX>("normal") ;
// if(!normal_Export.isValid())
// normal_Export = m_map_Export.addAttribute<VEC3, VERTEX>("normal") ;
//
// Algo::Surface::Geometry::computeNormalVertices<PFP2>(m_map_Export, m_obj_Export->m_positions, normal_Export) ;
// m_obj_Export->setNormalAttribute(normal_Export);
// }
//
// m_nbIndice_Export = m_obj_Export->createSimpleVBO_PTN(m_positionVBO_Export,m_texcoordVBO_Export,m_normalVBO_Export);
#endif
}
......
src/moving_obstacle.cpp
View file @ 1684fabf
......@@ -36,6 +36,7 @@ void MovingObstacle::addGeneralCell ( Dart d)
general_belonging.push_back(std::make_pair(d,1));
}
}
bool MovingObstacle::removeGeneralCell (Dart d)
{
std::vector< std::pair<Dart,int> >::iterator it =general_belonging.begin();
......@@ -142,7 +143,12 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
if(dInside==NIL)
dInside = sim_->envMap_.getBelongingCell(pos[0]);
#ifdef TWO_AND_HALF_DIM
parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>*[nbVertices];
#else
parts_ = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>*[nbVertices];
#endif
obstacles_ = new Obstacle*[nbVertices];
belonging_cells = new std::vector<Dart>[nbVertices];
neighbor_cells = new std::vector<Dart>[nbVertices];
......@@ -160,15 +166,20 @@ 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]);
CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>* part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2D<PFP>(sim_->envMap_.map, d, pos[i], sim_->envMap_.position);
parts_[i] = part;
center += pos[i];
{
Dart d = sim_->envMap_.getBelongingCell(pos[i]);
#ifdef TWO_AND_HALF_DIM
CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>* part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalf<PFP>(sim_->envMap_.map, d, pos[i], sim_->envMap_.position);
if(i==0)
dDir = d;
}
#else
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;
center += pos[i];
if(i==0)
dDir = d;
}
center /= nbVertices;
front=(pos[0] + pos[1]) / 2;
......@@ -224,6 +235,7 @@ 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) ;
// Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, 0.0f) ;
map.fillHole(groundFace);
groundFace = map.phi2(groundFace);
......@@ -278,7 +290,7 @@ void MovingObstacle::initGL()
// using simple shader with color
m_shader = new Utils::ShaderSimpleColor();
m_shader->setAttributePosition(m_positionVBO);
m_shader->setColor(Geom::Vec4f(0.,1.,0.,0.));
m_shader->setColor(Geom::Vec4f(0.,this->index==279,0.,0.));
// m_shader->setAmbiant(Geom::Vec4f(0.,1.,0.,0.));
// m_shader->setDiffuse(Geom::Vec4f(0.,1.,0.,0.));
......@@ -297,7 +309,8 @@ void MovingObstacle::draw()
// m_render->initPrimitives<PFP>(map, Algo::Render::GL2::LINES,false) ;
// m_render->initPrimitives<PFP>(map, Algo::Render::GL2::TRIANGLES,false) ;
m_positionVBO->updateData(position);
m_shader->setColor(Geom::Vec4f(movingObstacleNeighbors_.size()>1 ? 1.0f : 0,0.,0.,0.));
// m_shader->setColor(Geom::Vec4f(movingObstacleNeighbors_.size()==0 ? 1.0f : 0,0.,0.,0.));
m_shader->setColor(Geom::Vec4f(1.0f,0.,0.,0.0));
m_render->draw(m_shader, Algo::Render::GL2::TRIANGLES);
m_shader->setColor(Geom::Vec4f(0.,0.,0.,0.));
m_render->draw(m_shader, Algo::Render::GL2::LINES);
......@@ -774,15 +787,20 @@ void MovingObstacle::update()
Obstacle* o = obstacles_[i];
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;
if(!((sim_->envMap_.map.sameFace(d1,d2))&& (parts_[i]->crossCell==CGoGN::Algo::Surface::MovingObjects::NO_CROSS && parts_[(i+1)%nbVertices]->crossCell==CGoGN::Algo::Surface::MovingObjects::NO_CROSS)))
{
sim_->envMap_.popAndPushObstacleInCells(o,i);
sim_->envMap_.popAndPushObstacleInCells(o,i);
}
/////affichage des belonging cells
// CGoGNout<< CGoGNendl;
// CGoGNout << "Obstacle "<< i << ": ";
......@@ -829,7 +847,11 @@ void MovingObstacle::update()
std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1, Dart& d2)
{
#ifdef TWO_AND_HALF_DIM
CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalfMemo<PFP> * registering_part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DAndHalfMemo<PFP>(sim_->envMap_.map, d1,pos,sim_->envMap_.position);
#else
CGoGN::Algo::Surface::MovingObjects::ParticleCell2DMemo<PFP> * registering_part = new CGoGN::Algo::Surface::MovingObjects::ParticleCell2DMemo<PFP>(sim_->envMap_.map, d1,pos,sim_->envMap_.position);
#endif
std::vector<Dart> result =(registering_part->move(dest));
d2=registering_part->d;
// CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest<<CGoGNendl;
......@@ -858,13 +880,14 @@ void resetObstPartInFace(Obstacle* o, Dart d1)
{
MovingObstacle * mo = o->mo;
if (mo != NULL) {
if (mo != NULL)
{
unsigned int n =o->index;
VEC3 pos1 = mo->parts_[n]->getPosition();
if (Algo::Surface::Geometry::isPointInConvexFace2D < PFP
> (mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos1, true)) {
if (Algo::Surface::Geometry::isPointInConvexFace2D<PFP>(mo->sim_->envMap_.map, d1, mo->sim_->envMap_.position, pos1, true))
mo->parts_[n]->d = d1;
}
if(n==0)
mo->dDir = mo->parts_[0]->d;
}
......@@ -879,10 +902,9 @@ void resetPart(Obstacle * o, Dart d1)
if (mo->parts_[n]->d == mo->sim_->envMap_.map.phi1(d1))
mo->parts_[n]->d = d1;
if(n==0)
mo->dDir = mo->parts_[n]->d;
}
}
......@@ -901,7 +923,7 @@ void displayMO(Obstacle * o)
void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atteindre l'objectif // changer pour 2.5 ?
{
VEC3 goalVector;
if (index_parent<1)
if (index_parent<1) //not articulated
{
if(rigid_ || goals_.size()>1)
{
......@@ -975,9 +997,9 @@ void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atte
}
}
}
else
else //articulated
{
goalVector = get_center(parent,index_parent-1) -center;
goalVector = (get_center(parent,index_parent-1) -center);
float goalDist2 = goalVector.norm2() ;
if (goalDist2 > maxSpeed_)
{
......@@ -987,7 +1009,20 @@ void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atte
}
if (spinning)
angle =get_angle(goalVector,front-center);
{
float n_angle = get_angle(goalVector,front-center);
float bornAngle = M_PI/64.0f;
if(fabs(n_angle-angle)<bornAngle)
angle = n_angle;
else
{
if(n_angle-angle>=0.0f)