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
cb708f28
Commit
cb708f28
authored
Jan 18, 2013
by
pitiot
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
correction initialisation
parent
7f90ed0e
Changes
18
Hide whitespace changes
Inline
Side-by-side
Showing
18 changed files
with
1084 additions
and
353 deletions
+1084
-353
Release/CMakeLists.txt
Release/CMakeLists.txt
+2
-0
include/agent.h
include/agent.h
+14
-2
include/env_map.h
include/env_map.h
+2
-2
include/env_render.h
include/env_render.h
+74
-5
include/moving_mesh.h
include/moving_mesh.h
+12
-3
include/moving_obstacle.h
include/moving_obstacle.h
+38
-4
include/obstacle.h
include/obstacle.h
+5
-5
include/path_finder.hpp
include/path_finder.hpp
+7
-2
include/simulator.h
include/simulator.h
+3
-2
include/socialAgents.ui
include/socialAgents.ui
+13
-0
include/viewer.h
include/viewer.h
+23
-5
src/agent.cpp
src/agent.cpp
+120
-54
src/articulated_obstacle.cpp
src/articulated_obstacle.cpp
+2
-2
src/env_map.cpp
src/env_map.cpp
+10
-7
src/moving_mesh.cpp
src/moving_mesh.cpp
+151
-73
src/moving_obstacle.cpp
src/moving_obstacle.cpp
+358
-97
src/simulator.cpp
src/simulator.cpp
+152
-39
src/viewer.cpp
src/viewer.cpp
+98
-51
No files found.
Release/CMakeLists.txt
View file @
cb708f28
...
...
@@ -16,6 +16,8 @@ include_directories(
${
CMAKE_CURRENT_BINARY_DIR
}
)
#SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
QT4_WRAP_UI
(
socialAgents_ui ../include/socialAgents.ui
)
QT4_WRAP_CPP
(
socialAgents_moc ../include/viewer.h
)
add_executable
(
socialAgents
...
...
include/agent.h
View file @
cb708f28
...
...
@@ -6,7 +6,14 @@
#include "utils.h"
#include "env_map.h"
#include "spatialHashing.h"
#include "Algo/MovingObjects/particle_cell_2D.h"
//#define SECURED
#ifdef SECURED
#include "Algo/MovingObjects/particle_cell_2D_secured.h"
#else
#include "Algo/MovingObjects/particle_cell_2D.h"
#endif
class
Simulator
;
...
...
@@ -46,7 +53,11 @@ public:
#ifdef SPATIAL_HASHING
VEC3
pos
;
#else
CGoGN
::
Algo
::
MovingObjects
::
ParticleCell2D
<
PFP
>
part_
;
#ifdef SECURED
CGoGN
::
Algo
::
MovingObjects
::
ParticleCell2DSecured
<
PFP
>
part_
;
#else
CGoGN
::
Algo
::
MovingObjects
::
ParticleCell2D
<
PFP
>
part_
;
#endif
#endif
unsigned
int
curGoal_
;
...
...
@@ -63,6 +74,7 @@ public:
static
unsigned
int
maxMovingObstacles_
;
static
float
averageMaxSpeed_
;
static
float
neighborDist_
;
static
float
neighborDistSq_
;
static
float
radius_
;
...
...
include/env_map.h
View file @
cb708f28
...
...
@@ -79,7 +79,7 @@ public:
void
popAgentInCells
(
Agent
*
agent
,
Dart
d
)
;
void
agentChangeFace
(
Agent
*
agent
,
Dart
oldFace
)
;
void
popAndPushObstacleInCells
(
Obstacle
*
o
,
int
n
);
Dart
popAndPushObstacleInCells
(
Obstacle
*
o
,
int
n
);
void
pushObstacleInCells
(
Obstacle
*
mo
);
void
pushObstacleInOneRingCells
(
Obstacle
*
o
,
Dart
d
,
int
n
);
void
pushObstacleInCells
(
Obstacle
*
o
,
int
n
,
const
std
::
vector
<
Dart
>&
memo_cross
);
...
...
@@ -328,7 +328,7 @@ inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& b
}
while
(
first
==
NIL
&&
d
!=
beg
);
++
it
;
}
while
(
first
==
NIL
&&
it
!=
belonging_cells
.
end
());
assert
(
!
buildingMark
.
isMarked
(
d
))
;
//
assert(!buildingMark.isMarked(d)) ;
assert
(
first
!=
NIL
)
;
// CGoGNout<<"first : "<<first<<CGoGNendl;
d
=
first
;
...
...
include/env_render.h
View file @
cb708f28
...
...
@@ -5,6 +5,8 @@
#include "agent.h"
#include "Utils/colorMaps.h"
#include "Algo/Render/GL1/map_glRender.h"
inline
void
renderDart
(
EnvMap
&
m
,
Dart
d
)
{
PFP
::
VEC3
p1
=
m
.
position
[
d
]
;
...
...
@@ -44,17 +46,66 @@ inline void renderPredictionTriangle(EnvMap& m, Dart d, PFP::VEC3 p)
}
inline
void
renderObstacle
(
EnvMap
&
m
,
MovingObstacle
*
obst
,
bool
showBelonging
=
false
,
bool
renderPath
=
false
)
inline
void
renderObstacle
(
EnvMap
&
m
,
MovingObstacle
*
obst
,
bool
showBelonging
=
false
,
bool
renderPath
=
false
,
bool
showVertices
=
false
)
{
glBegin
(
GL_POLYGON
)
;
for
(
unsigned
int
i
=
0
;
i
<
(
obst
->
nbVertices
)
;
++
i
)
{
const
VEC3
&
p
=
obst
->
getPosition
(
i
)
;
glVertex3fv
(
p
.
data
())
;
}
glEnd
()
;
// glPolygonMode(GL_FRONT_AND_BACK, GL_LINE) ;
// glColor3f(0,1,0);
// Algo::Render::GL1::renderTriQuadPoly<PFP>(obst->map, Algo::Render::GL1::LINE,
// 1.0, obst->position,
// obst->normal) ;
if
(
showVertices
)
{
glPointSize
(
3
.
0
f
)
;
glColor3f
(
0
.
0
,
0
.
0
,
1
.
0
);
glBegin
(
GL_POINTS
)
;
for
(
unsigned
int
i
=
0
;
i
<
(
obst
->
nbVertices
)
;
++
i
)
{
const
VEC3
&
p
=
obst
->
getPosition
(
i
)
;
glVertex3fv
(
p
.
data
())
;
}
glEnd
()
;
glPointSize
(
1
.
0
f
)
;
}
glColor3f
(
1
.
0
,
0
.
0
,
1
.
0
);
glBegin
(
GL_LINE_LOOP
)
;
for
(
unsigned
int
i
=
0
;
i
<
(
obst
->
nbVertices
)
;
++
i
)
{
const
VEC3
&
p
=
obst
->
parts_
[
i
]
->
getPosition
(
)
;
const
VEC3
&
p
=
obst
->
getDilatedPosition
(
i
)
;
glVertex3fv
(
p
.
data
())
;
}
glEnd
()
;
// glColor3f(0.0,1.0,1.0);
// glBegin(GL_LINES) ;
// for (unsigned int i = 0 ; i < (obst->nbVertices) ; ++i)
// {
// const VEC3& p = obst->getPosition(i) ;
// glVertex3fv(p.data()) ;
// const VEC3& p2 = obst->getDilatedPosition(i) ;
// glVertex3fv(p2.data()) ;
// }
// glEnd() ;
glLineWidth
(
5
.
0
f
)
;
glColor3f
(
1
.
0
,
1
.
0
,
0
.
0
);
glBegin
(
GL_LINES
)
;
VEC3
p
=
obst
->
getPosition
(
0
)
;
glVertex3fv
(
p
.
data
())
;
p
+=
20
.
0
f
*
obst
->
velocity_
;
glVertex3fv
(
p
.
data
())
;
glEnd
()
;
if
(
renderPath
)
{
glLineWidth
(
3
.
0
f
)
;
...
...
@@ -62,10 +113,19 @@ inline void renderObstacle(EnvMap& m, MovingObstacle * obst, bool showBelonging=
for
(
std
::
vector
<
VEC3
>::
iterator
it
=
++
(
obst
->
goals_
.
begin
())
;
it
!=
obst
->
goals_
.
end
()
;
++
it
)
{
glVertex3f
((
*
it
)[
0
],
(
*
it
)[
1
],
(
*
it
)[
2
])
;
// glVertex3f((*it)[0], (*it)[1], (*it)[2]) ;
glVertex3f
((
*
it
)[
0
],
(
*
it
)[
1
],
obst
->
index
)
;
}
glEnd
()
;
glLineWidth
(
1
.
0
f
)
;
glBegin
(
GL_LINES
)
;
VEC3
p
=
obst
->
getPosition
(
0
)
;
glVertex3fv
(
p
.
data
())
;
p
=
obst
->
goals_
[
obst
->
curGoal_
];
glVertex3fv
(
p
.
data
())
;
glEnd
()
;
}
}
...
...
@@ -127,13 +187,22 @@ inline void renderAgent(EnvMap& m, Agent* agent, bool showNeighborDist = false,
{
glLineWidth
(
3
.
0
f
)
;
glBegin
(
GL_LINE_STRIP
)
;
for
(
std
::
vector
<
VEC3
>::
iterator
it
=
++
(
agent
->
goals_
.
begin
())
;
it
!=
agent
->
goals_
.
end
()
;
for
(
std
::
vector
<
VEC3
>::
iterator
it
=
(
agent
->
goals_
.
begin
())
;
it
!=
agent
->
goals_
.
end
()
;
++
it
)
{
glVertex3f
((
*
it
)[
0
],
(
*
it
)[
1
],
(
*
it
)[
2
]
)
;
glVertex3f
((
*
it
)[
0
],
(
*
it
)[
1
],
agent
->
agentNo
)
;
}
glVertex3f
((
*
(
agent
->
goals_
.
begin
()))[
0
],
(
*
(
agent
->
goals_
.
begin
()))[
1
],
(
*
(
agent
->
goals_
.
begin
()))[
2
])
;
glEnd
()
;
glLineWidth
(
1
.
0
f
)
;
glPointSize
(
5
.
0
f
)
;
glBegin
(
GL_POINTS
)
;
VEC3
g
=
agent
->
goals_
[
agent
->
curGoal_
];
glVertex3fv
(
g
.
data
())
;
glEnd
()
;
glPointSize
(
1
.
0
f
)
;
}
if
(
showNeighborDist
)
...
...
include/moving_mesh.h
View file @
cb708f28
...
...
@@ -15,23 +15,32 @@ using namespace std;
class
MovingMesh
{
public:
MovingMesh
(
EnvMap
*
envMap
,
Dart
d
,
std
::
string
filename
);
MovingMesh
(
EnvMap
&
envMap
,
Dart
d
,
std
::
string
filename
);
void
linkWithObstacle
(
MovingObstacle
*
mo
);
void
transform
(
Geom
::
Matrix44f
m
);
void
translate
(
VEC3
v
);
void
scale
(
float
val
);
void
moveInFace
(
PFP
::
MAP
&
motherMap
,
Dart
d
,
VertexAttribute
<
VEC3
>
pos
);
void
move
();
void
animate
();
std
::
vector
<
VEC3
>
computeProjectedPointSet
();
std
::
vector
<
VEC3
>
jarvisConvexHull
(
std
::
vector
<
VEC3
>
projectedPointSet
);
void
draw
();
std
::
vector
<
VEC3
>
computeProjectedPointSet
(
float
maxHeight
);
void
simplifyCurve
(
std
::
vector
<
VEC3
>&
pointSet
,
std
::
vector
<
bool
>&
active
,
int
start
,
int
end
,
float
epsilon
);
std
::
vector
<
VEC3
>
jarvisConvexHull
(
const
std
::
vector
<
VEC3
>&
projectedPointSet
);
std
::
vector
<
VEC3
>
computeSkeleton
(
std
::
vector
<
VEC3
>
projectedPointSet
,
unsigned
int
nodeNumber
);
PFP
::
MAP
&
motherMap
;
VertexAttribute
<
VEC3
>&
motherPosition
;
PFP
::
MAP
map
;
VertexAttribute
<
VEC3
>
position
;
VertexAttribute
<
VEC3
>
normal
;
float
scaleValue
;
ShapeMatchingQuadratic
<
PFP
>
*
smg
;
std
::
vector
<
VEC3
>
skeleton
;
...
...
include/moving_obstacle.h
View file @
cb708f28
...
...
@@ -7,16 +7,18 @@
#include "env_map.h"
#include <set>
#include "Algo/MovingObjects/particle_cell_2D_memo.h"
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
MovingMesh
;
class
MovingObstacle
{
public:
MovingObstacle
(
Simulator
*
sim
,
int
index
,
std
::
vector
<
PFP
::
VEC3
>
pos
,
std
::
vector
<
VEC3
>
goals
,
bool
spin
,
ArticulatedObstacle
*
art
=
NULL
,
int
ind2
=-
1
);
MovingObstacle
(
Simulator
*
sim
,
int
index
,
std
::
vector
<
PFP
::
VEC3
>
pos
,
std
::
vector
<
VEC3
>
goals
,
bool
rigid
,
bool
spin
,
int
curGoal
=
0
,
Dart
d
=
NIL
,
ArticulatedObstacle
*
art
=
NULL
,
int
indParent
=-
1
);
bool
test_opposition
(
VEC3
o
,
VEC3
p1
,
VEC3
p2
);
// void contournerBatiment();
void
updateAgentNeighbors
()
;
...
...
@@ -25,13 +27,40 @@ public:
void
computePrefVelocity
();
void
computeNewVelocity
();
std
::
vector
<
Dart
>
getMemoCross
(
const
VEC3
&
pos
,
const
VEC3
&
dest
,
Dart
&
d1
,
CGoGN
::
Algo
::
MovingObjects
::
ParticleCell2DMemo
<
PFP
>
*
registering_part
);
VEC3
getDilatedPosition
(
unsigned
int
ind
);
//vertex position with velocity dilatation
VEC3
getPosition
(
unsigned
int
ind
);
// vertex position
void
update
();
PFP
::
REAL
computeMVC
(
PFP
::
VEC3
p
,
Dart
vertex
);
void
computePointMVC
(
PFP
::
VEC3
point
,
std
::
vector
<
PFP
::
REAL
>&
coordinates
);
void
attachMesh
(
MovingMesh
*
mm
);
void
updateMesh
();
unsigned
int
nbVertices
;
std
::
vector
<
PFP
::
VEC3
>
vertices
;
CGoGN
::
Algo
::
MovingObjects
::
ParticleCell2D
<
PFP
>*
*
parts_
;
float
gravity_dist
;
PFP
::
MAP
map
;
VertexAttribute
<
VEC3
>
position
;
VertexAttribute
<
VEC3
>
normal
;
VertexAttribute
<
VEC3
>
deformation
;
VertexAttribute
<
VEC3
>
velocity
;
EdgeAttribute
<
float
>
edgeLength
;
Dart
groundFace
;
//vector de détails pour la deformation
// std::vector<PFP::VEC3> verticesDeformation;
//gestion pour du mass-ressort
// std::vector<PFP::VEC3> verticesVelocity;
// std::vector<float> edgeLength;
// std::vector<float> verticesAngle;
// float rigidity;
// float gravity_dist;
VEC3
center
;
int
nb_agents_voisins
;
int
nb_register_cells
;
...
...
@@ -53,6 +82,7 @@ public:
std
::
vector
<
VEC3
>
goals_
;
unsigned
int
curGoal_
;
Dart
dDir
;
// static float neighborDistSq_;
static
unsigned
int
maxNeighbors_
;
...
...
@@ -60,7 +90,7 @@ public:
static
float
neighborDist_
;
static
float
neighborDistSq_
;
static
float
maxSpeed_
;
float
obstacle_range
;
//
float obstacle_range;
static
float
timeHorizonObst_
;
float
velocity_factor
;
float
color1
;
...
...
@@ -71,9 +101,13 @@ public:
VEC3
newVelocity_
;
VEC3
prefVelocity_
;
Simulator
*
sim_
;
bool
rigid_
;
bool
spinning
;
ArticulatedObstacle
*
parent
;
int
index_parent
;
MovingMesh
*
mm_
;
VertexAttribute
<
NoMathIONameAttribute
<
std
::
vector
<
PFP
::
REAL
>
>
>
mvc_
;
};
#endif
include/obstacle.h
View file @
cb708f28
...
...
@@ -6,7 +6,7 @@
class
Obstacle
{
public:
Obstacle
(
const
VEC3
&
point1
,
const
VEC3
&
point2
,
const
VEC3
&
prevPoint
,
const
VEC3
&
nextPoint
,
Obstacle
(
const
VEC3
point1
,
const
VEC3
point2
,
const
VEC3
prevPoint
,
const
VEC3
nextPoint
,
MovingObstacle
*
moving1
,
unsigned
int
ind
)
:
p1
(
point1
),
p2
(
point2
),
prevP
(
prevPoint
),
nextP
(
nextPoint
),
mo
(
moving1
),
index
(
ind
)
...
...
@@ -17,10 +17,10 @@ public:
// nextP[2] = 0 ;
}
const
VEC3
&
p1
;
const
VEC3
&
p2
;
const
VEC3
&
prevP
;
const
VEC3
&
nextP
;
VEC3
p1
;
VEC3
p2
;
VEC3
prevP
;
VEC3
nextP
;
MovingObstacle
*
mo
;
unsigned
int
index
;
}
;
...
...
include/path_finder.hpp
View file @
cb708f28
...
...
@@ -83,8 +83,6 @@ std::vector<Dart> pathFindAStar(EnvMap& envMap, const typename PFP::TVEC3& posit
// inCurrentFace = map.phi1(inCurrentFace) ;
// } while (inCurrentFace != currentFace) ;
}
while
(
facesToTry
.
size
()
>
0
&&
!
map
.
sameFace
(
start
,
currentFace
))
;
// If a path has been found, push all predecessors from start to stop
...
...
@@ -96,6 +94,13 @@ std::vector<Dart> pathFindAStar(EnvMap& envMap, const typename PFP::TVEC3& posit
path
.
push_back
(
toPush
)
;
toPush
=
faceNextFaceToGoal
[
toPush
]
;
}
while
(
toPush
!=
goal
)
;
// Dart dS = path[0];
// do
// {
// dS = map.phi1(dS);
// } while(dS!=path[0] || !map.sameFace(map.phi2(dS),path[1]));
// path[0] = dS;
}
else
{
...
...
include/simulator.h
View file @
cb708f28
...
...
@@ -80,11 +80,11 @@ public:
class
Simulator
{
public:
Simulator
(
unsigned
int
config
,
unsigned
int
minSize
)
;
Simulator
(
unsigned
int
config
,
unsigned
int
minSize
,
unsigned
int
nbAgent
=
1000
,
unsigned
int
nbObst
=
20
)
;
~
Simulator
()
;
void
init
(
float
dimension
,
bool
enablePathFinding
=
false
)
;
void
init
(
float
dimension
,
unsigned
int
nbAgent
,
unsigned
int
nbObst
,
bool
enablePathFinding
=
false
)
;
void
doStep
()
;
...
...
@@ -120,6 +120,7 @@ public:
EnvMap
envMap_
;
std
::
vector
<
Agent
*>
agents_
;
std
::
vector
<
MovingObstacle
*>
movingObstacles_
;
std
::
vector
<
MovingMesh
*>
movingMeshes_
;
//for visualization only
int
minSize
;
unsigned
int
config
;
float
timeStep_
;
...
...
include/socialAgents.ui
View file @
cb708f28
...
...
@@ -129,6 +129,19 @@
<item>
<widget
class=
"QSpinBox"
name=
"check_slide"
/>
</item>
<item>
<widget
class=
"QCheckBox"
name=
"check_pos"
>
<property
name=
"text"
>
<string>
Draw X line
</string>
</property>
</widget>
</item>
<item>
<widget
class=
"QSpinBox"
name=
"check_x_val"
/>
</item>
<item>
<widget
class=
"QSpinBox"
name=
"check_y_val"
/>
</item>
<item>
<widget
class=
"QCheckBox"
name=
"check_elipse"
>
<property
name=
"text"
>
...
...
include/viewer.h
View file @
cb708f28
...
...
@@ -51,7 +51,7 @@ class SocialAgents : public Utils::QT::SimpleQT
Q_OBJECT
public:
SocialAgents
(
unsigned
int
config
,
unsigned
int
minSize
,
unsigned
int
iterations
)
;
SocialAgents
(
unsigned
int
config
,
unsigned
int
minSize
,
unsigned
int
nbAgent
,
unsigned
int
nbObst
,
unsigned
int
iterations
)
;
void
initGUI
()
;
...
...
@@ -103,8 +103,11 @@ public:
bool
drawObstPredictionTri
;
bool
drawObstPath
;
bool
draw_dart
;
bool
draw_posX
;
bool
draw_elipse
;
unsigned
int
dartSlider
;
int
posXSlider
;
int
posYSlider
;
public
slots
:
void
animate
()
;
...
...
@@ -183,9 +186,24 @@ public slots:
dartSlider
=
i
;
updateGL
();
}
void
slot_posX
(
bool
b
)
{
draw_posX
=
b
;
updateGL
()
;
}
void
slot_slide2
(
int
i
)
{
posXSlider
=
i
;
updateGL
();
}
void
slot_slide3
(
int
i
)
{
posYSlider
=
i
;
updateGL
();
}
void
slot_elipse
(
bool
b
)
{
draw_elipse
=
b
;
updateGL
();
}
{
draw_elipse
=
b
;
updateGL
();
}
}
;
src/agent.cpp
View file @
cb708f28
...
...
@@ -10,12 +10,15 @@
#include "simulator.h"
unsigned
int
Agent
::
maxNeighbors_
=
10
;
unsigned
int
Agent
::
maxMovingObstacles_
=
5
;
unsigned
int
Agent
::
maxMovingObstacles_
=
10
;
float
Agent
::
averageMaxSpeed_
=
2.0
f
;
float
Agent
::
neighborDist_
=
10.0
f
;
// float Agent::averageMaxSpeed_ = 20.0f ;
// float Agent::neighborDist_ = 10.0f ;
float
Agent
::
neighborDist_
=
20.0
f
;
float
Agent
::
neighborDistSq_
=
neighborDist_
*
neighborDist_
;
float
Agent
::
radius_
=
1.5
f
;
float
Agent
::
timeHorizon_
=
10.0
f
;
//float Agent::timeHorizon_ = 10.0f ;
float
Agent
::
timeHorizon_
=
100.0
f
;
float
Agent
::
timeHorizonObst_
=
10.0
f
;
float
Agent
::
range_
=
2
*
timeHorizonObst_
*
averageMaxSpeed_
+
radius_
;
...
...
@@ -37,7 +40,6 @@ Agent::Agent(Simulator* sim, const VEC3& start, const VEC3& goal, Dart d) :
sim_
(
sim
),
alive
(
true
)
{
init
(
start
,
goal
);
}
...
...
@@ -202,34 +204,32 @@ void Agent::updateObstacleNeighbors()
{
if
((
*
it
)
->
mo
==
NULL
)
{
float
distSq
=
distSqPointLineSegment
((
*
it
)
->
p1
,
(
*
it
)
->
p2
,
part_
.
getPosition
())
;
// CGoGNout<<"obst trouvé : " << distSq;
if
((
obstacleNeighbors_
.
size
()
<
maxNeighbors_
||
distSq
<
maxDistObst
)
&&
distSq
<
rangeSq_
)
&&
distSq
<
rangeSq_
)
{
// CGoGNout<<"obst assez pres :";
if
(
Geom
::
testOrientation2D
(
part_
.
getPosition
(),
(
*
it
)
->
p1
,
(
*
it
)
->
p2
)
==
Geom
::
RIGHT
)
{
// CGoGNout<<"obst bien orienté";
if
(
distSq
>
maxDistObst
)
maxDistObst
=
distSq
;
obstacleNeighbors_
.
push_back
(
std
::
make_pair
(
distSq
,
*
it
))
;
if
(
distSq
>
maxDistObst
)
maxDistObst
=
distSq
;
obstacleNeighbors_
.
push_back
(
std
::
make_pair
(
distSq
,
*
it
))
;
}
}
// CGoGNout<<CGoGNendl;
}
else
{
float
distSq
=
distSqPointLineSegment
((
*
it
)
->
p1
,
(
*
it
)
->
p2
,
part_
.
getPosition
())
;
if
((
movingObstacleNeighbors_
.
size
()
<
maxNeighbors_
||
distSq
<
maxDistMovingObst
)
&&
distSq
<
rangeSq_
)
&&
distSq
<
rangeSq_
)
{
if
(
Geom
::
testOrientation2D
(
part_
.
getPosition
(),
(
*
it
)
->
p1
,
(
*
it
)
->
p2
)
==
Geom
::
RIGHT
)
{
if
(
distSq
>
maxDistMovingObst
)
maxDistMovingObst
=
distSq
;
if
(
distSq
>
maxDistMovingObst
)
maxDistMovingObst
=
distSq
;
movingObstacleNeighbors_
.
push_back
(
std
::
make_pair
(
distSq
,
*
it
))
;
}
...
...
@@ -237,6 +237,7 @@ void Agent::updateObstacleNeighbors()
}
}
}
for
(
std
::
vector
<
Obstacle
*>::
const_iterator
it
=
neighborObst
.
begin
()
;
it
!=
neighborObst
.
end
()
;
++
it
)
{
if
((
*
it
)
->
mo
==
NULL
)
...
...
@@ -323,6 +324,11 @@ void Agent::update()
#else
VEC3
target
=
part_
.
getPosition
()
+
(
velocity_
*
sim_
->
timeStep_
)
;
part_
.
move
(
target
)
;
#ifdef SECURED
if
(
target
!=
part_
.
getPosition
())
std
::
cout
<<
"Problem agent "
<<
agentNo
<<
" (position : "
<<
part_
.
getPosition
()
<<
") time "
<<
sim_
->
globalTime_
<<
std
::
endl
;
#endif
#endif
meanDirection_
.
set
(
0
)
;
...
...
@@ -530,19 +536,32 @@ static int moAppend(MovingObstacle **moving_obstacles, int nb_mos, int max, Movi
return
(
nb_mos
);
}
void
Agent
::
computeNewVelocity
()
// de arash
//-----------------------------------------------------------------
//-----------------------------------------------------------------
//-----------------------------------------------------------------
// Search for the best new velocity.
void
Agent
::
computeNewVelocity
()
{
// The objective is to compute the sum of forces exerted on the agent.
double
collision_softening_factor
;
float
ag_ambient_damping
=
1.0
;
double
mass_var
=
0.9
;
double
mass_var
=
0.9
5
;
double
average_mass
=
1.0
;
// double average_radius = 20.f;
// double radius_var = 0.9;
// range_ = 2*timeHorizonObst_ * averageMaxSpeed_ + radius_ ;
// rangeSq_ = range_ * range_ ;
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
double
ag_mass
=
average_mass
*
(
1
+
rand
*
mass_var
);
// valeurs uniformement réparties entre min et max
/*
rand = 2.0*drand48()-1.0; // compris entre -1 et 1
radius_ = average_radius + rand*radius_var; // valeurs uniformement réparties entre min et max
range_ = 2*timeHorizonObst_ * averageMaxSpeed_ + radius_ ;
rangeSq_ = range_ * range_ ;
*/
//-------------
VEC3
vec
,
forces
,
previous_pos
;
...
...
@@ -557,14 +576,19 @@ void Agent::computeNewVelocity() // de arash
VEC3
u_goal
=
p_goal
-
getPosition
();
u_goal
.
normalize
();
// cerr << agentNo << "---------------- position = " << getPosition() << endl;
forces
+=
goal_attraction_force
*
u_goal
;
//----- forces dues à la répulsion des obstacles ----------
// cerr << "goal forces = " << forces << endl;
//----- forces dues à la répulsion des obstacles en mouvement ----------
VEC3
norm
;
double
obst_stiffness
=
5000.0
;
// agent-obstacle interaction stiffness
// double obst_stiffness = 10000.0; // agent-obstacle interaction stiffness
double
obst_stiffness
=
1000.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
int
obst_power
=
2
;
// the power to which elevate the agent-obstacle distance
Obstacle
*
obst
;
nb_mos
=
0
;
...
...
@@ -585,7 +609,6 @@ void Agent::computeNewVelocity() // de arash
{
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
)
{
...
...
@@ -606,12 +629,55 @@ void Agent::computeNewVelocity() // de arash
}
}
//----- forces dues à la répulsion des obstacles fixes ----------
// double fixed_obst_stiffness = 50000.0; // agent-obstacle interaction stiffness
double
fixed_obst_stiffness
=
50000.0
;
// agent-obstacle interaction stiffness
// double fixed_obst_damping = 1.0; // agent-obstacle interaction damping
int
fixed_obst_power
=
1
;
// the power to which elevate the agent-obstacle distance