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
cc85941a
Commit
cc85941a
authored
Jan 09, 2013
by
Thomas Jund
Browse files
limace improved
parent
d44c4a4f
Changes
6
Hide whitespace changes
Inline
Side-by-side
include/env_map.h
View file @
cc85941a
...
...
@@ -79,7 +79,7 @@ public:
void
popAgentInCells
(
Agent
*
agent
,
Dart
d
)
;
void
agentChangeFace
(
Agent
*
agent
,
Dart
oldFace
)
;
void
popAndPushObstacleInCells
(
Obstacle
*
o
,
int
n
);
Dart
popAndPushObstacleInCells
(
Obstacle
*
o
,
int
n
);
void
pushObstacleInCells
(
Obstacle
*
mo
);
void
pushObstacleInOneRingCells
(
Obstacle
*
o
,
Dart
d
,
int
n
);
void
pushObstacleInCells
(
Obstacle
*
o
,
int
n
,
const
std
::
vector
<
Dart
>&
memo_cross
);
...
...
include/moving_obstacle.h
View file @
cc85941a
...
...
@@ -75,6 +75,7 @@ public:
std
::
vector
<
VEC3
>
goals_
;
unsigned
int
curGoal_
;
Dart
dDir
;
// static float neighborDistSq_;
static
unsigned
int
maxNeighbors_
;
...
...
src/env_map.cpp
View file @
cc85941a
...
...
@@ -554,7 +554,7 @@ void EnvMap::pushObstacleInCells(Obstacle * o)// réenregistre l'obstacle en que
}
}
void
EnvMap
::
popAndPushObstacleInCells
(
Obstacle
*
o
,
int
n
)
// maj de l'enregistrement
Dart
EnvMap
::
popAndPushObstacleInCells
(
Obstacle
*
o
,
int
n
)
// maj de l'enregistrement
{
MovingObstacle
*
mo
=
o
->
mo
;
...
...
@@ -593,6 +593,8 @@ void EnvMap::popAndPushObstacleInCells(Obstacle* o, int n)// maj de l'enregistre
pushObstacleInCells
(
o
,
n
,
memo
);
}
}
return
d1
;
}
void
EnvMap
::
pushObstacleInOneRingCells
(
Obstacle
*
o
,
Dart
d
,
int
n
)
...
...
src/moving_obstacle.cpp
View file @
cc85941a
...
...
@@ -144,6 +144,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
{
edgeLength
[
d
]
=
VEC3
(
position
[
map
.
phi1
(
d
)]
-
position
[
d
]).
norm
();
}
dDir
=
dInside
;
}
center
/=
nbVertices
;
...
...
@@ -197,8 +199,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
VEC3
MovingObstacle
::
getDilatedPosition
(
unsigned
int
ind
)
{
Dart
d
(
ind
);
//WARNING : works only for one face created at start !
return
position
[
d
]
+
deformation
[
d
];
//
return position[d];
//
return position[d]+deformation[d];
return
position
[
d
];
}
VEC3
MovingObstacle
::
getPosition
(
unsigned
int
ind
)
...
...
@@ -422,11 +424,11 @@ void MovingObstacle::update()
VEC3
v1
=
(
position
[
map
.
phi1
(
dd
)]
-
position
[
dd
]);
//inertie
velocity
[
dd
]
=
velocity
[
dd
]
*
0.9
f
;
velocity
[
dd
]
=
velocity
[
dd
]
*
0.9
0
f
;
//stretch spring : /!\ max rigidity relative to the timestep used (unstable otherwise)
float
norm
=
v1
.
norm
();
float
rigidity
=
50
0.0
f
;
float
rigidity
=
4
0.0
f
;
float
stretch
=
rigidity
*
(
edgeLength
[
dd
]
-
v1
.
norm
());
VEC3
f
=
stretch
*
(
v1
/
norm
)
*
sim_
->
timeStep_
;
...
...
@@ -495,8 +497,8 @@ void MovingObstacle::update()
map
.
next
(
d
);
}
if
(
!
rigid_
)
center
=
position
[
groundFace
];
//
if(!rigid_)
//
center = position[groundFace];
//-------- code ajoute par Arash pour les obstacles rectangulaires --------------
...
...
@@ -526,7 +528,10 @@ void MovingObstacle::update()
o
->
p2
=
getDilatedPosition
((
i
+
1
)
%
nbVertices
);
o
->
prevP
=
getDilatedPosition
((
i
-
1
+
nbVertices
)
%
nbVertices
);
o
->
nextP
=
getDilatedPosition
((
i
+
2
+
nbVertices
)
%
nbVertices
);
sim_
->
envMap_
.
popAndPushObstacleInCells
(
o
,
i
);
Dart
d
=
sim_
->
envMap_
.
popAndPushObstacleInCells
(
o
,
i
);
if
(
i
==
0
)
dDir
=
d
;
if
(
sim_
->
detect_agent_collision
)
{
...
...
@@ -568,6 +573,7 @@ std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest
registering_part
->
move
(
pos
);
d1
=
registering_part
->
d
;
// CGoGNout<<"d1 : "<< *d1<<"|| d2 : "<< *d2<<"|| start : "<< pos<<"|| stop : "<< dest<<CGoGNendl;
return
(
registering_part
->
move
(
dest
));
}
...
...
@@ -621,23 +627,64 @@ void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atte
VEC3
goalVector
;
if
(
index_parent
<
1
)
{
goalVector
=
goals_
[
curGoal_
]
-
center
;
// goalVector = goals_[curGoal_] - position[groundFace] ;
float
goalDist2
=
goalVector
.
norm2
()
;
if
(
goalDist2
<
5.0
f
)
if
(
rigid_
)
{
curGoal_
=
(
curGoal_
+
1
)
%
goals_
.
size
()
;
goalVector
=
goals_
[
curGoal_
]
-
center
;
// goalVector = goals_[curGoal_] - position[groundFace] ;
goalDist2
=
goalVector
.
norm2
()
;
}
// goalVector = goals_[curGoal_] - position[groundFace] ;
if
(
goalDist2
>
maxSpeed_
)
float
goalDist2
=
goalVector
.
norm2
()
;
if
(
goalDist2
<
5.0
f
)
{
curGoal_
=
(
curGoal_
+
1
)
%
goals_
.
size
()
;
goalVector
=
goals_
[
curGoal_
]
-
center
;
// goalVector = goals_[curGoal_] - position[groundFace] ;
goalDist2
=
goalVector
.
norm2
()
;
}
if
(
goalDist2
>
maxSpeed_
)
{
goalVector
.
normalize
()
;
goalVector
*=
maxSpeed_
;
}
}
else
{
goalVector
.
normalize
()
;
goalVector
*=
maxSpeed_
;
// if(sim_->envMap_.pedWayMark.isMarked(sim_->envMap_.map.phi2(dDir)))
// dDir = sim_->envMap_.map.phi1(sim_->envMap_.map.phi1(dDir));
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
);
// goalVector = mid - center ;
// std::cout << "dDir " << dDir << " goalVec" << goalVector << std::endl;
// float goalDist2 = goalVector.norm2() ;
// if (goalDist2 > maxSpeed_)
{
goalVector
.
normalize
()
;
goalVector
*=
maxSpeed_
*
maxSpeed_
;
}
}
}
else
...
...
src/simulator.cpp
View file @
cc85941a
#include
"simulator.h"
Simulator
::
Simulator
(
unsigned
int
config
,
unsigned
int
minS
,
unsigned
int
nbAgent
,
unsigned
int
nbObst
)
:
timeStep_
(
0.0
2
f
),
timeStep_
(
0.0
5
f
),
globalTime_
(
0.0
f
),
nbSteps_
(
0
),
nbUpdates
(
0
),
...
...
@@ -36,7 +36,7 @@ void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst
{
case
0
:
setupCircleScenario
(
nbAgent
,
nbObst
)
;
addPathsToAgents
();
//
addPathsToAgents();
break
;
case
1
:
setupCorridorScenario
(
nbAgent
,
nbObst
)
;
...
...
@@ -58,7 +58,7 @@ void Simulator::init( float dimension, unsigned int nbAgent, unsigned int nbObst
addPathsToAgents
();
// SelectorCellNotMarked<PFP::MAP> scnm(envMap_.pedWayMark);
addMovingObstacles
(
nbObst
);
addPathToObstacles
();
//
addPathToObstacles();
break
;
default:
std
::
cout
<<
"Unknown scenario !"
<<
std
::
endl
;
...
...
@@ -175,8 +175,8 @@ void Simulator::doStep()
nbRefineCandidate
+=
envMap_
.
refineCandidate
.
size
()
;
nbCoarsenCandidate
+=
envMap_
.
coarsenCandidate
.
size
()
;
//
if (multires)
//
envMap_.updateMap() ;
if
(
multires
)
envMap_
.
updateMap
()
;
#endif
globalTime_
+=
timeStep_
;
...
...
@@ -641,6 +641,8 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
{
std
::
vector
<
VEC3
>
vPos
;
VEC3
start
;
MovingMesh
*
mm
;
float
maxHeight
=
10.0
f
;
switch
(
obstType
)
{
...
...
@@ -658,9 +660,8 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
break
;
case
1
:
{
MovingMesh
*
mm
=
new
MovingMesh
(
envMap_
,
d
,
"meshRessources/Limace.ply"
);
mm
=
new
MovingMesh
(
envMap_
,
d
,
"meshRessources/Limace.ply"
);
movingMeshes_
.
push_back
(
mm
);
float
maxHeight
=
10.0
f
;
vPos
=
mm
->
computeProjectedPointSet
(
maxHeight
);
}
break
;
...
...
@@ -669,6 +670,33 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
return
;
}
bool
orientAccordingToFace
=
true
;
if
(
orientAccordingToFace
)
{
while
(
envMap_
.
pedWayMark
.
isMarked
(
envMap_
.
map
.
phi2
(
d
)))
{
d
=
envMap_
.
map
.
phi1
(
d
);
}
VEC3
mid
=
(
envMap_
.
position
[
d
]
+
envMap_
.
position
[
envMap_
.
map
.
phi1
(
d
)])
*
0.5
f
;
VEC3
front
=
vPos
[
0
];
VEC3
bary
(
0
);
for
(
std
::
vector
<
VEC3
>::
iterator
it
=
vPos
.
begin
()
;
it
!=
vPos
.
end
()
;
++
it
)
bary
+=
*
it
;
bary
/=
vPos
.
size
();
float
angle
=
get_angle
(
mid
-
bary
,
front
-
bary
);
TraversorV
<
PFP
::
MAP
>
tv
(
mm
->
map
);
for
(
Dart
d
=
tv
.
begin
()
;
d
!=
tv
.
end
()
;
d
=
tv
.
next
())
{
mm
->
position
[
d
]
+=
rotate
(
mm
->
position
[
d
],
bary
,
angle
);
}
vPos
=
mm
->
computeProjectedPointSet
(
maxHeight
);
}
std
::
vector
<
VEC3
>
goals
;
goals
.
push_back
(
start
);
MovingObstacle
*
mo
=
new
MovingObstacle
(
this
,
movingObstacles_
.
size
(),
vPos
,
goals
,
(
obstType
==
0
),
false
);
...
...
@@ -723,7 +751,6 @@ void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
for
(
std
::
vector
<
Dart
>::
iterator
it
=
path
.
begin
()
;
it
!=
path
.
end
()
;
++
it
)
{
// VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
VEC3
dest
=
envMap_
.
position
[
*
it
]
+
envMap_
.
position
[
envMap_
.
map
.
phi1
(
*
it
)]
;
dest
/=
2.0
f
;
...
...
@@ -738,29 +765,29 @@ void Simulator::addPathToObstacle(MovingObstacle * mo, Dart dStart, Dart dGoal)
for
(
std
::
vector
<
Dart
>::
iterator
it
=
path
.
begin
()
;
it
!=
path
.
end
()
;
++
it
)
{
// VEC3 dest = Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position) ;
VEC3
dest
=
envMap_
.
position
[
*
it
]
+
envMap_
.
position
[
envMap_
.
map
.
phi1
(
*
it
)]
;
dest
/=
2.0
f
;
mo
->
goals_
.
push_back
(
dest
)
;
}
if
(((
mo
->
front
-
mo
->
center
)
*
(
mo
->
goals_
[
mo
->
curGoal_
+
1
]
-
mo
->
center
))
<
0
)
{
std
::
cout
<<
__FILE__
<<
" "
<<
__LINE__
<<
" rotate limace"
<<
std
::
endl
;
Geom
::
Matrix44f
m
;
m
.
identity
();
rotateZ
(
180.0
f
,
m
);
TraversorV
<
PFP
::
MAP
>
tv
(
mo
->
map
);
for
(
Dart
d
=
tv
.
begin
()
;
d
!=
tv
.
end
()
;
d
=
tv
.
next
())
{
mo
->
position
[
d
]
-=
mo
->
center
;
mo
->
position
[
d
]
=
Geom
::
transform
(
mo
->
position
[
d
],
m
);
mo
->
position
[
d
]
+=
mo
->
center
;
}
}
// if(((mo->front-mo->center)*(mo->goals_[mo->curGoal_]-mo->center))<0)
// {
// std::cout << __FILE__ << " " << __LINE__ << " rotate limace" << std::endl;
//
// Geom::Matrix44f m;
// m.identity();
// rotateZ(180.0f,m);
//
// TraversorV<PFP::MAP> tv(mo->map);
// for(Dart d = tv.begin() ; d != tv.end() ; d = tv.next())
// {
// mo->position[d] -= mo->center;
// mo->position[d] = Geom::transform(mo->position[d],m);
// mo->position[d] += mo->center;
// mo->position[d][2] += 10.0f;
// }
// }
}
#ifndef SPATIAL_HASHING
...
...
src/viewer.cpp
View file @
cc85941a
...
...
@@ -414,8 +414,13 @@ void SocialAgents::cb_redraw()
if
(
drawObstPredictionTri
)
{
// for (std::vector<MovingObstacle*>::iterator it = sim.movingObstacles_.begin() ; it != sim.movingObstacles_.end() ; ++it)
// {
for
(
std
::
vector
<
MovingObstacle
*>::
iterator
it
=
simulator
.
movingObstacles_
.
begin
()
;
it
!=
simulator
.
movingObstacles_
.
end
()
;
++
it
)
{
renderPredictionTriangle
(
simulator
.
envMap_
,
(
*
it
)
->
dDir
,
(
*
it
)
->
getDilatedPosition
(
0
))
;
std
::
cout
<<
"dDir2 "
<<
(
*
it
)
->
dDir
<<
std
::
endl
;
// for(unsigned int i = 0 ; i < (*it)->nbVertices ; ++i)
// {
// CGoGN::Algo::MovingObjects::ParticleCell2D<PFP> p = *(*it)->parts_[i];
...
...
@@ -428,7 +433,7 @@ void SocialAgents::cb_redraw()
// glVertex3fv(p.getPosition().data());
// glEnd();
// }
//
}
}
}
#endif
...
...
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