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
e6def94a
Commit
e6def94a
authored
Sep 12, 2013
by
pitiot
Browse files
update shapematching
parent
5de581be
Changes
5
Hide whitespace changes
Inline
Side-by-side
include/env_map.h
View file @
e6def94a
...
...
@@ -34,7 +34,7 @@ class ArticulatedObstacle;
//#define EXPORTING3
//
#define TWO_AND_HALF_DIM
#define TWO_AND_HALF_DIM
#ifdef EXPORTING3
...
...
include/moving_obstacle.h
View file @
e6def94a
...
...
@@ -2,12 +2,17 @@
#define M_MOVING_OBSTACLE_H
#include
<iostream>
#include
"ShapeMatching/shapeMatchingLinear.h"
#include
"utils.h"
#include
"env_map.h"
#include
<set>
#define LINEAR
// #define SECURED
#ifdef LINEAR
#include
"ShapeMatching/shapeMatchingLinear.h"
#else
#include
"ShapeMatching/shapeMatchingQuadratic.h"
#endif
#ifdef TWO_AND_HALF_DIM
#include
"Algo/MovingObjects/particle_cell_2DandHalf_memo.h"
...
...
@@ -166,8 +171,11 @@ public:
VertexAttribute
<
NoMathIONameAttribute
<
std
::
vector
<
PFP
::
REAL
>
>
>
mvc_
;
/////// ShapeMatching
#ifdef LINEAR
ShapeMatchingLinear
<
PFP
>
*
shape_
;
#else
ShapeMatchingQuadratic
<
PFP
>
*
shape_
;
#endif
PFP
::
REAL
beta
;
float
alpha
;
...
...
src/moving_obstacle.cpp
View file @
e6def94a
...
...
@@ -162,8 +162,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
ag_
(
NULL
),
index_parent
(
indParent
),
gravity_dist
(
0
),
beta
(
0.9
),
alpha
(
0.
1
)
beta
(
0.9
5
),
alpha
(
0.
2
)
{
assert
(
pos
.
size
()
>
2
);
...
...
@@ -371,8 +371,11 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
dDir
=
dInside
;
}
#ifdef LINEAR
shape_
=
new
ShapeMatchingLinear
<
PFP
>
(
map
,
position
,
bord
,
beta
);
#else
shape_
=
new
ShapeMatchingQuadratic
<
PFP
>
(
map
,
position
,
bord
,
beta
);
#endif
shape_
->
initialize
();
for
(
unsigned
int
i
=
0
;
i
<
nbVertices
;
++
i
)
...
...
@@ -447,7 +450,7 @@ void MovingObstacle::draw(bool showPath)
VEC3
col
=
Utils
::
color_map_BCGYR
(
float
(
index
)
/
float
(
sim_
->
movingObstacles_
.
size
()));
m_ds
->
color3f
(
col
[
0
],
col
[
1
],
col
[
2
]);
m_ds
->
vertex
(
center
);
m_ds
->
vertex
(
position
[
groundFace
]
);
for
(
unsigned
int
i
=
0
;
i
<
goals_
.
size
()
;
i
++
)
{
m_ds
->
vertex
(
goals_
[(
curGoal_
+
i
)
%
(
goals_
.
size
())]);
...
...
@@ -1141,7 +1144,7 @@ void MovingObstacle::updateForces()
//guiding vertex = first vertex (set the displacement)
// forces[groundFace] = VEC3(0);
velocity
[
groundFace
]
=
velocity_
;
velocity
[
map
.
phi_1
(
groundFace
)]
=
velocity_
;
//apply force to each vertex
/*
d = groundFace;
...
...
@@ -1238,11 +1241,17 @@ void MovingObstacle::applyForces()
if
(
!
rigid_
)
{
Dart
d
=
groundFace
;
shape_
->
animate
();
for
(
unsigned
int
i
=
0
;
i
<
nbVertices
;
++
i
)
{
velocity
[
d
]
+=
forces
[
d
]
*
sim_
->
timeStep_
;
velocity
[
d
]
+=
forces
[
d
]
*
sim_
->
timeStep_
+
(
alpha
/
sim_
->
timeStep_
)
*
(
shape_
->
goal
[
d
]
-
position
[
d
]);
velocity
[
map
.
phi_1
(
map
.
phi2
(
d
))]
+=
forces
[
d
]
*
sim_
->
timeStep_
+
(
alpha
/
sim_
->
timeStep_
)
*
(
shape_
->
goal
[
map
.
phi_1
(
map
.
phi2
(
d
))]
-
position
[
map
.
phi_1
(
map
.
phi2
(
d
))]);;
position
[
d
]
+=
(
velocity
[
d
]
*
sim_
->
timeStep_
);
position
[
map
.
phi_1
(
map
.
phi2
(
d
))]
+=
(
velocity
[
map
.
phi_1
(
map
.
phi2
(
d
))]
*
sim_
->
timeStep_
);
// 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.phi2(d))] = position[d]+normal;
...
...
@@ -1250,16 +1259,10 @@ void MovingObstacle::applyForces()
}
// velocity[centerDart] +=forces[centerDart] * sim_->timeStep_;
// position[centerDart] += (velocity[centerDart] * sim_->timeStep_);
shape_
->
animate
();
do
{
// CGoGNout<<"dart : "<<d<<" || pos : " <<position[d] <<" || obj : "<< shape_->goal[d]<<CGoGNendl;
position
[
d
]
+=
alpha
*
(
shape_
->
goal
[
d
]
-
position
[
d
]);
// position[d][2]=0;
position
[
map
.
phi_1
(
map
.
phi2
(
d
))]
=
shape_
->
goal
[
map
.
phi_1
(
map
.
phi2
(
d
))];
d
=
map
.
phi_1
(
d
);
}
while
(
d
!=
groundFace
);
}
...
...
@@ -1282,8 +1285,9 @@ void MovingObstacle::updateRegistration()
parts_
[
i
]
->
move
(
position
[
d
]);
#ifdef TWO_AND_HALF_DIM
// CGoGNout<<"avant : "<<position[d];
position
[
d
]
=
parts_
[
i
]
->
getPosition
();
//recalage de l'obstacle sur ses particules (qui elles ont bien suivi la carte au sol)
// CGoGNout<<" || apres : "<<position[d]<<CGoGNendl;
#endif
...
...
@@ -1528,29 +1532,32 @@ void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atte
{
// if(sim_->envMap_.pedWayMark.isMarked(sim_->envMap_.map.phi2(dDir)))
// dDir = sim_->envMap_.map.phi1(sim_->envMap_.map.phi1(dDir));
goalVector
=
goals_
[
curGoal_
]
-
position
[
groundFace
]
;
VEC3
mid
=
(
sim_
->
envMap_
.
position
[
dDir
]
+
sim_
->
envMap_
.
position
[
sim_
->
envMap_
.
map
.
phi1
(
dDir
)])
*
0.5
f
;
goalVector
=
mid
-
position
[
0
]
;
Dart
dStart
=
dDir
;
VEC3
dir
=
center
-
position
[
0
];
while
(
goalVector
.
norm2
()
==
0
||
sim_
->
envMap_
.
pedWayMark
.
isMarked
(
sim_
->
envMap_
.
map
.
phi2
(
dDir
))
||
((
dir
*
goalVector
)
>
0
))
{
// std::cout << "d " << dDir << std::endl;
// std::cout << "dir*goalVector " << dir*goalVector << std::endl;
// std::cout << "goal vec " << goalVector << std::endl;
dDir
=
sim_
->
envMap_
.
map
.
phi_1
(
dDir
);
mid
=
(
sim_
->
envMap_
.
position
[
dDir
]
+
sim_
->
envMap_
.
position
[
sim_
->
envMap_
.
map
.
phi1
(
dDir
)])
*
0.5
f
;
goalVector
=
mid
-
position
[
0
]
;
if
(
dDir
==
dStart
)
break
;
}
if
(
sim_
->
envMap_
.
map
.
isBoundaryEdge
(
sim_
->
envMap_
.
map
.
phi2
(
dDir
)))
goalVector
=
VEC3
(
0
);
///////////////////////////////////////////////WTH
// VEC3 mid = (sim_->envMap_.position[dDir]+sim_->envMap_.position[sim_->envMap_.map.phi1(dDir)])*0.5f;
// goalVector = mid - position[0] ;
//
// Dart dStart=dDir;
// VEC3 dir = center - position[0];
// while(goalVector.norm2()==0 || sim_->envMap_.pedWayMark.isMarked(sim_->envMap_.map.phi2(dDir)) || ((dir*goalVector)>0))
// {
// // std::cout << "d " << dDir << std::endl;
// // std::cout << "dir*goalVector " << dir*goalVector << std::endl;
// // std::cout << "goal vec " << goalVector << std::endl;
//
// dDir = sim_->envMap_.map.phi_1(dDir);
// mid = (sim_->envMap_.position[dDir]+sim_->envMap_.position[sim_->envMap_.map.phi1(dDir)])*0.5f;
// goalVector = mid - position[0] ;
//
// if(dDir==dStart)
// break;
// }
//
// if(sim_->envMap_.map.isBoundaryEdge(sim_->envMap_.map.phi2(dDir)))
// goalVector = VEC3(0);
/////////////////////////////////////////////////////////////////
// goalVector = mid - center ;
// std::cout << "dDir " << dDir << " goalVec" << goalVector << std::endl;
...
...
src/simulator.cpp
View file @
e6def94a
...
...
@@ -17,7 +17,7 @@ timespec timespec_delta(const timespec& t1, const timespec& t2) {
Simulator
::
Simulator
(
unsigned
int
config
,
unsigned
int
minS
,
unsigned
int
nbAgent
,
unsigned
int
nbObst
,
bool
resolution
)
:
#ifdef TWO_AND_HALF_DIM
timeStep_
(
0.
2
5
f
),
timeStep_
(
0.
0
5
f
),
#else
// timeStep_(config > 2 ? 0.01f : 0.25f),
timeStep_
(
0.01
f
),
...
...
src/viewer.cpp
View file @
e6def94a
...
...
@@ -773,6 +773,8 @@ void SocialAgents::cb_redraw()
m_ds
->
end
();
m_ds
->
endList
();
}
if
(
drawMovingObstacles
)
{
#ifdef SHADOWSHELL
...
...
@@ -873,6 +875,22 @@ void SocialAgents::cb_redraw()
if
(
draw_dart
&&
dartSlider
<
(
simulator
.
envMap_
.
map
.
end
().
index
))
m_renderTopo
->
overdrawDart
(
dartSlider
,
5.0
f
,
0
,
1.0
f
,
0.5
f
);
if
(
drawObstacles
)
{
CGoGN
::
CellMarkerStore
<
FACE
>
m
(
simulator
.
envMap_
.
map
)
;
for
(
Dart
d
=
simulator
.
envMap_
.
map
.
begin
();
d
!=
simulator
.
envMap_
.
map
.
end
();
simulator
.
envMap_
.
map
.
next
(
d
))
{
if
(
!
m
.
isMarked
(
d
))
{
m
.
mark
(
d
)
;
if
(
simulator
.
envMap_
.
obstvect
[
d
].
size
()
!=
0
)
{
drawCell
(
d
,
5.0
f
,
0
,
1.0
f
,
0.5
f
);
}
}
}
}
}
if
(
drawEnvFaces
)
...
...
@@ -1199,7 +1217,7 @@ void SocialAgents::animate()
}
struct
timespec
begTime
;
clock_gettime
(
CLOCK_MONOTONIC
,
&
begTime
)
;
for
(
int
n
=
1
;
n
<
30
;
n
++
)
for
(
int
n
=
1
;
n
<
2
;
n
++
)
simulator
.
doStep
()
;
struct
timespec
endTime
;
clock_gettime
(
CLOCK_MONOTONIC
,
&
endTime
)
;
...
...
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