Commit f9652c68 authored by Jund Thomas's avatar Jund Thomas

extraction enveloppe convexe imports de ply

parent 2538a137
......@@ -16,6 +16,8 @@ include_directories(
${CMAKE_CURRENT_BINARY_DIR}
)
#SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
QT4_WRAP_UI( socialAgents_ui ../include/socialAgents.ui )
QT4_WRAP_CPP( socialAgents_moc ../include/viewer.h )
add_executable( socialAgents
......
......@@ -44,11 +44,9 @@ inline void renderPredictionTriangle(EnvMap& m, Dart d, PFP::VEC3 p)
}
inline void renderObstacle(EnvMap& m, MovingObstacle * obst, bool showBelonging=false, bool renderPath = false)
inline void renderObstacle(EnvMap& m, MovingObstacle * obst, bool showBelonging=false, bool renderPath = false, bool showVertices=false)
{
glBegin(GL_POLYGON) ;
for (unsigned int i = 0 ; i < (obst->nbVertices) ; ++i)
{
......@@ -57,6 +55,20 @@ inline void renderObstacle(EnvMap& m, MovingObstacle * obst, bool showBelonging=
}
glEnd() ;
if(showVertices)
{
glPointSize(3.0f) ;
glColor3f(0.0,0.0,1.0);
glBegin(GL_POINTS) ;
for (unsigned int i = 0 ; i < (obst->nbVertices) ; ++i)
{
const VEC3& p = obst->getPosition(i) ;
glVertex3fv(p.data()) ;
}
glEnd() ;
glPointSize(1.0f) ;
}
glColor3f(1.0,0.0,1.0);
glBegin(GL_LINE_LOOP) ;
for (unsigned int i = 0 ; i < (obst->nbVertices) ; ++i)
......
......@@ -15,19 +15,24 @@ using namespace std;
class MovingMesh
{
public:
MovingMesh(EnvMap * envMap, Dart d, std::string filename);
MovingMesh(EnvMap& envMap, Dart d, std::string filename);
void linkWithObstacle(MovingObstacle * mo);
void translate(VEC3 v);
void scale(float val);
void moveInFace(PFP::MAP& motherMap, Dart d, VertexAttribute<VEC3> pos);
void move();
void animate();
void draw();
std::vector<VEC3> computeProjectedPointSet();
std::vector<VEC3> jarvisConvexHull(std::vector<VEC3> projectedPointSet);
std::vector<VEC3> computeSkeleton(std::vector<VEC3> projectedPointSet, unsigned int nodeNumber);
PFP::MAP& motherMap;
VertexAttribute<VEC3>& motherPosition;
PFP::MAP map;
VertexAttribute<VEC3> position;
VertexAttribute<VEC3> normal;
......
......@@ -80,11 +80,11 @@ public:
class Simulator
{
public:
Simulator(unsigned int config, unsigned int minSize) ;
Simulator(unsigned int config, unsigned int minSize, unsigned int nbAgent=1000, unsigned int nbObst=20) ;
~Simulator() ;
void init(float dimension, bool enablePathFinding = false) ;
void init(float dimension, unsigned int nbAgent, unsigned int nbObst, bool enablePathFinding = false) ;
void doStep() ;
......@@ -120,6 +120,7 @@ public:
EnvMap envMap_ ;
std::vector<Agent*> agents_ ;
std::vector<MovingObstacle*> movingObstacles_;
std::vector<MovingMesh*> movingMeshes_; //for visualization only
int minSize;
unsigned int config;
float timeStep_ ;
......
......@@ -51,7 +51,7 @@ class SocialAgents : public Utils::QT::SimpleQT
Q_OBJECT
public:
SocialAgents(unsigned int config, unsigned int minSize, unsigned int iterations) ;
SocialAgents(unsigned int config, unsigned int minSize, unsigned int nbAgent , unsigned int nbObst , unsigned int iterations) ;
void initGUI() ;
......
......@@ -204,13 +204,14 @@ void Agent::updateObstacleNeighbors()
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((obstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistObst)
&&distSq < rangeSq_)
&& distSq < rangeSq_)
{
if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
{
if (distSq > maxDistObst) maxDistObst = distSq ;
obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
if (distSq > maxDistObst)
maxDistObst = distSq ;
obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
......@@ -220,12 +221,13 @@ void Agent::updateObstacleNeighbors()
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((movingObstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistMovingObst)
&&distSq < rangeSq_)
&& distSq < rangeSq_)
{
// if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
{
if (distSq > maxDistMovingObst) maxDistMovingObst = distSq ;
if (distSq > maxDistMovingObst)
maxDistMovingObst = distSq ;
movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
......@@ -233,6 +235,7 @@ void Agent::updateObstacleNeighbors()
}
}
}
for(std::vector<Obstacle*>::const_iterator it = neighborObst.begin() ; it != neighborObst.end() ; ++it)
{
if ((*it)->mo==NULL)
......@@ -268,16 +271,18 @@ void Agent::updateObstacleNeighbors()
}
}
}
if (obstacleNeighbors_.size() > maxNeighbors_)
{
sim_->nbSorts++ ;
std::sort(obstacleNeighbors_.begin(), obstacleNeighbors_.end(), obstacleSort) ;
}
if (movingObstacleNeighbors_.size() > maxNeighbors_)
{
sim_->nbSorts++ ;
std::sort(movingObstacleNeighbors_.begin(), movingObstacleNeighbors_.end(), obstacleSort) ;
}
// if (obstacleNeighbors_.size() > maxNeighbors_)
// {
// sim_->nbSorts++ ;
// std::sort(obstacleNeighbors_.begin(), obstacleNeighbors_.end(), obstacleSort) ;
// }
//
// if (movingObstacleNeighbors_.size() > maxNeighbors_)
// {
// sim_->nbSorts++ ;
// std::sort(movingObstacleNeighbors_.begin(), movingObstacleNeighbors_.end(), obstacleSort) ;
// }
......
#include "moving_mesh.h"
#include "Algo/Render/GL1/map_glRender.h"
MovingMesh::MovingMesh(EnvMap * envMap, Dart d, std::string filename):
MovingMesh::MovingMesh(EnvMap& envMap, Dart d, std::string filename):
motherMap(envMap.map),
motherPosition(envMap.position),
constrainedV(map)
{
std::vector<std::string> attrNames ;
......@@ -12,13 +15,19 @@ constrainedV(map)
position = map.getAttribute<VEC3, VERTEX>(attrNames[0]) ;
normal = map.addAttribute<VEC3, VERTEX>("normal");
scale(2.5f);
scale(4.0f);
// 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 d = tv.begin() ; d != tv.end() ; d = tv.next())
for(Dart dd = tv.begin() ; dd != tv.end() ; dd = tv.next())
{
obstDarts.push_back(d);
obstDarts.push_back(dd);
}
smg = new ShapeMatchingQuadratic<PFP>(map,position,obstDarts,0.5f);
......@@ -30,6 +39,15 @@ void MovingMesh::linkWithObstacle(MovingObstacle * mo)
this->mo = mo;
}
void MovingMesh::translate(VEC3 v)
{
TraversorV<PFP::MAP> tv(map);
for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
{
position[d] += v;
}
}
void MovingMesh::scale(float val)
{
TraversorV<PFP::MAP> tv(map);
......@@ -99,10 +117,33 @@ void MovingMesh::animate()
}
}
void MovingMesh::draw()
{
glColor3f(0,1,0);
Algo::Render::GL1::renderTriQuadPoly<PFP>(map, Algo::Render::GL1::LINE,
1.0, position,
normal) ;
}
std::vector<VEC3> MovingMesh::computeProjectedPointSet()
{
std::vector<VEC3> res;
TraversorV<PFP::MAP> tv(map);
for(Dart dd = tv.begin() ; dd != tv.end() ; dd = tv.next())
{
res.push_back(position[dd]);
}
res = jarvisConvexHull(res);
Geom::Plane3D<float> pl = Algo::Geometry::facePlane<PFP>(motherMap, motherMap.begin(), motherPosition);
for(unsigned int i = 0; i < res.size() ; ++i)
{
VEC3& v = res[i];
pl.project(v);
}
return res;
}
......@@ -128,70 +169,125 @@ std::vector<VEC3> MovingMesh::jarvisConvexHull(std::vector<VEC3> pointSet)
assert(pointSet.size()>2);
std::vector<unsigned int> convHull;
int maxAngle=0, minAngle=0;
unsigned int minPoint=0, maxPoint=0;
unsigned int cP; //currPoint
for(unsigned int i=1; i<pointSet.size(); ++i)
if(pointSet[i][1]>pointSet[maxPoint][1])
maxPoint=i;
unsigned int minPoint=0;
unsigned int curr;
for(unsigned int i=1; i<pointSet.size(); ++i) //FOR ALL POINTS IN THE SET, FIND MIN POINT
if(pointSet[i][1]<pointSet[maxPoint][1])
if(pointSet[i][1]<pointSet[minPoint][1])
minPoint=i;
convHull.push_back(minPoint);
cP =minPoint;
while(cP!=maxPoint) //build left hand side
{
maxAngle=cP;
for(unsigned int i=0; i<pointSet.size(); ++i) //LOOP FOR ALL POINTS IN THE SET, FIND POINT WITH LOWEST RELATIVE ANGLE
{
if ((i==maxPoint || std::find(convHull.begin(), convHull.end(), i)==convHull.end())
&& jarvisFindAngle(pointSet[cP],pointSet[maxAngle]) < jarvisFindAngle(pointSet[cP],pointSet[i])
&& jarvisFindAngle(pointSet[cP],pointSet[i])<=270)
{
maxAngle=i;
}
}
cP=maxAngle;
convHull.push_back(cP); //add point to final perimeter list
}
cP=minPoint;
while(cP!=maxPoint) //build right hand side
{
minAngle=maxPoint;
for(unsigned int i=0; i<pointSet.size(); ++i) //LOOP FOR ALL POINTS IN THE SET, FIND POINT WITH GREATEST RELATIVE ANGLE
{
if ((i==maxPoint || std::find(convHull.begin(), convHull.end(), i)==convHull.end())
&& jarvisFindAngle(pointSet[cP],pointSet[minAngle])> jarvisFindAngle(pointSet[cP],pointSet[i])
&& jarvisFindAngle(pointSet[cP],pointSet[i])>=90)
{
minAngle=i;
}
}
cP=minAngle;
convHull.push_back(cP); //add point to final perimeter list
}
curr = minPoint;
do {
unsigned int i=0;
bool trouve=false;
for( ;!trouve && i<pointSet.size(); ++i)
{
if((pointSet[i][0] != pointSet[curr][0]) || (pointSet[i][1] != pointSet[curr][1]))
{
bool left=true;
for(unsigned int j=0;left && j<pointSet.size(); ++j)
{
if(Geom::testOrientation2D(pointSet[j],pointSet[curr],pointSet[i])==Geom::RIGHT)
{
left=false;
}
}
if(left)
trouve = true;
}
}
if(trouve)
{
curr = i-1;
convHull.push_back(curr);
}
} while(curr != minPoint);
std::vector<VEC3> res;
res.reserve(convHull.size());
for(std::vector<unsigned int>::iterator it = convHull.begin() ; it != convHull.end() ; ++it)
{
res.push_back(pointSet[*it]);
}
return res;
}
//std::vector<VEC3> MovingMesh::jarvisConvexHull(std::vector<VEC3> pointSet)
//{
// assert(pointSet.size()>2);
//
// std::vector<unsigned int> convHull;
// int maxAngle=0, minAngle=0;
// unsigned int minPoint=0, maxPoint=0;
// unsigned int cP; //currPoint
//
// for(unsigned int i=1; i<pointSet.size(); ++i)
// if(pointSet[i][1]>pointSet[maxPoint][1])
// maxPoint=i;
//
// for(unsigned int i=1; i<pointSet.size(); ++i) //FOR ALL POINTS IN THE SET, FIND MIN POINT
// if(pointSet[i][1]<pointSet[minPoint][1])
// minPoint=i;
//
// convHull.push_back(minPoint);
// cP =minPoint;
//
// while(cP!=maxPoint) //build left hand side
// {
// maxAngle=cP;
//
// for(unsigned int i=0; i<pointSet.size(); ++i) //LOOP FOR ALL POINTS IN THE SET, FIND POINT WITH LOWEST RELATIVE ANGLE
// {
// if ((i==maxPoint || std::find(convHull.begin(), convHull.end(), i)==convHull.end())
// && jarvisFindAngle(pointSet[cP],pointSet[maxAngle]) < jarvisFindAngle(pointSet[cP],pointSet[i])
// && jarvisFindAngle(pointSet[cP],pointSet[i])<=270)
// {
// maxAngle=i;
// }
// }
//
// cP=maxAngle;
// convHull.push_back(cP); //add point to final perimeter list
// }
//
// cP=minPoint;
//
// while(cP!=maxPoint) //build right hand side
// {
// minAngle=maxPoint;
//
// for(unsigned int i=0; i<pointSet.size(); ++i) //LOOP FOR ALL POINTS IN THE SET, FIND POINT WITH GREATEST RELATIVE ANGLE
// {
// if ((i==maxPoint || std::find(convHull.begin(), convHull.end(), i)==convHull.end())
// && jarvisFindAngle(pointSet[cP],pointSet[minAngle])> jarvisFindAngle(pointSet[cP],pointSet[i])
// && jarvisFindAngle(pointSet[cP],pointSet[i])>=90)
// {
// minAngle=i;
// }
// }
//
// cP=minAngle;
// convHull.push_back(cP); //add point to final perimeter list
// std::cout << "push right hand side" << std::endl;
// }
//
// std::vector<VEC3> res;
// res.reserve(convHull.size());
//
// for(std::vector<unsigned int>::iterator it = convHull.begin() ; it != convHull.end() ; ++it)
// {
// res.push_back(pointSet[*it]);
// }
//
// return res;
//}
bool vecSort(const VEC3& a1, const VEC3& a2)
{
return a1[0] < a2[0] || (a1[0]==a2[0] && a1[1]<a2[1]);
......
#include "simulator.h"
Simulator::Simulator(unsigned int config, unsigned int minS) :
Simulator::Simulator(unsigned int config, unsigned int minS, unsigned int nbAgent, unsigned int nbObst) :
timeStep_(0.2f),
globalTime_(0.0f),
nbSteps_(0),
......@@ -20,7 +20,7 @@ Simulator::Simulator(unsigned int config, unsigned int minS) :
nbStepsPerUnit_ = 1 / timeStep_ ;
this->config=config;
init(2.0f) ;
init(2.0f, nbAgent, nbObst) ;
}
......@@ -30,20 +30,20 @@ Simulator::~Simulator()
delete agents_[i] ;
}
void Simulator::init( float dimension, bool enablePathFinding)
void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst, bool enablePathFinding)
{
std::cout << "Setup scenario" << std::endl ;
switch (config)
{
case 0 :
setupCircleScenario(1000,20) ;
setupCircleScenario(nbAgent,nbObst) ;
break ;
case 1 :
setupCorridorScenario(1000,40) ;
setupCorridorScenario(nbAgent,nbObst) ;
break ;
case 2 :
setupSnakeCorridorScenario(1000,5,10) ;
setupSnakeCorridorScenario(nbAgent,nbObst,10) ;
break ;
// case 3 :
// setupCityScenario(20, 20) ;
......@@ -55,10 +55,10 @@ void Simulator::init( float dimension, bool enablePathFinding)
// break ;
case 5:
envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ; //svg import
setupScenario(30) ;
setupScenario(nbAgent) ;
addPathsToAgents();
// addMovingObstacles(5);
// addPathToObstacles();
addMovingObstacles(nbObst);
addPathToObstacles();
break;
default:
std::cout << "Unknown scenario !" << std::endl ;
......@@ -635,7 +635,7 @@ void Simulator::addMovingObstacles(unsigned int nb)
}
if (found)
addMovingObstacle(dCell) ;
addMovingObstacle(dCell,1) ;
}
}
......@@ -658,6 +658,13 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
vPos.push_back(start+xSide+ySide);
}
break;
case 1 :
{
MovingMesh * mm = new MovingMesh(envMap_, d, "meshRessources/Limace.ply");
movingMeshes_.push_back(mm);
vPos = mm->computeProjectedPointSet();
}
break;
default:
std::cout << "Unknown obstacle type" << std::endl;
return;
......
......@@ -26,12 +26,12 @@
#include "env_generator.h"
#include <string>
SocialAgents::SocialAgents(unsigned int config, unsigned int minSize, unsigned int iterations) :
SocialAgents::SocialAgents(unsigned int config, unsigned int minSize, unsigned int nbAgent, unsigned int nbObst, unsigned int iterations) :
nbIterations(0),
maxIterations(iterations),
frames(0),
nextUpdate(0),
simulator(config, minSize),
simulator(config, minSize, nbAgent, nbObst),
render_anim(false),
drawEnvLines(true),
drawEnvFaces(false),
......@@ -94,7 +94,7 @@ void SocialAgents::cb_initGL()
float tailleX = bb.size(0) ;
float tailleY = bb.size(1) ;
float tailleZ = bb.size(2) ;
float gWidthObj = std::max<float>(std::max<float>(tailleX, tailleY), tailleZ) ;
float gWidthObj = std::max<float>(std::max<float>(tailleX, tailleY), tailleZ)/10.0f ;
setParamObject(gWidthObj, gPosObj.data()) ;
}
......@@ -278,6 +278,11 @@ void SocialAgents::cb_redraw()
if (drawMovingObstacles)
{
for (std::vector<MovingMesh*>::iterator it = simulator.movingMeshes_.begin() ; it != simulator.movingMeshes_.end() ; ++it)
{
(*it)->draw();
}
glColor3f(1.0f, 1.0f, 1.0f) ;
for (std::vector<MovingObstacle*>::iterator it = simulator.movingObstacles_.begin() ; it != simulator.movingObstacles_.end() ; ++it)
{
......@@ -293,30 +298,14 @@ void SocialAgents::cb_redraw()
// Dessiner l'ellipse
drawEllipse((*it)->focus1,(*it)->focus2,(*it)->sum_dist_foci,20);
}
// MovingObstacle* mo = *it ;
//affiche les sommets des obstacles
// for (unsigned int i =0; i< (*it)->nbVertices;i++)
// {
// VEC3 pos = (*it)->obstacles_[i]->p1;
// glColor3f(1.0f, 0.8f, 0.0f);
// glBegin(GL_POLYGON);
// for(unsigned int i = 0; i < 5; ++i)
// glVertex3f(pos[0] + (cosT[i] * 1.5f), pos[1] + (sinT[i] * 1.5f), pos[2]+0.01f);
// glEnd();
// pos = (*it)->obstacles_[i]->p2;
// glColor3f(1.0f, 0.0f, 0.8f);
// glBegin(GL_POLYGON);
// for(unsigned int i = 0; i < 5; ++i)
// glVertex3f(pos[0] + (cosT[i] * 1.5f), pos[1] + (sinT[i] * 1.5f), pos[2]+0.01f);
// glEnd();
// }
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL) ;
glColor3f((*it)->color1, (*it)->color2, (*it)->color3) ;
glLineWidth(1.0f) ;
glBegin(GL_POLYGON) ;
renderObstacle(simulator.envMap_, *it,false,drawObstPath);
renderObstacle(simulator.envMap_, *it,false,drawObstPath, true);
//////Affiche l'objectif actuel
// glColor3f(1.0f, 0.0f, 0.0f) ;
......@@ -1379,20 +1368,30 @@ int main(int argc, char** argv)
{
QApplication app(argc, argv) ;
std::cout << "./socialAgents scenario minSize(=41.5) nbAgent(=1000) nbObst(=20) nbIterations" << std::endl;
unsigned int minSize = Agent::range_ ;
unsigned int iterations = 0 ;
unsigned int config = 1 ;
if (argc > 3) iterations = atoi(argv[3]) ;
unsigned int nbAgent = 1000;
unsigned int nbObst = 20;
if (argc > 5)
iterations = atoi(argv[5]) ;
if (argc > 4)
nbObst = atoi(argv[4]) ;
if (argc > 3)
nbAgent = atoi(argv[3]) ;
if (argc > 2)
{
minSize = atoi(argv[2]) ;
if (iterations == 0) iterations = 4000 ;
// if (iterations == 0)
// iterations = 4000 ;
}
if (argc > 1) config = atoi(argv[1]) ;
SocialAgents sa(config, minSize, iterations) ;
SocialAgents sa(config, minSize, nbAgent, nbObst, iterations) ;
sa.setGeometry(0, 0, 1800, 1200) ;
sa.initGUI() ;
sa.show() ;
......
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