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
14a82956
Commit
14a82956
authored
Feb 14, 2013
by
pitiot
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
version avec pb Dart==NIL
parent
cb708f28
Changes
12
Hide whitespace changes
Inline
Side-by-side
Showing
12 changed files
with
339 additions
and
228 deletions
+339
-228
include/env_map.h
include/env_map.h
+30
-15
include/env_render.h
include/env_render.h
+21
-11
include/moving_obstacle.h
include/moving_obstacle.h
+1
-7
include/obstacle.h
include/obstacle.h
+5
-5
include/simulator.h
include/simulator.h
+1
-1
include/socialAgents.ui
include/socialAgents.ui
+12
-2
include/viewer.h
include/viewer.h
+13
-1
src/agent.cpp
src/agent.cpp
+38
-27
src/env_map.cpp
src/env_map.cpp
+59
-47
src/moving_obstacle.cpp
src/moving_obstacle.cpp
+59
-35
src/simulator.cpp
src/simulator.cpp
+6
-5
src/viewer.cpp
src/viewer.cpp
+94
-72
No files found.
include/env_map.h
View file @
14a82956
...
...
@@ -188,6 +188,7 @@ void displayMO(Obstacle * o);
* INLINE FUNCTIONS *
**************************************/
template
<
typename
T
>
inline
void
addElementToVector
(
std
::
vector
<
T
>&
a
,
T
ag
)
{
...
...
@@ -245,6 +246,7 @@ inline void EnvMap::pushAgentInCells(Agent* agent, Dart d)
{
assert
(
map
.
getCurrentLevel
()
==
map
.
getMaxLevel
())
;
// assert(std::find(agentvect[d].begin(), agentvect[d].end(), agent) == agentvect[d].end());
// map.check();
addElementToVector
<
Agent
*>
(
agentvect
[
d
],
agent
);
// agentvect[d].push_back(agent) ;
...
...
@@ -265,6 +267,7 @@ inline void EnvMap::pushAgentInCells(Agent* agent, Dart d)
}
while
(
dd
!=
d
)
;
}
inline
void
EnvMap
::
popAgentInCells
(
Agent
*
agent
,
Dart
d
)
{
assert
(
map
.
getCurrentLevel
()
==
map
.
getMaxLevel
())
;
...
...
@@ -284,6 +287,16 @@ inline void EnvMap::popAgentInCells(Agent* agent, Dart d)
}
dd
=
map
.
phi1
(
dd
)
;
}
while
(
dd
!=
d
)
;
TraversorF
<
PFP
::
MAP
>
tF
(
map
);
for
(
Dart
ddd
=
tF
.
begin
()
;
ddd
!=
tF
.
end
()
;
ddd
=
tF
.
next
())
{
if
(
std
::
find
(
agentvect
[
ddd
].
begin
(),
agentvect
[
ddd
].
end
(),
agent
)
!=
agentvect
[
ddd
].
end
())
std
::
cout
<<
agent
<<
" SO WRONG"
<<
ddd
.
index
<<
std
::
endl
;
if
(
std
::
find
(
neighborAgentvect
[
ddd
].
begin
(),
neighborAgentvect
[
ddd
].
end
(),
agent
)
!=
neighborAgentvect
[
ddd
].
end
())
std
::
cout
<<
agent
<<
" SO SO WRONG "
<<
ddd
.
index
<<
std
::
endl
;
}
}
...
...
@@ -297,7 +310,7 @@ inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& b
neighbor_cells
->
clear
();
CellMarkerMemo
<
FACE
>
memo_mark
(
map
);
CellMarkerMemo
<
FACE
>
OneRingMark
(
map
);
memo_mark
.
unmarkAll
();
for
(
std
::
vector
<
Dart
>::
const_iterator
it
=
belonging_cells
.
begin
();
it
<
belonging_cells
.
end
();
++
it
)
memo_mark
.
mark
(
*
it
);
...
...
@@ -306,16 +319,25 @@ inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& b
Dart
first
=
NIL
;
Dart
d
=
NIL
;
Dart
dd
=
NIL
;
// CGoGNout<<"beg : "<<(*it)<<CGoGNendl;
//boucle pour trouver une face du voisinage de l'obstacle ne contenant pas l'obstacle
/////////////////////////////////////////////boucle pour trouver une face du voisinage de l'obstacle ne contenant pas l'obstacle
// CGoGNout<<"debut neighbors cellules : ";
// for (std::vector<Dart>::const_iterator it =belonging_cells.begin();it<belonging_cells.end();++it)
// CGoGNout<<(*it).index<<" ; ";
// CGoGNout<<CGoGNendl;
do
{
beg
=
*
it
;
first
=
NIL
;
d
=
beg
;
do
{
// CGoGNout<<"beg : " << beg;
do
{
dd
=
map
.
alpha1
(
map
.
alpha1
(
d
));
do
{
// CGoGNout<<"dd : " << dd;
do
{
if
(
!
memo_mark
.
isMarked
(
dd
))
{
first
=
dd
;
...
...
@@ -326,9 +348,11 @@ inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& b
d
=
map
.
phi1
(
d
);
}
while
(
first
==
NIL
&&
d
!=
beg
);
// CGoGNout<<CGoGNendl;
++
it
;
}
while
(
first
==
NIL
&&
it
!=
belonging_cells
.
end
());
// assert(!buildingMark.isMarked(d)) ;
assert
(
first
!=
NIL
)
;
// CGoGNout<<"first : "<<first<<CGoGNendl;
d
=
first
;
...
...
@@ -341,16 +365,7 @@ inline void EnvMap::addObstAsNeighbor (Obstacle * o, const std::vector<Dart>& b
}
find_next
(
o
,
&
d
,
memo_mark
);
if
(
d
==
NIL
)
{
CGoGNout
<<
"cellule de début : "
<<
first
<<
CGoGNendl
;
CGoGNout
<<
"cellules markées OneRing : "
<<
CGoGNendl
;
std
::
vector
<
Dart
>
v
=
OneRingMark
.
get_markedCells
();
for
(
std
::
vector
<
Dart
>::
iterator
it
=
v
.
begin
();
it
<
v
.
end
();
++
it
)
{
CGoGNout
<<
(
*
it
).
index
<<
CGoGNendl
;
}
}
// CGoGNout<<"d : "<<d<<CGoGNendl;
}
while
(
!
map
.
sameFace
(
d
,
first
));
}
...
...
include/env_render.h
View file @
14a82956
...
...
@@ -71,6 +71,16 @@ inline void renderObstacle(EnvMap& m, MovingObstacle * obst, bool showBelonging=
for
(
unsigned
int
i
=
0
;
i
<
(
obst
->
nbVertices
)
;
++
i
)
{
const
VEC3
&
p
=
obst
->
getPosition
(
i
)
;
if
(
obst
->
obstacles_
[
i
]
==
(
Obstacle
*
)
0xf09f80
||
obst
->
obstacles_
[(
i
-
1
)
%
(
obst
->
nbVertices
)]
==
(
Obstacle
*
)
0xf09f80
)
{
glPointSize
(
10
.
0
f
)
;
glColor3f
(
1
.
0
,
0
.
0
,
0
.
0
);
}
else
{
glPointSize
(
3
.
0
f
)
;
glColor3f
(
0
.
0
,
0
.
0
,
1
.
0
);
}
glVertex3fv
(
p
.
data
())
;
}
glEnd
()
;
...
...
@@ -97,14 +107,14 @@ inline void renderObstacle(EnvMap& m, MovingObstacle * obst, bool showBelonging=
// }
// 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
()
;
//
glLineWidth(5.0f) ;
//
glColor3f(1.0,1.0,0.0);
//
glBegin(GL_LINES) ;
//
VEC3 p = obst->getPosition(0) ;
//
glVertex3fv(p.data()) ;
//
p += 20.0f*obst->velocity_;
//
glVertex3fv(p.data()) ;
//
glEnd() ;
if
(
renderPath
)
{
...
...
@@ -148,9 +158,9 @@ inline void renderAgent(EnvMap& m, Agent* agent, bool showNeighborDist = false,
glLineWidth
(
1
.
0
f
)
;
VEC3
col
=
Utils
::
color_map_BCGYR
(
float
(
agent
->
agentNo
)
/
float
(
agent
->
sim_
->
agents_
.
size
()));
glColor3fv
(
col
.
data
());
//
glColor3f(c1,c2,c3) ;
//
VEC3 col = Utils::color_map_BCGYR(float(agent->agentNo)/float(agent->sim_->agents_.size()));
//
glColor3fv(col.data());
glColor3f
(
c1
,
c2
,
c3
)
;
glBegin
(
GL_POLYGON
)
;
for
(
unsigned
int
i
=
0
;
i
<
5
;
++
i
)
glVertex3f
(
pos
[
0
]
+
(
cosT
[
i
]
*
radius
),
pos
[
1
]
+
(
sinT
[
i
]
*
radius
),
pos
[
2
]
+
0
.
01
f
)
;
...
...
include/moving_obstacle.h
View file @
14a82956
...
...
@@ -26,7 +26,7 @@ public:
bool
is_inside
(
VEC3
p
);
void
computePrefVelocity
();
void
computeNewVelocity
();
std
::
vector
<
Dart
>
getMemoCross
(
const
VEC3
&
pos
,
const
VEC3
&
dest
,
Dart
&
d1
,
CGoGN
::
Algo
::
MovingObjects
::
ParticleCell2DMemo
<
PFP
>
*
registering_part
);
std
::
vector
<
Dart
>
getMemoCross
(
const
VEC3
&
pos
,
const
VEC3
&
dest
,
Dart
&
d1
,
Dart
&
d2
);
VEC3
getDilatedPosition
(
unsigned
int
ind
);
//vertex position with velocity dilatation
VEC3
getPosition
(
unsigned
int
ind
);
// vertex position
void
update
();
...
...
@@ -37,10 +37,7 @@ public:
void
updateMesh
();
unsigned
int
nbVertices
;
std
::
vector
<
PFP
::
VEC3
>
vertices
;
CGoGN
::
Algo
::
MovingObjects
::
ParticleCell2D
<
PFP
>*
*
parts_
;
PFP
::
MAP
map
;
VertexAttribute
<
VEC3
>
position
;
VertexAttribute
<
VEC3
>
normal
;
...
...
@@ -59,8 +56,6 @@ public:
// std::vector<float> verticesAngle;
// float rigidity;
// float gravity_dist;
VEC3
center
;
int
nb_agents_voisins
;
int
nb_register_cells
;
...
...
@@ -90,7 +85,6 @@ public:
static
float
neighborDist_
;
static
float
neighborDistSq_
;
static
float
maxSpeed_
;
// float obstacle_range;
static
float
timeHorizonObst_
;
float
velocity_factor
;
float
color1
;
...
...
include/obstacle.h
View file @
14a82956
...
...
@@ -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 ;
}
VEC3
p1
;
VEC3
p2
;
VEC3
prevP
;
VEC3
nextP
;
const
VEC3
&
p1
;
const
VEC3
&
p2
;
const
VEC3
&
prevP
;
const
VEC3
&
nextP
;
MovingObstacle
*
mo
;
unsigned
int
index
;
}
;
...
...
include/simulator.h
View file @
14a82956
...
...
@@ -80,7 +80,7 @@ public:
class
Simulator
{
public:
Simulator
(
unsigned
int
config
,
unsigned
int
minSize
,
unsigned
int
nbAgent
=
1000
,
unsigned
int
nbObst
=
20
)
;
Simulator
(
unsigned
int
config
,
unsigned
int
minSize
,
unsigned
int
nbAgent
=
1000
,
unsigned
int
nbObst
=
20
,
bool
resolution
=
true
)
;
~
Simulator
()
;
...
...
include/socialAgents.ui
View file @
14a82956
...
...
@@ -7,7 +7,7 @@
<x>
0
</x>
<y>
0
</y>
<width>
219
</width>
<height>
457
</height>
<height>
684
</height>
</rect>
</property>
<property
name=
"windowTitle"
>
...
...
@@ -112,7 +112,7 @@
</property>
</widget>
</item>
<item>
<item>
<widget
class=
"QCheckBox"
name=
"check_drawObstPath"
>
<property
name=
"text"
>
<string>
draw obst path
</string>
...
...
@@ -149,6 +149,16 @@
</property>
</widget>
</item>
<item>
<widget
class=
"QCheckBox"
name=
"checkAgent"
>
<property
name=
"text"
>
<string>
Agents n°
</string>
</property>
</widget>
</item>
<item>
<widget
class=
"QSpinBox"
name=
"AgentSelect"
/>
</item>
<item>
<spacer
name=
"verticalSpacer"
>
<property
name=
"orientation"
>
...
...
include/viewer.h
View file @
14a82956
...
...
@@ -51,7 +51,7 @@ class SocialAgents : public Utils::QT::SimpleQT
Q_OBJECT
public:
SocialAgents
(
unsigned
int
config
,
unsigned
int
minSize
,
unsigned
int
nbAgent
,
unsigned
int
nbObst
,
unsigned
int
iterations
)
;
SocialAgents
(
unsigned
int
config
,
unsigned
int
minSize
,
unsigned
int
nbAgent
,
unsigned
int
nbObst
,
bool
resolution
,
unsigned
int
iterations
)
;
void
initGUI
()
;
...
...
@@ -103,9 +103,11 @@ public:
bool
drawObstPredictionTri
;
bool
drawObstPath
;
bool
draw_dart
;
bool
target_Agent
;
bool
draw_posX
;
bool
draw_elipse
;
unsigned
int
dartSlider
;
unsigned
int
agentSlider
;
int
posXSlider
;
int
posYSlider
;
...
...
@@ -206,4 +208,14 @@ public slots:
draw_elipse
=
b
;
updateGL
();
}
void
slot_Agent
(
bool
b
)
{
target_Agent
=
b
;
updateGL
()
;
}
void
slot_AgentSlider
(
int
i
)
{
agentSlider
=
i
;
updateGL
()
;
}
}
;
src/agent.cpp
View file @
14a82956
...
...
@@ -40,6 +40,7 @@ Agent::Agent(Simulator* sim, const VEC3& start, const VEC3& goal, Dart d) :
sim_
(
sim
),
alive
(
true
)
{
init
(
start
,
goal
);
}
...
...
@@ -70,9 +71,17 @@ void Agent::init(const VEC3& start, const VEC3& goal)
goals_
.
push_back
(
start
)
;
float
ratio
=
(
80.0
f
+
rand
()
%
20
)
/
100.0
f
;
maxSpeed_
=
averageMaxSpeed_
*
ratio
;
// from 80% to 100% of averageMaxSpeed_
color1
=
1.0
f
*
ratio
;
// red = high speed agents
color2
=
1.0
f
*
5
*
(
1
-
ratio
)
;
// green = low speed agents
color3
=
0
;
// color1 = 1.0f * ratio ; // red = high speed agents
// color2 = 1.0f * 5 * (1 - ratio) ; // green = low speed agents
// color3 = 0 ;
color1
=
1.0
f
;
// red = high speed agents
color2
=
0.0
f
;
// green = low speed agents
color3
=
0.0
f
;
// if (agentNo==0)
// {
// color1=0;
// color2 = 1.0f;
// }
for
(
unsigned
int
i
=
0
;
i
<
4
;
++
i
)
meanVelocity_
[
i
].
set
(
0
)
;
agentNeighbors_
.
reserve
(
maxNeighbors_
*
2
)
;
obstacleNeighbors_
.
reserve
(
maxNeighbors_
*
2
)
;
...
...
@@ -545,7 +554,7 @@ 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
;
//
float ag_ambient_damping = 1.0;
double
mass_var
=
0.95
;
double
average_mass
=
1.0
;
// double average_radius = 20.f;
...
...
@@ -697,6 +706,7 @@ void Agent::computeNewVelocity()
float
previous_distSq
=
previous_relativePosition
.
norm2
();
float
previous_dist
=
sqrt
(
previous_distSq
);
// const VEC3 u_other(relativePosition);
VEC3
u_other
(
relativePosition
);
u_other
=
relativePosition
;
...
...
@@ -704,6 +714,7 @@ void Agent::computeNewVelocity()
// cerr << "dist=" << dist << " combinedRadius=" << combinedRadius << endl;
if
(
dist
<
combinedRadius
)
{
collision_softening_factor
=
pow
(
1
-
dist
/
combinedRadius
,
ag_power
);
...
...
@@ -734,29 +745,29 @@ void Agent::computeNewVelocity()
//------- 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
;
//
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;
}
///* Search for the best new velocity. */
...
...
src/env_map.cpp
View file @
14a82956
...
...
@@ -536,23 +536,24 @@ void EnvMap::pushObstacleInCells(Obstacle * o)// réenregistre l'obstacle en que
if
(
mo
!=
NULL
)
{
int
n
=
o
->
index
;
VEC3
p1
=
mo
->
parts_
[
n
]
->
getPosition
()
;
VEC3
p2
=
mo
->
parts_
[(
n
+
1
)
%
mo
->
nbVertices
]
->
getPosition
()
;
VEC3
p1
=
o
->
p1
;
VEC3
p2
=
o
->
p2
;
Dart
d1
=
NIL
;
Dart
d2
=
NIL
;
std
::
vector
<
Dart
>
memo
;
CGoGN
::
Algo
::
MovingObjects
::
ParticleCell2DMemo
<
PFP
>
*
registering_part
=
new
CGoGN
::
Algo
::
MovingObjects
::
ParticleCell2DMemo
<
PFP
>
(
map
,
mo
->
parts_
[
n
]
->
d
,
mo
->
parts_
[
n
]
->
getPosition
(),
position
)
;
d1
=
mo
->
parts_
[
n
]
->
d
;
memo
=
mo
->
getMemoCross
(
p1
,
p2
,
d1
,
registering_part
);
d2
=
registering_part
->
d
;
memo
=
mo
->
getMemoCross
(
p1
,
p2
,
d1
,
d2
);
if
(
map
.
sameFace
(
d1
,
d2
))
{
pushObstacleInOneRingCells
(
o
,
d1
,
n
);
}
else
// if(map.sameFace(d1,d2))
// {
// pushObstacleInOneRingCells(o , d1 , n);
// }
// else
{
pushObstacleInCells
(
o
,
n
,
memo
);
}
}
...
...
@@ -561,20 +562,20 @@ void EnvMap::pushObstacleInCells(Obstacle * o)// réenregistre l'obstacle en que
Dart
EnvMap
::
popAndPushObstacleInCells
(
Obstacle
*
o
,
int
n
)
// maj de l'enregistrement
{
MovingObstacle
*
mo
=
o
->
mo
;
VEC3
p1
=
mo
->
parts_
[
n
]
->
getPosition
()
;
VEC3
p2
=
mo
->
parts_
[(
n
+
1
)
%
mo
->
nbVertices
]
->
getPosition
()
;
VEC3
p1
=
o
->
p1
;
VEC3
p2
=
o
->
p2
;
Dart
d1
=
NIL
;
Dart
d2
=
NIL
;
std
::
vector
<
Dart
>
memo
;
CGoGN
::
Algo
::
MovingObjects
::
ParticleCell2DMemo
<
PFP
>
*
registering_part
=
new
CGoGN
::
Algo
::
MovingObjects
::
ParticleCell2DMemo
<
PFP
>
(
map
,
mo
->
parts_
[
n
]
->
d
,
mo
->
parts_
[
n
]
->
getPosition
(),
position
)
;
d1
=
mo
->
parts_
[
n
]
->
d
;
// bool modif=false;
// if(p1->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS
// || p2->crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
// {
memo
=
mo
->
getMemoCross
(
p1
,
p2
,
d1
,
registering_part
);
d2
=
registering_part
->
d
;
memo
=
mo
->
getMemoCross
(
p1
,
p2
,
d1
,
d2
);
// memo.sort();
// modif=true;
// }
...
...
@@ -590,11 +591,11 @@ Dart EnvMap::popAndPushObstacleInCells(Obstacle* o, int n)// maj de l'enregistre
{
popObstacleInCells
(
o
,
n
);
if
(
map
.
sameFace
(
d1
,
d2
))
{
pushObstacleInOneRingCells
(
o
,
d1
,
n
);
}
else
//
if(map.sameFace(d1,d2))
//
{
//
pushObstacleInOneRingCells(o , d1 , n);
//
}
//
else
{
pushObstacleInCells
(
o
,
n
,
memo
);
}
...
...
@@ -611,9 +612,9 @@ void EnvMap::pushObstacleInOneRingCells(Obstacle * o, Dart d, int n)
addElementToVector
<
Obstacle
*>
(
obstvect
[
d
],
o
);
mo
->
belonging_cells
[
n
].
clear
();
//
mo->belonging_cells[n].clear();
mo
->
belonging_cells
[
n
].
push_back
(
d
);
mo
->
neighbor_cells
[
n
].
clear
();
//
mo->neighbor_cells[n].clear();
Dart
dd
=
d
;
do
...
...
@@ -639,7 +640,8 @@ void EnvMap::pushObstacleInCells(Obstacle* o, int n, const std::vector<Dart>& me
assert
(
!
memo_cross
.
empty
());
MovingObstacle
*
mo
=
o
->
mo
;
mo
->
belonging_cells
[
n
].
clear
();
// mo->belonging_cells[n].clear();
// mo->neighbor_cells[n].clear();
mo
->
belonging_cells
[
n
]
=
memo_cross
;
for
(
std
::
vector
<
Dart
>::
iterator
it
=
mo
->
belonging_cells
[
n
].
begin
();
it
!=
mo
->
belonging_cells
[
n
].
end
();
++
it
)
...
...
@@ -675,6 +677,8 @@ void EnvMap::popObstacleInCells(Obstacle* o, int n)
{
removeElementFromVector
<
Obstacle
*>
(
neighborObstvect
[
*
it
],
o
)
;
}
mo
->
belonging_cells
[
n
].
clear
();
mo
->
neighbor_cells
[
n
].
clear
();
}
}
...
...
@@ -751,6 +755,7 @@ void EnvMap::refine()
if
(
subdivisable
)
{
// CGoGNout<<"debut refine "<<d.index <<CGoGNendl;
if
(
fLevel
==
-
1
)
fLevel
=
map
.
faceLevel
(
old
)
;
...
...
@@ -786,28 +791,6 @@ void EnvMap::refine()
}
map
.
setCurrentLevel
(
map
.
getMaxLevel
())
;
//agents contained in the subdivided cell are pushed correctly
for
(
PFP
::
AGENTS
::
iterator
ait
=
oldAgents
.
begin
();
ait
!=
oldAgents
.
end
();
++
ait
)
{
resetAgentInFace
(
*
ait
)
;
pushAgentInCells
(
*
ait
,
(
*
ait
)
->
part_
.
d
)
;
}
//same for obstacles contained
for
(
PFP
::
OBSTACLEVECT
::
iterator
ait
=
oldObst
.
begin
();
ait
!=
oldObst
.
end
();
++
ait
)
{
resetPartSubdiv
(
*
ait
);
pushObstacleInCells
(
*
ait
);
}
//same for adjacent obstacles
for
(
PFP
::
OBSTACLEVECT
::
iterator
ait
=
oldNeighborObst
.
begin
();
ait
!=
oldNeighborObst
.
end
();
++
ait
)
{
pushObstacleInCells
(
*
ait
)
;
}
//retrieve neighbors agents from onering cells
dd
=
old
;
do
...
...
@@ -854,6 +837,32 @@ void EnvMap::refine()
d3
=
map
.
phi1
(
d3
)
;
}
while
(
d3
!=
centerFace
)
;
}
//agents contained in the subdivided cell are pushed correctly
for
(
PFP
::
AGENTS
::
iterator
ait
=
oldAgents
.
begin
();
ait
!=
oldAgents
.
end
();
++
ait
)
{
resetAgentInFace
(
*
ait
)
;
pushAgentInCells
(
*
ait
,
(
*
ait
)
->
part_
.
d
)
;
}
//same for obstacles contained
for
(
PFP
::
OBSTACLEVECT
::
iterator
ait
=
oldObst
.
begin
();
ait
!=
oldObst
.
end
();
++
ait
)
{
resetPartSubdiv
(
*
ait
);
pushObstacleInCells
(
*
ait
);
}
//same for adjacent obstacles
for
(
PFP
::
OBSTACLEVECT
::
iterator
ait
=
oldNeighborObst
.
begin
();
ait
!=
oldNeighborObst
.
end
();
++
ait
)
{
pushObstacleInCells
(
*
ait
)
;
}
// CGoGNout<<"fin refine"<<CGoGNendl;
}
}
}
...
...
@@ -1123,15 +1132,18 @@ void EnvMap::updateMap()
assert
(
map
.
getCurrentLevel
()
==
map
.
getMaxLevel
())
;
refine
();
coarse
();
//
coarse();
}
void
EnvMap
::
resetAgentInFace
(
Agent
*
agent
)
{
Dart
old
=
agent
->
part_
.
d
;
VEC3
pos
=
agent
->
part_
.
getPosition
()
;
agent
->
part_
.
move
(
Algo
::
Geometry
::
faceCentroid
<
PFP
>
(
map
,
agent
->
part_
.
d
,
position
))
;
agent
->
part_
.
ParticleBase
<
PFP
>::
move
(
Algo
::
Geometry
::
faceCentroid
<
PFP
>
(
map
,
ol
d
,
position
))
;
agent
->
part_
.
setState
(
FACE
)
;
agent
->
part_
.
move
(
pos
)
;
}
#endif
...
...
src/moving_obstacle.cpp
View file @
14a82956
...
...
@@ -156,9 +156,12 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
Dart
d
=
sim_
->
envMap_
.
getBelongingCell
(
pos
[
i
]);
CGoGN
::
Algo
::
MovingObjects
::
ParticleCell2D
<
PFP
>*
part
=
new
CGoGN
::
Algo
::
MovingObjects
::
ParticleCell2D
<
PFP
>
(
sim_
->
envMap_
.
map
,
d
,
pos
[
i
],
sim_
->
envMap_
.
position
);
parts_
[
i
]
=
part
;
if
(
i
==
0
)
dDir
=
d
;
}
center
/=
nbVertices
;
front
=
(
p
arts_
[
1
]
->
getPosition
()
+
parts_
[
2
]
->
getPosition
()
)
/
2
;
front
=
(
p
osition
[
1
]
+
position
[
2
]
)
/
2
;
//-------- code ajoute par Arash pour les obstacles rectangulaires --------------
...
...
@@ -172,14 +175,15 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
if
(
spinning
&&
parent
==
NULL
)
//départ face à la cible en cas d'obstacles pouvant effectuer des rotations
{
CGoGNout
<<
"orienté "
<<
index
<<
CGoGNendl