Skip to content
GitLab
Projects
Groups
Snippets
Help
Loading...
Help
What's new
10
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Open sidebar
Thomas Pitiot
SocialAgents3D
Commits
adaf1f4a
Commit
adaf1f4a
authored
Mar 28, 2013
by
Arash HABIBI
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
embouteillage
parent
434230a2
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
with
56 additions
and
48 deletions
+56
-48
src/moving_obstacle.cpp
src/moving_obstacle.cpp
+56
-48
No files found.
src/moving_obstacle.cpp
View file @
adaf1f4a
...
...
@@ -312,7 +312,9 @@ void MovingObstacle::draw()
m_positionVBO
->
updateData
(
position
);
// m_shader->setColor(Geom::Vec4f(movingObstacleNeighbors_.size()==0 ? 1.0f : 0,0.,0.,0.));
VEC3
col
=
Utils
::
color_map_BCGYR
(
float
(
index
)
/
float
(
sim_
->
movingObstacles_
.
size
()));
if
(
movingObstacleNeighbors_
.
size
()
==
0
)
// if(obstacleNeighbors_.size()==0)
m_shader
->
setColor
(
Geom
::
Vec4f
(
col
[
0
],
col
[
1
],
col
[
2
],
0
));
else
m_shader
->
setColor
(
Geom
::
Vec4f
(
0.5
,
0.5
,
0.5
,
0
));
...
...
@@ -713,6 +715,47 @@ void MovingObstacle::initForces()
}
}
//-------------------------------------------------------------
VEC3
computeForce
(
VEC3
p
,
VEC3
p1
,
VEC3
p2
,
float
obst_radius_infl
,
float
obst_power
,
float
obst_stiffness
)
{
double
force_value
=
0.0
;
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
)
{
double
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
)
/
sum_of_dists
);
// 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
();
// force_value *= 10;
/*
VEC3 force_vector1, force_vector2;
getResponse1(force_vector,p,p1,p2,&force_vector1,&force_vector2);
Dart d1 = obst->d1;
Dart d2 = obst->d2;
obst->mo->forces[d1] += force_vector1;
obst->mo->forces[d2] += force_vector2;
*/
return
(
force_value
*
normal
);
}
else
return
(
VEC3
(
0
));
}
//-------------------------------------------------------------
void
MovingObstacle
::
updateForces
()
...
...
@@ -853,10 +896,11 @@ void MovingObstacle::updateForces()
obst_radius_infl
=
100.
;
// scenario 0
else
obst_radius_infl
=
30.
;
// scenario 1 et 3
float
force_value
;
VEC3
p
=
position
[
dd
]
+
(
velocity
[
dd
]
*
sim_
->
timeStep_
);
// Evitement d'obstacles mobiles
for
(
std
::
vector
<
std
::
pair
<
float
,
Obstacle
*>
>::
iterator
it
=
movingObstacleNeighbors_
.
begin
()
;
it
!=
movingObstacleNeighbors_
.
end
()
;
++
it
)
{
...
...
@@ -864,55 +908,19 @@ void MovingObstacle::updateForces()
VEC3
p1
=
obst
->
p1
;
VEC3
p2
=
obst
->
p2
;
MovingObstacle
*
mo
=
obst
->
mo
;
if
(
mo
!=
NULL
)
{
// int N = mo->nbVertices;
// int ind1 = obst->index;
// int ind2 = (obst->index+1)%N;
// fprintf(stderr,"%d %d [%d]\n",ind1,ind2,N);
// cout << "-------------------" << mo->forces[ind1] << endl;
}
else
fprintf
(
stderr
,
"NULL
\n
"
);
forces
[
dd
]
+=
computeForce
(
p
,
p1
,
p2
,
obst_radius_infl
,
obst_power
,
obst_stiffness
);
}
double
longueur2
=
(
p1
-
p2
).
norm2
();
double
rest_sum_of_dists
=
2
*
sqrt
(
obst_radius_infl
*
obst_radius_infl
+
longueur2
/
4
);
// Evitement d'obstacles fixes
double
d1
=
(
p
-
p1
).
norm
();
double
d2
=
(
p
-
p2
).
norm
();
double
sum_of_dists
=
d1
+
d2
;
if
(
sum_of_dists
<
rest_sum_of_dists
)
{
double
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
();
// force_value *= 10;
VEC3
force_vector
=
force_value
*
normal
;
forces
[
dd
]
+=
force_vector
;
// obst->index ??? indice local de l'obstacle dans le mo
// obst->mo : le moving_obstacle
// obst->mo->nbVertices
// obst->mo->forces[obst->index]
// obst->mo->forces[obst->index+1%obst->mo->nbVertices]
VEC3
force_vector1
,
force_vector2
;
getResponse1
(
force_vector
,
p
,
p1
,
p2
,
&
force_vector1
,
&
force_vector2
);
Dart
d1
=
obst
->
d1
;
Dart
d2
=
obst
->
d2
;
// obst->mo->forces[d1] += force_vector1;
// obst->mo->forces[d2] += force_vector2;
// VEC3 p11 = obst->mo->position[d1];
// VEC3 p22 = obst->mo->position[d2];
// cerr << "p1=" << p1 << " p11=" << p11 << " p2=" << p2 << " p22=" << p22 << endl;
}
for
(
std
::
vector
<
std
::
pair
<
float
,
Obstacle
*>
>::
iterator
it
=
obstacleNeighbors_
.
begin
()
;
it
!=
obstacleNeighbors_
.
end
()
;
++
it
)
{
Obstacle
*
obst
=
it
->
second
;
VEC3
p1
=
obst
->
p1
;
VEC3
p2
=
obst
->
p2
;
forces
[
dd
]
+=
computeForce
(
p
,
p1
,
p2
,
obst_radius_infl
,
obst_power
,
10
*
obst_stiffness
);
}
}
dd
=
map
.
phi1
(
dd
);
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a 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