Commit 900ad261 authored by Thomas Pitiot 's avatar Thomas Pitiot

ajout agents

parent 8d135897
......@@ -12,16 +12,16 @@ class Simulator;
class Agent
{
public:
Agent(Simulator* sim,VEC3 startingPoint);
Agent(Simulator* sim, VEC3 startingPoint,Dart d) ;
Agent(Simulator* sim,VEC3 startingPoint,std::vector<VEC3> goals);
Agent(Simulator* sim, VEC3 startingPoint,std::vector<VEC3> goals,Dart d) ;
~Agent();
init(VEC3 startingPoint,Dart d);
void move();
update();
VEC3 getPosition() ;
void init(VEC3 startingPoint,std::vector<VEC3> goals,Dart d);
void updatePosition();
void updateNeighbors() ;
void computePrefVelocity();
void computeAvoidanceVelocity();
void computeNewVelocity();
// affichage
void initGL();
......@@ -31,15 +31,16 @@ public:
//particule
Particule part_;
Particule * part_;
// objectif(s)
unsigned int curGoal_ ;
std::vector<VEC3> goals_ ;
//vitesses
VEC3 velocity_ ;
VEC3 newVelocity_ ;
VEC3 prefVelocity_ ;
VEC3 avoidanceVelocity_;
float maxVelocity_;
//voisins retenus pour l'évitement
......
......@@ -34,12 +34,16 @@
using namespace CGoGN ;
#include "pfp.h"
typedef CGoGN::Algo::Volume::MovingObjects::ParticleCell3D<PFP> Particule;
typedef CGoGN::Algo::Volume::MovingObjects::ParticleCell3DMemo<PFP> ParticuleMemo;
#include "segment.h"
#include "triangle.h"
#include "agent.h"
typedef CGoGN::Algo::Volume::MovingObjects::ParticleCell3D<PFP> Particule;
typedef CGoGN::Algo::Volume::MovingObjects::ParticleCell3DMemo<PFP> ParticuleMemo;
class EnvMap
{
......@@ -71,6 +75,8 @@ public:
FaceAttribute<VEC3, MAP> facecenter;
VolumeAttribute<VEC3, MAP> color ;
VolumeAttribute<VEC3, MAP> volumecenter;
VolumeAttribute<AGENTS,MAP> RegisteredAgents;
VolumeAttribute<AGENTS,MAP> RegisteredNeighborAgents;
VolumeAttribute<ARETES, MAP> RegisteredEdges ;
VolumeAttribute<ARETES, MAP> RegisteredNeighborEdges ;
VolumeAttribute<TRIANGLES, MAP> RegisteredTriangles ;
......@@ -107,6 +113,16 @@ public:
void popTriangleInCellAsNeighbor(Triangle* cell, Dart d);
void resetPartSubdiv(Triangle* cell);
/// enregistrement des agents
void agentChangeFace(Agent * ag , Dart oldCell);
void popAgent(Agent * agent,Dart oldCell) ;
void pushAgent(Agent * agent,Dart newVolume) ;
void pushAgentInCell(Agent* o, Dart d);
void popAgentInCell(Agent* o, Dart d);
void pushAgentInCellAsNeighbor(Agent* cell, Dart d);
void popAgentInCellAsNeighbor(Agent* cell, Dart d);
} ;
......@@ -151,6 +167,38 @@ inline std::vector<Dart> EnvMap::getMemoCross(const VEC3& pos, const VEC3& dest,
}
inline void EnvMap::pushAgentInCell(Agent* o, Dart d)
{
#ifdef IHMap
assert(map.getCurrentLevel() == map.getMaxLevel());
#endif
addElementToVector(RegisteredAgents[d],o);
}
inline void EnvMap::popAgentInCell(Agent* o, Dart d)
{
#ifdef IHMap
assert(map.getCurrentLevel() == map.getMaxLevel());
#endif
removeElementFromVector(RegisteredAgents[d],o);
}
inline void EnvMap::pushAgentInCellAsNeighbor(Agent* o, Dart d)
{
#ifdef IHMap
assert(map.getCurrentLevel() == map.getMaxLevel());
#endif
addElementToVector(RegisteredNeighborAgents[d],o);
}
inline void EnvMap::popAgentInCellAsNeighbor(Agent* o, Dart d)
{
#ifdef IHMap
assert(map.getCurrentLevel() == map.getMaxLevel());
#endif
removeElementFromVector(RegisteredNeighborAgents[d],o);
}
inline void EnvMap::FindNeighborCells (const std::vector<Dart>& belonging_cells, std::vector<Dart> *neighbor_cells)
{
#ifdef IHMap
......
......@@ -20,10 +20,9 @@
#include "Algo/Multiresolution/Map2MR/Masks/sqrt3.h"
#include "Topology/generic/parameters.h"
class Segment;
class Triangle;
class Agent;
typedef enum
{
......@@ -45,9 +44,12 @@ struct PFP : public PFP_STANDARD
// definition des listes d'arêtes
typedef std::vector<Segment*> ARETE ;
typedef NoTypeNameAttribute<ARETE> ARETES ;
typedef std::vector<Agent*> AGENT ;
typedef NoTypeNameAttribute<AGENT> AGENTS ;
typedef std::vector<Triangle*> TRIANGLE ;
typedef NoTypeNameAttribute<TRIANGLE> TRIANGLES ;
typedef NoTypeNameAttribute<ARETE> ARETES ;
//
// typedef NoMathIONameAttribute<std::pair<bool, bool> > BOOLATTRIB ;
......@@ -65,6 +67,7 @@ typedef PFPSurface::MAP SurfaceMap;
typedef PFP::MAP MAP;
typedef PFP::VEC3 VEC3 ;
typedef PFP::REAL REAL ;
typedef PFP::AGENTS AGENTS;
typedef PFP::ARETES ARETES;
typedef PFP::TRIANGLES TRIANGLES;
......
......@@ -18,13 +18,14 @@ public:
void init(int argc, char **argv);
void doStep();
void initAgents();
void initAgents(int nbAgents);
void initMovingObject();
void initSurfaces();
void initFixedObjects();
void initEmptyMovingObject();
EnvMap envMap_;
double nbSteps_;
float timeStep_;
MovingObject * aiguille;
std::vector<Agent *>agents;
std::vector<ArticulatedObject * >objects;
......
......@@ -49,7 +49,7 @@
#include "Utils/frameManipulator.h"
///// UI
#include "ui_Volusion.h"
#include "ui_SocialAgents3D.h"
#include "Utils/Qt/qtui.h"
......@@ -102,6 +102,12 @@ class Volusion: public Utils::QT::SimpleQGLV
//MovingObject
// Utils::VBO* m_closestVBO; // afichages des points proches dans les sufaces
// Utils::PointSprite* m_spriteClosest;
//for agents
Utils::VBO* m_agentsPosVBO;
Utils::VBO* m_agentsColorVBO;
Utils::PointSprite* m_agentSprite;
///
Utils::VBO* m_MovingObjectVBO;
Utils::PointSprite* m_sprite;
Utils::Drawer* m_ds;
......
#include "simulator.h"
// initialisation Agent
void Agent::init(VEC3 startingPoint,Dart d)
void Agent::init(VEC3 startingPoint,std::vector<VEC3> goals,Dart d)
{
goals_=goals;
r=0.0f;
g=0.0f;
b=1.0f;
part_= CGoGN::Algo::Volume::MovingObjects::ParticleCell3D<PFP>(sim_->envMap_.map, d, startingPoint , sim_->envMap_.position, &(sim_->envMap_.facecenter),&(sim_->envMap_.volumecenter));
part_= new CGoGN::Algo::Volume::MovingObjects::ParticleCell3D<PFP>(sim_->envMap_.map, d, startingPoint , sim_->envMap_.position, &(sim_->envMap_.facecenter),&(sim_->envMap_.volumecenter));
curGoal_=0;
//vitesses
avoidanceVelocity_ =VEC3(0);
newVelocity_ =VEC3(0);
prefVelocity_=VEC3(0);
maxVelocity_=3.0f;
//lien simulateur
agentNo=sim_->agents.size() ;
Simulator* sim_;
alive=true;
sim_->agents.push_back(this);
}
Agent::Agent(Simulator* sim,VEC3 startingPoint, Dart d):
Agent::Agent(Simulator* sim,VEC3 startingPoint,std::vector<VEC3> goals, Dart d):
sim_(sim)
{
init(startingPoint,d);
init(startingPoint,goals,d);
}
Agent::Agent(Simulator* sim,VEC3 startingPoint):
Agent::Agent(Simulator* sim,VEC3 startingPoint,std::vector<VEC3> goals):
sim_(sim)
{
Dart d=sim_->envMap_.getBelongingCell(startingPoint);
init(startingPoint,d);
init(startingPoint,goals,d);
}
void Agent::color_close()
void Agent::updatePosition()
{
if(nbVertices>0)
if (alive)
{
Dart volume = this->getParticule(0)->getCell();
VEC3 pos=getParticule(0)->getPosition();
float distSq=0;
for (Segment * s : sim_->envMap_.RegisteredEdges[volume])
Dart oldCell = part_->getCell();
VEC3 velocity_ = newVelocity_ ;
VEC3 diff=(velocity_ * sim_->timeStep_);
if(diff.norm()>sim_->envMap_.maxCellRay) // on limite la taille du déplacement pour pouvoir vérifier rapidement si l'objectif est dans la carte
{
if(s->nid!=this || s->index!=nbEdges)
{
distSq = distSqPointLineSegment(s->p1,s->p2,pos);
s->nid->changeColor(s,distSq);
}
// CGoGNout<< "déplacement trop grand, on raccourci "<<CGoGNendl;
diff.normalize();
diff*=sim_->envMap_.maxCellRay*0.9;
}
for (Segment * s : sim_->envMap_.RegisteredNeighborEdges[volume])
VEC3 target = part_->getPosition() + diff ;
if(sim_->envMap_.map.isVolumeIncidentToBoundary(oldCell))
{
if(s->nid!=this || s->index!=nbEdges)
if(!sim_->envMap_.checkPointInMap(target,oldCell))
{
distSq = distSqPointLineSegment(s->p1,s->p2,pos);
s->nid->changeColor(s,distSq);
target=target-diff;
}
}
for(Triangle * t : sim_->envMap_.RegisteredTriangles[volume])
{
VEC3 closest = closestPointInTriangle(t->p1,t->p2,t->p3,pos);
t->closest_point=closest;
// CGoGNout<<"pos :"<<pos<<" || closest :"<<closest<<CGoGNendl;
distSq = (pos-closest).norm2();
if(distSq < ((pos-closest_point).norm2()))
closest_point=closest;
t->surf->changeColor(t,distSq);
}
for(Triangle * t : sim_->envMap_.RegisteredNeighborTriangles[volume])
{
VEC3 closest = closestPointInTriangle(t->p1,t->p2,t->p3,pos);
t->closest_point=closest;
// CGoGNout<<"pos :"<<pos<<" || closest :"<<closest<<CGoGNendl;
distSq = (pos-closest).norm2();
if(distSq < ((pos-closest_point).norm2()))
closest_point=closest;
t->surf->changeColor(t,distSq);
}
part_->move(target) ;
}
}
void Agent::unColor_close(Dart volume)
void Agent::updateNeighbors()
{
for (Segment * s : sim_->envMap_.RegisteredEdges[volume])
{
if(s->nid!=this || s->index!=nbEdges)
{
s->nid->changeColor(s,-1);
}
}
for (Segment * s : sim_->envMap_.RegisteredNeighborEdges[volume])
{
if(s->nid!=this || s->index!=nbEdges)
{
s->nid->changeColor(s,-1);
}
}
for(Triangle * t : sim_->envMap_.RegisteredTriangles[volume])
{
t->surf->changeColor(t,-1);
}
for(Triangle * t : sim_->envMap_.RegisteredNeighborTriangles[volume])
{
t->surf->changeColor(t,-1);
}
}
void Agent::createEdge(int index)
void Agent::computePrefVelocity()
{
parts_.push_back(new CGoGN::Algo::Volume::MovingObjects::ParticleCell3D<PFP>(sim_->envMap_.map, this->getParticule(0)->getCell(), getParticule(0)->getPosition() , sim_->envMap_.position,&(sim_->envMap_.facecenter),&(sim_->envMap_.volumecenter)));
partNeedReset.push_back(0);
Segment* o = new Segment(getParticule(index+1)->getPosition(),
getParticule(0)->getPosition(),
this,index+1,0,index,r,g,b);
Segments_.push_back(o);
sim_->envMap_.FirstRegistrationSegment(o);
if(index>0)
prefVelocity_=goals_[curGoal_]-part_->getPosition();
if(prefVelocity_.norm()>maxVelocity_)
{
(Segments_[index-1])->indexPart2=index+1;
prefVelocity_.normalize();
prefVelocity_*=maxVelocity_;
}
nbEdges++;
nbVertices++;
}
void Agent::move(VEC3 diff)
void Agent::computeAvoidanceVelocity()
{
Dart oldCell = this->getParticule(0)->getCell();
if(diff.norm()>sim_->envMap_.maxCellRay) // on limite la taille du déplacement pour pouvoir vérifier rapidement si l'objectif est dans la carte
avoidanceVelocity_;
if(avoidanceVelocity_.norm()>maxVelocity_)
{
// CGoGNout<< "déplacement trop grand, on raccourci "<<CGoGNendl;
diff.normalize();
diff*=sim_->envMap_.maxCellRay*0.9;
}
VEC3 pos = getPosition(0)+diff;
if(sim_->envMap_.map.isVolumeIncidentToBoundary(oldCell))
{
if(!sim_->envMap_.checkPointInMap(pos,oldCell))
{
pos=pos-diff;
}
avoidanceVelocity_.normalize();
avoidanceVelocity_*=maxVelocity_;
}
}
void Agent::computeNewVelocity()
{
computePrefVelocity();
computeAvoidanceVelocity();
this->getParticule(0)->move(pos);
updateRegistration();
this->unColor_close(oldCell);
#ifdef IHMap
if(!getParticule(0)->crossCell==CGoGN::Algo::Volume::MovingObjects::NO_CROSS) /// a rajouter si les cellules sont assez petites
newVelocity_=prefVelocity_+avoidanceVelocity_;
if(newVelocity_.norm()>maxVelocity_)
{
refine();
newVelocity_.normalize();
newVelocity_*=maxVelocity_;
}
#endif
this->color_close();
if((getPosition(0)-getPosition(nbEdges)).norm2()>edgeSize)
{
createEdge(nbEdges);
}
}
void Agent::updateRegistration()
// affichage
void Agent::initGL()
{
Segment* o = Segments_[nbEdges-1];
o->p1 = getParticule(o->indexPart1)->getPosition();
o->p2 = getParticule(o->indexPart2)->getPosition();
Dart d1 = this->getParticule(o->indexPart1)->getCell();
Dart d2 = this->getParticule(o->indexPart2)->getCell();
if(!(
(sim_->envMap_.map.sameVolume(d1,d2))
&& (getParticule(o->indexPart1)->crossCell==CGoGN::Algo::Volume::MovingObjects::NO_CROSS && getParticule(o->indexPart2)->crossCell==CGoGN::Algo::Volume::MovingObjects::NO_CROSS)))
{
sim_->envMap_.popAndPushSegment(o);
}
/////affichage du general_belonging
// CGoGNout<< CGoGNendl;
// CGoGNout << "General : ";
// for(std::vector<std::pair<Dart , int> >::iterator ite = general_belonging.begin();ite!=general_belonging.end(); ++ite)
// {
// CGoGNout<<"< "<<(*ite).first.index<<" , "<<(*ite).second<<"> ; ";
// }
// CGoGNout<< CGoGNendl;
}
void Agent::refine()
void Agent::draw()
{
#ifdef IHMap
Dart volume = this->getParticule(0)->getCell();
REAL rayon = sim_->envMap_.volumeMaxdistance(volume) ;
if(rayon >2* sim_->envMap_.maxCellRay ) // on subdivise si le rayon est 2 fois le maxCellRay
{
if(sim_->envMap_.subdivideVolume(volume))
{
refine();
}
}
#endif
}
......@@ -543,6 +543,33 @@ void EnvMap::popTriangle(Triangle* o)
}
}
void EnvMap::agentChangeFace(Agent* agent, Dart oldCell)
{
Dart newVolume = agent->part_->getCell();
popAgent(agent, oldCell) ;
pushAgent(agent, newVolume) ;
}
void EnvMap::popAgent(Agent *agent, Dart oldCell)
{
popAgentInCell(agent, oldCell);
for(Vol v : volumesAdjacentByVertex3<PFP::MAP>(map,oldCell))
{
popAgentInCellAsNeighbor(agent, v);
}
}
void EnvMap::pushAgent(Agent *agent, Dart newVolume)
{
pushAgentInCell(agent, newVolume);
for(Vol v : volumesAdjacentByVertex3<PFP::MAP>(map,newVolume))
{
pushAgentInCellAsNeighbor(agent, v);
}
}
bool EnvMap::subdivideVolume(Dart dglobal, bool OneLevelDifference)
{
bool res =false;
......
......@@ -18,27 +18,76 @@ Simulator::~Simulator()
void Simulator::init(int argc, char **argv)
{
envMap_.init(argc,argv);
timeStep_=0.01f;
nbSteps_=0;
if (argc > 1)
Needle_or_knife = atoi(argv[1]) ;
else
Needle_or_knife=true;
initAgents(100);
// initFixedObjects();
initSurfaces();
initMovingObject();
// initSurfaces();
// initMovingObject();
// initEmptyMovingObject();
}
void Simulator::doStep()
{
for(Agent * ag : agents)
{
ag->updateNeighbors();
ag->computePrefVelocity();
ag->computeNewVelocity();
Dart oldCell=ag->part_->getCell();
ag->updatePosition();
if(ag->part_->crossCell!=CGoGN::Algo::Volume::MovingObjects::NO_CROSS)
{
envMap_.agentChangeFace(ag,oldCell);
}
}
++nbSteps_ ;
}
void Simulator::initAgents(int nbAgents)
{
VEC3 size=VEC3(envMap_.mapMaxX-envMap_.mapMinX,envMap_.mapMaxY-envMap_.mapMinY,envMap_.mapMaxZ-envMap_.mapMinZ);
// Bordure à éviter autour de la scène (10% de sa taille)
int xBorder = size[0] / 10 ;
int yBorder = size[1] / 10 ;
// Les coordonnées sont comprises entre xMin et xMin+xDelta
// Départ des agents sur un cercle centré
int xSize = size[0] - 2 * xBorder ;
int ySize = size[1] - 2 * yBorder ;
int radius = std::min(xSize, ySize) / 2 ;
std::cout << "radius " << radius << std::endl;
VEC3 center = (VEC3(envMap_.mapMaxX,envMap_.mapMaxY,envMap_.mapMaxZ) + VEC3(envMap_.mapMinX,envMap_.mapMinY,envMap_.mapMinZ)) / 2 ;
for ( int i = 0 ; i < nbAgents ; ++i)
{
double angle = i * 2.0f * M_PI / float(nbAgents) ;
VEC3 v(std::cos(angle) * radius, std::sin(angle) * radius, 0.12) ;
VEC3 start = center + v ;
VEC3 goal = center - v ;
std::vector<VEC3> goals;
goals.push_back(goal);
goals.push_back(start);
Agent * ag = new Agent(this,start,goals);
// CGoGNout<<"agent n°" << agents_[i]->agentNo << " : "<< agents_[i]<<CGoGNendl;
}
}
void Simulator::initSurfaces()
{
......
This diff is collapsed.
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