working but collisions

include/agent.h

@@ -16,16 +16,14 @@ public:
     Agent(Simulator* sim, VEC3 startingPoint,std::vector<VEC3> goals,Dart d) ;
     ~Agent();
     void init(Simulator * sim ,VEC3 startingPoint,std::vector<VEC3> goals,Dart d);
-
+    VEC3 getPosition();
     void updatePosition();
 
     void updateNeighbors() ;
     void computePrefVelocity();
     void computeAvoidanceVelocity();
     void computeNewVelocity();
-    // affichage
-    void initGL();
-    void draw();
+
 
 
 
@@ -34,6 +32,7 @@ public:
     Particule * part_;
 
     static float radius; // taille de l'agent
+    static float averageVelocity; // vitesse moyenne d'un agent
     // objectif(s)
     unsigned int curGoal_ ;
     std::vector<VEC3> goals_ ;
@@ -50,6 +49,18 @@ public:
     std::vector<std::pair<float, Segment*> > segmentNeighbors_ ;
     std::vector<std::pair<float, Triangle*> > triangleNeighbors_;
 
+    //avec les distances de détection
+    float neighborAgDistSq_;
+    float neighborSegDistSq_;
+    float neighborTriDistSq_;
+
+    //calcul des répulsions
+    VEC3 avoidanceAgents();
+    VEC3 avoidanceSegments();
+    VEC3 avoidanceTriangles();
+
+    // nb de voisins considérés par l'évitement
+//    int maxNeighbors_ ;
     //couleur
     float r;
     float g;

include/utils.h

@@ -4,13 +4,135 @@
 #include <cmath>
 #include <limits>
 #include <vector>
-
+#include <stdlib.h>
+#include <stdio.h>
+#include <time.h>
+#include <math.h>
 #include "Geometry/vector_gen.h"
 
 
 typedef CGoGN::Geom::Vec3f VEC3 ;
 
