diff --git a/VadereModelTests/TestOSM/scenarios/basic_2_density_osm1.scenario b/VadereModelTests/TestOSM/scenarios/basic_2_density_osm1.scenario index f52846521041c24d3fffc96f5a4207cafca7565d..28af420efdf64cf956987854600d547c21cb8123 100644 --- a/VadereModelTests/TestOSM/scenarios/basic_2_density_osm1.scenario +++ b/VadereModelTests/TestOSM/scenarios/basic_2_density_osm1.scenario @@ -84,16 +84,20 @@ "type" : "POLYGON", "points" : [ { "x" : 9.0, - "y" : 12.0 + "y" : 12.0, + "identifier" : -1 }, { "x" : 9.0, - "y" : 10.0 + "y" : 10.0, + "identifier" : -1 }, { "x" : 30.0, - "y" : 7.0 + "y" : 7.0, + "identifier" : -1 }, { "x" : 30.0, - "y" : 12.0 + "y" : 12.0, + "identifier" : -1 } ] }, "id" : -1 @@ -102,16 +106,20 @@ "type" : "POLYGON", "points" : [ { "x" : 9.0, - "y" : 0.0 + "y" : 0.0, + "identifier" : -1 }, { "x" : 9.0, - "y" : 2.0 + "y" : 2.0, + "identifier" : -1 }, { "x" : 30.0, - "y" : 5.0 + "y" : 5.0, + "identifier" : -1 }, { "x" : 30.0, - "y" : 0.0 + "y" : 0.0, + "identifier" : -1 } ] }, "id" : -1 diff --git a/VadereModelTests/TestOSM/scenarios/rimea_02_stairs_osm1.scenario b/VadereModelTests/TestOSM/scenarios/rimea_02_stairs_osm1.scenario index ca89c631dc1440b89d83d4902a1bdc73c5e9c763..35df0cc6a415dd9be2d67a8ae461f7d29a6d1a1a 100644 --- a/VadereModelTests/TestOSM/scenarios/rimea_02_stairs_osm1.scenario +++ b/VadereModelTests/TestOSM/scenarios/rimea_02_stairs_osm1.scenario @@ -146,7 +146,8 @@ "treadCount" : 1, "upwardDirection" : { "x" : 1.0, - "y" : 0.0 + "y" : 0.0, + "identifier" : -1 } } ], "targets" : [ { diff --git a/VadereModelTests/TestOSM/scenarios/rimea_03_stairs_osm1.scenario b/VadereModelTests/TestOSM/scenarios/rimea_03_stairs_osm1.scenario index 622f98452bedf989d10fff0571f182c8a13da9f1..9db85173b1e0e8d7a5e1b26213b450d1b333e243 100644 --- a/VadereModelTests/TestOSM/scenarios/rimea_03_stairs_osm1.scenario +++ b/VadereModelTests/TestOSM/scenarios/rimea_03_stairs_osm1.scenario @@ -146,7 +146,8 @@ "treadCount" : 1, "upwardDirection" : { "x" : -1.0, - "y" : 0.0 + "y" : 0.0, + "identifier" : -1 } } ], "targets" : [ { diff --git a/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_025_d.scenario b/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_025_d.scenario index a01443179138bd727c2b3957a91736303a6802ed..71706ddeed2f26e400c842f73fb469b0139cd9a3 100644 --- a/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_025_d.scenario +++ b/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_025_d.scenario @@ -92,7 +92,8 @@ "treadCount" : 100, "upwardDirection" : { "x" : -1.0, - "y" : 0.0 + "y" : 0.0, + "identifier" : -1 } } ], "targets" : [ { diff --git a/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_025_up.scenario b/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_025_up.scenario index 69e51def427d378a28d53ff7602d76fcdbd469a7..32773e7d29c391b238d495c3efbaed57307178c9 100644 --- a/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_025_up.scenario +++ b/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_025_up.scenario @@ -92,7 +92,8 @@ "treadCount" : 100, "upwardDirection" : { "x" : 1.0, - "y" : 0.0 + "y" : 0.0, + "identifier" : -1 } } ], "targets" : [ { diff --git a/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_050_d.scenario b/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_050_d.scenario index dbe88ee39e38e96e913aa01dd445d35687ab7547..9d6298e9ea6033f77b4ff9773e63aa949bb5a7e6 100644 --- a/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_050_d.scenario +++ b/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_050_d.scenario @@ -92,7 +92,8 @@ "treadCount" : 100, "upwardDirection" : { "x" : -1.0, - "y" : 0.0 + "y" : 0.0, + "identifier" : -1 } } ], "targets" : [ { diff --git a/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_050_up.scenario b/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_050_up.scenario index 03ebafb7af509039d447fac5a379bf82cbece094..a27d6d75cc72639f6a16d94d32882192cd4a2a77 100644 --- a/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_050_up.scenario +++ b/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_050_up.scenario @@ -92,7 +92,8 @@ "treadCount" : 100, "upwardDirection" : { "x" : 1.0, - "y" : 0.0 + "y" : 0.0, + "identifier" : -1 } } ], "targets" : [ { diff --git a/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_075_d.scenario b/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_075_d.scenario index c2aa0dfc2e92117e414bcdcf02987220bc16f3ed..f20fd0530bb9931be12afe85fe8551f3ba5f950d 100644 --- a/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_075_d.scenario +++ b/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_075_d.scenario @@ -92,7 +92,8 @@ "treadCount" : 100, "upwardDirection" : { "x" : -1.0, - "y" : 0.0 + "y" : 0.0, + "identifier" : -1 } } ], "targets" : [ { diff --git a/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_075_up.scenario b/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_075_up.scenario index 85a660232d52f825e43b6f9c9b5e283634dbd98a..cf8e38a4a198c3a69d5e351dc19527d45fd7bf64 100644 --- a/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_075_up.scenario +++ b/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_075_up.scenario @@ -92,7 +92,8 @@ "treadCount" : 100, "upwardDirection" : { "x" : 1.0, - "y" : 0.0 + "y" : 0.0, + "identifier" : -1 } } ], "targets" : [ { diff --git a/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_100_d.scenario b/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_100_d.scenario index 10b3d0bc4f9816957f607f6ce9805a127b69c57b..7477d602b12c0d9628370784dab3149b8bd130eb 100644 --- a/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_100_d.scenario +++ b/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_100_d.scenario @@ -92,7 +92,8 @@ "treadCount" : 100, "upwardDirection" : { "x" : -1.0, - "y" : 0.0 + "y" : 0.0, + "identifier" : -1 } } ], "targets" : [ { diff --git a/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_100_up.scenario b/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_100_up.scenario index 44247c3d91f1d8fe43295a16d8b25cd2693a6bc4..7ec7e26bb7622ada0c92146a205ea851dfd099f5 100644 --- a/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_100_up.scenario +++ b/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_100_up.scenario @@ -92,7 +92,8 @@ "treadCount" : 100, "upwardDirection" : { "x" : 1.0, - "y" : 0.0 + "y" : 0.0, + "identifier" : -1 } } ], "targets" : [ { diff --git a/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_125_d.scenario b/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_125_d.scenario index 5198052ba4162213fda37d86b7ec942f441fec6b..39bde5c864c9eca4a30a252472ea497e4aa1f2b8 100644 --- a/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_125_d.scenario +++ b/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_125_d.scenario @@ -92,7 +92,8 @@ "treadCount" : 100, "upwardDirection" : { "x" : -1.0, - "y" : 0.0 + "y" : 0.0, + "identifier" : -1 } } ], "targets" : [ { diff --git a/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_125_up.scenario b/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_125_up.scenario index 7cf2d0913207e4057f8b1a0771e0597085cb7475..e3f92334f9f818cc69932d72616e110343dfe870 100644 --- a/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_125_up.scenario +++ b/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_125_up.scenario @@ -92,7 +92,8 @@ "treadCount" : 100, "upwardDirection" : { "x" : 1.0, - "y" : 0.0 + "y" : 0.0, + "identifier" : -1 } } ], "targets" : [ { diff --git a/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_550_up.scenario b/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_550_up.scenario index 288e5dc5d352dd5d0cae5fb3e6a9aeeeefbf0368..7d3cd6fb55f37e47fe0354257ff3887d843f161f 100644 --- a/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_550_up.scenario +++ b/VadereModelTests/TestOSM/scenarios/rimea_04_flow_osm1_550_up.scenario @@ -92,7 +92,8 @@ "treadCount" : 133, "upwardDirection" : { "x" : 1.0, - "y" : 0.0 + "y" : 0.0, + "identifier" : -1 } } ], "targets" : [ { diff --git a/VadereModelTests/TestOSM/scenarios/rimea_06_corner_osm1.scenario b/VadereModelTests/TestOSM/scenarios/rimea_06_corner_osm1.scenario index da3d70f7de1c0135457660c5dccf6091918c4b80..3499163f36e50f2014e27bb3ff0e5ede5df5b6ce 100644 --- a/VadereModelTests/TestOSM/scenarios/rimea_06_corner_osm1.scenario +++ b/VadereModelTests/TestOSM/scenarios/rimea_06_corner_osm1.scenario @@ -3,8 +3,15 @@ "description" : "", "release" : "0.2", "processWriters" : { - "files" : [ ], - "processors" : [ ], + "files" : [ { + "type" : "org.vadere.simulator.projects.dataprocessing.outputfile.TimestepPedestrianIdOutputFile", + "filename" : "out.trajectories", + "processors" : [ 1 ] + } ], + "processors" : [ { + "type" : "org.vadere.simulator.projects.dataprocessing.processor.PedestrianPositionProcessor", + "id" : 1 + } ], "isTimestamped" : true }, "scenario" : { diff --git a/VadereModelTests/TestOSM/scenarios/rimea_06_corner_osm2.scenario b/VadereModelTests/TestOSM/scenarios/rimea_06_corner_osm2.scenario index c807f5f71e8b7c5826dff4153a4647bb6b8b5493..4e6fbae80e74167e3df7660d21401ce3688de74e 100644 --- a/VadereModelTests/TestOSM/scenarios/rimea_06_corner_osm2.scenario +++ b/VadereModelTests/TestOSM/scenarios/rimea_06_corner_osm2.scenario @@ -142,11 +142,13 @@ "targetIds" : [ 1 ], "position" : { "x" : 4.3, - "y" : 4.6 + "y" : 4.6, + "identifier" : -1 }, "velocity" : { "x" : 0.0, - "y" : 0.0 + "y" : 0.0, + "identifier" : -1 }, "nextTargetListIndex" : -1, "freeFlowSpeed" : 1.34, diff --git a/VadereModelTests/TestOSM/scenarios/rimea_08_parameter_osm1.scenario b/VadereModelTests/TestOSM/scenarios/rimea_08_parameter_osm1.scenario index 673d4e71d71beb6793c9b23952d149f6a314cd37..e4eb692871e294961b2c9c0bbee77f3d1abdcd8f 100644 --- a/VadereModelTests/TestOSM/scenarios/rimea_08_parameter_osm1.scenario +++ b/VadereModelTests/TestOSM/scenarios/rimea_08_parameter_osm1.scenario @@ -1568,7 +1568,8 @@ "treadCount" : 13, "upwardDirection" : { "x" : -1.0, - "y" : 0.0 + "y" : 0.0, + "identifier" : -1 } }, { "shape" : { @@ -1582,7 +1583,8 @@ "treadCount" : 13, "upwardDirection" : { "x" : 1.0, - "y" : 0.0 + "y" : 0.0, + "identifier" : -1 } } ], "targets" : [ { diff --git a/VadereModelTests/TestOSM/scenarios/rimea_09_publicRoom_osm1_2.scenario b/VadereModelTests/TestOSM/scenarios/rimea_09_publicRoom_osm1_2.scenario index b025d550fad5b2a184f7ab75c5b308cd77160856..603a5faa5274fe78816ff70c6d9cd05e55f66f9c 100644 --- a/VadereModelTests/TestOSM/scenarios/rimea_09_publicRoom_osm1_2.scenario +++ b/VadereModelTests/TestOSM/scenarios/rimea_09_publicRoom_osm1_2.scenario @@ -160,28 +160,36 @@ "type" : "POLYGON", "points" : [ { "x" : 31.0, - "y" : 27.0 + "y" : 27.0, + "identifier" : -1 }, { "x" : 2.0, - "y" : 27.0 + "y" : 27.0, + "identifier" : -1 }, { "x" : 2.0, - "y" : 29.0 + "y" : 29.0, + "identifier" : -1 }, { "x" : 33.0, - "y" : 29.0 + "y" : 29.0, + "identifier" : -1 }, { "x" : 33.0, - "y" : 2.0 + "y" : 2.0, + "identifier" : -1 }, { "x" : 2.0, - "y" : 2.0 + "y" : 2.0, + "identifier" : -1 }, { "x" : 2.0, - "y" : 4.0 + "y" : 4.0, + "identifier" : -1 }, { "x" : 31.0, - "y" : 4.0 + "y" : 4.0, + "identifier" : -1 } ] }, "waitingTime" : 0.0, diff --git a/VadereModelTests/TestOSM/scenarios/rimea_09_publicRoom_osm1_4.scenario b/VadereModelTests/TestOSM/scenarios/rimea_09_publicRoom_osm1_4.scenario index 04e2804d9775746895c0977a2aa42f1a8fc2cd96..170483ac17b83bacdeb49c3a67b41dfbc34b1b86 100644 --- a/VadereModelTests/TestOSM/scenarios/rimea_09_publicRoom_osm1_4.scenario +++ b/VadereModelTests/TestOSM/scenarios/rimea_09_publicRoom_osm1_4.scenario @@ -151,28 +151,36 @@ "type" : "POLYGON", "points" : [ { "x" : 31.0, - "y" : 27.0 + "y" : 27.0, + "identifier" : -1 }, { "x" : 2.0, - "y" : 27.0 + "y" : 27.0, + "identifier" : -1 }, { "x" : 2.0, - "y" : 29.0 + "y" : 29.0, + "identifier" : -1 }, { "x" : 33.0, - "y" : 29.0 + "y" : 29.0, + "identifier" : -1 }, { "x" : 33.0, - "y" : 2.0 + "y" : 2.0, + "identifier" : -1 }, { "x" : 2.0, - "y" : 2.0 + "y" : 2.0, + "identifier" : -1 }, { "x" : 2.0, - "y" : 4.0 + "y" : 4.0, + "identifier" : -1 }, { "x" : 31.0, - "y" : 4.0 + "y" : 4.0, + "identifier" : -1 } ] }, "waitingTime" : 0.0, diff --git a/VadereModelTests/TestOSM/scenarios/rimea_13_stairs_osm1_a.scenario b/VadereModelTests/TestOSM/scenarios/rimea_13_stairs_osm1_a.scenario index 5357a4a14490dfc831cf9c52539085f48b628b1e..bb7d6563e269cf849e654e4b8e4966cb5ab71e3b 100644 --- a/VadereModelTests/TestOSM/scenarios/rimea_13_stairs_osm1_a.scenario +++ b/VadereModelTests/TestOSM/scenarios/rimea_13_stairs_osm1_a.scenario @@ -110,7 +110,8 @@ "treadCount" : 15, "upwardDirection" : { "x" : 1.0, - "y" : 0.0 + "y" : 0.0, + "identifier" : -1 } } ], "targets" : [ { diff --git a/VadereModelTests/TestOSM/scenarios/rimea_13_stairs_osm1_b.scenario b/VadereModelTests/TestOSM/scenarios/rimea_13_stairs_osm1_b.scenario index 016881c57467cab504ac363ea03781b47fba1412..fc21f512e9289a8a02f62b3c4f8e32fd91d4f4f2 100644 --- a/VadereModelTests/TestOSM/scenarios/rimea_13_stairs_osm1_b.scenario +++ b/VadereModelTests/TestOSM/scenarios/rimea_13_stairs_osm1_b.scenario @@ -110,7 +110,8 @@ "treadCount" : 15, "upwardDirection" : { "x" : 1.0, - "y" : 0.0 + "y" : 0.0, + "identifier" : -1 } } ], "targets" : [ { diff --git a/VadereModelTests/TestOSM/scenarios/rimea_14_selectRoute_osm1.scenario b/VadereModelTests/TestOSM/scenarios/rimea_14_selectRoute_osm1.scenario index e087559febd7c1fc00edb68d49a1fc1bb4b647a9..6455278970c9dcc517279b79f8cbad0eeebd7b9a 100644 --- a/VadereModelTests/TestOSM/scenarios/rimea_14_selectRoute_osm1.scenario +++ b/VadereModelTests/TestOSM/scenarios/rimea_14_selectRoute_osm1.scenario @@ -146,7 +146,8 @@ "treadCount" : 13, "upwardDirection" : { "x" : 0.0, - "y" : 1.0 + "y" : 1.0, + "identifier" : -1 } }, { "shape" : { @@ -160,7 +161,8 @@ "treadCount" : 13, "upwardDirection" : { "x" : 0.0, - "y" : -1.0 + "y" : -1.0, + "identifier" : -1 } } ], "targets" : [ { diff --git a/VadereSimulator/src/org/vadere/simulator/projects/io/TrajectoryReader.java b/VadereSimulator/src/org/vadere/simulator/projects/io/TrajectoryReader.java index e3615a4b7817b85b6e4464ee49be75862703c367..c52b47a017235214c9bed1e209b032ab7880312d 100644 --- a/VadereSimulator/src/org/vadere/simulator/projects/io/TrajectoryReader.java +++ b/VadereSimulator/src/org/vadere/simulator/projects/io/TrajectoryReader.java @@ -129,6 +129,5 @@ public class TrajectoryReader { logger.warn("could not read trajectory file. The file format might not be compatible or it is missing."); throw e; } - } } diff --git a/VadereUtils/src/org/vadere/util/geometry/mesh/inter/IPolyConnectivity.java b/VadereUtils/src/org/vadere/util/geometry/mesh/inter/IPolyConnectivity.java index a9ba95c893fb2749e68ce769df4319d429581bbc..13d05664c2b5c4412de86fe2fdd3da6261bf9f2c 100644 --- a/VadereUtils/src/org/vadere/util/geometry/mesh/inter/IPolyConnectivity.java +++ b/VadereUtils/src/org/vadere/util/geometry/mesh/inter/IPolyConnectivity.java @@ -288,6 +288,13 @@ public interface IPolyConnectivity

, E ext return GeometryUtils.isRightOf(p1, p2, x1, y1); } + /** + * Tests if the line-segment edge intersects the line defined by p1 and p2. + * @param p1 + * @param p2 + * @param edge + * @return + */ default boolean intersects(final IPoint p1, final IPoint p2, E edge) { VPoint q1 = getMesh().toPoint(getMesh().getVertex(getMesh().getPrev(edge))); VPoint q2 = getMesh().toPoint(getMesh().getVertex(edge)); diff --git a/VadereUtils/src/org/vadere/util/geometry/mesh/inter/ITriConnectivity.java b/VadereUtils/src/org/vadere/util/geometry/mesh/inter/ITriConnectivity.java index 568c0be8cc4853e6b1003b8a2e00c157783d18f4..3a155c06ebf1f7d20a0e5b088981986a1e982c79 100644 --- a/VadereUtils/src/org/vadere/util/geometry/mesh/inter/ITriConnectivity.java +++ b/VadereUtils/src/org/vadere/util/geometry/mesh/inter/ITriConnectivity.java @@ -751,30 +751,17 @@ public interface ITriConnectivity

, E exte * @param stopCondition * @return */ - default F straightWalk2D(final E startVertex, final VPoint direction, final Predicate stopCondition) { + default F straightWalk2D(final E startVertex, final F face, final VPoint direction, final Predicate stopCondition) { - E startEdge = getMesh().getPrev(startVertex); + E edge = getMesh().getPrev(startVertex); VPoint p1 = getMesh().toPoint(startVertex); - VPoint p2 = p1.add(direction); - VPoint p3 = getMesh().toPoint(startEdge); - VPoint p4 = getMesh().toPoint(getMesh().getPrev(startEdge)); - VPoint q = p1; - VPoint p = GeometryUtils.intersectionPoint(p1.getX(), p1.getY(), p2.getX(), p2.getY(), p3.getX(), p3.getY(), p4.getX(), p4.getY()); - - F face; - E edge; - if(isRightOf(p.getX(), p.getY(), startEdge)) { - edge = startEdge; - face = getMesh().getFace(edge); - } - else { - edge = getMesh().getTwin(startEdge); - face = getMesh().getFace(edge); - } - - return straightWalk2D(q, p, face, edge, stopCondition); + assert intersects(p1, p1.add(direction), edge); + assert getMesh().getEdges(face).contains(startVertex); + // TODO: quick solution! + VPoint q = p1.add(direction.scalarMultiply(Double.MAX_VALUE * 0.5)); + return straightWalk2D(p1, q, face, edge, e -> (stopCondition.test(e) || !isRightOf(q.x, q.y, e))); } default F straightWalk2D(final double x1, final double y1, final F startFace, final Predicate stopCondition) { @@ -789,6 +776,19 @@ public interface ITriConnectivity

, E exte return straightWalk2D(q, p, face, edge, stopCondition); } + /** + * Walks along the line defined by q and p. The walking direction should be controlled by the stopCondition e.g. + * e -> !isRightOf(x1, y1, e) stops the walk if (x1, y1) is on the left side of each edge which is the case if the + * point is inside the reached face. The walk starts at the startFace and continues in the direction of line defined + * by q and p using the any edge which does not fulfill the stopCondition. + * + * @param q + * @param p + * @param startFace + * @param startEdge + * @param stopCondition + * @return + */ default F straightWalk2D(final VPoint q, final VPoint p, final F startFace, final E startEdge, final Predicate stopCondition) { Optional optEdge; F face = startFace; diff --git a/VadereUtils/src/org/vadere/util/geometry/shapes/VLine.java b/VadereUtils/src/org/vadere/util/geometry/shapes/VLine.java index 854b8faf26e0b82ce853d91f13504b7183698abb..c7f3ef63b7f7972ca7cbc33374df8e458eb5d590 100644 --- a/VadereUtils/src/org/vadere/util/geometry/shapes/VLine.java +++ b/VadereUtils/src/org/vadere/util/geometry/shapes/VLine.java @@ -15,9 +15,9 @@ public class VLine extends Line2D.Double { public VLine(final VPoint p1, final VPoint p2) { super(p1.getX(), p1.getY(), p2.getX(), p2.getY()); - if(p1.equals(p2)) { + /*if(p1.equals(p2)) { throw new IllegalArgumentException(p1 + " is equal " + p2); - } + }*/ this.p1 = p1; this.p2 = p2; } diff --git a/VadereUtils/src/org/vadere/util/potential/CellGrid.java b/VadereUtils/src/org/vadere/util/potential/CellGrid.java index cce9a89f472f3f5a7230cccde2c10113202a0cdb..fe0f5eb5de8a82fd011c0bb7b1ca19ba56660af3 100644 --- a/VadereUtils/src/org/vadere/util/potential/CellGrid.java +++ b/VadereUtils/src/org/vadere/util/potential/CellGrid.java @@ -31,6 +31,10 @@ public class CellGrid { /** Number of points along y axis. */ protected final int numPointsY; + protected final double xMin; + + protected final double yMin; + protected CellState[][] values; /** @@ -38,10 +42,12 @@ public class CellGrid { * point values are initialized with 'value'. */ public CellGrid(double width, double height, double resolution, - CellState value) { + CellState value, double xMin, double yMin) { this.width = width; this.height = height; this.resolution = resolution; + this.xMin = xMin; + this.yMin = yMin; /* 0.001 avoids that numPointsX/Y are too small due to numerical errors. */ numPointsX = (int) Math.floor(width / resolution + 0.001) + 1; @@ -52,6 +58,14 @@ public class CellGrid { reset(value); } + /** + * Creates an grid with the given width, height and resolution. All grid + * point values are initialized with 'value'. + */ + public CellGrid(double width, double height, double resolution, CellState value) { + this(width, height, resolution, value, 0, 0); + } + /** * Creates a deep copy of the given grid. */ @@ -62,6 +76,8 @@ public class CellGrid { numPointsX = grid.numPointsX; numPointsY = grid.numPointsY; values = new CellState[numPointsX][numPointsY]; + xMin = grid.xMin; + yMin = grid.yMin; for (int row = 0; row < numPointsY; row++) { for (int col = 0; col < numPointsX; col++) { @@ -136,7 +152,7 @@ public class CellGrid { * Converts the matrix indices to coordinates. */ public VPoint pointToCoord(int pointX, int pointY) { - return new VPoint(pointX * resolution, pointY * resolution); + return new VPoint(xMin + pointX * resolution, yMin + pointY * resolution); } /** @@ -181,20 +197,20 @@ public class CellGrid { * Returns the closest grid point (matrix index) to the given coordinates. */ public Point getNearestPoint(double x, double y) { - if (x < 0) { - x = 0; + if (x < xMin) { + x = xMin; } - if (y < 0) { - y = 0; + if (y < yMin) { + y = yMin; } - if (y > getHeight()) { - y = getHeight(); + if (y > getHeight() + yMin) { + y = yMin+getHeight(); } - if (x > getWidth()) { - x = getWidth(); + if (x > getWidth() + xMin) { + x = getWidth() + xMin; } - return new Point((int) (x / resolution + 0.5), - (int) (y / resolution + 0.5)); + return new Point((int) ((x - xMin) / resolution + 0.5), + (int) ((y - yMin) / resolution + 0.5)); } /** @@ -202,19 +218,19 @@ public class CellGrid { * towards origin. */ public Point getNearestPointTowardsOrigin(double x, double y) { - if (x < 0) { - x = 0; - } - if (y < 0) { - y = 0; - } - if (y > getHeight()) { - y = getHeight(); - } - if (x > getWidth()) { - x = getWidth(); - } - return new Point((int) (x / resolution), (int) (y / resolution)); + if (x < xMin) { + x = xMin; + } + if (y < yMin) { + y = yMin; + } + if (y > getHeight() + yMin) { + y = yMin+getHeight(); + } + if (x > getWidth() + xMin) { + x = getWidth() + xMin; + } + return new Point((int) ((x - xMin) / resolution), (int) ((y - yMin) / resolution)); } /** @@ -286,6 +302,10 @@ public class CellGrid { .flatMap(stream -> stream); } + public double getMinX() { return xMin; } + + public double getMinY() { return yMin; } + public boolean isValidPoint(Point point) { Point p = (point); diff --git a/VadereUtils/src/org/vadere/util/potential/calculators/AbstractGridEikonalSolver.java b/VadereUtils/src/org/vadere/util/potential/calculators/AbstractGridEikonalSolver.java index e5fd212a028cc8436cfef372d937fa387b12d05d..6d4f3393dcb8e046e092de6db758674193f68ae8 100644 --- a/VadereUtils/src/org/vadere/util/potential/calculators/AbstractGridEikonalSolver.java +++ b/VadereUtils/src/org/vadere/util/potential/calculators/AbstractGridEikonalSolver.java @@ -27,11 +27,11 @@ public abstract class AbstractGridEikonalSolver implements EikonalSolver { double gridPotentials[]; double weightOfKnown[] = new double[1]; - if (x >= potentialField.getWidth()) { + if (x >= potentialField.getWidth() + potentialField.getMinY()) { incX = 0; } - if (y >= potentialField.getHeight()) { + if (y >= potentialField.getHeight() + potentialField.getMinY()) { incY = 0; } diff --git a/VadereUtils/src/org/vadere/util/potential/calculators/EikonalSolverFMM.java b/VadereUtils/src/org/vadere/util/potential/calculators/EikonalSolverFMM.java index 77282deea27f9d46cecbdc8717000695142df8f5..81241b43b57a02883b3ea8f95b22c828520a251c 100644 --- a/VadereUtils/src/org/vadere/util/potential/calculators/EikonalSolverFMM.java +++ b/VadereUtils/src/org/vadere/util/potential/calculators/EikonalSolverFMM.java @@ -14,6 +14,7 @@ import org.vadere.util.potential.CellGrid; import org.vadere.util.potential.CellState; import org.vadere.util.potential.PathFindingTag; import org.vadere.util.potential.timecost.ITimeCostFunction; +import org.vadere.util.triangulation.adaptive.IDistanceFunction; /** * EikonalSolverFMM initializes a potential field on basis @@ -29,6 +30,8 @@ public class EikonalSolverFMM extends AbstractGridEikonalSolver { protected CellGrid cellGrid; protected List targetPoints; protected List targetShapes; + protected IDistanceFunction distFunc; + boolean isHighAccuracy = false; /** only for logging */ @@ -63,13 +66,41 @@ public class EikonalSolverFMM extends AbstractGridEikonalSolver { } } + /** + * Initializes the FM potential calculator with a time cost function F > 0. + */ + public EikonalSolverFMM( + final CellGrid potentialField, + final IDistanceFunction distFunc, + final boolean isHighAccuracy, + final ITimeCostFunction timeCostFunction, + final double unknownPenalty) { + super(potentialField, unknownPenalty); + this.cellGrid = potentialField; + this.targetPoints = cellGrid.pointStream().filter(p -> cellGrid.getValue(p).tag == PathFindingTag.Target) + .collect(Collectors.toList()); + this.distFunc = distFunc; + this.isHighAccuracy = isHighAccuracy; + + ComparatorPotentialFieldValue comparator = new ComparatorPotentialFieldValue( + potentialField); + this.narrowBand = new PriorityQueue<>(50, comparator); + this.timeCostFunction = timeCostFunction; + + if (targetPoints.size() == 0) { + logger.error("PotentialFieldInitializerFastMarching::Run(): " + + "Warning, no target points given. Target missing or grid resolution too low."); + return; + } + } + @Override public void initialize() { for (Point point : targetPoints) { - if(!targetShapes.isEmpty()) { - setTargetNeighborsDistances(point); - } + //if(!targetShapes.isEmpty()) { + setTargetNeighborsDistances(point); + //} narrowBand.add(point); } @@ -181,7 +212,7 @@ public class EikonalSolverFMM extends AbstractGridEikonalSolver { cellGrid.setValue( neighbor, - new CellState(minDistanceToTarget(cellGrid.pointToCoord(neighbor)), + new CellState(minDistanceToTarget(cellGrid.pointToCoord(neighbor)) / timeCostFunction.costAt(cellGrid.pointToCoord(neighbor)), PathFindingTag.Reachable)); narrowBand.add(neighbor); } @@ -195,13 +226,23 @@ public class EikonalSolverFMM extends AbstractGridEikonalSolver { // computed starting there. VPoint dp = new VPoint(cellGrid.getWidth() / (cellGrid.getNumPointsX() - 1) / 2.0, cellGrid.getHeight() / (cellGrid.getNumPointsY() - 1) / 2.0); - for (VShape targetShape : targetShapes) { - // negative distances are possible when point is inside the target - tmp = Math.max(0, targetShape.distance(point.add(dp))); - if (tmp < minDistance) { - minDistance = tmp; - } - } + + if(targetShapes != null && !targetShapes.isEmpty()) { + for (VShape targetShape : targetShapes) { + // negative distances are possible when point is inside the target + tmp = Math.max(0, targetShape.distance(point.add(dp))); + if (tmp < minDistance) { + minDistance = tmp; + } + } + } + else if(distFunc != null) { + tmp = Math.max(0, -distFunc.apply(point)); + if (tmp < minDistance) { + minDistance = tmp; + } + } + return minDistance; } } diff --git a/VadereUtils/src/org/vadere/util/potential/calculators/EikonalSolverFMMTriangulation.java b/VadereUtils/src/org/vadere/util/potential/calculators/EikonalSolverFMMTriangulation.java index c52a3fcd6062a9608fd8422c7cf35c4be9daa603..9dc29e5c303cd4999fee3edc8a79bb3a0c021f0b 100644 --- a/VadereUtils/src/org/vadere/util/potential/calculators/EikonalSolverFMMTriangulation.java +++ b/VadereUtils/src/org/vadere/util/potential/calculators/EikonalSolverFMMTriangulation.java @@ -129,7 +129,8 @@ public class EikonalSolverFMMTriangulation

triangulation, - @NotNull final Collection targetVertices + @NotNull final Collection targetVertices, + @NotNull final IDistanceFunction distFunc ) { this.triangulation = triangulation; this.calculationFinished = false; @@ -140,15 +141,27 @@ public class EikonalSolverFMMTriangulation

rightAngle + GeometryUtils.DOUBLE_EPS; + } + /** * Updates a point given a triangle. The point can only be updated if the * triangle triangleContains it and the other two points are in the frozen band. @@ -277,44 +303,47 @@ public class EikonalSolverFMMTriangulation

edges = mesh.getEdges(face); - edges.removeIf(edge -> mesh.getPoint(edge).equals(point)); + E halfEdge = edges.stream().filter(e -> mesh.getPoint(e).equals(point)).findAny().get(); + edges.removeIf(edge -> mesh.getPoint(edge).equals(point)); assert edges.size() == 2; + P p1 = mesh.getPoint(edges.get(0)); P p2 = mesh.getPoint(edges.get(1)); - if(feasableForComputation(p1) && feasableForComputation(p2)) { - if(!mesh.toTriangle(face).isNonAcute()) { + if(isFeasibleForComputation(p1) && isFeasibleForComputation(p2)) { + if(!isNonAcute(halfEdge)) { return computePotential(point, p1, p2); - } - /*else { - logger.info("special case for non-acute triangle"); - E halfEdge = mesh.getEdges(face).stream().filter(p -> mesh.getPoint(p).equals(point)).findAny().get(); - Optional

optPoint = walkToFeasablePoint(halfEdge, face); + } // we only try to find a virtual vertex if both points are already frozen + else { + //logger.info("special case for non-acute triangle"); + Optional

optPoint = walkToFeasiblePoint(halfEdge, face); + if(optPoint.isPresent()) { P surrogatePoint = optPoint.get(); - if(feasableForComputation(surrogatePoint)) { + if(isFeasibleForComputation(surrogatePoint)) { + //logger.info("feasible point found for " + point + " and " + face); return Math.min(computePotential(point, surrogatePoint, p2), computePotential(point, p1, surrogatePoint)); } else { - logger.info("no feasable point found for " + point + " and " + face); + logger.warn("no feasible point found for " + point + " and " + face); } } else { - logger.info("no point found for " + point + " and " + face); + logger.warn("no point found for " + point + " and " + face); } - }*/ + } } return Double.MAX_VALUE; } - private boolean feasableForComputation(final P p){ + private boolean isFeasibleForComputation(final P p){ //return p.getPathFindingTag().frozen; return p.getPathFindingTag() == PathFindingTag.Reachable || p.getPathFindingTag() == PathFindingTag.Reached; } - private Optional

walkToFeasablePoint(@NotNull final E halfEdge, @NotNull final F face) { + private Optional

walkToFeasiblePoint(@NotNull final E halfEdge, @NotNull final F face) { assert mesh.toTriangle(face).isNonAcute(); E next = mesh.getNext(halfEdge); @@ -324,12 +353,34 @@ public class EikonalSolverFMMTriangulation

feasableVertexPred = v -> GeometryUtils.isLeftOf(p, p.add(direction2), mesh.toPoint(v)) && GeometryUtils.isRightOf(p, p.add(direction1), mesh.toPoint(v)); + //logger.info(p + ", " + pNext + ", " + pPrev); + //logger.info(direction1 + ", " + direction2); + + Predicate isPointInCone = e -> + { + VPoint point = mesh.toPoint(e); + return GeometryUtils.isLeftOf(p, p.add(direction2), point) && + GeometryUtils.isRightOf(p, p.add(direction1), point); + }; + + Predicate isEdgeInCone = e -> isPointInCone.test(e) || isPointInCone.test(mesh.getPrev(e)); + + F destination = triangulation.straightWalk2D(halfEdge, face, direction1, isEdgeInCone); + + assert !destination.equals(face); + + if(!mesh.isBoundary(destination)) { + return mesh.streamEdges(destination).filter(e -> isPointInCone.test(e)).map(v -> mesh.getPoint(v)).findAny(); + } + else { + logger.warn("walked to boundary"); + return Optional.empty(); + } + + /*Predicate feasableVertexPred = v -> GeometryUtils.isLeftOf(p, p.add(direction2), mesh.toPoint(v)) && GeometryUtils.isRightOf(p, p.add(direction1), mesh.toPoint(v)); Predicate feasableEdgePred = e -> feasableVertexPred.test(mesh.getVertex(halfEdge)); Predicate stopCondition = e -> { VPoint midPoint = mesh.toLine(e).midPoint(); @@ -343,7 +394,7 @@ public class EikonalSolverFMMTriangulation

pointList = mesh.getEdges(face).stream() - .filter(he -> feasableForComputation(mesh.getPoint(he))) + .filter(he -> isFeasibleForComputation(mesh.getPoint(he))) .filter(he -> !mesh.getVertex(he).equals(acceptedPoint)) .filter(he -> !mesh.getVertex(he).equals(point)) .collect(Collectors.toList()); @@ -426,7 +477,7 @@ public class EikonalSolverFMMTriangulation

Math.abs(6 - Math.sqrt(p.getX() * p.getX() + p.getY() * p.getY())) - 4; //IDistanceFunction distanceFunc4 = p -> Math.max(Math.abs(p.getY()) - 4, Math.abs(p.getX()) - 25); //IEdgeLengthFunction edgeLengthFunc = p -> 1.0 + p.distanceToOrigin(); - IEdgeLengthFunction edgeLengthFunc = p -> 0.5; + //IEdgeLengthFunction edgeLengthFunc = p -> 1.0; + //IEdgeLengthFunction edgeLengthFunc = p -> 1.0 + Math.min(Math.abs(distanceFunc.apply(p) + 4), Math.abs(distanceFunc.apply(p))); + IEdgeLengthFunction edgeLengthFunc = p -> 1.0 + Math.abs(distanceFunc.apply(p)*0.5); + //IDistanceFunction distanceFunc = p -> Math.max(Math.max(Math.max(distanceFunc1.apply(p), distanceFunc2.apply(p)), distanceFunc3.apply(p)), distanceFunc4.apply(p)); //IEdgeLengthFunction edgeLengthFunc = p -> 1.0 + Math.abs(distanceFunc.apply(p))/2; @@ -29,7 +33,7 @@ public class TestEnhancedVersion3 extends JFrame { //IEdgeLengthFunction edgeLengthFunc = p -> 1.0 + Math.min(Math.abs(distanceFunc.apply(p) + 4), Math.abs(distanceFunc.apply(p))); //IEdgeLengthFunction edgeLengthFunc = p -> 1.0; VRectangle bbox = new VRectangle(-11, -11, 22, 22); - PSMeshing meshGenerator = new PSMeshing(distanceFunc, edgeLengthFunc, 6.0, bbox, new ArrayList<>()); + PSMeshing meshGenerator = new PSMeshing(distanceFunc, edgeLengthFunc, 0.6, bbox, new ArrayList<>()); meshGenerator.initialize(); PSMeshingPanel distmeshPanel = new PSMeshingPanel(meshGenerator, 1000, 800); @@ -44,7 +48,7 @@ public class TestEnhancedVersion3 extends JFrame { int counter = 0; long time = 0; - while (counter <= 560) { + while (counter <= 1000) { //obscuteTriangles = meshGenerator.getTriangles().stream().filter(tri -> tri.isNonAcute()).count(); //PriorityQueue> priorityQueue = meshGenerator.getQuailties(); //avgQuality = priorityQueue.stream().reduce(0.0, (aDouble, meshPointPFace) -> aDouble + meshGenerator.faceToQuality(meshPointPFace), (d1, d2) -> d1 + d2) / priorityQueue.size(); @@ -64,7 +68,9 @@ public class TestEnhancedVersion3 extends JFrame { System.out.println("Quality: " + meshGenerator.getQuality()); System.out.println("Step-Time: " + ms); } + meshGenerator.finalize(); System.out.print("overall time: " + time); + System.out.print(TexGraphGenerator.meshToGraph(meshGenerator.getMesh())); //System.out.print("finished:" + meshGenerator.getMesh().getVertices().stream().filter(v -> !meshGenerator.getMesh().isDestroyed(v)).count()); //System.out.print("finished:" + avgQuality); diff --git a/VadereUtils/tests/org/vadere/util/potential/TestFFMNonUniformTriangulation.java b/VadereUtils/tests/org/vadere/util/potential/TestFFMNonUniformTriangulation.java index 43ee3fc1855a697877e30eefd22c8481341ca359..a4011f1c1f806d6bed041f59b7195dc7d5d31a14 100644 --- a/VadereUtils/tests/org/vadere/util/potential/TestFFMNonUniformTriangulation.java +++ b/VadereUtils/tests/org/vadere/util/potential/TestFFMNonUniformTriangulation.java @@ -2,12 +2,14 @@ package org.vadere.util.potential; import org.apache.log4j.LogManager; import org.apache.log4j.Logger; +import org.jetbrains.annotations.NotNull; import org.junit.Before; import org.junit.Test; import org.vadere.util.geometry.mesh.gen.PFace; import org.vadere.util.geometry.mesh.gen.PHalfEdge; import org.vadere.util.geometry.mesh.gen.PVertex; import org.vadere.util.geometry.mesh.inter.ITriangulation; +import org.vadere.util.geometry.mesh.inter.IVertex; import org.vadere.util.geometry.shapes.IPoint; import org.vadere.util.geometry.shapes.VCircle; import org.vadere.util.geometry.shapes.VPoint; @@ -23,12 +25,15 @@ import org.vadere.util.triangulation.adaptive.IEdgeLengthFunction; import org.vadere.util.triangulation.adaptive.MeshPoint; import org.vadere.util.triangulation.adaptive.PSMeshing; +import java.awt.*; import java.io.FileNotFoundException; import java.io.FileWriter; import java.io.IOException; import java.util.ArrayList; +import java.util.Comparator; import java.util.List; import java.util.stream.Collectors; +import java.util.stream.Stream; public class TestFFMNonUniformTriangulation { @@ -60,9 +65,10 @@ public class TestFFMNonUniformTriangulation { @Test public void testTriangulationFMM() { - IEdgeLengthFunction edgeLengthFunc = p -> 1.0 + Math.min(Math.abs(distanceFunc.apply(p) + 4), Math.abs(distanceFunc.apply(p))); - IEdgeLengthFunction unifromEdgeLengthFunc = p -> 1.0; - List targetAreas = new ArrayList<>(); + //IEdgeLengthFunction edgeLengthFunc = p -> 1.0 + 0.5*Math.min(Math.abs(distanceFunc.apply(p) + 4), Math.abs(distanceFunc.apply(p))); + //IEdgeLengthFunction unifromEdgeLengthFunc = p -> 1.0; + IEdgeLengthFunction edgeLengthFunc = p -> 1.0 + Math.abs(distanceFunc.apply(p)) * 0.5; + List targetAreas = new ArrayList<>(); List targetPoints = new ArrayList<>(); PSMeshing meshGenerator = new PSMeshing(distanceFunc, edgeLengthFunc, 0.6, bbox, new ArrayList<>()); meshGenerator.execute(); @@ -74,16 +80,22 @@ public class TestFFMNonUniformTriangulation { VRectangle rect = new VRectangle(width / 2, height / 2, 100, 100); targetAreas.add(rect); - EikonalSolver solver = new EikonalSolverFMMTriangulation(new UnitTimeCostFunction(), triangulation, - triangulation.getMesh().getBoundaryVertices().stream().collect(Collectors.toList())); + EikonalSolver solver = new EikonalSolverFMMTriangulation( + new UnitTimeCostFunction(), + triangulation, + triangulation.getMesh().getBoundaryVertices().stream().collect(Collectors.toList()), + distanceFunc); log.info("start FFM"); solver.initialize(); log.info("FFM finished"); double maxError = 0; + double sum = 0; + int counter = 0; try { //System.out.println(getClass().getClassLoader().getResource("./potentialField.csv").getFile()); - FileWriter writer = new FileWriter("./potentialField.csv"); + FileWriter writer = new FileWriter("./potentialField_adapt_0_7.csv"); + for(double y = bbox.getMinY()+2; y <= bbox.getMaxY()-2; y += 0.1) { for(double x = bbox.getMinX()+2; x < bbox.getMaxX()-2; x += 0.1) { double val = solver.getValue(x ,y); @@ -91,6 +103,8 @@ public class TestFFMNonUniformTriangulation { double side = Math.min((new VPoint(x, y).distanceToOrigin()-2.0), (10 - new VPoint(x, y).distanceToOrigin())); side = Math.max(side, 0.0); maxError = Math.max(maxError, Math.abs(val - side)); + sum += Math.abs(val - side) * Math.abs(val - side); + counter++; } writer.write(""+solver.getValue(x ,y) + " "); } @@ -105,48 +119,146 @@ public class TestFFMNonUniformTriangulation { } log.info(triangulation.getMesh().getVertices().size()); - log.info("max edge length: " + triangulation.getMesh().streamEdges().map(e -> triangulation.getMesh().toLine(e).length()).max((d1, d2) -> Double.compare(d1, d2))); - log.info("min edge length: " +triangulation.getMesh().streamEdges().map(e -> triangulation.getMesh().toLine(e).length()).min((d1, d2) -> Double.compare(d1, d2))); - log.info("max error: " + maxError); + log.info("max edge length: " + triangulation.getMesh().streamEdges().map(e -> triangulation.getMesh().toLine(e).length()).max(Comparator.comparingDouble(d -> d))); + log.info("min edge length: " +triangulation.getMesh().streamEdges().map(e -> triangulation.getMesh().toLine(e).length()).min(Comparator.comparingDouble(d -> d))); + + log.info("max distance to boundary: " + triangulation.getMesh().getBoundaryVertices().stream().map(p -> Math.abs(distanceFunc.apply(p))).max(Comparator.comparingDouble(d -> d))); + //log.info("L2-Error: " + computeL2Error(triangulation, distanceFunc)); + log.info("max error: " + maxError); + log.info("max error-2: " + triangulation.getMesh().getVertices().stream().map(p -> Math.abs(Math.abs(p.getPoint().getPotential() + distanceFunc.apply(p)))).max(Comparator.comparingDouble(d -> d))); + + log.info("L2-error: " + Math.sqrt(sum / counter)); + log.info("L2-error-2: " + Math.sqrt(triangulation.getMesh().getVertices().stream() + .map(p -> Math.abs(Math.abs(p.getPoint().getPotential() + distanceFunc.apply(p)))) + .map(val -> val * val) + .reduce(0.0, (d1, d2) -> d1 + d2) / triangulation.getMesh().getNumberOfVertices())); //assertTrue(0.0 == solver.getValue(5, 5)); //assertTrue(0.0 < solver.getValue(1, 7)); } + @Test + public void testTriangulationFMMCase2() { + + //IEdgeLengthFunction edgeLengthFunc = p -> 1.0 + Math.min(Math.abs(distanceFunc.apply(p) + 4) * 2, Math.abs(distanceFunc.apply(p)) * 2); + //IEdgeLengthFunction unifromEdgeLengthFunc = p -> 1.0; + IEdgeLengthFunction edgeLengthFunc = p -> 1.0 + Math.abs(distanceFunc.apply(p)*0.5); + List targetAreas = new ArrayList<>(); + PSMeshing meshGenerator = new PSMeshing(distanceFunc, edgeLengthFunc, 0.6, bbox, new ArrayList<>()); + meshGenerator.execute(); + triangulation = meshGenerator.getTriangulation(); + + //targetPoints.add(new MeshPoint(0, 0, false)); + + + VRectangle rect = new VRectangle(width / 2, height / 2, 100, 100); + targetAreas.add(rect); + + List> targetPoints = triangulation.getMesh().getVertices().stream() + .filter(v -> triangulation.getMesh().isAtBoundary(v)) + .filter(p-> (Math.abs(new VPoint(p.getX(), p.getY()).distanceToOrigin()-2.0)) < 2).collect(Collectors.toList()); + + log.info(targetPoints); + + EikonalSolver solver = new EikonalSolverFMMTriangulation( + new UnitTimeCostFunction(), + triangulation, + targetPoints, + p -> -(new VPoint(p.getX(), p.getY()).distanceToOrigin()-2.0)); + + log.info("start FFM"); + solver.initialize(); + log.info("FFM finished"); + double maxError = 0; + double sum = 0; + int counter = 0; + try { + //System.out.println(getClass().getClassLoader().getResource("./potentialField.csv").getFile()); + FileWriter writer = new FileWriter("./potentialField_uniform_1_0_s.csv"); + + for(double y = bbox.getMinY()+2; y <= bbox.getMaxY()-2; y += 0.1) { + for(double x = bbox.getMinX()+2; x < bbox.getMaxX()-2; x += 0.1) { + double val = solver.getValue(x ,y); + if(val >= 0.0) { + double side = new VPoint(x, y).distanceToOrigin()-2.0; + side = Math.max(side, 0.0); + maxError = Math.max(maxError, Math.abs(val - side)); + sum += Math.abs(val - side) * Math.abs(val - side); + counter++; + } + writer.write(""+solver.getValue(x ,y) + " "); + } + writer.write("\n"); + } + writer.flush(); + + } catch (FileNotFoundException e) { + e.printStackTrace(); + } catch (IOException e) { + e.printStackTrace(); + } + + log.info(triangulation.getMesh().getVertices().size()); + log.info("max edge length: " + triangulation.getMesh().streamEdges().map(e -> triangulation.getMesh().toLine(e).length()).max(Comparator.comparingDouble(d -> d))); + log.info("min edge length: " +triangulation.getMesh().streamEdges().map(e -> triangulation.getMesh().toLine(e).length()).min(Comparator.comparingDouble(d -> d))); + //log.info("L2-Error: " + computeL2Error(triangulation, distanceFunc)); + log.info("max error: " + maxError); + log.info("L2-error: " + Math.sqrt(sum / counter)); + log.info("max distance to boundary: " + triangulation.getMesh().getBoundaryVertices().stream().map(p -> Math.abs(distanceFunc.apply(p))).max(Comparator.comparingDouble(d -> d))); + //assertTrue(0.0 == solver.getValue(5, 5)); + //assertTrue(0.0 < solver.getValue(1, 7)); + } + + private double computeL2Error(@NotNull final ITriangulation, PHalfEdge, PFace> triangulation, final IDistanceFunction distanceFunc) { + double sum = 0.0; + for(IVertex vertex : triangulation.getMesh().getVertices()) { + double diff = vertex.getPoint().getPotential() + distanceFunc.apply(vertex); + sum += diff * diff; + } + return Math.sqrt(sum); + } + @Test public void testRegularFMM() { - double resolution = 0.4; + double resolution = 0.1; double rad = 2.0; - CellGrid cellGrid = new CellGrid(bbox.getWidth()-4, bbox.getHeight()-4,resolution, new CellState()); + double xMin = -10.0; + double yMin = -10.0; + CellGrid cellGrid = new CellGrid(bbox.getWidth()-3, bbox.getHeight()-3,resolution, new CellState(), xMin, yMin); cellGrid.pointStream() - .filter(p -> new VPoint(p.getX() * resolution, p.getY() * resolution).distance(new VPoint(10, 10)) <= rad - || new VPoint(p.getX() * resolution, p.getY() * resolution).distance(new VPoint(10, 10)) >= 10) + .filter(p -> distanceFunc.apply(cellGrid.pointToCoord(p)) >= 0) .forEach(p -> cellGrid.setValue(p, new CellState(0.0, PathFindingTag.Target))); - EikonalSolver solver = new EikonalSolverFMM(cellGrid, new ArrayList<>(), false, new UnitTimeCostFunction(), 0.1); + EikonalSolver solver = new EikonalSolverFMM(cellGrid, distanceFunc, false, new UnitTimeCostFunction(), 0.1); log.info("start FFM"); solver.initialize(); log.info("FFM finished"); double maxError = 0; + double sum = 0.0; + int counter = 0; try { //System.out.println(getClass().getClassLoader().getResource("./potentialField.csv").getFile()); - FileWriter writer = new FileWriter("./potentialField_reg.csv"); - for(double y = 0; y <= bbox.getHeight()-4; y += 0.1) { - for(double x = 0; x < bbox.getWidth()-4; x += 0.1) { + FileWriter writer = new FileWriter("./potentialField_reg_0_4.csv"); + for(double y = yMin; y < bbox.getMaxY()-2.1; y += 0.1) { + for(double x = xMin; x < bbox.getMaxX()-2.1; x += 0.1) { double val = solver.getValue(x ,y); - if(val >= 0.0) { - double side = Math.min(new VPoint(x, y).distance(new VPoint(10, 10))-2.0, 10 - new VPoint(x, y).distance(new VPoint(10, 10))); + if(val > 0.0) { + double side = Math.min((new VPoint(x, y).distanceToOrigin()-2.0), (10 - new VPoint(x, y).distanceToOrigin())); side = Math.max(side, 0.0); + sum += Math.abs(val - side) * Math.abs(val - side); //double distance = (new VPoint(x, y).distance(new VPoint(10, 10)))-rad; //maxError = Math.max(maxError, Math.abs((distance >= 0.0 ? val-distance : val))); maxError = Math.max(maxError, Math.abs(val - side)); + counter++; } writer.write(""+solver.getValue(x,y) + " "); } writer.write("\n"); } + + writer.flush(); } catch (FileNotFoundException e) { @@ -155,5 +267,88 @@ public class TestFFMNonUniformTriangulation { e.printStackTrace(); } log.info("max error: " + maxError); + + Stream resultPoints = cellGrid.pointStream().filter(p -> cellGrid.getValue(p).tag != PathFindingTag.Obstacle); + double n = resultPoints.count(); + + resultPoints = cellGrid.pointStream().filter(p -> cellGrid.getValue(p).tag != PathFindingTag.Obstacle); + log.info("max error-2: " + resultPoints + .map(p -> Math.abs(cellGrid.getValue(p).potential - distanceFunc.apply(cellGrid.pointToCoord(p)))) + .max(Comparator.comparingDouble(d -> d))); + log.info("L2-error: " + Math.sqrt(sum / counter)); + + resultPoints = cellGrid.pointStream().filter(p -> cellGrid.getValue(p).tag != PathFindingTag.Obstacle); + log.info("max error-2: " + Math.sqrt(resultPoints + .map(p -> Math.abs(cellGrid.getValue(p).potential - distanceFunc.apply(cellGrid.pointToCoord(p)))) + .map(val -> val * val) + .reduce(0.0, (d1, d2) -> d1 + d2) / n)); + + //assertTrue(0.0 == solver.getValue(5, 5)); + //assertTrue(0.0 < solver.getValue(1, 7)); } + + @Test + public void testRegularFMMCase2() { + + double resolution = 0.1; + double rad = 2.0; + double xMin = -10.0; + double yMin = -10.0; + CellGrid cellGrid = new CellGrid(bbox.getWidth()-3, bbox.getHeight()-3,resolution, new CellState(), xMin, yMin); + + // 1. Define the target points + cellGrid.pointStream() + .filter(p -> (cellGrid.pointToCoord(p).distanceToOrigin()-2.0) <= 0) + .forEach(p -> cellGrid.setValue(p, new CellState(0.0, PathFindingTag.Target))); + + // 2. Define the obstacle points + cellGrid.pointStream() + .filter(p -> (cellGrid.pointToCoord(p).distanceToOrigin()) > 10) + .forEach(p -> cellGrid.setValue(p, new CellState(Double.MAX_VALUE, PathFindingTag.Obstacle))); + + EikonalSolver solver = new EikonalSolverFMM( + cellGrid, p -> -(new VPoint(p.getX(), p.getY()).distanceToOrigin()-2.0), + false, new UnitTimeCostFunction(), 0.1); + + + log.info("start FFM"); + solver.initialize(); + log.info("FFM finished"); + double maxError = 0; + double sum = 0.0; + double maxVal = 0.0; + int counter = 0; + try { + //System.out.println(getClass().getClassLoader().getResource("./potentialField.csv").getFile()); + FileWriter writer = new FileWriter("./potentialField_reg_0_6.csv"); + for(double y = yMin; y < bbox.getMaxY()-2; y += 0.1) { + for(double x = xMin; x < bbox.getMaxY()-2; x += 0.1) { + double val = solver.getValue(x ,y); + if(val < Double.MAX_VALUE) { + double side = Math.min(new VPoint(x, y).distanceToOrigin()-2.0, 8); + maxVal = Math.max(maxVal, val); + //log.info(maxVal); + side = Math.max(side, 0.0); + sum += Math.abs(val - side) * Math.abs(val - side); + //double distance = (new VPoint(x, y).distance(new VPoint(10, 10)))-rad; + //maxError = Math.max(maxError, Math.abs((distance >= 0.0 ? val-distance : val))); + maxError = Math.max(maxError, Math.abs(val - side)); + counter++; + } + writer.write(""+solver.getValue(x,y) + " "); + } + writer.write("\n"); + } + writer.flush(); + + } catch (FileNotFoundException e) { + e.printStackTrace(); + } catch (IOException e) { + e.printStackTrace(); + } + log.info("max error: " + maxError); + log.info("L2-error: " + Math.sqrt(sum / counter)); + //assertTrue(0.0 == solver.getValue(5, 5)); + //assertTrue(0.0 < solver.getValue(1, 7)); + } }