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
c5ea16b4
Commit
c5ea16b4
authored
May 30, 2013
by
Thomas Jund
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
ajout de particule secured pour les moving obst
parent
ed23a69b
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
with
90 additions
and
159 deletions
+90
-159
include/moving_obstacle.h
include/moving_obstacle.h
+15
-5
src/moving_obstacle.cpp
src/moving_obstacle.cpp
+75
-154
No files found.
include/moving_obstacle.h
View file @
c5ea16b4
...
...
@@ -7,11 +7,16 @@
#include "env_map.h"
#include <set>
// #define SECURED
#ifdef TWO_AND_HALF_DIM
#include "Algo/MovingObjects/particle_cell_2DandHalf_memo.h"
#else
#ifdef SECURED
#include "Algo/MovingObjects/particle_cell_2D_secured.h"
#else
#include "Algo/MovingObjects/particle_cell_2D_memo.h"
#endif
#endif
#define EXPORTING_BOXES
...
...
@@ -69,10 +74,15 @@ public:
unsigned
int
nbVertices
;
#ifdef TWO_AND_HALF_DIM
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2DAndHalf
<
PFP
>*
*
parts_
;
#else
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2D
<
PFP
>*
*
parts_
;
#ifdef SECURED
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2DSecured
<
PFP
>
*
*
parts_
;
#else
#ifdef TWO_AND_HALF_DIM
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2DAndHalf
<
PFP
>
*
*
parts_
;
#else
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2D
<
PFP
>*
*
parts_
;
#endif
#endif
PFP
::
MAP
map
;
...
...
@@ -147,7 +157,7 @@ public:
bool
rigid_
;
bool
spinning
;
ArticulatedObstacle
*
parent
;
float
hight
;
float
h
e
ight
;
MovingMesh
*
mm_
;
Agent
*
ag_
;
int
index_parent
;
...
...
src/moving_obstacle.cpp
View file @
c5ea16b4
...
...
@@ -146,25 +146,18 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
if
(
dInside
==
NIL
)
dInside
=
sim_
->
envMap_
.
getBelongingCell
(
pos
[
0
]);
unsigned
int
nbParticles
=
nbVertices
;
if
(
!
rigid
);
nbParticles
+=
1
;
//a center particle for the mass-spring
#ifdef TWO_AND_HALF_DIM
if
(
!
rigid_
)
{
parts_
=
new
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2DAndHalf
<
PFP
>*
[
nbVertices
+
1
];
}
else
{
parts_
=
new
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2DAndHalf
<
PFP
>*
[
nbVertices
];
}
parts_
=
new
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2DAndHalf
<
PFP
>*
[
nbParticles
];
#else
if
(
!
rigid_
)
{
parts_
=
new
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2D
<
PFP
>*
[
nbVertices
+
1
];
}
else
{
parts_
=
new
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2D
<
PFP
>*
[
nbVertices
];
}
#ifdef SECURED
parts_
=
new
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2DSecured
<
PFP
>*
[
nbParticles
];
#else
parts_
=
new
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2D
<
PFP
>*
[
nbParticles
];
#endif
#endif
obstacles_
=
new
Obstacle
*
[
nbVertices
];
...
...
@@ -186,82 +179,40 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
for
(
unsigned
int
i
=
0
;
i
<
nbVertices
;
++
i
)
{
#ifdef TWO_AND_HALF_DIM
Dart
d
=
dInside
;
/*sim_->envMap_.getBelongingCellOnSurface(pos[i]); TO CHECK*/
// CGoGNout<<" d trouvée :"<< d <<CGoGNendl;
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2DAndHalf
<
PFP
>*
part
=
new
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2DAndHalf
<
PFP
>
(
sim_
->
envMap_
.
map
,
d
,
pos
[
i
],
sim_
->
envMap_
.
position
);
Dart
d
=
dInside
;
parts_
[
i
]
=
new
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2DAndHalf
<
PFP
>
(
sim_
->
envMap_
.
map
,
d
,
pos
[
i
],
sim_
->
envMap_
.
position
);
#else
Dart
d
=
sim_
->
envMap_
.
getBelongingCell
(
pos
[
i
]);
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2D
<
PFP
>*
part
=
new
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2D
<
PFP
>
(
sim_
->
envMap_
.
map
,
d
,
pos
[
i
],
sim_
->
envMap_
.
position
);
#ifdef SECURED
parts_
[
i
]
=
new
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2DSecured
<
PFP
>
(
sim_
->
envMap_
.
map
,
d
,
pos
[
i
],
sim_
->
envMap_
.
position
);
#else
parts_
[
i
]
=
new
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2D
<
PFP
>
(
sim_
->
envMap_
.
map
,
d
,
pos
[
i
],
sim_
->
envMap_
.
position
);
#endif
#endif
parts_
[
i
]
=
part
;
center
+=
pos
[
i
];
if
(
i
==
0
)
dDir
=
d
;
}
center
/=
nbVertices
;
front
=
(
pos
[
0
]
+
pos
[
1
])
/
2
;
center
/=
nbVertices
;
front
=
(
pos
[
0
]
+
pos
[
1
])
/
2
;
if
(
!
rigid_
)
{
#ifdef TWO_AND_HALF_DIM
if
(
!
rigid_
)
{
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2DAndHalf
<
PFP
>*
part
=
new
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2DAndHalf
<
PFP
>
(
sim_
->
envMap_
.
map
,
dInside
,
center
,
sim_
->
envMap_
.
position
);
parts_
[
nbVertices
]
=
part
;
}
parts_
[
nbVertices
]
=
new
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2DAndHalf
<
PFP
>
(
sim_
->
envMap_
.
map
,
dInside
,
center
,
sim_
->
envMap_
.
position
);
#else
if
(
!
rigid_
)
{
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2D
<
PFP
>*
part
=
new
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2D
<
PFP
>
(
sim_
->
envMap_
.
map
,
dInside
,
center
,
sim_
->
envMap_
.
position
);
parts_
[
nbVertices
]
=
part
;
}
#ifdef SECURED
parts_
[
nbVertices
]
=
new
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2DSecured
<
PFP
>
(
sim_
->
envMap_
.
map
,
dInside
,
center
,
sim_
->
envMap_
.
position
);
#else
parts_
[
nbVertices
]
=
new
CGoGN
::
Algo
::
Surface
::
MovingObjects
::
ParticleCell2D
<
PFP
>
(
sim_
->
envMap_
.
map
,
dInside
,
center
,
sim_
->
envMap_
.
position
);
#endif
// M appartient à l'ellipse ssi MF1 + MF2 = sum_dist_foci est une constante
// où F1 et F2 sont les deux foyers.
// length = (pos[0]-pos[1]).norm();
// width = (pos[1]-pos[2]).norm();
// sum_dist_foci_rest = 2*(length + width*(sqrt(2)-0.5));
if
(
parent
==
NULL
)
//départ face à la cible en cas d'obstacles pouvant effectuer des rotations
{
// VEC3 axeZ=VEC3 (0,0,1);
//#ifdef TWO_AND_HALF_DIM
//
// VEC3 normale = Algo::Surface::Geometry::faceNormal<PFP>(sim->envMap_.map, parts_[nbVertices]->d, sim->envMap_.position);
//#else
// VEC3 normale =axeZ;
//#endif
// Geom::Matrix44f rot ;
// rot.identity() ;
// angle = Geom::angle(goals_[curGoal_] - center,front - center);
// Geom::rotate(axeZ,angle,rot);
//
// float angle1 = Geom::angle(normale, VEC3 (0,0,1) ) ;
// // CGoGNout<<"angle : "<<angle<<CGoGNendl;
// VEC3 axis = VEC3(0,0,1) ^ normale ;
//
// // Geom::translate(center[0],center[1],center[2],rot);
// Geom::rotate(axis, angle1, rot) ;
//
//
//// std::cout<<" angle : "<< angle;
//
// for (unsigned int i = 0; i < nbVertices; ++i)
// {
//// std::cout<<" || pos[i] avant : "<< pos [i];
// Geom::transform(pos[i],rot);
//// std::cout<<" || pos[i] APRES : "<< pos [i]<<std::endl;
//
// parts_[i]->move(pos[i]);
//
//
// }
// angle=0;
// front=(pos[0] + pos[1]) / 2;
}
#endif
}
groundFace
=
map
.
newFace
(
nbVertices
);
for
(
unsigned
int
i
=
0
;
i
<
nbVertices
;
++
i
)
...
...
@@ -280,24 +231,16 @@ if(!rigid_)
velocity
[
d
]
=
VEC3
(
0
);
forces
[
d
]
=
VEC3
(
0
);
}
}
//extrude face to build a cage
// compute edgeLength for mass-spring
hight
=
rigid_
?
10.0
f
:
-
10.0
f
;
Algo
::
Surface
::
Modelisation
::
extrudeFace
<
PFP
>
(
map
,
position
,
groundFace
,
hight
)
;
// Dart dT = map.phi1(map.phi1(map.phi2(Dart(0))));
// std::cout << __FUNCTION__ << " val " << dT << std::endl;
// position[dT]= VEC3(0);
// std::cout << __FUNCTION__ << " pos " << position[dT] << std::endl;
// Algo::Surface::Modelisation::extrudeFace<PFP>(map, position, groundFace, 0.0f) ;
height
=
rigid_
?
10.0
f
:
10.0
f
;
Algo
::
Surface
::
Modelisation
::
extrudeFace
<
PFP
>
(
map
,
position
,
groundFace
,
height
)
;
map
.
fillHole
(
groundFace
);
groundFace
=
map
.
phi2
(
groundFace
);
if
(
!
rigid_
)
{
Dart
d
=
Algo
::
Surface
::
Modelisation
::
trianguleFace
<
PFP
>
(
map
,
groundFace
);
...
...
@@ -891,9 +834,6 @@ void MovingObstacle::updateForces()
Dart
d
;
velocity_
=
newVelocity_
*
velocity_factor
;
// velocity_[0] = 0.0;
// velocity_[1] = 0.0;
// MAJ des particules
float
abs_angle
=
angle
>
0
?
1
:
-
1
;
...
...
@@ -916,7 +856,6 @@ void MovingObstacle::updateForces()
Dart
e
=
map
.
phi1
(
d
);
//initialisation of forces with viscosity
forces
[
e
]
+=
-
0.9
f
*
velocity
[
e
];
// forces[dd] = VEC3(0.0);
VEC3
p1Next
=
position
[
map
.
phi1
(
e
)]
+
(
velocity
[
map
.
phi1
(
e
)]
*
sim_
->
timeStep_
);
// ressorts sur le bord
VEC3
p2Next
=
position
[
e
]
+
(
velocity
[
e
]
*
sim_
->
timeStep_
);
// p1Next et p2Next sont la position des extremites de l'arete.
...
...
@@ -924,7 +863,7 @@ void MovingObstacle::updateForces()
//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
float
norm
=
v1
.
norm
();
float
rigidity
=
5
0.0
f
;
float
rigidity
=
1
0.0
f
;
float
stretch
=
4
*
rigidity
*
(
edgeLength
[
e
]
-
norm
);
float
angularStretch
=
0
,
angularStretch2
=
0
;
...
...
@@ -1178,96 +1117,77 @@ void MovingObstacle::applyForces()
velocity
[
e
]
+=
forces
[
e
]
*
sim_
->
timeStep_
;
position
[
e
]
+=
(
velocity
[
e
]
*
sim_
->
timeStep_
);
d
=
map
.
phi
<
21
>
(
d
);
}
velocity
[
centerDart
]
+=
forces
[
centerDart
]
*
sim_
->
timeStep_
;
position
[
centerDart
]
+=
(
velocity
[
centerDart
]
*
sim_
->
timeStep_
);
// MAJ des obstacles
}
}
void
MovingObstacle
::
updateRegistration
()
{
PFP
::
VEC3
bary
(
0
);
Dart
centerDart
=
map
.
phi
<
11
>
(
groundFace
);
Dart
centerDart
=
map
.
phi
<
11
>
(
groundFace
);
if
(
!
rigid_
)
if
(
!
rigid_
)
{
Dart
d
=
centerDart
;
for
(
unsigned
int
i
=
0
;
i
<
nbVertices
;
++
i
)
{
Dart
d
=
centerDart
;
for
(
unsigned
int
i
=
0
;
i
<
nbVertices
;
++
i
)
{
Dart
e
=
map
.
phi_1
(
d
);
Dart
e
=
map
.
phi_1
(
d
);
parts_
[
i
]
->
move
(
position
[
e
]);
#ifdef TWO_AND_HALF_DIM
parts_
[
i
]
->
move
(
position
[
e
]);
position
[
e
]
=
parts_
[
i
]
->
getPosition
();
//recalage de l'obstacle sur ses particules (qui elles ont bien suivi la carte au sol)
#ifdef TWO_AND_HALF_DIM
PFP
::
VEC3
normal
=
CGoGN
::
Algo
::
Surface
::
Geometry
::
faceNormal
<
PFP
>
(
sim_
->
envMap_
.
map
,
parts_
[
i
]
->
d
,
sim_
->
envMap_
.
position
);
normal
*=
hight
;
// std::cout << " phi11 d : " << map.phi<112>(d) <<" || d : "<<d<< std::endl;
position
[
map
.
phi_1
(
map
.
phi
<
12
>
(
d
))]
=
position
[
e
]
+
normal
;
position
[
e
]
=
parts_
[
i
]
->
getPosition
();
//recalage de l'obstacle sur ses particules (qui elles ont bien suivi la carte au sol)
#endif
bary
+=
position
[
e
];
d
=
map
.
phi
<
21
>
(
d
);
}
PFP
::
VEC3
normal
=
CGoGN
::
Algo
::
Surface
::
Geometry
::
faceNormal
<
PFP
>
(
sim_
->
envMap_
.
map
,
parts_
[
i
]
->
d
,
sim_
->
envMap_
.
position
);
normal
*=
height
;
position
[
map
.
phi_1
(
map
.
phi
<
12
>
(
d
))]
=
position
[
e
]
+
normal
;
center
=
bary
/
nbVertices
;
parts_
[
nbVertices
]
->
move
(
position
[
centerDart
]);
#endif
bary
+=
position
[
e
];
d
=
map
.
phi
<
21
>
(
d
);
}
#ifdef TWO_AND_HALF_DIM
position
[
centerDart
]
=
parts_
[
nbVertices
]
->
getPosition
();
//recalage de l'obstacle sur ses particules (qui elles ont bien suivi la carte au sol)
#endif
// MAJ des obstacles
center
=
bary
/
nbVertices
;
parts_
[
nbVertices
]
->
move
(
position
[
centerDart
]);
#ifdef TWO_AND_HALF_DIM
position
[
centerDart
]
=
parts_
[
nbVertices
]
->
getPosition
();
//recalage de l'obstacle sur ses particules (qui elles ont bien suivi la carte au sol)
#endif
}
}
else
{
for
(
unsigned
int
i
=
0
;
i
<
nbVertices
;
++
i
)
{
for
(
unsigned
int
i
=
0
;
i
<
nbVertices
;
++
i
)
{
VEC3
pos
=
getDilatedPosition
(
i
);
parts_
[
i
]
->
move
(
pos
);
VEC3
pos
=
getDilatedPosition
(
i
);
parts_
[
i
]
->
move
(
pos
);
#ifdef TWO_AND_HALF_DIM
#ifdef TWO_AND_HALF_DIM
Dart
d
(
i
);
position
[
d
]
=
parts_
[
i
]
->
getPosition
();
//recalage de l'obstacle sur ses particules (qui elles ont bien suivi la carte au sol)
Dart
d
(
i
);
position
[
d
]
=
parts_
[
i
]
->
getPosition
();
//recalage de l'obstacle sur ses particules (qui elles ont bien suivi la carte au sol)
PFP
::
VEC3
normal
=
CGoGN
::
Algo
::
Surface
::
Geometry
::
faceNormal
<
PFP
>
(
sim_
->
envMap_
.
map
,
parts_
[
i
]
->
d
,
sim_
->
envMap_
.
position
);
normal
*=
hight
;
// std::cout << " phi11 d : " << map.phi<112>(d) <<" || d : "<<d<< std::endl;
position
[
map
.
phi_1
(
d
)]
=
position
[
d
]
+
normal
;
PFP
::
VEC3
normal
=
CGoGN
::
Algo
::
Surface
::
Geometry
::
faceNormal
<
PFP
>
(
sim_
->
envMap_
.
map
,
parts_
[
i
]
->
d
,
sim_
->
envMap_
.
position
);
normal
*=
hight
;
// std::cout << " phi11 d : " << map.phi<112>(d) <<" || d : "<<d<< std::endl;
position
[
map
.
phi_1
(
d
)]
=
position
[
d
]
+
normal
;
#endif
}
#endif
}
}
for
(
unsigned
int
i
=
0
;
i
<
nbVertices
;
++
i
)
{
// CGoGNout << "avant une etape : Obstacle "<< i << CGoGNendl;
{
// CGoGNout << "avant une etape : Obstacle "<< i << CGoGNendl;
Obstacle
*
o
=
obstacles_
[
i
];
o
->
p1
=
getDilatedPosition
(
i
);
o
->
p2
=
getDilatedPosition
((
i
+
1
)
%
nbVertices
);
...
...
@@ -1285,7 +1205,7 @@ void MovingObstacle::updateRegistration()
sim_
->
envMap_
.
popAndPushObstacleInCells
(
o
,
i
);
}
/////affichage des belonging cells
/////affichage des belonging cells
// CGoGNout<< CGoGNendl;
// CGoGNout << "Obstacle "<< i << ": ";
// for(std::vector<Dart>::iterator ite = belonging_cells[i].begin();ite!=belonging_cells[i].end(); ++ite)
...
...
@@ -1293,8 +1213,9 @@ void MovingObstacle::updateRegistration()
// CGoGNout<<(*ite).index<<" ; ";
// }
// CGoGNout<< CGoGNendl;
// CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl;
// CGoGNout << "Apres une etape : Obstacle "<< i << CGoGNendl;
}
/////affichage du general_belonging
// CGoGNout<< CGoGNendl;
// CGoGNout << "General : ";
...
...
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