Commit fe70a9c8 authored by Thomas Pitiot 's avatar Thomas Pitiot

working but collisions

parent e9c23882
......@@ -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;
......
......@@ -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
{
......
#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_;
}
}
}
......@@ -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");
......
......@@ -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;
......
......@@ -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);
......
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