collisions ok subdiv pb save scenars

include/agent.h

@@ -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_ ;

include/env_map.h

@@ -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);

include/simulator.h

@@ -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;
 
 };
 

initPos/10.dat

+-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 

initPos/100.dat

+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 

src/agent.cpp

@@ -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();

src/simulator.cpp

@@ -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;
+
+}

src/viewer.cpp

@@ -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_ ;