Skip to content

GitLab

Commit 9469482c authored by Benedikt Zoennchen's avatar Benedikt Zoennchen
Browse files

fix wrong intersection point calculation line with circle.

parent c654a766
Hide whitespace changes
Inline Side-by-side
Showing with 126 additions and 317 deletions
VadereModelTests/TestOSM/scenarios/regular_wall_pso_ok.scenario
View file @ 9469482c
......@@ -77,7 +77,7 @@
},
"org.vadere.state.attributes.models.AttributesOSM" : {
"stepCircleResolution" : 18,
"numberOfCircles" : 1,
"numberOfCircles" : 3,
"varyStepDirection" : false,
"stepLengthIntercept" : 0.4625,
"stepLengthSlopeSpeed" : 0.2345,
......@@ -137,294 +137,6 @@
"type" : "RECTANGLE"
},
"id" : -1
}, {
"shape" : {
"type" : "POLYGON",
"points" : [ {
"x" : -1.0E-4,
"y" : -1.0E-4
}, {
"x" : 20.0001,
"y" : -1.0E-4
}, {
"x" : 20.0001,
"y" : 0.5001
}, {
"x" : -1.0E-4,
"y" : 0.5001
} ]
},
"id" : -1
}, {
"shape" : {
"type" : "POLYGON",
"points" : [ {
"x" : 20.0001,
"y" : -1.0E-4
}, {
"x" : 20.0001,
"y" : 10.0001
}, {
"x" : 19.4999,
"y" : 10.0001
}, {
"x" : 19.4999,
"y" : -1.0E-4
} ]
},
"id" : -1
}, {
"shape" : {
"type" : "POLYGON",
"points" : [ {
"x" : 20.0001,
"y" : 10.0001
}, {
"x" : -1.0E-4,
"y" : 10.0001
}, {
"x" : -1.0E-4,
"y" : 9.4999
}, {
"x" : 20.0001,
"y" : 9.4999
} ]
},
"id" : -1
}, {
"shape" : {
"type" : "POLYGON",
"points" : [ {
"x" : -1.0E-4,
"y" : 10.0001
}, {
"x" : -1.0E-4,
"y" : -1.0E-4
}, {
"x" : 0.5001,
"y" : -1.0E-4
}, {
"x" : 0.5001,
"y" : 10.0001
} ]
},
"id" : -1
}, {
"shape" : {
"type" : "POLYGON",
"points" : [ {
"x" : -1.0E-4,
"y" : -1.0E-4
}, {
"x" : 20.0001,
"y" : -1.0E-4
}, {
"x" : 20.0001,
"y" : 0.5001
}, {
"x" : -1.0E-4,
"y" : 0.5001
} ]
},
"id" : -1
}, {
"shape" : {
"type" : "POLYGON",
"points" : [ {
"x" : 20.0001,
"y" : -1.0E-4
}, {
"x" : 20.0001,
"y" : 10.0001
}, {
"x" : 19.4999,
"y" : 10.0001
}, {
"x" : 19.4999,
"y" : -1.0E-4
} ]
},
"id" : -1
}, {
"shape" : {
"type" : "POLYGON",
"points" : [ {
"x" : 20.0001,
"y" : 10.0001
}, {
"x" : -1.0E-4,
"y" : 10.0001
}, {
"x" : -1.0E-4,
"y" : 9.4999
}, {
"x" : 20.0001,
"y" : 9.4999
} ]
},
"id" : -1
}, {
"shape" : {
"type" : "POLYGON",
"points" : [ {
"x" : -1.0E-4,
"y" : 10.0001
}, {
"x" : -1.0E-4,
"y" : -1.0E-4
}, {
"x" : 0.5001,
"y" : -1.0E-4
}, {
"x" : 0.5001,
"y" : 10.0001
} ]
},
"id" : -1
}, {
"shape" : {
"type" : "POLYGON",
"points" : [ {
"x" : -1.0E-4,
"y" : -1.0E-4
}, {
"x" : 20.0001,
"y" : -1.0E-4
}, {
"x" : 20.0001,
"y" : 0.5001
}, {
"x" : -1.0E-4,
"y" : 0.5001
} ]
},
"id" : -1
}, {
"shape" : {
"type" : "POLYGON",
"points" : [ {
"x" : 20.0001,
"y" : -1.0E-4
}, {
"x" : 20.0001,
"y" : 10.0001
}, {
"x" : 19.4999,
"y" : 10.0001
}, {
"x" : 19.4999,
"y" : -1.0E-4
} ]
},
"id" : -1
}, {
"shape" : {
"type" : "POLYGON",
"points" : [ {
"x" : 20.0001,
"y" : 10.0001
}, {
"x" : -1.0E-4,
"y" : 10.0001
}, {
"x" : -1.0E-4,
"y" : 9.4999
}, {
"x" : 20.0001,
"y" : 9.4999
} ]
},
"id" : -1
}, {
"shape" : {
"type" : "POLYGON",
"points" : [ {
"x" : -1.0E-4,
"y" : 10.0001
}, {
"x" : -1.0E-4,
"y" : -1.0E-4
}, {
"x" : 0.5001,
"y" : -1.0E-4
}, {
"x" : 0.5001,
"y" : 10.0001
} ]
},
"id" : -1
}, {
"shape" : {
"type" : "POLYGON",
"points" : [ {
"x" : -1.0E-4,
"y" : -1.0E-4
}, {
"x" : 20.0001,
"y" : -1.0E-4
}, {
"x" : 20.0001,
"y" : 0.5001
}, {
"x" : -1.0E-4,
"y" : 0.5001
} ]
},
"id" : -1
}, {
"shape" : {
"type" : "POLYGON",
"points" : [ {
"x" : 20.0001,
"y" : -1.0E-4
}, {
"x" : 20.0001,
"y" : 10.0001
}, {
"x" : 19.4999,
"y" : 10.0001
}, {
"x" : 19.4999,
"y" : -1.0E-4
} ]
},
"id" : -1
}, {
"shape" : {
"type" : "POLYGON",
"points" : [ {
"x" : 20.0001,
"y" : 10.0001
}, {
"x" : -1.0E-4,
"y" : 10.0001
}, {
"x" : -1.0E-4,
"y" : 9.4999
}, {
"x" : 20.0001,
"y" : 9.4999
} ]
},
"id" : -1
}, {
"shape" : {
"type" : "POLYGON",
"points" : [ {
"x" : -1.0E-4,
"y" : 10.0001
}, {
"x" : -1.0E-4,
"y" : -1.0E-4
}, {
"x" : 0.5001,
"y" : -1.0E-4
}, {
"x" : 0.5001,
"y" : 10.0001
} ]
},
"id" : -1
} ],
"stairs" : [ ],
"targets" : [ {
......
VadereModelTests/TestOSM/scenarios/rimea_09_public_room_2_exits_pso.scenario
View file @ 9469482c
......@@ -87,7 +87,7 @@
}
},
"org.vadere.state.attributes.models.AttributesOSM" : {
"stepCircleResolution" : 18,
"stepCircleResolution" : 10,
"numberOfCircles" : 3,
"varyStepDirection" : false,
"stepLengthIntercept" : 0.4625,
......
VadereModelTests/TestOSM/scenarios/rimea_09_public_room_4_exits_pso.scenario
View file @ 9469482c
......@@ -87,7 +87,7 @@
}
},
"org.vadere.state.attributes.models.AttributesOSM" : {
"stepCircleResolution" : 18,
"stepCircleResolution" : 10,
"numberOfCircles" : 3,
"varyStepDirection" : false,
"stepLengthIntercept" : 0.4625,
......
VadereModelTests/TestOSM/scenarios/rimea_11_exit_selection_pso.scenario
View file @ 9469482c
......@@ -45,8 +45,8 @@
}
},
"org.vadere.state.attributes.models.AttributesOSM" : {
"stepCircleResolution" : 18,
"numberOfCircles" : 1,
"stepCircleResolution" : 10,
"numberOfCircles" : 3,
"varyStepDirection" : false,
"stepLengthIntercept" : 0.4625,
"stepLengthSlopeSpeed" : 0.2345,
......
VadereModelTests/TestOSM/scenarios/rimea_12_evacuation_pso.scenario
View file @ 9469482c
......@@ -45,14 +45,14 @@
}
},
"org.vadere.state.attributes.models.AttributesOSM" : {
"stepCircleResolution" : 18,
"numberOfCircles" : 1,
"stepCircleResolution" : 10,
"numberOfCircles" : 3,
"varyStepDirection" : true,
"stepLengthIntercept" : 0.4625,
"stepLengthSlopeSpeed" : 0.2345,
"stepLengthSD" : 0.036,
"movementThreshold" : 0.0,
"optimizationType" : "NELDER_MEAD",
"optimizationType" : "PSO",
"movementType" : "ARBITRARY",
"dynamicStepLength" : false,
"updateType" : "EVENT_DRIVEN",
......
VadereSimulator/src/org/vadere/simulator/models/osm/optimization/ParticleSwarmOptimizer.java
View file @ 9469482c
......@@ -63,7 +63,7 @@ public class ParticleSwarmOptimizer implements StepCircleOptimizer {
circleSector = circle;
}
PSO pso = new PSO(p -> getValue(p, pedestrian, stepSize), circleSector, anchorAngle, anchorAngle + 2 * angle, random, stepSize / 5.0, positions);
PSO pso = new PSO(p -> getValue(p, pedestrian, stepSize), circleSector, anchorAngle, anchorAngle + 2 * angle, random, stepSize, positions);
VPoint curPos = pedestrian.getPosition();
double curPosPotential = pedestrian.getPotential(curPos);
......
VadereUtils/src/org/vadere/util/geometry/GeometryUtils.java
View file @ 9469482c
......@@ -231,12 +231,19 @@ public class GeometryUtils {
* @return the point on the line that is closest to p
*/
public static VPoint closestToSegment(@NotNull final VLine line, @NotNull final IPoint point) {
if (new VPoint((Point2D.Double) line.getP1()).equals(point)) {
VPoint a2p = new VPoint(point.getX() - line.x1, point.getY() - line.y1);
VPoint a2b = new VPoint(line.x2 - line.x1, line.y2 - line.y1);
// the line is not a line or a very short line
if(Math.abs(a2b.x) < GeometryUtils.DOUBLE_EPS && Math.abs(a2b.y) < GeometryUtils.DOUBLE_EPS) {
return new VPoint(line.x1, line.y1);
}
VPoint a2p = new VPoint(point.getX() - line.x1, point.getY() - line.y1);
VPoint a2b = new VPoint(line.x2 - line.x1, line.y2 - line.y1);
// the point is very close or equal to one of the points of the line
if(Math.abs(a2p.x) < GeometryUtils.DOUBLE_EPS && Math.abs(a2p.y) < GeometryUtils.DOUBLE_EPS) {
return new VPoint(point.getX(), point.getY());
}
double distAB = a2b.x * a2b.x + a2b.y * a2b.y;
double a2p_dot_a2b = a2p.x * a2b.x + a2p.y * a2b.y;
......
VadereUtils/src/org/vadere/util/geometry/shapes/ICircleSector.java
View file @ 9469482c
......@@ -27,7 +27,8 @@ public interface ICircleSector {
}
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> result = getClosestIntersectionPoint(p.getX(), p.getY(), q.getX(), q.getY(), r.getX(), r.getY());
return result;
}
Optional<VPoint> getClosestIntersectionPoint(final double x1, final double y1, final double x2, final double y2, final double x3, final double y3);
......
VadereUtils/src/org/vadere/util/geometry/shapes/IPoint.java
View file @ 9469482c
......@@ -38,6 +38,31 @@ public interface IPoint extends Cloneable {
IPoint norm(double len);
default IPoint setMagnitude(double len) {
assert len >= 0;
double length = distanceToOrigin();
if(length <= 0) {
if(len != 0.0) {
throw new IllegalArgumentException("a vector with zero length can not be set to a specific magnitude != 0.");
}
else {
return this;
}
}
return scalarMultiply(len / distanceToOrigin());
}
default IPoint limit(double len) {
assert len >= 0;
double length = distanceToOrigin();
if(length > len) {
return setMagnitude(len);
}
else {
return this;
}
}
IPoint normZeroSafe();
/**
......
VadereUtils/src/org/vadere/util/geometry/shapes/VCircle.java
View file @ 9469482c
......@@ -96,9 +96,11 @@ public class VCircle implements VShape, ICircleSector {
double D = radius * radius * dr * dr - disc * disc;
double sign = dy < 0 ? -1 : 1;
assert (Math.abs(dx) > 0.0 || Math.abs(dy) > 0.0) && dr * dr > 0.0;
if (D == 0) {
x1 = (disc * dy + sign * dx * Math.sqrt(D)) / (dr * dr);
y1 = (-disc * dx + Math.abs(dy) * Math.sqrt(D)) / (dr * dr);
x1 = (disc * dy) / (dr * dr);
y1 = (-disc * dx) / (dr * dr);
return ImmutableList.of(new VPoint(x1 + this.getCenter().x, y1 + this.getCenter().y));
} else if (D < 0) {
return ImmutableList.of();
......
VadereUtils/src/org/vadere/util/geometry/shapes/VPoint.java
View file @ 9469482c
......@@ -27,21 +27,25 @@ public class VPoint implements Cloneable, IPoint {
public VPoint(double x, double y) {
this.x = x;
this.y = y;
assert !Double.isNaN(x) && !Double.isNaN(y) && Double.isFinite(x) && Double.isFinite(y);
}
public VPoint(Point2D.Double copy) {
this.x = copy.x;
this.y = copy.y;
assert !Double.isNaN(x) && !Double.isNaN(y) && Double.isFinite(x) && Double.isFinite(y);
}
public VPoint(Point2D copy) {
this.x = copy.getX();
this.y = copy.getY();
assert !Double.isNaN(x) && !Double.isNaN(y) && Double.isFinite(x) && Double.isFinite(y);
}
public VPoint(Point copy) {
this.x = copy.x;
this.y = copy.y;
assert !Double.isNaN(x) && !Double.isNaN(y) && Double.isFinite(x) && Double.isFinite(y);
}
@Override
......@@ -124,6 +128,12 @@ public class VPoint implements Cloneable, IPoint {
}
public VPoint rotate(final double radAngle) {
VPoint result = new VPoint(x * Math.cos(radAngle) - y * Math.sin(radAngle),
x * Math.sin(radAngle) + y * Math.cos(radAngle));
if(Double.isNaN(result.getX())) {
System.out.println("wtf");
}
return new VPoint(x * Math.cos(radAngle) - y * Math.sin(radAngle),
x * Math.sin(radAngle) + y * Math.cos(radAngle));
}
......@@ -168,6 +178,32 @@ public class VPoint implements Cloneable, IPoint {
return new VPoint(x / len, y / len);
}
@Override
public VPoint setMagnitude(double len) {
double length = distanceToOrigin();
if(length <= 0) {
if(len != 0.0) {
throw new IllegalArgumentException("a vector with zero length can not be set to a specific magnitude != 0.");
}
else {
return this;
}
}
return scalarMultiply(len / distanceToOrigin());
}
@Override
public VPoint limit(double len) {
assert len >= 0;
double length = distanceToOrigin();
if(length > len) {
return setMagnitude(len);
}
else {
return this;
}
}
@Override
public VPoint normZeroSafe() {
......
VadereUtils/src/org/vadere/util/math/pso/AttributesPSO.java
View file @ 9469482c
......@@ -7,8 +7,8 @@ public class AttributesPSO {
final int numberOfInformedParticles = 3;
final int swarmSize = 30;
final int minIteration = 4;
final int maxNoUpdate = 6;
final int maxIteration = 10;
final int maxNoUpdate = 5;
final int maxIteration = 15;
final int problemDimension = 2;
final double c1 = 2.0;
final double c2 = 2.0;
......
VadereUtils/src/org/vadere/util/math/pso/PSO.java
View file @ 9469482c
package org.vadere.util.math.pso;
import org.jetbrains.annotations.NotNull;
import org.vadere.util.geometry.GeometryUtils;
import org.vadere.util.geometry.shapes.ICircleSector;
import org.vadere.util.geometry.shapes.VPoint;
......@@ -45,6 +46,7 @@ public class PSO {
this.maxVelocity = maxVelocity;
this.minAngle = minAngle;
this.maxAngle = maxAngle;
assert swarmPositions.stream().allMatch(p -> circle.getCenter().distance(p) < circle.getRadius() + GeometryUtils.DOUBLE_EPS);
this.particles = initialSwarm(swarmPositions);
this.improvementIterations = 0;
}
......@@ -92,22 +94,22 @@ public class PSO {
}
private void updateParticle(@NotNull final Particle particle, final double omega) {
double rLocal = random.nextDouble();
double rGlobal = random.nextDouble();
double rLocal = random();
double rGlobal = random();
VPoint dirLocalBest = particle.getLocalBestLocation().subtract(particle.getLocation());
VPoint dirGobalBest = particle.getGlobalBestLocation().subtract(particle.getLocation());
VPoint dirGlobalBest = particle.getGlobalBestLocation().subtract(particle.getLocation());
VPoint velocity = particle.getVelocity().scalarMultiply(omega)
.add(dirLocalBest.scalarMultiply(rLocal * attributesPSO.c1))
.add(dirGobalBest.scalarMultiply(rGlobal * attributesPSO.c2))
.norm(maxVelocity);
.add(dirGlobalBest.scalarMultiply(rGlobal * attributesPSO.c2))
.limit(maxVelocity);
VPoint currentLocation = particle.getLocation();
particle.setVelocity(velocity);
particle.setLocation(particle.getLocation().add(particle.getVelocity()));
if (!circle.contains(particle.getLocation())) {
if (!particle.getLocation().equals(currentLocation) && !circle.contains(particle.getLocation())) {
particle.setVelocity(particle.getVelocity().scalarMultiply(-0.5));
particle.setLocation(circle.getClosestIntersectionPoint(currentLocation, particle.getLocation(), particle.getLocation()).orElse(particle.getLocation()));
}
......@@ -158,7 +160,7 @@ public class PSO {
// no global improvement
if (gBest >= gLastBest) {
informKParticle();
informAllParticles();
}
}
......@@ -175,13 +177,30 @@ public class PSO {
private void informKParticle() {
for (Particle particle : particles) {
for (int i = 0; i < attributesPSO.numberOfInformedParticles; i++) {
int index = (int) Math.floor(random.nextDouble() * particles.size());
int index = (int) Math.floor(random() * particles.size());
Particle otherParticle = particles.get(index);
informParticles(particle, otherParticle);
}
}