Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
Thomas Pitiot
CGoGN
Commits
de0cd3b6
Commit
de0cd3b6
authored
Feb 01, 2011
by
Thomas
Browse files
proprification et correction de la recherche de chemin
>>>>>> .r274
parent
0966f73e
Changes
11
Hide whitespace changes
Inline
Side-by-side
Apps/Examples/SocialAgents/include/agent.h
View file @
de0cd3b6
...
...
@@ -61,7 +61,7 @@ class Simulator;
size_t
agentNo_
;
size_t
goalNo_
;
Dart
nearestDart
;
//
Dart nearestDart;
// std::vector<Dart> includingFaces;
CGoGN
::
Algo
::
MovingObjects
::
ParticleCell2D
<
PFP
>
*
part
;
float
rangeSq
;
...
...
Apps/Examples/SocialAgents/include/env_generator.hpp
View file @
de0cd3b6
...
...
@@ -148,7 +148,7 @@ void generateSmallCity(typename PFP::MAP& map, EMBV& position, DartMarker& close
// sideSize *= 0.2f;
unsigned
int
nbBuilding
=
1000
;
float
height
=
sideSize
/
2.0
f
;
unsigned
int
side
=
20
;
unsigned
int
side
=
3
;
generateGrid
<
PFP
,
EMBV
>
(
map
,
position
,
side
,
side
,
sideSize
,
closeMark
);
Dart
dEnd
=
map
.
end
();
...
...
Apps/Examples/SocialAgents/include/env_render.h
View file @
de0cd3b6
...
...
@@ -5,13 +5,13 @@
#include
<GL/glut.h>
static
void
renderPoint
(
EnvMap
&
m
,
Dart
&
d
)
{
PFP
::
VEC3
p
=
m
.
position
[
d
];
glBegin
(
GL_POINTS
);
glVertex3f
(
p
[
0
],
p
[
1
],
p
[
2
]);
glEnd
();
}
//
static void renderPoint(EnvMap& m, Dart& d)
//
{
//
PFP::VEC3 p = m.position[d];
//
glBegin(GL_POINTS);
//
glVertex3f(p[0],p[1],p[2]);
//
glEnd();
//
}
static
void
renderDart
(
EnvMap
&
m
,
Dart
d
)
{
...
...
@@ -33,24 +33,24 @@ static void renderFace(EnvMap& m, Dart& d)
}
while
(
dd
!=
d
);
}
static
void
renderCellOfDim
(
EnvMap
&
m
,
Dart
d
,
unsigned
int
dim
)
{
switch
(
dim
)
{
case
0
:
renderPoint
(
m
,
d
);
break
;
case
1
:
renderDart
(
m
,
d
);
break
;
case
2
:
renderFace
(
m
,
d
);
break
;
}
}
static
void
renderAllPoints
(
EnvMap
&
m
)
{
glBegin
(
GL_POINTS
);
for
(
Dart
d
=
m
.
map
.
begin
();
d
!=
m
.
map
.
end
();
m
.
map
.
next
(
d
))
{
PFP
::
VEC3
p
=
m
.
position
[
d
];
glVertex3f
(
p
[
0
],
p
[
1
],
p
[
2
]);
}
glEnd
();
}
//
static void renderCellOfDim(EnvMap& m,Dart d, unsigned int dim)
//
{
//
switch(dim) {
//
case 0 : renderPoint(m,d); break;
//
case 1 : renderDart(m,d); break;
//
case 2 : renderFace(m,d); break;
//
}
//
}
//
//
static void renderAllPoints(EnvMap& m)
//
{
//
glBegin(GL_POINTS);
//
for(Dart d=m.map.begin();d!=m.map.end();m.map.next(d)) {
//
PFP::VEC3 p = m.position[d];
//
glVertex3f(p[0],p[1],p[2]);
//
}
//
glEnd();
//
}
static
void
renderPredictionTriangle
(
EnvMap
&
m
,
Dart
d
,
PFP
::
VEC3
p
)
{
...
...
Apps/Examples/SocialAgents/include/path_finder.hpp
View file @
de0cd3b6
...
...
@@ -24,6 +24,8 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3
std
::
vector
<
std
::
pair
<
float
,
Dart
>
>
vToDev
;
Dart
toDev
=
stopPos
;
vToDev
.
push_back
(
std
::
make_pair
(
0
,
toDev
));
tablePred
[
toDev
]
=
toDev
;
do
{
//dev cell
//get all vertex-adjacent cells
...
...
@@ -33,12 +35,12 @@ 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
))
{
if
(
!
map
.
foreach_dart_of_face
(
ddd
,
bad
)
&&
tablePred
[
ddd
]
==
EMBNULL
)
{
//evaluate their cost and push them in the vector to dev
if
(
tablePred
[
ddd
]
==
EMBNULL
)
tablePred
[
ddd
]
=
toDev
;
std
::
vector
<
std
::
pair
<
float
,
Dart
>
>::
iterator
it
=
vToDev
.
begin
();
float
costDDD
=
pathCostSqr
<
PFP
>
(
map
,
position
,
startPos
,
ddd
);
float
costDDD
=
pathCostSqr
<
PFP
>
(
map
,
position
,
startPos
,
ddd
)
+
(
vToDev
.
begin
())
->
first
;
while
(
it
!=
vToDev
.
end
()
&&
(
*
it
).
first
<
costDDD
)
++
it
;
vToDev
.
insert
(
it
,
std
::
make_pair
(
costDDD
,
ddd
));
...
...
@@ -58,6 +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
{
...
...
@@ -72,4 +75,4 @@ std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3
//namespace
}
}
\ No newline at end of file
}
Apps/Examples/SocialAgents/include/simulator.h
View file @
de0cd3b6
...
...
@@ -73,6 +73,7 @@ class Simulator
//scenarii
void
setupScenario
();
void
setupTobogganScenario
(
float
rMax
,
float
rMin
);
void
setupHelicoidScenario
(
float
rMax
,
float
rMin
);
};
...
...
Apps/Examples/SocialAgents/src/agent.cpp
View file @
de0cd3b6
...
...
@@ -8,22 +8,21 @@ Agent::Agent(Simulator* sim, size_t agentNo) : agentNeighbors_(), maxNeighbors_(
Agent
::
Agent
(
Simulator
*
sim
,
const
VEC_A
&
position
,
size_t
agentNo
)
:
agentNeighbors_
(),
maxNeighbors_
(
sim
->
defaultAgent_
->
maxNeighbors_
),
maxSpeed_
(
sim
->
defaultAgent_
->
maxSpeed_
),
neighborDist_
(
sim
->
defaultAgent_
->
neighborDist_
),
newVelocity_
(
sim
->
defaultAgent_
->
velocity_
),
obstacleNeighbors_
(),
orcaLines_
(),
position_
(
position
),
prefVelocity_
(),
radius_
(
sim
->
defaultAgent_
->
radius_
),
sim_
(
sim
),
timeHorizon_
(
sim
->
defaultAgent_
->
timeHorizon_
),
timeHorizonObst_
(
sim
->
defaultAgent_
->
timeHorizonObst_
),
velocity_
(
sim
->
defaultAgent_
->
velocity_
),
agentNo_
(
agentNo
),
goalNo_
(
agentNo
),
treated
(
false
)
{
nearest
Dart
=
sim
->
envMap
.
getBelongingCell
(
VEC3
(
position
[
0
],
position
[
1
],
0
));
part
=
new
Algo
::
MovingObjects
::
ParticleCell2D
<
PFP
>
(
sim
->
envMap
.
map
,
nearestDart
,
position
,
sim
->
envMap
.
position
,
sim
->
envMap
.
closeMark
);
Dart
d
=
sim
->
envMap
.
getBelongingCell
(
VEC3
(
position
[
0
],
position
[
1
],
0
));
part
=
new
Algo
::
MovingObjects
::
ParticleCell2D
<
PFP
>
(
sim
->
envMap
.
map
,
d
,
position
,
sim
->
envMap
.
position
,
sim
->
envMap
.
closeMark
);
rangeSq
=
sqr
(
timeHorizonObst_
*
maxSpeed_
+
radius_
);
}
Agent
::
Agent
(
Simulator
*
sim
,
const
VEC_A
&
position
,
size_t
agentNo
,
Dart
d
)
:
agentNeighbors_
(),
maxNeighbors_
(
sim
->
defaultAgent_
->
maxNeighbors_
),
maxSpeed_
(
sim
->
defaultAgent_
->
maxSpeed_
),
neighborDist_
(
sim
->
defaultAgent_
->
neighborDist_
),
newVelocity_
(
sim
->
defaultAgent_
->
velocity_
),
obstacleNeighbors_
(),
orcaLines_
(),
position_
(
position
),
prefVelocity_
(),
radius_
(
sim
->
defaultAgent_
->
radius_
),
sim_
(
sim
),
timeHorizon_
(
sim
->
defaultAgent_
->
timeHorizon_
),
timeHorizonObst_
(
sim
->
defaultAgent_
->
timeHorizonObst_
),
velocity_
(
sim
->
defaultAgent_
->
velocity_
),
agentNo_
(
agentNo
),
goalNo_
(
agentNo
),
treated
(
false
)
{
nearestDart
=
d
;
part
=
new
Algo
::
MovingObjects
::
ParticleCell2D
<
PFP
>
(
sim
->
envMap
.
map
,
nearestDart
,
position
,
sim
->
envMap
.
position
,
sim
->
envMap
.
closeMark
);
part
=
new
Algo
::
MovingObjects
::
ParticleCell2D
<
PFP
>
(
sim
->
envMap
.
map
,
d
,
position
,
sim
->
envMap
.
position
,
sim
->
envMap
.
closeMark
);
rangeSq
=
sqr
(
timeHorizonObst_
*
maxSpeed_
+
radius_
);
}
Agent
::
Agent
(
Simulator
*
sim
,
const
VEC_A
&
position
,
float
neighborDist
,
size_t
maxNeighbors
,
float
timeHorizon
,
float
timeHorizonObst
,
float
radius
,
const
VEC_A
&
velocity
,
float
maxSpeed
,
size_t
agentNo
)
:
agentNeighbors_
(),
maxNeighbors_
(
maxNeighbors
),
maxSpeed_
(
maxSpeed
),
neighborDist_
(
neighborDist
),
newVelocity_
(
velocity
),
obstacleNeighbors_
(),
orcaLines_
(),
position_
(
position
),
prefVelocity_
(),
radius_
(
radius
),
sim_
(
sim
),
timeHorizon_
(
timeHorizon
),
timeHorizonObst_
(
timeHorizonObst
),
velocity_
(
velocity
),
agentNo_
(
agentNo
),
goalNo_
(
agentNo
),
treated
(
false
)
{
nearest
Dart
=
sim
->
envMap
.
getBelongingCell
(
VEC3
(
position
[
0
],
position
[
1
],
0
));
part
=
new
Algo
::
MovingObjects
::
ParticleCell2D
<
PFP
>
(
sim
->
envMap
.
map
,
nearestDart
,
position
,
sim
->
envMap
.
position
,
sim
->
envMap
.
closeMark
);
Dart
d
=
sim
->
envMap
.
getBelongingCell
(
VEC3
(
position
[
0
],
position
[
1
],
0
));
part
=
new
Algo
::
MovingObjects
::
ParticleCell2D
<
PFP
>
(
sim
->
envMap
.
map
,
d
,
position
,
sim
->
envMap
.
position
,
sim
->
envMap
.
closeMark
);
rangeSq
=
sqr
(
timeHorizonObst_
*
maxSpeed_
+
radius_
);
}
...
...
@@ -355,7 +354,6 @@ void Agent::update()
position_
+=
velocity_
*
sim_
->
timeStep_
;
part
->
move
(
VEC3
(
position_
[
0
],
position_
[
1
],
0
));
nearestDart
=
part
->
getCell
();
}
bool
linearProgram1
(
const
std
::
vector
<
Line
>&
lines
,
size_t
lineNo
,
float
radius
,
const
Agent
::
Agent
::
VEC_A
&
optVelocity
,
bool
directionOpt
,
Agent
::
VEC_A
&
result
)
...
...
Apps/Examples/SocialAgents/src/env_map.cpp
View file @
de0cd3b6
...
...
@@ -218,6 +218,7 @@ Dart EnvMap::getBelongingCell(const PFP::VEC3& pos)
void
EnvMap
::
insertObstacleOfFace
(
PFP
::
AGENTS
agents
,
const
Dart
d
)
{
map
.
setCurrentLevel
(
map
.
getMinLevel
())
;
Dart
dd
=
d
;
do
{
if
(
closeMark
.
isMarked
(
dd
)
/*&& (position[map.phi2(dd)][2]==0.0 || position[map.phi1(map.phi2(dd))][2] ==0.0f)*/
)
{
...
...
@@ -230,6 +231,8 @@ void EnvMap::insertObstacleOfFace(PFP::AGENTS agents,const Dart d)
}
dd
=
map
.
phi1
(
dd
);
}
while
(
dd
!=
d
);
map
.
setCurrentLevel
(
map
.
getMaxLevel
())
;
}
void
EnvMap
::
getAllFacesOfAgents
(
Dart
d
)
...
...
@@ -431,13 +434,15 @@ void EnvMap::subdivideFaces()
for
(
std
::
vector
<
Dart
>::
iterator
it
=
marked
.
begin
();
it
!=
marked
.
end
();
++
it
)
closeMark
.
mark
(
map
.
phi2
(
*
it
))
;
map
.
setCurrentLevel
(
cur
)
;
for
(
PFP
::
AGENTS
::
iterator
it
=
agents
.
begin
();
it
!=
agents
.
end
();
++
it
)
{
resetAgentInFace
(
*
it
)
;
agentvect
[(
*
it
)
->
part
->
d
].
push_back
(
*
it
)
;
}
map
.
setCurrentLevel
(
cur
)
;
}
}
}
...
...
@@ -684,7 +689,7 @@ VEC3 EnvMap::faceCenter(Dart d)
void
EnvMap
::
resetAgentInFace
(
Agent
*
agent
)
{
// agent->part->state = VERTEX_ORBIT;
// agent->part->m_position = position[d];
// agent->part->m_position = position[
agent->part->
d];
// agent->part->m_position = agent->part->pointInFace(agent->part->d);
agent
->
part
->
m_position
=
Algo
::
Geometry
::
faceCentroid
<
PFP
>
(
map
,
agent
->
part
->
d
,
position
);
...
...
Apps/Examples/SocialAgents/src/viewer.cpp
View file @
de0cd3b6
...
...
@@ -191,7 +191,7 @@ void MyGlutWin::myRedraw()
// glColor3f(i%int(2.0f/3.0f*v.size())/(v.size()/3.0f),i%int(1.0f/3.0f*v.size())/(v.size()/3.0f),i/(v.size()/3.0f));
// glCircle3i(v[i][0],v[i][1],1.5f);
glPushMatrix
();
// Geom::Plane3D<PFP::REAL> pl = Algo::Geometry::trianglePlane<PFP>(sim->envMap.map,sim->agents_[i]->
nearestDart
,sim->envMap.position);
// Geom::Plane3D<PFP::REAL> pl = Algo::Geometry::trianglePlane<PFP>(sim->envMap.map,sim->agents_[i]->
part->d
,sim->envMap.position);
VEC3
pBase
(
sim
->
agents_
[
i
]
->
position_
);
VEC3
posR
=
pBase
;
// pl.normal() = -1.0f*pl.normal();
...
...
@@ -340,7 +340,7 @@ void MyGlutWin::myRedraw()
}
else
if
(
visu
==
3
)
{
// renderDart(sim->envMap,sim->envMap.getDart(33701));
// renderDart(sim->envMap,sim->envMap.getDart(
45
2));
// renderDart(sim->envMap,sim->envMap.getDart(2));
// renderCellOfDim(sim->envMap,sim->envMap.getDart(1257),2);
// glBegin(GL_POINTS);
// glVertex3f(-494.518,-672,0);
...
...
@@ -474,7 +474,7 @@ bool MyGlutWin::exportScenePov(PFP::MAP& map, PFP::TVEC3& position, const std::s
std
::
vector
<
PFP
::
VEC3
>
v
=
sim
->
getAgentsPosition
();
for
(
unsigned
int
i
=
0
;
i
<
sim
->
agents_
.
size
()
;
++
i
)
{
out
<<
"cylinder {"
<<
std
::
endl
;
Geom
::
Plane3D
<
PFP
::
REAL
>
pl
=
Algo
::
Geometry
::
trianglePlane
<
PFP
>
(
sim
->
envMap
.
map
,
sim
->
agents_
[
i
]
->
nearestDart
,
sim
->
envMap
.
position
);
Geom
::
Plane3D
<
PFP
::
REAL
>
pl
=
Algo
::
Geometry
::
trianglePlane
<
PFP
>
(
sim
->
envMap
.
map
,
sim
->
agents_
[
i
]
->
part
->
d
,
sim
->
envMap
.
position
);
VEC3
pBase
(
sim
->
agents_
[
i
]
->
position_
);
VEC3
posR
;
pl
.
normal
()
=
-
1.0
f
*
pl
.
normal
();
...
...
include/Algo/MovingObjects/particle_cell_2D.h
View file @
de0cd3b6
...
...
@@ -72,7 +72,6 @@ class ParticleCell2D : public ParticleBase
void
move
(
const
VEC3
&
newCurrent
)
{
if
(
!
Geom
::
arePointsEquals
(
newCurrent
,
m_position
))
{
// std::cout << "move to : " << newCurrent << std::endl;
prevPos
=
m_position
;
switch
(
state
)
{
...
...
@@ -80,6 +79,8 @@ class ParticleCell2D : public ParticleBase
case
EDGE_ORBIT
:
edgeState
(
newCurrent
);
break
;
case
FACE_ORBIT
:
faceState
(
newCurrent
);
break
;
}
display
();
}
}
...
...
include/Algo/MovingObjects/particle_cell_2D.hpp
View file @
de0cd3b6
...
...
@@ -200,6 +200,7 @@ void ParticleCell2D<PFP>::faceState(const VEC3& current)
edgeState
(
current
);
return
;
case
Geom
::
RIGHT
:
std
::
cout
<<
std
::
setprecision
(
10
);
std
::
cout
<<
"smthg went bad "
<<
m_position
<<
" "
<<
current
<<
std
::
endl
;
std
::
cout
<<
"d1 "
<<
m_positions
[
d
]
<<
" d2 "
<<
m_positions
[
m
.
phi1
(
d
)]
<<
std
::
endl
;
m_position
=
intersectLineEdge
(
current
,
m_position
,
d
);
...
...
include/Geometry/inclusion.hpp
View file @
de0cd3b6
...
...
@@ -163,7 +163,7 @@ bool arePointsEquals(const VEC3& point1, const VEC3& point2)
{
VEC3
v
(
point1
-
point2
);
#define PRECISION 1e-
20
#define PRECISION 1e-
6
return
v
.
norm2
()
<=
PRECISION
;
#undef PRECISION
}
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment