Fixed problems with calculation, splitted files

src/main/kotlin/name/lamperi/orbital/geometry.kt

@@ -0,0 +1,138 @@
+package name.lamperi.orbital
+
+
+// Latitude = South - North (-90...90)
+// Longitude = East West (-180...180)
+
+val EARTH_RADIUS = 6371.0; // KM
+
+
+data class Point3D (
+        val x : Double, val y : Double, val z : Double)
+
+data class Vec3 (
+        val x : Double, val y : Double, val z : Double
+) {
+    fun len() : Double {
+        return Math.sqrt(x * x + y * y + z * z)
+    }
+
+    fun unit() : Vec3 {
+        val l = len()
+        return Vec3(x/l, y/l, z/l)
+    }
+
+    fun dot(o: Vec3) : Double {
+        return x * o.x + y * o.y + z * o.z
+    }
+
+    operator fun plus(o : Vec3) : Vec3 {
+        return Vec3(x + o.x, y + o.y, z + o.z)
+    }
+
+    operator fun minus(o : Vec3) : Vec3 {
+        return Vec3(x - o.x, y - o.y, z - o.z)
+    }
+
+    operator fun times(d : Double) : Vec3 {
+        return Vec3(x * d, y * d, z * d)
+    }
+}
+
+fun createVec(start : Point3D, end : Point3D) : Vec3 {
+    return Vec3(end.x - start.x, end.y - start.y, end.z - start.z)
+}
+
+data class SphericalCoordinate (
+        val radius : Double, val theta : Double, val phi : Double)
+
+fun toVec(point : Point3D) : Vec3 {
+    return Vec3(point.x, point.y, point.z)
+}
+
+// Coordinate system
+// x = + when lon = 0°, - when lon = 180°
+// y = + when lon = 90°, - when lon = -90°
+// z = + when lat = 90°, - when lat = -90°
+
+fun rad(degrees : Double) = degrees * Math.PI / 180.0
+
+// let theta = rad(lon) in (-pi, pi)
+// let phi = rad(-lat + 90°) in (0, pi)
+fun toSpherical(lat : Double, lon : Double, radius : Double = EARTH_RADIUS)
+        =  SphericalCoordinate(radius = radius, theta = rad(lat), phi = rad(lon))
+
+// Cartesian coordinates can be calculated using
+// x = r * sin(theta) * cos(phi)
+// y = r * sin(theta) * sin(phi)
+// z = r * cos(theta)
+fun toCartesian(c : SphericalCoordinate)
+        = Point3D(
+        x = c.radius * Math.cos(c.theta) * Math.cos(c.phi),
+        y = c.radius * Math.cos(c.theta) * Math.sin(c.phi),
+        z = c.radius * Math.sin(c.theta))
+
+fun toCartesian(lat : Double, lon : Double)
+        = toCartesian(toSpherical(lat, lon))
+
+fun toCartesian(lat : Double, lon : Double, height : Double) : Point3D
+        = toCartesian(toSpherical(lat, lon, EARTH_RADIUS + height))
+
+
+// https://en.wikipedia.org/wiki/Line–sphere_intersection
+// Solutions are of quadratic form
+// l = unit vector from point 0 to point 1
+// o = point 0
+// c = [0,0,0]
+// r = EARTH_RADIUS
+// Calculate (l · o) +- Math.sqrt((l · o)^2 - (o · o)  + r^2)
+// If it is non-zero, then the route cannot be made
+fun intersectsEarth(p0 : Point3D, p1 : Point3D) : Boolean {
+    val dist = createVec(p0, p1).len()
+    val l = createVec(p0, p1).unit()
+    val o = toVec(p0)
+
+    val l_dot_o = l.dot(o)
+    val o_dot_o = o.dot(o)
+    val dis = l_dot_o * l_dot_o - o_dot_o + EARTH_RADIUS * EARTH_RADIUS
+    val d1 = - l_dot_o + Math.sqrt(dis)
+    val d2 = - l_dot_o - Math.sqrt(dis)
+
+    val sol1 = o + l * d1
+    val sol2 = o + l * d2
+
+    val sigma = 0
+    val sol1intersects = d1 > sigma && dist > d1 - sigma
+    val sol2intersects = d2 > sigma && dist > d2 - sigma
+
+    //println("$p0 $p1 $dist => $d1 $sol1intersects $sol1 and $d2 $sol2intersects $sol2")
+
+    return sol1intersects || sol2intersects
+}
+
+// SAT0 Point3D(x=3372.4860600513102, y=5850.5619038216755, z=-1254.2621619796448)
+// SAT5 Point3D(x=-60.473722764130805, y=408.66870956674063, z=-6972.741992625009)
+// Start Point3D(x=-70.20728008240212, y=132.20869520700555, z=-6369.241147792654)
+// End Point3D(x=215.17891267006544, y=-3374.1448308119298, z=5399.859784869165))
+
+fun main(args : Array<String>) {
+    val SAT0 = Point3D(x=3372.4860600513102, y=5850.5619038216755, z=-1254.2621619796448)
+    val SAT5 = Point3D(x=-60.473722764130805, y=408.66870956674063, z=-6972.741992625009)
+    val Start = Point3D(x=-70.20728008240212, y=132.20869520700555, z=-6369.241147792654)
+    val End = Point3D(x=215.17891267006544, y=-3374.1448308119298, z=5399.859784869165)
+
+    println(intersectsEarth(Start, SAT5))
+    println(intersectsEarth(SAT5, Start))
+
+    println(intersectsEarth(End, SAT5))
+    println(intersectsEarth(SAT5, End))
+
+    println(intersectsEarth(Start, SAT0))
+    println(intersectsEarth(SAT0, Start))
+
+    println(intersectsEarth(End, SAT0))
+    println(intersectsEarth(SAT0, End))
+
+    println(intersectsEarth(Start, End))
+    println(intersectsEarth(End, Start))
+}