-static const float RVO_EPSILON = 0.00001f ;
+
+
+
+typedef double vec3[3];
+
+inline double frand(void){return ((rand()-(RAND_MAX/2))/(RAND_MAX/2.));}
+
+inline double dot(vec3 v1,vec3 v2){ return v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2];}
+
+inline double length(vec3 v){  return sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]); }
+
+inline double length(vec3 v1,vec3 v2)
+{
+  vec3 v;
+  v[0] = v2[0] - v1[0]; v[1] = v2[1] - v1[1]; v[2] = v2[2] - v1[2];
+  return length(v);
+}
+
+inline double get_coulomb_energy(int N,vec3 p[])
+{
+  double e = 0;
+  for(int i = 0;i<N;i++)
+    for(int j = i+1; j<N; j++ ) {
+      e += 1/ length(p[i],p[j]);
+    }
+  return e;
+}
+
+inline void get_forces(int N,vec3 f[], vec3 p[])
+{
+  int i,j;
+  for(i = 0;i<N;i++){
+    f[i][0] = 0;f[i][1] = 0;f[i][2] = 0;
+  }
+  for(i = 0;i<N;i++){
+    for(j = i+1; j<N; j++ ) {
+      vec3 r = {p[i][0]-p[j][0],p[i][1]-p[j][1],p[i][2]-p[j][2]};
+      double l = length(r); l = 1/(l*l*l);double ff;
+      ff = l*r[0]; f[i][0] += ff; f[j][0] -= ff;
+      ff = l*r[1]; f[i][1] += ff; f[j][1] -= ff;
+      ff = l*r[2]; f[i][2] += ff; f[j][2] -= ff;
+    }
+  }
+}
+
+inline std::vector<VEC3> sphere (int nbVertices,VEC3 origin,double rayon)
+{
+  static int N=100,Nstep=1000;
+  static double step=0.01;
+  static double minimal_step=1.e-10;
+  std::vector<VEC3> res;
+  N = nbVertices;
+
+  vec3 *p0 = new vec3[N];
+  vec3 *p1 = new vec3[N];
+  vec3 *f = new vec3[N];
+  int i,k;
+  vec3 *pp0 = p0, *pp1 = p1;
+
+  srand(10);
+
+  for(i = 0; i<N; i++ ) {
+    p0[i][0] = 2*frand();
+    p0[i][1] = 2*frand();
+    p0[i][2] = 2*frand();
+    double l = length(p0[i]);
+    if(l!=0.0){
+      p0[i][0] /= l;
+      p0[i][1] /= l;
+      p0[i][2] /= l;
+    } else
+      i--;
+  }
+
+  double e0 = get_coulomb_energy(N,p0);
+  for(k = 0;k<Nstep;k++) {
+    get_forces(N,f,p0);
+    for(i=0; i < N;i++) {
+      double d = dot(f[i],pp0[i]);
+      f[i][0]  -= pp0[i][0]*d;
+      f[i][1]  -= pp0[i][1]*d;
+      f[i][2]  -= pp0[i][2]*d;
+      pp1[i][0] = pp0[i][0]+f[i][0]*step;
+      pp1[i][1] = pp0[i][1]+f[i][1]*step;
+      pp1[i][2] = pp0[i][2]+f[i][2]*step;
+      double l = length(pp1[i]);
+      pp1[i][0] /= l;
+      pp1[i][1] /= l;
+      pp1[i][2] /= l;
+    }
+    double e = get_coulomb_energy(N,pp1);
+    if(e >= e0){  // not successfull step
+      step /= 2;
+      if(step < minimal_step)
+    break;
+      continue;
+    } else {   // successfull step
+      vec3 *t = pp0;      pp0 = pp1; pp1 = t;
+      e0 = e;
+      step*=2;
+    }
+  }
+
+
+
+  for(i = 0;i<N;i++)
+  {
+      p0[i][0]=p0[i][0]*rayon+origin[0];
+      p0[i][1]=p0[i][1]*rayon+origin[1];
+      p0[i][2]=p0[i][2]*rayon+origin[2];
+    res.push_back(VEC3(p0[i][0],p0[i][1],p0[i][2]));
+  }
+
+
+  delete[] p0;
+  delete[] p1;
+  delete[] f;
+
+  return res;
+}
 
 struct Line
 {

src/agent.cpp

 #include "simulator.h"
 
 float Agent::radius=0.05;
-
+float Agent ::averageVelocity=3.0f;
 // initialisation Agent
 void Agent::init(Simulator * sim ,VEC3 startingPoint,std::vector<VEC3> goals,Dart d)
 {
     sim_=sim;
     goals_=goals;
-    r=0.0f;
-    g=0.0f;
-    b=1.0f;
+    agentNo=sim_->agents.size() ;
     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;
+    srand(agentNo);
+
+    float ratio = (80.0f + rand() % 20) / 100.0f ;
+    r=1.0f*ratio;
+    g=1.0f*5.0f*(1-ratio);
+    b=0.0f;
+
+    maxVelocity_=std::min(averageVelocity*ratio,sim_->envMap_.maxCellRay/sim_->timeStep_);
+
+    //distances de détection
+    float neighborDist=sim_->envMap_.maxCellRay;
+    neighborAgDistSq_=neighborDist*neighborDist;
+    neighborSegDistSq_=neighborDist*neighborDist;
+    neighborTriDistSq_=neighborDist*neighborDist;
     //lien simulateur
-    agentNo=sim_->agents.size() ;
+
     alive=true;
+    sim_->envMap_.pushAgent(this,d);
     sim_->agents.push_back(this);
 }
 
@@ -40,13 +52,6 @@ void Agent::updatePosition()
         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
-        {
-    //        CGoGNout<< "déplacement trop grand, on raccourci "<<CGoGNendl;
-
-            diff.normalize();
-            diff*=sim_->envMap_.maxCellRay*0.9;
-        }
         VEC3 target = part_->getPosition() + diff ;
         if(sim_->envMap_.map.isVolumeIncidentToBoundary(oldCell))
         {
@@ -63,9 +68,68 @@ void Agent::updatePosition()
 
 
 }
+VEC3 Agent::getPosition()
+{
+    return part_->getPosition();
+}
 
 void Agent::updateNeighbors()
 {
+    if (alive)
+    {
+        agentNeighbors_.clear();
+        VEC3 pos =getPosition();
+        for(Agent * ag : sim_->envMap_.RegisteredAgents[part_->getCell()])
+        {
+            if (ag->alive && ag!=this)
+            {
+                float distSq = (pos - ag->getPosition()).norm2() ;
+                if ( distSq < neighborAgDistSq_)
+                {
+                    if(distSq < radius*radius)
+                    {
+                       alive = false;
+                       ag->alive=false;
+                       r=0;
+                       g=0;
+                       b=1;
+                       ag->r=0;
+                       ag->g=0;
+                       ag->b=1;
+                       return;
+                    }
+                    agentNeighbors_.push_back(std::make_pair(distSq, ag)) ;
+                }
+            }
+
+        }
+        for(Agent * ag : sim_->envMap_.RegisteredNeighborAgents[part_->getCell()])
+        {
+            if (ag->alive && ag!=this)
+            {
+                float distSq = (pos - ag->getPosition()).norm2() ;
+                if ( distSq < neighborAgDistSq_)
+                {
+                    if(distSq < radius*radius) // cas de collision
+                    {
+                       alive = false;
+                       ag->alive=false;
+                       r=0;
+                       g=0;
+                       b=1;
+                       ag->r=0;
+                       ag->g=0;
+                       ag->b=1;
+                       return;
+                    }
+                    agentNeighbors_.push_back(std::make_pair(distSq, ag)) ;
+                }
+            }
+
+
+        }
+    }
+
 
 }
 void Agent::computePrefVelocity()
@@ -100,32 +164,92 @@ void Agent::computePrefVelocity()
     }
 }
 
-void Agent::computeAvoidanceVelocity()
+VEC3 Agent::avoidanceAgents()
 {
-    if(avoidanceVelocity_.norm()>maxVelocity_)
+    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_)
     {
-        avoidanceVelocity_.normalize();
-        avoidanceVelocity_*=maxVelocity_;
+        Agent* other = pere.second ;
+
+
+        const float distSq = pere.first;
+        const float dist   = sqrt(distSq);
+        const float combinedRadius = ag_phys_radius_coef*(radius + other->radius) ;
+        // const float combinedRadiusSq = combinedRadius * combinedRadius ;
+
+        VEC3 other_previous_pos = other->getPosition() - (other->newVelocity_*sim_->timeStep_);
+        VEC3 previous_relativePosition = other_previous_pos - previous_pos;
+        float previous_distSq = previous_relativePosition.norm2();
+        float previous_dist = sqrt(previous_distSq);
+
+        // const VEC3 u_other(relativePosition);
+        VEC3 u_other(other->getPosition()-getPosition());
+        u_other.normalize();
+
+        // cerr << "dist=" << dist << " combinedRadius=" << combinedRadius << endl;
+
+        if(dist < combinedRadius)
+        {
+            // collision_softening_factor = pow(1-dist/combinedRadius,ag_power);
+            float collision_softening_factor = 1;
+            float force_value = - ag_stiffness*collision_softening_factor*(combinedRadius-dist)
+                - ag_damping * (dist - previous_dist) / sim_->timeStep_;
+
+            // cerr << "autres agents : force_value = " << force_value << endl;
+
+            res += force_value * u_other;
+        }
+
     }
+
+    res= res* (sim_->timeStep_ / ag_mass);
+
+
+    return res;
 }
