Build graph of satellites

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

@@ -7,20 +7,89 @@ import java.io.File;
 
 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()
-    var seed : Double = contents.splitToSequence('\n').first().splitToSequence(':').last().toDouble()
+    val seed : Double = contents.splitToSequence('\n').first().splitToSequence(':').last().toDouble()
     val satellites : Set<Satellite> =  contents.splitToSequence('\n')
             .filter { it.startsWith("SAT") }
             .map {
@@ -28,7 +97,7 @@ fun parseFile(file : java.io.File) : Problem {
                 Satellite(id = parts[0], lat = parts[1].toDouble(),
                     lon = parts[2].toDouble(), height = parts[3].toDouble())
             }.toSet()
-    var route : Route = contents.splitToSequence('\n')
+    val route : Route = contents.splitToSequence('\n')
             .filter { it.startsWith("ROUTE") }
             .map {
                 val parts = it.split(',')
@@ -38,8 +107,78 @@ fun parseFile(file : java.io.File) : Problem {
     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)
+}
+
+
 fun main(args : Array<String>) {
     val file = File("src/main/resources/satellites.txt")
     val problem = parseFile(file)
     println(problem)
+
+    val graph = buildGraph(problem)
+    println(graph)
+
+    val startNode = graph.nodes["Start"]
+    if (startNode != null) {
+        startNode.neighbors.forEach { n ->
+            val neighbor = graph.nodes[n]
+            if (neighbor != null) {
+                println("${startNode.info.point} vs ${neighbor.info.point}")
+            }
+        }
+    }
+
 }