Skip to content
GitLab
Projects
Groups
Snippets
Help
Loading...
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
S
SocialAgents3D
Project overview
Project overview
Details
Activity
Releases
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Issues
0
Issues
0
List
Boards
Labels
Service Desk
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Operations
Operations
Incidents
Environments
Analytics
Analytics
CI / CD
Repository
Value Stream
Wiki
Wiki
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Thomas Pitiot
SocialAgents3D
Commits
b3b10bd1
Commit
b3b10bd1
authored
Apr 18, 2011
by
Pierre Kraemer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
subdiv ok.. perf++
parent
b8f34fe9
Changes
7
Hide whitespace changes
Inline
Side-by-side
Showing
7 changed files
with
301 additions
and
192 deletions
+301
-192
include/agent.h
include/agent.h
+2
-18
include/env_map.h
include/env_map.h
+47
-13
include/env_render.h
include/env_render.h
+4
-4
src/agent.cpp
src/agent.cpp
+32
-54
src/env_map.cpp
src/env_map.cpp
+175
-70
src/simulator.cpp
src/simulator.cpp
+31
-23
src/viewer.cpp
src/viewer.cpp
+10
-10
No files found.
include/agent.h
View file @
b3b10bd1
...
...
@@ -14,21 +14,12 @@ class Agent
public:
Agent
(
Simulator
*
sim
,
const
VEC3
&
position
,
Dart
d
);
void
insertAgentNeighbor
(
Agent
*
other
);
void
insertAgentNeighbor
(
Agent
*
other
,
float
distSq
);
void
removeAgentNeighbor
(
Agent
*
other
);
void
updateAgentNeighbors
();
void
clearAgentNeighbors
();
void
insertObstacleNeighbor
(
Dart
d
);
void
removeObstacleNeighbor
(
Dart
d
);
void
updateObstacleNeighbors
();
void
clearObstacleNeighbors
();
void
update
();
void
computePrefVelocity
();
void
computeNewVelocity
();
bool
linearProgram1
(
...
...
@@ -47,14 +38,9 @@ public:
const
std
::
vector
<
Line
>&
lines
,
unsigned
int
numObstLines
,
float
radius
,
VEC3
&
result
);
std
::
multimap
<
float
,
Agent
*>
agentNeighbors_
;
std
::
multimap
<
float
,
Agent
*>
newAgentNeighbors_
;
// std::map<const Agent*, std::multimap<float, Agent*>::iterator> neighborsIt_;
std
::
multimap
<
float
,
Dart
>
obstacleNeighbors_
;
std
::
multimap
<
float
,
Dart
>
newObstacleNeighbors_
;
std
::
vector
<
std
::
pair
<
float
,
Agent
*>
>
agentNeighbors_
;
std
::
vector
<
std
::
pair
<
float
,
Dart
>
>
obstacleNeighbors_
;
CGoGN
::
Algo
::
MovingObjects
::
ParticleCell2D
<
PFP
>
part_
;
...
...
@@ -74,8 +60,6 @@ public:
VEC3
prefVelocity_
;
Simulator
*
sim_
;
bool
treated
;
};
#endif
...
...
include/env_map.h
View file @
b3b10bd1
...
...
@@ -54,18 +54,18 @@ public :
void
init
();
void
foreach_neighborFace
(
Dart
d
,
FunctorType
&
f
);
void
registerObstaclesInFaces
();
void
addNeighborObstacles
(
PFP
::
OBSTACLES
&
obst
,
Dart
d
,
bool
edgeNeighbor
);
void
setAgentNeighborsAndObstacles
(
Agent
*
agent
);
void
addNeighborAgents
(
Agent
*
agent
,
Dart
d
);
// void agentChangeFaceThroughEdge(Agent* agent, Dart oldFace
);
void
agentChangeFaceThroughEdge
(
Agent
*
agent
);
void
agentChangeFace
(
Agent
*
agent
,
Dart
oldFace
);
void
pushAgentInCell
(
Agent
*
agent
,
Dart
d
);
void
popAgentInCell
(
Agent
*
agent
,
Dart
d
);
void
pushAgentInCell
s
(
Agent
*
agent
,
Dart
d
);
void
popAgentInCell
s
(
Agent
*
agent
,
Dart
d
);
void
addRefineCandidate
(
Dart
d
);
void
addCoarsenCandidate
(
Dart
d
);
...
...
@@ -81,13 +81,15 @@ public :
PFP
::
TVEC3
normal
;
PFP
::
TAB_AGENTVECT
agentvect
;
PFP
::
TAB_AGENTVECT
neighborAgentvect
;
PFP
::
TAB_OBSTACLEVECT
obstvect
;
CellMarker
obstacleMark
;
CellMarker
buildingMark
;
static
const
unsigned
int
nbAgentsToSubdivide
=
12
;
static
const
unsigned
int
nbAgentsToSimplify
=
8
;
static
const
unsigned
int
nbAgentsToSimplify
=
7
;
std
::
map
<
unsigned
int
,
Dart
>
refineCandidate
;
std
::
map
<
unsigned
int
,
Dart
>
coarsenCandidate
;
...
...
@@ -97,31 +99,64 @@ public :
* INLINE FUNCTIONS *
**************************************/
inline
void
EnvMap
::
pushAgentInCell
(
Agent
*
agent
,
Dart
d
)
inline
void
EnvMap
::
pushAgentInCell
s
(
Agent
*
agent
,
Dart
d
)
{
assert
(
map
.
getCurrentLevel
()
==
map
.
getMaxLevel
());
assert
(
std
::
find
(
agentvect
[
d
].
begin
(),
agentvect
[
d
].
end
(),
agent
)
==
agentvect
[
d
].
end
());
agentvect
[
d
].
push_back
(
agent
);
addRefineCandidate
(
d
);
Dart
dd
=
d
;
do
{
Dart
ddd
=
map
.
alpha1
(
map
.
alpha1
(
dd
));
while
(
ddd
!=
dd
)
{
neighborAgentvect
[
ddd
].
push_back
(
agent
);
ddd
=
map
.
alpha1
(
ddd
);
}
dd
=
map
.
phi1
(
dd
);
}
while
(
dd
!=
d
);
}
inline
void
EnvMap
::
popAgentInCell
(
Agent
*
agent
,
Dart
d
)
inline
void
EnvMap
::
popAgentInCell
s
(
Agent
*
agent
,
Dart
d
)
{
assert
(
map
.
getCurrentLevel
()
==
map
.
getMaxLevel
());
assert
(
std
::
find
(
agentvect
[
d
].
begin
(),
agentvect
[
d
].
end
(),
agent
)
!=
agentvect
[
d
].
end
());
bool
found
=
false
;
PFP
::
AGENTS
::
iterator
end
=
agentvect
[
d
].
end
();
for
(
PFP
::
AGENTS
::
iterator
it
=
agentvect
[
d
].
begin
();
it
!=
end
;
++
it
)
for
(
PFP
::
AGENTS
::
iterator
it
=
agentvect
[
d
].
begin
();
it
!=
end
&&
!
found
;
++
it
)
{
if
(
*
it
==
agent
)
{
*
it
=
agentvect
[
d
].
back
();
agentvect
[
d
].
pop_back
();
break
;
found
=
true
;
}
}
addCoarsenCandidate
(
d
);
Dart
dd
=
d
;
do
{
Dart
ddd
=
map
.
alpha1
(
map
.
alpha1
(
dd
));
while
(
ddd
!=
dd
)
{
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
();
neighborAgentvect
[
ddd
].
pop_back
();
found
=
true
;
}
}
ddd
=
map
.
alpha1
(
ddd
);
}
dd
=
map
.
phi1
(
dd
);
}
while
(
dd
!=
d
);
}
inline
void
EnvMap
::
addRefineCandidate
(
Dart
d
)
...
...
@@ -141,4 +176,3 @@ inline void EnvMap::clearUpdateCandidates()
}
#endif
include/env_render.h
View file @
b3b10bd1
...
...
@@ -49,9 +49,9 @@ void renderAgent(Agent* agent, bool showNeighborDist = false, bool showObstacleD
glLineWidth
(
1
.
0
f
);
glColor3f
(
1
.
0
f
,
0
.
0
f
,
0
.
0
f
);
glBegin
(
GL_
LINE_LOOP
);
glBegin
(
GL_
POLYGON
);
for
(
unsigned
int
i
=
0
;
i
<
5
;
++
i
)
glVertex
2f
(
pos
[
0
]
+
(
cosT
[
i
]
*
radius
),
pos
[
1
]
+
(
sinT
[
i
]
*
radius
)
);
glVertex
3f
(
pos
[
0
]
+
(
cosT
[
i
]
*
radius
),
pos
[
1
]
+
(
sinT
[
i
]
*
radius
),
0
.
01
f
);
glEnd
();
if
(
showNeighborDist
)
...
...
@@ -60,7 +60,7 @@ void renderAgent(Agent* agent, bool showNeighborDist = false, bool showObstacleD
glColor3f
(
0
.
0
f
,
1
.
0
f
,
0
.
0
f
);
glBegin
(
GL_LINE_LOOP
);
for
(
unsigned
int
i
=
0
;
i
<
5
;
++
i
)
glVertex
2f
(
pos
[
0
]
+
(
cosT
[
i
]
*
radius
),
pos
[
1
]
+
(
sinT
[
i
]
*
radius
)
);
glVertex
3f
(
pos
[
0
]
+
(
cosT
[
i
]
*
radius
),
pos
[
1
]
+
(
sinT
[
i
]
*
radius
),
0
.
01
f
);
glEnd
();
}
...
...
@@ -70,7 +70,7 @@ void renderAgent(Agent* agent, bool showNeighborDist = false, bool showObstacleD
glColor3f
(
0
.
0
f
,
0
.
0
f
,
1
.
0
f
);
glBegin
(
GL_LINE_LOOP
);
for
(
unsigned
int
i
=
0
;
i
<
5
;
++
i
)
glVertex
2f
(
pos
[
0
]
+
(
cosT
[
i
]
*
radius
),
pos
[
1
]
+
(
sinT
[
i
]
*
radius
)
);
glVertex
3f
(
pos
[
0
]
+
(
cosT
[
i
]
*
radius
),
pos
[
1
]
+
(
sinT
[
i
]
*
radius
),
0
.
01
f
);
glEnd
();
}
}
...
...
src/agent.cpp
View file @
b3b10bd1
...
...
@@ -13,79 +13,57 @@ Agent::Agent(Simulator* sim, const VEC3& position, Dart d) :
velocity_
(
0
),
newVelocity_
(
0
),
prefVelocity_
(
0
),
sim_
(
sim
),
treated
(
false
)
{
rangeSq_
=
(
timeHorizonObst_
*
maxSpeed_
+
radius_
)
*
(
timeHorizonObst_
*
maxSpeed_
+
radius_
);
}
void
Agent
::
insertAgentNeighbor
(
Agent
*
other
)
sim_
(
sim
)
{
if
(
other
!=
this
)
{
VEC3
pop
=
part_
.
m_position
-
other
->
part_
.
m_position
;
float
distSq
=
pop
.
norm2
();
newAgentNeighbors_
.
insert
(
std
::
make_pair
(
distSq
,
other
));
// neighborsIt_[other] = agentNeighbors_.insert(std::make_pair(distSq, other));
}
rangeSq_
=
(
timeHorizonObst_
*
maxSpeed_
+
radius_
)
*
(
timeHorizonObst_
*
maxSpeed_
+
radius_
);
}
void
Agent
::
insertAgentNeighbor
(
Agent
*
other
,
float
distSq
)
bool
agentSort
(
const
std
::
pair
<
float
,
Agent
*>&
a1
,
const
std
::
pair
<
float
,
Agent
*>&
a2
)
{
newAgentNeighbors_
.
insert
(
std
::
make_pair
(
distSq
,
other
));
// neighborsIt_[other] = agentNeighbors_.insert(std::make_pair(distSq, other));
return
a1
.
first
<
a2
.
first
;
}
void
Agent
::
removeAgentNeighbor
(
Agent
*
other
)
bool
obstacleSort
(
const
std
::
pair
<
float
,
Dart
>&
o1
,
const
std
::
pair
<
float
,
Dart
>&
o2
)
{
// agentNeighbors_.erase(neighborsIt_[other]);
// neighborsIt_.erase(other);
return
o1
.
first
<
o2
.
first
;
}
void
Agent
::
updateAgentNeighbors
()
{
for
(
std
::
multimap
<
float
,
Agent
*>::
iterator
it
=
agentNeighbors_
.
begin
();
it
!=
agentNeighbors_
.
end
();
++
it
)
std
::
vector
<
Agent
*>&
agents
=
sim_
->
envMap_
.
agentvect
[
part_
.
d
];
std
::
vector
<
Agent
*>&
neighborAgents
=
sim_
->
envMap_
.
neighborAgentvect
[
part_
.
d
];
agentNeighbors_
.
clear
();
for
(
std
::
vector
<
Agent
*>::
iterator
it
=
agents
.
begin
();
it
!=
agents
.
end
();
++
it
)
{
Agent
*
other
=
it
->
second
;
VEC3
pop
=
part_
.
m_position
-
other
->
part_
.
m_position
;
VEC3
pop
=
part_
.
m_position
-
(
*
it
)
->
part_
.
m_position
;
float
distSq
=
pop
.
norm2
();
newAgentNeighbors_
.
insert
(
std
::
make_pair
(
distSq
,
other
));
agentNeighbors_
.
push_back
(
std
::
make_pair
(
distSq
,
*
it
));
}
for
(
std
::
vector
<
Agent
*>::
iterator
it
=
neighborAgents
.
begin
();
it
!=
neighborAgents
.
end
();
++
it
)
{
VEC3
pop
=
part_
.
m_position
-
(
*
it
)
->
part_
.
m_position
;
float
distSq
=
pop
.
norm2
();
agentNeighbors_
.
push_back
(
std
::
make_pair
(
distSq
,
*
it
));
}
newAgentNeighbors_
.
swap
(
agentNeighbors_
);
newAgentNeighbors_
.
clear
();
}
void
Agent
::
clearAgentNeighbors
()
{
agentNeighbors_
.
clear
();
// neighborsIt_.clear();
std
::
sort
(
agentNeighbors_
.
begin
(),
agentNeighbors_
.
end
(),
agentSort
);
}
void
Agent
::
insertObstacleNeighbor
(
Dart
d
)
void
Agent
::
updateObstacleNeighbors
(
)
{
float
distSq
=
distSqPointLineSegment
(
sim_
->
envMap_
.
position
[
d
],
sim_
->
envMap_
.
position
[
sim_
->
envMap_
.
map
.
phi1
(
d
)],
part_
.
m_position
);
newObstacleNeighbors_
.
insert
(
std
::
make_pair
(
distSq
,
d
));
}
std
::
vector
<
Dart
>&
obst
=
sim_
->
envMap_
.
obstvect
[
part_
.
d
];
void
Agent
::
removeObstacleNeighbor
(
Dart
d
)
{
}
obstacleNeighbors_
.
clear
();
void
Agent
::
updateObstacleNeighbors
()
{
for
(
std
::
multimap
<
float
,
Dart
>::
iterator
it
=
obstacleNeighbors_
.
begin
();
it
!=
obstacleNeighbors_
.
end
();
++
it
)
for
(
std
::
vector
<
Dart
>::
iterator
it
=
obst
.
begin
();
it
!=
obst
.
end
();
++
it
)
{
Dart
d
=
it
->
second
;
float
distSq
=
distSqPointLineSegment
(
sim_
->
envMap_
.
position
[
d
],
sim_
->
envMap_
.
position
[
sim_
->
envMap_
.
map
.
phi1
(
d
)],
part_
.
m_position
);
newObstacleNeighbors_
.
insert
(
std
::
make_pair
(
distSq
,
d
));
float
distSq
=
distSqPointLineSegment
(
sim_
->
envMap_
.
position
[
*
it
],
sim_
->
envMap_
.
position
[
sim_
->
envMap_
.
map
.
phi1
(
*
it
)],
part_
.
m_position
);
obstacleNeighbors_
.
push_back
(
std
::
make_pair
(
distSq
,
*
it
));
}
newObstacleNeighbors_
.
swap
(
obstacleNeighbors_
);
newObstacleNeighbors_
.
clear
();
}
void
Agent
::
clearObstacleNeighbors
()
{
obstacleNeighbors_
.
clear
();
std
::
sort
(
obstacleNeighbors_
.
begin
(),
obstacleNeighbors_
.
end
(),
obstacleSort
);
}
void
Agent
::
update
()
...
...
@@ -95,8 +73,6 @@ void Agent::update()
VEC3
target
=
part_
.
m_position
+
(
velocity_
*
sim_
->
timeStep_
);
part_
.
move
(
target
);
treated
=
false
;
}
void
Agent
::
computePrefVelocity
()
...
...
@@ -129,7 +105,8 @@ void Agent::computeNewVelocity()
std
::
vector
<
Line
>
orcaLines_
;
/* Create obstacle ORCA lines. */
for
(
std
::
multimap
<
float
,
Dart
>::
iterator
it
=
obstacleNeighbors_
.
begin
();
// for(std::multimap<float, Dart>::iterator it = obstacleNeighbors_.begin();
for
(
std
::
vector
<
std
::
pair
<
float
,
Dart
>
>::
iterator
it
=
obstacleNeighbors_
.
begin
();
it
!=
obstacleNeighbors_
.
end
()
&&
it
->
first
<
rangeSq_
;
++
it
)
{
...
...
@@ -353,7 +330,8 @@ void Agent::computeNewVelocity()
/* Create agent ORCA lines. */
unsigned
int
nbA
=
0
;
for
(
std
::
multimap
<
float
,
Agent
*>::
iterator
it
=
agentNeighbors_
.
begin
();
// for(std::multimap<float, Agent*>::iterator it = agentNeighbors_.begin();
for
(
std
::
vector
<
std
::
pair
<
float
,
Agent
*>
>::
iterator
it
=
agentNeighbors_
.
begin
();
it
!=
agentNeighbors_
.
end
()
&&
it
->
first
<
neighborDistSq_
&&
nbA
<
maxNeighbors_
;
++
it
,
++
nbA
)
{
...
...
src/env_map.cpp
View file @
b3b10bd1
#include "env_map.h"
#include "Geometry/inclusion.h"
#include "utils.h"
#include "agent.h"
#include "Algo/MovingObjects/particle_cell_2D_memo.h"
#include "Geometry/inclusion.h"
#include "Algo/MovingObjects/particle_cell_2D_memo.h"
#include "Algo/Modelisation/subdivision.h"
#include "Algo/Geometry/normal.h"
...
...
@@ -14,6 +14,7 @@ EnvMap::EnvMap() : obstacleMark(map, EDGE_CELL), buildingMark(map, FACE_CELL)
position
=
map
.
addAttribute
<
PFP
::
VEC3
>
(
VERTEX_ORBIT
,
"position"
);
normal
=
map
.
addAttribute
<
PFP
::
VEC3
>
(
VERTEX_ORBIT
,
"normal"
);
agentvect
=
map
.
addAttribute
<
PFP
::
AGENTVECT
>
(
FACE_ORBIT
,
"agents"
);
neighborAgentvect
=
map
.
addAttribute
<
PFP
::
AGENTVECT
>
(
FACE_ORBIT
,
"neighborAgents"
);
obstvect
=
map
.
addAttribute
<
PFP
::
OBSTACLEVECT
>
(
FACE_ORBIT
,
"obstacles"
);
}
...
...
@@ -42,11 +43,26 @@ void EnvMap::init()
float
sideSize
=
70.0
f
;
unsigned
int
nbSquares
=
25
;
CityGenerator
::
generateCity
<
PFP
>
(
map
,
position
,
obstacleMark
,
buildingMark
,
sideSize
,
nbSquares
);
CityGenerator
::
simplifyFreeSpace
<
PFP
>
(
map
,
position
,
obstacleMark
,
buildingMark
);
//
CityGenerator::simplifyFreeSpace<PFP>(map, position, obstacleMark, buildingMark);
map
.
init
();
registerObstaclesInFaces
();
}
void
EnvMap
::
foreach_neighborFace
(
Dart
d
,
FunctorType
&
f
)
{
Dart
dd
=
d
;
do
{
Dart
ddd
=
map
.
alpha1
(
map
.
alpha1
(
dd
));
while
(
ddd
!=
dd
)
{
f
(
ddd
);
ddd
=
map
.
alpha1
(
ddd
);
}
dd
=
map
.
phi1
(
dd
);
}
while
(
dd
!=
d
);
}
void
EnvMap
::
registerObstaclesInFaces
()
{
CellMarker
m
(
map
,
FACE_CELL
);
...
...
@@ -66,16 +82,13 @@ void EnvMap::registerObstaclesInFaces()
do
{
Dart
ddd
=
map
.
alpha1
(
dd
);
do
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
);
if
(
ddd
!=
dd
)
{
if
(
!
buildingMark
.
isMarked
(
ddd
))
addNeighborObstacles
(
obstvect
[
d
],
ddd
,
ddd
==
map
.
alpha_1
(
dd
));
}
}
while
(
ddd
!=
dd
);
}
dd
=
map
.
phi1
(
dd
);
}
while
(
dd
!=
d
);
}
...
...
@@ -104,59 +117,88 @@ void EnvMap::addNeighborObstacles(PFP::OBSTACLES& obst, Dart d, bool edgeNeighbo
}
while
(
dd
!=
d
);
}
void
EnvMap
::
setAgentNeighborsAndObstacles
(
Agent
*
agent
)
void
EnvMap
::
agentChangeFaceThroughEdge
(
Agent
*
agent
)
{
agent
->
clearAgentNeighbors
();
agent
->
clearObstacleNeighbors
();
Dart
d
=
agent
->
part_
.
d
;
PFP
::
OBSTACLEVECT
::
iterator
end
=
obstvect
[
d
].
end
();
for
(
PFP
::
OBSTACLEVECT
::
iterator
it
=
obstvect
[
d
].
begin
();
it
!=
end
;
++
it
)
agent
->
insertObstacleNeighbor
(
*
it
);
// add agents of current face
addNeighborAgents
(
agent
,
d
);
// add obstacles and agents of connex faces
Dart
dd
=
d
;
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
{
Dart
ddd
=
map
.
alpha1
(
dd
);
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
();
neighborAgentvect
[
ddd
].
pop_back
();
found
=
true
;
}
}
}
ddd
=
map
.
alpha1
(
ddd
);
if
(
ddd
!=
dd
)
addNeighborAgents
(
agent
,
ddd
);
}
while
(
ddd
!=
dd
);
}
dd
=
map
.
phi1
(
dd
);
}
while
(
dd
!=
d
);
}
}
while
(
dd
!=
oldFace
);
void
EnvMap
::
addNeighborAgents
(
Agent
*
agent
,
Dart
d
)
{
PFP
::
AGENTS
&
agents
=
agentvect
[
d
];
for
(
PFP
::
AGENTS
::
iterator
it
=
agents
.
begin
();
it
!=
agents
.
end
();
++
it
)
agent
->
insertAgentNeighbor
(
*
it
);
}
/*
void EnvMap::agentChangeFaceThroughEdge(Agent* agent, Dart oldFace)
{
Dart newFace = agent->part_.d;
popAgentInCell(agent, oldFace);
pushAgentInCell(agent, newFace);
CellMarker f(map, FACE_CELL);
// add agent from cells adjacent to oldFace but not to newFace
// and unmark shared cells
dd
=
newFace
;
do
{
Dart
ddd
=
map
.
alpha1
(
map
.
alpha1
(
dd
));
while
(
ddd
!=
dd
)
{
if
(
!
f
.
isMarked
(
ddd
))
neighborAgentvect
[
ddd
].
push_back
(
agent
);
else
f
.
unmark
(
ddd
);
ddd
=
map
.
alpha1
(
ddd
);
}
dd
=
map
.
phi1
(
dd
);
}
while
(
dd
!=
newFace
);
}
*/
void
EnvMap
::
agentChangeFace
(
Agent
*
agent
,
Dart
oldFace
)
{
Dart
newFace
=
agent
->
part_
.
d
;
popAgentInCell
(
agent
,
oldFace
);
pushAgentInCell
(
agent
,
newFace
);
setAgentNeighborsAndObstacles
(
agent
);
popAgentInCells
(
agent
,
oldFace
);
pushAgentInCells
(
agent
,
agent
->
part_
.
d
);
}
void
EnvMap
::
updateMap
()
...
...
@@ -172,18 +214,62 @@ void EnvMap::updateMap()
if
(
agentvect
[
old
].
size
()
>
nbAgentsToSubdivide
)
{
PFP
::
AGENTS
agents
;
agents
.
swap
(
agentvect
[
old
])
;
PFP
::
AGENTS
oldAgents
(
agentvect
[
old
]);
for
(
PFP
::
AGENTS
::
iterator
ait
=
oldAgents
.
begin
();
ait
!=
oldAgents
.
end
();
++
ait
)
popAgentInCells
(
*
ait
,
old
);
neighborAgentvect
[
old
].
clear
()
;
map
.
setCurrentLevel
(
fLevel
)
;
CellMarkerStore
newF
(
map
,
FACE_CELL
);
newF
.
mark
(
old
);
Algo
::
IHM
::
subdivideFace
<
PFP
>
(
map
,
old
,
position
)
;
map
.
setCurrentLevel
(
map
.
getMaxLevel
())
;
for
(
PFP
::
AGENTS
::
iterator
ait
=
agents
.
begin
();
ait
!=
a
gents
.
end
();
++
ait
)
for
(
PFP
::
AGENTS
::
iterator
ait
=
oldAgents
.
begin
();
ait
!=
oldA
gents
.
end
();
++
ait
)
{
resetAgentInFace
(
*
ait
)
;
agentvect
[(
*
ait
)
->
part_
.
d
].
push_back
(
*
ait
)
;
pushAgentInCells
(
*
ait
,
(
*
ait
)
->
part_
.
d
)
;
}
unsigned
int
degree
=
0
;
Dart
dd
=
old
;
do
{
++
degree
;
Dart
d3
=
dd
;
do
{
Dart
d4
=
map
.
alpha1
(
map
.
alpha1
(
d3
));
while
(
d4
!=
d3
)
{
if
(
!
newF
.
isMarked
(
d4
))
neighborAgentvect
[
d3
].
insert
(
neighborAgentvect
[
d3
].
end
(),
agentvect
[
d4
].
begin
(),
agentvect
[
d4
].
end
());
d4
=
map
.
alpha1
(
d4
);
}
d3
=
map
.
phi1
(
d3
);
}
while
(
d3
!=
dd
);
map
.
setCurrentLevel
(
fLevel
)
;
dd
=
map
.
phi1
(
dd
);
map
.
setCurrentLevel
(
map
.
getMaxLevel
())
;
}
while
(
dd
!=
old
);
if
(
degree
==
3
)
{
Dart
centerFace
=
map
.
phi2
(
map
.
phi1
(
old
));
Dart
d3
=
centerFace
;
do
{
Dart
d4
=
map
.
alpha1
(
map
.
alpha1
(
d3
));
while
(
d4
!=
d3
)
{
if
(
!
newF
.
isMarked
(
d4
))
neighborAgentvect
[
d3
].
insert
(
neighborAgentvect
[
d3
].
end
(),
agentvect
[
d4
].
begin
(),
agentvect
[
d4
].
end
());
d4
=
map
.
alpha1
(
d4
);
}
d3
=
map
.
phi1
(
d3
);
}
while
(
d3
!=
centerFace
);
}
}
}
...
...
@@ -208,8 +294,7 @@ void EnvMap::updateMap()
Dart fit = old ;
do
{
unsigned nbA = agentvect[fit].size() ;
nbAgents += nbA ;
nbAgents += agentvect[fit].size() ;
++degree ;
fit = map.phi1(fit) ;
} while(fit != old) ;
...
...
@@ -231,9 +316,11 @@ void EnvMap::updateMap()
unsigned int fEmb = map.getEmbedding(fit, FACE_ORBIT) ;
if(fEmb != face)
coarsenCandidate.erase(fEmb) ;
PFP::AGENTS& a = agentvect[fit] ;
if(a.size() > 0)
agents.insert(agents.end(), a.begin(), a.end()) ;
PFP::AGENTS a(agentvect[fit]) ;