Commit 853966ab authored by pitiot's avatar pitiot
Browse files

okish limaces

parent b7f81b10
......@@ -54,7 +54,7 @@ public:
MovingObstacle(Simulator* sim, int index, std::vector<PFP::VEC3> pos, std::vector<VEC3> goals, bool rigid, bool spin,int curGoal=0, Dart d=NIL, ArticulatedObstacle * art=NULL, int indParent=-1);
void initGL();
void draw(bool showPath=false);
void draw(bool showPath=false, bool showPrediction=false);
void addGeneralCell ( Dart d);
bool removeGeneralCell (Dart d);
bool test_opposition(VEC3 o, VEC3 p1, VEC3 p2);
......@@ -143,6 +143,7 @@ public:
VEC3 finalGoal;
float angle;
float rotor;
// Dart finalDart;
std::vector<VEC3> goals_ ;
......
......@@ -56,13 +56,13 @@
#include "shaderCustom.h"
#include "shaderPhongTexCust.h"
#define DRAW_REGISTRATION //montre l'enregistrement des obstacles mobiles
//#define DRAW_REGISTRATION //montre l'enregistrement détaillé des obstacles mobiles dansla topologie
#define AFFICHE_MESH
//#define EXPORTING // ne pas activer
#define ONERING
//#define SHADOWSHELL
//#define ONERING // affiche l'enregistrement global des obstacles mobiles en tant que cellules
//#define SHADOWSHELL // shadowshells ... old
using namespace CGoGN ;
......
......@@ -15,7 +15,9 @@ VEC3 Agent::xyPlane = VEC3(0,0,1);
unsigned int Agent::maxNeighbors_ = 10 ;
unsigned int Agent::maxMovingObstacles_ = 20;
float Agent::averageMaxSpeed_ =2.0f ;
float Agent::averageMaxSpeed_ =3.0f ;
//float Agent::panicSpeed = averageMaxSpeed_;
float Agent::panicSpeed = 2* averageMaxSpeed_; // scenario 4
// float Agent::averageMaxSpeed_ = 20.0f ;
float Agent::neighborDist_ = 10.0f ;
//float Agent::neighborDist_ = 20.0f ;
......@@ -110,7 +112,7 @@ void Agent::init(const VEC3& start, const VEC3& goal)
goals_.push_back(goal) ;
goals_.push_back(start) ;
float ratio = (80.0f + rand() % 20) / 100.0f ;
ratio = (80.0f + rand() % 20) / 100.0f ;
maxSpeed_ = averageMaxSpeed_ * ratio ; // from 80% to 100% of averageMaxSpeed_
// color1 = 1.0f * ratio ; // red = high speed agents
// color2 = 1.0f * 5 * (1 - ratio) ; // green = low speed agents
......@@ -709,7 +711,7 @@ void Agent::computePrefVelocity()
// la vitesse sera de maxSpeed_. Sinon, s'il est plus
// proche, la vitesse diminue.
if (goalDist2 > maxSpeed_)
if (goalDist2 > maxSpeed_*maxSpeed_)
{
goalVector.normalize() ;
goalVector *= maxSpeed_;
......@@ -810,7 +812,7 @@ void Agent::computeNewVelocity()
VEC3 norm;
int obst_power = 2 ; // the power to which elevate the agent-obstacle distance
int obst_power = 3 ; // the power to which elevate the agent-obstacle distance
float force_value;
......@@ -819,12 +821,23 @@ void Agent::computeNewVelocity()
#ifdef ARASH
/// test thomas vitesse panique
bool test=false;
for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
it != movingObstacleNeighbors_.end() ; ++it)
{
Obstacle * obst = it->second;
if(obst->mo!=NULL)
{
if(obst->mo->nbVertices<obst->mo->nbParticles && obst->index==obst->mo->nbVertices)
{
maxSpeed_= panicSpeed*ratio;
test=true;
// CGoGNout<<this->agentNo<<" agent panique ! "<<movingObstacleNeighbors_.size()<< CGoGNendl;
}
VEC3 p1=obst->p1 ;
VEC3 p2=obst->p2 ;
......@@ -837,7 +850,7 @@ void Agent::computeNewVelocity()
double sum_of_dists = d1+d2;
if(sum_of_dists < rest_sum_of_dists)
{
collision_softening_factor = pow(1-sum_of_dists/rest_sum_of_dists,obst_power);
collision_softening_factor = pow(sum_of_dists/rest_sum_of_dists,obst_power);
force_value = obst->obst_stiffness_agent*collision_softening_factor*(rest_sum_of_dists - sum_of_dists);
VEC3 v_obst = p2 - p1;
......@@ -856,6 +869,10 @@ void Agent::computeNewVelocity()
}
}
}
if(!test)
{
maxSpeed_ = averageMaxSpeed_ * ratio ;
}
#else
for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
......@@ -899,6 +916,7 @@ void Agent::computeNewVelocity()
int fixed_obst_power = 1; // the power to which elevate the agent-obstacle distance
Obstacle* fixed_obst ;
for(std::vector<std::pair<float, Obstacle*> >::iterator it = obstacleNeighbors_.begin() ;
it != obstacleNeighbors_.end() ;
++it)
......
......@@ -79,9 +79,9 @@ void EnvMap::init(unsigned int config, REAL width, REAL height, REAL minSize, RE
{
case 0 :
// CityGenerator::generateGrid<PFP>(*this) ;
// CityGenerator::generateGrid<PFP>(*this) ;
CityGenerator::generateTrianGrid<PFP>(*this,obstacleMark, buildingMark);
CityGenerator::generateCity<PFP>(*this,0,500.0f) ;
CityGenerator::generateGrid<PFP>(*this) ;
// CityGenerator::generateTrianGrid<PFP>(*this,obstacleMark, buildingMark);
// CityGenerator::generateCity<PFP>(*this,0,500.0f) ;
break ;
case 1 :
// CityGenerator::generateGrid<PFP>(*this) ;
......
......@@ -9,7 +9,7 @@
//float MovingObstacle::neighborDistSq_ = 5.0f * 5.0f;
float MovingObstacle::maxSpeed_ = 2.0f; // 8 pour limaces ?
float MovingObstacle::maxSpeed_ = 2.0f; // 8 pour limaces ? 3 pour scn 0 et 4
float MovingObstacle::neighborDist_ = 10.0f ;
float MovingObstacle::neighborDistSq_ = neighborDist_ * neighborDist_ ;
float MovingObstacle::timeHorizonObst_ = 10.0f;
......@@ -74,19 +74,21 @@ bool MovingObstacle::removeGeneralCell (Dart d)
bool MovingObstacle::is_inside(VEC3 p)
{
// return false;
VEC3 vec, norm, p1,vec2,p2;
VEC3 vec, norm, p1,vec2,p2, dir;
norm = VEC3 (0,0,1);
for (unsigned int i = 0; i < nbVertices; i++)
{
p1 = getPosition(i);
p2 = center;
p1 = parts_[i]->getPosition();
p2 = parts_[(i+1)%nbVertices]->getPosition();
vec = VEC3(p2 - p1);
dir = vec ^norm;
vec2 = VEC3(p -p1);
if (vec*vec2 < 0)
if (dir*vec2 < 0)
return false;
}
......@@ -170,17 +172,17 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
beta(0.60), // params shpaematching
alpha(0.95),
#endif
speed_detection_factor(10) // facteur qui détermine la taille du vecteur d'anticipation
speed_detection_factor(1) // facteur qui détermine la taille du vecteur d'anticipation 8 pour scn 0 et 4
{
if (index_parent>0) velocity_factor=2.0f;
stiff_agent=10000;
radius_agent=15;
stiff_agent=1000000;
radius_agent=17;
stiff_obst=0.02;
radius_obst=20;
long_radius_agent=25;
long_radius_agent=20;
long_radius_obst=5;
for (unsigned int i = 0; i < nbVertices; ++i)
......@@ -254,7 +256,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
if(i==0)
dDir = d;
}
front=(parts_[0]->getPosition() + parts_[1]->getPosition()) / 2;
front=(parts_[0]->getPosition() + parts_[nbVertices-1]->getPosition()) / 2;
......@@ -264,45 +266,50 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
// length = (pos[0]-pos[1]).norm();
// width = (pos[1]-pos[2]).norm();
// sum_dist_foci_rest = 2*(length + width*(sqrt(2)-0.5));
#ifndef TWO_AND_HALF_DIM
if ( parent==NULL && spinning) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
{
VEC3 axeZ=VEC3 (0,0,1);
#ifdef TWO_AND_HALF_DIM
// if ( parent==NULL) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
// {
// VEC3 axeZ=VEC3 (0,0,1);
//#ifdef TWO_AND_HALF_DIM
//
// VEC3 normale = Algo::Surface::Geometry::faceNormal<PFP>(sim->envMap_.map, parts_[0]->d, sim->envMap_.position);
//#else
// VEC3 normale =axeZ;
//#endif
// Geom::Matrix44f rot ;
// rot.identity() ;
// angle = Geom::angle(goals_[curGoal_] - center,front - center);
// Geom::rotate(axeZ,angle,rot);
//
// float angle1 = Geom::angle(normale, VEC3 (0,0,1) ) ;
// // CGoGNout<<"angle : "<<angle<<CGoGNendl;
// VEC3 axis = VEC3(0,0,1) ^ normale ;
//
// // Geom::translate(center[0],center[1],center[2],rot);
// Geom::rotate(axis, angle1, rot) ;
//
//
//// std::cout<<" angle : "<< angle;
//
// for (unsigned int i = 0; i < nbVertices; ++i)
// {
//// std::cout<<" || pos[i] avant : "<< pos [i];
// Geom::transform(pos[i],rot);
//// std::cout<<" || pos[i] APRES : "<< pos [i]<<std::endl;
//
// parts_[i]->move(pos[i]);
//
//
// }
// angle=0;
// front=(pos[0] + pos[1]) / 2;
// }
VEC3 normale = Algo::Surface::Geometry::faceNormal<PFP>(sim->envMap_.map, parts_[0]->d, sim->envMap_.position);
#else
VEC3 normale =axeZ;
#endif
Geom::Matrix44f rot ;
rot.identity() ;
// Geom::translate(center[0],center[1],center[2],rot);
angle =get_angle(goals_[curGoal_] - center,front - center);
Geom::rotate(axeZ,angle,rot);
float angle1 = Geom::angle(normale, VEC3 (0,0,1) ) ;
// CGoGNout<<"angle : "<<angle << " || angle1 : "<< angle1<<CGoGNendl;
if(angle1!=0)
{
VEC3 axis = VEC3(0,0,1) ^ normale ;
// Geom::translate(center[0],center[1],center[2],rot);
Geom::rotate(axis, angle1, rot) ;
}
// Geom::translate(center[0],center[1],center[2],rot);
// std::cout<<" angle : "<< angle;
for (unsigned int i = 0; i < nbVertices; ++i)
{
// std::cout<<" || pos[i] avant : "<< pos [i];
pos[i]=center + Geom::transform(pos[i]-center,rot);
// pos[i]+=rotate2D(pos[i],center,angle);
// std::cout<<" || pos[i] APRES : "<< pos [i]<<std::endl;
parts_[i]->move(pos[i]);
}
angle=0;
front=(pos[0] + pos[nbVertices-1]) / 2;
}
#endif
groundFace = map.newFace(nbVertices);
for (unsigned int i = 0; i < nbVertices; ++i)
......@@ -453,7 +460,7 @@ void MovingObstacle::initGL()
m_ds = new Utils::Drawer();
}
void MovingObstacle::draw(bool showPath)
void MovingObstacle::draw(bool showPath, bool showPrediction)
{
#ifdef EXPORTING_BOXES
// m_render->initPrimitives<PFP>(map, Algo::Render::GL2::LINES,false) ;
......@@ -512,7 +519,7 @@ void MovingObstacle::draw(bool showPath)
m_ds->end();
m_ds->endList();
}
if(true)// affichage anticipation
if(showPrediction)// affichage anticipation
{
m_ds->newList(GL_COMPILE_AND_EXECUTE);
......@@ -1212,12 +1219,17 @@ void MovingObstacle::updateForces()
{
float abs_angle= angle > 0 ? 1 : -1;
float rotor=0;
rotor=0;
float max_angle = maxSpeed_*sim_->timeStep_;
if (index_parent==0)
rotor = abs_angle*angle > 0.005f ? abs_angle *0.005f : angle ;
{
if (sim_->config ==4) rotor = abs_angle*angle > max_angle/10 ? abs_angle *max_angle/10 : angle ;
else rotor = abs_angle*angle > 10*max_angle ? abs_angle *10*max_angle : angle ;
}
else
rotor = abs_angle*angle > 0.02f ? abs_angle *0.005f : angle ;
rotor = abs_angle*angle > max_angle ? abs_angle * max_angle : angle ;
// CGoGNout << "Obstacle "<< index << CGoGNendl;
// CGoGNout << "vitesse : "<< velocity_ << CGoGNendl;
// on fait tourner l'obstacle
......@@ -1383,8 +1395,13 @@ void MovingObstacle::updateRegistration()
//////ajouts evitement lointain
if(index_parent<1)
{
#ifndef TWO_AND_HALF_DIM
VEC3 vitesse=velocity_;
if (angle>0.5) vitesse+=rotate2D(velocity_,VEC3(0,0,0), 5*rotor);
// CGoGNout<<" vitesse : "<<vitesse<< " || velocity : "<<velocity_<< " || rotor :"<< rotor<<CGoGNendl;
#endif
parts_[nbVertices]->move(center);
parts_[nbVertices+1]->move(center+speed_detection_factor*velocity_);
parts_[nbVertices+1]->move(center+speed_detection_factor*vitesse);
}
}
......@@ -1424,8 +1441,8 @@ void MovingObstacle::updateRegistration()
o->p2 = parts_[nbVertices+1]->getPosition();
Dart d1 = parts_[nbVertices]->d;
Dart d2 = parts_[nbVertices+1]->d;
o->obst_radius_infl_agent=radius_agent*velocity_factor;
o->obst_stiffness_agent=stiff_agent*velocity_factor;
o->obst_radius_infl_agent=long_radius_agent;
o->obst_stiffness_agent=stiff_agent;
if(!((sim_->envMap_.map.sameFace(d1,d2))&& (parts_[nbVertices]->crossCell==CGoGN::Algo::Surface::MovingObjects::NO_CROSS && parts_[nbVertices+1]->crossCell==CGoGN::Algo::Surface::MovingObjects::NO_CROSS)))
......@@ -1760,8 +1777,8 @@ void MovingObstacle::computeNewVelocity() //comportement des obstacles en tenan
forward *= objective;
}
// if(index_parent==0 && angle>0.005f)
// forward/=10;
// if(index_parent==0 && parent!=NULL && angle>0.75)
// forward/=2;
newVelocity_=forward;
}
......
......@@ -19,7 +19,7 @@ Simulator::Simulator(unsigned int config, unsigned int minS, unsigned int nbAgen
#ifdef TWO_AND_HALF_DIM
timeStep_(0.1f),
#else
timeStep_(config > 2 ? 0.02f : 0.25f),
timeStep_(config > 2 ? 0.1f : 0.25f),
// timeStep_(0.1f),
#endif
......@@ -71,7 +71,7 @@ void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst
break ;
case 3 :
envMap_.init(config, 500.0f, 500.0f, minSize, 50.0f) ; //grosses cases
addMovingObstacles(nbObst, 1);
addMovingObstacles(nbObst, 1, 1000);
setupScenario(nbAgent, false) ;
addPathToObstacles(envMap_.buildingMark, true);
......@@ -161,6 +161,7 @@ void Simulator::doStep()
envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
struct timespec begTime, endTime ;
// CGoGNout<<" valide dart 559 "<<envMap_.map.isDartValid(Dart(559))<<CGoGNendl;
for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
......@@ -462,8 +463,8 @@ void Simulator::setupCrowdedScenario(unsigned int nbAgents , unsigned int nbObst
Dart d = envMap_.getBelongingCell(center);
unsigned int nbx = 25;
unsigned int nby = 25;
unsigned int nbx = 50;
unsigned int nby = 30;
VEC3 pos=center;
......
......@@ -728,6 +728,26 @@ void SocialAgents::cb_redraw()
m_shaderTexAgent->disableVertexAttribs();
}
#else
//test thomas P
// glDisable(GL_LIGHTING);
// for (unsigned int i=0; i< simulator.agents_.size(); ++i)
// {
// VEC3 pos =simulator.agents_[i]->part_.getPosition();
// m_ds->newList(GL_COMPILE_AND_EXECUTE);
// m_ds->lineWidth(5.0f);
// m_ds->pointSize(10.0f);
// m_ds->begin(GL_POINTS);
// // fait varier la couleur du plus pres au plus loin
//
// m_ds->color3f(simulator.agents_[i]->color1 , simulator.agents_[i]->color2, simulator.agents_[i]->color3);
//
// m_ds->vertex(pos);
//
// m_ds->end();
// m_ds->endList();
// }
// /// save
PFP::VEC3* data = static_cast<PFP::VEC3*>(m_agentsVBO->lockPtr());
for (unsigned int i=0; i< simulator.agents_.size(); ++i)
{
......@@ -816,7 +836,7 @@ void SocialAgents::cb_redraw()
#endif
simulator.movingObstacles_[i]->draw(drawObstPath);
simulator.movingObstacles_[i]->draw(drawObstPath, drawObstacles);
#ifdef DRAW_REGISTRATION
MovingObstacle * mo =simulator.movingObstacles_[i];
if (drawEnvTopo)
......@@ -892,7 +912,7 @@ void SocialAgents::cb_redraw()
m_renderTopo->drawTopo();
if (draw_dart && dartSlider < (simulator.envMap_.map.end().index))
m_renderTopo->overdrawDart(dartSlider,5.0f,0,1.0f,0.5f);
m_renderTopo->overdrawDart(dartSlider,10.0f,0.5f,1.0f,0.5f);
if(drawObstacles)
{
......
Supports Markdown
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