Skip to content

GitLab

Commit 5089513b authored by Benedikt Zoennchen's avatar Benedikt Zoennchen
Browse files

PSO implementation for the OSM.

parent eff139a6
Pipeline #55289 passed with stage
in 46 seconds
Hide whitespace changes
Inline Side-by-side
VadereModelTests/TestOSM/scenarios/rimea_06_corner_osm2.scenario
View file @ 5089513b
......@@ -3,8 +3,15 @@
"description" : "",
"release" : "0.2",
"processWriters" : {
"files" : [ ],
"processors" : [ ],
"files" : [ {
"type" : "org.vadere.simulator.projects.dataprocessing.outputfile.TimestepPedestrianIdOutputFile",
"filename" : "postvis.trajectories",
"processors" : [ 1 ]
} ],
"processors" : [ {
"type" : "org.vadere.simulator.projects.dataprocessing.processor.PedestrianPositionProcessor",
"id" : 1
} ],
"isTimestamped" : true
},
"scenario" : {
......
VadereSimulator/src/org/vadere/simulator/models/osm/OptimalStepsModel.java
View file @ 5089513b
......@@ -7,6 +7,7 @@ import org.vadere.simulator.models.SubModelBuilder;
import org.vadere.simulator.models.groups.CentroidGroupModel;
import org.vadere.simulator.models.groups.CentroidGroupPotential;
import org.vadere.simulator.models.groups.CentroidGroupSpeedAdjuster;
import org.vadere.simulator.models.osm.optimization.ParticleSwarmOptimizer;
import org.vadere.simulator.models.osm.optimization.StepCircleOptimizer;
import org.vadere.simulator.models.osm.optimization.StepCircleOptimizerBrent;
import org.vadere.simulator.models.osm.optimization.StepCircleOptimizerDiscrete;
......@@ -171,6 +172,9 @@ public class OptimalStepsModel implements MainModel, PotentialFieldModel {
case POWELL:
result = new StepCircleOptimizerPowell(random);
break;
case PSO:
result = new ParticleSwarmOptimizer(movementThreshold, random);
break;
case GRADIENT:
result = new StepCircleOptimizerGradient(topography,
potentialFieldTarget, attributesOSM);
......
VadereSimulator/src/org/vadere/simulator/models/osm/optimization/ParticleSwarmOptimizer.java 0 → 100644
View file @ 5089513b
package org.vadere.simulator.models.osm.optimization;
import org.apache.log4j.Logger;
import org.jetbrains.annotations.NotNull;
import org.vadere.simulator.models.osm.PedestrianOSM;
import org.vadere.state.attributes.models.AttributesOSM;
import org.vadere.state.scenario.Pedestrian;
import org.vadere.state.types.MovementType;
import org.vadere.util.geometry.GeometryUtils;
import org.vadere.util.geometry.Vector2D;
import org.vadere.util.geometry.shapes.ICircleSector;
import org.vadere.util.geometry.shapes.VCircle;
import org.vadere.util.geometry.shapes.VCircleSector;
import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.math.pso.PSO;
import java.awt.Shape;
import java.util.*;
import java.util.List;
/**
* @author Benedikt Zoennchen
*/
public class ParticleSwarmOptimizer implements StepCircleOptimizer {
private final double movementThreshold;
private final Random random;
public ParticleSwarmOptimizer(final double movementThreshold, @NotNull final Random random) {
this.movementThreshold = movementThreshold;
this.random = random;
}
@Override
public VPoint getNextPosition(@NotNull final PedestrianOSM pedestrian, @NotNull final Shape reachableArea) {
assert reachableArea instanceof VCircle;
VCircle circle = ((VCircle) reachableArea);
double stepSize = circle.getRadius();
final PotentialEvaluationFunction potentialEvaluationFunction = new PotentialEvaluationFunction(pedestrian);
potentialEvaluationFunction.setStepSize(stepSize);
List<VPoint> positions = StepCircleOptimizerDiscrete.getReachablePositions(pedestrian, random);
// maximum possible angle of movement relative to ankerAngle
double angle;
// smallest possible angle of movement
double anchorAngle;
ICircleSector circleSector;
// TODO: dupclated code see StepCircleOptimizerDiscrete!
if (pedestrian.getAttributesOSM().getMovementType() == MovementType.DIRECTIONAL) {
angle = StepCircleOptimizerDiscrete.getMovementAngle(pedestrian);
Vector2D velocity = pedestrian.getVelocity();
anchorAngle = velocity.angleToZero() - angle;
angle = 2 * angle;
circleSector = new VCircleSector(circle.getCenter(), circle.getRadius(), anchorAngle, anchorAngle + 2 * angle);
} else {
angle = 2 * Math.PI;
anchorAngle = 0;
circleSector = circle;
}
PSO pso = new PSO(p -> getValue(p, pedestrian, stepSize), circleSector, anchorAngle, anchorAngle + 2 * angle, random, stepSize / 5.0, positions);
VPoint nextPos = pso.getOptimumArg();
VPoint curPos = pedestrian.getPosition();
double curPosPotential = pedestrian.getPotential(curPos);
double potential = pso.getOptimum();
if (curPosPotential - potential < movementThreshold) {
nextPos = curPos;
}
return nextPos;
}
private double getValue(@NotNull final VPoint newPos, @NotNull final PedestrianOSM ped, final double stepSize) {
VPoint pedPos = ped.getPosition();
double result = 100000;
// step is not too small nor too large?
if (Math.pow(newPos.x - pedPos.x, 2) + Math.pow(newPos.y - pedPos.y, 2) <= Math.pow(stepSize, 2) + 0.00001
&& Math.pow(newPos.x - pedPos.x, 2) + Math.pow(newPos.y - pedPos.y, 2) >= Math.pow(ped.getMinStepLength(), 2)
- 0.00001) {
result = ped.getPotential(newPos);
}
return result;
}
@Override
public StepCircleOptimizer clone() {
return new ParticleSwarmOptimizer(movementThreshold, random);
}
}
VadereSimulator/src/org/vadere/simulator/models/osm/optimization/StepCircleOptimizer.java
View file @ 5089513b
......@@ -14,5 +14,5 @@ public interface StepCircleOptimizer {
/** Returns the reachable position with the minimal potential. */
VPoint getNextPosition(PedestrianOSM pedestrian, Shape reachableArea);
public StepCircleOptimizer clone();
StepCircleOptimizer clone();
}
VadereSimulator/src/org/vadere/simulator/models/osm/optimization/StepCircleOptimizerDiscrete.java
View file @ 5089513b
package org.vadere.simulator.models.osm.optimization;
import org.apache.log4j.Logger;
import org.jetbrains.annotations.NotNull;
import org.vadere.simulator.models.osm.PedestrianOSM;
import org.vadere.state.attributes.models.AttributesOSM;
import org.vadere.state.types.MovementType;
import org.vadere.util.geometry.GeometryUtils;
import org.vadere.util.geometry.Vector2D;
import org.vadere.util.geometry.shapes.VCircle;
import org.vadere.util.geometry.shapes.VPoint;
import java.awt.*;
import java.util.LinkedList;
import java.awt.Shape;
import java.util.List;
import java.util.Random;
/**
......@@ -22,19 +24,19 @@ public class StepCircleOptimizerDiscrete implements StepCircleOptimizer {
private final double movementThreshold;
private final Random random;
public StepCircleOptimizerDiscrete(final double movementThreshold, Random random) {
public StepCircleOptimizerDiscrete(final double movementThreshold, @NotNull final Random random) {
this.movementThreshold = movementThreshold;
this.random = random;
}
@Override
public VPoint getNextPosition(PedestrianOSM pedestrian, Shape reachableArea) {
public VPoint getNextPosition(@NotNull final PedestrianOSM pedestrian, @NotNull final Shape reachableArea) {
assert reachableArea instanceof VCircle;
double stepSize = ((VCircle) reachableArea).getRadius();
LinkedList<VPoint> positions = getReachablePositions(pedestrian, random);
PotentialEvaluationFunction potentialEvaluationFunction = new PotentialEvaluationFunction(
pedestrian);
List<VPoint> positions = getReachablePositions(pedestrian, random);
PotentialEvaluationFunction potentialEvaluationFunction = new PotentialEvaluationFunction(pedestrian);
potentialEvaluationFunction.setStepSize(stepSize);
VPoint curPos = pedestrian.getPosition();
......@@ -43,8 +45,6 @@ public class StepCircleOptimizerDiscrete implements StepCircleOptimizer {
double potential = curPosPotential;
double tmpPotential = 0;
for (VPoint tmpPos : positions) {
try {
tmpPotential = potentialEvaluationFunction.getValue(tmpPos);
......@@ -53,7 +53,7 @@ public class StepCircleOptimizerDiscrete implements StepCircleOptimizer {
|| (Math.abs(tmpPotential - potential) <= 0.0001 && random
.nextBoolean())) {
potential = tmpPotential;
nextPos = tmpPos.clone();
nextPos = tmpPos;
}
} catch (Exception e) {
Logger.getLogger(StepCircleOptimizerDiscrete.class).error("Potential evaluation threw an error.");
......@@ -63,26 +63,20 @@ public class StepCircleOptimizerDiscrete implements StepCircleOptimizer {
if (curPosPotential - potential < movementThreshold) {
nextPos = curPos;
potential = curPosPotential;
}
return nextPos;
}
public StepCircleOptimizer clone() {
return new StepCircleOptimizerDiscrete(movementThreshold, random);
}
public static LinkedList<VPoint> getReachablePositions(final PedestrianOSM pedestrian, final Random random) {
public static List<VPoint> getReachablePositions(@NotNull final PedestrianOSM pedestrian, @NotNull final Random random) {
final AttributesOSM attributesOSM = pedestrian.getAttributesOSM();
double randOffset = attributesOSM.isVaryStepDirection() ? random.nextDouble() : 0;
VPoint currentPosition = pedestrian.getPosition();
LinkedList<VPoint> reachablePositions = new LinkedList<VPoint>();
int numberOfCircles = attributesOSM.getNumberOfCircles();
double circleOfGrid = 0;
int numberOfGridPoints;
// if number of circle is negative, choose number of circles according to
// StepCircleResolution
if (attributesOSM.getNumberOfCircles() < 0) {
......@@ -107,38 +101,22 @@ public class StepCircleOptimizerDiscrete implements StepCircleOptimizer {
anchorAngle = 0;
}
// iterate through all circles
for (int j = 1; j <= numberOfCircles; j++) {
circleOfGrid = pedestrian.getStepSize() * j / numberOfCircles;
return GeometryUtils.getDiscDiscretizationPoints(
random,
attributesOSM.isVaryStepDirection(),
new VCircle(pedestrian.getPosition(),
pedestrian.getStepSize()),
numberOfCircles,
attributesOSM.getStepCircleResolution(),
anchorAngle,
angle);
numberOfGridPoints = (int) Math.ceil(circleOfGrid / pedestrian.getStepSize()
* attributesOSM.getStepCircleResolution());
// reduce number of grid points proportional to the constraint of direction
if (attributesOSM.getMovementType() == MovementType.DIRECTIONAL) {
numberOfGridPoints = (int) Math.ceil(numberOfGridPoints * angle / (2 * Math.PI));
}
double angleDelta = angle / numberOfGridPoints;
// iterate through all angles and compute absolute positions of grid points
for (int i = 0; i < numberOfGridPoints; i++) {
double x = circleOfGrid * Math.cos(anchorAngle + angleDelta * (randOffset + i)) + currentPosition.x;
double y = circleOfGrid * Math.sin(anchorAngle + angleDelta * (randOffset + i)) + currentPosition.y;
VPoint tmpPos = new VPoint(x, y);
reachablePositions.add(tmpPos);
}
}
return reachablePositions;
}
/**
* The maximum deviation from the last movement direction given the current speed.
*/
private static double getMovementAngle(PedestrianOSM pedestrian) {
public static double getMovementAngle(@NotNull final PedestrianOSM pedestrian) {
final double speed = pedestrian.getVelocity().getLength();
double result = Math.PI - speed;
......
VadereSimulator/src/org/vadere/simulator/models/osm/optimization/StepCircleOptimizerEvolStrat.java
View file @ 5089513b
......@@ -38,7 +38,7 @@ public class StepCircleOptimizerEvolStrat implements StepCircleOptimizer {
public VPoint getNextPosition(PedestrianOSM ped, Shape reachableArea) {
double stepSize = ((VCircle) reachableArea).getRadius();
LinkedList<VPoint> positions = StepCircleOptimizerDiscrete.getReachablePositions(ped, random);
List<VPoint> positions = StepCircleOptimizerDiscrete.getReachablePositions(ped, random);
int numberOfParents = positions.size();
int numberOfChildren = numberOfParents * 7;
......
VadereSimulator/src/org/vadere/simulator/models/osm/optimization/StepCircleOptimizerNelderMead.java
View file @ 5089513b
......@@ -2,6 +2,7 @@ package org.vadere.simulator.models.osm.optimization;
import java.awt.Shape;
import java.util.LinkedList;
import java.util.List;
import java.util.Random;
import org.apache.commons.math.ConvergenceException;
......@@ -35,7 +36,7 @@ public class StepCircleOptimizerNelderMead implements StepCircleOptimizer {
public VPoint getNextPosition(PedestrianOSM pedestrian, Shape reachableArea) {
double stepSize = ((VCircle) reachableArea).getRadius();
LinkedList<VPoint> positions = StepCircleOptimizerDiscrete.getReachablePositions(pedestrian, random);
List<VPoint> positions = StepCircleOptimizerDiscrete.getReachablePositions(pedestrian, random);
PotentialEvaluationFunction potentialEvaluationFunction = new PotentialEvaluationFunction(pedestrian);
potentialEvaluationFunction.setStepSize(stepSize);
......
VadereSimulator/src/org/vadere/simulator/models/osm/optimization/StepCircleOptimizerPowell.java
View file @ 5089513b
......@@ -2,6 +2,7 @@ package org.vadere.simulator.models.osm.optimization;
import java.awt.Shape;
import java.util.LinkedList;
import java.util.List;
import java.util.Random;
import org.apache.commons.math3.optimization.direct.BOBYQAOptimizer;
......@@ -38,7 +39,7 @@ public class StepCircleOptimizerPowell implements StepCircleOptimizer {
PointValuePair newMinimum = new PointValuePair(position, potentialEvaluationFunction.value(position));
LinkedList<VPoint> positions = StepCircleOptimizerDiscrete.getReachablePositions(pedestrian, random);
List<VPoint> positions = StepCircleOptimizerDiscrete.getReachablePositions(pedestrian, random);
try {
newMinimum = optimizer.optimize(1000, (MultivariateFunction) potentialEvaluationFunction, GoalType.MINIMIZE,
......
VadereState/src/org/vadere/state/types/OptimizationType.java
View file @ 5089513b
package org.vadere.state.types;
public enum OptimizationType {
NELDER_MEAD, POWELL, EVOLUTION_STRATEGY, BRENT, GRADIENT, DISCRETE, NONE
PSO, NELDER_MEAD, POWELL, EVOLUTION_STRATEGY, BRENT, GRADIENT, DISCRETE, NONE
}
VadereUtils/src/org/vadere/util/geometry/GeometryUtils.java
View file @ 5089513b
......@@ -8,7 +8,10 @@ import java.util.Collection;
import java.util.Collections;
import java.util.LinkedList;
import java.util.List;
import java.util.Random;
import org.jetbrains.annotations.NotNull;
import org.vadere.util.geometry.shapes.VCircle;
import org.vadere.util.geometry.shapes.VLine;
import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.geometry.shapes.VPolygon;
......@@ -40,6 +43,45 @@ public class GeometryUtils {
return false;
}
public static List<VPoint> getDiscDiscretizationPoints(
@NotNull final Random random,
final boolean varyDirection,
@NotNull final VCircle circle,
final int numberOfCircles,
final int numberOfPointsOfLargestCircle,
final double anchorAngle,
final double angle) {
double randOffset = varyDirection ? random.nextDouble() : 0;
List<VPoint> reachablePositions = new ArrayList<>();
// iterate through all circles
for (int j = 1; j <= numberOfCircles; j++) {
double circleOfGrid = circle.getRadius() * j / numberOfCircles;
int numberOfGridPoints = (int) Math.ceil(circleOfGrid / circle.getRadius() * numberOfPointsOfLargestCircle);
// reduce number of grid points proportional to the constraint of direction
if (angle < 2 * Math.PI) {
numberOfGridPoints = (int) Math.ceil(numberOfGridPoints * angle / (2 * Math.PI));
}
double angleDelta = angle / numberOfGridPoints;
// iterate through all angles and compute absolute positions of grid points
for (int i = 0; i < numberOfGridPoints; i++) {
double x = circleOfGrid * Math.cos(anchorAngle + angleDelta * (randOffset + i)) + circle.getCenter().getX();
double y = circleOfGrid * Math.sin(anchorAngle + angleDelta * (randOffset + i)) + circle.getCenter().getY();
VPoint tmpPos = new VPoint(x, y);
reachablePositions.add(tmpPos);
}
}
return reachablePositions;
}
/**
* Computes the point on the line segment that is closest to the given point
* point. from:
......@@ -89,6 +131,22 @@ public class GeometryUtils {
return orderedList;
}
/**
* Returns the angle between the x-axis, p1 and p2.
* @param p1
* @param p2
* @return
*/
public static double angleTo(VPoint p1, VPoint p2) {
double atan2 = Math.atan2(p1.y - p2.y, p1.x - p2.x);
if (atan2 < 0.0) {
atan2 = Math.PI * 2 + atan2;
}
return atan2;
}
/**
* Calculate the counter clockwise result for the three given points.<br>
* ccw(p1,p2,p3) < 0 if p3 is left of Line(p1,p2)<br>
......@@ -150,4 +208,33 @@ public class GeometryUtils {
public static VPoint add(VPoint p1, VPoint p2) {
return new VPoint(p1.x + p2.x, p1.y + p2.y);
}
public static VPoint lineIntersectionPoint(final double x1,
final double y1,
final double x2,
final double y2,
final double x3,
final double y3,
final double x4,
final double y4) {
assert new VLine(new VPoint(x1,y1), new VPoint(x2, y2)).intersectsLine(x3, y3, x4, y4);
double d = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
assert d != 0;
double x = ((x1 * y2 - y1 - x2) * (x3 - x4) - (x1 - x2) * (x3 * y4 - y3 * x4)) / d;
double y = ((x1 * y2 - y1 * x2) * (y3 - y4) - (y1 - y2) * (y3 * y4 - y3 * x4)) / d;
return new VPoint(x, y);
}
public static VPoint lineIntersectionPoint(final VPoint p1, final VPoint p2, final VPoint q1, final VPoint q2) {
return lineIntersectionPoint(p1.x, p1.y, p2.x, p2.y, q1.x, q1.y, q2.x, q2.y);
}
public static VPoint lineIntersectionPoint(final VLine line,
final double x3,
final double y3,
final double x4,
final double y4) {
return lineIntersectionPoint(line.getX1(), line.getY1(), line.getX2(), line.getY2(), x3, y3, x4, y4);
}
}
VadereUtils/src/org/vadere/util/geometry/Vector2D.java
View file @ 5089513b
......@@ -85,13 +85,7 @@ public class Vector2D extends VPoint {
* Result is in interval (0,2*PI) according to standard math usage.
*/
public double angleTo(VPoint center) {
double atan2 = Math.atan2(this.y - center.y, this.x - center.x);
if (atan2 < 0.0) {
atan2 = Math.PI * 2 + atan2;
}
return atan2;
return GeometryUtils.angleTo(this, center);
}
public Vector2D add(VPoint p) {
......
VadereUtils/src/org/vadere/util/geometry/shapes/ICircleSector.java 0 → 100644
View file @ 5089513b
package org.vadere.util.geometry.shapes;
import com.google.common.collect.ImmutableList;
import org.jetbrains.annotations.NotNull;
import java.util.Optional;
/**
* @author Benedikt Zoennchen
*/
public interface ICircleSector {
boolean contains(VPoint point);
default ImmutableList<VPoint> getIntersectionPoints(@NotNull final VLine line) {
return getIntersectionPoints(line.getX1(), line.getY1(), line.getX2(), line.getY2());
}
default ImmutableList<VPoint> getIntersectionPoints(@NotNull final VPoint p, @NotNull final VPoint q) {
return getIntersectionPoints(p.getX(), p.getY(), q.getX(), q.getY());
}
ImmutableList<VPoint> getIntersectionPoints(final double x1, final double y1, final double x2, final double y2);
default Optional<VPoint> getClosestIntersectionPoint(@NotNull final VLine line, @NotNull final VPoint r) {
return getClosestIntersectionPoint(line.getX1(), line.getY1(), line.getX2(), line.getY2(), r.getX(), r.getY());
}
default Optional<VPoint> getClosestIntersectionPoint(@NotNull final VPoint p, @NotNull final VPoint q, @NotNull final VPoint r) {
return getClosestIntersectionPoint(p.getX(), p.getY(), q.getX(), q.getY(), r.getX(), r.getY());
}
Optional<VPoint> getClosestIntersectionPoint(final double x1, final double y1, final double x2, final double y2, final double x3, final double y3);
default boolean intersectsLine(@NotNull VLine intersectingLine) {
return intersectsLine(intersectingLine.getX1(), intersectingLine.getY1(), intersectingLine.getX2(), intersectingLine.getY2());
}
default boolean intersectsLine(@NotNull VPoint p, @NotNull VPoint q) {
return intersectsLine(p.getX(), p.getY(), q.getX(), q.getY());
}
boolean intersectsLine(final double x1, final double y1, final double x2, final double y2);
double getRadius();
VPoint getCenter();
}
VadereUtils/src/org/vadere/util/geometry/shapes/VCircle.java
View file @ 5089513b
package org.vadere.util.geometry.shapes;
import com.google.common.collect.ImmutableList;
import java.awt.Rectangle;
import java.awt.geom.AffineTransform;
import java.awt.geom.Ellipse2D;
import java.awt.geom.PathIterator;
import java.awt.geom.Point2D;
import java.awt.geom.Rectangle2D;
import java.util.Comparator;
import java.util.Optional;
import org.jetbrains.annotations.NotNull;
import org.vadere.util.geometry.ShapeType;
import org.vadere.util.geometry.Vector2D;
public class VCircle implements VShape {
public class VCircle implements VShape, ICircleSector {
private final VPoint center;
private final double radius;
......@@ -52,9 +57,6 @@ public class VCircle implements VShape {
return this.center;
}
/**
* Not implemented.
*/
@Override
public VPoint closestPoint(VPoint point) {
Vector2D direction = new Vector2D(point.x - center.x, point.y
......@@ -63,6 +65,83 @@ public class VCircle implements VShape {
return new VPoint(vector.x + center.x, vector.y + center.y);
}
/**
* Returns zero, one or two points which are the intersection of this circle and the line defined
* by p = (x11, y11) and q = (x22, y22).
*
* @param x11
* @param y11
* @param x22
* @param y22
* @return
*/
public ImmutableList<VPoint> getIntersectionPoints(final double x11, final double y11, final double x22, final double y22) {
double x1 = x11 - center.x;
double y1 = y11 - center.y;
double x2 = x22 - center.x;
double y2 = y22 - center.y;
double dx = x2 - x1;