From 75adda0234c582bb178a31cde758064e1d3ac72d Mon Sep 17 00:00:00 2001 From: Aren Babikian Date: Fri, 22 Jan 2021 06:17:04 +0100 Subject: Almost done implementing collision correctly --- .../queries/crossingScenarioQueries.vql | 65 +++++++---- .../src/crossingScenario/run/DrawScenario.java | 127 +++++---------------- .../viatra2logic/NumericDrealProblemSolver.java | 4 +- 3 files changed, 75 insertions(+), 121 deletions(-) diff --git a/Domains/crossingScenario/queries/crossingScenarioQueries.vql b/Domains/crossingScenario/queries/crossingScenarioQueries.vql index d1c72820..6c94a702 100644 --- a/Domains/crossingScenario/queries/crossingScenarioQueries.vql +++ b/Domains/crossingScenario/queries/crossingScenarioQueries.vql @@ -463,30 +463,41 @@ pattern collisionExists_qualAbstr(a1:Actor, a2:Actor) { } //<> -/* + //// //VS VisionBlocked //// //TODO Very prone to corner cases - -@Constraint(severity="error", key={a1, a2}, message="x") -pattern collisionExists_vsVisionBlocked(a1:Actor, a2:Actor) { +/* +@Constraint(severity="error", key={a1}, message="x") +pattern collisionExists_vsVisionBlocked(a1:Actor) { VisionBlocked.source(vb, a1); VisionBlocked.target(vb, a2); - neg find helper_collidingActors(a1, a2); + CollisionExists.source(ce, a1); + CollisionExists.target(ce, a3); + a2 != a3; } or { VisionBlocked.source(vb, a1); VisionBlocked.target(vb, a2); - neg find helper_collidingActors(a2, a1); -} - -private pattern helper_collidingActors(a1:Actor, a2:Actor){ - CollisionExists.source(vb, a1); - CollisionExists.target(vb, a2); + CollisionExists.source(ce, a3); + CollisionExists.target(ce, a1); + a2 != a3; +} or { + VisionBlocked.source(vb, a2); + VisionBlocked.target(vb, a1); + CollisionExists.source(ce, a3); + CollisionExists.target(ce, a1); + a2 != a3; +} or { + VisionBlocked.source(vb, a2); + VisionBlocked.target(vb, a1); + CollisionExists.source(ce, a1); + CollisionExists.target(ce, a3); + a2 != a3; } - +*/ //<> - +/* //// //CollisionExists - Time //// @@ -592,7 +603,7 @@ pattern collisionExists_defineCollision_x2(a1:Actor, a2:Actor) { check((xPos1 + (xSpeed1 * cTime)) + (w1/2) < (xPos2 + (xSpeed2 * cTime)) - (w2/2)); } -*/ + ///////*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*////// //VisionBlocked ///////*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*////// @@ -708,8 +719,10 @@ pattern visionBlocked_ites_bottom(a1:Actor, a2:Actor, vb:VisionBlocked) { > ((y1-y2)/(x1-x2))); } -@Constraint(severity="error", key={a1, a2}, message="x") -pattern visionBlocked_blockerNotToRightOfBoth(a1:Actor, a2:Actor) { +//TODO refactor? +//TODO CORNER CASES +@Constraint(severity="error", key={a1, vb}, message="x") +pattern visionBlocked_xdistBSlargerThanxdistTS(a1:Actor, a2:Actor, vb:VisionBlocked) { //This second one is required because we do not want to enforce both a1->c->a2 and a2->c->a1 VisionBlocked.source(vb, a1); VisionBlocked.target(vb, a2); @@ -719,23 +732,31 @@ pattern visionBlocked_blockerNotToRightOfBoth(a1:Actor, a2:Actor) { Actor.xPos(a2, x2); Actor.xPos(aBlocker, xBlocker); - check( x1 < xBlocker && x2 < xBlocker); + //check(slope of a1-to-BlockerBottom > slope of a1-to-a2) + //TODO implement ABSOLUTE VALUE or MULTI-CHECK + check((x1-xBlocker)*(x1-xBlocker) > (x1-x2)*(x1-x2)); } -@Constraint(severity="error", key={a1, a2}, message="x") -pattern visionBlocked_blockerNotToLeftOfBoth(a1:Actor, a2:Actor) { +//TODO refactor? +//TODO CORNER CASES +@Constraint(severity="error", key={a1, vb}, message="x") +pattern visionBlocked_xdistBTlargerThanxdistST(a1:Actor, a2:Actor, vb:VisionBlocked) { //This second one is required because we do not want to enforce both a1->c->a2 and a2->c->a1 - VisionBlocked.source(vb, a1); - VisionBlocked.target(vb, a2); + VisionBlocked.source(vb, a2); + VisionBlocked.target(vb, a1); VisionBlocked.blockedBy(vb, aBlocker); Actor.xPos(a1, x1); Actor.xPos(a2, x2); Actor.xPos(aBlocker, xBlocker); - check( x1 > xBlocker && x2 > xBlocker); + //check(slope of a1-to-BlockerBottom > slope of a1-to-a2) + //TODO implement ABSOLUTE VALUE or MULTI-CHECK + check((x1-xBlocker)*(x1-xBlocker) > (x1-x2)*(x1-x2)); } +//TODO same as above for Y??? +//TODO same as above for Y ???? /////*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*-_-*////// //SeparationDistance diff --git a/Domains/crossingScenario/src/crossingScenario/run/DrawScenario.java b/Domains/crossingScenario/src/crossingScenario/run/DrawScenario.java index 0ccfc4a1..7f90beb2 100644 --- a/Domains/crossingScenario/src/crossingScenario/run/DrawScenario.java +++ b/Domains/crossingScenario/src/crossingScenario/run/DrawScenario.java @@ -18,18 +18,20 @@ import org.eclipse.emf.ecore.resource.impl.ResourceSetImpl; import org.eclipse.emf.ecore.xmi.impl.XMIResourceFactoryImpl; import crossingScenario.Actor; +import crossingScenario.CollisionExists; import crossingScenario.CrossingScenario; import crossingScenario.CrossingScenarioPackage; import crossingScenario.Lane; import crossingScenario.Lane_Horizontal; import crossingScenario.Lane_Vertical; +import crossingScenario.Relation; import crossingScenario.VisionBlocked; public class DrawScenario { public static final int SIZE = 1000; public static void main(String[] args) throws IOException { - drawScenario("outputs/models/1.xmi"); + drawScenario("outputs/models/4.xmi"); } public static File drawScenario(String pathToXmi) throws IOException { @@ -100,68 +102,36 @@ public class DrawScenario { g.setPaint(Color.GREEN); g.drawRect(left, bot, width, length); + } + + g.setPaint(Color.BLUE); + for (Relation ce : cs.getRelations().stream(). + filter(r -> r instanceof CollisionExists).collect(Collectors.toList())) { + Actor a1 = ce.getSource(); + Actor a2 = ce.getTarget(); + Double t = ((CollisionExists) ce).getCollisionTime(); + + for (Actor a : new Actor[] {a1, a2}) { + //Draw final pos + g.setStroke(new BasicStroke(3)); + int left = (int) (((a.getXPos() + (t * a.getXSpeed()))-a.getWidth()/2) * multiplier); + int bot = (int) (((a.getYPos() + (t * a.getYSpeed()))-a.getLength()/2) * multiplier); + int width = (int) (a.getWidth() * multiplier); + int length = (int) (a.getLength() * multiplier); + + - //Draw Speed? + //Draw line + g.drawRect(left, bot, width, length); + g.setStroke(new BasicStroke(1)); + int x1 = (int) (a.getXPos() * multiplier); + int y1 = (int) (a.getYPos() * multiplier); + int x2 = (int) ((a.getXPos() + (t * a.getXSpeed())) * multiplier); + int y2 = (int) ((a.getYPos() + (t * a.getYSpeed())) * multiplier); + g.drawLine(x1, y1, x2, y2); - //Draw location at collision? + } } - -// int numVars = vals.size(); -// final int numActors = (numVars - 1) / 4; -// -// double time = 0; -// if (!is3) -// time = vals.get(numVars - 1); -// else -// time = vals.get(numVars / 2); -// -// for (int i = 0; i < numActors; i++) { -// -// int x = 0, y = 0, vx = 0, vy = 0; -// if (!is3) { -// int first = i * 4; -// x = (int) (vals.get(first) * 100); -// y = (int) (vals.get(first + 1) * 100); -// vx = (int) (vals.get(first + 2) * 100); -// vy = (int) (vals.get(first + 3) * 100); -// } else { -// int first = i * 2; -// x = (int) (vals.get(first) * 100); -// y = (int) (vals.get(first + 1) * 100); -// vx = (int) (vals.get(first + 1 + numVars / 2) * 100); -// vy = (int) (vals.get(first + 2 + numVars / 2) * 100); -// } -// g.setStroke(new BasicStroke(10)); -// g.setPaint(Color.BLACK); -// g.drawOval(x - 2, y - 2, 4, 4); -// boolean isEGO = (!is3 && i == 0) || (is3 && i == numActors - 2); -// boolean isPED = (i == numActors - 1); -// if (isEGO) { -// // EGO -// g.drawRect(x - 50, y - 150, 100, 300); -// g.setPaint(Color.PINK); -// if (acceleration) -// g.drawRect((int) (x + (vx * time * time) - 50), (int) (y + (vy * time * time) - 150), 100, 300); -// else -// g.drawRect((int) (x + (vx * time) - 50), (int) (y + (vy * time) - 150), 100, 300); -// } else if (isPED) { -// // PED -// g.drawRect(x - 50, y - 50, 100, 100); -// g.setPaint(Color.PINK); -// if (acceleration) -// g.drawRect((int) (x + (vx * time * time) - 50), (int) (y + (vy * time * time) - 50), 100, 100); -// else -// g.drawRect((int) (x + (vx * time) - 50), (int) (y + (vy * time) - 50), 100, 100); -// } else { -// // CAR -// g.drawRect(x - 50, y - 150, 100, 300); -// } -// -// g.setStroke(new BasicStroke(4)); -// g.setPaint(Color.GREEN); -// g.drawLine(x, y, x + vx, y + vy); -// -// } g.dispose(); File f = new File("outputs/drawnModel.png"); @@ -174,42 +144,5 @@ public class DrawScenario { // Desktop.getDesktop().open(f); System.out.println("finished!"); return f; - -// CrossingScenario cs = ((CrossingScenario) res.getContents().get(0)); -// -// for (Actor o : cs.getActors()) { -// String nodeName = "A(" + rndbl(o.getXPos(), 1) + "," + rndbl(o.getYPos(), 1) + ")"; -// printer.println(o.hashCode() + " " + nodeName); -// } -// -// for (Lane o : cs.getLanes()) { -// String prefix = ""; -// if (cs.getHorizontal_head().equals(o) || cs.getVertical_head().equals(o)) { -// prefix = "HEAD"; -// } -// String nodeName = prefix + "L(" + rndbl(o.getReferenceCoord(), 3) + ")" -// + o.eClass().getName().substring(5, 9); -// printer.println(o.hashCode() + " " + nodeName); -// } -// printer.println("#"); -// for (Lane o : cs.getLanes()) { -// if (o.getPrevLane() != null) { -// int curName = o.hashCode(); -// int curPrev = o.getPrevLane().hashCode(); -// double edgeLabel = rndbl(o.getPrevLane().getNumWidth(), 1); -// printer.println(curName + " " + curPrev + " " + edgeLabel); -// } -// } -// -// for (Actor o : cs.getActors()) { -// int actName = o.hashCode(); -// int lanName = o.getPlacedOn().hashCode(); -// printer.println(actName + " " + lanName); -// } -// -// printer.flush(); -// printer.close(); -// System.out.println("TGF CREATED"); - } } diff --git a/Framework/hu.bme.mit.inf.dslreasoner.viatra2logic/src/hu/bme/mit/inf/dslreasoner/viatra2logic/NumericDrealProblemSolver.java b/Framework/hu.bme.mit.inf.dslreasoner.viatra2logic/src/hu/bme/mit/inf/dslreasoner/viatra2logic/NumericDrealProblemSolver.java index b8ab8e95..e3bb2cbe 100644 --- a/Framework/hu.bme.mit.inf.dslreasoner.viatra2logic/src/hu/bme/mit/inf/dslreasoner/viatra2logic/NumericDrealProblemSolver.java +++ b/Framework/hu.bme.mit.inf.dslreasoner.viatra2logic/src/hu/bme/mit/inf/dslreasoner/viatra2logic/NumericDrealProblemSolver.java @@ -437,8 +437,8 @@ public class NumericDrealProblemSolver extends NumericProblemSolver{ //DEBUG - Print things System.out.println("Getting Solution!"); - printOutput(numProbContent); - printOutput(outputs.get(0)); +// printOutput(numProbContent); +// printOutput(outputs.get(0)); // System.out.println(result); //END DEBUG -- cgit v1.2.3