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
895e18f6
Commit
895e18f6
authored
Mar 14, 2013
by
Arash HABIBI
Browse files
03_2013
parent
52c29bed
Changes
2
Hide whitespace changes
Inline
Side-by-side
include/moving_obstacle.h
View file @
895e18f6
...
...
@@ -79,6 +79,11 @@ public:
std
::
set
<
Dart
>
general_belonging
;
VEC3
front
;
// voisinage
std
::
vector
<
std
::
pair
<
float
,
Obstacle
*>
>
obstacleNeighbors_
;
std
::
vector
<
std
::
pair
<
float
,
Obstacle
*>
>
movingObstacleNeighbors_
;
VEC3
finalGoal
;
float
angle
;
// Dart finalDart;
...
...
src/moving_obstacle.cpp
View file @
895e18f6
...
...
@@ -398,42 +398,44 @@ void MovingObstacle::updateAgentNeighbors() // agents voisins avec distance au b
void
MovingObstacle
::
updateObstacleNeighbors
()
// obstacles voisins , distance par rapport aux centres des segments// a mettre en place si besoin
{
// obstacleNeighbors_.clear() ;
// movingObstacleNeighbors_.clear() ;
//
// std::vector<Obstacle*>& obst = sim_->envMap_.obstvect[part_.d] ;
// std::vector<Obstacle*>& neighborObst = sim_->envMap_.neighborObstvect[part_.d] ;
// for(std::vector<Obstacle*>::const_iterator it = obst.begin() ; it != obst.end() ; ++it)
// {
// float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
// if (distSq < rangeSq_)
// {
// if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
// {
// if ((*it)->mo==NULL)
// obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
// else
// {
// movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
// }
// }
//
// }
// }
// for(std::vector<Obstacle*>::const_iterator it = neighborObst.begin() ; it != neighborObst.end() ; ++it)
// {
// float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
// if(distSq < rangeSq_)
// {
// if(Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
// {
// if ((*it)->mo==NULL)
// obstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
// else
// movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
// }
// }
// }
// ARASH en decommentant ce qui suit, on rend les obstacles perceptibles par la tete de la limace (ou du chariot)
obstacleNeighbors_
.
clear
()
;
movingObstacleNeighbors_
.
clear
()
;
std
::
vector
<
Obstacle
*>&
obst
=
sim_
->
envMap_
.
obstvect
[
part_
.
d
]
;
std
::
vector
<
Obstacle
*>&
neighborObst
=
sim_
->
envMap_
.
neighborObstvect
[
part_
.
d
]
;
for
(
std
::
vector
<
Obstacle
*>::
const_iterator
it
=
obst
.
begin
()
;
it
!=
obst
.
end
()
;
++
it
)
{
float
distSq
=
distSqPointLineSegment
((
*
it
)
->
p1
,
(
*
it
)
->
p2
,
part_
.
getPosition
())
;
if
(
distSq
<
rangeSq_
)
{
if
(
Geom
::
testOrientation2D
(
part_
.
getPosition
(),
(
*
it
)
->
p1
,
(
*
it
)
->
p2
)
==
Geom
::
RIGHT
)
{
if
((
*
it
)
->
mo
==
NULL
)
obstacleNeighbors_
.
push_back
(
std
::
make_pair
(
distSq
,
*
it
))
;
else
{
movingObstacleNeighbors_
.
push_back
(
std
::
make_pair
(
distSq
,
*
it
))
;
}
}
}
}
for
(
std
::
vector
<
Obstacle
*>::
const_iterator
it
=
neighborObst
.
begin
()
;
it
!=
neighborObst
.
end
()
;
++
it
)
{
float
distSq
=
distSqPointLineSegment
((
*
it
)
->
p1
,
(
*
it
)
->
p2
,
part_
.
getPosition
())
;
if
(
distSq
<
rangeSq_
)
{
if
(
Geom
::
testOrientation2D
(
part_
.
getPosition
(),
(
*
it
)
->
p1
,
(
*
it
)
->
p2
)
==
Geom
::
RIGHT
)
{
if
((
*
it
)
->
mo
==
NULL
)
obstacleNeighbors_
.
push_back
(
std
::
make_pair
(
distSq
,
*
it
))
;
else
movingObstacleNeighbors_
.
push_back
(
std
::
make_pair
(
distSq
,
*
it
))
;
}
}
}
}
void
MovingObstacle
::
update
()
...
...
@@ -467,6 +469,8 @@ void MovingObstacle::update()
else
rotor
=
abs_angle
*
angle
;
// masse ressort pour la limace
if
(
!
rigid_
)
{
Dart
d
=
groundFace
;
...
...
@@ -482,15 +486,19 @@ void MovingObstacle::update()
CellMarkerStore
<
EDGE
>
cm
(
map
);
DartMarkerStore
dm
(
map
);
// ARASH : On parcourt les sommets de la grande face
for
(
unsigned
int
i
=
1
;
i
<
nbVertices
;
++
i
)
{
Dart
dd
=
d
;
// ARASH : On parcourt les sous-faces triangulaire de la grande face
do
{
if
(
!
cm
.
isMarked
(
dd
))
{
cm
.
mark
(
dd
);
VEC3
p1Next
=
position
[
map
.
phi1
(
dd
)]
+
(
velocity
[
map
.
phi1
(
dd
)]
*
sim_
->
timeStep_
);
VEC3
p2Next
=
position
[
dd
]
+
(
velocity
[
dd
]
*
sim_
->
timeStep_
);
// p1Next et p2Next sont la position des extremites de l'arete.
VEC3
v1
=
(
p1Next
-
p2Next
);
//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
...
...
@@ -508,6 +516,7 @@ void MovingObstacle::update()
dd
=
map
.
phi1
(
dd
);
}
while
(
dd
!=
d
);
// ARASH : les ressorts angulaires
do
{
if
(
!
dm
.
isMarked
(
dd
))
{
...
...
@@ -537,6 +546,48 @@ void MovingObstacle::update()
dd
=
map
.
phi1
(
dd
);
}
while
(
dd
!=
d
);
//-------------------------------------------------------------------------
// ARASH : A présent on calcule les interactions avec les autres obstacles.
do
{
if
(
!
cm
.
isMarked
(
dd
))
{
cm
.
mark
(
dd
);
VEC3
p
=
position
[
dd
]
+
(
velocity
[
dd
]
*
sim_
->
timeStep_
);
for
(
std
::
vector
<
std
::
pair
<
float
,
Obstacle
*>
>::
iterator
it
=
movingObstacleNeighbors_
.
begin
()
;
it
!=
movingObstacleNeighbors_
.
end
()
;
++
it
)
{
Obstacle
*
obst
=
it
->
second
;
VEC3
p1
=
obst
->
p1
;
VEC3
p2
=
obst
->
p2
;
double
longueur2
=
(
p1
-
p2
).
norm2
();
double
rest_sum_of_dists
=
2
*
sqrt
(
obst_radius_infl
*
obst_radius_infl
+
longueur2
/
4
);
double
d1
=
(
p
-
p1
).
norm
();
double
d2
=
(
p
-
p2
).
norm
();
double
sum_of_dists
=
d1
+
d2
;
if
(
sum_of_dists
<
rest_sum_of_dists
)
{
collision_softening_factor
=
pow
(
1
-
sum_of_dists
/
rest_sum_of_dists
,
obst_power
);
force_value
=
obst_stiffness
*
collision_softening_factor
*
(
rest_sum_of_dists
-
sum_of_dists
);
VEC3
v_obst
=
p2
-
p1
;
VEC3
normal
=
VEC3
(
v_obst
[
1
],
-
v_obst
[
0
],
0
);
// Ajouter une composante tangentielle
normal
+=
v_obst
*
((
d1
-
d2
)
/
(
5
*
sum_of_dists
));
// Le facteur 5 est là seulement pour diminuer l'influence de la composante tangentielle
normal
.
normalize
();
forces
[
dd
]
-=
force_value
*
normal
;
// This force is not symmetrically applied
// We assume that a homologous vertex in the other obstacle
// is also intersecting the current obstacle
}
}
}
dd
=
map
.
phi1
(
dd
);
}
while
(
dd
!=
d
);
map
.
next
(
d
);
}
...
...
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