Commit ac9025fd authored by Thomas's avatar Thomas
Browse files

modifcation path_finder pour CellMarker, modif export povray, projection agents ds leurs faces

parent 82820f7b
......@@ -4,6 +4,13 @@ project(SocialAgents)
SET(CMAKE_BUILD_TYPE Debug)
# FOR Qt4
FIND_PACKAGE(Qt4 REQUIRED)
SET(QT_USE_QTOPENGL TRUE)
INCLUDE(${QT_USE_FILE})
ADD_DEFINITIONS(${QT_DEFINITIONS})
#SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pg")
link_directories(
......@@ -36,4 +43,4 @@ add_executable( socialAgentsD
)
target_link_libraries( socialAgentsD
containerD topologyD utilsD algoD ${COMMON_LIBS} boost_thread-mt)
containerD topologyD utilsD algoD ${CGoGN_LIBS_R} ${COMMON_LIBS} ${QT_LIBRARIES} boost_thread-mt)
......@@ -4,6 +4,12 @@ project(SocialAgents)
SET(CMAKE_BUILD_TYPE Release)
# FOR Qt4
FIND_PACKAGE(Qt4 REQUIRED)
SET(QT_USE_QTOPENGL TRUE)
INCLUDE(${QT_USE_FILE})
ADD_DEFINITIONS(${QT_DEFINITIONS})
link_directories(
${CGoGN_ROOT_DIR}/lib/Release/
)
......@@ -33,4 +39,4 @@ add_executable( socialAgents
)
target_link_libraries( socialAgents
container topology utils algo ${COMMON_LIBS} boost_thread-mt)
container topology utils algo ${CGoGN_LIBS_R} ${COMMON_LIBS} ${QT_LIBRARIES} boost_thread-mt)
......@@ -25,6 +25,11 @@ template <typename PFP>
void generateCity(typename PFP::MAP& map, typename PFP::TVEC3& position, CellMarker& obstacleMark, CellMarker& buildingMark, float sideSize, unsigned int nbSquares);
template <typename PFP>
void generateMall(typename PFP::MAP& map,typename PFP::TVEC3& position, CellMarker& obstacleMark, CellMarker& buildingMark, float sideSize);
template <typename PFP>
void generatePathToUpperStair(typename PFP::MAP& map,typename PFP::TVEC3& position, Dart dLower,Dart dUpper);
template <typename PFP>
......@@ -39,6 +44,9 @@ bool canRemoveEdgeConvex(typename PFP::MAP& map, typename PFP::TVEC3& position,
template <typename PFP>
void convexifyFreeSpace(typename PFP::MAP& map, typename PFP::TVEC3& position, CellMarker& obstacleMark, CellMarker& buildingMark);
template <typename PFP>
void installGuardRail(typename PFP::MAP& map,typename PFP::TVEC3& position, CellMarker& obstacleMark, CellMarker& buildingMark, float height);
}
}
......
......@@ -110,7 +110,7 @@ void generateBuilding(typename PFP::MAP& map, typename PFP::TVEC3& position, Dar
dd = map.phi1(dd);
} while (dd != dRoofSmall);
dRoof = extrudeFaceAndMark<PFP>(map, position, dRoofSmall, buildingMark, height / 2.0f);
}/*
}
bool spike= rand() % 2;
if(spike)
{
......@@ -118,7 +118,7 @@ void generateBuilding(typename PFP::MAP& map, typename PFP::TVEC3& position, Dar
c[2] += height / 1.5f;
dRoof = Algo::Modelisation::trianguleFace<PFP>(map, dRoof);
position[dRoof] = c;
}*/
}
break;
}
}
......@@ -143,6 +143,119 @@ void generateCity(typename PFP::MAP& map, typename PFP::TVEC3& position, CellMar
}
}
template <typename PFP>
void generateMall(typename PFP::MAP& map,typename PFP::TVEC3& position, CellMarker& obstacleMark, CellMarker& buildingMark, float sideSize)
{
unsigned int side = 5;
std::vector<Algo::Modelisation::Polyhedron<PFP> > floors;
Algo::Modelisation::Polyhedron<PFP> * floor2 = new Algo::Modelisation::Polyhedron<PFP>(map,position);
floor2->grid_topo(side,side);
floor2->embedGrid(sideSize*side,sideSize*side);
for(unsigned int i=0;i<5;++i)
{
float floorHeight = 100;
typename PFP::VEC3 transl(0,0,floorHeight);
CellMarker m(map, VERTEX_CELL) ;
for(Dart d=map.begin();d!=map.end();map.next(d))
{
if(!m.isMarked(d))
{
m.mark(d);
position[d] += transl;
}
}
Algo::Modelisation::Polyhedron<PFP> * floor1 = new Algo::Modelisation::Polyhedron<PFP>(map,position);
floor1->grid_topo(side,side);
floor1->embedGrid(sideSize*side,sideSize*side);
Dart d1 = map.template phi<121>(floor1->getDart());
Dart d2 = map.template phi<121121121>(floor2->getDart());
generatePathToUpperStair<PFP>(map,position,d1,d2);
floor2=floor1;
}
Dart boundary;
for(Dart d = map.begin(); d != map.end(); map.next(d))
{
if(map.phi2(d) == d)
{
obstacleMark.mark(d);
boundary = d;
}
}
map.closeHole(boundary);
buildingMark.mark(map.phi2(boundary));
installGuardRail<PFP>(map,position,obstacleMark,buildingMark,sideSize/5.0f);
}
template <typename PFP>
void generatePathToUpperStair(typename PFP::MAP& map,typename PFP::TVEC3& position, Dart dLower,Dart dUpper)
{
if(dLower!= map.phi2(dLower) || dUpper!=map.phi2(dUpper)) {
std::cout << "generatePathToUpperStair : lower and upper stair darts must be fixpoint in phi2" << std::endl;
return;
}
//create the path
Dart dPathUp = map.newOrientedFace(4);
Dart dInBetweenUp = map.newOrientedFace(4);
Dart dInBetweenDown = map.newOrientedFace(4);
Dart dInBetweenMid = map.newOrientedFace(4);
Dart dPathDown = map.newOrientedFace(4);
//sew the path
map.sewFaces(dUpper,map.phi1(map.phi1(dPathUp)));
map.sewFaces(dPathUp,map.phi1(map.phi1(dInBetweenUp)));
map.sewFaces(dLower,map.phi1(map.phi1(dPathDown)));
map.sewFaces(dPathDown,map.phi1(map.phi1(dInBetweenDown)));
map.sewFaces(map.phi1(dInBetweenDown),map.phi_1(dInBetweenMid));
map.sewFaces(map.phi_1(dInBetweenUp),map.phi1(dInBetweenMid));
//embed the inBetween floor
float z = (position[dLower][2]+position[dUpper][2])/2.0f;
//from lower path
typename PFP::VEC3 vDown = position[map.phi1(dLower)]-position[map.phi1(map.phi1(dLower))];
position[map.phi1(map.phi1(dInBetweenDown))] = position[map.phi1(dLower)] + vDown;
position[map.phi_1(dInBetweenDown)] = position[dLower] + vDown;
position[dInBetweenDown] = position[map.phi_1(dInBetweenDown)]+vDown;
position[map.phi1(dInBetweenDown)] = position[map.phi1(map.phi1(dInBetweenDown))]+vDown;
//from upper path
position[map.phi1(map.phi1(dInBetweenUp))] = position[map.phi1(dUpper)] + vDown;
position[map.phi_1(dInBetweenUp)] = position[dUpper] + vDown;
position[dInBetweenUp] = position[map.phi_1(dInBetweenUp)]+vDown;
position[map.phi1(dInBetweenUp)] = position[map.phi1(map.phi1(dInBetweenUp))]+vDown;
Dart dd = dInBetweenUp;
do
{
position[dd][2] = z;
dd = map.phi1(dd);
} while(dd!=dInBetweenUp);
dd = dInBetweenDown;
do
{
position[dd][2] = z;
dd = map.phi1(dd);
} while(dd!=dInBetweenDown);
}
template <typename PFP>
void simplifyFreeSpace(typename PFP::MAP& map, typename PFP::TVEC3& position, CellMarker& obstacleMark, CellMarker& buildingMark)
{
......@@ -423,6 +536,67 @@ void convexifyFreeSpace(typename PFP::MAP& map, typename PFP::TVEC3& position, C
map.mergeFaces(d);
}
template <typename PFP>
void installGuardRail(typename PFP::MAP& map,typename PFP::TVEC3& position, CellMarker& obstacleMark, CellMarker& buildingMark, float height)
{
CellMarker cm(map,EDGE_CELL);
CellMarkerNoUnmark railVert(map,EDGE_CELL);
for(Dart d = map.begin(); d != map.end(); map.next(d))
{
if(!cm.isMarked(d))
{
cm.mark(d);
if(obstacleMark.isMarked(d))
{
Dart dExt = map.newFace(4);
Dart dIn = map.newFace(4);
//mark new faces as obstacles, building and treated darts
obstacleMark.mark(dIn);
buildingMark.mark(dIn);
buildingMark.mark(dExt);
railVert.mark(map.phi_1(dIn));
railVert.mark(map.phi1(dExt));
// obstacleMark.mark(map.phi_1(dIn));
// obstacleMark.mark(map.phi1(dExt));
Dart ddExt = dExt;
Dart ddIn = dIn;
do
{
cm.mark(ddExt);
cm.mark(ddIn);
ddExt = map.phi1(ddExt);
ddIn = map.phi1(ddIn);
} while(ddExt!=dExt);
//sew the upper part of the guard rail
map.sewFaces(map.phi1(map.phi1(dExt)),dIn);
//sew the guard rail to the floor
Dart d2 = map.phi2(d);
map.unsewFaces(d);
map.sewFaces(d,dExt);
map.sewFaces(d2,map.phi1(map.phi1(dIn)));
//set the position of the guard rail
position[dIn] = position[d2];
position[dIn][2] += height;
position[map.phi2(dIn)] = position[d];
position[map.phi2(dIn)][2] += height;
}
}
}
for(Dart d = map.begin(); d != map.end(); map.next(d))
{
if(railVert.isMarked(d))
{
railVert.unmark(d);
map.sewFaces(d,map.phi_1(map.alpha_1(d)));
}
}
}
}
}
......
......@@ -61,7 +61,7 @@ public :
void setAgentNeighborsAndObstacles(Agent* agent);
void agentChangeFaceThroughEdge(Agent* agent);
// void agentChangeFaceThroughEdge(Agent* agent);
void agentChangeFace(Agent* agent, Dart oldFace);
void pushAgentInCells(Agent* agent, Dart d);
......@@ -88,7 +88,7 @@ public :
CellMarker obstacleMark;
CellMarker buildingMark;
static const unsigned int nbAgentsToSubdivide = 12;
static const unsigned int nbAgentsToSubdivide = 3;
static const unsigned int nbAgentsToSimplify = 7;
std::map<unsigned int, Dart> refineCandidate;
......
......@@ -28,6 +28,10 @@ void renderFace(EnvMap& m, Dart& d)
void renderPredictionTriangle(EnvMap& m, Dart d, PFP::VEC3 p)
{
Geom::Plane3D<float> pl = Algo::Geometry::facePlane<PFP>(m.map,d,m.position);
p[2] -= 1000;
Geom::intersectionPlaneRay(pl,p,VEC3(0,0,-1),p);
VEC3 pos1(m.position[d]);
VEC3 pos2(m.position[m.map.phi1(d)]);
pos2[2] += 2;
......@@ -41,45 +45,43 @@ void renderPredictionTriangle(EnvMap& m, Dart d, PFP::VEC3 p)
static const float cosT[5] = { 1, 0.309017, -0.809017, -0.809017, 0.309017 };
static const float sinT[5] = { 0, 0.951057, 0.587785, -0.587785, -0.951057 };
void renderAgent(Agent* agent, bool showNeighborDist = false, bool showObstacleDist = false)
void renderAgent(EnvMap& m, Agent* agent, bool showNeighborDist = false, bool showObstacleDist = false)
{
VEC3& pos = agent->part_.m_position;
VEC3 pos = agent->part_.m_position;
float radius = agent->radius_;
Geom::Plane3D<float> pl = Algo::Geometry::facePlane<PFP>(m.map,agent->part_.d,m.position);
pos[2] -= 1000;
Geom::intersectionPlaneRay(pl,pos,VEC3(0,0,-1),pos);
glLineWidth(1.0f);
glColor3f(1.0f,0.0f,0.0f);
glBegin(GL_POLYGON);
for(unsigned int i = 0; i < 5; ++i)
glVertex3f(pos[0] + (cosT[i] * radius), pos[1] + (sinT[i] * radius), 0.01f);
glVertex3f(pos[0] + (cosT[i] * radius), pos[1] + (sinT[i] * radius), pos[2]+0.01f);
glEnd();
VEC3 dir = agent->velocity_;
dir.normalize();
//draw the speed vector
VEC3 base(0,0,1);
VEC3 myAxisRot = base^dir;
myAxisRot.normalize();
float myRot = acos(base*dir);
glPushMatrix();
glTranslatef(pos[0],pos[1],pos[2]);
// VEC3 base(0,0,1);
// VEC3 myAxisRot = base^dir;
// myAxisRot.normalize();
// float myRot = acos(base*dir);
//
// glPushMatrix();
// glTranslatef(pos[0],pos[1],pos[2]);
// glRotatef(myRot,myAxisRot[0],myAxisRot[1],myAxisRot[2]);
Geom::Matrix44f myMat;
myMat.identity();
Geom::rotate<float>(myAxisRot[0],myAxisRot[1],myAxisRot[2],-myRot,myMat);
glMultMatrixf(&myMat(0,0));
glColor3f(0.0,0.0,0.0);
glLineWidth(5.0f);
glBegin(GL_LINES);
glVertex3f(0.,0.,0.0);
glVertex3f(0.,0.,agent->radius_*2.0f);
glEnd();
glPopMatrix();
//
// glColor3f(0.0,0.0,0.0);
// glLineWidth(5.0f);
// glBegin(GL_LINES);
// glVertex3f(0.,0.,0.0);
// glVertex3f(0.,0.,agent->radius_*2.0f);
// glEnd();
// glPopMatrix();
if(showNeighborDist)
{
......@@ -87,7 +89,7 @@ void renderAgent(Agent* agent, bool showNeighborDist = false, bool showObstacleD
glColor3f(0.0f,1.0f,0.0f);
glBegin(GL_LINE_LOOP);
for(unsigned int i = 0; i < 5; ++i)
glVertex3f(pos[0] + (cosT[i] * radius), pos[1] + (sinT[i] * radius), 0.01f);
glVertex3f(pos[0] + (cosT[i] * radius), pos[1] + (sinT[i] * radius), pos[2]+0.01f);
glEnd();
}
......@@ -97,7 +99,7 @@ void renderAgent(Agent* agent, bool showNeighborDist = false, bool showObstacleD
glColor3f(0.0f,0.0f,1.0f);
glBegin(GL_LINE_LOOP);
for(unsigned int i = 0; i < 5; ++i)
glVertex3f(pos[0] + (cosT[i] * radius), pos[1] + (sinT[i] * radius), 0.01f);
glVertex3f(pos[0] + (cosT[i] * radius), pos[1] + (sinT[i] * radius), pos[2]+0.01f);
glEnd();
}
}
......
......@@ -11,7 +11,7 @@ namespace PathFinder
float pathCostSqr(typename PFP::MAP& map, const typename PFP::TVEC3& position, Dart startPos, Dart stopPos);
template <typename PFP>
std::vector<Dart> pathFindAStar(typename PFP::MAP& map, const typename PFP::TVEC3& position, Dart startPos, Dart stopPos, FunctorType& bad = FunctorType());
std::vector<Dart> pathFindAStar(typename PFP::MAP& map, const typename PFP::TVEC3& position, Dart startPos, Dart stopPos, CellMarker& bad);
}
......
......@@ -16,7 +16,7 @@ float pathCostSqr(typename PFP::MAP& map,const typename PFP::TVEC3& position, Da
}
template <typename PFP>
std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3& position, Dart startPos, Dart stopPos, FunctorType& bad)
std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3& position, Dart startPos, Dart stopPos, CellMarker& bad)
{
std::vector<Dart> path;
......@@ -35,7 +35,7 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3
do {
ddd = map.alpha1(ddd);
if(ddd!=dd) {
if(!map.foreach_dart_of_face(ddd,bad) && tablePred[ddd]==EMBNULL) {
if(!bad.isMarked(ddd) && tablePred[ddd]==EMBNULL) {
//evaluate their cost and push them in the vector to dev
if(tablePred[ddd]==EMBNULL)
tablePred[ddd]=toDev;
......@@ -60,9 +60,7 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3
//if path found : from start to stop -> push all predecessors
if(map.sameOrientedFace(startPos,toDev))
{
std::cout << "found" << std::endl;
Dart toPush=startPos;
std::cout << tablePred[startPos] << std::endl;
do {
path.push_back(toPush);
toPush = tablePred[toPush];
......
......@@ -28,7 +28,7 @@ public:
void doStep();
void setupScenario();
// void addPathsToAgents();
void addPathsToAgents();
bool importAgents(std::string filename);
bool exportAgents(std::string filename);
......
......@@ -21,8 +21,11 @@ inline float det2D(const VEC3& vector1, const VEC3& vector2)
return vector1[0] * vector2[1] - vector1[1] * vector2[0];
}
inline float distSqPointLineSegment(const VEC3& a1, const VEC3& b1, const VEC3& c1)
inline float distSqPointLineSegment( VEC3 a1, VEC3 b1, const VEC3& c1)
{
a1[2] = 0;
b1[2] = 0;
VEC3 a1b1 = b1 - a1;
VEC3 a1c1 = c1 - a1;
const float r = (a1c1 * a1b1) / a1b1.norm2();
......@@ -43,16 +46,6 @@ inline float distSqPointLineSegment(const VEC3& a1, const VEC3& b1, const VEC3&
}
}
inline void projectPointOnLine(const VEC3& p, const VEC3& p1, const VEC3& p2, VEC3& proj)
{
VEC3 v = p - p1;
VEC3 seg = p2 - p1;
seg.normalize();
proj = p1 + seg * (v * seg);
}
inline float leftOf(const VEC3& a, const VEC3& b, const VEC3& c)
{
return det2D(a - c, b - a);
......
......@@ -130,6 +130,8 @@ void Agent::computeNewVelocity()
VEC3 obst1Pos(sim_->envMap_.position[obst1]);
VEC3 obst2Pos(sim_->envMap_.position[obst2]);
obst1Pos[2]=0;
obst2Pos[2]=0;
const VEC3 relativePosition1(obst1Pos - part_.m_position);
const VEC3 relativePosition2(obst2Pos - part_.m_position);
......@@ -236,10 +238,12 @@ void Agent::computeNewVelocity()
// const Obstacle* const leftNeighbor = sim_->obstacles_[obstacle1->prevObstacleNo_];
Dart leftObst1 = sim_->envMap_.map.phi_1(obst1);
VEC3 leftObstPos = sim_->envMap_.position[leftObst1];
leftObstPos[2]=0;
// const Obstacle* const rightNeighbor = sim_->obstacles_[obstacle2->nextObstacleNo_];
Dart rightObst2 = sim_->envMap_.map.phi1(obst2);
VEC3 rightObstPos = sim_->envMap_.position[rightObst2];
rightObstPos[2]=0;
bool isLeftLegForeign = false;
bool isRightLegForeign = false;
......
......@@ -41,8 +41,9 @@ Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
void EnvMap::init()
{
float sideSize = 70.0f;
unsigned int nbSquares = 5;
unsigned int nbSquares = 7;
CityGenerator::generateCity<PFP>(map, position, obstacleMark, buildingMark, sideSize, nbSquares);
// CityGenerator::generateMall<PFP>(map, position, obstacleMark, buildingMark, sideSize);
CityGenerator::simplifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
map.init();
registerObstaclesInFaces();
......@@ -71,26 +72,36 @@ void EnvMap::registerObstaclesInFaces()
if(!m.isMarked(d))
{
m.mark(d);
Dart dd = d;
do
if(!buildingMark.isMarked(d))
{
if(obstacleMark.isMarked(dd))
obstvect[d].push_back(map.phi2(dd));
dd = map.phi1(dd);
} while(dd != d);
Dart dd = d;
do
{
if(obstacleMark.isMarked(dd))
obstvect[d].push_back(map.phi2(dd));
dd = map.phi1(dd);
} while(dd != d);
do
{
Dart ddd = map.alpha1(map.alpha1(dd));
while(ddd != dd)
do
{
if(!buildingMark.isMarked(ddd))
addNeighborObstacles(obstvect[d], ddd, ddd == map.alpha_1(dd));
ddd = map.alpha1(ddd);
}
dd = map.phi1(dd);
} while(dd != d);
Dart ddd = map.alpha1(map.alpha1(dd));
while(ddd != dd)
{
if(!buildingMark.isMarked(ddd))
addNeighborObstacles(obstvect[d], ddd, ddd == map.alpha_1(dd));
ddd = map.alpha1(ddd);
}
dd = map.phi1(dd);
} while(dd != d);
}
}
}
for(Dart d = map.begin(); d != map.end(); map.next(d))
{
if(m.isMarked(d) && !buildingMark.isMarked(d)) {
m.unmark(d);
std::cout << "obstvect size " << obstvect[d].size() << std::endl;
}
}
}
......@@ -117,83 +128,83 @@ void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbo
} while(dd != d);
}
void EnvMap::agentChangeFaceThroughEdge(Agent* agent)
{
Dart oldFace = agent->part_.lastCrossed;
Dart newFace = agent->part_.d;
agentvect[newFace].push_back(agent);
PFP::AGENTS::iterator end = agentvect[oldFace].end();
for(PFP::AGENTS::iterator it = agentvect[oldFace].begin(); it != end; ++it)
{
if(*it == agent)
{
*it = agentvect[oldFace].back();
agentvect[oldFace].pop_back();
break;
}
}
// mark adjacent cells shared by newFace and oldFace
CellMarkerNoUnmark f(map, FACE_CELL);
Dart d = oldFace;
do
{
f.mark(d);
d = map.alpha1(d);
} while(d != oldFace);
d = map.phi1(oldFace);
do
{
f.mark(d);
d = map.alpha1(d);
} while(d != map.phi1(oldFace));
// remove agent from cells adjacent to oldFace but not to newFace
Dart dd = oldFace;
do
{
Dart ddd = map.alpha1(map.alpha1(dd));
while(ddd != dd)
{
if(!f.isMarked(ddd))
{
bool found = false;
PFP::AGENTS::iterator end = neighborAgentvect[ddd].end();
for(PFP::AGENTS::iterator it = neighborAgentvect[ddd].begin(); it != end && !found; ++it)
{
if(*it == agent)
{
*it = neighborAgentvect[ddd].back();