-void Agent::computeNewVelocity()
-{
-    computePrefVelocity();
-    computeAvoidanceVelocity();
 
-    newVelocity_=prefVelocity_+avoidanceVelocity_;
-    if(newVelocity_.norm()>maxVelocity_)
-    {
-        newVelocity_.normalize();
-        newVelocity_*=maxVelocity_;
-    }
+VEC3 Agent::avoidanceTriangles()
+{
+    return VEC3(0);
+}
+VEC3 Agent::avoidanceSegments()
+{
+    return VEC3(0);
 }
-// affichage
-void Agent::initGL()
+void Agent::computeAvoidanceVelocity()
 {
+    avoidanceVelocity_=avoidanceAgents()+avoidanceSegments()+avoidanceTriangles();
 
+
+    if(avoidanceVelocity_.norm2()>maxVelocity_*maxVelocity_)
+    {
+        avoidanceVelocity_.normalize();
+        avoidanceVelocity_*=maxVelocity_;
+    }
 }
-void Agent::draw()
+void Agent::computeNewVelocity()
 {
+    if (alive)
+    {
+        computePrefVelocity();
+        computeAvoidanceVelocity();
 
+        newVelocity_=prefVelocity_+avoidanceVelocity_;
+        if(newVelocity_.norm2()>maxVelocity_*maxVelocity_)
+        {
+            newVelocity_.normalize();
+            newVelocity_*=maxVelocity_;
+        }
+    }
 }

src/env_map.cpp

@@ -75,6 +75,8 @@ void EnvMap::init(int argc, char **argv)
     facecenter = map.addAttribute<VEC3, FACE, MAP>("facecenter");
     volumecenter=map.addAttribute<VEC3, VOLUME, MAP>("volumecenter");
     color = map.addAttribute<VEC3, VOLUME, MAP>("color");
+    RegisteredAgents=map.addAttribute<AGENTS,VOLUME, MAP>("RegisteredAgents");
+    RegisteredNeighborAgents=map.addAttribute<AGENTS,VOLUME, MAP>("RegisteredNeighborAgents");
     RegisteredEdges=map.addAttribute<ARETES,VOLUME, MAP>("RegisteredEdges");
     RegisteredNeighborEdges=map.addAttribute<ARETES,VOLUME, MAP>("RegisteredNeighborEdges");
     RegisteredTriangles=map.addAttribute<TRIANGLES,VOLUME, MAP>("RegisteredTriangles");

src/simulator.cpp

@@ -20,12 +20,11 @@ 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);
+       initAgents(500);
 //     initFixedObjects();
 //     initSurfaces();
 //     initMovingObject();
@@ -39,17 +38,18 @@ 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)
+        if(ag->alive)
         {
-            envMap_.agentChangeFace(ag,oldCell);
+            ag->updateNeighbors();
+            ag->computeNewVelocity();
+            Dart oldCell=ag->part_->getCell();
+            ag->updatePosition();
+
+            if(ag->part_->crossCell!=CGoGN::Algo::Volume::MovingObjects::NO_CROSS)
+            {
+                envMap_.agentChangeFace(ag,oldCell);
+            }
         }
-
     }
 
 	++nbSteps_ ;
@@ -71,10 +71,13 @@ 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 ;
 
-    for ( int i = 0 ; i < nbAgents ; ++i)
+    std::vector<VEC3> posAgents = sphere(nbAgents,center,radius);
+
+//    for ( int i = 0 ; i < nbAgents ; ++i)
+    for(VEC3 v : posAgents)
     {
-        double angle = i * 2.0f * M_PI / float(nbAgents) ;
-        VEC3 v(std::cos(angle) * radius, std::sin(angle) * radius, 0.12) ;
+//        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;

src/viewer.cpp

@@ -292,6 +292,7 @@ void Volusion::cb_initGL()
     for(Agent * ag :simul.agents)
     {
         data[i]=ag->part_->getPosition();
+//        CGoGNout<<"agent (num, colors)"<<ag->agentNo<<" "<<ag->r<<" "<<ag->g<<" "<<ag->b<<CGoGNendl;
         dataColor[i]=VEC3(ag->r,ag->g,ag->b);
         i++;
     }
@@ -299,7 +300,7 @@ void Volusion::cb_initGL()
     m_agentsPosVBO->releasePtr();
     m_agentsColorVBO->releasePtr();
 
-    m_agentSprite = new Utils::PointSprite();
+    m_agentSprite = new Utils::PointSprite(true);
     m_agentSprite->setAttributePosition(m_agentsPosVBO);
     m_agentSprite->setAttributeColor(m_agentsColorVBO);
     m_agentSprite->setSize(Agent::radius);