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
9b8d3a29
Commit
9b8d3a29
authored
Dec 13, 2012
by
pitiot
Browse files
ajout obstacles articulés + evitement arash
parent
6eea98b7
Changes
11
Hide whitespace changes
Inline
Side-by-side
Debug/CMakeLists.txt
View file @
9b8d3a29
...
...
@@ -25,6 +25,7 @@ add_executable( socialAgentsD
../src/env_map.cpp
../src/agent.cpp
../src/moving_obstacle.cpp
../src/articulated_obstacle.cpp
../src/simulator.cpp
../src/moving_mesh.cpp
../src/gl2ps.c
...
...
Release/CMakeLists.txt
View file @
9b8d3a29
...
...
@@ -23,6 +23,7 @@ add_executable( socialAgents
../src/env_map.cpp
../src/agent.cpp
../src/moving_obstacle.cpp
../src/articulated_obstacle.cpp
../src/simulator.cpp
../src/moving_mesh.cpp
../src/gl2ps.c
...
...
include/articulated_obstacle.h
0 → 100644
View file @
9b8d3a29
#ifndef M_ARTICULATED_OBSTACLE_H
#define M_ARTICULATED_OBSTACLE_H
#include
"moving_obstacle.h"
using
namespace
std
;
class
ArticulatedObstacle
{
public:
ArticulatedObstacle
(
Simulator
*
sim
,
int
index
,
int
currentIndex
,
std
::
vector
<
PFP
::
VEC3
>
*
pos
,
int
nbParts
,
std
::
vector
<
VEC3
>
goals
);
std
::
vector
<
MovingObstacle
*>
members
;
int
index
;
int
nbBodyPart
;
std
::
vector
<
VEC3
>
goals
;
VEC3
curGoal
;
};
#endif
include/env_map.h
View file @
9b8d3a29
...
...
@@ -28,6 +28,7 @@ using namespace CGoGN ;
class
Agent
;
class
Obstacle
;
class
MovingObstacle
;
class
ArticulatedObstacle
;
struct
PFP
:
public
PFP_STANDARD
{
...
...
include/moving_obstacle.h
View file @
9b8d3a29
...
...
@@ -10,13 +10,13 @@
using
namespace
std
;
PFP
::
VEC3
rotate
(
PFP
::
VEC3
pos1
,
PFP
::
VEC3
center
,
float
angle
);
float
get_angle
(
PFP
::
VEC3
v1
,
PFP
::
VEC3
v2
);
PFP
::
VEC3
get_center
(
ArticulatedObstacle
*
art
,
int
index
);
class
Simulator
;
class
MovingObstacle
{
public:
MovingObstacle
(
Simulator
*
sim
,
int
index
,
std
::
vector
<
PFP
::
VEC3
>
pos
,
std
::
vector
<
VEC3
>
goals
,
bool
spin
);
MovingObstacle
(
Simulator
*
sim
,
int
index
,
std
::
vector
<
PFP
::
VEC3
>
pos
,
std
::
vector
<
VEC3
>
goals
,
bool
spin
,
ArticulatedObstacle
*
art
=
NULL
,
int
ind2
=-
1
);
bool
test_opposition
(
VEC3
o
,
VEC3
p1
,
VEC3
p2
);
// void contournerBatiment();
void
updateAgentNeighbors
()
;
...
...
@@ -66,6 +66,8 @@ public:
VEC3
prefVelocity_
;
Simulator
*
sim_
;
bool
spinning
;
ArticulatedObstacle
*
parent
;
int
index_parent
;
};
#endif
include/simulator.h
View file @
9b8d3a29
...
...
@@ -7,7 +7,7 @@
#include
"env_map.h"
#include
"agent.h"
#include
"obstacle.h"
#include
"
moving
_obstacle.h"
#include
"
articulated
_obstacle.h"
#include
"moving_mesh.h"
#include
"path_finder.h"
...
...
@@ -98,6 +98,7 @@ public:
void
setupCircleScenario
(
unsigned
int
nbAgents
,
unsigned
int
nbObstacles
)
;
void
setupCorridorScenario
(
unsigned
int
nbAgents
,
unsigned
int
nbObstacles
)
;
void
setupSnakeCorridorScenario
(
unsigned
int
nbAgents
,
unsigned
int
nbSnakes
,
int
snakeSize
)
;
void
setupCityScenario
(
int
nbLines
,
int
nbRank
)
;
void
setupScenario
(
unsigned
int
nbMaxAgent
)
;
...
...
src/agent.cpp
View file @
9b8d3a29
...
...
@@ -338,6 +338,132 @@ void Agent::computePrefVelocity() // calcul vitesse optimale pour atteindre l'ob
}
//void Agent::computeNewVelocity() // de Arash
//{
//
// // 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;
//
// //-------------
//
// VEC3 p1, p2, vec, forces, previous_pos;
// forces.zero();
//
// previous_pos = getPosition() - velocity_*sim_->timeStep_;
//
// //----- force due à l'attraction de l'objectif ----------
//
// float goal_attraction_force = 200.0; // agent-goal interaction stiffness
// VEC3 p_goal = goals_[curGoal_];
// VEC3 u_goal = p_goal - getPosition();
// u_goal.normalize();
//
// forces += goal_attraction_force * u_goal;
//
// //----- 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
// Obstacle* obst ;
//
// 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)
// {
// 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] ;
// }
// 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 -------
//
// 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
//
// unsigned int nbA = 0 ;
//
// for (std::vector<std::pair<float, Agent*> >::iterator it = agentNeighbors_.begin() ;
// it != agentNeighbors_.end() && nbA < maxNeighbors_ ; ++it, ++nbA)
// {
// Agent* other = it->second ;
//
// const VEC3 relativePosition(other->getPosition()-getPosition());
// const float distSq = relativePosition.norm2() ;
// const float dist = sqrt(distSq);
// const float combinedRadius = ag_phys_radius_coef*(radius_ + other->radius_) ;
// // const float combinedRadiusSq = combinedRadius * combinedRadius ;
//
// VEC3 other_previous_pos = other->getPosition() - (other->velocity_*sim_->timeStep_);
// VEC3 previous_relativePosition = other_previous_pos - previous_pos;
// float previous_distSq = previous_relativePosition.norm2();
// float previous_dist = sqrt(previous_distSq);
//
// // const VEC3 u_other(relativePosition);
// VEC3 u_other(relativePosition);
// u_other = relativePosition;
// u_other.normalize();
//
// // cerr << "dist=" << dist << " combinedRadius=" << combinedRadius << endl;
//
// if(dist < combinedRadius)
// {
// collision_softening_factor = pow(1 - dist/combinedRadius,ag_power);
// float force_value = - ag_stiffness*collision_softening_factor*(combinedRadius-dist)
// - ag_damping * (dist - previous_dist) / sim_->timeStep_;
//
// forces += force_value * u_other;
// }
// }
//
// //------- calcul de la trainee --------------------------------------
//
// forces -= ag_ambient_damping * velocity_;
//
// //------- calcul de la nouvelle valeur de la vitesse ----------------
//
// VEC3 velocity_tmp;
//
// velocity_tmp = velocity_ + forces * (sim_->timeStep_ / ag_mass);
// if(velocity_tmp.norm2() > maxSpeed_*maxSpeed_)
// {
// velocity_tmp.normalize();
// velocity_tmp *= maxSpeed_;
// }
// newVelocity_ = velocity_tmp;
//}
/* Search for the best new velocity. */
void
Agent
::
computeNewVelocity
()
// RVO2 : évitement agents entres eux et avec les obstacles fixes + ajout obstacles mobiles
{
...
...
@@ -779,7 +905,7 @@ bool Agent::linearProgram1(const std::vector<Line>& lines, unsigned int lineNo,
unsigned
int
Agent
::
linearProgram2
(
const
std
::
vector
<
Line
>&
lines
,
float
radius
,
const
VEC3
&
optVelocity
,
bool
directionOpt
,
VEC3
&
result
)
{
/*
/*
* Optimize direction. Note that the optimization velocity is of unit
* length in this case.
*/
...
...
src/articulated_obstacle.cpp
0 → 100644
View file @
9b8d3a29
#include
"articulated_obstacle.h"
ArticulatedObstacle
::
ArticulatedObstacle
(
Simulator
*
sim
,
int
index
,
int
currentIndex
,
std
::
vector
<
PFP
::
VEC3
>
*
pos
,
int
size
,
std
::
vector
<
VEC3
>
goals
)
{
this
->
index
=
index
;
nbBodyPart
=
size
;
MovingObstacle
*
mo4
=
new
MovingObstacle
(
sim
,
currentIndex
+
1
,
pos
[
0
],
goals
,
true
,
this
,
0
);
members
.
push_back
(
mo4
);
for
(
int
i
=
1
;
i
<
nbBodyPart
;
i
++
)
{
std
::
vector
<
VEC3
>
goal
;
goal
.
push_back
(
members
[
i
-
1
]
->
center
);
MovingObstacle
*
mo4
=
new
MovingObstacle
(
sim
,
currentIndex
+
1
+
i
,
pos
[
i
],
goal
,
true
,
this
,
i
);
members
.
push_back
(
mo4
);
}
}
PFP
::
VEC3
get_center
(
ArticulatedObstacle
*
art
,
int
index
)
{
return
art
->
members
[
index
]
->
center
+
(
art
->
members
[
index
]
->
center
-
art
->
members
[
index
]
->
front
);
}
src/moving_obstacle.cpp
View file @
9b8d3a29
...
...
@@ -70,10 +70,12 @@ VEC3 rotate(VEC3 pos1, VEC3 center, float angle) // renvoie le déplacement nece
return
pos2
;
}
MovingObstacle
::
MovingObstacle
(
Simulator
*
sim
,
int
ind
,
std
::
vector
<
VEC3
>
pos
,
std
::
vector
<
VEC3
>
goals
,
bool
spin
)
:
MovingObstacle
::
MovingObstacle
(
Simulator
*
sim
,
int
ind
,
std
::
vector
<
VEC3
>
pos
,
std
::
vector
<
VEC3
>
goals
,
bool
spin
,
ArticulatedObstacle
*
art
,
int
ind2
)
:
index
(
ind
),
newVelocity_
(
0
),
sim_
(
sim
)
sim_
(
sim
),
parent
(
art
),
index_parent
(
ind2
)
{
assert
(
pos
.
size
()
>
2
);
...
...
@@ -102,7 +104,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
}
center
=
sum
/
nbVertices
;
front
=
(
vertices
[
1
]
+
vertices
[
2
])
/
2
;
if
(
spinning
)
//départ face à la cible en cas d'obstacles pouvant effectuer des rotations
if
(
spinning
&&
parent
==
NULL
)
//départ face à la cible en cas d'obstacles pouvant effectuer des rotations
{
angle
=
get_angle
(
goals_
[
curGoal_
]
-
center
,
front
-
center
);
for
(
unsigned
int
i
=
0
;
i
<
nbVertices
;
++
i
)
...
...
@@ -327,7 +329,7 @@ void MovingObstacle::update()
// MAJ des particules
float
abs_angle
=
angle
>
0
?
1
:
-
1
;
float
rotor
=
abs_angle
*
angle
>
0.0
1
f
?
0.0
1
f
:
abs_angle
*
angle
;
float
rotor
=
abs_angle
*
angle
>
0.0
3
f
?
0.0
3
f
:
abs_angle
*
angle
;
// CGoGNout << "Obstacle "<< index << CGoGNendl;
// CGoGNout << "vitesse : "<< velocity_ << CGoGNendl;
// on fait tourner l'obstacle
...
...
@@ -459,20 +461,35 @@ void displayMO(Obstacle * o)
// TODO Check position
void
MovingObstacle
::
computePrefVelocity
()
//calcul du vecteur optimal pour atteindre l'objectif // changer pour 2.5 ?
{
VEC3
goalVector
=
goals_
[
curGoal_
]
-
center
;
float
goalDist2
=
goalVector
.
norm2
()
;
if
(
goalDist2
<
2.0
f
)
VEC3
goalVector
;
if
(
index_parent
<
1
)
{
curGoal_
=
(
curGoal_
+
1
)
%
goals_
.
size
()
;
goalVector
=
goals_
[
curGoal_
]
-
center
;
goalDist2
=
goalVector
.
norm2
()
;
}
if
(
goalDist2
>
maxSpeed_
)
float
goalDist2
=
goalVector
.
norm2
()
;
if
(
goalDist2
<
5.0
f
)
{
curGoal_
=
(
curGoal_
+
1
)
%
goals_
.
size
()
;
goalVector
=
goals_
[
curGoal_
]
-
center
;
goalDist2
=
goalVector
.
norm2
()
;
}
if
(
goalDist2
>
maxSpeed_
)
{
goalVector
.
normalize
()
;
goalVector
*=
maxSpeed_
;
}
}
else
{
goalVector
.
normalize
()
;
goalVector
*=
maxSpeed_
;
goalVector
=
get_center
(
parent
,
index_parent
-
1
)
-
center
;
float
goalDist2
=
goalVector
.
norm2
()
;
if
(
goalDist2
>
maxSpeed_
)
{
goalVector
.
normalize
()
;
goalVector
*=
maxSpeed_
;
}
}
if
(
spinning
)
angle
=
get_angle
(
goalVector
,
front
-
center
);
prefVelocity_
=
goalVector
;
...
...
src/simulator.cpp
View file @
9b8d3a29
...
...
@@ -18,7 +18,7 @@ Simulator::Simulator(int minS) :
detect_agent_collision
=
false
;
srand
(
10
)
;
nbStepsPerUnit_
=
1
/
timeStep_
;
config
=
0
;
config
=
2
;
init
(
minSize
,
2.0
f
)
;
}
...
...
@@ -45,9 +45,9 @@ void Simulator::init( float dimension, bool enablePathFinding)
case
1
:
setupCorridorScenario
(
1000
,
40
)
;
break
;
//
case 2 :
//
setupScenario(1000) ;
//
break ;
case
2
:
setupS
nakeCorridorS
cenario
(
1000
,
5
,
10
)
;
break
;
// case 3 :
// setupCityScenario(20, 20) ;
//// setupCityScenario(-1.0f * (12 * (70.0f / 2.0f) - 10),
...
...
@@ -429,6 +429,131 @@ void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObst
}
}
void
Simulator
::
setupSnakeCorridorScenario
(
unsigned
int
nbAgents
,
unsigned
int
nbSnakes
,
int
snakeSize
)
{
if
(
multires
)
{
envMap_
.
init
(
config
,
1600.0
f
,
960.0
f
,
minSize
,
320.0
f
)
;
//grosses cases
}
else
{
envMap_
.
init
(
config
,
1600.0
f
,
960.0
f
,
minSize
,
20.0
f
)
;
//cases fines
}
std
::
cout
<<
" - Setup Snake Corridor Scenario : "
<<
nbAgents
<<
" agents et "
<<
nbSnakes
*
snakeSize
<<
" obstacles"
<<
std
::
endl
;
// Bordure à éviter autour de la scène (10% de sa taille)
int
xBorder
=
envMap_
.
geometry
.
size
(
0
)
/
5
;
int
yBorder
=
envMap_
.
geometry
.
size
(
1
)
/
5
;
// Les coordonnées sont comprises entre xMin et xMin+xDelta
// Départ des agents du quart gauche sur toute la hauteur
int
xStartMin
=
envMap_
.
geometry
.
min
()[
0
]
+
xBorder
;
int
xStartDelta
=
envMap_
.
geometry
.
size
(
0
)
/
5
;
int
yStartMin
=
envMap_
.
geometry
.
min
()[
1
]
+
yBorder
;
int
yStartDelta
=
envMap_
.
geometry
.
size
(
1
)
-
2
*
yBorder
;
// Arrivée des agents à l'opposée
int
xGoalDelta
=
envMap_
.
geometry
.
size
(
0
)
/
5
;
int
xGoalMin
=
envMap_
.
geometry
.
max
()[
0
]
-
xBorder
-
xGoalDelta
;
int
yGoalMin
=
yStartMin
;
int
yGoalDelta
=
yStartDelta
;
for
(
unsigned
int
i
=
0
;
i
<
nbAgents
;
++
i
)
{
VEC3
start
(
xStartMin
+
rand
()
%
xStartDelta
,
yStartMin
+
rand
()
%
yStartDelta
,
0
)
;
VEC3
goal
(
xGoalMin
+
rand
()
%
xGoalDelta
,
yGoalMin
+
rand
()
%
yGoalDelta
,
0
)
;
// Un agent sur 2 va de droite à gauche
VEC3
tmp
;
if
(
i
%
2
==
1
)
{
tmp
=
goal
;
goal
=
start
;
start
=
tmp
;
}
std
::
vector
<
VEC3
>
goals
;
goals
.
push_back
(
start
);
goals
.
push_back
(
goal
);
addAgent
(
start
,
goals
)
;
}
// 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
;
yStartMin
=
envMap_
.
geometry
.
min
()[
1
]
+
yBorder
;
yStartDelta
=
envMap_
.
geometry
.
size
(
1
)
/
20
;
// Arrivée des obstacles à l'opposée
yGoalDelta
=
envMap_
.
geometry
.
size
(
1
)
/
5
;
yGoalMin
=
envMap_
.
geometry
.
max
()[
1
]
-
yBorder
-
yGoalDelta
;
VEC3
xSide
(
5.0
f
,
0.0
f
,
0.0
f
);
VEC3
ySide
(
0.0
f
,
10.0
f
,
0.0
f
);
int
sumObstacles
=
0
;
for
(
unsigned
int
j
=
0
;
j
<
nbSnakes
;
j
++
)
{
std
::
vector
<
PFP
::
VEC3
>
positions
[
snakeSize
]
;
float
x
=
xStartMin
+
rand
()
%
xStartDelta
;
VEC3
goal
;
VEC3
start
(
x
,
yStartMin
+
rand
()
%
yStartDelta
,
0
)
;
std
::
vector
<
VEC3
>
vPos
;
vPos
.
push_back
(
start
+
xSide
-
ySide
);
vPos
.
push_back
(
start
+
xSide
+
ySide
);
vPos
.
push_back
(
start
-
xSide
+
ySide
);
vPos
.
push_back
(
start
-
xSide
-
ySide
);
std
::
vector
<
VEC3
>
goals
;
goals
.
push_back
(
start
);
int
r
=
0
;
while
(
r
<
40
)
{
x
=
xStartMin
+
rand
()
%
xStartDelta
;
goal
=
VEC3
(
x
,
yGoalMin
+
rand
()
%
yGoalDelta
,
0
)
;
if
((
goal
-
goals
[
r
]).
norm2
()
>
1000
){
goals
.
push_back
(
goal
);
r
++
;
}
}
positions
[
0
]
=
vPos
;
for
(
int
i
=
1
;
i
<
snakeSize
;
i
++
)
{
start
=
start
-
ySide
-
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
);
positions
[
i
]
=
vPos
;
}
// CGoGNout<<"positions : "<< positions[0][0]<<CGoGNendl;
ArticulatedObstacle
*
art
=
new
ArticulatedObstacle
(
this
,
j
,
sumObstacles
,
positions
,
snakeSize
,
goals
);
sumObstacles
+=
snakeSize
;
for
(
int
i
=
0
;
i
<
snakeSize
;
i
++
)
{
movingObstacles_
.
push_back
(
art
->
members
[
i
]);
}
}
}
void
Simulator
::
setupCityScenario
(
int
nbLines
,
int
nbRank
)
{
std
::
cout
<<
" - Setup City Scenario : "
<<
nbLines
<<
" x "
<<
nbRank
<<
std
::
endl
;
...
...
src/viewer.cpp
View file @
9b8d3a29
...
...
@@ -249,15 +249,18 @@ void SocialAgents::cb_redraw()
glVertex3fv
(
p
.
data
())
;
}
glEnd
()
;
//////Affiche l'objectif actuel
// glColor3f(1.0f, 0.0f, 0.0f) ;
// glLineWidth(3.0f) ;
// glBegin(GL_LINES);
// const VEC3& p = (*it)->front ;
// glVertex3fv(p.data());
// const VEC3& p2 = (*it)->goals_[(*it)->curGoal_] ;
// glVertex3fv(p2.data());
// glEnd();
//////Affiche l'objectif actuel des tetes articulées
// if((*it)->index_parent==0)
// {
// glColor3f(1.0f, 0.0f, 0.0f) ;
// glLineWidth(3.0f) ;
// glBegin(GL_LINES);
// const VEC3& p = (*it)->front ;
// glVertex3fv(p.data());
// const VEC3& p2 = (*it)->goals_[(*it)->curGoal_] ;
// glVertex3fv(p2.data());
// glEnd();
// }
//////Affiche la direction
// glColor3f(0.0f, 1.0f, 1.0f) ;
...
...
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