Commit 6ee29e1a authored by Thomas Jund's avatar Thomas Jund
Browse files

qq modif pr limaces dans la ville et affichage pr arash

parent 762c7fdb
......@@ -405,7 +405,7 @@ Dart generateBuilding(EnvMap& envMap, Dart d, float height, unsigned int buildin
Dart x =map.phi2(dd);
if(!map.isBoundaryMarked(x))
{
addElementToVector<Obstacle*>(envMap.obstvect[d],o);
addElementToVector<Obstacle*>(envMap.obstvect[x],o);
if(!map.isBoundaryMarked(map.phi2(map.phi1(x))))
addElementToVector<Obstacle*>(envMap.obstvect[map.phi2(map.phi1(x))],o);
......
......@@ -113,7 +113,9 @@ inline void renderObstacle(EnvMap& m, MovingObstacle * obst, bool showBelonging=
for (std::vector<VEC3>::iterator it = ++(obst->goals_.begin()) ; it != obst->goals_.end() ;
++it)
{
glVertex3f((*it)[0], (*it)[1], (*it)[2]) ;
// glVertex3f((*it)[0], (*it)[1], (*it)[2]) ;
glVertex3f((*it)[0], (*it)[1], obst->index) ;
}
glEnd() ;
glLineWidth(1.0f) ;
......@@ -178,13 +180,22 @@ inline void renderAgent(EnvMap& m, Agent* agent, bool showNeighborDist = false,
{
glLineWidth(3.0f) ;
glBegin(GL_LINE_STRIP) ;
for (std::vector<VEC3>::iterator it = ++(agent->goals_.begin()) ; it != agent->goals_.end() ;
for (std::vector<VEC3>::iterator it = (agent->goals_.begin()) ; it != agent->goals_.end() ;
++it)
{
glVertex3f((*it)[0], (*it)[1], (*it)[2]) ;
}
glVertex3f((*(agent->goals_.begin()))[0], (*(agent->goals_.begin()))[1], (*(agent->goals_.begin()))[2]) ;
glEnd() ;
glLineWidth(1.0f) ;
glPointSize(5.0f) ;
glBegin(GL_POINTS) ;
VEC3 g = agent->goals_[agent->curGoal_];
glVertex3fv(g.data()) ;
glEnd() ;
glPointSize(1.0f) ;
}
if (showNeighborDist)
......
......@@ -29,7 +29,7 @@ public:
void draw();
std::vector<VEC3> computeProjectedPointSet(float maxHeight);
std::vector<VEC3> jarvisConvexHull(std::vector<VEC3> projectedPointSet);
std::vector<VEC3> jarvisConvexHull(const std::vector<VEC3>& projectedPointSet);
std::vector<VEC3> computeSkeleton(std::vector<VEC3> projectedPointSet, unsigned int nodeNumber);
PFP::MAP& motherMap;
......@@ -38,6 +38,8 @@ public:
VertexAttribute<VEC3> position;
VertexAttribute<VEC3> normal;
float scaleValue;
ShapeMatchingQuadratic<PFP> * smg;
std::vector<VEC3> skeleton;
......
......@@ -16,7 +16,7 @@ class Simulator ;
class MovingObstacle
{
public:
MovingObstacle(Simulator* sim, int index, std::vector<PFP::VEC3> pos, std::vector<VEC3> goals, bool rigid, bool spin, ArticulatedObstacle * art=NULL, int indParent=-1);
MovingObstacle(Simulator* sim, int index, std::vector<PFP::VEC3> pos, std::vector<VEC3> goals, bool rigid, bool spin, Dart d=NIL, ArticulatedObstacle * art=NULL, int indParent=-1);
bool test_opposition(VEC3 o, VEC3 p1, VEC3 p2);
// void contournerBatiment();
void updateAgentNeighbors() ;
......@@ -55,10 +55,8 @@ public:
// float rigidity;
float gravity_dist;
// float gravity_dist;
VEC3 center;
int nb_agents_voisins;
int nb_register_cells;
int index;
int max_x_ind;
int min_x_ind;
......@@ -84,7 +82,7 @@ public:
static float neighborDist_;
static float neighborDistSq_;
static float maxSpeed_;
float obstacle_range;
// float obstacle_range;
static float timeHorizonObst_;
float velocity_factor;
float color1;
......
......@@ -83,8 +83,6 @@ std::vector<Dart> pathFindAStar(EnvMap& envMap, const typename PFP::TVEC3& posit
// inCurrentFace = map.phi1(inCurrentFace) ;
// } while (inCurrentFace != currentFace) ;
} while (facesToTry.size() > 0 && !map.sameFace(start, currentFace)) ;
// If a path has been found, push all predecessors from start to stop
......@@ -96,6 +94,13 @@ std::vector<Dart> pathFindAStar(EnvMap& envMap, const typename PFP::TVEC3& posit
path.push_back(toPush) ;
toPush = faceNextFaceToGoal[toPush] ;
} while (toPush != goal) ;
// Dart dS = path[0];
// do
// {
// dS = map.phi1(dS);
// } while(dS!=path[0] || !map.sameFace(map.phi2(dS),path[1]));
// path[0] = dS;
}
else
{
......
......@@ -10,7 +10,7 @@ ArticulatedObstacle::ArticulatedObstacle(Simulator* sim, int index, int currentI
{
std::vector<VEC3> goal;
goal.push_back(members[i-1]->center);
MovingObstacle * mo4= new MovingObstacle(sim,currentIndex+1+i ,pos[i],goal, true, true, this,i);
MovingObstacle * mo4= new MovingObstacle(sim,currentIndex+1+i ,pos[i],goal, true, true, NIL, this,i);
members.push_back(mo4);
}
......
......@@ -15,23 +15,26 @@ constrainedV(map)
position = map.getAttribute<VEC3, VERTEX>(attrNames[0]) ;
normal = map.addAttribute<VEC3, VERTEX>("normal");
scale(4.0f);
float area = Algo::Geometry::convexFaceArea<PFP>(envMap.map, d, envMap.position);
scaleValue = std::min(area/1400.0f,100.0f);
std::cout << "scaleVal " << scaleValue << std::endl;
// scaleValue = 4.0f;
scale(scaleValue);
// scale(2.5f);
// scale(0.5f);
VEC3 v = Algo::Geometry::faceCentroid<PFP>(motherMap, d, motherPosition) ;
translate(v);
TraversorV<PFP::MAP> tv(map);
std::vector<Dart> obstDarts;
for(Dart dd = tv.begin() ; dd != tv.end() ; dd = tv.next())
{
obstDarts.push_back(dd);
}
smg = new ShapeMatchingQuadratic<PFP>(map,position,obstDarts,0.5f);
smg->initialize();
// TraversorV<PFP::MAP> tv(map);
// std::vector<Dart> obstDarts;
// for(Dart dd = tv.begin() ; dd != tv.end() ; dd = tv.next())
// {
// obstDarts.push_back(dd);
// }
//
// smg = new ShapeMatchingQuadratic<PFP>(map,position,obstDarts,0.5f);
// smg->initialize();
}
void MovingMesh::linkWithObstacle(MovingObstacle * mo)
......@@ -128,10 +131,10 @@ void MovingMesh::animate()
void MovingMesh::draw()
{
glColor3f(0,1,0);
Algo::Render::GL1::renderTriQuadPoly<PFP>(map, Algo::Render::GL1::LINE,
1.0, position,
normal) ;
// glColor3f(0,1,0);
// Algo::Render::GL1::renderTriQuadPoly<PFP>(map, Algo::Render::GL1::LINE,
// 1.0, position,
// normal) ;
}
std::vector<VEC3> MovingMesh::computeProjectedPointSet(float maxHeight)
......@@ -158,8 +161,9 @@ std::vector<VEC3> MovingMesh::computeProjectedPointSet(float maxHeight)
return res;
}
std::vector<VEC3> MovingMesh::jarvisConvexHull(std::vector<VEC3> pointSet)
std::vector<VEC3> MovingMesh::jarvisConvexHull(const std::vector<VEC3>& pointSet)
{
std::cout << "pointSet " << pointSet.size() << std::endl;
assert(pointSet.size()>2);
std::vector<unsigned int> convHull;
......@@ -200,6 +204,11 @@ std::vector<VEC3> MovingMesh::jarvisConvexHull(std::vector<VEC3> pointSet)
curr = i-1;
convHull.push_back(curr);
}
else
{
std::cout << "Pb : !trouve" << std::endl;
exit(0);
}
} while(curr != minPoint);
......
......@@ -71,10 +71,19 @@ VEC3 rotate(VEC3 pos1, VEC3 center, float angle) // renvoie le déplacement nece
return pos2;
}
MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool rigid, bool spin, ArticulatedObstacle * art, int indParent) :
MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool rigid, bool spin, Dart dInside, ArticulatedObstacle * art, int indParent) :
nbVertices(pos.size()),
center(0),
index(ind),
goals_(goals),
curGoal_(1),
velocity_factor(0.8f),
color1(1.0f),
color2(1.0f),
color3(1.0f),
seen(false),
newVelocity_(0),
sim_(sim),
rigid_(rigid),
......@@ -82,42 +91,38 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
parent(art),
index_parent(indParent)
{
seen = false;
color1=1.0f;
color2=1.0f;
color3=1.0f;
assert(pos.size() > 2);
// movingObstNeighbors_.reserve(maxNeighbors_);
curGoal_ = 1;
nb_agents_voisins = 0;
nb_register_cells = 0;
float pos_max = 0;
if(dInside==NIL)
dInside = sim_->envMap_.getBelongingCell(pos[0]);
velocity_factor = 0.8f;
nbVertices = pos.size();
Dart d = sim_->envMap_.getBelongingCell(pos[0]);
registering_part = new CGoGN::Algo::MovingObjects::ParticleCell2DMemo<PFP>(sim_->envMap_.map, d, pos[0], sim_->envMap_.position);
registering_part = new CGoGN::Algo::MovingObjects::ParticleCell2DMemo<PFP>(sim_->envMap_.map, dInside, pos[0], sim_->envMap_.position);
obstacles_ = new Obstacle*[nbVertices];
belonging_cells = new std::vector<Dart>[nbVertices];
neighbor_cells = new std::vector<Dart>[nbVertices];
// for (unsigned int i = 0; i < nbVertices; ++i)
position = map.addAttribute<VEC3, VERTEX>("position") ;
normal = map.addAttribute<VEC3, VERTEX>("normal") ;
deformation = map.addAttribute<VEC3, VERTEX>("deformation") ;
velocity = map.addAttribute<VEC3, VERTEX>("velocity") ;
edgeLength = map.addAttribute<float, EDGE>("edgeLength") ;
if(!rigid_)
{
velocity = map.addAttribute<VEC3, VERTEX>("velocity") ;
edgeLength = map.addAttribute<float, EDGE>("edgeLength") ;
}
groundFace = map.newFace(nbVertices);
d = groundFace;
Dart d = groundFace;
for (unsigned int i = 0; i < nbVertices; ++i)
{
position[d] = pos[i];
deformation[d] = VEC3(0);
velocity[d] = VEC3(0);
if(!rigid_)
velocity[d] = VEC3(0);
center += pos[i];
d = map.phi1(d);
......@@ -164,6 +169,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
}
angle=0;
}
for (unsigned int i = 0; i < nbVertices; ++i)
{
......@@ -176,21 +182,23 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
sim_->envMap_.pushObstacleInCells(o);
}
for (unsigned int i = 0; i < nbVertices; ++i)
{
float it = (center - pos[i]).norm();
if (it > pos_max)
pos_max = it;
}
gravity_dist = pos_max + 5.0f;
obstacle_range = 15.0f * 15.0f;
// float pos_max = 0;
//
// for (unsigned int i = 0; i < nbVertices; ++i)
// {
// float it = (center - pos[i]).norm();
// if (it > pos_max)
// pos_max = it;
// }
// gravity_dist = pos_max + 5.0f;
// obstacle_range = 15.0f * 15.0f;
}
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]+deformation[d];
return position[d];
}
VEC3 MovingObstacle::getPosition(unsigned int ind)
......
......@@ -38,6 +38,7 @@ void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst
{
case 0 :
setupCircleScenario(nbAgent,nbObst) ;
addPathsToAgents();
break ;
case 1 :
setupCorridorScenario(nbAgent,nbObst) ;
......@@ -424,15 +425,9 @@ void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObst
void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize)
{
if (multires)
{
envMap_.init(config, 1600.0f, 960.0f, minSize, 160.0f) ; //grosses cases
}
else
{
envMap_.init(config, 1600.0f, 960.0f, minSize, 160.0f) ; //cases fines
}
std::cout << " - Setup Snake Corridor Scenario : " << nbAgents << " agents et " << nbSnakes*snakeSize << " obstacles" << std::endl ;
......@@ -513,9 +508,10 @@ void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int n
{
x = xStartMin + rand() % xStartDelta;
goal=VEC3 (x, yGoalMin + rand() % yGoalDelta, 0) ;
if ((goal-goals[r]).norm2()>1000){
if ((goal-goals[r]).norm2()>1000)
{
goals.push_back(goal);
r++;
r++;
}
}
......@@ -523,14 +519,12 @@ void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int n
positions[0]=vPos;
for (int i = 1 ; i < snakeSize ; i++)
{
start=start-ySide-ySide;
vPos.clear();
vPos.push_back(start+xSide-ySide);
vPos.push_back(start+xSide+ySide);
vPos.push_back(start-xSide+ySide);
......@@ -540,10 +534,9 @@ void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int n
// CGoGNout<<"positions : "<< positions[0][0]<<CGoGNendl;
ArticulatedObstacle * art=new ArticulatedObstacle (this,j,sumObstacles,positions,snakeSize,goals);
sumObstacles += snakeSize;
for (int i = 0 ; i < snakeSize ; i++)
{
movingObstacles_.push_back(art->members[i]);
}
}
}
......@@ -631,8 +624,12 @@ void Simulator::addMovingObstacles(unsigned int nb)
&& !envMap_.pedWayMark.isMarked(d)
)
{
dCell = d ;
found = true ;
float area = Algo::Geometry::convexFaceArea<PFP>(envMap_.map, d, envMap_.position);
if(area>1400)
{
dCell = d ;
found = true ;
}
}
d = tF.next() ;
}
......@@ -665,7 +662,7 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
{
MovingMesh * mm = new MovingMesh(envMap_, d, "meshRessources/Limace.ply");
movingMeshes_.push_back(mm);
float maxHeight=1.0f;
float maxHeight=10.0f;
vPos = mm->computeProjectedPointSet(maxHeight);
}
break;
......@@ -693,12 +690,21 @@ void Simulator::addPathToObstacles()
for (std::vector<MovingObstacle*>::iterator it = movingObstacles_.begin() ; it != movingObstacles_.end() ; ++it)
{
Dart dStart = (*it)->registering_part->d;
while(envMap_.pedWayMark.isMarked(dStart))
{
if(!envMap_.pedWayMark.isMarked(envMap_.map.phi2(dStart)))
dStart = envMap_.map.phi2(dStart);
else
dStart = envMap_.map.phi1(dStart);
}
Dart dStop = dStart;
for (unsigned int j = 0 ; envMap_.buildingMark.isMarked(dStop)
|| j < dartDistForPath + rand() * 20
|| envMap_.map.sameFace(dStop, dStart)
|| !envMap_.pedWayMark.isMarked(dStop)
|| envMap_.pedWayMark.isMarked(dStop)
; ++j)
{
envMap_.map.next(dStop) ;
......@@ -715,11 +721,13 @@ void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_,
envMap_.position, dStart,
dGoal,
envMap_.buildingMark) ;
envMap_.pedWayMark) ;
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 = 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) ;
}
......@@ -728,14 +736,33 @@ void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
envMap_.position,
dGoal,
dStart,
envMap_.buildingMark) ;
envMap_.pedWayMark) ;
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 = 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;
}
}
}
#ifndef SPATIAL_HASHING
......
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