src/main/kotlin/name/lamperi/orbital/graph.kt

@@ -0,0 +1,90 @@
+package name.lamperi.orbital
+
+import java.util.LinkedList
+
+data class Graph (
+        val nodes : Map<String, Node>)
+
+data class Node (
+        val info : NamedPoint3D, val neighbors : Set<String>)
+
+data class NamedPoint3D (
+        val name : String, val point : Point3D)
+
+fun named(name : String, point : Point3D) = NamedPoint3D(name = name, point = point)
+
+fun canRoute(p0 : Point3D, p1 : Point3D) : Boolean  = !intersectsEarth(p0, p1)
+
+fun <E> concat(set : Collection<E>, vararg more : E) : Set<E> {
+    val mutable = set.toMutableSet()
+    for (e in more) mutable.add(e)
+    return mutable.toSet()
+}
+
+fun buildGraph(problem : Problem) : Graph {
+    val start = named("Start", toCartesian(problem.route.startLat, problem.route.startLon))
+    val end = named("End", toCartesian(problem.route.endLat, problem.route.endLon))
+
+    val satellites = problem.satellites.map { sat ->
+        named(sat.id, toCartesian(sat.lat, sat.lon, sat.height))
+    }.toList()
+
+    val startNode = Node(info = start, neighbors = satellites.filter {
+        canRoute(start.point, it.point)
+    }.map { it.name }.toSet())
+
+    val allPoints = concat(satellites, start, end)
+
+    val otherNodes = satellites.map { sat ->
+        Node(info = sat, neighbors = allPoints.filter { other ->
+            !sat.name.equals(other.name) && canRoute(sat.point, other.point)
+        }.map { it.name }.toSet())
+    }
+
+    val endNode = Node(info = end, neighbors = satellites.filter {
+        canRoute(it.point, end.point)
+    }.map { it.name }.toSet())
+
+    val nodeMap = concat(otherNodes, startNode, endNode).associateBy({ it.info.name }, { it })
+    return Graph(nodeMap)
+}
+
+data class Routing (val node : String, var parent : String?, var distance : Int = Integer.MAX_VALUE)
+
+fun solveGraph(start : String, end : String, graph : Graph) : List<String> {
+    val routes : Map<String,Routing> = graph.nodes.map { Routing(node = it.key, parent = null) }.associateBy { it.node }
+    val queue = LinkedList<String>()
+    val route = routes[start]!!
+    route.distance = 0
+    queue.add(start)
+    while (!queue.isEmpty()) {
+        val current = queue.remove()
+        val currentRoute = routes[current]!!
+        val route = routes[current]!!
+        val node = graph.nodes[current]!!
+        for (neighbor in node.neighbors) {
+            val neighborRoute = routes[neighbor]!!
+            if (neighborRoute.distance == Integer.MAX_VALUE) {
+                neighborRoute.distance = currentRoute.distance + 1
+                neighborRoute.parent = current
+                queue.add(neighbor)
+            }
+        }
+    }
+    val endRoute = routes[end]
+    val nodes = LinkedList<String>()
+    if (endRoute != null) {
+        var current = endRoute
+        while (current != null) {
+            nodes.addFirst(current.node)
+            val parent = current.parent
+            if (parent != null) {
+                current = routes[parent]
+            } else {
+                current = null
+            }
+        }
+
+    }
+    return nodes
+}

