SphericalWCV.hs

{-# OPTIONS_GHC -fno-warn-name-shadowing -fno-warn-unused-do-bind #-}

-- | A version of WCV with input in spherical coordinates.
module SphericalWCV where

import Prelude ()

import Copilot.Language
import Copilot.Language.Reify
import Copilot.Theorem
import Copilot.Theorem.Prover.SMT (def, debug, onlySat, onlyValidity, z3)

import qualified Copilot.Language.Operators.Propositional as P

type Vect2 = (Stream Double, Stream Double)
type Vect3 = (Stream Double, Stream Double, Stream Double)

-- | Ownship: lat, lon (rad); ground speed, track, vert speed (m/s); altitude (m).
latO, lonO, gsO, trkO, vsO, altO :: Stream Double
latO = extern "latO" Nothing
lonO = extern "lonO" Nothing
gsO  = extern "gsO" Nothing
trkO = extern "trkO" Nothing
vsO  = extern "vsO" Nothing
altO = extern "altO" Nothing

-- | Intruder: lat, lon (rad); ground speed, track, vert speed (m/s); altitude (m).
latI, lonI, gsI, trkI, vsI, altI :: Stream Double
latI = extern "latI" Nothing
lonI = extern "lonI" Nothing
gsI  = extern "gsI" Nothing
trkI = extern "trkI" Nothing
vsI  = extern "vsI" Nothing
altI = extern "altI" Nothing

dthr, tthr, zthr, tcoathr :: Stream Double
dthr    = extern "dthr" Nothing
tthr    = extern "tthr" Nothing
zthr    = extern "zthr" Nothing
tcoathr = extern "tcoathr" Nothing

spherical2xyz :: Stream Double -> Stream Double -> Vect3
spherical2xyz lat lon = (x, y, z)
      where
        r     = 6371000 -- Radius of the earth in meters
        theta = pi / 2 - lat
        phi   = pi - lon
        x     = r * sin theta * cos phi
        y     = r * sin theta * sin phi
        z     = r * cos theta

dot3 :: Vect3 -> Vect3 -> Stream Double
dot3 (x1, y1, z1) (x2, y2, z2) = x1 * x2 + y1 * y2 + z1 * z2

norm3 :: Vect3 -> Stream Double
norm3 v = sqrt (v `dot3` v)

vect3_orthog_toy :: Vect3 -> Vect3
vect3_orthog_toy (x, y, _) = (mux (x /= 0 || y /= 0) y 1, mux (x /= 0 || y /= 0) (-x) 0, constD 0)

cross3 :: Vect3 -> Vect3 -> Vect3
cross3 (x1, y1, z1) (x2, y2, z2) = (y1 * z2 - z1 * y2, z1 * x2 - x1 * z2, x1 * y2 - y1 * x2)

vect3_orthog_toz :: Vect3 -> Vect3
vect3_orthog_toz v = v `cross3` vect3_orthog_toy v

unit :: Vect3 -> Vect3
unit (x, y, z) = (x / n, y / n, z / n)
      where
        n = norm3 (x, y, z)

vect3_orthonorm_toy :: Vect3 -> Vect3
vect3_orthonorm_toy = unit . vect3_orthog_toy

vect3_orthonorm_toz :: Vect3 -> Vect3
vect3_orthonorm_toz = unit . vect3_orthog_toz

sphere_to_2D_plane :: Vect3 -> Vect3 -> Vect2
sphere_to_2D_plane nzv w = (ymult `dot3` w, zmult `dot3` w)
      where
        ymult = vect3_orthonorm_toy nzv
        zmult = vect3_orthonorm_toz nzv

pO :: Vect3
pO = spherical2xyz latO lonO

pI :: Vect3
pI = spherical2xyz latI lonI

sOx, sOy, sOz :: Stream Double
(sOx, sOy, sOz) = (0, 0, altO)

sIx, sIy, sIz :: Stream Double
(sIx, sIy, sIz) = (sI2x, sI2y, altI)
      where
        (sI2x, sI2y) = sphere_to_2D_plane pO pI

vOx, vOy, vOz :: Stream Double
(vOx, vOy, vOz) = (gsO * sin trkO, gsO * cos trkO, vsO)

vIx, vIy, vIz :: Stream Double
(vIx, vIy, vIz) = (gsI * sin trkI, gsI * cos trkI, vsI)

-- latI velocity/position --

vx, vy, vz :: Stream Double
(vx, vy, vz) = (vOx - vIx, vOy - vIy, vOz - vIz)

v :: Vect2
v = (vx, vy)

sx, sy, sz :: Stream Double
(sx, sy, sz) = (sOx - sIx, sOy - sIy, sOz - sIz)

s :: Vect2
s = (sx, sy)

-- Vector stuff --

(|*|) :: Vect2 -> Vect2 -> Stream Double
(|*|) (x1, y1) (x2, y2) = (x1 * x2) + (y1 * y2)

sq :: Vect2 -> Stream Double
sq x = x |*| x

norm :: Vect2 -> Stream Double
norm = sqrt . sq

det :: Vect2 -> Vect2 -> Stream Double
det (x1, y1) (x2, y2) = (x1 * y2) - (x2 * y1)

(~=) :: Stream Double -> Stream Double -> Stream Bool
a ~= b = abs (a - b) < 0.001

neg :: Vect2 -> Vect2
neg (x, y) = (negate x, negate y)

-- Time variables --

tau :: Vect2 -> Vect2 -> Stream Double
tau s v = mux (s |*| v < 0) ((-(sq s)) / (s |*| v)) (-1)

tcpa :: Vect2 -> Vect2 -> Stream Double
tcpa s v@(vx, vy) = mux (vx ~= 0 && vy ~= 0) 0 (-(s |*| v) / sq v)

taumod :: Vect2 -> Vect2 -> Stream Double
taumod s v = mux (s |*| v < 0) ((dthr * dthr - sq s)/(s |*| v)) (-1)

tep :: Vect2 -> Vect2 -> Stream Double
tep s v = mux ((s |*| v < 0) && (delta s v dthr >= 0))
  (theta s v dthr (-1))
  (-1)

delta :: Vect2 -> Vect2 -> Stream Double -> Stream Double
delta s v d = d * d * sq v - (det s v * det s v)
-- Here the formula says : (s . orth v)^2 which is the same as det(s,v)^2

theta :: Vect2 -> Vect2 -> Stream Double -> Stream Double -> Stream Double
theta s v d e = (-(s |*| v) + e * sqrt (delta s v d)) / sq v

-- Some tools for times --

tcoa :: Stream Double -> Stream Double -> Stream Double
tcoa sz vz = mux ((sz * vz) < 0) ((-sz) / vz) (-1)

dcpa :: Vect2 -> Vect2 -> Stream Double
dcpa s@(sx, sy) v@(vx, vy) = norm (sx + tcpa s v * vx, sy + tcpa s v * vy)

-- Well clear Violation --

wcv :: (Vect2 -> Vect2 -> Stream Double) ->
       Vect2 -> Stream Double ->
       Vect2 -> Stream Double ->
       Stream Bool
wcv tvar s sz v vz = horizontalWCV tvar s v && verticalWCV sz vz

verticalWCV :: Stream Double -> Stream Double -> Stream Bool
verticalWCV sz vz = (abs sz <= zthr) || (0 <= tcoa sz vz && tcoa sz vz <= tcoathr)

horizontalWCV :: (Vect2 -> Vect2 -> Stream Double) -> Vect2 -> Vect2 -> Stream Bool
horizontalWCV tvar s v = (norm s <= dthr) || ((dcpa s v <= dthr) && (0 <= tvar s v) && (tvar s v <= tthr))

-- Theorems --

-- Horizontal symmetry --
horizSymmetry = do
  theorem "1a" (forAll $ tau s v    ~= tau (neg s) (neg v))     arith
  theorem "1b" (forAll $ tcpa s v   ~= tcpa (neg s) (neg v))    arith
  theorem "1c" (forAll $ taumod s v ~= taumod (neg s) (neg v))  arith
  theorem "1d" (forAll $ tep s v    ~= tep (neg s) (neg v))     arith

-- Horizontal ordering --
horizOrdering = do
  theorem "2a" (forAll $ ((s |*| v) < 0 && norm s > dthr && dcpa s v <= dthr)
    ==> (tep s v <= taumod s v))
    arith
  theorem "2b" (forAll $ ((s |*| v) < 0 && norm s > dthr && dcpa s v <= dthr)
    ==> (taumod s v <= tcpa s v))
    arith
  theorem "2c" (forAll $ ((s |*| v) < 0 && norm s > dthr && dcpa s v <= dthr)
    ==> (tcpa s v <= tau s v))
    arith

-- Symmetry --
symmetry = do
  theorem "3a" (forAll $ wcv tau s sz v vz    == wcv tau (neg s) (-sz) (neg v) (-vz))
    arith
  theorem "3b" (forAll $ wcv tcpa s sz v vz   == wcv tcpa (neg s) (-sz) (neg v) (-vz))
    arith
  theorem "3c" (forAll $ wcv taumod s sz v vz == wcv taumod (neg s) (-sz) (neg v) (-vz))
    arith
  theorem "3d" (forAll $ wcv tep s sz v vz    == wcv tep (neg s) (-sz) (neg v) (-vz))
    arith

-- Inclusion --
inclusion = do
  theorem "4i"   (forAll $ wcv tau s sz v vz    ==> wcv tcpa s sz v vz)
    arith
  theorem "4ii"  (forAll $ wcv tcpa s sz v vz   ==> wcv taumod s sz v vz )
    arith
  theorem "4iii" (forAll $ wcv taumod s sz v vz ==> wcv tep s sz v vz)
    arith

-- Local convexity --

t1, t2, t3 :: Stream Double
t1 = extern "t1" Nothing
t2 = extern "t2" Nothing
t3 = extern "t3" Nothing

locallyConvex :: (Vect2 -> Vect2 -> Stream Double) -> Stream Bool
locallyConvex tvar = (0 <= t1 && t1 <= t2 && t2 <= t3)
   ==> not ( wcv tvar (sx + t1*vx, sy + t1*vy) (sz + t1*vz) v vz
      &&  not (wcv tvar (sx + t2*vx, sy + t2*vy) (sz + t2*vz) v vz)
      &&  wcv tvar (sx + t3*vx, sy + t3*vy) (sz + t3*vz) v vz)

localConvexity = do
  theorem "5a" (forAll $ locallyConvex tcpa)        arith
  theorem "5b" (forAll $ locallyConvex taumod)      arith
  theorem "5c" (forAll $ locallyConvex tep)         arith
  theorem "6"  (P.not (forAll $ locallyConvex tau)) arithSat

arith :: Proof Universal
arith    = onlyValidity def { debug = False } z3

arithSat :: Proof Existential
arithSat = onlySat      def { debug = False } z3