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
SocialAgents3D
Commits
a89ac220
Commit
a89ac220
authored
Dec 18, 2012
by
Arash HABIBI
Browse files
Voici donc un commentaire :
parent
d380cc70
Changes
6
Hide whitespace changes
Inline
Side-by-side
include/agent.h
View file @
a89ac220
...
...
@@ -57,7 +57,11 @@ public:
unsigned
int
curGoal_
;
MovingObstacle
**
movingObstacles_
;
int
nb_mos
;
static
unsigned
int
maxNeighbors_
;
static
unsigned
int
maxMovingObstacles_
;
static
float
maxSpeed_
;
static
float
neighborDist_
;
static
float
neighborDistSq_
;
...
...
include/moving_obstacle.h
View file @
a89ac220
...
...
@@ -44,6 +44,8 @@ public:
std
::
vector
<
Dart
>
*
neighbor_cells
;
std
::
set
<
Dart
>
general_belonging
;
VEC3
front
;
VEC3
focus1
,
focus2
;
double
length
,
width
,
sum_dist_foci
,
sum_dist_foci_rest
;
VEC3
finalGoal
;
float
angle
;
...
...
src/agent.cpp
View file @
a89ac220
...
...
@@ -10,13 +10,15 @@
#include
"simulator.h"
unsigned
int
Agent
::
maxNeighbors_
=
10
;
unsigned
int
Agent
::
maxMovingObstacles_
=
10
;
float
Agent
::
maxSpeed_
=
2.0
f
;
float
Agent
::
neighborDist_
=
10.0
f
;
float
Agent
::
neighborDistSq_
=
neighborDist_
*
neighborDist_
;
float
Agent
::
radius_
=
1.5
f
;
float
Agent
::
timeHorizon_
=
10.0
f
;
float
Agent
::
timeHorizonObst_
=
10.0
f
;
float
Agent
::
range_
=
(
timeHorizonObst_
*
maxSpeed_
+
radius_
)
;
float
Agent
::
range_
=
20
*
timeHorizonObst_
*
maxSpeed_
+
radius_
;
// float Agent::range_ = timeHorizonObst_ * maxSpeed_ + radius_ ;
float
Agent
::
rangeSq_
=
range_
*
range_
;
unsigned
int
Agent
::
cptAgent
=
0
;
...
...
@@ -34,6 +36,8 @@ Agent::Agent(Simulator* sim, const VEC3& position) :
{
agentNeighbors_
.
reserve
(
maxNeighbors_
*
2
)
;
obstacleNeighbors_
.
reserve
(
maxNeighbors_
*
2
)
;
movingObstacles_
=
new
MovingObstacle
*
[
maxMovingObstacles_
];
nb_mos
=
0
;
agentNo
=
cptAgent
++
;
}
#else
...
...
@@ -53,6 +57,8 @@ Agent::Agent(Simulator* sim, const VEC3& position, Dart d) :
agentNeighbors_
.
reserve
(
maxNeighbors_
*
2
)
;
obstacleNeighbors_
.
reserve
(
maxNeighbors_
*
2
)
;
movingObstacleNeighbors_
.
reserve
(
maxNeighbors_
*
2
)
;
movingObstacles_
=
new
MovingObstacle
*
[
maxMovingObstacles_
];
nb_mos
=
0
;
agentNo
=
cptAgent
++
;
}
#endif
...
...
@@ -401,22 +407,56 @@ void Agent::computePrefVelocity()
prefVelocity_
=
goalVector
;
}
//-----------------------------------------------------------------
static
int
moAppend
(
MovingObstacle
**
moving_obstacles
,
int
nb_mos
,
int
max
,
MovingObstacle
*
mo
)
{
unsigned
char
already_processed
=
false
;
int
mo_count
;
if
(
nb_mos
>=
max
-
1
)
{
// cerr << "Agent::moPush : the maximum number of moving obstacles is assumed to be " << max << endl;
// cerr << "There is visibly a need for more." << endl;
}
else
{
for
(
mo_count
=
0
;
(
mo_count
<
nb_mos
)
&&
(
already_processed
==
false
);
mo_count
++
)
if
(
moving_obstacles
[
mo_count
]
==
mo
)
already_processed
=
true
;
if
(
mo_count
==
nb_mos
)
{
moving_obstacles
[
mo_count
]
=
mo
;
nb_mos
++
;
}
}
return
(
nb_mos
);
}
//-----------------------------------------------------------------
//-----------------------------------------------------------------
//-----------------------------------------------------------------
// Search for the best new velocity.
void
Agent
::
computeNewVelocity
()
// RVO2 : évitement agents entres eux et avec les obstacles fixes + ajout obstacles mobiles
void
Agent
::
computeNewVelocity
()
{
// The objective is to compute the sum of forces exerted on the agent.
double
collision_softening_factor
;
double
ag_mass
=
1.0
;
float
ag_ambient_damping
=
1.0
;
double
mass_var
=
0.9
;
double
average_mass
=
1.0
;
srand48
(
agentNo
);
double
rand
=
2.0
*
drand48
()
-
1.0
;
// compris entre -1 et 1
// double ag_mass = average_mass + rand*rand*rand*mass_var; // valeurs plus denses autour de la moyenne
double
ag_mass
=
average_mass
+
rand
*
mass_var
;
// valeurs uniformement réparties entre min et max
//-------------
VEC3
p1
,
p2
,
vec
,
forces
,
previous_pos
;
VEC3
vec
,
forces
,
previous_pos
;
forces
.
zero
();
previous_pos
=
getPosition
()
-
velocity_
*
sim_
->
timeStep_
;
...
...
@@ -433,43 +473,48 @@ void Agent::computeNewVelocity() // RVO2 : évitement agents entres eux et avec
//----- forces dues à la répulsion des obstacles ----------
VEC3
norm
;
double
obst_stiffness
=
50.0
;
// agent-obstacle interaction stiffness
// double obst_damping
= 1.0; // agent-obstacle interaction damping
int
obst_power
=
4
;
// the power to which elevate the agent-obstacle distance
double
obst_stiffness
=
50
00
.0
;
// agent-obstacle interaction stiffness
// double obst_damping = 1.0; // agent-obstacle interaction damping
int
obst_power
=
1
;
// the power to which elevate the agent-obstacle distance
Obstacle
*
obst
;
nb_mos
=
0
;
for
(
std
::
vector
<
std
::
pair
<
float
,
Obstacle
*>
>::
iterator
it
=
movingObstacleNeighbors_
.
begin
()
;
it
!=
movingObstacleNeighbors_
.
end
()
;
++
it
)
{
double
dist
=
it
->
first
;
// double effective_range = 50*range_;
double
effective_range
=
10
*
range_
;
float
force_value
=
0.0
;
if
(
dist
<
effective_range
)
obst
=
it
->
second
;
nb_mos
=
moAppend
(
movingObstacles_
,
nb_mos
,
maxMovingObstacles_
,
obst
->
mo
);
}
MovingObstacle
*
mo
;
float
force_value
;
int
mo_count
;
for
(
mo_count
=
0
;
mo_count
<
nb_mos
;
mo_count
++
)
{
mo
=
movingObstacles_
[
mo_count
];
float
dist
=
(
getPosition
()
-
mo
->
focus2
).
norm
()
+
(
getPosition
()
-
mo
->
focus1
).
norm
();
// double effective_range = 3*mo->sum_dist_foci;
double
effective_range
=
mo
->
sum_dist_foci
;
if
(
dist
<=
effective_range
)
{
collision_softening_factor
=
pow
(
1
-
dist
/
effective_range
,
obst_power
);
force_value
=
obst_stiffness
*
collision_softening_factor
*
(
effective_range
-
dist
);
}
obst
=
it
->
second
;
p1
=
obst
->
p1
;
p2
=
obst
->
p2
;
vec
=
p2
-
p1
;
vec
.
normalize
();
norm
.
zero
();
if
(
sim_
->
avoidance
==
0
)
// avoids with normal of obstacle side
{
norm
[
0
]
=
vec
[
1
]
;
norm
[
1
]
=-
vec
[
0
]
;
norm
.
zero
();
// norm = (getPosition()-mo->focus2) + (getPosition()-mo->focus1);
// norm.normalize();
VEC3
vec1
=
getPosition
()
-
mo
->
focus1
;
vec1
.
normalize
();
VEC3
vec2
=
getPosition
()
-
mo
->
focus2
;
vec2
.
normalize
();
norm
=
vec1
+
vec2
;
norm
.
normalize
();
forces
+=
force_value
*
norm
;
}
else
if
(
sim_
->
avoidance
==
1
)
// avoids with direction from center of the obstacle
{
MovingObstacle
*
mo
=
obst
->
mo
;
norm
=
this
->
part_
.
getPosition
()
-
mo
->
center
;
}
forces
+=
force_value
*
norm
;
}
//----- forces dues à la répulsion des autres agents -------
...
...
@@ -477,7 +522,7 @@ void Agent::computeNewVelocity() // RVO2 : évitement agents entres eux et avec
double
ag_stiffness
=
20.0
;
// agent-agent interaction stiffness
double
ag_damping
=
1.0
;
// agent-agent interaction damping
double
ag_phys_radius_coef
=
20.0
;
int
ag_power
=
8
;
// the power to which elevate the agent-agent distance
double
ag_power
=
8
;
// the power to which elevate the agent-agent distance
unsigned
int
nbA
=
0
;
...
...
@@ -529,6 +574,32 @@ void Agent::computeNewVelocity() // RVO2 : évitement agents entres eux et avec
velocity_tmp
*=
maxSpeed_
;
}
newVelocity_
=
velocity_tmp
;
//------- color depending on velocity -------------------------------
double
vmax
=
0.5
*
maxSpeed_
;
VEC3
min_vx_color
(
1.0
,
0.0
,
0.0
);
// agents going towards the positive xs are red
VEC3
max_vx_color
(
0.0
,
0.1
,
0.1
);
// agents going towards the negative xs are cyan
VEC3
min_vy_color
(
0.0
,
1.0
,
0.0
);
VEC3
max_vy_color
(
1.0
,
0.0
,
1.1
);
double
alpha_x
=
(
newVelocity_
[
0
]
+
vmax
)
/
(
2
*
vmax
);
double
alpha_y
=
(
newVelocity_
[
1
]
+
vmax
)
/
(
2
*
vmax
);
double
tmp_color1
=
abs
(
alpha_x
-
0.5
)
*
(
alpha_x
*
max_vx_color
[
0
]
+
(
1
-
alpha_x
)
*
min_vx_color
[
0
])
+
abs
(
alpha_y
-
0.5
)
*
(
alpha_y
*
max_vy_color
[
0
]
+
(
1
-
alpha_y
)
*
min_vy_color
[
0
]);
double
tmp_color2
=
abs
(
alpha_x
-
0.5
)
*
(
alpha_x
*
max_vx_color
[
1
]
+
(
1
-
alpha_x
)
*
min_vx_color
[
1
])
+
abs
(
alpha_y
-
0.5
)
*
(
alpha_y
*
max_vy_color
[
1
]
+
(
1
-
alpha_y
)
*
min_vy_color
[
1
]);
double
tmp_color3
=
abs
(
alpha_x
-
0.5
)
*
(
alpha_x
*
max_vx_color
[
2
]
+
(
1
-
alpha_x
)
*
min_vx_color
[
2
])
+
abs
(
alpha_y
-
0.5
)
*
(
alpha_y
*
max_vy_color
[
2
]
+
(
1
-
alpha_y
)
*
min_vy_color
[
2
]);
float
blend
=
0.1
;
color1
=
blend
*
tmp_color1
+
(
1
-
blend
)
*
color1
;
color2
=
blend
*
tmp_color2
+
(
1
-
blend
)
*
color2
;
color3
=
blend
*
tmp_color3
+
(
1
-
blend
)
*
color3
;
}
//-----------------------------------------------------------------
...
...
src/moving_obstacle.cpp
View file @
a89ac220
...
...
@@ -102,6 +102,17 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
}
center
=
sum
/
nbVertices
;
front
=
(
vertices
[
1
]
+
vertices
[
2
])
/
2
;
//-------- code ajoute par Arash pour les obstacles rectangulaires --------------
length
=
(
vertices
[
0
]
-
vertices
[
1
]).
norm
();
width
=
(
vertices
[
1
]
-
vertices
[
2
]).
norm
();
sum_dist_foci_rest
=
2
*
(
length
+
width
*
(
sqrt
(
2
)
-
0.5
));
// M appartient à l'ellipse ssi MF1 + MF2 = sum_dist_foci est une constante
// où F1 et F2 sont les deux foyers.
//-------- fin du code ajoute par Arash pour les obstacles rectangulaires -------
if
(
spinning
)
//départ face à la cible en cas d'obstacles pouvant effectuer des rotations
{
angle
=
get_angle
(
goals_
[
curGoal_
]
-
center
,
front
-
center
);
...
...
@@ -136,11 +147,6 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
gravity_dist
=
pos_max
+
5.0
f
;
obstacle_range
=
15.0
f
*
15.0
f
;
}
bool
MovingObstacle
::
test_opposition
(
VEC3
o
,
VEC3
p1
,
VEC3
p2
)
...
...
@@ -345,6 +351,7 @@ void MovingObstacle::update()
bary
+=
target
;
}
front
=
(
vertices
[
1
]
+
vertices
[
2
])
/
2
;
if
(
angle
>
0
)
{
angle
-=
rotor
;
...
...
@@ -356,6 +363,25 @@ void MovingObstacle::update()
center
=
bary
/
nbVertices
;
//-------- code ajoute par Arash pour les obstacles rectangulaires --------------
VEC3
P0_P1
=
vertices
[
1
]
-
vertices
[
0
];
float
velocity_coef
=
50.0
;
if
(
P0_P1
*
velocity_
>
0
)
// P0_P1 est dans le sens de la vitesse
{
focus1
=
center
-
P0_P1
*
(
1
-
(
width
/
length
)
*
(
sqrt
(
2
)
-
0.5
));
focus2
=
center
+
P0_P1
*
(
1
-
(
width
/
length
)
*
(
sqrt
(
2
)
-
0.5
))
+
(
velocity_coef
*
velocity_
);
}
else
{
focus1
=
center
-
P0_P1
*
(
1
-
(
width
/
length
)
*
(
sqrt
(
2
)
-
0.5
))
+
(
velocity_coef
*
velocity_
);
focus2
=
center
+
P0_P1
*
(
1
-
(
width
/
length
)
*
(
sqrt
(
2
)
-
0.5
));
}
sum_dist_foci
=
sum_dist_foci_rest
+
velocity_coef
*
velocity_
.
norm
();
//-------- fin du code ajoute par Arash pour les obstacles rectangulaires -------
// MAJ des obstacles
for
(
unsigned
int
i
=
0
;
i
<
nbVertices
;
++
i
)
{
...
...
src/simulator.cpp
View file @
a89ac220
...
...
@@ -18,8 +18,8 @@ Simulator::Simulator(int minS) :
detect_agent_collision
=
false
;
srand
(
10
)
;
nbStepsPerUnit_
=
1
/
timeStep_
;
//
config=0;
config
=
1
;
config
=
0
;
//
config=1;
init
(
minSize
,
2.0
f
)
;
}
...
...
@@ -168,8 +168,9 @@ void Simulator::doStep()
nbRefineCandidate
+=
envMap_
.
refineCandidate
.
size
()
;
nbCoarsenCandidate
+=
envMap_
.
coarsenCandidate
.
size
()
;
if
(
multires
)
envMap_
.
updateMap
()
;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>><
// if (multires)
// envMap_.updateMap() ;
#endif
globalTime_
+=
timeStep_
;
...
...
src/viewer.cpp
View file @
a89ac220
...
...
@@ -93,6 +93,59 @@ void SocialAgents::cb_initGL()
setParamObject
(
gWidthObj
,
gPosObj
.
data
())
;
}
//-----------------------------------
// An ellipse has two foci placed at
// F1 and F2. The ellipse is the set
// of points M for whom MF1 + MF2 = s
static
void
drawEllipse
(
VEC3
F1
,
VEC3
F2
,
float
s
,
int
N
)
{
float
a
,
b
;
VEC3
O
,
u
,
v
;
O
=
0.5
f
*
(
F1
+
F2
);
u
=
F1
-
F2
;
float
c
=
(
F1
-
F2
).
norm
();
if
(
c
==
0
)
{
u
[
0
]
=
1.0
;
u
[
1
]
=
0.0
;
v
[
0
]
=
0.0
;
v
[
1
]
=
1.0
;
a
=
s
/
2
;
b
=
s
/
2
;
}
else
// c>0
{
u
=
(
1.0
f
/
c
)
*
u
;
v
.
zero
();
v
[
0
]
=
u
[
1
];
v
[
1
]
=
-
u
[
0
];
c
=
0.5
*
c
;
a
=
s
/
2.0
;
if
(
a
<
c
)
{
cerr
<<
"a="
<<
a
<<
" and c="
<<
c
<<
endl
;
cerr
<<
"No such an ellipse is possible"
<<
endl
;
}
else
// a >= c
b
=
sqrt
(
a
*
a
-
c
*
c
);
}
glBegin
(
GL_LINE_LOOP
);
VEC3
M
;
float
dtheta
=
2
*
M_PI
/
N
;
float
theta
;
for
(
theta
=
0
;
theta
<
2
*
M_PI
;
theta
+=
dtheta
)
{
M
=
O
+
a
*
cos
(
theta
)
*
u
+
b
*
sin
(
theta
)
*
v
;
glColor3f
(
0
,
0
,
1
);
glVertex2f
(
M
[
0
],
M
[
1
]);
}
glEnd
();
}
//-----------------------------------
void
SocialAgents
::
cb_redraw
()
{
dock
.
check_slide
->
setRange
(
0
,
sim
.
envMap_
.
map
.
end
().
index
)
;
...
...
@@ -217,6 +270,16 @@ void SocialAgents::cb_redraw()
{
for
(
std
::
vector
<
MovingObstacle
*>::
iterator
it
=
sim
.
movingObstacles_
.
begin
()
;
it
!=
sim
.
movingObstacles_
.
end
()
;
++
it
)
{
// Dessiner les foyers
glBegin
(
GL_POINTS
);
glColor3f
(
1.0
,
0.0
,
0.0
);
glVertex3fv
((
*
it
)
->
focus1
.
data
());
glColor3f
(
0.0
,
1.0
,
0.0
);
glVertex3fv
((
*
it
)
->
focus2
.
data
());
glEnd
();
// 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++)
...
...
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