src/main/kotlin/name/lamperi/orbital/main.kt

@@ -1,219 +1,49 @@
 package name.lamperi.orbital
 
 import java.io.File
-import java.util.*
 
-
-// Latitude = South - North (-90...90)
-// Longitude = East West (-180...180)
-
-val EARTH_RADIUS = 6371.0; // KM
-
-
-data class Problem (
-        val seed : Double, val satellites : Set<Satellite>, val route : Route)
-
-data class Satellite (
-        val id : String, val lat : Double, val lon : Double, val height : Double)
-
-data class Route (
-        val startLat : Double, val startLon : Double, val endLat : Double, val endLon : Double)
-
-data class Point3D (
-        val x : Double, val y : Double, val z : Double)
-
-data class Vec3 (
-        val x : Double, val y : Double, val z : Double
-) {
-    fun len() : Double {
-        return Math.sqrt(x * x + y * y + z * z)
-    }
-
-    fun unit() : Vec3 {
-        val l = len()
-        return Vec3(x/l, y/l, z/l)
-    }
-
-    fun dot(o: Vec3) : Double {
-        return x * o.x + y * o.y + z * o.z
-    }
-}
-
-fun toVec(point : Point3D) : Vec3 {
-    return Vec3(point.x, point.y, point.z)
-}
-
-fun createVec(start : Point3D, end : Point3D) : Vec3 {
-    return Vec3(end.x - start.x, end.y - start.y, end.z - start.z)
-}
-
-data class SphericalCoordinate (
-        val radius : Double, val theta : Double, val phi : Double)
-
-data class NamedPoint3D (
-        val name : String, val point : Point3D)
-
-data class Graph (
-        val nodes : Map<String,Node>)
-
-data class Node (
-        val info : NamedPoint3D, val neighbors : Set<String>)
-
-// Coordinate system
-// x = + when lon = 0°, - when lon = 180°
-// y = + when lon = 90°, - when lon = -90°
-// z = + when lat = 90°, - when lat = -90°
-
-fun rad(degrees : Double) = degrees * Math.PI / 180.0
-
-// let theta = rad(lon) in (-pi, pi)
-// let phi = rad(-lat + 90°) in (0, pi)
-fun toSpherical(lat : Double, lon : Double, radius : Double = EARTH_RADIUS)
-        =  SphericalCoordinate(radius = radius, theta = rad(lon), phi = rad(-lat + 90.0))
-
-// Cartesian coordinates can be calculated using
-// x = r * sin(theta) * cos(phi)
-// y = r * sin(theta) * sin(phi)
-// z = r * cos(theta)
-fun toCartesian(c : SphericalCoordinate)
-        = Point3D(
-            x = c.radius * Math.sin(c.theta) * Math.cos(c.phi),
-            y = c.radius * Math.sin(c.theta) * Math.sin(c.phi),
-            z = c.radius * Math.cos(c.theta))
-
-fun toCartesian(lat : Double, lon : Double)
-        = toCartesian(toSpherical(lat, lon))
-
-fun toCartesian(lat : Double, lon : Double, height : Double) : Point3D
-        = toCartesian(toSpherical(lat, lon, EARTH_RADIUS + height))
-
-fun named(name : String, point : Point3D) = NamedPoint3D(name = name, point = point)
-
-fun parseFile(file : java.io.File) : Problem {
-    val contents : String = file.readText()
-    val seed : Double = contents.splitToSequence('\n').first().splitToSequence(':').last().toDouble()
-    val satellites : Set<Satellite> =  contents.splitToSequence('\n')
-            .filter { it.startsWith("SAT") }
-            .map {
-                val parts = it.split(',')
-                Satellite(id = parts[0], lat = parts[1].toDouble(),
-                    lon = parts[2].toDouble(), height = parts[3].toDouble())
-            }.toSet()
-    val route : Route = contents.splitToSequence('\n')
-            .filter { it.startsWith("ROUTE") }
-            .map {
-                val parts = it.split(',')
-                Route(startLat = parts[1].toDouble(), startLon = parts[2].toDouble(),
-                        endLat =  parts[3].toDouble(), endLon = parts[4].toDouble())
-            }.first()
-    return Problem(seed = seed, satellites = satellites, route = route)
-}
-
-// https://en.wikipedia.org/wiki/Line–sphere_intersection
-// Solutions are of quadratic form
-// l = unit vector from point 0 to point 1
-// o = point 0
-// c = [0,0,0]
-// r = EARTH_RADIUS
-// Calculate (l · o)^2 - (o · o)  + r^2
-// If it is non-zero, then the route cannot be made
-fun canRoute(p0 : Point3D, p1 : Point3D) : Boolean {
-    val l = createVec(p0, p1).unit()
-    val o = toVec(p0)
-
-    val l_dot_o = l.dot(o)
-    val o_dot_o = o.dot(o)
-    val result = l_dot_o * l_dot_o - o_dot_o + EARTH_RADIUS * EARTH_RADIUS
-    if (result <= 0) {
-        println("$p0 $p1 => $result")
-    }
-    return result > 0
-}
-
-fun <E> concat(set : Collection<E>, vararg more : E) : Set<E> {
-    val mutable = set.toMutableSet()
-    for (e in more) mutable.add(e)
-    return mutable.toSet()
-}
-
-fun buildGraph(problem : Problem) : Graph {
-    val start = named("Start", toCartesian(problem.route.startLat, problem.route.startLon))
-    val end = named("End", toCartesian(problem.route.endLat, problem.route.endLon))
-
-    val satellites = problem.satellites.map { sat ->
-        named(sat.id, toCartesian(sat.lat, sat.lon, sat.height))
-    }.toList()
-
-    val startNode = Node(info = start, neighbors = satellites.filter {
-        canRoute(start.point, it.point)
-    }.map { it.name }.toSet())
-
-    val allPoints = concat(satellites, start, end)
-
-    val otherNodes = satellites.map { sat ->
-        Node(info = sat, neighbors = allPoints.filter { other ->
-            !sat.name.equals(other.name) && canRoute(sat.point, other.point)
-        }.map { it.name }.toSet())
-    }
-
-    val endNode = Node(info = end, neighbors = satellites.filter {
-        canRoute(it.point, end.point)
-    }.map { it.name }.toSet())
-
-    val nodeMap = concat(otherNodes, startNode, endNode).associateBy({ it.info.name }, { it })
-    return Graph(nodeMap)
-}
-
-data class Routing (val node : String, var parent : String?, var distance : Int = Integer.MAX_VALUE)
-
-fun solveGraph(start : String, end : String, graph : Graph) {
-    val routes : Map<String,Routing> = graph.nodes.map { Routing(node = it.key, parent = null) }.associateBy { it.node }
-
-    val queue = LinkedList<String>()
-
-    val route = routes[start]!!
-    route.distance = 0
-
-    queue.add(start)
-
-    while (!queue.isEmpty()) {
-        val current = queue.remove()
-        val currentRoute = routes[current]!!
-        val route = routes[current]!!
-
-        val node = graph.nodes[current]!!
-        for (neighbor in node.neighbors) {
-            val neighborRoute = routes[neighbor]!!
-            if (neighborRoute.distance == Integer.MAX_VALUE) {
-                neighborRoute.distance = currentRoute.distance + 1
-                neighborRoute.parent = current
-                queue.add(neighbor)
-            }
-        }
-    }
-
-    val endRoute = routes[end]
-    if (endRoute != null) {
-        println("Distance was ${endRoute.distance}")
-    }
-}
-
-fun main(args : Array<String>) {
-    val file = File("src/main/resources/satellites.txt")
+fun solveFile(file : File) {
     val problem = parseFile(file)
     println(problem)
 
     val graph = buildGraph(problem)
     println(graph)
 
-    solveGraph("Start", "End", graph)
-
-    /*
-    graph.nodes.values.forEach {
-        println("[\"${it.info.name}\", ${it.info.point.x}, ${it.info.point.y}, ${it.info.point.z}]")
+    val solution = solveGraph("Start", "End", graph)
+    println("Distance was ${solution.size} and route is $solution.")
+    println(solution.map {
+        val n = graph.nodes[it]!!
+        n.info.point
+    })
+    println(solution.map {
+        val n = graph.nodes[it]!!
+        toVec(n.info.point).len()
+    })
+
+    val nodeList = graph.nodes.toList()
+    for (i in 1..solution.size-1) {
+        val a = graph.nodes[solution[i-1]]!!
+        val b = graph.nodes[solution[i]]!!
+
+        val av = toVec(a.info.point)
+        val bv = toVec(b.info.point)
+
+        val cv = bv - av
+
+        println("From ${a.info.name} to ${b.info.name}")
+        for (j in 1 .. 1000) {
+            val f = j * 0.001
+            val dist = (av + cv*f).len()
+            if (dist < EARTH_RADIUS) {
+                println(dist)
+            }
+        }
+        println("")
     }
-    */
-
-
+}
 
+fun main(args : Array<String>) {
+    //solveFile(File("src/main/resources/satellites.txt"))
+    //solveFile(File("src/main/resources/satellites2.txt"))
+    solveFile(File("src/main/resources/satellites3.txt"))
 }

src/main/kotlin/name/lamperi/orbital/problem.kt

@@ -0,0 +1,32 @@
+package name.lamperi.orbital
+
+import java.io.File
+
+data class Problem (
+        val seed : Double, val satellites : Set<Satellite>, val route : Route)
+
+data class Satellite (
+        val id : String, val lat : Double, val lon : Double, val height : Double)
+
+data class Route (
+        val startLat : Double, val startLon : Double, val endLat : Double, val endLon : Double)
+
+fun parseFile(file : File) : Problem {
+    val contents : String = file.readText()
+    val seed : Double = contents.splitToSequence('\n').first().splitToSequence(':').last().toDouble()
+    val satellites : Set<Satellite> =  contents.splitToSequence('\n')
+            .filter { it.startsWith("SAT") }
+            .map {
+                val parts = it.split(',')
+                Satellite(id = parts[0], lat = parts[1].toDouble(),
+                        lon = parts[2].toDouble(), height = parts[3].toDouble())
+            }.toSet()
+    val route : Route = contents.splitToSequence('\n')
+            .filter { it.startsWith("ROUTE") }
+            .map {
+                val parts = it.split(',')
+                Route(startLat = parts[1].toDouble(), startLon = parts[2].toDouble(),
+                        endLat = parts[3].toDouble(), endLon = parts[4].toDouble())
+            }.first()
+    return Problem(seed = seed, satellites = satellites, route = route)
+}

src/main/resources/satellites2.txt

@@ -0,0 +1,22 @@
+#SEED: 0.8338924294803292
+SAT0,29.960781584875704,100.5219106681426,497.4722017828519
+SAT1,35.63822561651885,112.43332215054551,456.47908238828165
+SAT2,-55.80162550543868,-47.223517349881064,640.9142246937936
+SAT3,32.86579634661054,-174.0355355962846,698.19931306801
+SAT4,-15.591464895709976,-33.59277356598329,304.7933580025571
+SAT5,-8.417394325772605,176.60932116923584,613.9694402366624
+SAT6,-31.337812266388397,155.2314281053836,560.0516265972703
+SAT7,49.14156578802084,74.11283751074791,526.6089406961383
+SAT8,14.574135057446483,10.185507527941525,591.2718668651619
+SAT9,-70.25509810245808,98.82497601385126,554.9574524324642
+SAT10,-12.262521826261334,-108.02116402606393,511.18350543382644
+SAT11,88.60697902520695,-6.794758753832582,465.2422688852638
+SAT12,-74.31452614616971,-122.12821653985417,499.0860910406019
+SAT13,39.45914461312282,160.23885267889574,338.17382027741985
+SAT14,65.35304485452289,-73.56474105609837,593.8472290023869
+SAT15,-58.167779487750366,-42.38551559997467,319.37893022472576
+SAT16,-9.76622565405492,14.153656079874906,622.198243045025
+SAT17,52.69128802012051,16.10254297232882,328.9426940234299
+SAT18,-29.893807409233766,-4.61504227361857,466.55133207065796
+SAT19,-13.659850262548488,-111.17258206114808,314.37740871224423
+ROUTE,-27.969810777069718,178.65364839117876,-3.648975104018305,-32.051802501382355

src/main/resources/satellites3.txt

@@ -0,0 +1,22 @@
+#SEED: 0.2870806611608714
+SAT0,-72.88739122139259,88.37189957420514,357.72708394243614
+SAT1,-61.551791451352095,-5.202493771452197,550.3065489323669
+SAT2,-40.47561565887025,-96.34566789975146,356.26443437386456
+SAT3,-29.236492259448127,131.4125359961049,440.25116142954755
+SAT4,-73.41486832164533,-72.82799390357344,407.34756551842503
+SAT5,53.87023393776735,-19.72508238842161,579.4188734782899
+SAT6,-28.111501808932736,44.41336256964237,678.9061048441274
+SAT7,41.85417111315573,-162.74550858447142,621.3134570736974
+SAT8,22.63435513250704,-45.81501413773705,423.72436693061235
+SAT9,-4.497114575798491,30.626326604533205,580.6365972545068
+SAT10,42.70955120863053,-78.4354238981028,410.31511446513844
+SAT11,39.389485293832905,114.16219122746367,424.9717160822081
+SAT12,-14.922870540031496,-129.6140791328477,528.1994774237129
+SAT13,50.28262493639784,-148.66305153149187,667.9730765836259
+SAT14,15.451225630694111,-111.3528503196552,668.3063334222632
+SAT15,-4.6531482250959755,71.88045498382726,338.9641279066445
+SAT16,41.111820036412496,96.70488116609306,647.4434963429919
+SAT17,-67.42281105897038,56.77472423719638,644.359680146807
+SAT18,-82.92341653749764,38.62374824248681,501.21586915106195
+SAT19,-3.111895215084914,-65.58327679758636,378.5792354737234
+ROUTE,5.714156454649,-111.94544651946063,-84.94681944535331,137.24150287707027