Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Sign in
Toggle navigation
Menu
Open sidebar
vadere
vadere
Commits
d099754c
Commit
d099754c
authored
Dec 16, 2016
by
Michael Seitz
Browse files
Add BehaviouralHeuristicsModel.
parent
0b345001
Changes
10
Hide whitespace changes
Inline
Side-by-side
VadereSimulator/src/org/vadere/simulator/models/bhm/BehaviouralHeuristicsModel.java
0 → 100644
View file @
d099754c
package
org.vadere.simulator.models.bhm
;
import
java.util.Comparator
;
import
java.util.LinkedList
;
import
java.util.List
;
import
java.util.PriorityQueue
;
import
java.util.Random
;
import
org.vadere.simulator.models.MainModel
;
import
org.vadere.simulator.models.Model
;
import
org.vadere.state.attributes.Attributes
;
import
org.vadere.state.attributes.models.AttributesBHM
;
import
org.vadere.state.attributes.scenario.AttributesAgent
;
import
org.vadere.state.scenario.DynamicElement
;
import
org.vadere.state.scenario.Pedestrian
;
import
org.vadere.state.scenario.Target
;
import
org.vadere.state.scenario.Topography
;
import
org.vadere.util.geometry.shapes.VPoint
;
public
class
BehaviouralHeuristicsModel
implements
MainModel
{
/**
* Compares the time of the next possible move.
*/
private
class
ComparatorPedestrianBHM
implements
Comparator
<
PedestrianBHM
>
{
@Override
public
int
compare
(
PedestrianBHM
ped1
,
PedestrianBHM
ped2
)
{
if
(
ped1
.
getTimeOfNextStep
()
<
ped2
.
getTimeOfNextStep
())
{
return
-
1
;
}
else
{
return
1
;
}
}
}
private
List
<
Model
>
models
=
new
LinkedList
<>();
private
AttributesBHM
attributesBHM
;
private
AttributesAgent
attributesPedestrian
;
private
Random
random
;
private
Topography
topography
;
private
double
lastSimTimeInSec
;
private
int
pedestrianIdCounter
;
private
PriorityQueue
<
PedestrianBHM
>
pedestrianEventsQueue
;
public
BehaviouralHeuristicsModel
()
{
this
.
pedestrianIdCounter
=
0
;
this
.
pedestrianEventsQueue
=
new
PriorityQueue
<>(
100
,
new
ComparatorPedestrianBHM
());
}
@Override
public
void
initialize
(
List
<
Attributes
>
modelAttributesList
,
Topography
topography
,
AttributesAgent
attributesPedestrian
,
Random
random
)
{
this
.
attributesBHM
=
Model
.
findAttributes
(
modelAttributesList
,
AttributesBHM
.
class
);
this
.
attributesPedestrian
=
attributesPedestrian
;
this
.
topography
=
topography
;
this
.
random
=
random
;
this
.
models
.
add
(
this
);
}
@Override
public
<
T
extends
DynamicElement
>
PedestrianBHM
createElement
(
VPoint
position
,
int
id
,
Class
<
T
>
type
)
{
if
(!
Pedestrian
.
class
.
isAssignableFrom
(
type
))
throw
new
IllegalArgumentException
(
"BHM cannot initialize "
+
type
.
getCanonicalName
());
pedestrianIdCounter
++;
AttributesAgent
pedAttributes
=
new
AttributesAgent
(
this
.
attributesPedestrian
,
id
>
0
?
id
:
pedestrianIdCounter
);
PedestrianBHM
pedestrian
=
new
PedestrianBHM
(
topography
,
pedAttributes
,
attributesBHM
,
random
);
pedestrian
.
setPosition
(
position
);
this
.
pedestrianEventsQueue
.
add
(
pedestrian
);
return
pedestrian
;
}
@Override
public
void
preLoop
(
final
double
simTimeInSec
)
{
this
.
lastSimTimeInSec
=
simTimeInSec
;
}
@Override
public
void
postLoop
(
double
simTimeInSec
)
{}
@Override
public
void
update
(
final
double
simTimeInSec
)
{
// event driven update
if
(!
pedestrianEventsQueue
.
isEmpty
())
{
while
(
pedestrianEventsQueue
.
peek
().
getTimeOfNextStep
()
<
simTimeInSec
)
{
PedestrianBHM
ped
=
pedestrianEventsQueue
.
poll
();
if
(
ped
.
hasNextTarget
())
{
ped
.
update
(
simTimeInSec
);
Target
target
=
topography
.
getTarget
(
ped
.
getNextTargetId
());
if
(!(
target
.
getShape
().
contains
(
ped
.
getPosition
())
&&
target
.
isAbsorbing
()))
{
pedestrianEventsQueue
.
add
(
ped
);
}
}
if
(
pedestrianEventsQueue
.
isEmpty
())
{
break
;
}
}
}
}
@Override
public
List
<
Model
>
getSubmodels
()
{
return
models
;
}
}
VadereSimulator/src/org/vadere/simulator/models/bhm/DirectionAddend.java
0 → 100644
View file @
d099754c
package
org.vadere.simulator.models.bhm
;
import
org.vadere.util.geometry.shapes.VPoint
;
public
interface
DirectionAddend
{
public
VPoint
getDirectionAddend
();
}
VadereSimulator/src/org/vadere/simulator/models/bhm/DirectionAddendObstacle.java
0 → 100644
View file @
d099754c
package
org.vadere.simulator.models.bhm
;
import
java.util.List
;
import
org.apache.log4j.LogManager
;
import
org.apache.log4j.Logger
;
import
org.vadere.state.attributes.models.AttributesBHM
;
import
org.vadere.state.scenario.Obstacle
;
import
org.vadere.util.geometry.shapes.VPoint
;
public
class
DirectionAddendObstacle
implements
DirectionAddend
{
private
static
Logger
logger
=
LogManager
.
getLogger
(
DirectionAddendObstacle
.
class
);
private
final
AttributesBHM
attributesBHM
;
private
final
PedestrianBHM
me
;
public
DirectionAddendObstacle
(
PedestrianBHM
me
)
{
this
.
me
=
me
;
this
.
attributesBHM
=
me
.
getAttributesBHM
();
}
@Override
public
VPoint
getDirectionAddend
()
{
return
getTargetObstacleDirection
();
}
public
VPoint
getTargetObstacleDirection
()
{
VPoint
result
=
VPoint
.
ZERO
;
List
<
Obstacle
>
closeObstacles
=
me
.
detectObstacleProximity
(
me
.
getPosition
(),
me
.
getRadius
()
+
attributesBHM
.
getObstacleRepulsionReach
());
double
normFactor
=
attributesBHM
.
getObstacleRepulsionMaxWeight
()
/
sumOfObstacleWeights
(
closeObstacles
);
for
(
Obstacle
obstacle
:
closeObstacles
)
{
double
weight
=
weightObstacleDistance
(
obstacle
.
getShape
().
distance
(
me
.
getPosition
())
+
me
.
getRadius
());
if
(
weight
>
0.001
&&
normFactor
>
0.001
)
{
weight
=
weight
*
weight
*
normFactor
;
VPoint
direction
=
me
.
getPosition
().
subtract
(
obstacle
.
getShape
().
closestPoint
(
me
.
getPosition
())).
norm
();
result
=
result
.
add
(
direction
.
scalarMultiply
(
weight
));
}
}
return
result
;
}
private
double
sumOfObstacleWeights
(
List
<
Obstacle
>
closeObstacles
)
{
double
result
=
0
;
for
(
Obstacle
obstacle
:
closeObstacles
)
{
result
=
result
+
weightObstacleDistance
(
obstacle
.
getShape
().
distance
(
me
.
getPosition
())
+
me
.
getRadius
());
}
return
result
;
}
private
double
weightObstacleDistance
(
double
distance
)
{
double
result
=
0
;
if
(
distance
<
attributesBHM
.
getObstacleRepulsionReach
())
{
result
=
attributesBHM
.
getObstacleRepulsionReach
()
-
distance
;
result
=
result
/
attributesBHM
.
getObstacleRepulsionReach
();
}
return
result
;
}
}
VadereSimulator/src/org/vadere/simulator/models/bhm/Navigation.java
0 → 100644
View file @
d099754c
package
org.vadere.simulator.models.bhm
;
import
org.vadere.util.geometry.shapes.VPoint
;
public
interface
Navigation
{
public
VPoint
getNavigationPosition
();
}
VadereSimulator/src/org/vadere/simulator/models/bhm/NavigationCluster.java
0 → 100644
View file @
d099754c
package
org.vadere.simulator.models.bhm
;
import
java.util.LinkedList
;
import
java.util.List
;
import
java.util.Random
;
import
org.apache.log4j.LogManager
;
import
org.apache.log4j.Logger
;
import
org.vadere.state.attributes.models.AttributesBHM
;
import
org.vadere.state.scenario.Pedestrian
;
import
org.vadere.state.scenario.Topography
;
import
org.vadere.util.geometry.shapes.VPoint
;
import
org.vadere.util.geometry.shapes.VShape
;
public
class
NavigationCluster
implements
Navigation
{
private
static
Logger
logger
=
LogManager
.
getLogger
(
NavigationCluster
.
class
);
private
final
Topography
topography
;
private
final
AttributesBHM
attributesBHM
;
private
final
PedestrianBHM
me
;
private
final
NavigationProximity
proximityNavigation
;
public
NavigationCluster
(
PedestrianBHM
me
,
Topography
topography
,
Random
random
)
{
this
.
me
=
me
;
this
.
topography
=
topography
;
this
.
attributesBHM
=
me
.
getAttributesBHM
();
this
.
proximityNavigation
=
new
NavigationProximity
(
me
,
random
);
}
@Override
public
VPoint
getNavigationPosition
()
{
VPoint
result
;
Pedestrian
futureCollision
=
me
.
findCollisionPedestrian
(
me
.
computeMovementProjection
(),
false
);
if
(
futureCollision
!=
null
)
{
result
=
evadeCollision
(
futureCollision
);
}
else
{
result
=
me
.
computeTargetStep
();
}
// no good, stay where you are
if
(
me
.
collidesWithPedestrian
(
result
,
0
)
||
me
.
collidesWithObstacle
(
result
))
{
// result = me.getPosition();
result
=
proximityNavigation
.
getNavigationPosition
();
}
return
result
;
}
private
VPoint
evadeCollision
(
final
Pedestrian
collisionPed
)
{
VPoint
result
=
null
;
List
<
Pedestrian
>
cluster
=
determineCluster
(
collisionPed
);
cluster
.
remove
(
collisionPed
);
Pedestrian
avoidPedestrianMin
=
collisionPed
;
Pedestrian
avoidPedestrianMax
=
collisionPed
;
List
<
VPoint
>
evasionPoints
=
getRelativeEvasionPoints
(
collisionPed
);
// pedestrians are in collision state
if
(
evasionPoints
==
null
)
{
logger
.
warn
(
"Collision with pedestrian "
+
collisionPed
.
getId
()
+
". Pedestrian "
+
me
.
getId
()
+
" stepped away."
);
return
me
.
stepAwayFromCollision
(
collisionPed
);
}
double
angleToTarget1
=
UtilsBHM
.
angleTo
(
me
.
getTargetDirection
(),
evasionPoints
.
get
(
0
).
norm
());
double
angleToTarget2
=
UtilsBHM
.
angleTo
(
me
.
getTargetDirection
(),
evasionPoints
.
get
(
1
).
norm
());
double
extremeMinAngle
,
extremeMaxAngle
;
VPoint
extremeMinEvasionPoint
,
extremeMaxEvasionPoint
;
if
(
angleToTarget1
<
angleToTarget2
)
{
extremeMinAngle
=
angleToTarget1
;
extremeMaxAngle
=
angleToTarget2
;
extremeMinEvasionPoint
=
evasionPoints
.
get
(
0
);
extremeMaxEvasionPoint
=
evasionPoints
.
get
(
1
);
}
else
{
extremeMinAngle
=
angleToTarget2
;
extremeMaxAngle
=
angleToTarget1
;
extremeMinEvasionPoint
=
evasionPoints
.
get
(
1
);
extremeMaxEvasionPoint
=
evasionPoints
.
get
(
0
);
}
for
(
Pedestrian
ped
:
cluster
)
{
evasionPoints
=
getRelativeEvasionPoints
(
ped
);
// pedestrians are in collision state
if
(
evasionPoints
==
null
)
{
logger
.
warn
(
"Collision with pedestrian "
+
ped
.
getId
()
+
". Pedestrian "
+
me
.
getId
()
+
" stepped away."
);
return
me
.
stepAwayFromCollision
(
ped
);
}
angleToTarget1
=
UtilsBHM
.
angleTo
(
me
.
getTargetDirection
(),
evasionPoints
.
get
(
0
).
norm
());
angleToTarget2
=
UtilsBHM
.
angleTo
(
me
.
getTargetDirection
(),
evasionPoints
.
get
(
1
).
norm
());
double
angleToTargetThisMin
,
angleToTargetThisMax
;
VPoint
evasionPointMin
,
evasionPointMax
;
// only the greater angle is a candidate for a new extreme value
if
(
angleToTarget1
<
angleToTarget2
)
{
angleToTargetThisMin
=
angleToTarget1
;
angleToTargetThisMax
=
angleToTarget2
;
evasionPointMin
=
evasionPoints
.
get
(
0
);
evasionPointMax
=
evasionPoints
.
get
(
1
);
}
else
{
angleToTargetThisMin
=
angleToTarget2
;
angleToTargetThisMax
=
angleToTarget1
;
evasionPointMin
=
evasionPoints
.
get
(
1
);
evasionPointMax
=
evasionPoints
.
get
(
0
);
}
if
(
angleToTargetThisMin
<
extremeMinAngle
)
{
extremeMinAngle
=
angleToTargetThisMin
;
extremeMinEvasionPoint
=
evasionPointMin
;
avoidPedestrianMin
=
ped
;
}
if
(
angleToTargetThisMax
>
extremeMaxAngle
)
{
extremeMaxAngle
=
angleToTargetThisMax
;
extremeMaxEvasionPoint
=
evasionPointMax
;
avoidPedestrianMax
=
ped
;
}
}
result
=
selectClusterDetourStep
(
extremeMinEvasionPoint
,
extremeMaxEvasionPoint
);
// DEBUG
// logger.info("Pedestrians from cluster to avoid are " + avoidPedestrianMin.getId() + " and
// " + avoidPedestrianMax.getId());
return
result
;
}
private
VPoint
selectClusterDetourStep
(
VPoint
evasionPoint1
,
VPoint
evasionPoint2
)
{
VPoint
result
;
VPoint
evasionStep1
=
evasionPoint1
.
norm
().
scalarMultiply
(
me
.
getStepLength
()).
add
(
me
.
getPosition
());
VPoint
evasionStep2
=
evasionPoint2
.
norm
().
scalarMultiply
(
me
.
getStepLength
()).
add
(
me
.
getPosition
());
// in case of collision return other
if
(
me
.
collidesWithObstacle
(
evasionStep1
))
{
if
(
me
.
collidesWithObstacle
(
evasionStep2
))
{
// both positions are colliding, fall back to current position
return
me
.
getPosition
();
}
else
{
return
evasionStep2
;
}
}
else
if
(
me
.
collidesWithObstacle
(
evasionStep2
))
{
return
evasionStep1
;
}
VShape
target
=
topography
.
getTarget
(
me
.
getNextTargetId
()).
getShape
();
double
lastDirectionAngle1
=
UtilsBHM
.
angle
(
me
.
getTargetDirection
(),
evasionPoint1
);
double
lastDirectionAngle2
=
UtilsBHM
.
angle
(
me
.
getTargetDirection
(),
evasionPoint2
);
// compute walking distance to target through evasion points
double
detour1
=
target
.
distance
(
me
.
getPosition
().
add
(
evasionPoint1
))
+
evasionPoint1
.
distanceToOrigin
();
double
detour2
=
target
.
distance
(
me
.
getPosition
().
add
(
evasionPoint2
))
+
evasionPoint2
.
distanceToOrigin
();
// avoid walking backwards
if
(
lastDirectionAngle1
>
attributesBHM
.
getBackwardsAngle
())
{
if
(
lastDirectionAngle2
>
attributesBHM
.
getBackwardsAngle
())
if
(
detour1
<
detour2
)
{
result
=
evasionStep1
;
}
else
{
result
=
evasionStep2
;
}
else
{
result
=
evasionStep2
;
}
}
else
if
(
lastDirectionAngle2
>
attributesBHM
.
getBackwardsAngle
())
{
result
=
evasionStep1
;
}
else
{
// choose shorter detour to target
if
(
detour1
<
detour2
)
{
result
=
evasionStep1
;
}
else
{
result
=
evasionStep2
;
}
}
return
result
;
}
private
List
<
VPoint
>
getRelativeEvasionPoints
(
final
Pedestrian
collisionPed
)
{
List
<
VPoint
>
result
;
VPoint
relativeThisPosition
=
me
.
getPosition
().
subtract
(
collisionPed
.
getPosition
());
double
radius
=
me
.
getRadius
()
+
collisionPed
.
getRadius
()
+
UtilsBHM
.
DOUBLE_EPSILON
;
// pedestrians are in collision state
if
(
relativeThisPosition
.
distanceToOrigin
()
<
radius
)
{
result
=
null
;
}
else
{
List
<
VPoint
>
tangentialPoints
=
UtilsBHM
.
getTangentialPoints
(
relativeThisPosition
,
radius
);
// DEBUG
if
(
Double
.
isNaN
(
tangentialPoints
.
get
(
0
).
x
)
||
Double
.
isNaN
(
tangentialPoints
.
get
(
0
).
y
)
||
Double
.
isNaN
(
tangentialPoints
.
get
(
1
).
x
)
||
Double
.
isNaN
(
tangentialPoints
.
get
(
1
).
y
))
{
logger
.
error
(
"Tangential point NaN for pedestrian "
+
collisionPed
.
getId
()
+
"."
);
}
result
=
UtilsBHM
.
getRelativeEvasionPointFromTangential
(
relativeThisPosition
,
tangentialPoints
);
}
return
result
;
}
private
List
<
Pedestrian
>
determineCluster
(
Pedestrian
collisionPed
)
{
List
<
Pedestrian
>
result
=
new
LinkedList
<>();
LinkedList
<
Pedestrian
>
contained
=
new
LinkedList
<>();
List
<
Pedestrian
>
notContained
;
List
<
Pedestrian
>
notContainedNext
=
new
LinkedList
<>();
for
(
Pedestrian
other
:
topography
.
getElements
(
Pedestrian
.
class
))
{
// select pedestrians ahead
if
(
UtilsBHM
.
angleBetweenTarget
(
me
,
other
)
<
attributesBHM
.
getBackwardsAngle
())
{
// skip this and collision pedestrian (the latter is first to be investigated)
if
(
other
.
getId
()
!=
me
.
getId
()
&&
other
.
getId
()
!=
collisionPed
.
getId
())
{
notContainedNext
.
add
(
other
);
}
}
}
Pedestrian
containedNext
=
collisionPed
;
while
(
containedNext
!=
null
)
{
notContained
=
notContainedNext
;
notContainedNext
=
new
LinkedList
<>();
result
.
add
(
containedNext
);
// iterate through all pedestrians outside of the cluster
for
(
Pedestrian
pedNotContained
:
notContained
)
{
// add pedestrian to cluster, who is closer to the cluster than this pedestrian's
// diameter
if
(
containedNext
.
getPosition
().
distance
(
pedNotContained
.
getPosition
())
<
containedNext
.
getRadius
()
+
pedNotContained
.
getRadius
()
+
me
.
getRadius
()
*
2
+
attributesBHM
.
getDistanceToKeep
())
{
if
(
attributesBHM
.
isOnlyEvadeContraFlow
())
{
double
angleBetween
=
UtilsBHM
.
angleBetweenTargetDirection
(
me
,
pedNotContained
);
if
(
angleBetween
>
attributesBHM
.
getOnlyEvadeContraFlowAngle
())
{
contained
.
add
(
pedNotContained
);
}
}
else
{
contained
.
add
(
pedNotContained
);
}
}
else
{
notContainedNext
.
add
(
pedNotContained
);
}
}
if
(
contained
.
isEmpty
())
{
containedNext
=
null
;
}
else
{
containedNext
=
contained
.
pop
();
}
}
return
result
;
}
}
VadereSimulator/src/org/vadere/simulator/models/bhm/NavigationFollower.java
0 → 100644
View file @
d099754c
package
org.vadere.simulator.models.bhm
;
import
java.util.LinkedList
;
import
java.util.List
;
import
java.util.Random
;
import
org.apache.log4j.LogManager
;
import
org.apache.log4j.Logger
;
import
org.vadere.state.attributes.models.AttributesBHM
;
import
org.vadere.state.scenario.Pedestrian
;
import
org.vadere.state.scenario.Topography
;
import
org.vadere.util.geometry.shapes.VPoint
;
public
class
NavigationFollower
implements
Navigation
{
private
static
Logger
logger
=
LogManager
.
getLogger
(
NavigationFollower
.
class
);
private
final
Topography
topography
;
private
final
AttributesBHM
attributesBHM
;
private
final
PedestrianBHM
me
;
private
final
NavigationProximity
proximityNavigation
;
public
NavigationFollower
(
PedestrianBHM
me
,
Topography
topography
,
Random
random
)
{
this
.
me
=
me
;
this
.
topography
=
topography
;
this
.
attributesBHM
=
me
.
getAttributesBHM
();
this
.
proximityNavigation
=
new
NavigationProximity
(
me
,
random
);
}
@Override
public
VPoint
getNavigationPosition
()
{
VPoint
result
;
Pedestrian
futureCollision
=
me
.
findCollisionPedestrian
(
me
.
computeMovementProjection
(),
false
);
if
(
futureCollision
!=
null
)
{
result
=
selectFollowingPosition
();
if
(
result
==
null
)
{
result
=
proximityNavigation
.
getNavigationPosition
();
}
else
if
(
me
.
collidesWithObstacle
(
result
)
||
me
.
collidesWithPedestrianOnPath
(
result
))
{
if
(
attributesBHM
.
isFollowerProximityNavigation
())
{
result
=
proximityNavigation
.
getNavigationPosition
();
}
else
{
result
=
me
.
getPosition
();
}
}
}
else
{
result
=
proximityNavigation
.
getNavigationPosition
();
}
return
result
;
}
private
VPoint
selectFollowingPosition
()
{
VPoint
result
=
null
;
Pedestrian
pedestrianToFollow
=
selectPedestrianToFollow
();
if
(
pedestrianToFollow
!=
null
)
{
VPoint
followDirection
=
pedestrianToFollow
.
getPosition
().
subtract
(
me
.
getPosition
()).
norm
();
result
=
followDirection
.
scalarMultiply
(
me
.
getStepLength
()).
add
(
me
.
getPosition
());
}
return
result
;
}