Commit 78d89f5c authored by Thomas Pitiot 's avatar Thomas Pitiot

collisions ok subdiv pb save scenars

parent fe70a9c8
......@@ -20,8 +20,6 @@ public:
void updatePosition();
void updateNeighbors() ;
void computePrefVelocity();
void computeAvoidanceVelocity();
void computeNewVelocity();
......@@ -39,8 +37,10 @@ public:
//vitesses
VEC3 newVelocity_ ;
VEC3 prefVelocity_ ;
VEC3 avoidanceVelocity_;
VEC3 forces;
double ag_stiffness; // agent-agent interaction stiffness
double ag_damping ; // agent-agent interaction damping
double ag_mass ;
float maxVelocity_;
......@@ -55,9 +55,10 @@ public:
float neighborTriDistSq_;
//calcul des répulsions
VEC3 avoidanceAgents();
VEC3 avoidanceSegments();
VEC3 avoidanceTriangles();
void goalAttraction();
void avoidanceAgents();
void avoidanceSegments();
void avoidanceTriangles();
// nb de voisins considérés par l'évitement
// int maxNeighbors_ ;
......
......@@ -66,12 +66,20 @@ public:
float mapMinZ;
float mapMaxZ;
float maxCellRay;
bool simplifyVolume (Dart d , bool OneLevelDifference=true);
bool subdivideVolume(Dart d , bool OneLevelDifference=true);
void subdivideToMaxLevel();
bool subdivideMap();
bool simplifyMap();
Dart getBelongingCell(const PFP::VEC3& pos);
unsigned int nbAgentsToSubdivide;
unsigned int nbAgentsToSimplify;
PFP::MAP map ;
PFP::MAP map ;
CellMarkerMemo<MAP,VOLUME> refineMark;
CellMarkerMemo<MAP,VOLUME> simplifyMark;
VertexAttribute<VEC3, MAP> position ;
FaceAttribute<VEC3, MAP> facecenter;
VolumeAttribute<VEC3, MAP> color ;
......@@ -82,6 +90,7 @@ public:
VolumeAttribute<ARETES, MAP> RegisteredNeighborEdges ;
VolumeAttribute<TRIANGLES, MAP> RegisteredTriangles ;
VolumeAttribute<TRIANGLES, MAP> RegisteredNeighborTriangles ;
VolumeAttribute<bool,MAP> CellOnMaxLevel;
Geom::BoundingBox<PFP::VEC3> bb;
std::vector<Dart> getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1, unsigned int state);
......
......@@ -6,7 +6,9 @@
#include "tree.h"
#include "surface.h"
#include "agent.h"
#include <iostream>
#include <string>
#include <fstream>
using namespace CGoGN ;
class Simulator
......@@ -23,14 +25,27 @@ public:
void initSurfaces();
void initFixedObjects();
void initEmptyMovingObject();
std::vector<VEC3> getPositions(int nbAgents, VEC3 center, float radius);
// MAP
EnvMap envMap_;
//simulation values
double nbSteps_;
float timeStep_;
//objects presents
MovingObject * aiguille;
std::vector<Agent *>agents;
std::vector<ArticulatedObject * >objects;
std::vector<Surface * > surfaces;
bool Needle_or_knife;
int nb_dead;
};
......
-0.61772 3.60283 1.62419
-1.48199 2.68791 -2.56493
1.16573 -1.60738 -3.47238
-1.02685 -3.8191 0.600066
-0.295962 -0.998211 3.86212
-3.67441 0.476184 1.50731
2.96766 1.33807 2.32434
2.8046 2.12959 -1.89712
-2.96749 -1.3382 -2.32448
3.12642 -2.4717 0.34087
0.806392 3.78769 -1.00158
-2.79591 2.51577 -1.36154
-2.88817 -1.87547 2.03497
-1.79961 -1.7458 3.11666
0.374747 -0.0700964 3.98179
-1.52709 3.20203 -1.84797
-0.778678 2.51655 -3.01009
3.49432 1.4943 1.24771
-0.699406 -1.3948 -3.68312
1.98005 -3.14092 -1.48797
0.0411536 1.36292 -3.76042
-3.96162 -0.511432 0.20979
0.477949 1.45454 3.69539
-1.97023 3.45794 -0.401068
3.8383 -0.986692 0.5421
0.998243 -3.83334 -0.555887
-2.34204 2.14397 2.43275
2.58106 2.4422 1.83678
-1.56562 -0.401666 3.65889
3.64843 1.59062 -0.398588
-1.78351 -3.56629 -0.317332
1.77357 3.47046 0.900199
0.0866235 3.6189 1.70178
3.22569 -2.27481 0.648199
1.13808 -3.67359 1.09977
2.31619 -2.82919 1.62202
-2.60496 -2.90073 0.894378
-2.29914 -1.00398 -3.11544
-3.28805 1.21049 -1.92963
3.02388 -2.51383 -0.732659
-1.38675 -0.111189 -3.75027
2.25453 -1.08586 -3.12058
3.88086 0.133844 -0.959687
3.35452 1.12297 -1.86712
-0.508439 -3.85243 -0.948834
2.82042 -1.93635 -2.07263
-0.787659 -3.24775 -2.19811
3.57751 0.0118241 1.78922
-2.98679 -2.60423 -0.545047
-3.32195 -0.436451 -2.18498
1.75094 0.617141 3.54307
-3.64018 -1.35795 -0.951343
0.739034 -1.98161 3.39515
2.06617 3.38851 -0.498973
-3.6033 -0.587249 1.63442
-2.20984 2.0564 -2.62447
-1.16182 3.11049 2.23048
-0.638258 3.87605 -0.754242
-0.10344 -3.97665 0.418998
-3.51133 -1.80691 0.636886
3.95586 0.412219 0.425749
3.32072 -0.517908 -2.169
-2.05635 -3.02716 -1.61485
1.55965 -2.42262 -2.7746
2.72591 -0.409553 2.89857
2.34505 1.96621 -2.5758
-3.24581 0.759923 2.21071
3.21752 -1.45101 1.88206
0.751497 2.52401 -3.01075
-1.3337 -3.62997 1.02205
-3.02214 2.6188 0.0925826
0.653426 -3.4165 -1.97499
2.18606 -3.34664 0.145371
0.830487 -1.39235 -3.65673
2.62551 0.463342 -2.98195
-2.19567 3.11826 1.20643
-0.429003 -1.24465 3.77714
1.61652 3.07453 -1.98347
0.154774 2.67281 2.97189
2.88175 2.46816 -1.26637
0.0772892 -2.52184 -3.10392
-0.527369 -2.58869 3.00343
-3.23649 1.94549 1.31915
1.44637 1.33312 -3.48293
-2.76772 -0.560737 2.8329
-3.67816 1.50346 -0.459043
-1.73793 -2.86727 2.18137
-3.86517 0.198737 -1.01042
1.41895 -0.0465355 -3.73957
-2.5764 0.549533 -3.01001
-2.85942 -1.90303 -2.04992
-2.20487 0.890527 3.21644
-1.00153 3.80522 0.71918
-1.10698 2.02001 3.27019
1.51341 -0.831607 3.60805
1.30671 3.08863 2.18011
-1.58068 -2.24834 -2.90628
2.16288 -1.90576 2.77309
0.508542 3.94996 0.373106
2.85286 1.0963 2.58056
0.0223685 -0.055024 -3.99956
-0.192531 -3.50102 1.92504
-1.35986 1.27587 -3.53877
1.06714 -2.98761 2.43627
1.69828 1.98534 3.02891
2.98369 2.63617 0.38498
-0.790731 0.751983 3.84828
-3.84637 0.769461 0.783188
-0.0406946 3.40001 -2.10672
3.68754 -1.2806 -0.873001
This diff is collapsed.
This diff is collapsed.
......@@ -11,9 +11,7 @@ void Agent::init(Simulator * sim ,VEC3 startingPoint,std::vector<VEC3> goals,Dar
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);
srand(agentNo);
float ratio = (80.0f + rand() % 20) / 100.0f ;
......@@ -21,6 +19,11 @@ void Agent::init(Simulator * sim ,VEC3 startingPoint,std::vector<VEC3> goals,Dar
g=1.0f*5.0f*(1-ratio);
b=0.0f;
ag_stiffness = 1000.0; // agent-agent interaction stiffness
ag_damping = 1.0; // agent-agent interaction damping
ag_mass = 0.1;
forces=VEC3(0);
maxVelocity_=std::min(averageVelocity*ratio,sim_->envMap_.maxCellRay/sim_->timeStep_);
//distances de détection
......@@ -96,6 +99,7 @@ void Agent::updateNeighbors()
ag->r=0;
ag->g=0;
ag->b=1;
sim_->nb_dead+=2;
return;
}
agentNeighbors_.push_back(std::make_pair(distSq, ag)) ;
......@@ -120,6 +124,7 @@ void Agent::updateNeighbors()
ag->r=0;
ag->g=0;
ag->b=1;
sim_->nb_dead+=2;
return;
}
agentNeighbors_.push_back(std::make_pair(distSq, ag)) ;
......@@ -132,50 +137,13 @@ void Agent::updateNeighbors()
}
void Agent::computePrefVelocity()
{
VEC3 goalVector = goals_[curGoal_] - part_->getPosition() ;
float goalDist2 = goalVector.norm2() ;
// Si l'agent arrive à proximité de l'objectif,
// alors il passe à l'objectif suivant
// CGoGNout<<"goalDist :"<<goalDist2<<" et radius :" <<radius *radius<<CGoGNendl;
if (goalDist2 < 0.1)
{
// CGoGNout<<"obj atteint"<<CGoGNendl;
curGoal_ = (curGoal_ + 1) % goals_.size() ;
goalVector = goals_[curGoal_] - part_->getPosition() ;
goalDist2 = goalVector.norm2() ;
}
// goadDist2 est le vecteur reliant l'agent à l'objectif.
// Tant que ce vecteur a une norme supérieure à maxSpeed_
// (i.e. tant qu'il est encore assez loin de l'objectif)
// la vitesse sera de maxSpeed_. Sinon, s'il est plus
// proche, la vitesse diminue.
prefVelocity_ = goalVector ;
if(prefVelocity_.norm2()>maxVelocity_*maxVelocity_)
{
prefVelocity_.normalize();
prefVelocity_*=maxVelocity_;
}
}
VEC3 Agent::avoidanceAgents()
void Agent::avoidanceAgents()
{
VEC3 res =VEC3(0);
VEC3 previous_pos = getPosition() - newVelocity_*sim_->timeStep_;
double ag_stiffness = 1000.0; // agent-agent interaction stiffness
double ag_damping = 1.0; // agent-agent interaction damping
double ag_mass = 0.1;
// double ag_power = 1; // the power to which elevate the agent-agent distance
srand48(agentNo);
double rand = 2.0*drand48()-1.0; // compris entre -1 et 1
double radius_var = 0.5;
double ag_phys_radius_coef = 2.0*(1 + rand*radius_var); // valeurs uniformement réparties entre min et max
for(std::pair<float, Agent*> pere : agentNeighbors_)
......@@ -185,7 +153,7 @@ VEC3 Agent::avoidanceAgents()
const float distSq = pere.first;
const float dist = sqrt(distSq);
const float combinedRadius = ag_phys_radius_coef*(radius + other->radius) ;
const float combinedRadius = 3*(radius + other->radius) ;
// const float combinedRadiusSq = combinedRadius * combinedRadius ;
VEC3 other_previous_pos = other->getPosition() - (other->newVelocity_*sim_->timeStep_);
......@@ -208,44 +176,58 @@ VEC3 Agent::avoidanceAgents()
// cerr << "autres agents : force_value = " << force_value << endl;
res += force_value * u_other;
forces += force_value * u_other;
}
}
res= res* (sim_->timeStep_ / ag_mass);
return res;
}
VEC3 Agent::avoidanceTriangles()
void Agent::goalAttraction()
{
return VEC3(0);
//----- force due à l'attraction de l'objectif ----------
float goal_attraction_force = 100.0; // agent-goal interaction stiffness
VEC3 p_goal = goals_[curGoal_];
VEC3 u_goal = p_goal - getPosition();
if (u_goal.norm2() < 0.1)
{
// CGoGNout<<"obj atteint"<<CGoGNendl;
curGoal_ = (curGoal_ + 1) % goals_.size() ;
u_goal = p_goal - getPosition();
}
u_goal.normalize();
// cerr << agentNo << "---------------- position = " << getPosition() << endl;
forces += goal_attraction_force * u_goal;
}
VEC3 Agent::avoidanceSegments()
void Agent::avoidanceTriangles()
{
return VEC3(0);
}
void Agent::computeAvoidanceVelocity()
void Agent::avoidanceSegments()
{
avoidanceVelocity_=avoidanceAgents()+avoidanceSegments()+avoidanceTriangles();
if(avoidanceVelocity_.norm2()>maxVelocity_*maxVelocity_)
{
avoidanceVelocity_.normalize();
avoidanceVelocity_*=maxVelocity_;
}
}
void Agent::computeNewVelocity()
{
if (alive)
{
computePrefVelocity();
computeAvoidanceVelocity();
forces=VEC3(0);
goalAttraction();
avoidanceAgents();
avoidanceSegments();
avoidanceTriangles();
newVelocity_=prefVelocity_+avoidanceVelocity_;
newVelocity_=forces * (sim_->timeStep_ / ag_mass);
if(newVelocity_.norm2()>maxVelocity_*maxVelocity_)
{
newVelocity_.normalize();
......
This diff is collapsed.
......@@ -18,6 +18,7 @@ Simulator::~Simulator()
void Simulator::init(int argc, char **argv)
{
envMap_.init(argc,argv);
nb_dead=0;
timeStep_=0.01f;
nbSteps_=0;
if (argc > 1)
......@@ -25,6 +26,7 @@ void Simulator::init(int argc, char **argv)
else
Needle_or_knife=true;
initAgents(500);
envMap_.subdivideToMaxLevel();
// initFixedObjects();
// initSurfaces();
// initMovingObject();
......@@ -51,7 +53,7 @@ void Simulator::doStep()
}
}
}
envMap_.subdivideMap();
++nbSteps_ ;
}
......@@ -62,7 +64,7 @@ void Simulator::initAgents(int nbAgents)
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 ;
......@@ -71,7 +73,7 @@ void Simulator::initAgents(int nbAgents)
std::cout << "radius " << radius << std::endl;
VEC3 center = (VEC3(envMap_.mapMaxX,envMap_.mapMaxY,envMap_.mapMaxZ) + VEC3(envMap_.mapMinX,envMap_.mapMinY,envMap_.mapMinZ)) / 2 ;
std::vector<VEC3> posAgents = sphere(nbAgents,center,radius);
std::vector<VEC3> posAgents = getPositions(nbAgents,center,radius);
// for ( int i = 0 ; i < nbAgents ; ++i)
for(VEC3 v : posAgents)
......@@ -203,3 +205,66 @@ void Simulator::initMovingObject()
CGoGNout<<"Aiguille initialisée : "<<aiguille->index_articulated<<CGoGNendl;
// CGoGNout<<"points :"<<p.first<<" ; "<<p2.first<<" ; "<</*p3.first<<*/CGoGNendl;
}
std::vector<VEC3> Simulator::getPositions(int nbAgents,VEC3 center,float radius)
{
// on essaye d'ouvrir le fichier
std::vector<VEC3> res;
std::stringstream filename;
filename<<nbAgents;
std::stringstream datas;
datas<<"/home/pitiot/CGoGN_Apps/SocialAgents3D/initPos/"<<filename.str().c_str()<<".dat";
std::string data = datas.str();
std::ifstream fichier(data.c_str(),std::ios::in ); //déclaration du flux et ouverture du fichier
if(fichier) // si l'ouverture a réussi
{
// instructions
CGoGNout<<"ouverture fichier OK : lecture"<<CGoGNendl;
VEC3 f;
for(int i = 0 ; i< nbAgents; i++)
{
fichier>>f;
res.push_back(f);
}
// while(std::getline(fichier,f))
// {
// res.push_back(f);
// }
fichier.close();
}
else // sinon
{
std::cerr << "fichier non trouvé, écriture" << std::endl;
res=sphere(nbAgents,center,radius);
std::ofstream fichier2(data.c_str(), std::ios::out | std::ios::trunc);
if(fichier2)
{
for(VEC3 f :res)
{
fichier2 << f<<std::endl;
}
fichier2.close();
}
else
{
std::cerr << "fichier non ouvert ..." << std::endl;
}
}
return res;
}
......@@ -802,6 +802,7 @@ void Volusion::cb_redraw()
// Affichage des stats dans la barre d'état
std::ostringstream oss ;
oss <<" Dead Agents : "<<simul.nb_dead;
// oss << "Elapsed time : " << elapsedTime/(1000000000.0f*refresh);
// oss << " | " << ((float)frames)/refresh << " fps" ;
oss << " | Iterations " << simul.nbSteps_ ;
......
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