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
6a420ce1
Commit
6a420ce1
authored
Mar 28, 2013
by
Arash HABIBI
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
unidir
parent
ebf53f8a
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
with
149 additions
and
8 deletions
+149
-8
include/moving_obstacle.h
include/moving_obstacle.h
+1
-0
src/moving_obstacle.cpp
src/moving_obstacle.cpp
+142
-5
src/simulator.cpp
src/simulator.cpp
+6
-3
No files found.
include/moving_obstacle.h
View file @
6a420ce1
...
...
@@ -50,6 +50,7 @@ public:
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
initForces
();
void
update
();
PFP
::
REAL
computeMVC
(
PFP
::
VEC3
p
,
Dart
vertex
);
...
...
src/moving_obstacle.cpp
View file @
6a420ce1
...
...
@@ -599,6 +599,117 @@ void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance p
}
}
//-------------------------------------------------------------
int
matrixInverse
(
VEC3
v1
,
VEC3
v2
,
VEC3
v3
,
VEC3
*
inv1
,
VEC3
*
inv2
,
VEC3
*
inv3
)
{
int
res
=
0
;
float
determinant
=
v1
[
0
]
*
v2
[
1
]
*
v3
[
2
]
+
v2
[
0
]
*
v3
[
1
]
*
v1
[
2
]
+
v3
[
0
]
*
v1
[
1
]
*
v2
[
2
]
-
v3
[
0
]
*
v2
[
1
]
*
v1
[
2
]
-
v2
[
0
]
*
v1
[
1
]
*
v3
[
2
]
-
v1
[
0
]
*
v3
[
1
]
*
v2
[
2
];
if
(
determinant
==
0.0
)
res
=
1
;
else
{
float
un_sur
=
1.0
/
determinant
;
*
inv1
=
VEC3
(
v2
[
1
]
*
v3
[
2
]
-
v3
[
1
]
*
v2
[
2
],
v2
[
0
]
*
v3
[
2
]
-
v3
[
0
]
*
v2
[
2
],
v2
[
0
]
*
v3
[
1
]
-
v3
[
0
]
*
v2
[
1
]
);
*
inv2
=
VEC3
(
v1
[
1
]
*
v3
[
2
]
-
v3
[
1
]
*
v1
[
2
],
v1
[
0
]
*
v3
[
2
]
-
v3
[
0
]
*
v1
[
2
],
v1
[
0
]
*
v3
[
1
]
-
v3
[
0
]
*
v1
[
1
]
);
*
inv3
=
VEC3
(
v1
[
1
]
*
v2
[
2
]
-
v2
[
1
]
*
v1
[
2
],
v1
[
0
]
*
v2
[
2
]
-
v2
[
0
]
*
v1
[
2
],
v1
[
0
]
*
v2
[
1
]
-
v2
[
0
]
*
v1
[
1
]
);
res
=
0
;
*
inv1
=
*
inv1
*
un_sur
;
*
inv2
=
*
inv2
*
un_sur
;
*
inv3
=
*
inv3
*
un_sur
;
}
return
res
;
}
//-------------------------------------------------------------
float
distToLine
(
VEC3
M
,
VEC3
P
,
VEC3
u
)
{
u
.
normalize
();
VEC3
MP
=
P
-
M
;
return
(
MP
*
(
u
^
(
MP
^
u
)));
}
//-------------------------------------------------------------
// Première stratégie : appliquer à p1 et à p2 une force
// colinéaire à f, mais d'intensité différente. Dans ce cas,
// il est nécessaire de calculer la somme des moments et de
// l'annuler. *f1 = f*d2/(d1+d2) et *f2 = f*d1/(d1+d2).
void
getResponse1
(
VEC3
f
,
VEC3
p
,
VEC3
p1
,
VEC3
p2
,
VEC3
*
f1
,
VEC3
*
f2
)
{
float
d1
=
distToLine
(
p1
,
p
,
f
);
float
d2
=
distToLine
(
p2
,
p
,
f
);
*
f1
=
f
*
(
-
d2
/
(
d1
+
d2
));
*
f2
=
f
*
(
-
d1
/
(
d1
+
d2
));
}
//-------------------------------------------------------------
// Deuxième stratégie, les forces appliquées à p1 et p2
// doivent etre colinaires respectivement à pp1 et pp2.
// Donc la somme des moments est automatiquement nulle.
// Pour calculer les intensites, il faut décomposer f
// dans la base (pp1,pp2), ce qui donnera l'intensite de
// chaque force. Problème : on peut générer des forces
// d'étirement importantes.
void
getResponse2
(
VEC3
f
,
VEC3
p
,
VEC3
p1
,
VEC3
p2
,
VEC3
*
f1
,
VEC3
*
f2
)
{
float
d1
=
(
p1
-
p
).
norm
();
float
d2
=
(
p2
-
p
).
norm
();
float
d12
=
(
p2
-
p1
).
norm
();
VEC3
ortho
=
(
p1
-
p
)
^
(
p2
-
p
);
if
(
ortho
.
norm
()
==
0.0
)
{
if
(
d12
==
0
)
// comprend le cas où d1==0 et d2==0
{
*
f2
=
(
-
0.5
)
*
f
;
*
f1
=
(
-
0.5
)
*
f
;
}
else
if
(
d1
==
0
)
{
*
f2
=
VEC3
(
0
,
0
,
0
);
*
f1
=
-
f
;
// meilleure idee : projeter f sur p1p2 -> f2 et f1 <- f-f2
}
else
if
(
d2
==
0
)
{
*
f1
=
VEC3
(
0
,
0
,
0
);
*
f2
=
-
f
;
// meilleure idee : projeter f sur p1p2 -> f1 et f2 <- f-f1
}
else
// trois points distincts, mais alignés
{
// A suivre
}
}
// int res = matrixInverse(p1-p,p2-p,ortho, VEC3 *inv1, VEC3 *inv2, VEC3 *inv3);
// inv1, inv2 et inv3 sont les composantes de la matrice inverse. L'image de
// p1-p et p2-p par cette matrice donne la valeur des forces f1 et f2.
}
//-------------------------------------------------------------
void
MovingObstacle
::
initForces
()
{
if
(
!
rigid_
)
for
(
unsigned
int
i
=
1
;
i
<
nbVertices
;
++
i
)
{
//initialisation of forces
forces
[
dd
]
=
VEC3
(
0.0
);
map
.
next
(
dd
);
}
}
//-------------------------------------------------------------
void
MovingObstacle
::
update
()
{
assert
(
sim_
->
envMap_
.
map
.
getCurrentLevel
()
==
sim_
->
envMap_
.
map
.
getMaxLevel
())
;
...
...
@@ -643,7 +754,7 @@ void MovingObstacle::update()
for
(
unsigned
int
i
=
1
;
i
<
nbVertices
;
++
i
)
{
//initialisation of forces with viscosity
forces
[
dd
]
=
-
0.2
f
*
velocity
[
dd
];
forces
[
dd
]
+
=
-
0.2
f
*
velocity
[
dd
];
// forces[dd] = VEC3(0.0);
map
.
next
(
dd
);
}
...
...
@@ -747,6 +858,19 @@ void MovingObstacle::update()
Obstacle
*
obst
=
it
->
second
;
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
"
);
double
longueur2
=
(
p1
-
p2
).
norm2
();
double
rest_sum_of_dists
=
2
*
sqrt
(
obst_radius_infl
*
obst_radius_infl
+
longueur2
/
4
);
...
...
@@ -765,10 +889,23 @@ void MovingObstacle::update()
normal
.
normalize
();
// force_value *= 10;
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
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
);
int
indice1
=
obst
->
index
;
int
indice2
=
(
obst
->
index
+
1
)
%
(
obst
->
mo
->
nbVertices
);
// obst->mo->forces[indice1] += force_vector1;
// obst->mo->forces[indice2] += force_vector2;
VEC3
p11
=
obst
->
mo
->
position
[
indice1
];
VEC3
p22
=
obst
->
mo
->
position
[
indice2
];
// cerr << "p1=" << p1 << " p11=" << p11 << " p2=" << p2 << " p22=" << p22 << endl;
}
}
}
...
...
src/simulator.cpp
View file @
6a420ce1
...
...
@@ -129,10 +129,13 @@ void Simulator::doStep()
#ifndef SPATIAL_HASHING
envMap_
.
map
.
setCurrentLevel
(
envMap_
.
map
.
getMaxLevel
())
;
#endif
movingObstacles_
[
i
]
->
update
()
;
movingObstacles_
[
i
]
->
initForces
();
}
// commente par Arash
movingObstacles_
[
i
]
->
updateMesh
()
;
for
(
unsigned
int
i
=
0
;
i
<
movingObstacles_
.
size
()
;
++
i
)
{
movingObstacles_
[
i
]
->
update
()
;
movingObstacles_
[
i
]
->
updateMesh
()
;
}
endTime
=
clock
()
;
time_obstacle
+=
endTime
-
begTime
;
...
...
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