Skip to content

GitLab

S
SocialAgents3D
Commit cc85941a authored by Thomas Jund's avatar Thomas Jund
Browse files
Options

limace improved

parent d44c4a4f
Hide whitespace changes
Inline Side-by-side
Showing with 132 additions and 50 deletions
include/env_map.h
View file @ cc85941a
......@@ -79,7 +79,7 @@ public:
void popAgentInCells(Agent* agent, Dart d) ;
void agentChangeFace(Agent* agent, Dart oldFace) ;
void popAndPushObstacleInCells(Obstacle* o, int n);
Dart popAndPushObstacleInCells(Obstacle* o, int n);
void pushObstacleInCells(Obstacle * mo);
void pushObstacleInOneRingCells(Obstacle * o, Dart d, int n);
void pushObstacleInCells(Obstacle* o, int n, const std::vector<Dart>& memo_cross);
......
include/moving_obstacle.h
View file @ cc85941a
......@@ -75,6 +75,7 @@ public:
std::vector<VEC3> goals_ ;
unsigned int curGoal_;
Dart dDir;
// static float neighborDistSq_;
static unsigned int maxNeighbors_;
......
src/env_map.cpp
View file @ cc85941a
......@@ -554,7 +554,7 @@ void EnvMap::pushObstacleInCells(Obstacle * o)// réenregistre l'obstacle en que
}
}
void EnvMap::popAndPushObstacleInCells(Obstacle* o, int n)// maj de l'enregistrement
Dart EnvMap::popAndPushObstacleInCells(Obstacle* o, int n)// maj de l'enregistrement
{
MovingObstacle * mo = o->mo;
......@@ -593,6 +593,8 @@ void EnvMap::popAndPushObstacleInCells(Obstacle* o, int n)// maj de l'enregistre
pushObstacleInCells(o, n, memo);
}
}
return d1;
}
void EnvMap::pushObstacleInOneRingCells(Obstacle * o, Dart d, int n)
......
src/moving_obstacle.cpp
View file @ cc85941a
......@@ -144,6 +144,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
{
edgeLength[d] = VEC3(position[map.phi1(d)]-position[d]).norm();
}
dDir=dInside;
}
center /= nbVertices;
......@@ -197,8 +199,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{
Dart d(ind); //WARNING : works only for one face created at start !
return position[d]+deformation[d];
// return position[d];
// return position[d]+deformation[d];
return position[d];
}
VEC3 MovingObstacle::getPosition(unsigned int ind)
......@@ -422,11 +424,11 @@ void MovingObstacle::update()
VEC3 v1 = (position[map.phi1(dd)]-position[dd]);
//inertie
velocity[dd] = velocity[dd]*0.9f;
velocity[dd] = velocity[dd]*0.90f;
//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
float norm = v1.norm();
float rigidity = 500.0f;
float rigidity = 40.0f;
float stretch = rigidity*(edgeLength[dd]-v1.norm());
VEC3 f = stretch*(v1/norm)*sim_->timeStep_;
......@@ -495,8 +497,8 @@ void MovingObstacle::update()
map.next(d);
}
if(!rigid_)
center = position[groundFace];
// if(!rigid_)
// center = position[groundFace];
//-------- code ajoute par Arash pour les obstacles rectangulaires --------------
......@@ -526,7 +528,10 @@ void MovingObstacle::update()
o->p2 = getDilatedPosition((i+1) % nbVertices);
o->prevP = getDilatedPosition((i - 1 + nbVertices) % nbVertices);
o->nextP = getDilatedPosition((i + 2 + nbVertices) % nbVertices);
sim_->envMap_.popAndPushObstacleInCells(o,i);
Dart d = sim_->envMap_.popAndPushObstacleInCells(o,i);
if(i==0)
dDir = d;
if(sim_->detect_agent_collision)
{
......@@ -568,6 +573,7 @@ std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest
registering_part->move(pos);
d1=registering_part->d;
// CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest<<CGoGNendl;
return (registering_part->move(dest));
}
......@@ -621,23 +627,64 @@ void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atte
VEC3 goalVector;
if (index_parent<1)
{
goalVector = goals_[curGoal_] - center ;
// goalVector = goals_[curGoal_] - position[groundFace] ;
float goalDist2 = goalVector.norm2() ;
if (goalDist2 < 5.0f)
if(rigid_)
{
curGoal_ = (curGoal_ + 1) % goals_.size() ;
goalVector = goals_[curGoal_] - center ;
// goalVector = goals_[curGoal_] - position[groundFace] ;
goalDist2 = goalVector.norm2() ;
}
// goalVector = goals_[curGoal_] - position[groundFace] ;
if (goalDist2 > maxSpeed_)
float goalDist2 = goalVector.norm2() ;
if (goalDist2 < 5.0f)
{
curGoal_ = (curGoal_ + 1) % goals_.size() ;
goalVector = goals_[curGoal_] - center ;
// goalVector = goals_[curGoal_] - position[groundFace] ;
goalDist2 = goalVector.norm2() ;
}
if (goalDist2 > maxSpeed_)
{
goalVector.normalize() ;
goalVector *= maxSpeed_;
}
}
else
{
goalVector.normalize() ;
goalVector *= maxSpeed_;
// if(sim_->envMap_.pedWayMark.isMarked(sim_->envMap_.map.phi2(dDir)))
// dDir = sim_->envMap_.map.phi1(sim_->envMap_.map.phi1(dDir));
VEC3 mid = (sim_->envMap_.position[dDir]+sim_->envMap_.position[sim_->envMap_.map.phi1(dDir)])*0.5f;
goalVector = mid - position[0] ;
Dart dStart=dDir;
VEC3 dir = center - position[0];
while(goalVector.norm2()==0 || sim_->envMap_.pedWayMark.isMarked(sim_->envMap_.map.phi2(dDir)) || ((dir*goalVector)>0))
{
// std::cout << "d " << dDir << std::endl;
// std::cout << "dir*goalVector " << dir*goalVector << std::endl;
// std::cout << "goal vec " << goalVector << std::endl;
dDir = sim_->envMap_.map.phi_1(dDir);
mid = (sim_->envMap_.position[dDir]+sim_->envMap_.position[sim_->envMap_.map.phi1(dDir)])*0.5f;
goalVector = mid - position[0] ;
if(dDir==dStart)
break;
}
if(sim_->envMap_.map.isBoundaryEdge(sim_->envMap_.map.phi2(dDir)))
goalVector = VEC3(0);
// goalVector = mid - center ;
// std::cout << "dDir " << dDir << " goalVec" << goalVector << std::endl;
// float goalDist2 = goalVector.norm2() ;
// if (goalDist2 > maxSpeed_)
{
goalVector.normalize() ;
goalVector *= maxSpeed_*maxSpeed_;
}
}
}
else
......
src/simulator.cpp
View file @ cc85941a
#include "simulator.h"
Simulator::Simulator(unsigned int config, unsigned int minS, unsigned int nbAgent, unsigned int nbObst) :
timeStep_(0.02f),
timeStep_(0.05f),
globalTime_(0.0f),
nbSteps_(0),
nbUpdates(0),
......@@ -36,7 +36,7 @@ void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst
{
case 0 :
setupCircleScenario(nbAgent,nbObst) ;
addPathsToAgents();
// addPathsToAgents();
break ;
case 1 :
setupCorridorScenario(nbAgent,nbObst) ;
......@@ -58,7 +58,7 @@ void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst
addPathsToAgents();
// SelectorCellNotMarked<PFP::MAP> scnm(envMap_.pedWayMark);
addMovingObstacles(nbObst);
addPathToObstacles();
// addPathToObstacles();
break;
default:
std::cout << "Unknown scenario !" << std::endl ;
......@@ -175,8 +175,8 @@ void Simulator::doStep()
nbRefineCandidate += envMap_.refineCandidate.size() ;
nbCoarsenCandidate += envMap_.coarsenCandidate.size() ;
// if (multires)
// envMap_.updateMap() ;
if (multires)
envMap_.updateMap() ;
#endif
globalTime_ += timeStep_ ;
......@@ -641,6 +641,8 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
{
std::vector<VEC3> vPos;
VEC3 start;
MovingMesh * mm;
float maxHeight=10.0f;
switch(obstType)
{
......@@ -658,9 +660,8 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
break;
case 1 :
{
MovingMesh * mm = new MovingMesh(envMap_, d, "meshRessources/Limace.ply");
mm = new MovingMesh(envMap_, d, "meshRessources/Limace.ply");
movingMeshes_.push_back(mm);
float maxHeight=10.0f;
vPos = mm->computeProjectedPointSet(maxHeight);
}
break;
......@@ -669,6 +670,33 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
return;
}
bool orientAccordingToFace=true;
if(orientAccordingToFace)
{
while(envMap_.pedWayMark.isMarked(envMap_.map.phi2(d)))
{
d = envMap_.map.phi1(d);
}
VEC3 mid = (envMap_.position[d]+envMap_.position[envMap_.map.phi1(d)])*0.5f;
VEC3 front = vPos[0];
VEC3 bary(0);
for(std::vector<VEC3>::iterator it = vPos.begin() ; it != vPos.end() ; ++it)
bary += *it;
bary /= vPos.size();
float angle = get_angle(mid - bary,front - bary);
TraversorV<PFP::MAP> tv(mm->map);
for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
{
mm->position[d]+=rotate(mm->position[d], bary, angle);
}
vPos = mm->computeProjectedPointSet(maxHeight);
}
std::vector<VEC3> goals;
goals.push_back(start);
MovingObstacle * mo = new MovingObstacle(this, movingObstacles_.size(),vPos,goals, (obstType==0), false);
......@@ -723,7 +751,6 @@ void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
{
// VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
dest /= 2.0f ;
......@@ -738,29 +765,29 @@ void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
for (std::vector<Dart>::iterator it = path.begin() ; it != path.end() ; ++it)
{
// VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
VEC3 dest = envMap_.position[*it] + envMap_.position[envMap_.map.phi1(*it)] ;
dest /= 2.0f ;
mo->goals_.push_back(dest) ;
}
if(((mo->front-mo->center)*(mo->goals_[mo->curGoal_+1]-mo->center))<0)
{
std::cout << __FILE__ << " " << __LINE__ << " rotate limace" << std::endl;
Geom::Matrix44f m;
m.identity();
rotateZ(180.0f,m);
TraversorV<PFP::MAP> tv(mo->map);
for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
{
mo->position[d] -= mo->center;
mo->position[d] = Geom::transform(mo->position[d],m);
mo->position[d] += mo->center;
}
}
// if(((mo->front-mo->center)*(mo->goals_[mo->curGoal_]-mo->center))<0)
// {
// std::cout << __FILE__ << " " << __LINE__ << " rotate limace" << std::endl;
//
// Geom::Matrix44f m;
// m.identity();
// rotateZ(180.0f,m);
//
// TraversorV<PFP::MAP> tv(mo->map);
// for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
// {
// mo->position[d] -= mo->center;
// mo->position[d] = Geom::transform(mo->position[d],m);
// mo->position[d] += mo->center;
// mo->position[d][2] += 10.0f;
// }
// }
}
#ifndef SPATIAL_HASHING
......
src/viewer.cpp
View file @ cc85941a
......@@ -414,8 +414,13 @@ void SocialAgents::cb_redraw()
if (drawObstPredictionTri)
{
// for (std::vector<MovingObstacle*>::iterator it = sim.movingObstacles_.begin() ; it != sim.movingObstacles_.end() ; ++it)
// {
for (std::vector<MovingObstacle*>::iterator it = simulator.movingObstacles_.begin() ; it != simulator.movingObstacles_.end() ; ++it)
{
renderPredictionTriangle(simulator.envMap_, (*it)->dDir, (*it)->getDilatedPosition(0)) ;
std::cout << "dDir2 " << (*it)->dDir << std::endl;
// for(unsigned int i = 0 ; i < (*it)->nbVertices ; ++i)
// {
// CGoGN::Algo::MovingObjects::ParticleCell2D<PFP> p = *(*it)->parts_[i];
......@@ -428,7 +433,7 @@ void SocialAgents::cb_redraw()
// glVertex3fv(p.getPosition().data());
// glEnd();
// }
// }
}
}
#endif
......
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