// Skrevet av Dag Langmyhr (dag@ifi.uio.no) // til NM i programmering 2000. import java.io.*; import java.text.*; import java.util.*; class Beacon { static int nBeacons = 0; Point position; String id; Beacon(String new_id, Point new_p) { id = new_id; position = new_p; } static Beacon find(Beacon b[], String idx) { int i; for (i = 1; i <= nBeacons; ++i) if (b[i].id.equals(idx)) return b[i]; return null; } static void debug(String s) { System.out.println("Debug> " + s); } private static final NumberFormat fmt = DecimalFormat.getInstance(Locale.UK); static String num(double x) { fmt.setMaximumFractionDigits(2); fmt.setMinimumFractionDigits(2); return fmt.format(x); } public static void main(String arg[]) { Beacon beacons[]; PrintWriter outData; StreamTokenizer inData; int nScenarios, i; try { inData = new StreamTokenizer(new FileReader(new File("infil.F"))); outData = new PrintWriter(new FileWriter("utfil.F")); inData.nextToken(); nBeacons = (int)inData.nval; beacons = new Beacon[nBeacons+1]; for (i = 1; i <= nBeacons; ++i) { String s; double x, y; inData.nextToken(); s = inData.sval; inData.nextToken(); x = inData.nval; inData.nextToken(); y = inData.nval; beacons[i] = new Beacon(s, new Point(x,y)); } inData.nextToken(); nScenarios = (int)inData.nval; for (i = 1; i <= nScenarios; ++i) { Beacon b1, b2; Line beam1, beam2; Point beamCross; double course, speed, time1, time2, angle1, angle2; // Read the data: inData.nextToken(); course = 90-inData.nval; inData.nextToken(); speed = inData.nval; inData.nextToken(); time1 = inData.nval; inData.nextToken(); b1 = Beacon.find(beacons, inData.sval); inData.nextToken(); angle1 = course - inData.nval; inData.nextToken(); time2 = inData.nval; inData.nextToken(); b2 = Beacon.find(beacons, inData.sval); inData.nextToken(); angle2 = course - inData.nval; // Find the solution: outData.print("Scenario " + i + ": "); debug("Scenario " + i + ":"); beam1 = b1.position.extend(angle1); beam2 = b2.position.extend(angle2); beamCross = beam1.crosses(beam2); if (beamCross == null) { outData.print("Position cannot be determined"); } else { Point test1a = beamCross.translate(Vector.unitVector(angle1)); Point test2a = beamCross.translate(Vector.unitVector(angle1+180)); Point test1b = test1a.extend(course).crosses(beam2); Point test2b = test2a.extend(course).crosses(beam2); double dist = speed*(time2-time1); double corrDist = dist/test1a.distance(test1b); Point test1bx, test2bx; double test1L; boolean test1OK, test2OK; debug("Correct distance is " + num(corrDist)); test1a = beamCross.translate(Vector.angVector(angle1,corrDist)); test2a = beamCross.translate(Vector.angVector(angle1+180,corrDist)); test1b = test1a.extend(course).crosses(beam2); test2b = test2a.extend(course).crosses(beam2); test1bx = test1a.translate(Vector.angVector(course,dist)); test2bx = test2a.translate(Vector.angVector(course,dist)); test1OK = test1b.closeTo(test1bx); test2OK = test2b.closeTo(test2bx); if (test1OK && ! test2OK) { outData.print("Position is " + test1b.deb()); Beacon.debug("Found " + test1b.deb()); } else if (test2OK && ! test1OK) { outData.print("Position is " + test2b.deb()); Beacon.debug("Found " + test2b.deb()); } else { outData.print("Position error"); Beacon.debug("Algorithm error!"); } } outData.println(""); } outData.close(); } catch (Exception e) { System.err.println("Exception occurred: " + e); } } } class Line { // Represents the line ax + by + c = 0. double a, b, c; Line(double new_a, double new_b, double new_c) { a = new_a; b = new_b; c = new_c; } Point crosses(Line l2) { Point new_p = null; double denom; denom = a*l2.b - l2.a*b; if (Math.abs(denom) > 1e-6) new_p = new Point((b*l2.c-l2.b*c)/denom, (l2.a*c-a*l2.c)/denom); Beacon.debug(deb() + " crosses " + l2.deb() + " ==> " + (new_p==null ? "null" : new_p.deb())); return new_p; } String deb() { return Beacon.num(a) + "x" + (b<0 ? "" : "+") + Beacon.num(b) + "y" + (c<0 ? "" : "+") + Beacon.num(c); } } class Point { double x, y; static final Point origo = new Point(0,0); Point(double new_x, double new_y) { x = new_x; y = new_y; } String deb() { return "(" + Beacon.num(x) + "," + Beacon.num(y) + ")"; } boolean closeTo(Point p2) { boolean cl = distance(p2) < 0.001; Beacon.debug(deb() + " closeTo " + p2.deb() + " ==> " + (cl ? "true" : "false")); return cl; } double distance(Point p2) { double d = new Vector(this,p2).length(); Beacon.debug(deb() + " distance " + p2.deb() + " ==> " + Beacon.num(d)); return d; } Line extend(double dir) { Line new_l; double a, b, c; a = Vector.sinD(dir); b = -Vector.cosD(dir); c = -(a*x + b*y); new_l = new Line(a, b, c); Beacon.debug(deb() + " extend " + Beacon.num(dir) + " ==> " + new_l.deb()); return new_l; } Point translate(Vector v) { Point new_p = new Point(x+v.p2.x-v.p1.x, y+v.p2.y-v.p1.y); Beacon.debug(deb() + " translate " + v.deb() + " ==> " + new_p.deb()); return new_p; } } class Vector { Point p1, p2; Vector(Point new_p1, Point new_p2) { p1 = new_p1; p2 = new_p2; } static double cosD(double v) { return Math.cos(v*Math.PI/180); } static double sinD(double v) { return Math.sin(v*Math.PI/180); } static Vector angVector(double dir, double len) { return new Vector(Point.origo, new Point(len*cosD(dir),len*sinD(dir))); } static Vector unitVector(double dir) { return angVector(dir,1); } String deb() { return "[" + p1.deb() + "-->" + p2.deb() + "]"; } double length() { double d = Math.sqrt(Math.pow(p2.x-p1.x, 2) + Math.pow(p2.y-p1.y, 2)); Beacon.debug(deb() + " Length ==> " + Beacon.num(d)); return d; } }