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
76df679c
Commit
76df679c
authored
Oct 31, 2012
by
David Cazier
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Correction timer
parent
7904ec91
Changes
12
Hide whitespace changes
Inline
Side-by-side
Showing
12 changed files
with
110 additions
and
265 deletions
+110
-265
CMakeLists.txt
CMakeLists.txt
+1
-1
include/agent.h
include/agent.h
+0
-3
include/env_generator.hpp
include/env_generator.hpp
+13
-5
include/env_map.h
include/env_map.h
+0
-1
include/moving_obstacle.h
include/moving_obstacle.h
+4
-4
include/simulator.h
include/simulator.h
+1
-0
include/viewer.h
include/viewer.h
+2
-1
src/agent.cpp
src/agent.cpp
+9
-78
src/env_map.cpp
src/env_map.cpp
+1
-6
src/moving_obstacle.cpp
src/moving_obstacle.cpp
+40
-37
src/simulator.cpp
src/simulator.cpp
+15
-110
src/viewer.cpp
src/viewer.cpp
+24
-19
No files found.
CMakeLists.txt
View file @
76df679c
...
...
@@ -4,7 +4,7 @@ project(SocialAgents)
#add_definitions(-DSPATIAL_HASHING)
SET
(
CGoGN_ROOT_DIR
${
CMAKE_SOURCE_DIR
}
/../
../
CGoGN CACHE STRING
"CGoGN root dir"
)
SET
(
CGoGN_ROOT_DIR
${
CMAKE_SOURCE_DIR
}
/../CGoGN CACHE STRING
"CGoGN root dir"
)
include
(
${
CGoGN_ROOT_DIR
}
/apps_cmake.txt
)
add_subdirectory
(
${
CMAKE_SOURCE_DIR
}
/Release Release
)
...
...
include/agent.h
View file @
76df679c
...
...
@@ -67,9 +67,6 @@ public:
static
float
range_
;
static
float
rangeSq_
;
static
float
timeStep
;
static
unsigned
int
cptAgent
;
float
color1
;
...
...
include/env_generator.hpp
View file @
76df679c
...
...
@@ -528,13 +528,21 @@ void generatePathToUpperStair(typename PFP::MAP& map, typename PFP::TVEC3& posit
}
template
<
typename
PFP
>
void
generatePlanet
(
typename
PFP
::
MAP
&
map
,
typename
PFP
::
TVEC3
&
position
,
CellMarker
<
EDGE
>&
obstacleMark
,
CellMarker
<
FACE
>&
buildingMark
,
float
radius
,
unsigned
int
nbSquares
)
void
generatePlanet
(
EnvMap
&
envMap
)
{
Algo
::
Modelisation
::
Polyhedron
<
PFP
>
prim
(
map
,
position
)
;
prim
.
cylinder_topo
(
nbSquares
,
nbSquares
,
true
,
true
)
;
unsigned
int
nx
=
envMap
.
geometry
.
size
(
0
)
/
envMap
.
maxCellSize
;
unsigned
int
ny
=
envMap
.
geometry
.
size
(
1
)
/
envMap
.
maxCellSize
;
if
(
nx
<
1
)
nx
=
1
;
if
(
ny
<
1
)
ny
=
1
;
Algo
::
Modelisation
::
Polyhedron
<
PFP
>
prim
(
envMap
.
map
,
envMap
.
position
)
;
prim
.
cylinder_topo
(
nx
,
ny
,
true
,
true
)
;
double
pi
=
3.14159265358979323846
f
;
double
xRadius
=
envMap
.
geometry
.
size
(
0
)
/
2
*
pi
;
double
yRadius
=
envMap
.
geometry
.
size
(
1
)
/
2
*
pi
;
prim
.
embedSphere
(
radius
)
;
prim
.
embedSphere
(
(
xRadius
+
yRadius
)
/
2.0
f
)
;
}
template
<
typename
PFP
>
...
...
include/env_map.h
View file @
76df679c
...
...
@@ -69,7 +69,6 @@ public:
REAL
maxCellSize
;
REAL
minCellSize
;
REAL
obstacleDistance
;
REAL
max_for_obstacles
;
EnvMap
()
;
void
init
(
unsigned
int
config
,
REAL
width
,
REAL
height
,
REAL
minSize
,
REAL
maxSize
)
;
...
...
include/moving_obstacle.h
View file @
76df679c
...
...
@@ -12,10 +12,12 @@ PFP::VEC3 rotate (PFP::VEC3 pos1, PFP::VEC3 center, float angle);
float
get_angle
(
PFP
::
VEC3
v1
,
PFP
::
VEC3
v2
);
void
register_add
(
Obstacle
*
o
,
int
n
,
const
std
::
list
<
Dart
>&
memo
);
class
Simulator
;
class
MovingObstacle
{
public:
MovingObstacle
(
int
index
,
EnvMap
*
envmap
,
std
::
vector
<
PFP
::
VEC3
>
pos
,
VEC3
goal
,
float
rota
);
MovingObstacle
(
Simulator
*
sim
,
int
index
,
std
::
vector
<
PFP
::
VEC3
>
pos
,
VEC3
goal
,
float
rota
);
bool
test_opposition
(
VEC3
o
,
VEC3
p1
,
VEC3
p2
);
void
updateFixedObstNeighbors
();
bool
is_inside
(
VEC3
p
);
...
...
@@ -60,15 +62,13 @@ public:
static
unsigned
int
maxNeighbors_
;
static
float
detectionFixedObst
;
static
float
maxSpeed_
;
static
float
timeStep
;
float
obstacle_range
;
static
float
timeHorizonObst_
;
float
velocity_factor
;
VEC3
velocity_
;
VEC3
newVelocity_
;
VEC3
prefVelocity_
;
EnvMap
*
envMap_
;
Simulator
*
sim_
;
};
#endif
include/simulator.h
View file @
76df679c
...
...
@@ -2,6 +2,7 @@
#define SIMULATOR_H
#include <limits>
#include <time.h>
#include "env_map.h"
#include "agent.h"
...
...
include/viewer.h
View file @
76df679c
...
...
@@ -77,7 +77,8 @@ public:
int
maxIterations
;
// to count fps
int
frames
;
clock_t
nextUpdate
;
struct
timespec
startTime
;
time_t
nextUpdate
;
Simulator
sim
;
SelectorTrue
allDarts
;
...
...
src/agent.cpp
View file @
76df679c
...
...
@@ -11,8 +11,6 @@ float Agent::timeHorizonObst_ = 10.0f ;
float
Agent
::
range_
=
timeHorizonObst_
*
maxSpeed_
+
radius_
;
float
Agent
::
rangeSq_
=
range_
*
range_
;
float
Agent
::
timeStep
=
0.25
f
;
unsigned
int
Agent
::
cptAgent
=
0
;
#ifdef SPATIAL_HASHING
...
...
@@ -28,7 +26,6 @@ Agent::Agent(Simulator* sim, const VEC3& position) :
{
agentNeighbors_
.
reserve
(
maxNeighbors_
*
2
)
;
obstacleNeighbors_
.
reserve
(
maxNeighbors_
*
2
)
;
timeStep
=
sim
->
timeStep_
;
agentNo
=
cptAgent
++
;
}
#else
...
...
@@ -48,7 +45,6 @@ Agent::Agent(Simulator* sim, const VEC3& position, Dart d) :
agentNeighbors_
.
reserve
(
maxNeighbors_
*
2
)
;
obstacleNeighbors_
.
reserve
(
maxNeighbors_
*
2
)
;
movingObstacleNeighbors_
.
reserve
(
maxNeighbors_
*
2
)
;
timeStep
=
sim
->
timeStep_
;
agentNo
=
cptAgent
++
;
}
#endif
...
...
@@ -192,9 +188,9 @@ void Agent::update()
velocity_
[
0
]
=
newVelocity_
[
0
]
;
velocity_
[
1
]
=
newVelocity_
[
1
]
;
#ifdef SPATIAL_HASHING
pos
=
pos
+
(
velocity_
*
timeStep
)
;
pos
=
pos
+
(
velocity_
*
sim
->
timeStep_
)
;
#else
VEC3
target
=
part_
.
getPosition
()
+
(
velocity_
*
timeStep
)
;
VEC3
target
=
part_
.
getPosition
()
+
(
velocity_
*
sim_
->
timeStep_
)
;
#endif
meanSpeed_
*=
3.0
f
;
...
...
@@ -258,83 +254,18 @@ void Agent::computePrefVelocity()
VEC3
goalVector
=
goals_
[
curGoal_
]
-
getPosition
()
;
float
goalDist2
=
goalVector
.
norm2
()
;
if
(
goalDist2
<
35.0
f
)
if
(
goalDist2
<
radius_
*
radius_
)
{
// if(color1==0)
// {
// if(color2==0)
// {
// color3-=0.5f;
// color1+=0.5f;
// }
// else {
// color2-=0.5f;
// color3+=0.5f;
// }
// }
// else
// {
// if(color3!=0)
// {
// color3-=0.5f;
// color1+=0.5f;
// }
// else
// {
// color1-=0.5f;color2+=0.5f;
// }
//
// }
// curGoal_ = (curGoal_ + 1) % goals_.size();
#ifdef SPATIAL_HASHING
curGoal_
=
(
curGoal_
+
1
)
%
goals_
.
size
()
;
goalVector
=
goals_
[
curGoal_
]
-
getPosition
()
;
#else
VEC3
possiblepos
=
goals_
[
curGoal_
]
;
int
x
=
rand
()
%
2
+
1
;
if
(
x
==
2
)
{
possiblepos
[
1
]
*=-
(
x
-
1
)
*
1.0
f
;
x
=
rand
()
%
2
+
1
;
if
(
x
==
2
)
possiblepos
[
0
]
*=-
(
x
-
1
)
*
1.0
f
;
}
else
possiblepos
[
0
]
*=-
1.0
f
;
Dart
d1
=
sim_
->
envMap_
.
getBelongingCell
(
possiblepos
)
;
if
(
sim_
->
envMap_
.
buildingMark
.
isMarked
(
d1
))
{
float
sideSize
=
0.0
f
;
if
(((
possiblepos
[
0
]
+
sideSize
)
<
sim_
->
envMap_
.
max_for_obstacles
)
&&
((
possiblepos
[
1
]
+
sideSize
)
<
sim_
->
envMap_
.
max_for_obstacles
))
possiblepos
+=
VEC3
(
sideSize
,
sideSize
,
0
)
;
else
if
(((
possiblepos
[
0
]
-
sideSize
)
<
sim_
->
envMap_
.
max_for_obstacles
)
&&
((
possiblepos
[
1
]
-
sideSize
)
<
sim_
->
envMap_
.
max_for_obstacles
))
possiblepos
+=
VEC3
(
-
sideSize
,
-
sideSize
,
0
)
;
else
if
(((
possiblepos
[
0
]
-
sideSize
)
<
sim_
->
envMap_
.
max_for_obstacles
)
&&
((
possiblepos
[
1
]
+
sideSize
)
<
sim_
->
envMap_
.
max_for_obstacles
))
possiblepos
+=
VEC3
(
-
sideSize
,
sideSize
,
0
)
;
else
possiblepos
+=
VEC3
(
sideSize
,
-
sideSize
,
0
)
;
}
goals_
[
curGoal_
]
=
possiblepos
;
goalVector
=
goals_
[
curGoal_
]
-
part_
.
getPosition
()
;
#endif
goalDist2
=
goalVector
.
norm2
()
;
}
if
(
goalDist2
>
1.0
f
)
goalVector
.
normalize
()
;
obstacle_priority
(
&
goalVector
)
;
goalDist2
=
goalVector
.
norm2
()
;
if
(
goalDist2
>
1.0
f
)
goalVector
.
normalize
()
;
if
(
goalDist2
>
maxSpeed_
)
{
goalVector
.
normalize
()
;
goalVector
*=
maxSpeed_
;
}
/*
* Perturb a little to avoid deadlocks due to perfect symmetry.
*/
// float angle = std::rand() * 2.0f * M_PI / RAND_MAX;
// float dist = std::rand() * 0.0001f / RAND_MAX;
// prefVelocity_ = goalVector + VEC3(dist * cos(angle), dist * sin(angle), 0.0f);
prefVelocity_
=
goalVector
;
}
...
...
@@ -740,7 +671,7 @@ void Agent::computeNewVelocity()
else
{
/* Project on cut-off circle of time timeStep. */
const
float
invTimeStep
=
1.0
f
/
timeStep
;
const
float
invTimeStep
=
1.0
f
/
sim_
->
timeStep_
;
/* Vector from cutoff center to relative velocity. */
const
VEC3
w
=
relativeVelocity
-
invTimeStep
*
relativePosition
;
...
...
src/env_map.cpp
View file @
76df679c
...
...
@@ -47,10 +47,6 @@ EnvMap::EnvMap() :
void
EnvMap
::
init
(
unsigned
int
config
,
REAL
width
,
REAL
height
,
REAL
minSize
,
REAL
maxSize
)
{
int
nbSquares
=
6
;
float
sideSize
=
400.0
f
;
max_for_obstacles
=
(
sideSize
*
nbSquares
/
2
)
-
sideSize
/
2
;
std
::
cout
<<
"Init EnvMap"
<<
std
::
endl
;
VEC3
bottomLeft
(
-
width
/
2
,
-
height
/
2
,
0.0
f
)
;
VEC3
topRight
(
width
/
2
,
height
/
2
,
0.0
f
)
;
...
...
@@ -95,8 +91,7 @@ void EnvMap::init(unsigned int config, REAL width, REAL height, REAL minSize, RE
CityGenerator
::
generateCity
<
PFP
>
(
*
this
)
;
break
;
case
4
:
CityGenerator
::
generatePlanet
<
PFP
>
(
map
,
position
,
obstacleMark
,
buildingMark
,
200.0
f
,
nbSquares
)
;
CityGenerator
::
generatePlanet
<
PFP
>
(
*
this
)
;
break
;
}
...
...
src/moving_obstacle.cpp
View file @
76df679c
#include "moving_obstacle.h"
#include "obstacle.h"
#include "agent.h"
#include "simulator.h"
//float MovingObstacle::neighborDistSq_ = 5.0f * 5.0f;
float
MovingObstacle
::
timeStep
=
0.25
f
;
float
MovingObstacle
::
maxSpeed_
=
1.0
f
;
float
MovingObstacle
::
timeHorizonObst_
=
10.0
f
;
...
...
@@ -63,10 +63,10 @@ VEC3 rotate(VEC3 pos1, VEC3 center, float angle)
return
pos2
;
}
MovingObstacle
::
MovingObstacle
(
int
ind
,
EnvMap
*
envmap
,
std
::
vector
<
VEC3
>
pos
,
VEC3
goal
,
float
rota
)
:
MovingObstacle
::
MovingObstacle
(
Simulator
*
sim
,
int
ind
,
std
::
vector
<
VEC3
>
pos
,
VEC3
goal
,
float
rota
)
:
index
(
ind
),
newVelocity_
(
0
),
envMap_
(
envmap
)
sim_
(
sim
)
{
assert
(
pos
.
size
()
>
2
);
...
...
@@ -92,9 +92,9 @@ MovingObstacle::MovingObstacle(int ind, EnvMap* envmap, std::vector<VEC3> pos, V
sum
+=
pos
[
i
];
// CGoGNout << "Au début : Obstacle "<< o << " position :" << pos[i] << CGoGNendl;
Dart
d
=
envMap_
->
getBelongingCell
(
pos
[
i
]);
Dart
d
=
sim_
->
envMap_
.
getBelongingCell
(
pos
[
i
]);
CGoGN
::
Algo
::
MovingObjects
::
ParticleCell2D
<
PFP
>*
part
=
new
CGoGN
::
Algo
::
MovingObjects
::
ParticleCell2D
<
PFP
>
(
envMap_
->
map
,
d
,
pos
[
i
],
envMap_
->
position
);
new
CGoGN
::
Algo
::
MovingObjects
::
ParticleCell2D
<
PFP
>
(
sim_
->
envMap_
.
map
,
d
,
pos
[
i
],
sim_
->
envMap_
.
position
);
parts_
[
i
]
=
part
;
}
...
...
@@ -120,7 +120,7 @@ MovingObstacle::MovingObstacle(int ind, EnvMap* envmap, std::vector<VEC3> pos, V
gravity_dist
=
pos_max
+
5.0
f
;
obstacle_range
=
15.0
f
*
15.0
f
;
make_half_turn
=
get_angle
(
finalGoal
-
center
,
parts_
[
0
]
->
getPosition
()
-
center
)
*
nbVertices
;
make_half_turn
=
get_angle
(
finalGoal
-
center
,
(
parts_
[
0
]
->
getPosition
()
+
parts_
[
1
]
->
getPosition
())
/
2
-
center
)
*
nbVertices
;
}
bool
MovingObstacle
::
test_opposition
(
VEC3
o
,
VEC3
p1
,
VEC3
p2
)
...
...
@@ -143,7 +143,7 @@ void MovingObstacle::updateFixedObstNeighbors()
if
(
k
==
2
)
k
=
nbVertices
-
1
;
std
::
vector
<
Obstacle
*>&
obst
=
envMap_
->
obstvect
[
parts_
[
k
]
->
d
];
std
::
vector
<
Obstacle
*>&
obst
=
sim_
->
envMap_
.
obstvect
[
parts_
[
k
]
->
d
];
//search all obstacles around
for
(
std
::
vector
<
Obstacle
*>::
const_iterator
it
=
obst
.
begin
();
it
!=
obst
.
end
();
++
it
)
...
...
@@ -161,19 +161,22 @@ void MovingObstacle::updateFixedObstNeighbors()
if
(
test_opposition
(
toto_norm
,
parts_
[
0
]
->
getPosition
(),
center
))
//// à changer ////////////
{
int
co
=
rand
()
%
2
+
1
;
if
(
co
==
2
)
co
=-
1
;
int
co
=
rand
()
%
2
;
if
(
toto
[
0
]
==
0
)
{
finalGoal
[
0
]
=
parts_
[
0
]
->
getPosition
()[
0
];
finalGoal
[
1
]
=
co
*
envMap_
->
max_for_obstacles
;
if
(
co
==
0
)
finalGoal
[
0
]
=
sim_
->
envMap_
.
geometry
.
max
()[
1
]
-
maxNeighbors_
;
else
finalGoal
[
0
]
=
sim_
->
envMap_
.
geometry
.
min
()[
1
]
+
maxNeighbors_
;
}
else
{
finalGoal
[
1
]
=
parts_
[
0
]
->
getPosition
()[
1
];
finalGoal
[
0
]
=
co
*
envMap_
->
max_for_obstacles
;
if
(
co
==
0
)
finalGoal
[
0
]
=
sim_
->
envMap_
.
geometry
.
max
()[
1
]
-
maxNeighbors_
;
else
finalGoal
[
0
]
=
sim_
->
envMap_
.
geometry
.
min
()[
1
]
+
maxNeighbors_
;
}
float
angle
=
get_angle
(
finalGoal
-
center
,(
parts_
[
0
]
->
getPosition
())
-
center
);
...
...
@@ -243,7 +246,7 @@ void MovingObstacle::update()
{
target
+=
rotate
(
parts_
[
i
]
->
getPosition
(),
center
,
rot
);
}
target
+=
(
velocity_
*
timeStep
);
target
+=
(
velocity_
*
sim_
->
timeStep_
);
}
else
...
...
@@ -311,24 +314,24 @@ void MovingObstacle::register_(Obstacle* o, Dart d, int n)
VEC3
stop
=
o
->
p2
;
CGoGN
::
Algo
::
MovingObjects
::
ParticleCell2DMemo
<
PFP
>
*
part
=
new
CGoGN
::
Algo
::
MovingObjects
::
ParticleCell2DMemo
<
PFP
>
(
envMap_
->
map
,
d
,
start
,
envMap_
->
position
);
new
CGoGN
::
Algo
::
MovingObjects
::
ParticleCell2DMemo
<
PFP
>
(
sim_
->
envMap_
.
map
,
d
,
start
,
sim_
->
envMap_
.
position
);
part
->
move
(
stop
);
belonging_cells
[
n
]
=
part
->
memo_cross
;
envMap_
->
addObstAsNeighbor
(
o
,
belonging_cells
[
n
],
&
(
neighbor_cells
[
n
]));
sim_
->
envMap_
.
addObstAsNeighbor
(
o
,
belonging_cells
[
n
],
&
(
neighbor_cells
[
n
]));
// CGoGNout << "coucou je suis sorti de addNeighbor" << CGoGNendl;
for
(
std
::
list
<
Dart
>::
iterator
it
=
part
->
memo_cross
.
begin
();
it
!=
part
->
memo_cross
.
end
();
++
it
)
{
general_belonging
.
push_back
(
*
it
);
envMap_
->
pushObstacleInCells
(
o
,
*
it
);
sim_
->
envMap_
.
pushObstacleInCells
(
o
,
*
it
);
}
for
(
std
::
list
<
Dart
>::
iterator
it
=
this
->
neighbor_cells
[
n
].
begin
();
it
!=
this
->
neighbor_cells
[
n
].
end
();
++
it
)
{
this
->
envMap_
->
pushObstNeighborInCells
(
o
,
*
it
);
sim_
->
envMap_
.
pushObstNeighborInCells
(
o
,
*
it
);
}
}
...
...
@@ -340,22 +343,22 @@ void register_pop(Obstacle* o, int n)
{
for
(
std
::
list
<
Dart
>::
iterator
it
=
mo
->
belonging_cells
[
n
].
begin
();
it
!=
mo
->
belonging_cells
[
n
].
end
();
++
it
)
{
mo
->
envMap_
->
popObstacleInCells
(
o
,
*
it
);
mo
->
sim_
->
envMap_
.
popObstacleInCells
(
o
,
*
it
);
}
for
(
std
::
list
<
Dart
>::
iterator
it
=
mo
->
neighbor_cells
[
n
].
begin
();
it
!=
mo
->
neighbor_cells
[
n
].
end
();
++
it
)
{
mo
->
envMap_
->
popObstNeighborInCells
(
o
,
*
it
);
mo
->
sim_
->
envMap_
.
popObstNeighborInCells
(
o
,
*
it
);
}
}
}
void
MovingObstacle
::
register_oneRingAdd
(
Obstacle
*
o
,
Dart
d
,
int
n
)
{
assert
(
this
->
envMap_
->
map
.
getCurrentLevel
()
==
this
->
envMap_
->
map
.
getMaxLevel
());
assert
(
std
::
find
(
this
->
envMap_
->
obstvect
[
d
].
begin
(),
this
->
envMap_
->
obstvect
[
d
].
end
(),
o
)
==
this
->
envMap_
->
obstvect
[
d
].
end
());
assert
(
sim_
->
envMap_
.
map
.
getCurrentLevel
()
==
sim_
->
envMap_
.
map
.
getMaxLevel
());
assert
(
std
::
find
(
sim_
->
envMap_
.
obstvect
[
d
].
begin
(),
sim_
->
envMap_
.
obstvect
[
d
].
end
(),
o
)
==
sim_
->
envMap_
.
obstvect
[
d
].
end
());
envMap_
->
pushObstacleInCells
(
o
,
d
);
sim_
->
envMap_
.
pushObstacleInCells
(
o
,
d
);
belonging_cells
[
n
].
clear
();
belonging_cells
[
n
].
push_back
(
d
);
...
...
@@ -364,15 +367,15 @@ void MovingObstacle::register_oneRingAdd(Obstacle * o, Dart d, int n)
Dart
dd
=
d
;
do
{
Dart
ddd
=
this
->
envMap_
->
map
.
alpha1
(
this
->
envMap_
->
map
.
alpha1
(
dd
));
Dart
ddd
=
sim_
->
envMap_
.
map
.
alpha1
(
sim_
->
envMap_
.
map
.
alpha1
(
dd
));
while
(
ddd
!=
dd
)
{
envMap_
->
pushObstNeighborInCells
(
o
,
ddd
);
sim_
->
envMap_
.
pushObstNeighborInCells
(
o
,
ddd
);
this
->
neighbor_cells
[
n
].
push_back
(
ddd
);
ddd
=
this
->
envMap_
->
map
.
alpha1
(
ddd
);
ddd
=
sim_
->
envMap_
.
map
.
alpha1
(
ddd
);
}
dd
=
this
->
envMap_
->
map
.
phi1
(
dd
);
dd
=
sim_
->
envMap_
.
map
.
phi1
(
dd
);
}
while
(
dd
!=
d
);
}
...
...
@@ -386,15 +389,15 @@ void register_add(Obstacle* o, int n, const std::list<Dart>& memo_cross)
for
(
std
::
list
<
Dart
>::
iterator
it
=
mo
->
belonging_cells
[
n
].
begin
();
it
!=
mo
->
belonging_cells
[
n
].
end
();
++
it
)
{
// CGoGNout <<"obstacle"<< o->p1 << "new cell : "<< *it << CGoGNendl;
mo
->
envMap_
->
pushObstacleInCells
(
o
,
*
it
);
mo
->
sim_
->
envMap_
.
pushObstacleInCells
(
o
,
*
it
);
}
mo
->
envMap_
->
addObstAsNeighbor
(
o
,
mo
->
belonging_cells
[
n
],
&
(
mo
->
neighbor_cells
[
n
]));
mo
->
sim_
->
envMap_
.
addObstAsNeighbor
(
o
,
mo
->
belonging_cells
[
n
],
&
(
mo
->
neighbor_cells
[
n
]));
for
(
std
::
list
<
Dart
>::
iterator
it
=
mo
->
neighbor_cells
[
n
].
begin
();
it
!=
mo
->
neighbor_cells
[
n
].
end
();
++
it
)
{
// CGoGNout <<"obstacle"<< o->p1 << "new cell : "<< *it << CGoGNendl;
mo
->
envMap_
->
pushObstNeighborInCells
(
o
,
*
it
);
mo
->
sim_
->
envMap_
.
pushObstNeighborInCells
(
o
,
*
it
);
}
}
...
...
@@ -405,7 +408,7 @@ std::list<Dart> MovingObstacle::getMemoCross(const Algo::MovingObjects::Particle
const
VEC3
&
start
=
p1
->
getPosition
();
const
VEC3
&
stop
=
p2
->
getPosition
();
CGoGN
::
Algo
::
MovingObjects
::
ParticleCell2DMemo
<
PFP
>
part
(
envMap_
->
map
,
d
,
start
,
envMap_
->
position
);
CGoGN
::
Algo
::
MovingObjects
::
ParticleCell2DMemo
<
PFP
>
part
(
sim_
->
envMap_
.
map
,
d
,
start
,
sim_
->
envMap_
.
position
);
part
.
move
(
stop
);
return
part
.
memo_cross
;
...
...
@@ -427,7 +430,7 @@ void MovingObstacle::register_update(Obstacle* o, Dart d, int n)
memo
.
sort
();
modif
=
true
;
}
else
if
(
!
envMap_
->
map
.
sameFace
(
p1
->
d
,
p2
->
d
))
else
if
(
!
sim_
->
envMap_
.
map
.
sameFace
(
p1
->
d
,
p2
->
d
))
{
memo
=
getMemoCross
(
p1
,
p2
);
memo
.
sort
();
...
...
@@ -439,7 +442,7 @@ void MovingObstacle::register_update(Obstacle* o, Dart d, int n)
{
register_pop
(
o
,
n
);
if
(
envMap_
->
map
.
sameFace
(
p1
->
d
,
p2
->
d
))
if
(
sim_
->
envMap_
.
map
.
sameFace
(
p1
->
d
,
p2
->
d
))
{
register_oneRingAdd
(
o
,
p1
->
d
,
n
);
}
...
...
@@ -479,8 +482,8 @@ void resetObstInFace(Obstacle* o)
if
(
mo
!=
NULL
)
{
VEC3
pos
=
o
->
p1
;
mo
->
parts_
[
ind
]
->
ParticleBase
<
PFP
>::
move
(
Algo
::
Geometry
::
faceCentroid
<
PFP
>
(
mo
->
envMap_
->
map
,
mo
->
parts_
[
ind
]
->
d
,
mo
->
envMap_
->
position
));
mo
->
parts_
[
ind
]
->
ParticleBase
<
PFP
>::
move
(
Algo
::
Geometry
::
faceCentroid
<
PFP
>
(
mo
->
sim_
->
envMap_
.
map
,
mo
->
parts_
[
ind
]
->
d
,
mo
->
sim_
->
envMap_
.
position
));
mo
->
parts_
[
ind
]
->
setState
(
FACE
);
mo
->
parts_
[
ind
]
->
move
(
pos
);
}
...
...
@@ -494,7 +497,7 @@ void resetObstPartInFace(Obstacle* o, Dart d1)
VEC3
pos1
=
o
->
p1
;
if
(
Algo
::
Geometry
::
isPointInConvexFace2D
<
PFP
>
(
mo
->
envMap_
->
map
,
d1
,
mo
->
envMap_
->
position
,
pos1
,
true
))
{
>
(
mo
->
sim_
->
envMap_
.
map
,
d1
,
mo
->
sim_
->
envMap_
.
position
,
pos1
,
true
))
{
mo
->
parts_
[
ind
]
->
d
=
d1
;
}
...
...
@@ -505,7 +508,7 @@ void resetPart(MovingObstacle * mo, Dart d1)
{
for
(
unsigned
int
i
=
0
;
i
<
mo
->
nbVertices
;
i
++
)
{
if
(
mo
->
parts_
[
i
]
->
d
==
mo
->
envMap_
->
map
.
phi1
(
d1
))
if
(
mo
->
parts_
[
i
]
->
d
==
mo
->
sim_
->
envMap_
.
map
.
phi1
(
d1
))
mo
->
parts_
[
i
]
->
d
=
d1
;
}
}
...
...
src/simulator.cpp
View file @
76df679c
...
...
@@ -27,7 +27,7 @@ Simulator::~Simulator()
void
Simulator
::
init
(
unsigned
int
config
,
int
minSize
,
float
dimension
,
bool
enablePathFinding
)
{
std
::
cout
<<
"Setup scenario"
<<
std
::
endl
;
envMap_
.
init
(
config
,
2500.0
f
,
2000.0
f
,
minSize
,
20
0.0
f
)
;
envMap_
.
init
(
config
,
1500.0
f
,
1000.0
f
,
minSize
,
5
0.0
f
)
;
switch
(
config
)
{
...
...
@@ -35,7 +35,7 @@ void Simulator::init(unsigned int config, int minSize, float dimension, bool ena
setupCircleScenario
(
100
)
;
break
;
case
1
:
setupCorridorScenario
(
5
00
,
10
)
;
setupCorridorScenario
(
1
00
,
10
)
;
break
;
case
2
:
setupScenario
(
1000
)
;
...
...
@@ -196,7 +196,7 @@ void Simulator::setupCircleScenario(unsigned int nbAgents)
int
radius
=
min
(
xSize
,
ySize
)
/
2
;
VEC3
center
=
(
envMap_
.
geometry
.
min
()
+
envMap_
.
geometry
.
max
())
/
2
;
float
pi
=
3.14159265358979323846
f
;
double
pi
=
3.14159265358979323846
f
;
for
(
unsigned
int
i
=
0
;
i
<
nbAgents
;
++
i
)
{
...
...
@@ -262,8 +262,6 @@ void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObst
addAgent
(
start
,
goal
)
;
}
std
::
cout
<<
"nombre d'agents : "
<<
agents_
.
size
()
<<
std
::
endl
;
// Départ des obstacles du quart haut sur toute une demi-largeur
xStartMin
=
envMap_
.
geometry
.
min
()[
0
]
+
envMap_
.
geometry
.
size
(
0
)
/
4
;
xStartDelta
=
envMap_
.
geometry
.
size
(
0
)
/
2
;
...
...
@@ -285,6 +283,7 @@ void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObst
VEC3
start
(
x
,
yStartMin
+
rand
()
%
yStartDelta
,
0
)
;
VEC3
goal
(
x
,
yGoalMin
+
rand
()
%
yGoalDelta
,
0
)
;
vPos
.
clear
();
// Un obstacle sur deux va vers le haut
VEC3
tmp
;
if
(
i
%
2
==
1
)
...
...
@@ -292,18 +291,20 @@ void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObst
tmp
=
goal
;
goal
=
start
;
start
=
tmp
;
vPos
.
push_back
(
start
-
xSide
+
ySide
);
vPos
.
push_back
(
start
-
xSide
-
ySide
);
vPos
.
push_back
(
start
+
xSide
-
ySide
);
vPos
.
push_back
(
start
+
xSide
+
ySide
);
}
vPos
.
clear
();
vPos
.
push_back
(
start
-
xSide
-
ySide
);
vPos
.
push_back
(
start
+
xSide
-
ySide
);
vPos
.
push_back
(
start
+
xSide
+
ySide
);
vPos
.
push_back
(
start
-
xSide
+
ySide
);
mo4
=
new
MovingObstacle
(
i
,
&
envMap_
,
vPos
,
goal
,
0
);
else
{
vPos
.
push_back
(
start
+
xSide
-
ySide
);
vPos
.
push_back
(
start
+
xSide
+
ySide
);
vPos
.
push_back
(
start
-
xSide
+
ySide
);
vPos
.
push_back
(
start
-
xSide
-
ySide
);
}
mo4
=
new
MovingObstacle
(
this
,
i
,
vPos
,
goal
,
0
);
movingObstacles_
.
push_back
(
mo4
);
}
std
::
cout
<<
"nombre d'obstacles : "
<<
movingObstacles_
.
size
()
<<
std
::
endl
;
}
void
Simulator
::
setupCityScenario
(
int
nbLines
,
int
nbRank
)
...
...
@@ -421,16 +422,6 @@ void Simulator::setupScenario(unsigned int nbMaxAgent)
agents_
.
back
()
->
goals_
.
push_back
(
posagent
)
;
agents_
.
back
()
->
goals_
.
push_back
(
-
1.0
f
*
posagent
)
;
// TODO Code Thomas à vérifier
// VEC3 oppos = -1.0f * posagent;
// Dart d1 =envMap_.getBelongingCell(oppos);
//
// if (!envMap_.buildingMark.isMarked(d1))
// agents_.back()->goals_.push_back(oppos);
// else if (oppos[0]+50 <envMap_.max_for_obstacles && oppos[1]+50 <envMap_.max_for_obstacles)
// agents_.back()->goals_.push_back(oppos + VEC3(50,50,0));
// else
// agents_.back()->goals_.push_back(oppos + VEC3(-50,-50,0));
agents_
.
back
()
->
curGoal_
=
1
;
}
}
...
...
@@ -699,89 +690,3 @@ Geom::BoundingBox<VEC3> Simulator::getAgentsBB()
return
bb
;
}
//void Simulator::setupMovingObstacle(unsigned int nb_obst)
//{
// float xSize = 10.0f ;
// float ySize = 45.0f ;
// VEC3 goal(50.0f, 50.0f, 0) ;
// VEC3 posInit(0, 0, 0) ;
// VEC3 possiblepos(0, 0, 0) ;
// std::vector<VEC3> vPos ;
//
// MovingObstacle* mo4 ;