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
e4472e93
Commit
e4472e93
authored
Apr 26, 2018
by
Benedikt Zoennchen
Browse files
replace obstacle distance computation by fast marching method.
parent
9bcf02f7
Changes
13
Expand all
Hide whitespace changes
Inline
Side-by-side
VadereModelTests/TestOSM/scenarios/basic_1_chicken_osm1.scenario
View file @
e4472e93
This diff is collapsed.
Click to expand it.
VadereSimulator/src/org/vadere/simulator/control/Simulation.java
View file @
e4472e93
...
...
@@ -6,19 +6,26 @@ import org.vadere.simulator.models.DynamicElementFactory;
import
org.vadere.simulator.models.MainModel
;
import
org.vadere.simulator.models.Model
;
import
org.vadere.simulator.models.potential.PotentialFieldModel
;
import
org.vadere.simulator.models.potential.fields.IPotentialField
;
import
org.vadere.simulator.models.potential.fields.IPotentialFieldTarget
;
import
org.vadere.simulator.models.potential.fields.ObstacleDistancePotential
;
import
org.vadere.simulator.projects.ScenarioStore
;
import
org.vadere.simulator.projects.dataprocessing.ProcessorManager
;
import
org.vadere.state.attributes.AttributesSimulation
;
import
org.vadere.state.attributes.models.AttributesFloorField
;
import
org.vadere.state.attributes.scenario.AttributesAgent
;
import
org.vadere.state.scenario.Source
;
import
org.vadere.state.scenario.Target
;
import
org.vadere.state.scenario.Topography
;
import
org.vadere.util.geometry.shapes.VPoint
;
import
org.vadere.util.geometry.shapes.VRectangle
;
import
java.util.Collection
;
import
java.util.LinkedList
;
import
java.util.List
;
import
java.util.Random
;
import
java.util.function.Function
;
import
java.util.stream.Collectors
;
public
class
Simulation
{
...
...
@@ -72,6 +79,13 @@ public class Simulation {
this
.
targetControllers
=
new
LinkedList
<>();
this
.
topography
=
scenarioStore
.
topography
;
IPotentialField
distanceField
=
new
ObstacleDistancePotential
(
topography
.
getObstacles
().
stream
().
map
(
obs
->
obs
.
getShape
()).
collect
(
Collectors
.
toList
()),
new
VRectangle
(
topography
.
getBounds
()),
new
AttributesFloorField
());
Function
<
VPoint
,
Double
>
obstacleDistance
=
p
->
distanceField
.
getPotential
(
p
,
null
);
this
.
topography
.
setObstacleDistanceFunction
(
obstacleDistance
);
this
.
runTimeInSec
=
attributesSimulation
.
getFinishTime
();
this
.
startTimeInSec
=
startTimeInSec
;
this
.
simTimeInSec
=
startTimeInSec
;
...
...
VadereSimulator/src/org/vadere/simulator/models/potential/PotentialFieldObstacleCompactSoftshell.java
View file @
e4472e93
...
...
@@ -24,25 +24,27 @@ public class PotentialFieldObstacleCompactSoftshell implements PotentialFieldObs
private
double
width
;
private
double
height
;
private
Collection
<
Obstacle
>
obstacles
;
private
Topography
topography
;
public
PotentialFieldObstacleCompactSoftshell
(
AttributesPotentialCompactSoftshell
attributesPotential
,
Collection
<
Obstacle
>
obstacles
,
Random
random
)
{
Topography
topography
,
Random
random
)
{
this
.
attributes
=
attributesPotential
;
this
.
random
=
random
;
this
.
width
=
attributesPotential
.
getObstPotentialWidth
();
this
.
height
=
attributesPotential
.
getObstPotentialHeight
();
this
.
obstacles
=
o
bstacles
;
this
.
topography
=
topography
;
this
.
obstacles
=
topography
.
getO
bstacles
()
;
}
@Override
public
double
getObstaclePotential
(
VPoint
pos
,
Agent
pedestrian
)
{
double
potential
=
0
;
for
(
Obstacle
obstacle
:
obstacles
)
{
//
for (Obstacle obstacle : obstacles) {
double
distance
=
obstacle
.
getShape
().
distance
(
pos
);
//double distance = obstacle.getShape().distance(pos);
double
distance
=
topography
.
distanceToObstacle
(
pos
);
double
radius
=
pedestrian
.
getRadius
();
double
currentPotential
=
0
;
...
...
@@ -56,7 +58,7 @@ public class PotentialFieldObstacleCompactSoftshell implements PotentialFieldObs
if
(
potential
<
currentPotential
)
potential
=
currentPotential
;
}
//
}
return
potential
;
}
...
...
@@ -69,7 +71,7 @@ public class PotentialFieldObstacleCompactSoftshell implements PotentialFieldObs
@Override
public
PotentialFieldObstacle
copy
()
{
return
new
PotentialFieldObstacleCompactSoftshell
(
attributes
,
new
LinkedList
<>(
obstacles
)
,
random
);
return
new
PotentialFieldObstacleCompactSoftshell
(
attributes
,
topography
,
random
);
}
@Override
...
...
VadereSimulator/src/org/vadere/simulator/models/potential/PotentialFieldPedestrianCompactSoftshell.java
View file @
e4472e93
...
...
@@ -47,25 +47,29 @@ public class PotentialFieldPedestrianCompactSoftshell implements PotentialFieldA
@Override
public
double
getAgentPotential
(
VPoint
pos
,
Agent
pedestrian
,
Agent
otherPedestrian
)
{
double
distance
=
otherPedestrian
.
getPosition
().
distance
(
pos
);
int
intPower
=
this
.
attributes
.
getIntimateSpacePower
();
int
perPower
=
this
.
attributes
.
getPersonalSpacePower
();
double
factor
=
this
.
attributes
.
getIntimateSpaceFactor
();
double
radii
=
pedestrian
.
getRadius
()
+
otherPedestrian
.
getRadius
();
double
potential
=
0
;
double
distnaceSq
=
otherPedestrian
.
getPosition
().
distanceSq
(
pos
);
double
maxDistanceSq
=
(
Math
.
max
(
personalWidth
,
intimateWidth
)
+
radii
)
*
(
Math
.
max
(
personalWidth
,
intimateWidth
)
+
radii
);
double
radii
=
pedestrian
.
getRadius
()
+
otherPedestrian
.
getRadius
();
if
(
distnaceSq
<
maxDistanceSq
)
{
double
distance
=
otherPedestrian
.
getPosition
().
distance
(
pos
);
if
(
distance
<
personalWidth
+
radii
)
{
potential
+=
this
.
height
*
Math
.
exp
(
4
/
(
Math
.
pow
(
distance
/
(
personalWidth
+
radii
),
(
2
*
perPower
))
-
1
));
}
if
(
distance
<
this
.
intimateWidth
+
radii
)
{
potential
+=
this
.
height
/
factor
*
Math
.
exp
(
4
/
(
Math
.
pow
(
distance
/
(
this
.
intimateWidth
+
radii
),
(
2
*
intPower
))
-
1
));
}
if
(
distance
<
radii
)
{
potential
+=
1000
*
Math
.
exp
(
1
/
(
Math
.
pow
(
distance
/
radii
,
4
)
-
1
));
int
intPower
=
this
.
attributes
.
getIntimateSpacePower
();
int
perPower
=
this
.
attributes
.
getPersonalSpacePower
();
double
factor
=
this
.
attributes
.
getIntimateSpaceFactor
();
if
(
distance
<
personalWidth
+
radii
)
{
potential
+=
this
.
height
*
Math
.
exp
(
4
/
(
Math
.
pow
(
distance
/
(
personalWidth
+
radii
),
(
2
*
perPower
))
-
1
));
}
if
(
distance
<
this
.
intimateWidth
+
radii
)
{
potential
+=
this
.
height
/
factor
*
Math
.
exp
(
4
/
(
Math
.
pow
(
distance
/
(
this
.
intimateWidth
+
radii
),
(
2
*
intPower
))
-
1
));
}
if
(
distance
<
radii
)
{
potential
+=
1000
*
Math
.
exp
(
1
/
(
Math
.
pow
(
distance
/
radii
,
4
)
-
1
));
}
}
return
potential
;
...
...
VadereSimulator/src/org/vadere/simulator/models/potential/fields/AbstractPotentialFieldTarget.java
View file @
e4472e93
...
...
@@ -62,16 +62,18 @@ public abstract class AbstractPotentialFieldTarget implements IPotentialFieldTar
int
targetId
=
agent
.
getNextTargetId
();
// the agent has reached his current target
if
(
topography
.
getTarget
(
targetId
).
getShape
().
contains
(
pos
))
{
// TODO: expensive operation
/*if (topography.getTarget(targetId).getShape().contains(pos)) {
return 0.0;
}
}
*/
// the agent is inside an obstacle
for
(
ScenarioElement
b
:
topography
.
getObstacles
())
{
// TODO: expensive operation
/*for (ScenarioElement b : topography.getObstacles()) {
if (b.getShape().contains(pos)) {
return Double.MAX_VALUE;
}
}
}
*/
/* Find minimal potential of given targets. */
Optional
<
PotentialFieldAndInitializer
>
optionalPotentialFieldAndAnalyser
=
getPotentialFieldAndInitializer
(
targetId
);
...
...
VadereSimulator/src/org/vadere/simulator/models/potential/fields/ObstacleDistancePotential.java
0 → 100644
View file @
e4472e93
package
org.vadere.simulator.models.potential.fields
;
import
org.apache.log4j.LogManager
;
import
org.apache.log4j.Logger
;
import
org.jetbrains.annotations.NotNull
;
import
org.jetbrains.annotations.Nullable
;
import
org.vadere.state.attributes.models.AttributesFloorField
;
import
org.vadere.state.scenario.Agent
;
import
org.vadere.util.geometry.shapes.VPoint
;
import
org.vadere.util.geometry.shapes.VRectangle
;
import
org.vadere.util.geometry.shapes.VShape
;
import
org.vadere.util.potential.CellGrid
;
import
org.vadere.util.potential.CellState
;
import
org.vadere.util.potential.FloorDiscretizer
;
import
org.vadere.util.potential.PathFindingTag
;
import
org.vadere.util.potential.calculators.EikonalSolver
;
import
org.vadere.util.potential.calculators.EikonalSolverFIM
;
import
org.vadere.util.potential.calculators.EikonalSolverFMM
;
import
org.vadere.util.potential.calculators.EikonalSolverFSM
;
import
org.vadere.util.potential.calculators.PotentialFieldCalculatorNone
;
import
org.vadere.util.potential.timecost.UnitTimeCostFunction
;
import
java.util.Collection
;
/**
* @author Benedikt Zoennchen
*/
public
class
ObstacleDistancePotential
implements
IPotentialField
{
private
static
Logger
logger
=
LogManager
.
getLogger
(
ObstacleDistancePotential
.
class
);
private
final
EikonalSolver
eikonalSolver
;
public
ObstacleDistancePotential
(
@NotNull
final
Collection
<
VShape
>
obstacles
,
@NotNull
final
VRectangle
bounds
,
@NotNull
final
AttributesFloorField
attributesFloorField
)
{
CellGrid
cellGrid
=
new
CellGrid
(
bounds
.
getWidth
(),
bounds
.
getHeight
(),
attributesFloorField
.
getPotentialFieldResolution
(),
new
CellState
());
for
(
VShape
shape
:
obstacles
)
{
FloorDiscretizer
.
setGridValuesForShapeCentered
(
cellGrid
,
shape
,
new
CellState
(
0.0
,
PathFindingTag
.
Target
));
}
boolean
isHighAccuracyFM
=
attributesFloorField
.
getCreateMethod
().
isHighAccuracy
();
/* copy the static grid */
switch
(
attributesFloorField
.
getCreateMethod
())
{
case
NONE:
eikonalSolver
=
new
PotentialFieldCalculatorNone
();
break
;
case
FAST_ITERATIVE_METHOD:
eikonalSolver
=
new
EikonalSolverFIM
(
cellGrid
,
obstacles
,
isHighAccuracyFM
,
new
UnitTimeCostFunction
());
break
;
case
FAST_SWEEPING_METHOD:
eikonalSolver
=
new
EikonalSolverFSM
(
cellGrid
,
obstacles
,
isHighAccuracyFM
,
new
UnitTimeCostFunction
());
break
;
default
:
eikonalSolver
=
new
EikonalSolverFMM
(
cellGrid
,
obstacles
,
isHighAccuracyFM
,
new
UnitTimeCostFunction
());
}
long
ms
=
System
.
currentTimeMillis
();
eikonalSolver
.
initialize
();
logger
.
info
(
"floor field initialization time:"
+
(
System
.
currentTimeMillis
()
-
ms
+
"[ms]"
));
}
@Override
public
double
getPotential
(
@NotNull
VPoint
pos
,
@Nullable
Agent
agent
)
{
return
eikonalSolver
.
getPotential
(
pos
,
0.0
,
1.0
);
}
}
VadereSimulator/src/org/vadere/simulator/models/potential/fields/PotentialFieldObstacle.java
View file @
e4472e93
...
...
@@ -77,7 +77,7 @@ public interface PotentialFieldObstacle extends Model {
AttributesPotentialCompactSoftshell
attributesPotentialCompactSoftshell
=
Model
.
findAttributes
(
modelAttributesList
,
AttributesPotentialCompactSoftshell
.
class
);
result
=
new
PotentialFieldObstacleCompactSoftshell
(
attributesPotentialCompactSoftshell
,
topography
.
getObstacles
()
,
random
);
topography
,
random
);
}
else
{
throw
new
VadereClassNotFoundException
();
}
...
...
VadereState/src/org/vadere/state/scenario/Topography.java
View file @
e4472e93
...
...
@@ -10,10 +10,12 @@ import java.util.LinkedList;
import
java.util.List
;
import
java.util.Map
;
import
java.util.Set
;
import
java.util.function.Function
;
import
java.util.function.Predicate
;
import
java.util.stream.Collectors
;
import
org.apache.log4j.Logger
;
import
org.jetbrains.annotations.NotNull
;
import
org.vadere.state.attributes.Attributes
;
import
org.vadere.state.attributes.scenario.AttributesAgent
;
import
org.vadere.state.attributes.scenario.AttributesCar
;
...
...
@@ -23,11 +25,13 @@ import org.vadere.util.geometry.LinkedCellsGrid;
import
org.vadere.util.geometry.shapes.VPoint
;
import
org.vadere.util.geometry.shapes.VShape
;
@JsonIgnoreProperties
(
value
=
{
"allOtherAttributes"
})
@JsonIgnoreProperties
(
value
=
{
"allOtherAttributes"
,
"obstacleDistanceFunction"
})
public
class
Topography
{
/** Transient to prevent JSON serialization. */
private
static
Logger
logger
=
Logger
.
getLogger
(
Topography
.
class
);
private
Function
<
VPoint
,
Double
>
obstacleDistanceFunction
;
// TODO [priority=low] [task=feature] magic number, use attributes / parameter?
/**
...
...
@@ -74,7 +78,9 @@ public class Topography {
/** Used to store links to all attributes that are not part of scenario elements. */
private
Set
<
Attributes
>
allOtherAttributes
=
new
HashSet
<>();
// will be filled in the constructor
public
Topography
(
AttributesTopography
attributes
,
AttributesAgent
attributesPedestrian
,
public
Topography
(
AttributesTopography
attributes
,
AttributesAgent
attributesPedestrian
,
AttributesCar
attributesCar
)
{
this
.
attributes
=
attributes
;
...
...
@@ -106,6 +112,8 @@ public class Topography {
this
.
pedestrians
=
new
DynamicElementContainer
<>(
bounds
,
CELL_SIZE
);
this
.
cars
=
new
DynamicElementContainer
<>(
bounds
,
CELL_SIZE
);
this
.
obstacleDistanceFunction
=
p
->
obstacles
.
stream
().
map
(
obs
->
obs
.
getShape
()).
map
(
shape
->
shape
.
distance
(
p
)).
min
(
Double:
:
compareTo
).
orElse
(
Double
.
MAX_VALUE
);
}
...
...
@@ -136,6 +144,14 @@ public class Topography {
return
null
;
}
public
double
distanceToObstacle
(
@NotNull
VPoint
point
)
{
return
this
.
obstacleDistanceFunction
.
apply
(
point
);
}
public
void
setObstacleDistanceFunction
(
@NotNull
Function
<
VPoint
,
Double
>
obstacleDistanceFunction
)
{
this
.
obstacleDistanceFunction
=
obstacleDistanceFunction
;
}
public
boolean
containsTarget
(
final
Predicate
<
Target
>
targetPredicate
)
{
return
getTargets
().
stream
().
anyMatch
(
targetPredicate
);
}
...
...
VadereUtils/src/org/vadere/util/geometry/shapes/VPoint.java
View file @
e4472e93
...
...
@@ -39,6 +39,10 @@ public class VPoint implements Cloneable {
return
Point2D
.
distance
(
x
,
y
,
other
.
x
,
other
.
y
);
}
public
double
distanceSq
(
final
VPoint
other
)
{
return
Point2D
.
distanceSq
(
x
,
y
,
other
.
x
,
other
.
y
);
}
public
double
distance
(
Point2D
other
)
{
return
Point2D
.
distance
(
x
,
y
,
other
.
getX
(),
other
.
getY
());
}
...
...
VadereUtils/src/org/vadere/util/potential/calculators/EikonalSolver.java
View file @
e4472e93
...
...
@@ -80,7 +80,7 @@ public interface EikonalSolver {
Point
gridPoint
=
potentialField
.
getNearestPointTowardsOrigin
(
pos
);
VPoint
gridPointCoord
=
potentialField
.
pointToCoord
(
gridPoint
);
int
incX
=
1
,
incY
=
1
;
double
gridPotentials
[];
double
gridPotentials
[]
=
new
double
[
4
]
;
if
(
pos
.
x
>=
potentialField
.
getWidth
())
{
incX
=
0
;
...
...
@@ -90,12 +90,11 @@ public interface EikonalSolver {
incY
=
0
;
}
java
.
util
.
List
<
Point
>
points
=
new
LinkedList
<>();
points
.
add
(
gridPoint
);
points
.
add
(
new
Point
(
gridPoint
.
x
+
incX
,
gridPoint
.
y
));
points
.
add
(
new
Point
(
gridPoint
.
x
+
incX
,
gridPoint
.
y
+
incY
));
points
.
add
(
new
Point
(
gridPoint
.
x
,
gridPoint
.
y
+
incY
));
gridPotentials
=
getGridPotentials
(
points
,
potentialField
);
gridPotentials
[
0
]
=
potentialField
.
getValue
(
gridPoint
).
potential
;
gridPotentials
[
1
]
=
potentialField
.
getValue
(
gridPoint
.
x
+
incX
,
gridPoint
.
y
).
potential
;
gridPotentials
[
2
]
=
potentialField
.
getValue
(
gridPoint
.
x
+
incX
,
gridPoint
.
y
+
incY
).
potential
;
gridPotentials
[
3
]
=
potentialField
.
getValue
(
gridPoint
.
x
,
gridPoint
.
y
+
incY
).
potential
;
/* Interpolate the known (potential < Double.MAX_VALUE) values. */
Pair
<
Double
,
Double
>
result
=
InterpolationUtil
.
bilinearInterpolationWithUnkown
(
...
...
VadereUtils/src/org/vadere/util/potential/calculators/EikonalSolverFIM.java
View file @
e4472e93
...
...
@@ -28,7 +28,7 @@ import java.util.stream.Collectors;
public
class
EikonalSolverFIM
implements
EikonalSolver
{
private
CellGrid
cellGrid
;
private
List
<
VShape
>
targetShapes
;
private
Collection
<
VShape
>
targetShapes
;
private
List
<
Point
>
targetPoints
;
private
static
Logger
logger
=
LogManager
.
getLogger
(
EikonalSolverFIM
.
class
);
private
ITimeCostFunction
timeCostFunction
;
...
...
@@ -40,7 +40,7 @@ public class EikonalSolverFIM implements EikonalSolver {
private
LinkedList
<
Point
>
activeList
;
public
EikonalSolverFIM
(
final
CellGrid
cellGrid
,
final
List
<
VShape
>
targetShapes
,
final
Collection
<
VShape
>
targetShapes
,
final
boolean
isHighAccuracy
,
final
ITimeCostFunction
timeCostFunction
)
{
this
.
timeCostFunction
=
timeCostFunction
;
...
...
VadereUtils/src/org/vadere/util/potential/calculators/EikonalSolverFMM.java
View file @
e4472e93
...
...
@@ -2,6 +2,7 @@ package org.vadere.util.potential.calculators;
import
java.awt.Point
;
import
java.util.ArrayList
;
import
java.util.Collection
;
import
java.util.List
;
import
java.util.PriorityQueue
;
import
java.util.stream.Collectors
;
...
...
@@ -29,7 +30,7 @@ public class EikonalSolverFMM implements EikonalSolver {
protected
CellGrid
cellGrid
;
protected
List
<
Point
>
targetPoints
;
protected
List
<
VShape
>
targetShapes
;
protected
Collection
<
VShape
>
targetShapes
;
boolean
isHighAccuracy
=
false
;
/** only for logging */
...
...
@@ -40,7 +41,7 @@ public class EikonalSolverFMM implements EikonalSolver {
* Initializes the FM potential calculator with a time cost function F > 0.
*/
public
EikonalSolverFMM
(
CellGrid
potentialField
,
List
<
VShape
>
targetShapes
,
Collection
<
VShape
>
targetShapes
,
boolean
isHighAccuracy
,
ITimeCostFunction
timeCostFunction
)
{
this
.
cellGrid
=
potentialField
;
...
...
VadereUtils/src/org/vadere/util/potential/calculators/EikonalSolverFSM.java
View file @
e4472e93
...
...
@@ -13,6 +13,7 @@ import java.io.BufferedWriter;
import
java.io.File
;
import
java.io.FileWriter
;
import
java.io.IOException
;
import
java.util.Collection
;
import
java.util.List
;
import
java.util.stream.Collectors
;
...
...
@@ -36,12 +37,12 @@ public class EikonalSolverFSM implements EikonalSolver {
private
ITimeCostFunction
timeCostFunction
;
private
boolean
isHighAccuracy
;
private
List
<
Point
>
targetPoints
;
private
List
<
VShape
>
targetShapes
;
private
Collection
<
VShape
>
targetShapes
;
private
static
final
double
EPSILON
=
0.001
;
public
EikonalSolverFSM
(
final
CellGrid
cellGrid
,
final
List
<
VShape
>
targetShapes
,
final
Collection
<
VShape
>
targetShapes
,
final
boolean
isHighAccuracy
,
final
ITimeCostFunction
timeCostFunction
)
{
this
.
timeCostFunction
=
timeCostFunction
;
...
...
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