Skip to content

Instantly share code, notes, and snippets.

@mharju
Created May 17, 2016 18:54
Show Gist options
  • Save mharju/195072bc1577898827080cd9d06e32a5 to your computer and use it in GitHub Desktop.
Save mharju/195072bc1577898827080cd9d06e32a5 to your computer and use it in GitHub Desktop.
(ns satnav.core
(:require [clojure.string :as string]))
; Helpers
(def radius-earth 6371.0)
(defn degrees [rad] (* rad (/ 180.0 Math/PI)))
(defn radians [deg] (* deg (/ Math/PI 180.0)))
(defn normalize-lat [lat] (mod (+ (* 0.5 Math/PI) lat) (* 2 Math/PI)))
(defn normalize-lon [lon] (mod (+ Math/PI lon) (* 2 Math/PI)))
(defn hav [a] (/ (- 1 (Math/cos a)) 2))
(defn haversine [lat1 lon1 lat2 lon2]
(+ (hav (- lat2 lat1)) (* (Math/cos lat1) (Math/cos lat2) (hav (- lon2 lon1)))))
(defn to-xyz [lat lon radius]
(let [lat' (normalize-lat lat)
lon' (normalize-lon lon)]
[(* radius (Math/sin lat') (Math/cos lon')) (* radius (Math/sin lat') (Math/sin lon')) (* radius (Math/cos lat'))]))
(defn print-graph [graph]
(let [edges (for [[k v] graph e (doall v)] (str k " -- " e))]
(str "graph {" (clojure.string/join "\n" edges) "}")))
; Data parsing
(defn build-satellite [[id lat lon height]]
{:id id
:lat (radians (Float/parseFloat lat))
:lon (radians (Float/parseFloat lon))
:height (+ radius-earth (Float/parseFloat height))})
(defn build-route [[route from-lat from-lon to-lat to-lon]]
{:from [(radians (Float/parseFloat from-lat)) (radians (Float/parseFloat from-lon))]
:to [(radians (Float/parseFloat to-lat)) (radians (Float/parseFloat to-lon))]})
(defn get-data [resource]
(let [[route & satellites] (->> (string/split (slurp resource) #"\n")
(drop 1)
(map (fn [row]
(let [data (string/split row #",")]
(condp re-matches (first data)
#"SAT\d+" (build-satellite data)
#"ROUTE" (build-route data)))))
(reverse))]
[(into [] (reverse satellites)) route]))
; Calculations
(defn closest [satellites lat lon]
(apply min-key #(haversine (:lat %1) (:lon %1) lat lon) satellites))
(defn discriminant [[x1 y1 z1] [x2 y2 z2]]
(let [dx (- x2 x1)
dy (- y2 y1)
dz (- z2 z1)
a (+ (* dx dx) (* dy dy) (* dz dz))
b (* 2 (+ (* dx x1) (* dy y1) (* dz z1)))
c (+ (* x1 x1) (* y1 y1) (* z1 z1) (- (* radius-earth radius-earth)))]
(- (* b b) (* 4 a c))))
(defn visible? [{id1 :id height1 :height lat1 :lat lon1 :lon} {id2 :id height2 :height lat2 :lat lon2 :lon}]
(if-not (= id1 id2)
(< (discriminant (to-xyz lat1 lon1 height1) (to-xyz lat2 lon2 height2)) 0)
false))
(defn visible-satellites [satellite satellites] (filter (fn [candidate] (visible? satellite candidate)) satellites))
(defn build-graph [satellites]
(->> (map #(vector (:id %) (map :id (visible-satellites % satellites))) satellites)
(into {})))
; Simple depth-first search, the problem space does not deserve anything fancier :)
(defn iter-route [graph node last-sat edges visited]
(let [last? (when (= last-sat node) node)
first? (when (not (contains? visited (first edges)))
(iter-route graph (first edges) last-sat (get graph (first edges)) (conj visited node)))
rest? (when (not (empty? edges))
(iter-route graph node last-sat (rest edges) (conj visited node)))]
(if last? [last?]
(if first? (apply conj [node] first?)
(if rest? rest?
nil)))))
(defn find-route [satellites {[from-lat from-lon] :from [to-lat to-lon] :to}]
(let [first-sat (:id (closest satellites from-lat from-lon))
last-sat (:id (closest satellites to-lat to-lon))
graph (build-graph satellites)]
(iter-route graph first-sat last-sat (get graph first-sat) #{})))
(defn -main [& args]
(let [[satellites endpoints] (get-data "resources/test.txt")
graph (build-graph satellites)]
(spit "/tmp/satellites.dot" (print-graph graph))
(println (clojure.string/join "," (find-route satellites endpoints)))))
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment