jord 0.4.0.0 → 0.4.1.0
raw patch · 14 files changed
+1495/−1421 lines, 14 files
Files
- ChangeLog.md +7/−0
- README.md +5/−5
- app/Eval.hs +647/−822
- app/Main.hs +56/−38
- app/Show.hs +101/−0
- app/State.hs +132/−0
- jord.cabal +4/−3
- src/Data/Geo/Jord.hs +2/−1
- src/Data/Geo/Jord/Frames.hs +13/−29
- src/Data/Geo/Jord/Kinematics.hs +332/−334
- src/Data/Geo/Jord/LatLong.hs +5/−3
- src/Data/Geo/Jord/Length.hs +2/−2
- test/Data/Geo/Jord/KinematicsSpec.hs +188/−183
- test/Data/Geo/Jord/LengthSpec.hs +1/−1
ChangeLog.md view
@@ -1,3 +1,10 @@+### 0.4.1.0 + +- Fixed interceptBySpeed +- Nautical miles symbol is "nm" +- REPL: intercept for intercept, interceptBySpeed and interceptByTime +- REPL: show length and speed in user selected unit + ### 0.4.0.0 - Added ECEF, frames and delta to REPL
README.md view
@@ -79,14 +79,14 @@ roll : 30°0'0.000" (30.0) jord> d = delta 3000 2000 100 jord> Delta: - x: 3000.0m <-> 3.0km <-> 1.6198704103671706nm <-> 9842.51968503937ft - y: 2000.0m <-> 2.0km <-> 1.079913606911447nm <-> 6561.679790026246ft - z: 100.0m <-> 0.1km <-> 5.399568034557235e-2nm <-> 328.0839895013123ft + x: 3.0km + y: 2.0km + z: 0.1km jord> p0 = geo 49.66618 3.45063 0 jord> latlong: 49°39'58.248"N,3°27'2.268"E (49.66618, 3.45063) - height : 0.0m <-> 0.0km <-> 0.0nm <-> 0.0ft + height : 0.0km jord> target p0 f d wgs84 jord> latlong: 49°41'30.486"N,3°28'52.561"E (49.69180166666667, 3.4812669444444446) - height : 6.0077m <-> 6.0077e-3km <-> 3.24389848812095e-3nm <-> 19.71030183727034ft + height : 6.0077e-3km jord> ```
app/Eval.hs view
@@ -1,822 +1,647 @@-{-# LANGUAGE FlexibleInstances #-}-{-# LANGUAGE TupleSections #-}-{-# OPTIONS_GHC -fno-warn-orphans #-}---- | --- Module: Eval --- Copyright: (c) 2018 Cedric Liegeois --- License: BSD3 --- Maintainer: Cedric Liegeois <ofmooseandmen@yahoo.fr> --- Stability: experimental --- Portability: portable --- --- Types and functions for evaluating expressions in textual form. --- -module Eval- ( Value(..)- , Vault- , Result- , emptyVault- , eval- , functions- , insert- , delete- , lookup- ) where--import Control.Monad.Fail-import Data.Bifunctor-import Data.Either (rights)-import Data.Geo.Jord-import Data.List hiding (delete, insert, lookup)-import Data.Maybe-import Prelude hiding (fail, lookup)-import Text.ParserCombinators.ReadP-import Text.Read (readEither, readMaybe)---- | A value accepted and returned by 'eval'. -data Value- = Ang Angle -- ^ angle - | Bool Bool -- ^ boolean - | Cpa (Cpa (AngularPosition NVector)) -- ^ CPA - | Dlt Delta -- ^ delta - | Dur Duration -- ^ duration - | Double Double -- ^ double - | Ep EcefPosition -- ^ ECEF position - | Em Earth -- ^ earth model - | FrmB Angle- Angle- Angle -- ^ yaw, pitch and roll of Body frame - | FrmL Angle -- ^ wander azimuth of Local frame - | FrmN -- ^ North, east, down frame - | Gc GreatCircle -- ^ great circle - | Gp (AngularPosition LatLong) -- ^ latitude, longitude and height - | Intp (Intercept (AngularPosition NVector)) -- ^ Intercept - | Len Length -- ^ length - | Ned Ned -- ^ north east down - | Np (AngularPosition NVector) -- ^ n-vector and height - | Spd Speed -- ^ speed - | Trk (Track (AngularPosition NVector)) -- ^ track - | Vals [Value] -- array of values ---- | show value. -instance Show Value where- show (Ang a) = "angle: " ++ showAng a- show (Bool b) = show b- show (Cpa c) =- "closest point of approach:" ++- "\n time : " ++- show (cpaTime c) ++- "\n distance: " ++- showLen (cpaDistance c) ++- "\n pos1 : " ++- showLl (fromNVector . cpaPosition1 $ c :: LatLong) ++- "\n pos2 : " ++ showLl (fromNVector . cpaPosition2 $ c :: LatLong)- show (Dlt d) =- "Delta:" ++- "\n x: " ++- showLen (dx d) ++ "\n y: " ++ showLen (dy d) ++ "\n z: " ++ showLen (dz d)- show (Dur d) = "duration: " ++ show d- show (Double d) = show d- show (Em m) = "Earth model: " ++ show m- show (Ep p) =- "ECEF:" ++- "\n x: " ++- showLen (ex p) ++ "\n y: " ++ showLen (ey p) ++ "\n z: " ++ showLen (ez p)- show (FrmB y p r) =- "Body (vehicle) frame:" ++- "\n yaw : " ++- showAng y ++ "\n pitch: " ++ showAng p ++ "\n roll : " ++ showAng r- show (FrmL w) = "Local frame:" ++ "\n wander azimuth: " ++ showAng w- show FrmN = "North, East, Down frame"- show (Gc gc) = "great circle: " ++ show gc- show (Gp g) = "latlong: " ++ showLl ll ++ "\n height : " ++ showLen h- where- ll = pos g- h = height g- show (Intp i) =- "intercept:" ++- "\n time : " ++- show (interceptTime i) ++- "\n distance : " ++- showLen (interceptDistance i) ++- "\n pos : " ++- showLl (fromNVector . interceptPosition $ i :: LatLong) ++- "\n interceptor speed : " ++- showSpd (interceptorSpeed i) ++- "\n interceptor bearing: " ++ showAng (interceptorBearing i)- show (Len l) = "length: " ++ showLen l- show (Ned d) =- "NED:" ++- "\n north: " ++- showLen (north d) ++- "\n east : " ++ showLen (east d) ++ "\n down : " ++ showLen (down d)- show (Np nv) =- "n-vector: " ++- show x ++ ", " ++ show y ++ ", " ++ show z ++ "\n height : " ++ showLen h- where- v = vec (pos nv)- x = vx v- y = vy v- z = vz v- h = height nv- show (Trk t) =- "track:" ++- "\n position: " ++- showLl (fromNVector . trackPos $ t :: LatLong) ++- "\n height : " ++- showLen (height . trackPos $ t) ++- "\n bearing : " ++- showAng (trackBearing t) ++ "\n speed : " ++ showSpd (trackSpeed t)- show (Spd s) = "speed: " ++ showSpd s- show (Vals []) = "empty"- show (Vals vs) = "\n " ++ intercalate "\n\n " (map show vs)--showAng :: Angle -> String-showAng a = show a ++ " (" ++ show (toDecimalDegrees a) ++ ")"--showLl :: LatLong -> String-showLl ll =- show ll ++- " (" ++- show (toDecimalDegrees (latitude ll)) ++ ", " ++ show (toDecimalDegrees (longitude ll)) ++ ")"--showLen :: Length -> String-showLen l =- show (toMetres l) ++- "m <-> " ++- show (toKilometres l) ++- "km <-> " ++ show (toNauticalMiles l) ++ "nm <-> " ++ show (toFeet l) ++ "ft"--showSpd :: Speed -> String-showSpd s =- show (toKilometresPerHour s) ++- "km/h <-> " ++- show (toMetresPerSecond s) ++- "m/s <-> " ++- show (toKnots s) ++- "kt <-> " ++ show (toMilesPerHour s) ++ "mph <-> " ++ show (toFeetPerSecond s) ++ "ft/s"---- | 'Either' an error or a 'Value'. -type Result = Either String Value---- | A location for 'Value's to be shared by successive evalations. -newtype Vault =- Vault [(String, Value)]---- | An empty 'Vault'. -emptyVault :: Vault-emptyVault = Vault []--instance MonadFail (Either String) where- fail = Left---- | Evaluates @s@, an expression of the form @"(f x y ..)"@. --- --- >>> eval "finalBearing (destination (antipode 54°N,154°E) 54° 1000m) 54°N,154°E" --- 126° --- --- @f@ must be one of the supported 'functions' and each parameter @x@, @y@, .. , is either another function call --- or a 'String' parameter. Parameters are either resolved by name using the 'Resolve' --- function @r@ or if it returns 'Nothing', 'read' to an 'Angle', a 'Length' or a 'LatLong'. --- --- If the evaluation is successful, returns the resulting 'Value' ('Right') otherwise --- a description of the error ('Left'). --- --- @ --- vault = emptyVault --- angle = eval "finalBearing 54N154E 54S154W" vault -- Right Ang --- length = eval "surfaceDistance (antipode 54N154E) 54S154W" vault -- Right Len --- -- parameter resolution from vault --- a1 = eval "finalBearing 54N154E 54S154W" vault --- vault = insert "a1" vault --- a2 = eval "(finalBearing a1 54S154W)" vault --- @ --- --- All returned positions are 'LatLong' by default, to get back a n-vector the --- expression must be wrapped by 'toNVector'. --- --- @ --- dest = eval "destination 54°N,154°E 54° 1000m" -- Right Ll --- dest = eval "toNVector (destination 54°N,154°E 54° 1000m)" -- Right Np --- @ --- --- Every function call must be wrapped between parentheses, however they can be ommitted for the top level call. --- --- @ --- angle = eval "finalBearing 54N154E 54S154W" -- Right Ang --- angle = eval "(finalBearing 54N154E 54S154W)" -- Right Ang --- length = eval "distance (antipode 54N154E) 54S154W" -- Right Len --- length = eval "distance antipode 54N154E 54S154W" -- Left String --- @ --- -eval :: String -> Vault -> Result-eval s r =- case expr s of- Left err -> Left err- Right (rvec, expr') -> convert (evalExpr expr' r) rvec--convert :: Result -> Bool -> Result-convert r True = r-convert r False =- case r of- Right v@(Np _) -> Right (toGeo v)- Right (Vals vs) -> Right (Vals (map toGeo vs))- oth -> oth--toGeo :: Value -> Value-toGeo (Np v) = Gp (fromNVector v)-toGeo val = val---- | All supported functions. -functions :: [String]-functions =- [ "antipode"- , "crossTrackDistance"- , "cpa"- , "delta"- , "deltaBetween"- , "destination"- , "ecef"- , "frameB"- , "frameL"- , "frameN"- , "finalBearing"- , "fromEcef"- , "geo"- , "greatCircle"- , "initialBearing"- , "intercept"- , "interceptBySpeed"- , "interceptByTime"- , "interpolate"- , "intersections"- , "insideSurface"- , "mean"- , "ned"- , "nedBetween"- , "position"- , "surfaceDistance"- , "target"- , "targetN"- , "track"- , "toEcef"- , "toNVector"- ]---- | @insert k v vault@ inserts value @v@ for key @k@. Overwrites any previous value. -insert :: String -> Value -> Vault -> Vault-insert k v vault = Vault (e ++ [(k, v)])- where- Vault e = delete k vault---- | @lookup k vault@ looks up the value of key @k@ in the vault. -lookup :: String -> Vault -> Maybe Value-lookup k (Vault es) = fmap snd (find (\e -> fst e == k) es)---- | @delete k vault@ deletes key @k@ from the vault. -delete :: String -> Vault -> Vault-delete k (Vault es) = Vault (filter (\e -> fst e /= k) es)--expr :: (MonadFail m) => String -> m (Bool, Expr)-expr s = do- ts <- tokenise s- ast <- parse ts- fmap (expectVec ts, ) (transform ast)--expectVec :: [Token] -> Bool-expectVec (_:Func "toNVector":_) = True-expectVec _ = False--evalExpr :: Expr -> Vault -> Result-evalExpr (Param p) vault =- case lookup p vault of- Just (Gp geo) -> Right (Np (toNVector geo))- Just v -> Right v- Nothing -> tryRead p-evalExpr (Antipode a) vault =- case evalExpr a vault of- (Right (Np p)) -> Right (Np (antipode p))- r -> Left ("Call error: antipode " ++ showErr [r])-evalExpr (ClosestPointOfApproach a b) vault =- case [evalExpr a vault, evalExpr b vault] of- [Right (Trk t1), Right (Trk t2)] ->- maybe (Left "closest point of approach in the past") (Right . Cpa) (cpa84 t1 t2)- r -> Left ("Call error: cpa " ++ showErr r)-evalExpr (CrossTrackDistance a b) vault =- case [evalExpr a vault, evalExpr b vault] of- [Right (Np p), Right (Gc gc)] -> Right (Len (crossTrackDistance84 p gc))- r -> Left ("Call error: crossTrackDistance " ++ showErr r)-evalExpr (DeltaBetween a b c d) vault =- case [evalExpr a vault, evalExpr b vault, evalExpr c vault, evalEarth d] of- [Right (Np p1), Right (Np p2), Right (FrmB y p r), Right (Em m)] ->- Right (Dlt (deltaBetween p1 p2 (frameB y p r) m))- [Right (Np p1), Right (Np p2), Right (FrmL w), Right (Em m)] ->- Right (Dlt (deltaBetween p1 p2 (frameL w) m))- [Right (Np p1), Right (Np p2), Right FrmN, Right (Em m)] ->- Right (Dlt (deltaBetween p1 p2 frameN m))- r -> Left ("Call error: deltaBetween " ++ showErr r)-evalExpr (DeltaV a b c) vault =- case [evalExpr a vault, evalExpr b vault, evalExpr c vault] of- [Right (Len x), Right (Len y), Right (Len z)] -> Right (Dlt (delta x y z))- [Right (Double x), Right (Double y), Right (Double z)] -> Right (Dlt (deltaMetres x y z))- r -> Left ("Call error: delta " ++ showErr r)-evalExpr (Destination a b c) vault =- case [evalExpr a vault, evalExpr b vault, evalExpr c vault] of- [Right (Np p), Right (Ang a'), Right (Len l)] -> Right (Np (destination84 p a' l))- [Right (Np p), Right (Double a'), Right (Len l)] ->- Right (Np (destination84 p (decimalDegrees a') l))- r -> Left ("Call error: destination " ++ showErr r)-evalExpr (Ecef a b c) vault =- case [evalExpr a vault, evalExpr b vault, evalExpr c vault] of- [Right (Len x), Right (Len y), Right (Len z)] -> Right (Ep (ecef x y z))- [Right (Double x), Right (Double y), Right (Double z)] -> Right (Ep (ecefMetres x y z))- r -> Left ("Call error: ecef " ++ showErr r)-evalExpr (FrameB a b c) vault =- case [evalExpr a vault, evalExpr b vault, evalExpr c vault] of- [Right (Ang a'), Right (Ang b'), Right (Ang c')] -> Right (FrmB a' b' c')- r -> Left ("Call error: frameB " ++ showErr r)-evalExpr (FrameL a) vault =- case evalExpr a vault of- (Right (Ang a')) -> Right (FrmL a')- r -> Left ("Call error: frameL " ++ showErr [r])-evalExpr FrameN _ = Right FrmN-evalExpr (FromEcef a b) vault =- case [evalExpr a vault, evalEarth b] of- [Right (Ep p), Right (Em m)] -> Right (Np (fromEcef p m))- r -> Left ("Call error: fromEcef " ++ showErr r)-evalExpr (FinalBearing a b) vault =- case [evalExpr a vault, evalExpr b vault] of- [Right (Np p1), Right (Np p2)] ->- maybe- (Left "Call error: finalBearing identical points")- (Right . Ang)- (finalBearing p1 p2)- r -> Left ("Call error: finalBearing " ++ showErr r)-evalExpr (Geo as) vault =- case vs of- [Right p@(Np _)] -> Right p- [Right (Np v), Right (Len h)] -> Right (Np (AngularPosition (pos v) h))- [Right (Double lat), Right (Double lon)] ->- bimap- (\e -> "Call error: geo " ++ e)- (Np . toNVector)- (decimalLatLongHeightE lat lon zero)- [Right (Double lat), Right (Double lon), Right (Len h)] ->- bimap (\e -> "Call error: geo " ++ e) (Np . toNVector) (decimalLatLongHeightE lat lon h)- [Right (Double lat), Right (Double lon), Right (Double h)] ->- bimap- (\e -> "Call error: geo " ++ e)- (Np . toNVector)- (decimalLatLongHeightE lat lon (metres h))- r -> Left ("Call error: geo " ++ showErr r)- where- vs = map (`evalExpr` vault) as-evalExpr (GreatCircleE as) vault =- case fmap (`evalExpr` vault) as of- [Right (Np p1), Right (Np p2)] -> bimap id Gc (greatCircleE (p1, p2))- [Right (Np p), Right (Ang a')] -> bimap id Gc (greatCircleE (p, a'))- [Right (Trk t)] -> bimap id Gc (greatCircleE t)- r -> Left ("Call error: greatCircle " ++ showErr r)-evalExpr (InitialBearing a b) vault =- case [evalExpr a vault, evalExpr b vault] of- [Right (Np p1), Right (Np p2)] ->- maybe- (Left "Call error: initialBearing identical points")- (Right . Ang)- (initialBearing p1 p2)- r -> Left ("Call error: initialBearing " ++ showErr r)-evalExpr (Intercept a b) vault =- case [evalExpr a vault, evalExpr b vault] of- [Right (Trk t), Right (Np i)] ->- maybe (Left "intercept impossible") (Right . Intp) (intercept84 t i)- r -> Left ("Call error: intercept " ++ showErr r)-evalExpr (InterceptBySpeed a b c) vault =- case [evalExpr a vault, evalExpr b vault, evalExpr c vault] of- [Right (Trk t), Right (Np i), Right (Spd s)] ->- maybe (Left "intercept impossible") (Right . Intp) (interceptBySpeed84 t i s)- r -> Left ("Call error: interceptBySpeed " ++ showErr r)-evalExpr (InterceptByTime a b c) vault =- case [evalExpr a vault, evalExpr b vault, evalExpr c vault] of- [Right (Trk t), Right (Np i), Right (Dur d)] ->- maybe (Left "intercept impossible") (Right . Intp) (interceptByTime84 t i d)- r -> Left ("Call error: interceptByTime " ++ showErr r)-evalExpr (Interpolate a b c) vault =- case [evalExpr a vault, evalExpr b vault] of- [Right (Np p1), Right (Np p2)] -> Right (Np (interpolate p1 p2 c))- r -> Left ("Call error: interpolate " ++ showErr r)-evalExpr (Intersections a b) vault =- case [evalExpr a vault, evalExpr b vault] of- [Right (Gc gc1), Right (Gc gc2)] ->- maybe- (Right (Vals []))- (\is -> Right (Vals [Np (fst is), Np (snd is)]))- (intersections gc1 gc2 :: Maybe (AngularPosition NVector, AngularPosition NVector))- r -> Left ("Call error: intersections " ++ showErr r)-evalExpr (InsideSurface as) vault =- let m = map (`evalExpr` vault) as- ps = [p | Right (Np p) <- m]- in if length m == length ps && length ps > 3- then Right (Bool (insideSurface (head ps) (tail ps)))- else Left ("Call error: insideSurface " ++ showErr m)-evalExpr (Mean as) vault =- let m = map (`evalExpr` vault) as- ps = [p | Right (Np p) <- m]- in if length m == length ps- then maybe (Left ("Call error: mean " ++ showErr m)) (Right . Np) (mean ps)- else Left ("Call error: mean " ++ showErr m)-evalExpr (NedBetween a b c) vault =- case [evalExpr a vault, evalExpr b vault, evalEarth c] of- [Right (Np p1), Right (Np p2), Right (Em m)] -> Right (Ned (nedBetween p1 p2 m))- r -> Left ("Call error: nedBetween " ++ showErr r)-evalExpr (NedV a b c) vault =- case [evalExpr a vault, evalExpr b vault, evalExpr c vault] of- [Right (Len x), Right (Len y), Right (Len z)] -> Right (Ned (ned x y z))- [Right (Double x), Right (Double y), Right (Double z)] -> Right (Ned (nedMetres x y z))- r -> Left ("Call error: ned " ++ showErr r)-evalExpr (Position a b) vault =- case [evalExpr a vault, evalExpr b vault] of- [Right (Trk t), Right (Dur d)] -> Right (Np (position84 t d))- r -> Left ("Call error: position " ++ showErr r)-evalExpr (SurfaceDistance a b) vault =- case [evalExpr a vault, evalExpr b vault] of- [Right (Np p1), Right (Np p2)] -> Right (Len (surfaceDistance84 p1 p2))- r -> Left ("Call error: surfaceDistance " ++ showErr r)-evalExpr (Target a b c d) vault =- case [evalExpr a vault, evalExpr b vault, evalExpr c vault, evalEarth d] of- [Right (Np p0), Right (FrmB y p r), Right (Dlt d'), Right (Em m)] ->- Right (Np (target p0 (frameB y p r) d' m))- [Right (Np p0), Right (FrmL w), Right (Dlt d'), Right (Em m)] ->- Right (Np (target p0 (frameL w) d' m))- [Right (Np p0), Right FrmN, Right (Dlt d'), Right (Em m)] ->- Right (Np (target p0 frameN d' m))- r -> Left ("Call error: target " ++ showErr r)-evalExpr (TargetN a b c) vault =- case [evalExpr a vault, evalExpr b vault, evalEarth c] of- [Right (Np p0), Right (Ned d), Right (Em m)] -> Right (Np (targetN p0 d m))- r -> Left ("Call error: targetN " ++ showErr r)-evalExpr (TrackE a b c) vault =- case [evalExpr a vault, evalExpr b vault, evalExpr c vault] of- [Right (Np p), Right (Ang b'), Right (Spd s)] -> Right (Trk (Track p b' s))- r -> Left ("Call error: track " ++ showErr r)-evalExpr (ToEcef a b) vault =- case [evalExpr a vault, evalEarth b] of- [Right (Np p), Right (Em m)] -> Right (Ep (toEcef p m))- r -> Left ("Call error: toEcef " ++ showErr r)-evalExpr (ToNVector a) vault =- case evalExpr a vault of- r@(Right (Np _)) -> r- r -> Left ("Call error: toNVector " ++ showErr [r])--evalEarth :: String -> Result-evalEarth "wgs84" = Right (Em wgs84)-evalEarth "grs80" = Right (Em grs80)-evalEarth "wgs72" = Right (Em wgs72)-evalEarth "s84" = Right (Em s84)-evalEarth "s80" = Right (Em s80)-evalEarth "s72" = Right (Em s72)-evalEarth s = Left s--showErr :: [Result] -> String-showErr rs = " > " ++ intercalate " & " (map (either id show) rs)--tryRead :: String -> Result-tryRead s- | null r = Left ("couldn't read " ++ s)- | otherwise = Right (head r)- where- r =- rights- (map ($ s)- [ readE readAngleE Ang- , readE readLengthE Len- , readE readSpeedE Spd- , readE readDurationE Dur- , readE readLatLongE (\ll -> Np (toNVector (AngularPosition ll zero)))- , readE readEither Double- ])--readE :: (String -> Either String a) -> (a -> Value) -> String -> Either String Value-readE p v s = bimap id v (p s)-------------------------------------------- --- Lexical Analysis: String -> [Token] -- ------------------------------------------- -data Token- = Paren Char- | Func String- | Str String- deriving (Show)--tokenise :: (MonadFail m) => String -> m [Token]-tokenise s- | null r = fail ("Lexical error: " ++ s)- | (e, "") <- last r = return (wrap e)- | otherwise = fail ("Lexical error: " ++ snd (last r))- where- r = readP_to_S tokens s---- | wraps top level expression between () if needed. -wrap :: [Token] -> [Token]-wrap ts- | null ts = ts- | (Paren '(') <- head ts = ts- | otherwise = Paren '(' : ts ++ [Paren ')']--tokens :: ReadP [Token]-tokens = many1 token--token :: ReadP Token-token = (<++) ((<++) paren func) str--paren :: ReadP Token-paren = (<++) parenO parenC--parenO :: ReadP Token-parenO = do- optional (char ' ')- c <- char '('- return (Paren c)--parenC :: ReadP Token-parenC = do- c <- char ')'- optional (char ' ')- return (Paren c)--func :: ReadP Token-func = do- n <- choice (map string functions)- _ <- char ' '- return (Func n)--str :: ReadP Token-str = do- optional (char ' ')- v <- munch1 (\c -> c /= '(' && c /= ')' && c /= ' ')- if v `elem` functions- then pfail- else return (Str v)------------------------------------------- --- Syntactic Analysis: [Token] -> Ast -- ------------------------------------------ -data Ast- = Call String- [Ast]- | Lit String- deriving (Show)---- | syntax is (f x y) where x and y can be function themselves. -parse :: (MonadFail m) => [Token] -> m Ast-parse ts = fmap fst (walk ts)--walk :: (MonadFail m) => [Token] -> m (Ast, [Token])-walk [] = fail "Syntax error: empty"-walk (h:t)- | (Str s) <- h = return (Lit s, t)- | (Paren '(') <- h = walkFunc t- | otherwise = fail ("Syntax error: expected String or '(' but got " ++ show h)--walkFunc :: (MonadFail m) => [Token] -> m (Ast, [Token])-walkFunc [] = fail "Syntax error: '(' unexpected"-walkFunc (h:t)- | (Func n) <- h = walkParams n t []- | otherwise = fail ("Syntax error: expected Function but got " ++ show h)--walkParams :: (MonadFail m) => String -> [Token] -> [Ast] -> m (Ast, [Token])-walkParams _ [] _ = fail "Syntax error: ')' not found"-walkParams n ts@(h:t) acc- | (Paren ')') <- h = return (Call n (reverse acc), t)- | otherwise = do- (el, t') <- walk ts- walkParams n t' (el : acc)--------------------------------------- --- Semantic Analysis: Ast -> Expr -- -------------------------------------- -data Expr- = Param String- | Antipode Expr- | ClosestPointOfApproach Expr- Expr- | CrossTrackDistance Expr- Expr- | DeltaBetween Expr- Expr- Expr- String- | DeltaV Expr- Expr- Expr- | Destination Expr- Expr- Expr- | Ecef Expr- Expr- Expr- | FrameB Expr- Expr- Expr- | FrameL Expr- | FrameN- | FinalBearing Expr- Expr- | FromEcef Expr- String- | Geo [Expr]- | GreatCircleE [Expr]- | InitialBearing Expr- Expr- | Intercept Expr- Expr- | InterceptBySpeed Expr- Expr- Expr- | InterceptByTime Expr- Expr- Expr- | Interpolate Expr- Expr- Double- | Intersections Expr- Expr- | InsideSurface [Expr]- | Mean [Expr]- | NedBetween Expr- Expr- String- | NedV Expr- Expr- Expr- | Position Expr- Expr- | SurfaceDistance Expr- Expr- | Target Expr- Expr- Expr- String- | TargetN Expr- Expr- String- | TrackE Expr- Expr- Expr- | ToEcef Expr- String- | ToNVector Expr- deriving (Show)--transform :: (MonadFail m) => Ast -> m Expr-transform (Call "antipode" [e]) = fmap Antipode (transform e)-transform (Call "cpa" [e1, e2]) = do- t1 <- transform e1- t2 <- transform e2- return (ClosestPointOfApproach t1 t2)-transform (Call "crossTrackDistance" [e1, e2]) = do- p <- transform e1- gc <- transform e2- return (CrossTrackDistance p gc)-transform (Call "delta" [e1, e2, e3]) = do- p1 <- transform e1- p2 <- transform e2- p3 <- transform e3- return (DeltaV p1 p2 p3)-transform (Call "deltaBetween" [e1, e2, e3]) = do- p1 <- transform e1- p2 <- transform e2- f <- transform e3- return (DeltaBetween p1 p2 f "wgs84")-transform (Call "deltaBetween" [e1, e2, e3, Lit s]) = do- p1 <- transform e1- p2 <- transform e2- f <- transform e3- return (DeltaBetween p1 p2 f s)-transform (Call "destination" [e1, e2, e3]) = do- p1 <- transform e1- p2 <- transform e2- p3 <- transform e3- return (Destination p1 p2 p3)-transform (Call "ecef" [e1, e2, e3]) = do- p1 <- transform e1- p2 <- transform e2- p3 <- transform e3- return (Ecef p1 p2 p3)-transform (Call "frameB" [e1, e2, e3]) = do- p1 <- transform e1- p2 <- transform e2- p3 <- transform e3- return (FrameB p1 p2 p3)-transform (Call "frameL" [e]) = fmap FrameL (transform e)-transform (Call "frameN" []) = return FrameN-transform (Call "fromEcef" [e]) = do- p <- transform e- return (FromEcef p "wgs84")-transform (Call "fromEcef" [e, Lit s]) = do- p <- transform e- return (FromEcef p s)-transform (Call "finalBearing" [e1, e2]) = do- p1 <- transform e1- p2 <- transform e2- return (FinalBearing p1 p2)-transform (Call "geo" e) = do- ps <- mapM transform e- return (Geo ps)-transform (Call "greatCircle" e) = do- ps <- mapM transform e- return (GreatCircleE ps)-transform (Call "initialBearing" [e1, e2]) = do- p1 <- transform e1- p2 <- transform e2- return (InitialBearing p1 p2)-transform (Call "intercept" [e1, e2]) = do- t <- transform e1- i <- transform e2- return (Intercept t i)-transform (Call "interceptBySpeed" [e1, e2, e3]) = do- t <- transform e1- i <- transform e2- s <- transform e3- return (InterceptBySpeed t i s)-transform (Call "interceptTime" [e1, e2, e3]) = do- t <- transform e1- i <- transform e2- d <- transform e3- return (InterceptByTime t i d)-transform (Call "interpolate" [e1, e2, Lit s]) = do- p1 <- transform e1- p2 <- transform e2- d <- readDouble s- if d >= 0.0 && d <= 1.0- then return (Interpolate p1 p2 d)- else fail "Semantic error: interpolate expects [0..1] as last argument"-transform (Call "intersections" [e1, e2]) = do- gc1 <- transform e1- gc2 <- transform e2- return (Intersections gc1 gc2)-transform (Call "insideSurface" e) = do- ps <- mapM transform e- return (InsideSurface ps)-transform (Call "mean" e) = do- ps <- mapM transform e- return (Mean ps)-transform (Call "ned" [e1, e2, e3]) = do- p1 <- transform e1- p2 <- transform e2- p3 <- transform e3- return (NedV p1 p2 p3)-transform (Call "nedBetween" [e1, e2]) = do- p1 <- transform e1- p2 <- transform e2- return (NedBetween p1 p2 "wgs84")-transform (Call "nedBetween" [e1, e2, Lit s]) = do- p1 <- transform e1- p2 <- transform e2- return (NedBetween p1 p2 s)-transform (Call "position" [e1, e2]) = do- t <- transform e1- d <- transform e2- return (Position t d)-transform (Call "surfaceDistance" [e1, e2]) = do- p1 <- transform e1- p2 <- transform e2- return (SurfaceDistance p1 p2)-transform (Call "target" [e1, e2, e3]) = do- p0 <- transform e1- f <- transform e2- d <- transform e3- return (Target p0 f d "wgs84")-transform (Call "target" [e1, e2, e3, Lit s]) = do- p0 <- transform e1- f <- transform e2- d <- transform e3- return (Target p0 f d s)-transform (Call "targetN" [e1, e2]) = do- p0 <- transform e1- d <- transform e2- return (TargetN p0 d "wgs84")-transform (Call "targetN" [e1, e2, Lit s]) = do- p0 <- transform e1- d <- transform e2- return (TargetN p0 d s)-transform (Call "track" [e1, e2, e3]) = do- p0 <- transform e1- b <- transform e2- s <- transform e3- return (TrackE p0 b s)-transform (Call "toEcef" [e]) = do- p <- transform e- return (ToEcef p "wgs84")-transform (Call "toEcef" [e, Lit s]) = do- p <- transform e- return (ToEcef p s)-transform (Call "toNVector" [e]) = fmap ToNVector (transform e)-transform (Call f e) = fail ("Semantic error: " ++ f ++ " does not accept " ++ show e)-transform (Lit s) = return (Param s)--readDouble :: (MonadFail m) => String -> m Double-readDouble s =- case readMaybe s of- Just d -> return d- Nothing -> fail ("Unparsable double: " ++ s)+{-# LANGUAGE FlexibleInstances #-} +{-# LANGUAGE TupleSections #-} +{-# OPTIONS_GHC -fno-warn-orphans #-} + +-- | +-- Module: Eval +-- Copyright: (c) 2018 Cedric Liegeois +-- License: BSD3 +-- Maintainer: Cedric Liegeois <ofmooseandmen@yahoo.fr> +-- Stability: experimental +-- Portability: portable +-- +-- Types and functions for evaluating expressions in textual form. +-- +module Eval + ( Result + , eval + , functions + ) where + +import Control.Monad.Fail +import Data.Bifunctor +import Data.Either (rights) +import Data.Geo.Jord +import Data.List (intercalate) +import Data.Maybe +import Prelude hiding (fail, lookup) +import Show +import State +import Text.ParserCombinators.ReadP +import Text.Read (readEither, readMaybe) + +-- | 'Either' an error or a 'Value'. +type Result = Either String Value + +instance MonadFail (Either String) where + fail = Left + +-- | Evaluates @s@, an expression of the form @"(f x y ..)"@. +-- +-- >>> eval "finalBearing (destination (antipode 54°N,154°E) 54° 1000m) 54°N,154°E" +-- 126° +-- +-- @f@ must be one of the supported 'functions' and each parameter @x@, @y@, .. , is either another function call +-- or a 'String' parameter. Parameters are either resolved by name using the 'Resolve' +-- function @r@ or if it returns 'Nothing', 'read' to an 'Angle', a 'Length' or a 'LatLong'. +-- +-- If the evaluation is successful, returns the resulting 'Value' ('Right') otherwise +-- a description of the error ('Left'). +-- +-- @ +-- state = emptyState +-- angle = eval "finalBearing 54N154E 54S154W" state -- Right Ang +-- length = eval "surfaceDistance (antipode 54N154E) 54S154W" state -- Right Len +-- -- parameter resolution from state +-- a1 = eval "finalBearing 54N154E 54S154W" state +-- state = insert "a1" state +-- a2 = eval "(finalBearing a1 54S154W)" state +-- @ +-- +-- All returned positions are 'LatLong' by default, to get back a n-vector the +-- expression must be wrapped by 'toNVector'. +-- +-- @ +-- dest = eval "destination 54°N,154°E 54° 1000m" -- Right Ll +-- dest = eval "toNVector (destination 54°N,154°E 54° 1000m)" -- Right Np +-- @ +-- +-- Every function call must be wrapped between parentheses, however they can be ommitted for the top level call. +-- +-- @ +-- angle = eval "finalBearing 54N154E 54S154W" -- Right Ang +-- angle = eval "(finalBearing 54N154E 54S154W)" -- Right Ang +-- length = eval "distance (antipode 54N154E) 54S154W" -- Right Len +-- length = eval "distance antipode 54N154E 54S154W" -- Left String +-- @ +-- +eval :: String -> State -> Result +eval st state = + case expr st of + Left err -> Left err + Right (rvec, expr') -> convert (evalExpr expr' state) rvec + +convert :: Result -> Bool -> Result +convert r True = r +convert r False = + case r of + Right v@(Np _) -> Right (toGeo v) + Right (Vals vs) -> Right (Vals (map toGeo vs)) + oth -> oth + +toGeo :: Value -> Value +toGeo (Np v) = Gp (fromNVector v) +toGeo val = val + +-- | All supported functions. +functions :: [String] +functions = + [ "antipode" + , "crossTrackDistance" + , "cpa" + , "delta" + , "deltaBetween" + , "destination" + , "ecef" + , "frameB" + , "frameL" + , "frameN" + , "finalBearing" + , "fromEcef" + , "geo" + , "greatCircle" + , "initialBearing" + , "intercept" + , "interpolate" + , "intersections" + , "insideSurface" + , "mean" + , "ned" + , "nedBetween" + , "position" + , "surfaceDistance" + , "target" + , "targetN" + , "track" + , "toEcef" + , "toNVector" + ] + +expr :: (MonadFail m) => String -> m (Bool, Expr) +expr s = do + ts <- tokenise s + ast <- parse ts + fmap (expectVec ts, ) (transform ast) + +expectVec :: [Token] -> Bool +expectVec (_:Func "toNVector":_) = True +expectVec _ = False + +evalExpr :: Expr -> State -> Result +evalExpr (Param p) state = + case lookup p state of + Just (Gp geo) -> Right (Np (toNVector geo)) + Just v -> Right v + Nothing -> tryRead p +evalExpr (Antipode a) state = + case evalExpr a state of + (Right (Np p)) -> Right (Np (antipode p)) + r -> Left ("Call error: antipode " ++ showErr [r] state) +evalExpr (ClosestPointOfApproach a b) state = + case [evalExpr a state, evalExpr b state] of + [Right (Trk t1), Right (Trk t2)] -> + maybe (Left "closest point of approach in the past") (Right . Cpa) (cpa84 t1 t2) + r -> Left ("Call error: cpa " ++ showErr r state) +evalExpr (CrossTrackDistance a b) state = + case [evalExpr a state, evalExpr b state] of + [Right (Np p), Right (Gc gc)] -> Right (Len (crossTrackDistance84 p gc)) + r -> Left ("Call error: crossTrackDistance " ++ showErr r state) +evalExpr (DeltaBetween a b c d) state = + case [evalExpr a state, evalExpr b state, evalExpr c state, evalEarth d] of + [Right (Np p1), Right (Np p2), Right (FrmB y p r), Right (Em m)] -> + Right (Dlt (deltaBetween p1 p2 (frameB y p r) m)) + [Right (Np p1), Right (Np p2), Right (FrmL w), Right (Em m)] -> + Right (Dlt (deltaBetween p1 p2 (frameL w) m)) + [Right (Np p1), Right (Np p2), Right FrmN, Right (Em m)] -> + Right (Dlt (deltaBetween p1 p2 frameN m)) + r -> Left ("Call error: deltaBetween " ++ showErr r state) +evalExpr (DeltaV a b c) state = + case [evalExpr a state, evalExpr b state, evalExpr c state] of + [Right (Len x), Right (Len y), Right (Len z)] -> Right (Dlt (delta x y z)) + [Right (Double x), Right (Double y), Right (Double z)] -> Right (Dlt (deltaMetres x y z)) + r -> Left ("Call error: delta " ++ showErr r state) +evalExpr (Destination a b c) state = + case [evalExpr a state, evalExpr b state, evalExpr c state] of + [Right (Np p), Right (Ang a'), Right (Len l)] -> Right (Np (destination84 p a' l)) + [Right (Np p), Right (Double a'), Right (Len l)] -> + Right (Np (destination84 p (decimalDegrees a') l)) + r -> Left ("Call error: destination " ++ showErr r state) +evalExpr (Ecef a b c) state = + case [evalExpr a state, evalExpr b state, evalExpr c state] of + [Right (Len x), Right (Len y), Right (Len z)] -> Right (Ep (ecef x y z)) + [Right (Double x), Right (Double y), Right (Double z)] -> Right (Ep (ecefMetres x y z)) + r -> Left ("Call error: ecef " ++ showErr r state) +evalExpr (FrameB a b c) state = + case [evalExpr a state, evalExpr b state, evalExpr c state] of + [Right (Ang a'), Right (Ang b'), Right (Ang c')] -> Right (FrmB a' b' c') + r -> Left ("Call error: frameB " ++ showErr r state) +evalExpr (FrameL a) state = + case evalExpr a state of + (Right (Ang a')) -> Right (FrmL a') + r -> Left ("Call error: frameL " ++ showErr [r] state) +evalExpr FrameN _ = Right FrmN +evalExpr (FromEcef a b) state = + case [evalExpr a state, evalEarth b] of + [Right (Ep p), Right (Em m)] -> Right (Np (fromEcef p m)) + r -> Left ("Call error: fromEcef " ++ showErr r state) +evalExpr (FinalBearing a b) state = + case [evalExpr a state, evalExpr b state] of + [Right (Np p1), Right (Np p2)] -> + maybe + (Left "Call error: finalBearing identical points") + (Right . Ang) + (finalBearing p1 p2) + r -> Left ("Call error: finalBearing " ++ showErr r state) +evalExpr (Geo as) state = + case vs of + [Right p@(Np _)] -> Right p + [Right (Np v), Right (Len h)] -> Right (Np (AngularPosition (pos v) h)) + [Right (Double lat), Right (Double lon)] -> + bimap + (\e -> "Call error: geo " ++ e) + (Np . toNVector) + (decimalLatLongHeightE lat lon zero) + [Right (Double lat), Right (Double lon), Right (Len h)] -> + bimap (\e -> "Call error: geo " ++ e) (Np . toNVector) (decimalLatLongHeightE lat lon h) + [Right (Double lat), Right (Double lon), Right (Double h)] -> + bimap + (\e -> "Call error: geo " ++ e) + (Np . toNVector) + (decimalLatLongHeightE lat lon (metres h)) + r -> Left ("Call error: geo " ++ showErr r state) + where + vs = map (`evalExpr` state) as +evalExpr (GreatCircleE as) state = + case fmap (`evalExpr` state) as of + [Right (Np p1), Right (Np p2)] -> bimap id Gc (greatCircleE (p1, p2)) + [Right (Np p), Right (Ang a')] -> bimap id Gc (greatCircleE (p, a')) + [Right (Trk t)] -> bimap id Gc (greatCircleE t) + r -> Left ("Call error: greatCircle " ++ showErr r state) +evalExpr (InitialBearing a b) state = + case [evalExpr a state, evalExpr b state] of + [Right (Np p1), Right (Np p2)] -> + maybe + (Left "Call error: initialBearing identical points") + (Right . Ang) + (initialBearing p1 p2) + r -> Left ("Call error: initialBearing " ++ showErr r state) +evalExpr (Intercept as) state = + case fmap (`evalExpr` state) as of + [Right (Trk t), Right (Np i)] -> + maybe (Left "undefined minimum speed intercept") (Right . Intp) (intercept84 t i) + [Right (Trk t), Right (Np i), Right (Spd s)] -> + maybe (Left "undefined time to intercept") (Right . Intp) (interceptBySpeed84 t i s) + [Right (Trk t), Right (Np i), Right (Dur d)] -> + maybe (Left "undefined speed to intercept") (Right . Intp) (interceptByTime84 t i d) + r -> Left ("Call error: intercept " ++ showErr r state) +evalExpr (Interpolate a b c) state = + case [evalExpr a state, evalExpr b state] of + [Right (Np p1), Right (Np p2)] -> Right (Np (interpolate p1 p2 c)) + r -> Left ("Call error: interpolate " ++ showErr r state) +evalExpr (Intersections a b) state = + case [evalExpr a state, evalExpr b state] of + [Right (Gc gc1), Right (Gc gc2)] -> + maybe + (Right (Vals [])) + (\is -> Right (Vals [Np (fst is), Np (snd is)])) + (intersections gc1 gc2 :: Maybe (AngularPosition NVector, AngularPosition NVector)) + r -> Left ("Call error: intersections " ++ showErr r state) +evalExpr (InsideSurface as) state = + let m = map (`evalExpr` state) as + ps = [p | Right (Np p) <- m] + in if length m == length ps && length ps > 3 + then Right (Bool (insideSurface (head ps) (tail ps))) + else Left ("Call error: insideSurface " ++ showErr m state) +evalExpr (Mean as) state = + let m = map (`evalExpr` state) as + ps = [p | Right (Np p) <- m] + in if length m == length ps + then maybe (Left ("Call error: mean " ++ showErr m state)) (Right . Np) (mean ps) + else Left ("Call error: mean " ++ showErr m state) +evalExpr (NedBetween a b c) state = + case [evalExpr a state, evalExpr b state, evalEarth c] of + [Right (Np p1), Right (Np p2), Right (Em m)] -> Right (Ned (nedBetween p1 p2 m)) + r -> Left ("Call error: nedBetween " ++ showErr r state) +evalExpr (NedV a b c) state = + case [evalExpr a state, evalExpr b state, evalExpr c state] of + [Right (Len x), Right (Len y), Right (Len z)] -> Right (Ned (ned x y z)) + [Right (Double x), Right (Double y), Right (Double z)] -> Right (Ned (nedMetres x y z)) + r -> Left ("Call error: ned " ++ showErr r state) +evalExpr (Position a b) state = + case [evalExpr a state, evalExpr b state] of + [Right (Trk t), Right (Dur d)] -> Right (Np (position84 t d)) + r -> Left ("Call error: position " ++ showErr r state) +evalExpr (SurfaceDistance a b) state = + case [evalExpr a state, evalExpr b state] of + [Right (Np p1), Right (Np p2)] -> Right (Len (surfaceDistance84 p1 p2)) + r -> Left ("Call error: surfaceDistance " ++ showErr r state) +evalExpr (Target a b c d) state = + case [evalExpr a state, evalExpr b state, evalExpr c state, evalEarth d] of + [Right (Np p0), Right (FrmB y p r), Right (Dlt d'), Right (Em m)] -> + Right (Np (target p0 (frameB y p r) d' m)) + [Right (Np p0), Right (FrmL w), Right (Dlt d'), Right (Em m)] -> + Right (Np (target p0 (frameL w) d' m)) + [Right (Np p0), Right FrmN, Right (Dlt d'), Right (Em m)] -> + Right (Np (target p0 frameN d' m)) + r -> Left ("Call error: target " ++ showErr r state) +evalExpr (TargetN a b c) state = + case [evalExpr a state, evalExpr b state, evalEarth c] of + [Right (Np p0), Right (Ned d), Right (Em m)] -> Right (Np (targetN p0 d m)) + r -> Left ("Call error: targetN " ++ showErr r state) +evalExpr (TrackE a b c) state = + case [evalExpr a state, evalExpr b state, evalExpr c state] of + [Right (Np p), Right (Ang b'), Right (Spd s)] -> Right (Trk (Track p b' s)) + r -> Left ("Call error: track " ++ showErr r state) +evalExpr (ToEcef a b) state = + case [evalExpr a state, evalEarth b] of + [Right (Np p), Right (Em m)] -> Right (Ep (toEcef p m)) + r -> Left ("Call error: toEcef " ++ showErr r state) +evalExpr (ToNVector a) state = + case evalExpr a state of + r@(Right (Np _)) -> r + r -> Left ("Call error: toNVector " ++ showErr [r] state) + +evalEarth :: String -> Result +evalEarth "wgs84" = Right (Em wgs84) +evalEarth "grs80" = Right (Em grs80) +evalEarth "wgs72" = Right (Em wgs72) +evalEarth "s84" = Right (Em s84) +evalEarth "s80" = Right (Em s80) +evalEarth "s72" = Right (Em s72) +evalEarth s = Left s + +showErr :: [Result] -> State -> String +showErr rs s = " > " ++ intercalate " & " (map (either id (`showV` s)) rs) + +tryRead :: String -> Result +tryRead s + | null r = Left ("couldn't read " ++ s) + | otherwise = Right (head r) + where + r = + rights + (map ($ s) + [ readE readAngleE Ang + , readE readLengthE Len + , readE readSpeedE Spd + , readE readDurationE Dur + , readE readLatLongE (\ll -> Np (toNVector (AngularPosition ll zero))) + , readE readEither Double + ]) + +readE :: (String -> Either String a) -> (a -> Value) -> String -> Either String Value +readE p v s = bimap id v (p s) + +------------------------------------------ +-- Lexical Analysis: String -> [Token] -- +------------------------------------------ +data Token + = Paren Char + | Func String + | Str String + deriving (Show) + +tokenise :: (MonadFail m) => String -> m [Token] +tokenise s + | null r = fail ("Lexical error: " ++ s) + | (e, "") <- last r = return (wrap e) + | otherwise = fail ("Lexical error: " ++ snd (last r)) + where + r = readP_to_S tokens s + +-- | wraps top level expression between () if needed. +wrap :: [Token] -> [Token] +wrap ts + | null ts = ts + | (Paren '(') <- head ts = ts + | otherwise = Paren '(' : ts ++ [Paren ')'] + +tokens :: ReadP [Token] +tokens = many1 token + +token :: ReadP Token +token = (<++) ((<++) paren func) str + +paren :: ReadP Token +paren = (<++) parenO parenC + +parenO :: ReadP Token +parenO = do + optional (char ' ') + c <- char '(' + return (Paren c) + +parenC :: ReadP Token +parenC = do + c <- char ')' + optional (char ' ') + return (Paren c) + +func :: ReadP Token +func = do + n <- choice (map string functions) + _ <- char ' ' + return (Func n) + +str :: ReadP Token +str = do + optional (char ' ') + v <- munch1 (\c -> c /= '(' && c /= ')' && c /= ' ') + if v `elem` functions + then pfail + else return (Str v) + +----------------------------------------- +-- Syntactic Analysis: [Token] -> Ast -- +----------------------------------------- +data Ast + = Call String + [Ast] + | Lit String + deriving (Show) + +-- | syntax is (f x y) where x and y can be function themselves. +parse :: (MonadFail m) => [Token] -> m Ast +parse ts = fmap fst (walk ts) + +walk :: (MonadFail m) => [Token] -> m (Ast, [Token]) +walk [] = fail "Syntax error: empty" +walk (h:t) + | (Str s) <- h = return (Lit s, t) + | (Paren '(') <- h = walkFunc t + | otherwise = fail ("Syntax error: expected String or '(' but got " ++ show h) + +walkFunc :: (MonadFail m) => [Token] -> m (Ast, [Token]) +walkFunc [] = fail "Syntax error: '(' unexpected" +walkFunc (h:t) + | (Func n) <- h = walkParams n t [] + | otherwise = fail ("Syntax error: expected Function but got " ++ show h) + +walkParams :: (MonadFail m) => String -> [Token] -> [Ast] -> m (Ast, [Token]) +walkParams _ [] _ = fail "Syntax error: ')' not found" +walkParams n ts@(h:t) acc + | (Paren ')') <- h = return (Call n (reverse acc), t) + | otherwise = do + (el, t') <- walk ts + walkParams n t' (el : acc) + +------------------------------------- +-- Semantic Analysis: Ast -> Expr -- +------------------------------------- +data Expr + = Param String + | Antipode Expr + | ClosestPointOfApproach Expr + Expr + | CrossTrackDistance Expr + Expr + | DeltaBetween Expr + Expr + Expr + String + | DeltaV Expr + Expr + Expr + | Destination Expr + Expr + Expr + | Ecef Expr + Expr + Expr + | FrameB Expr + Expr + Expr + | FrameL Expr + | FrameN + | FinalBearing Expr + Expr + | FromEcef Expr + String + | Geo [Expr] + | GreatCircleE [Expr] + | InitialBearing Expr + Expr + | Intercept [Expr] + | Interpolate Expr + Expr + Double + | Intersections Expr + Expr + | InsideSurface [Expr] + | Mean [Expr] + | NedBetween Expr + Expr + String + | NedV Expr + Expr + Expr + | Position Expr + Expr + | SurfaceDistance Expr + Expr + | Target Expr + Expr + Expr + String + | TargetN Expr + Expr + String + | TrackE Expr + Expr + Expr + | ToEcef Expr + String + | ToNVector Expr + deriving (Show) + +transform :: (MonadFail m) => Ast -> m Expr +transform (Call "antipode" [e]) = fmap Antipode (transform e) +transform (Call "cpa" [e1, e2]) = do + t1 <- transform e1 + t2 <- transform e2 + return (ClosestPointOfApproach t1 t2) +transform (Call "crossTrackDistance" [e1, e2]) = do + p <- transform e1 + gc <- transform e2 + return (CrossTrackDistance p gc) +transform (Call "delta" [e1, e2, e3]) = do + p1 <- transform e1 + p2 <- transform e2 + p3 <- transform e3 + return (DeltaV p1 p2 p3) +transform (Call "deltaBetween" [e1, e2, e3]) = do + p1 <- transform e1 + p2 <- transform e2 + f <- transform e3 + return (DeltaBetween p1 p2 f "wgs84") +transform (Call "deltaBetween" [e1, e2, e3, Lit s]) = do + p1 <- transform e1 + p2 <- transform e2 + f <- transform e3 + return (DeltaBetween p1 p2 f s) +transform (Call "destination" [e1, e2, e3]) = do + p1 <- transform e1 + p2 <- transform e2 + p3 <- transform e3 + return (Destination p1 p2 p3) +transform (Call "ecef" [e1, e2, e3]) = do + p1 <- transform e1 + p2 <- transform e2 + p3 <- transform e3 + return (Ecef p1 p2 p3) +transform (Call "frameB" [e1, e2, e3]) = do + p1 <- transform e1 + p2 <- transform e2 + p3 <- transform e3 + return (FrameB p1 p2 p3) +transform (Call "frameL" [e]) = fmap FrameL (transform e) +transform (Call "frameN" []) = return FrameN +transform (Call "fromEcef" [e]) = do + p <- transform e + return (FromEcef p "wgs84") +transform (Call "fromEcef" [e, Lit s]) = do + p <- transform e + return (FromEcef p s) +transform (Call "finalBearing" [e1, e2]) = do + p1 <- transform e1 + p2 <- transform e2 + return (FinalBearing p1 p2) +transform (Call "geo" e) = do + ps <- mapM transform e + return (Geo ps) +transform (Call "greatCircle" e) = do + ps <- mapM transform e + return (GreatCircleE ps) +transform (Call "initialBearing" [e1, e2]) = do + p1 <- transform e1 + p2 <- transform e2 + return (InitialBearing p1 p2) +transform (Call "intercept" e) = do + ps <- mapM transform e + return (Intercept ps) +transform (Call "interpolate" [e1, e2, Lit s]) = do + p1 <- transform e1 + p2 <- transform e2 + d <- readDouble s + if d >= 0.0 && d <= 1.0 + then return (Interpolate p1 p2 d) + else fail "Semantic error: interpolate expects [0..1] as last argument" +transform (Call "intersections" [e1, e2]) = do + gc1 <- transform e1 + gc2 <- transform e2 + return (Intersections gc1 gc2) +transform (Call "insideSurface" e) = do + ps <- mapM transform e + return (InsideSurface ps) +transform (Call "mean" e) = do + ps <- mapM transform e + return (Mean ps) +transform (Call "ned" [e1, e2, e3]) = do + p1 <- transform e1 + p2 <- transform e2 + p3 <- transform e3 + return (NedV p1 p2 p3) +transform (Call "nedBetween" [e1, e2]) = do + p1 <- transform e1 + p2 <- transform e2 + return (NedBetween p1 p2 "wgs84") +transform (Call "nedBetween" [e1, e2, Lit s]) = do + p1 <- transform e1 + p2 <- transform e2 + return (NedBetween p1 p2 s) +transform (Call "position" [e1, e2]) = do + t <- transform e1 + d <- transform e2 + return (Position t d) +transform (Call "surfaceDistance" [e1, e2]) = do + p1 <- transform e1 + p2 <- transform e2 + return (SurfaceDistance p1 p2) +transform (Call "target" [e1, e2, e3]) = do + p0 <- transform e1 + f <- transform e2 + d <- transform e3 + return (Target p0 f d "wgs84") +transform (Call "target" [e1, e2, e3, Lit s]) = do + p0 <- transform e1 + f <- transform e2 + d <- transform e3 + return (Target p0 f d s) +transform (Call "targetN" [e1, e2]) = do + p0 <- transform e1 + d <- transform e2 + return (TargetN p0 d "wgs84") +transform (Call "targetN" [e1, e2, Lit s]) = do + p0 <- transform e1 + d <- transform e2 + return (TargetN p0 d s) +transform (Call "track" [e1, e2, e3]) = do + p0 <- transform e1 + b <- transform e2 + s <- transform e3 + return (TrackE p0 b s) +transform (Call "toEcef" [e]) = do + p <- transform e + return (ToEcef p "wgs84") +transform (Call "toEcef" [e, Lit s]) = do + p <- transform e + return (ToEcef p s) +transform (Call "toNVector" [e]) = fmap ToNVector (transform e) +transform (Call f e) = fail ("Semantic error: " ++ f ++ " does not accept " ++ show e) +transform (Lit s) = return (Param s) + +readDouble :: (MonadFail m) => String -> m Double +readDouble s = + case readMaybe s of + Just d -> return d + Nothing -> fail ("Unparsable double: " ++ s)
app/Main.hs view
@@ -14,6 +14,8 @@ import Data.List ((\\), dropWhileEnd, isPrefixOf) import Eval import Prelude hiding (lookup) +import Show +import State import System.Console.Haskeline search :: String -> [Completion] @@ -39,7 +41,7 @@ putStrLn ("jord interpreter, version " ++ jordVersion ++ ": https://github.com/ofmooseandmen/jord :? for help") - runInputT mySettings $ withInterrupt $ loop emptyVault + runInputT mySettings $ withInterrupt $ loop emptyState where loop state = do input <- handleInterrupt (return (Just "")) $ getInputLine "jord> " @@ -57,44 +59,60 @@ printS (Right "") = return () printS (Right r) = outputStrLn ("jord> " ++ r) -evalS :: String -> Vault -> (Either String String, Vault) -evalS s vault - | null s = (Right "", vault) - | head s == ':' = evalC w vault +evalS :: String -> State -> (Either String String, State) +evalS s state + | null s = (Right "", state) + | head s == ':' = evalC w state | (v:"=":e) <- w = - let r = eval (unwords e) vault - vault' = save r v vault - in (showR r, vault') - | otherwise = (showR (eval s vault), vault) + if v `elem` functions + then (Left (v ++ " is a reserved keyword"), state) + else let r = eval (unwords e) state + state' = save r v state + in (showR r state', state') + | otherwise = (showR (eval s state) state, state) where w = words s -evalC :: [String] -> Vault -> (Either String String, Vault) -evalC [":show", v] vault = (evalShow v vault, vault) -evalC [":delete", v] vault = evalDel (Just v) vault -evalC [":clear"] vault = evalDel Nothing vault -evalC [":help"] vault = (Right help, vault) -evalC [":?"] vault = (Right help, vault) -evalC c vault = (Left ("Unsupported command " ++ unwords c ++ "; :? for help"), vault) +evalC :: [String] -> State -> (Either String String, State) +evalC [":help"] state = (Right (help state), state) +evalC [":?"] state = (Right (help state), state) +evalC [":show", v] state = (evalShow v state, state) +evalC [":delete", v] state = (Right ("deleted var: " ++ v), delete v state) +evalC [":units", u1, u2] state = evalUnits [u1, u2] state +evalC [":units", u] state = evalUnits [u] state +evalC [":units"] state = showUnits state +evalC [":reset"] _ = (Right "REPL reset ", emptyState) +evalC c state = (Left ("Unsupported command " ++ unwords c ++ "; :? for help"), state) -evalShow :: String -> Vault -> Either String String -evalShow n vault = maybe (Left ("Unbound variable: " ++ n)) (Right . showVar n) (lookup n vault) +evalShow :: String -> State -> Either String String +evalShow n state = + maybe (Left ("Unbound variable: " ++ n)) (\v -> Right (showVar n v state)) (lookup n state) -evalDel :: Maybe String -> Vault -> (Either String String, Vault) -evalDel (Just n) vault = (Right ("deleted var: " ++ n), delete n vault) -evalDel Nothing _ = (Right "deleted all variable ", emptyVault) +evalUnits :: [String] -> State -> (Either String String, State) +evalUnits us s = showUnits (setUnits us s) -help :: String -help = +showUnits :: State -> (Either String String, State) +showUnits s = (Right ("Units:\n length = " ++ lengthUnit s ++ "\n speed = " ++ speedUnit s), s) + +help :: State -> String +help s = "\njord interpreter, version " ++ jordVersion ++ "\n" ++ "\n Commands available from the prompt:\n\n" ++ " :help, :? display this list of commands\n" ++ " :quit, :q quit jord\n" ++ - " :show {var} shows {var}\n" ++ - " :delete {var} deletes {var}\n" ++ - " :clear deletes all variable(s)\n" ++ + " :show {var} show {var}\n" ++ + " :delete {var} delete {var}\n" ++ + " :units length speed set length and speed units used for display\n" ++ + " see supported length and speed format\n" ++ + " currently: length = " ++ + lengthUnit s ++ + "; speed = " ++ + speedUnit s ++ + "\n" ++ + " :units show length and speed units used for display\n" ++ + " :reset reset REPL to default state (including deleting all variables)\n" ++ "\n Jord expressions:\n\n" ++ " (f x y) where f is one of function described below and x and y\n" ++ " are either parameters in one of the format described below or\n" ++ @@ -113,7 +131,7 @@ "\n Position calculations (Spherical Earth):\n\n" ++ " The following calculations assume a spherical earth model with a radius\n" ++ " derived from the WGS84 ellipsoid: " ++ - show r84 ++ + showLength r84 s ++ "\n" ++ "\n antipode pos antipodal point of pos\n" ++ " crossTrackDistance pos gc signed distance from pos to great circle gc\n" ++ @@ -130,13 +148,13 @@ "\n Kinematics calculations (Spherical Earth):\n\n" ++ " The following calculations assume a spherical earth model with a radius\n" ++ " derived from the WGS84 ellipsoid: " ++ - show r84 ++ + showLength r84 s ++ "\n" ++ "\n position track dur position of track after duration\n" ++ " cpa track1 track2 closest point of approach between two tracks\n" ++ " intercept track pos minimum speed of interceptor at pos to intercept target\n" ++ - " interceptBySpeed track pos spd time needed by interceptor at pos and travelling at spd to intercept target\n" ++ - " interceptByTime track pos dur speed needed by interceptor at pos to intercept target after duration\n" ++ + " intercept track pos spd time needed by interceptor at pos and travelling at spd to intercept target\n" ++ + " intercept track pos dur speed needed by interceptor at pos to intercept target after duration\n" ++ "\n Constructors and conversions:\n\n" ++ " ecef len len len earth-centred earth-fixed position from x, y, z lengths\n" ++ " ecef metres metres metres earth-centred earth-fixed position from x, y, z metres\n" ++ @@ -183,13 +201,13 @@ " jord> d = delta 3000 2000 100\n" ++ " jord> p0 = geo 49.66618 3.45063 0\n" ++ " jord> target p0 f d wgs84\n" -save :: Result -> String -> Vault -> Vault -save (Right v) k vault = insert k v vault -save _ _ vault = vault +save :: Result -> String -> State -> State +save (Right v) k state = insert k v state +save _ _ state = state -showR :: Result -> Either String String -showR (Left err) = Left err -showR (Right v) = Right (show v) +showR :: Result -> State -> Either String String +showR (Left err) _ = Left err +showR (Right v) s = Right (showV v s) -showVar :: String -> Value -> String -showVar n v = n ++ "=" ++ show v +showVar :: String -> Value -> State -> String +showVar n v s = n ++ "=" ++ showV v s
+ app/Show.hs view
@@ -0,0 +1,101 @@+-- | +-- Module: Show +-- Copyright: (c) 2018 Cedric Liegeois +-- License: BSD3 +-- Maintainer: Cedric Liegeois <ofmooseandmen@yahoo.fr> +-- Stability: experimental +-- Portability: portable +-- +-- Show 'Value's. +-- +module Show + ( showV + ) where + +import Data.Geo.Jord +import Data.List (intercalate) +import State + +-- | show value. +showV :: Value -> State -> String +showV (Ang a) _ = "angle: " ++ showAng a +showV (Bool b) _ = show b +showV (Cpa c) s = + "closest point of approach:" ++ + "\n time : " ++ + show (cpaTime c) ++ + "\n distance: " ++ + showLength (cpaDistance c) s ++ + "\n pos1 : " ++ + showLl (fromNVector . cpaPosition1 $ c :: LatLong) ++ + "\n pos2 : " ++ showLl (fromNVector . cpaPosition2 $ c :: LatLong) +showV (Dlt d) s = + "Delta:" ++ + "\n x: " ++ + showLength (dx d) s ++ + "\n y: " ++ showLength (dy d) s ++ "\n z: " ++ showLength (dz d) s +showV (Dur d) _ = "duration: " ++ show d +showV (Double d) _ = show d +showV (Em m) _ = "Earth model: " ++ show m +showV (Ep p) s = + "ECEF:" ++ + "\n x: " ++ + showLength (ex p) s ++ + "\n y: " ++ showLength (ey p) s ++ "\n z: " ++ showLength (ez p) s +showV (FrmB y p r) _ = + "Body (vehicle) frame:" ++ + "\n yaw : " ++ + showAng y ++ "\n pitch: " ++ showAng p ++ "\n roll : " ++ showAng r +showV (FrmL w) _ = "Local frame:" ++ "\n wander azimuth: " ++ showAng w +showV FrmN _ = "North, East, Down frame" +showV (Gc gc) _ = "great circle: " ++ show gc +showV (Gp g) s = "latlong: " ++ showLl ll ++ "\n height : " ++ showLength h s + where + ll = pos g + h = height g +showV (Intp i) s = + "intercept:" ++ + "\n time : " ++ + show (interceptTime i) ++ + "\n distance : " ++ + showLength (interceptDistance i) s ++ + "\n pos : " ++ + showLl (fromNVector . interceptPosition $ i :: LatLong) ++ + "\n interceptor speed : " ++ + showSpeed (interceptorSpeed i) s ++ + "\n interceptor bearing: " ++ showAng (interceptorBearing i) +showV (Len l) s = "length: " ++ showLength l s +showV (Ned d) s = + "NED:" ++ + "\n north: " ++ + showLength (north d) s ++ + "\n east : " ++ showLength (east d) s ++ "\n down : " ++ showLength (down d) s +showV (Np nv) s = + "n-vector: " ++ + show x ++ ", " ++ show y ++ ", " ++ show z ++ "\n height : " ++ showLength h s + where + v = vec (pos nv) + x = vx v + y = vy v + z = vz v + h = height nv +showV (Trk t) s = + "track:" ++ + "\n position: " ++ + showLl (fromNVector . trackPos $ t :: LatLong) ++ + "\n height : " ++ + showLength (height . trackPos $ t) s ++ + "\n bearing : " ++ + showAng (trackBearing t) ++ "\n speed : " ++ showSpeed (trackSpeed t) s +showV (Spd spd) s = "speed: " ++ showSpeed spd s +showV (Vals []) _ = "empty" +showV (Vals vs) s = "\n " ++ intercalate "\n\n " (map (`showV` s) vs) + +showAng :: Angle -> String +showAng a = show a ++ " (" ++ show (toDecimalDegrees a) ++ ")" + +showLl :: LatLong -> String +showLl ll = + show ll ++ + " (" ++ + show (toDecimalDegrees (latitude ll)) ++ ", " ++ show (toDecimalDegrees (longitude ll)) ++ ")"
+ app/State.hs view
@@ -0,0 +1,132 @@+-- | +-- Module: State +-- Copyright: (c) 2018 Cedric Liegeois +-- License: BSD3 +-- Maintainer: Cedric Liegeois <ofmooseandmen@yahoo.fr> +-- Stability: experimental +-- Portability: portable +-- +-- REPL state. +-- +module State + ( State + , emptyState + , Value(..) + , setUnits + , lengthUnit + , speedUnit + , showLength + , showSpeed + , insert + , delete + , lookup + ) where + +import Control.Applicative +import Data.Char (isLetter) +import Data.Geo.Jord +import Data.List hiding (delete, insert, lookup) +import Data.Maybe (fromMaybe) +import Prelude hiding (lookup) + +-- | REPL state. +data State = + State Units + Vault + +-- | A value accepted and returned by 'eval'. +data Value + = Ang Angle -- ^ angle + | Bool Bool -- ^ boolean + | Cpa (Cpa (AngularPosition NVector)) -- ^ CPA + | Dlt Delta -- ^ delta + | Dur Duration -- ^ duration + | Double Double -- ^ double + | Ep EcefPosition -- ^ ECEF position + | Em Earth -- ^ earth model + | FrmB Angle + Angle + Angle -- ^ yaw, pitch and roll of Body frame + | FrmL Angle -- ^ wander azimuth of Local frame + | FrmN -- ^ North, east, down frame + | Gc GreatCircle -- ^ great circle + | Gp (AngularPosition LatLong) -- ^ latitude, longitude and height + | Intp (Intercept (AngularPosition NVector)) -- ^ Intercept + | Len Length -- ^ length + | Ned Ned -- ^ north east down + | Np (AngularPosition NVector) -- ^ n-vector and height + | Spd Speed -- ^ speed + | Trk (Track (AngularPosition NVector)) -- ^ track + | Vals [Value] -- array of values + +-- | A location for 'Value's to be shared by successive evalations. +newtype Vault = + Vault [(String, Value)] + +-- | functions to show values with a pre-defined unit. +data Units = + Units (Length -> String) + (Speed -> String) + +-- | empty state: length in kilometres, speed in kilometres/hour and empty vault. +emptyState :: State +emptyState = State (Units len spd) (Vault []) + where + len l = show (toKilometres l) ++ "km" + spd s = show (toKilometresPerHour s) ++ "km/h" + +-- | sets length and or speed units, ignore all invalid units. +setUnits :: [String] -> State -> State +setUnits us (State (Units l s) v) = State (Units (fromMaybe l lu) (fromMaybe s su)) v + where + lu = foldl (<|>) Nothing (fmap toLenUnit us) + su = foldl (<|>) Nothing (fmap toSpdUnit us) + +toLenUnit :: String -> Maybe (Length -> String) +toLenUnit "m" = Just (\l -> show (toMetres l) ++ "m") +toLenUnit "km" = Just (\l -> show (toKilometres l) ++ "km") +toLenUnit "nm" = Just (\l -> show (toNauticalMiles l) ++ "nm") +toLenUnit "ft" = Just (\l -> show (toFeet l) ++ "ft") +toLenUnit _ = Nothing + +toSpdUnit :: String -> Maybe (Speed -> String) +toSpdUnit "m/s" = Just (\l -> show (toMetresPerSecond l) ++ "m/s") +toSpdUnit "km/h" = Just (\l -> show (toKilometresPerHour l) ++ "km/h") +toSpdUnit "mph" = Just (\l -> show (toMilesPerHour l) ++ "mph") +toSpdUnit "kt" = Just (\l -> show (toKnots l) ++ "kt") +toSpdUnit "ft/s" = Just (\l -> show (toFeetPerSecond l) ++ "ft/s") +toSpdUnit _ = Nothing + +-- | length unit. +lengthUnit :: State -> String +lengthUnit s = filter isLetter (showLength zero s) + +-- | speed unit. +speedUnit :: State -> String +speedUnit s = filter (\c -> isLetter c || c == '/') (showSpeed zero s) + +-- | show length in selected unit. +showLength :: Length -> State -> String +showLength l (State (Units len _) _) = len l + +-- | show speed in selected unit. +showSpeed :: Speed -> State -> String +showSpeed s (State (Units _ spd) _) = spd s + +-- | @insert k v state@ inserts value @v@ for key @k@. Overwrites any previous value. +insert :: String -> Value -> State -> State +insert k v (State u vault) = State u (Vault (e ++ [(k, v)])) + where + Vault e = delete' k vault + +-- | @lookup k state@ looks up the value of key @k@ in the vault. +lookup :: String -> State -> Maybe Value +lookup k (State _ (Vault es)) = fmap snd (find (\e -> fst e == k) es) + +-- | @delete k state@ deletes key @k@ from the vault. +delete :: String -> State -> State +delete k (State u v) = State u (delete' k v) + +-- | @delete k vault@ deletes key @k@ from the vault. +delete' :: String -> Vault -> Vault +delete' k (Vault es) = Vault (filter (\e -> fst e /= k) es)
jord.cabal view
@@ -2,10 +2,10 @@ -- -- see: https://github.com/sol/hpack -- --- hash: 080acd4c3e6004551525d8dfe07551b2c77ba7644db59ffa50b4b70d53061a64 +-- hash: 7710cef1545699151a4207c29a268327d2f17ff0a5c3256a90cd3698d3fbff38 name: jord -version: 0.4.0.0 +version: 0.4.1.0 synopsis: Geographical Position Calculations description: Please see the README on GitHub at <https://github.com/ofmooseandmen/jord#readme> category: Geography @@ -59,7 +59,8 @@ main-is: Main.hs other-modules: Eval - Paths_jord + Show + State hs-source-dirs: app ghc-options: -Wall
src/Data/Geo/Jord.hs view
@@ -8,6 +8,7 @@ -- -- Geographic position calculations (distance, bearing, intersection, etc...) on great circles using -- the algorithms described in <http://www.navlab.net/Publications/A_Nonsingular_Horizontal_Position_Representation.pdf Gade, K. (2010). A Non-singular Horizontal Position Representation>. +-- and in <https://calhoun.nps.edu/bitstream/handle/10945/29516/sometacticalalgo00shud.pdf Shudde, Rex H. (1986). Some tactical algorithms for spherical geometry> -- -- See <http://www.navlab.net/nvector Position calculations - simple and exact solutions> -- @@ -54,4 +55,4 @@ -- | version. jordVersion :: String -jordVersion = "0.4.0.0" +jordVersion = "0.4.1.0"
src/Data/Geo/Jord/Frames.hs view
@@ -87,24 +87,12 @@ -- -- * Comments: The frame is fixed to the vehicle. -- -data FrameB = - FrameB Angle - Angle - Angle - Vector3d - deriving (Eq, Show) - --- | body yaw angle (vertical axis). -yaw :: FrameB -> Angle -yaw (FrameB a _ _ _) = a - --- | body pitch angle (transverse axis). -pitch :: FrameB -> Angle -pitch (FrameB _ a _ _) = a - --- | body roll angle (longitudinal axis). -roll :: FrameB -> Angle -roll (FrameB _ _ a _) = a +data FrameB = FrameB + { yaw :: Angle -- ^ body yaw angle (vertical axis). + , pitch :: Angle -- ^ body pitch angle (transverse axis). + , roll :: Angle -- ^ body roll angle (longitudinal axis). + , bOrg :: Vector3d -- ^ frame origin (n-vector). + } deriving (Eq, Show) -- | 'FrameB' from given yaw, pitch, roll, position (origin) and earth model. frameB :: (ETransform a) => Angle -> Angle -> Angle -> a -> Earth -> FrameB @@ -136,14 +124,10 @@ -- this angle is called the wander azimuth angle. The L-frame is well suited for general -- calculations, as it is non-singular. -- -data FrameL = - FrameL Angle - LatLong - deriving (Eq, Show) - --- | wander azimuth: angle between x-axis of the frame L and the north direction. -wanderAzimuth :: FrameL -> Angle -wanderAzimuth (FrameL a _) = a +data FrameL = FrameL + { wanderAzimuth :: Angle -- ^ wander azimuth: angle between x-axis of the frame L and the north direction. + , lOrg :: LatLong -- ^ frame origin (latlong). + } deriving (Eq, Show) -- | R_EL: frame L to Earth instance Frame FrameL where @@ -174,9 +158,9 @@ -- the x- and y-axes are not defined here. Hence, this coordinate frame is not suitable for -- general calculations. -- -newtype FrameN = - FrameN Vector3d - deriving (Eq, Show) +newtype FrameN = FrameN + { nOrg :: Vector3d -- ^ frame origin (n-vector). + } deriving (Eq, Show) -- | R_EN: frame N to Earth instance Frame FrameN where
src/Data/Geo/Jord/Kinematics.hs view
@@ -12,98 +12,98 @@ -- <http://www.navlab.net/Publications/A_Nonsingular_Horizontal_Position_Representation.pdf Gade, K. (2010). A Non-singular Horizontal Position Representation> -- and in <https://calhoun.nps.edu/bitstream/handle/10945/29516/sometacticalalgo00shud.pdf Shudde, Rex H. (1986). Some tactical algorithms for spherical geometry> -- -module Data.Geo.Jord.Kinematics- (- -- * The 'Track' type.- Track(..)+module Data.Geo.Jord.Kinematics + ( + -- * The 'Track' type. + Track(..) -- * The 'Course' type. - , Course+ , Course -- * The 'Cpa' type. - , Cpa- , cpaTime- , cpaDistance- , cpaPosition1- , cpaPosition2+ , Cpa + , cpaTime + , cpaDistance + , cpaPosition1 + , cpaPosition2 -- * The 'Intercept' type. - , Intercept- , interceptTime- , interceptDistance- , interceptPosition- , interceptorBearing- , interceptorSpeed+ , Intercept + , interceptTime + , interceptDistance + , interceptPosition + , interceptorBearing + , interceptorSpeed -- * Calculations - , course- , position- , position84- , cpa- , cpa84- , intercept- , intercept84- , interceptBySpeed- , interceptBySpeed84- , interceptByTime- , interceptByTime84- ) where--import Control.Applicative-import Data.Geo.Jord.Angle-import Data.Geo.Jord.AngularPosition-import Data.Geo.Jord.Duration-import Data.Geo.Jord.Earth-import Data.Geo.Jord.Geodetics-import Data.Geo.Jord.LatLong-import Data.Geo.Jord.Length-import Data.Geo.Jord.NVector-import Data.Geo.Jord.Quantity-import Data.Geo.Jord.Speed-import Data.Geo.Jord.Transformation-import Data.Geo.Jord.Vector3d-+ , course + , position + , position84 + , cpa + , cpa84 + , intercept + , intercept84 + , interceptBySpeed + , interceptBySpeed84 + , interceptByTime + , interceptByTime84 + ) where + +import Control.Applicative +import Data.Geo.Jord.Angle +import Data.Geo.Jord.AngularPosition +import Data.Geo.Jord.Duration +import Data.Geo.Jord.Earth +import Data.Geo.Jord.Geodetics +import Data.Geo.Jord.LatLong +import Data.Geo.Jord.Length +import Data.Geo.Jord.NVector +import Data.Geo.Jord.Quantity +import Data.Geo.Jord.Speed +import Data.Geo.Jord.Transformation +import Data.Geo.Jord.Vector3d + -- | 'Track' represents the state of a vehicle by its current position, bearing and speed. -data Track a = Track+data Track a = Track { trackPos :: a -- ^ position of the track. , trackBearing :: Angle -- ^ bearing of the track. , trackSpeed :: Speed -- ^ speed of the track. - } deriving (Eq, Show)-+ } deriving (Eq, Show) + -- | 'GreatCircle' from track. -instance (NTransform a, Show a) => IsGreatCircle (Track a) where- greatCircleE t = greatCircleE (trackPos t, trackBearing t)-+instance (NTransform a, Show a) => IsGreatCircle (Track a) where + greatCircleE t = greatCircleE (trackPos t, trackBearing t) + -- | 'Course' represents the cardinal direction in which the vehicle is to be steered. -newtype Course =- Course Vector3d- deriving (Eq, Show)--instance IsVector3d Course where- vec (Course v) = v-+newtype Course = + Course Vector3d + deriving (Eq, Show) + +instance IsVector3d Course where + vec (Course v) = v + -- | Time to, and distance at, closest point of approach (CPA) as well as position of both tracks at CPA. -data Cpa a = Cpa+data Cpa a = Cpa { cpaTime :: Duration -- ^ time to CPA. , cpaDistance :: Length -- ^ distance at CPA. , cpaPosition1 :: a -- ^ position of track 1 at CPA. , cpaPosition2 :: a -- ^ position of track 2 at CPA. - } deriving (Eq, Show)-+ } deriving (Eq, Show) + -- | Time, distance and position of intercept as well as speed and initial bearing of interceptor. -data Intercept a = Intercept+data Intercept a = Intercept { interceptTime :: Duration -- ^ time to intercept. , interceptDistance :: Length -- ^ distance at intercept. , interceptPosition :: a -- ^ position of intercept. , interceptorBearing :: Angle -- ^ initial bearing of interceptor. , interceptorSpeed :: Speed -- ^ speed of interceptor. - } deriving (Eq, Show)-+ } deriving (Eq, Show) + -- | @course p b@ computes the course of a vehicle currently at position @p@ and following bearing @b@. -course :: (NTransform a) => a -> Angle -> Course-course p b = Course (Vector3d (vz (head r)) (vz (r !! 1)) (vz (r !! 2)))- where- ll = nvectorToLatLong . pos . toNVector $ p- lat = latitude ll- lon = longitude ll- r = mdot (mdot (rz (negate' lon)) (ry lat)) (rx b)-+course :: (NTransform a) => a -> Angle -> Course +course p b = Course (Vector3d (vz (head r)) (vz (r !! 1)) (vz (r !! 2))) + where + ll = nvectorToLatLong . pos . toNVector $ p + lat = latitude ll + lon = longitude ll + r = mdot (mdot (rz (negate' lon)) (ry lat)) (rx b) + -- | @position t d r@ computes the position of a track @t@ after duration @d@ has elapsed and using the earth radius @r@. -- -- @ @@ -113,13 +113,13 @@ -- let p1 = decimalLatLongHeight 53.1882691 0.1332741 (metres 15000) -- position (Track p0 b s) (hours 1) r84 = p1 -- @ -position :: (NTransform a) => Track a -> Duration -> Length -> a-position (Track p0 b s) d = position' p0 s (course p0 b) (toSeconds d)-+position :: (NTransform a) => Track a -> Duration -> Length -> a +position (Track p0 b s) d = position' p0 s (course p0 b) (toSeconds d) + -- | 'position' using the mean radius of the WGS84 reference ellipsoid. -position84 :: (NTransform a) => Track a -> Duration -> a-position84 t d = position t d r84-+position84 :: (NTransform a) => Track a -> Duration -> a +position84 t d = position t d r84 + -- | @cpa t1 t2 r@ computes the closest point of approach between tracks @t1@ and @t2@ and using the earth radius @r@. -- -- @ @@ -135,23 +135,23 @@ -- fmap cpaTime c = Just (milliseconds 11396155) -- fmap cpaDistance c = Just (kilometres 124.2317453) -- @ -cpa :: (Eq a, NTransform a) => Track a -> Track a -> Length -> Maybe (Cpa a)-cpa (Track p1 b1 s1) (Track p2 b2 s2) r- | p1 == p2 = Just (Cpa zero zero p1 p2)- | t < 0 = Nothing- | otherwise = Just (Cpa (seconds t) d cp1 cp2)- where- c1 = course p1 b1- c2 = course p2 b2- t = timeToCpa p1 c1 s1 p2 c2 s2 r- cp1 = position' p1 s1 c1 t r- cp2 = position' p2 s2 c2 t r- d = surfaceDistance cp1 cp2 r-+cpa :: (Eq a, NTransform a) => Track a -> Track a -> Length -> Maybe (Cpa a) +cpa (Track p1 b1 s1) (Track p2 b2 s2) r + | p1 == p2 = Just (Cpa zero zero p1 p2) + | t < 0 = Nothing + | otherwise = Just (Cpa (seconds t) d cp1 cp2) + where + c1 = course p1 b1 + c2 = course p2 b2 + t = timeToCpa p1 c1 s1 p2 c2 s2 r + cp1 = position' p1 s1 c1 t r + cp2 = position' p2 s2 c2 t r + d = surfaceDistance cp1 cp2 r + -- | 'cpa' using the mean radius of the WGS84 reference ellipsoid. -cpa84 :: (Eq a, NTransform a) => Track a -> Track a -> Maybe (Cpa a)-cpa84 t1 t2 = cpa t1 t2 r84-+cpa84 :: (Eq a, NTransform a) => Track a -> Track a -> Maybe (Cpa a) +cpa84 t1 t2 = cpa t1 t2 r84 + -- | @intercept t p r@ computes the __minimum__ speed of interceptor at -- position @p@ needed for an intercept with target track @t@ to take place -- using the earth radius @r@. Intercept time, position, distance and interceptor @@ -169,34 +169,34 @@ -- fmap interceptorSpeed i = Just (knots 52.837096) -- fmap interceptTime i = Just (seconds 5947.698) -- @ -intercept :: (Eq a, NTransform a) => Track a -> a -> Length -> Maybe (Intercept a)-intercept t@(Track tp tb ts) p r = interceptByTime t p (seconds d) r- where- ct0 = course tp tb- d = timeToIntercept tp ts ct0 p r-+intercept :: (Eq a, NTransform a) => Track a -> a -> Length -> Maybe (Intercept a) +intercept t@(Track tp tb ts) p r = interceptByTime t p (seconds d) r + where + ct0 = course tp tb + d = timeToIntercept tp ts ct0 p r + -- | 'intercept' using the mean radius of the WGS84 reference ellipsoid. -intercept84 :: (Eq a, NTransform a) => Track a -> a -> Maybe (Intercept a)-intercept84 t p = intercept t p r84---- | @interceptBySpeed t p s r@ computes the time needed by interceptor at--- position @p@ and travelling at speed @s@ to intercept target track @t@--- using the earth radius @r@. Returns 'Nothing' if intercept--- cannot be achieved e.g.:------ * interceptor and target are at the same position------ * interceptor speed is below minimum speed-interceptBySpeed :: (Eq a, NTransform a) => Track a -> a -> Speed -> Length -> Maybe (Intercept a)-interceptBySpeed t@(Track tp tb ts) p s r = interceptByTime t p (seconds d) r- where- ct0 = course tp tb- d = timeToInterceptSpeed tp ts ct0 p s r---- | 'interceptBySpeed' using the mean radius of the WGS84 reference ellipsoid.-interceptBySpeed84 :: (Eq a, NTransform a) => Track a -> a -> Speed -> Maybe (Intercept a)-interceptBySpeed84 t p s = interceptBySpeed t p s r84-+intercept84 :: (Eq a, NTransform a) => Track a -> a -> Maybe (Intercept a) +intercept84 t p = intercept t p r84 + +-- | @interceptBySpeed t p s r@ computes the time needed by interceptor at +-- position @p@ and travelling at speed @s@ to intercept target track @t@ +-- using the earth radius @r@. Returns 'Nothing' if intercept +-- cannot be achieved e.g.: +-- +-- * interceptor and target are at the same position +-- +-- * interceptor speed is below minimum speed +interceptBySpeed :: (Eq a, NTransform a) => Track a -> a -> Speed -> Length -> Maybe (Intercept a) +interceptBySpeed t@(Track tp tb ts) p s r = interceptByTime t p (seconds d) r + where + ct0 = course tp tb + d = timeToInterceptSpeed tp ts ct0 p s r + +-- | 'interceptBySpeed' using the mean radius of the WGS84 reference ellipsoid. +interceptBySpeed84 :: (Eq a, NTransform a) => Track a -> a -> Speed -> Maybe (Intercept a) +interceptBySpeed84 t p s = interceptBySpeed t p s r84 + -- | @interceptByTime t p d r@ computes the speed of interceptor at -- position @p@ needed for an intercept with target track @t@ to take place -- after duration @d@ and using the earth radius @r@. Returns 'Nothing' if @@ -213,232 +213,230 @@ -- fmap interceptDistance i = Just (metres 1015302.3815) -- fmap interceptTime i = Just (seconds 2700) -- @ -interceptByTime :: (Eq a, NTransform a) => Track a -> a -> Duration -> Length -> Maybe (Intercept a)-interceptByTime t p d r- | toMilliseconds d <= 0 = Nothing- | trackPos t == p = Nothing- | otherwise = fmap (\b -> Intercept d idist ipos b is) ib- where- ipos = position t d r- idist = surfaceDistance p ipos r- ib = initialBearing p ipos <|> initialBearing p (trackPos t)- is = metresPerSecond (toMetres idist / toSeconds d)-+interceptByTime :: (Eq a, NTransform a) => Track a -> a -> Duration -> Length -> Maybe (Intercept a) +interceptByTime t p d r + | toMilliseconds d <= 0 = Nothing + | trackPos t == p = Nothing + | otherwise = fmap (\b -> Intercept d idist ipos b is) ib + where + ipos = position t d r + idist = surfaceDistance p ipos r + ib = initialBearing p ipos <|> initialBearing p (trackPos t) + is = metresPerSecond (toMetres idist / toSeconds d) + -- | 'interceptByTime' using the mean radius of the WGS84 reference ellipsoid. -interceptByTime84 :: (Eq a, NTransform a) => Track a -> a -> Duration -> Maybe (Intercept a)-interceptByTime84 t p d = interceptByTime t p d r84-+interceptByTime84 :: (Eq a, NTransform a) => Track a -> a -> Duration -> Maybe (Intercept a) +interceptByTime84 t p d = interceptByTime t p d r84 + -- | position from speed course and seconds. -position' :: (NTransform a) => a -> Speed -> Course -> Double -> Length -> a-position' p0 s c sec r = fromNVector (nvectorHeight (nvector (vx v1) (vy v1) (vz v1)) h0)- where- nv0 = toNVector p0- v0 = vec . pos $nv0- h0 = height nv0- v1 = position'' v0 s (vec c) sec r-+position' :: (NTransform a) => a -> Speed -> Course -> Double -> Length -> a +position' p0 s c sec r = fromNVector (nvectorHeight (nvector (vx v1) (vy v1) (vz v1)) h0) + where + nv0 = toNVector p0 + v0 = vec . pos $nv0 + h0 = height nv0 + v1 = position'' v0 s (vec c) sec r + -- | position from speed course and seconds. -position'' :: Vector3d -> Speed -> Vector3d -> Double -> Length -> Vector3d-position'' v0 s c sec r = v1- where- w = toMetresPerSecond s / toMetres r- v1 = vadd (vscale v0 (cos (w * sec))) (vscale c (sin (w * sec)))-+position'' :: Vector3d -> Speed -> Vector3d -> Double -> Length -> Vector3d +position'' v0 s c sec r = v1 + where + w = toMetresPerSecond s / toMetres r + v1 = vadd (vscale v0 (cos (w * sec))) (vscale c (sin (w * sec))) + -- | time to CPA. -timeToCpa :: (NTransform a) => a -> Course -> Speed -> a -> Course -> Speed -> Length -> Double-timeToCpa p1 c1 s1 p2 c2 s2 r = cpaNrRec v10 c10 w1 v20 c20 w2 0 0- where- v10 = vec . pos . toNVector $ p1- c10 = vec c1- rm = toMetres r- w1 = toMetresPerSecond s1 / rm- v20 = vec . pos . toNVector $ p2- c20 = vec c2- w2 = toMetresPerSecond s2 / rm---- | time to intercept with minimum speed. -timeToIntercept :: (NTransform a) => a -> Speed -> Course -> a -> Length -> Double-timeToIntercept p2 s2 c20 p1 r = intMinNrRec v10 v20 (vec c20) s2 w2 r s0 t0 0- where- v10 = vec . pos . toNVector $ p1- v20 = vec . pos . toNVector $ p2- s2mps = toMetresPerSecond s2- rm = toMetres r- w2 = s2mps / rm- s0 = ad v10 v20- t0 = rm * s0 / s2mps---- | time to intercept with speed.-timeToInterceptSpeed :: (NTransform a) => a -> Speed -> Course -> a -> Speed -> Length -> Double-timeToInterceptSpeed p2 s2 c20 p1 s1 r = intSpdNrRec v10 w1 v20 (vec c20) s2 w2 r s0 t0 0- where- v10 = vec . pos . toNVector $ p1- v20 = vec . pos . toNVector $ p2- s1mps = toMetresPerSecond s1- s2mps = toMetresPerSecond s2- rm = toMetres r- w2 = s2mps / rm- w1 = s1mps / rm- s0 = ad v10 v20- t0 = rm * s0 / s2mps--rx :: Angle -> [Vector3d]-rx a = [Vector3d 1 0 0, Vector3d 0 c s, Vector3d 0 (-s) c]- where- c = cos' a- s = sin' a--ry :: Angle -> [Vector3d]-ry a = [Vector3d c 0 (-s), Vector3d 0 1 0, Vector3d s 0 c]- where- c = cos' a- s = sin' a--rz :: Angle -> [Vector3d]-rz a = [Vector3d c s 0, Vector3d (-s) c 0, Vector3d 0 0 1]- where- c = cos' a- s = sin' a--cpaA :: Vector3d -> Vector3d -> Double -> Vector3d -> Vector3d -> Double -> Double-cpaA v10 c10 w1 v20 c20 w2 = negate (vdot (vscale v10 w1) c20 + vdot (vscale v20 w2) c10)--cpaB :: Vector3d -> Vector3d -> Double -> Vector3d -> Vector3d -> Double -> Double-cpaB v10 c10 w1 v20 c20 w2 = vdot (vscale c10 w1) v20 + vdot (vscale c20 w2) v10--cpaC :: Vector3d -> Vector3d -> Double -> Vector3d -> Vector3d -> Double -> Double-cpaC v10 c10 w1 v20 c20 w2 = negate (vdot (vscale v10 w1) v20 - vdot (vscale c20 w2) c10)--cpaD :: Vector3d -> Vector3d -> Double -> Vector3d -> Vector3d -> Double -> Double-cpaD v10 c10 w1 v20 c20 w2 = vdot (vscale c10 w1) c20 - vdot (vscale v20 w2) v10--cpaFt :: Double -> Double -> Double -> Double -> Double -> Double -> Double -> Double -> Double-cpaFt cw1t cw2t sw1t sw2t a b c d =- a * sw1t * sw2t + b * cw1t * cw2t + c * sw1t * cw2t + d * cw1t * sw2t--cpaDft ::- Double- -> Double- -> Double- -> Double- -> Double- -> Double- -> Double- -> Double- -> Double- -> Double- -> Double-cpaDft w1 w2 cw1t cw2t sw1t sw2t a b c d =- negate ((c * w2 + d * w1) * sw1t * sw2t) + (d * w2 + c * w1) * cw1t * cw2t +- (a * w2 - b * w1) * sw1t * cw2t -- (b * w2 - a * w1) * cw1t * sw2t--cpaStep :: Vector3d -> Vector3d -> Double -> Vector3d -> Vector3d -> Double -> Double -> Double-cpaStep v10 c10 w1 v20 c20 w2 t =- cpaFt cw1t cw2t sw1t sw2t a b c d / cpaDft w1 w2 cw1t cw2t sw1t sw2t a b c d- where- cw1t = cos (w1 * t)- cw2t = cos (w2 * t)- sw1t = sin (w1 * t)- sw2t = sin (w2 * t)- a = cpaA v10 c10 w1 v20 c20 w2- b = cpaB v10 c10 w1 v20 c20 w2- c = cpaC v10 c10 w1 v20 c20 w2- d = cpaD v10 c10 w1 v20 c20 w2-+timeToCpa :: (NTransform a) => a -> Course -> Speed -> a -> Course -> Speed -> Length -> Double +timeToCpa p1 c1 s1 p2 c2 s2 r = cpaNrRec v10 c10 w1 v20 c20 w2 0 0 + where + v10 = vec . pos . toNVector $ p1 + c10 = vec c1 + rm = toMetres r + w1 = toMetresPerSecond s1 / rm + v20 = vec . pos . toNVector $ p2 + c20 = vec c2 + w2 = toMetresPerSecond s2 / rm + +-- | time to intercept with minimum speed +timeToIntercept :: (NTransform a) => a -> Speed -> Course -> a -> Length -> Double +timeToIntercept p2 s2 c20 p1 r = intMinNrRec v10 v20 (vec c20) s2 w2 r s0 t0 0 + where + v10 = vec . pos . toNVector $ p1 + v20 = vec . pos . toNVector $ p2 + s2mps = toMetresPerSecond s2 + rm = toMetres r + w2 = s2mps / rm + s0 = ad v10 v20 + t0 = rm * s0 / s2mps + +-- | time to intercept with speed. +timeToInterceptSpeed :: (NTransform a) => a -> Speed -> Course -> a -> Speed -> Length -> Double +timeToInterceptSpeed p2 s2 c20 p1 s1 r = intSpdNrRec v10 w1 v20 (vec c20) s2 w2 r s0 t0 0 + where + v10 = vec . pos . toNVector $ p1 + v20 = vec . pos . toNVector $ p2 + rm = toMetres r + w2 = toMetresPerSecond s2 / rm + w1 = toMetresPerSecond s1 / rm + t0 = 0.1 + s0 = ad v10 v20 + +rx :: Angle -> [Vector3d] +rx a = [Vector3d 1 0 0, Vector3d 0 c s, Vector3d 0 (-s) c] + where + c = cos' a + s = sin' a + +ry :: Angle -> [Vector3d] +ry a = [Vector3d c 0 (-s), Vector3d 0 1 0, Vector3d s 0 c] + where + c = cos' a + s = sin' a + +rz :: Angle -> [Vector3d] +rz a = [Vector3d c s 0, Vector3d (-s) c 0, Vector3d 0 0 1] + where + c = cos' a + s = sin' a + +cpaA :: Vector3d -> Vector3d -> Double -> Vector3d -> Vector3d -> Double -> Double +cpaA v10 c10 w1 v20 c20 w2 = negate (vdot (vscale v10 w1) c20 + vdot (vscale v20 w2) c10) + +cpaB :: Vector3d -> Vector3d -> Double -> Vector3d -> Vector3d -> Double -> Double +cpaB v10 c10 w1 v20 c20 w2 = vdot (vscale c10 w1) v20 + vdot (vscale c20 w2) v10 + +cpaC :: Vector3d -> Vector3d -> Double -> Vector3d -> Vector3d -> Double -> Double +cpaC v10 c10 w1 v20 c20 w2 = negate (vdot (vscale v10 w1) v20 - vdot (vscale c20 w2) c10) + +cpaD :: Vector3d -> Vector3d -> Double -> Vector3d -> Vector3d -> Double -> Double +cpaD v10 c10 w1 v20 c20 w2 = vdot (vscale c10 w1) c20 - vdot (vscale v20 w2) v10 + +cpaFt :: Double -> Double -> Double -> Double -> Double -> Double -> Double -> Double -> Double +cpaFt cw1t cw2t sw1t sw2t a b c d = + a * sw1t * sw2t + b * cw1t * cw2t + c * sw1t * cw2t + d * cw1t * sw2t + +cpaDft :: + Double + -> Double + -> Double + -> Double + -> Double + -> Double + -> Double + -> Double + -> Double + -> Double + -> Double +cpaDft w1 w2 cw1t cw2t sw1t sw2t a b c d = + negate ((c * w2 + d * w1) * sw1t * sw2t) + (d * w2 + c * w1) * cw1t * cw2t + + (a * w2 - b * w1) * sw1t * cw2t - + (b * w2 - a * w1) * cw1t * sw2t + +cpaStep :: Vector3d -> Vector3d -> Double -> Vector3d -> Vector3d -> Double -> Double -> Double +cpaStep v10 c10 w1 v20 c20 w2 t = + cpaFt cw1t cw2t sw1t sw2t a b c d / cpaDft w1 w2 cw1t cw2t sw1t sw2t a b c d + where + cw1t = cos (w1 * t) + cw2t = cos (w2 * t) + sw1t = sin (w1 * t) + sw2t = sin (w2 * t) + a = cpaA v10 c10 w1 v20 c20 w2 + b = cpaB v10 c10 w1 v20 c20 w2 + c = cpaC v10 c10 w1 v20 c20 w2 + d = cpaD v10 c10 w1 v20 c20 w2 + -- | Newton-Raphson for CPA time. -- note: this should always converge to the minimum time given -- that the assumptions made in the proof of quadratic convergence are met -cpaNrRec ::- Vector3d -> Vector3d -> Double -> Vector3d -> Vector3d -> Double -> Double -> Int -> Double-cpaNrRec v10 c10 w1 v20 c20 w2 ti i+cpaNrRec :: + Vector3d -> Vector3d -> Double -> Vector3d -> Vector3d -> Double -> Double -> Int -> Double +cpaNrRec v10 c10 w1 v20 c20 w2 ti i | i == 50 = -1.0 -- no convergence - | abs fi < 1e-12 = ti1- | otherwise = cpaNrRec v10 c10 w1 v20 c20 w2 ti1 (i + 1)- where- fi = cpaStep v10 c10 w1 v20 c20 w2 ti- ti1 = ti - fi-+ | abs fi < 1e-12 = ti1 + | otherwise = cpaNrRec v10 c10 w1 v20 c20 w2 ti1 (i + 1) + where + fi = cpaStep v10 c10 w1 v20 c20 w2 ti + ti1 = ti - fi + -- | Newton-Raphson for min speed intercept. -- note: this should always converge to the minimum time given -- that the assumptions made in the proof of quadratic convergence are met -intMinNrRec ::- Vector3d- -> Vector3d- -> Vector3d- -> Speed- -> Double- -> Length- -> Double- -> Double- -> Int- -> Double-intMinNrRec v10 v20 c20 s2 w2 r si ti i+intMinNrRec :: + Vector3d + -> Vector3d + -> Vector3d + -> Speed + -> Double + -> Length + -> Double + -> Double + -> Int + -> Double +intMinNrRec v10 v20 c20 s2 w2 r si ti i | i == 50 = -1.0 -- no convergence - | abs fi < 1e-12 = ti1- | otherwise = intMinNrRec v10 v20 c20 s2 w2 r si1 ti1 (i + 1)- where- fi = intMinStep v10 v20 c20 w2 si ti- ti1 = ti - fi- v2t = position'' v20 s2 c20 ti1 r- si1 = ad v10 v2t--intMinStep :: Vector3d -> Vector3d -> Vector3d -> Double -> Double -> Double -> Double-intMinStep v10 v20 c20 w2 s t =- dsdt s w2 v10v20 v10c20 sinw2t cosw2t / d2sdt2 s w2 v10v20 v10c20 sinw2t cosw2t- where- cosw2t = cos (w2 * t)- sinw2t = sin (w2 * t)- v10v20 = vdot v10 v20- v10c20 = vdot v10 c20---- | Newton-Raphson for speed intercept.--- note: this should always converge to the minimum time given--- that the assumptions made in the proof of quadratic convergence are met-intSpdNrRec ::- Vector3d- -> Double- -> Vector3d- -> Vector3d- -> Speed- -> Double- -> Length- -> Double- -> Double- -> Int- -> Double-intSpdNrRec v10 w1 v20 c20 s2 w2 r si ti i- | i == 50 = -1.0 -- no convergence- | abs fi < 1e-12 = ti1- | otherwise = intSpdNrRec v10 w1 v20 c20 s2 w2 r si1 ti1 (i + 1)- where- fi = intSpdStep v10 w1 v20 c20 w2 si ti- ti1 = ti - fi- v2t = position'' v20 s2 c20 ti1 r- si1 = ad v10 v2t--intSpdStep :: Vector3d -> Double -> Vector3d -> Vector3d -> Double -> Double -> Double -> Double-intSpdStep v10 w1 v20 c20 w2 s t = f / df- where- cosw2t = cos (w2 * t)- sinw2t = sin (w2 * t)- v10v20 = vdot v10 v20- v10c20 = vdot v10 c20- f = s / t - w1- df = (1.0 / t) * (dsdt s w2 v10v20 v10c20 sinw2t cosw2t - s / t)--dsdt :: Double -> Double -> Double -> Double -> Double -> Double -> Double-dsdt s w2 v10v20 v10c20 sinw2t cosw2t =- ((-1.0) / sin s) * ((-w2) * (v10v20 * sinw2t - v10c20 * cosw2t))--d2sdt2 :: Double -> Double -> Double -> Double -> Double -> Double -> Double-d2sdt2 s w2 v10v20 v10c20 sinw2t cosw2t =- ((-1.0) / sin s) * (cos s / (sins * sins) * x10d2x2dt2 * x10d2x2dt2 + x10d2x2dt2)- where- sins = sin s- x10d2x2dt2 = negate (w2 * w2) * (v10v20 * cosw2t + v10c20 * sinw2t)-+ | abs fi < 1e-12 = ti1 + | otherwise = intMinNrRec v10 v20 c20 s2 w2 r si1 ti1 (i + 1) + where + fi = intMinStep v10 v20 c20 w2 si ti + ti1 = ti - fi + v2t = position'' v20 s2 c20 ti1 r + si1 = ad v10 v2t + +intMinStep :: Vector3d -> Vector3d -> Vector3d -> Double -> Double -> Double -> Double +intMinStep v10 v20 c20 w2 s t = + dsdt s w2 v10v20 v10c20 sinw2t cosw2t / d2sdt2 s w2 v10v20 v10c20 sinw2t cosw2t + where + cosw2t = cos (w2 * t) + sinw2t = sin (w2 * t) + v10v20 = vdot v10 v20 + v10c20 = vdot v10 c20 + +-- | Newton-Raphson for speed intercept. +-- note: this should always converge to the minimum time given +-- that the assumptions made in the proof of quadratic convergence are met +intSpdNrRec :: + Vector3d + -> Double + -> Vector3d + -> Vector3d + -> Speed + -> Double + -> Length + -> Double + -> Double + -> Int + -> Double +intSpdNrRec v10 w1 v20 c20 s2 w2 r si ti i + | i == 50 = -1.0 -- no convergence + | abs fi < 1e-12 = ti1 + | otherwise = intSpdNrRec v10 w1 v20 c20 s2 w2 r si1 ti1 (i + 1) + where + fi = intSpdStep v10 w1 v20 c20 w2 si ti + ti1 = ti - fi + v2t = position'' v20 s2 c20 ti1 r + si1 = ad v10 v2t + +intSpdStep :: Vector3d -> Double -> Vector3d -> Vector3d -> Double -> Double -> Double -> Double +intSpdStep v10 w1 v20 c20 w2 s t = f / df + where + cosw2t = cos (w2 * t) + sinw2t = sin (w2 * t) + v10v20 = vdot v10 v20 + v10c20 = vdot v10 c20 + f = s / t - w1 + df = (1.0 / t) * (dsdt s w2 v10v20 v10c20 sinw2t cosw2t - s / t) + +dsdt :: Double -> Double -> Double -> Double -> Double -> Double -> Double +dsdt s w2 v10v20 v10c20 sinw2t cosw2t = + ((-1.0) / sin s) * ((-w2) * (v10v20 * sinw2t - v10c20 * cosw2t)) + +d2sdt2 :: Double -> Double -> Double -> Double -> Double -> Double -> Double +d2sdt2 s w2 v10v20 v10c20 sinw2t cosw2t = + ((-1.0) / sin s) * (cos s / (sins * sins) * x10d2x2dt2 * x10d2x2dt2 + x10d2x2dt2) + where + sins = sin s + x10d2x2dt2 = negate (w2 * w2) * (v10v20 * cosw2t + v10c20 * sinw2t) + -- | angle in radians between 2 n-vectors (as vector3d), copied from Geodetics -- without the sign and returing radians. -ad :: Vector3d -> Vector3d -> Double-ad v1 v2 = atan2 (vnorm (vcross v1 v2)) (vdot v1 v2)+ad :: Vector3d -> Vector3d -> Double +ad v1 v2 = atan2 (vnorm (vcross v1 v2)) (vdot v1 v2)
src/Data/Geo/Jord/LatLong.hs view
@@ -11,7 +11,9 @@ module Data.Geo.Jord.LatLong ( -- * The 'LatLong' type - LatLong(latitude, longitude) + LatLong + , latitude + , longitude -- * Smart constructors , latLong , latLongE @@ -39,8 +41,8 @@ -- | Horizontal position defined by its geodetic latitude and longitude. data LatLong = LatLong - { latitude :: Angle - , longitude :: Angle + { latitude :: Angle -- ^ geodetic latitude + , longitude :: Angle -- ^ longitude } deriving (Eq) -- | See 'readLatLong'.
src/Data/Geo/Jord/Length.hs view
@@ -118,10 +118,10 @@ length = do v <- number skipSpaces - u <- string "m" <|> string "km" <|> string "Nm" <|> string "ft" + u <- string "m" <|> string "km" <|> string "nm" <|> string "ft" case u of "m" -> return (metres v) "km" -> return (kilometres v) - "Nm" -> return (nauticalMiles v) + "nm" -> return (nauticalMiles v) "ft" -> return (feet v) _ -> pfail
test/Data/Geo/Jord/KinematicsSpec.hs view
@@ -1,191 +1,196 @@-module Data.Geo.Jord.KinematicsSpec- ( spec- ) where--import Data.Geo.Jord-import Data.Maybe (fromJust)-import Test.Hspec--spec :: Spec-spec =- describe "kinematics" $ do- describe "position" $ do- it "computes position at t from p0, bearing and speed" $ do- let p0 = latLongHeight (readLatLong "531914N0014347W") (metres 15000)- let p1 = decimalLatLongHeight 53.1882691 0.1332741 (metres 15000)- let t = Track p0 (decimalDegrees 96.0217) (kilometresPerHour 124.8)- position84 t (hours 1) `shouldBe` p1- it "handles poles" $+module Data.Geo.Jord.KinematicsSpec + ( spec + ) where + +import Data.Geo.Jord +import Data.Maybe (fromJust) +import Test.Hspec + +spec :: Spec +spec = + describe "kinematics" $ do + describe "position" $ do + it "computes position at t from p0, bearing and speed" $ do + let p0 = latLongHeight (readLatLong "531914N0014347W") (metres 15000) + let p1 = decimalLatLongHeight 53.1882691 0.1332741 (metres 15000) + let t = Track p0 (decimalDegrees 96.0217) (kilometresPerHour 124.8) + position84 t (hours 1) `shouldBe` p1 + it "handles poles" $ do -- distance between poles assuming a spherical earth (WGS84) = 20015.114352200002km -- track at north pole travelling at 20015.114352200002km/h and true north reaches the -- south pole after 1 hour. - do- let t = Track (decimalLatLong 90 0) zero (kilometresPerHour 20015.114352200002)- position84 t (hours 1) `shouldBe` decimalLatLong (-90) 180.0- it "return p0 if speed is 0" $ do- let p0 = latLongHeight (readLatLong "531914N0014347W") (metres 15000)- let t = Track p0 (decimalDegrees 96.0217) zero- position84 t (hours 1) `shouldBe` p0- it "return p0 if duration is 0" $ do- let p0 = latLongHeight (readLatLong "531914N0014347W") (metres 15000)- let t = Track p0 (decimalDegrees 96.0217) (kilometresPerHour 124.8)- position84 t zero `shouldBe` p0- describe "cpa" $ do- it "handles trailing tracks" $ do- let p1 = decimalLatLong 20 30- let px = destination84 p1 (decimalDegrees 20) (kilometres 1)- let p2 = interpolate p1 px 0.25- let b1 = fromJust (initialBearing p1 px)- let b2 = fromJust (initialBearing p2 px)- let t1 = Track p1 b1 (knots 400)- let t2 = Track p2 b2 (knots 400)- let c = cpa84 t1 t2+ let t = Track (decimalLatLong 90 0) zero (kilometresPerHour 20015.114352200002) + position84 t (hours 1) `shouldBe` decimalLatLong (-90) 180.0 + it "return p0 if speed is 0" $ do + let p0 = latLongHeight (readLatLong "531914N0014347W") (metres 15000) + let t = Track p0 (decimalDegrees 96.0217) zero + position84 t (hours 1) `shouldBe` p0 + it "return p0 if duration is 0" $ do + let p0 = latLongHeight (readLatLong "531914N0014347W") (metres 15000) + let t = Track p0 (decimalDegrees 96.0217) (kilometresPerHour 124.8) + position84 t zero `shouldBe` p0 + describe "cpa" $ do + it "handles trailing tracks" $ do + let p1 = decimalLatLong 20 30 + let px = destination84 p1 (decimalDegrees 20) (kilometres 1) + let p2 = interpolate p1 px 0.25 + let b1 = fromJust (initialBearing p1 px) + let b2 = fromJust (initialBearing p2 px) + let t1 = Track p1 b1 (knots 400) + let t2 = Track p2 b2 (knots 400) + let c = cpa84 t1 t2 -- any time is correct but it should be close to zero since that's -- our initial value - fmap (\r -> toMilliseconds (cpaTime r) < 5000) c `shouldBe` Just True- fmap cpaDistance c `shouldBe` Just (metres 250.0036)- it "handles heading tracks" $ do- let p1 = decimalLatLong 20 30- let p2 = decimalLatLong 21 31- let b1 = fromJust (initialBearing p1 p2)- let b2 = fromJust (initialBearing p2 p1)- let t1 = Track p1 b1 (knots 400)- let t2 = Track p2 b2 (knots 400)- let c = cpa84 t1 t2+ fmap (\r -> toMilliseconds (cpaTime r) < 5000) c `shouldBe` Just True + fmap cpaDistance c `shouldBe` Just (metres 250.0036) + it "handles heading tracks" $ do + let p1 = decimalLatLong 20 30 + let p2 = decimalLatLong 21 31 + let b1 = fromJust (initialBearing p1 p2) + let b2 = fromJust (initialBearing p2 p1) + let t1 = Track p1 b1 (knots 400) + let t2 = Track p2 b2 (knots 400) + let c = cpa84 t1 t2 -- distance between p1 and p2 = 152.354309 km -- speed = 740.8 km/h -- time = 152.354309 / 740.8 / 2 - fmap cpaTime c `shouldBe` Just (milliseconds 370191)- fmap cpaDistance c `shouldBe` Just zero- it "handles tracks at the same position" $ do- let p = decimalLatLong 20 30- let t1 = Track p (decimalDegrees 45) (knots 300)- let t2 = Track p (decimalDegrees 135) (knots 500)- let c = cpa84 t1 t2- fmap cpaTime c `shouldBe` Just zero- fmap cpaDistance c `shouldBe` Just zero- it "computes time to CPA, positions and distance at CPA" $ do- let p1 = decimalLatLong 20 (-60)- let b1 = decimalDegrees 10- let s1 = knots 15- let p2 = decimalLatLong 34 (-50)- let b2 = decimalDegrees 220- let s2 = knots 300- let t1 = Track p1 b1 s1- let t2 = Track p2 b2 s2- let c = cpa84 t1 t2- fmap cpaTime c `shouldBe` Just (milliseconds 11396155)- fmap cpaDistance c `shouldBe` Just (kilometres 124.2317453)- it "returns Nothing if time to CPA is in the past" $ do- let t1 = Track (decimalLatLong 30 30) (decimalDegrees 45) (knots 400)- let t2 = Track (decimalLatLong 30.01 30) (decimalDegrees 315) (knots 400)- cpa84 t1 t2 `shouldBe` Nothing- describe "intercept" $ do- it "returns Nothing if target and interceptor are at the same position" $- intercept84- (Track (decimalLatLong 30 30) (decimalDegrees 45) (knots 400))- (decimalLatLong 30 30) `shouldBe`- Nothing- it "returns Nothing if interceptor is on the great circle of target and behind" $ do- -- minimum speed would be ideally target speed + epsillon.- let ip = decimalLatLong 20 30- let px = destination84 ip (decimalDegrees 20) (kilometres 1)- let tp = interpolate ip px 0.25- let b = fromJust (initialBearing tp px)- let t = Track tp b (knots 400)- intercept84 t ip `shouldBe` Nothing- it "handles interceptor on the great circle of target and in front" $ do- let tp = decimalLatLong 20 30- let px = destination84 tp (decimalDegrees 20) (kilometres 1)- let ip = interpolate tp px 0.25- let b = fromJust (initialBearing tp px)- let t = Track tp b (knots 400)- let i = intercept84 t ip- fmap interceptorSpeed i `shouldBe` Just zero- fmap interceptPosition i `shouldBe` Just ip- fmap interceptTime i `shouldBe` Just (seconds 1.215)- it "returns the minimum speed required for intercept to take place" $ do- let t = Track (decimalLatLong 34 (-50)) (decimalDegrees 220) (knots 600)- let ip = decimalLatLong 20 (-60)- let i = intercept84 t ip- fmap interceptorSpeed i `shouldBe` Just (knots 52.837096)- fmap interceptTime i `shouldBe` Just (seconds 5947.698)- let interceptor =- Track- ip- (fromJust (fmap interceptorBearing i))- (fromJust (fmap interceptorSpeed i))- fmap interceptPosition i `shouldBe`- Just (position84 interceptor (fromJust (fmap interceptTime i)))- describe "interceptBySpeed" $ do- it "returns Nothing if target and interceptor are at the same position" $- interceptBySpeed84- (Track (decimalLatLong 30 30) (decimalDegrees 45) (knots 400))- (decimalLatLong 30 30)- (knots 400) `shouldBe`- Nothing- it "returns Nothing if interceptor speed is below minimum speed" $ do- let t = Track (decimalLatLong 34 (-50)) (decimalDegrees 220) (knots 600)- let ip = decimalLatLong 20 (-60)- interceptBySpeed84 t ip (knots 50) `shouldBe` Nothing- it "handles interceptor on the great circle of target and behind" $ do- let ip = decimalLatLong 20 30- let px = destination84 ip (decimalDegrees 20) (kilometres 1)- let tp = interpolate ip px 0.25- let b = fromJust (initialBearing tp px)- let t = Track tp b (metresPerSecond 400)- let i = interceptBySpeed84 t ip (metresPerSecond 500)- fmap interceptTime i `shouldBe` Just (seconds 2.5)- it "returns the time needed for intercept to take place" $ do- let t = Track (decimalLatLong 34 (-50)) (decimalDegrees 220) (knots 600)- let ip = decimalLatLong 20 (-60)- let i = interceptBySpeed84 t ip (knots 700)- fmap interceptTime i `shouldBe` Just (seconds 2764.692)- fmap interceptorBearing i `shouldBe` Just (decimalDegrees 25.93541277)- fmap interceptDistance i `shouldBe` Just (kilometres 995.5960805999999)- describe "interceptByTime" $ do- it "returns Nothing if duration is zero" $- interceptByTime84- (Track (decimalLatLong 30 30) (decimalDegrees 45) (knots 400))- (decimalLatLong 34 (-50))- zero `shouldBe`- Nothing- it "returns Nothing if duration is negative" $- interceptByTime84- (Track (decimalLatLong 30 30) (decimalDegrees 45) (knots 400))- (decimalLatLong 34 (-50))- (seconds (-1)) `shouldBe`- Nothing- it "returns Nothing if target and interceptor are at the same position" $- interceptByTime84- (Track (decimalLatLong 30 30) (decimalDegrees 45) (knots 400))- (decimalLatLong 30 30)- (seconds 10) `shouldBe`- Nothing- it "returns the speed needed for intercept to take place" $ do- let t = Track (decimalLatLong 34 (-50)) (decimalDegrees 220) (knots 600)- let ip = decimalLatLong 20 (-60)- let d = seconds 2700- let i = interceptByTime84 t ip d- fmap interceptorSpeed i `shouldBe` Just (knots 730.959238)- fmap interceptorBearing i `shouldBe` Just (decimalDegrees 26.1199030)- fmap interceptPosition i `shouldBe` Just (decimalLatLong 28.1366797 (-55.4559475))- fmap interceptDistance i `shouldBe` Just (metres 1015302.3815)- fmap interceptTime i `shouldBe` Just (seconds 2700)- it "handles the poles" $ do- -- distance between poles assuming a spherical earth (WGS84) = 20015.114352200002km- -- target at north pole travelling at 500km/h and true north can be intercepted from- -- the south pole by an interceptor travelling at ~ 19515.114352200002km/h and 180 degrees.- let t = Track (decimalLatLong 90 0) zero (kilometresPerHour 500)- let ip = decimalLatLong (-90) 0- let i = interceptByTime84 t ip (seconds 3600)- fmap interceptorSpeed i `shouldBe` Just (kilometresPerHour 19515.11434)- fmap interceptorBearing i `shouldBe` Just (decimalDegrees 180)- it "handles the interceptor being at the intercept position at t" $ do- let tp = decimalLatLong 34 (-50)- let t = Track tp (decimalDegrees 220) (knots 600)- let d = seconds 3600- let ip = position84 t d- let i = interceptByTime84 t ip d- fmap interceptorSpeed i `shouldBe` Just zero- fmap interceptorBearing i `shouldBe` initialBearing ip tp+ fmap cpaTime c `shouldBe` Just (milliseconds 370191) + fmap cpaDistance c `shouldBe` Just zero + it "handles tracks at the same position" $ do + let p = decimalLatLong 20 30 + let t1 = Track p (decimalDegrees 45) (knots 300) + let t2 = Track p (decimalDegrees 135) (knots 500) + let c = cpa84 t1 t2 + fmap cpaTime c `shouldBe` Just zero + fmap cpaDistance c `shouldBe` Just zero + it "computes time to CPA, positions and distance at CPA" $ do + let p1 = decimalLatLong 20 (-60) + let b1 = decimalDegrees 10 + let s1 = knots 15 + let p2 = decimalLatLong 34 (-50) + let b2 = decimalDegrees 220 + let s2 = knots 300 + let t1 = Track p1 b1 s1 + let t2 = Track p2 b2 s2 + let c = cpa84 t1 t2 + fmap cpaTime c `shouldBe` Just (milliseconds 11396155) + fmap cpaDistance c `shouldBe` Just (kilometres 124.2317453) + it "returns Nothing if time to CPA is in the past" $ do + let t1 = Track (decimalLatLong 30 30) (decimalDegrees 45) (knots 400) + let t2 = Track (decimalLatLong 30.01 30) (decimalDegrees 315) (knots 400) + cpa84 t1 t2 `shouldBe` Nothing + describe "intercept" $ do + it "returns Nothing if target and interceptor are at the same position" $ + intercept84 + (Track (decimalLatLong 30 30) (decimalDegrees 45) (knots 400)) + (decimalLatLong 30 30) `shouldBe` + Nothing + it "returns Nothing if interceptor is on the great circle of target and behind" $ do + -- minimum speed would be ideally target speed + epsillon. + let ip = decimalLatLong 20 30 + let px = destination84 ip (decimalDegrees 20) (kilometres 1) + let tp = interpolate ip px 0.25 + let b = fromJust (initialBearing tp px) + let t = Track tp b (knots 400) + intercept84 t ip `shouldBe` Nothing + it "handles interceptor on the great circle of target and in front" $ do + let tp = decimalLatLong 20 30 + let px = destination84 tp (decimalDegrees 20) (kilometres 1) + let ip = interpolate tp px 0.25 + let b = fromJust (initialBearing tp px) + let t = Track tp b (knots 400) + let i = intercept84 t ip + fmap interceptorSpeed i `shouldBe` Just zero + fmap interceptPosition i `shouldBe` Just ip + fmap interceptTime i `shouldBe` Just (seconds 1.215) + it "returns the minimum speed required for intercept to take place" $ do + let t = Track (decimalLatLong 34 (-50)) (decimalDegrees 220) (knots 600) + let ip = decimalLatLong 20 (-60) + let i = intercept84 t ip + fmap interceptorSpeed i `shouldBe` Just (knots 52.837096) + fmap interceptTime i `shouldBe` Just (seconds 5947.698) + let interceptor = + Track + ip + (fromJust (fmap interceptorBearing i)) + (fromJust (fmap interceptorSpeed i)) + fmap interceptPosition i `shouldBe` + Just (position84 interceptor (fromJust (fmap interceptTime i))) + describe "interceptBySpeed" $ do + it "returns Nothing if target and interceptor are at the same position" $ + interceptBySpeed84 + (Track (decimalLatLong 30 30) (decimalDegrees 45) (knots 400)) + (decimalLatLong 30 30) + (knots 400) `shouldBe` + Nothing + it "returns Nothing if interceptor speed is below minimum speed" $ do + let t = Track (decimalLatLong 34 (-50)) (decimalDegrees 220) (knots 600) + let ip = decimalLatLong 20 (-60) + interceptBySpeed84 t ip (knots 50) `shouldBe` Nothing + it "handles interceptor on the great circle of target and behind" $ do + let ip = decimalLatLong 20 30 + let px = destination84 ip (decimalDegrees 20) (kilometres 1) + let tp = interpolate ip px 0.25 + let b = fromJust (initialBearing tp px) + let t = Track tp b (metresPerSecond 400) + let i = interceptBySpeed84 t ip (metresPerSecond 500) + fmap interceptTime i `shouldBe` Just (seconds 2.5) + it "returns the speed needed for intercept to take place" $ do + let t = Track (decimalLatLong 34 (-50)) (decimalDegrees 220) (knots 600) + let ip = decimalLatLong 20 (-60) + let i = interceptBySpeed84 t ip (knots 700) + fmap interceptTime i `shouldBe` Just (seconds 2764.692) + fmap interceptorBearing i `shouldBe` Just (decimalDegrees 25.93541277) + fmap interceptDistance i `shouldBe` Just (kilometres 995.5960805999999) + it "returns the same as intercept when called with minimum speed" $ do + let t = Track (decimalLatLong 45 50) (decimalDegrees 54) (knots 500) + let ip = decimalLatLong 70 30 + let mi = intercept84 t ip + let i = interceptBySpeed84 t ip (fromJust (fmap interceptorSpeed mi)) + fmap interceptTime i `shouldBe` fmap interceptTime mi + describe "interceptByTime" $ do + it "returns Nothing if duration is zero" $ + interceptByTime84 + (Track (decimalLatLong 30 30) (decimalDegrees 45) (knots 400)) + (decimalLatLong 34 (-50)) + zero `shouldBe` + Nothing + it "returns Nothing if duration is negative" $ + interceptByTime84 + (Track (decimalLatLong 30 30) (decimalDegrees 45) (knots 400)) + (decimalLatLong 34 (-50)) + (seconds (-1)) `shouldBe` + Nothing + it "returns Nothing if target and interceptor are at the same position" $ + interceptByTime84 + (Track (decimalLatLong 30 30) (decimalDegrees 45) (knots 400)) + (decimalLatLong 30 30) + (seconds 10) `shouldBe` + Nothing + it "returns the speed needed for intercept to take place" $ do + let t = Track (decimalLatLong 34 (-50)) (decimalDegrees 220) (knots 600) + let ip = decimalLatLong 20 (-60) + let d = seconds 2700 + let i = interceptByTime84 t ip d + fmap interceptorSpeed i `shouldBe` Just (knots 730.959238) + fmap interceptorBearing i `shouldBe` Just (decimalDegrees 26.1199030) + fmap interceptPosition i `shouldBe` Just (decimalLatLong 28.1366797 (-55.4559475)) + fmap interceptDistance i `shouldBe` Just (metres 1015302.3815) + fmap interceptTime i `shouldBe` Just (seconds 2700) + it "handles the poles" $ do + -- distance between poles assuming a spherical earth (WGS84) = 20015.114352200002km + -- target at north pole travelling at 500km/h and true north can be intercepted from + -- the south pole by an interceptor travelling at ~ 19515.114352200002km/h and 180 degrees. + let t = Track (decimalLatLong 90 0) zero (kilometresPerHour 500) + let ip = decimalLatLong (-90) 0 + let i = interceptByTime84 t ip (seconds 3600) + fmap interceptorSpeed i `shouldBe` Just (kilometresPerHour 19515.11434) + fmap interceptorBearing i `shouldBe` Just (decimalDegrees 180) + it "handles the interceptor being at the intercept position at t" $ do + let tp = decimalLatLong 34 (-50) + let t = Track tp (decimalDegrees 220) (knots 600) + let d = seconds 3600 + let ip = position84 t d + let i = interceptByTime84 t ip d + fmap interceptorSpeed i `shouldBe` Just zero + fmap interceptorBearing i `shouldBe` initialBearing ip tp
test/Data/Geo/Jord/LengthSpec.hs view
@@ -10,7 +10,7 @@ describe "Reading valid lengths" $ do it "reads -15.2m" $ readLength "-15.2m" `shouldBe` metres (-15.2) it "reads 154km" $ readLength "154km" `shouldBe` kilometres 154 - it "reads 1000Nm" $ readLength "1000Nm" `shouldBe` nauticalMiles 1000 + it "reads 1000nm" $ readLength "1000nm" `shouldBe` nauticalMiles 1000 it "reads 25000ft" $ readLength "25000ft" `shouldBe` feet 25000 describe "Reading invalid lengths" $ do it "fails to read 5" $ readLengthE "5" `shouldBe` Left "couldn't read length 5"