diff --git a/ChangeLog.md b/ChangeLog.md --- a/ChangeLog.md +++ b/ChangeLog.md @@ -1,3 +1,8 @@ +### 0.4.2.0 + +- Fixed intercept +- jord-exe renamed jord-repl + ### 0.4.1.0 - Fixed interceptBySpeed diff --git a/README.md b/README.md --- a/README.md +++ b/README.md @@ -9,13 +9,13 @@ ## What is this? Jord is a [Haskell](https://www.haskell.org) library that implements various geographical position calculations using the algorithms described in [Gade, K. (2010). A Non-singular Horizontal Position Representation](http://www.navlab.net/Publications/A_Nonsingular_Horizontal_Position_Representation.pdf) and in -[Shudde, Rex H. (1986). Some tactical algorithms for spherical geometry](https://calhoun.nps.edu/bitstream/handle/10945/29516/sometacticalalgo00shud.pdf) +[Shudde, Rex H. (1986). Some tactical algorithms for spherical geometry](https://calhoun.nps.edu/bitstream/handle/10945/29516/sometacticalalgo00shud.pdf): -- Transformation between ECEF (earth-centred, earth-fixed), Latitude/Longitude and N-Vector positions for spherical and ellipsoidal earth model. -- Transformation between Latitude/Longitude and N-Vector positions. -- Local, Body and North, East, Down Frames: delta between positions, target position from reference position and delta. -- Geodetics: surface distance, initial & final bearing, interpolated position, great circle intersections, cross track distance, ... -- Kinematics: position from p0, bearing and speed, closest point of approach between tracks, intercept (time, speed, min speed). +- transformation between ECEF (earth-centred, earth-fixed), latitude/longitude and [*n*-vector](https://www.navlab.net/nvector) positions for spherical and ellipsoidal earth model, +- transformation between latitude/longitude and *n*-vector positions, +- local, body and north, east, down Frames: delta between positions, target position from reference position and delta, +- geodetics: surface distance, initial & final bearing, interpolated position, great circle intersections, cross track distance, ..., +- kinematics: position from p0, bearing and speed, closest point of approach between tracks, intercept (time, speed, minimum speed). ## How do I build it? diff --git a/jord.cabal b/jord.cabal --- a/jord.cabal +++ b/jord.cabal @@ -1,97 +1,97 @@ --- This file has been generated from package.yaml by hpack version 0.28.2. --- --- see: https://github.com/sol/hpack --- --- hash: 7710cef1545699151a4207c29a268327d2f17ff0a5c3256a90cd3698d3fbff38 - -name: jord -version: 0.4.1.0 -synopsis: Geographical Position Calculations -description: Please see the README on GitHub at -category: Geography -stability: experimental -homepage: https://github.com/ofmooseandmen/jord -bug-reports: https://github.com/ofmooseandmen/jord/issues -author: Cedric Liegeois -maintainer: Cedric Liegeois -copyright: 2018 Cedric Liegeois -license: BSD3 -license-file: LICENSE -build-type: Simple -cabal-version: >= 1.10 -extra-source-files: - ChangeLog.md - README.md - -source-repository head - type: git - location: https://github.com/ofmooseandmen/jord - -library - exposed-modules: - Data.Geo.Jord - Data.Geo.Jord.Angle - Data.Geo.Jord.AngularPosition - Data.Geo.Jord.Duration - Data.Geo.Jord.Earth - Data.Geo.Jord.EcefPosition - Data.Geo.Jord.Frames - Data.Geo.Jord.Geodetics - Data.Geo.Jord.Kinematics - Data.Geo.Jord.LatLong - Data.Geo.Jord.Length - Data.Geo.Jord.NVector - Data.Geo.Jord.Quantity - Data.Geo.Jord.Rotation - Data.Geo.Jord.Speed - Data.Geo.Jord.Transformation - Data.Geo.Jord.Vector3d - other-modules: - Data.Geo.Jord.Parse - hs-source-dirs: - src - ghc-options: -Wall - build-depends: - base >=4.9 && <5 - default-language: Haskell2010 - -executable jord-exe - main-is: Main.hs - other-modules: - Eval - Show - State - hs-source-dirs: - app - ghc-options: -Wall - build-depends: - base >=4.9 && <5 - , haskeline >=0.7 && <0.8 - , jord - default-language: Haskell2010 - -test-suite jord-test - type: exitcode-stdio-1.0 - main-is: Spec.hs - other-modules: - Data.Geo.Jord.AngleSpec - Data.Geo.Jord.DurationSpec - Data.Geo.Jord.EarthSpec - Data.Geo.Jord.FramesSpec - Data.Geo.Jord.GeodeticsSpec - Data.Geo.Jord.KinematicsSpec - Data.Geo.Jord.LatLongSpec - Data.Geo.Jord.LengthSpec - Data.Geo.Jord.RotationSpec - Data.Geo.Jord.SpeedSpec - Data.Geo.Jord.TransformationSpec - Paths_jord - hs-source-dirs: - test - ghc-options: -Wall - build-depends: - HUnit ==1.6.* - , base >=4.9 && <5 - , hspec ==2.* - , jord - default-language: Haskell2010 +-- This file has been generated from package.yaml by hpack version 0.28.2. +-- +-- see: https://github.com/sol/hpack +-- +-- hash: 8bc0043cf9b7f8b4b203c71df609cfa6cc47735768a22d31029d559d8f49ad70 + +name: jord +version: 0.4.2.0 +synopsis: Geographical Position Calculations +description: Please see the README on GitHub at +category: Geography +stability: experimental +homepage: https://github.com/ofmooseandmen/jord +bug-reports: https://github.com/ofmooseandmen/jord/issues +author: Cedric Liegeois +maintainer: Cedric Liegeois +copyright: 2018 Cedric Liegeois +license: BSD3 +license-file: LICENSE +build-type: Simple +cabal-version: >= 1.10 +extra-source-files: + ChangeLog.md + README.md + +source-repository head + type: git + location: https://github.com/ofmooseandmen/jord + +library + exposed-modules: + Data.Geo.Jord + Data.Geo.Jord.Angle + Data.Geo.Jord.AngularPosition + Data.Geo.Jord.Duration + Data.Geo.Jord.Earth + Data.Geo.Jord.EcefPosition + Data.Geo.Jord.Frames + Data.Geo.Jord.Geodetics + Data.Geo.Jord.Kinematics + Data.Geo.Jord.LatLong + Data.Geo.Jord.Length + Data.Geo.Jord.NVector + Data.Geo.Jord.Quantity + Data.Geo.Jord.Rotation + Data.Geo.Jord.Speed + Data.Geo.Jord.Transformation + Data.Geo.Jord.Vector3d + other-modules: + Data.Geo.Jord.Parse + hs-source-dirs: + src + ghc-options: -Wall + build-depends: + base >=4.9 && <5 + default-language: Haskell2010 + +executable jord-repl + main-is: Main.hs + other-modules: + Eval + Show + State + hs-source-dirs: + app + ghc-options: -Wall + build-depends: + base >=4.9 && <5 + , haskeline >=0.7 && <0.8 + , jord + default-language: Haskell2010 + +test-suite jord-test + type: exitcode-stdio-1.0 + main-is: Spec.hs + other-modules: + Data.Geo.Jord.AngleSpec + Data.Geo.Jord.DurationSpec + Data.Geo.Jord.EarthSpec + Data.Geo.Jord.FramesSpec + Data.Geo.Jord.GeodeticsSpec + Data.Geo.Jord.KinematicsSpec + Data.Geo.Jord.LatLongSpec + Data.Geo.Jord.LengthSpec + Data.Geo.Jord.RotationSpec + Data.Geo.Jord.SpeedSpec + Data.Geo.Jord.TransformationSpec + Paths_jord + hs-source-dirs: + test + ghc-options: -Wall + build-depends: + HUnit ==1.6.* + , base >=4.9 && <5 + , hspec ==2.* + , jord + default-language: Haskell2010 diff --git a/src/Data/Geo/Jord.hs b/src/Data/Geo/Jord.hs --- a/src/Data/Geo/Jord.hs +++ b/src/Data/Geo/Jord.hs @@ -55,4 +55,4 @@ -- | version. jordVersion :: String -jordVersion = "0.4.1.0" +jordVersion = "0.4.2.0" diff --git a/src/Data/Geo/Jord/EcefPosition.hs b/src/Data/Geo/Jord/EcefPosition.hs --- a/src/Data/Geo/Jord/EcefPosition.hs +++ b/src/Data/Geo/Jord/EcefPosition.hs @@ -26,7 +26,7 @@ -- -- @ex-ey@ plane is the equatorial plane, @ey@ is on the prime meridian, and @ez@ on the polar axis. -- --- Note: on a spherical model earth, an n-vector is equivalent to a normalised version of an (ECEF) cartesian coordinate. +-- Note: on a spherical model earth, an /n/-vector is equivalent to a normalised version of an (ECEF) cartesian coordinate. newtype EcefPosition = EcefPosition Vector3d deriving (Eq, Show) diff --git a/src/Data/Geo/Jord/Kinematics.hs b/src/Data/Geo/Jord/Kinematics.hs --- a/src/Data/Geo/Jord/Kinematics.hs +++ b/src/Data/Geo/Jord/Kinematics.hs @@ -12,98 +12,99 @@ -- -- and in -- -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 +import Data.Maybe (isNothing) + -- | '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 +114,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 +136,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 @@ -160,25 +161,22 @@ -- -- * interceptor and target are at the same position -- --- * interceptor is on the great circle of target and behind as the minimum speed would be target speed + epsillon +-- * interceptor is "behind" the target -- -- @ -- let t = Track (decimalLatLong 34 (-50)) (decimalDegrees 220) (knots 600) -- let ip = (decimalLatLong 20 (-60)) -- let i = intercept t ip r84 --- fmap interceptorSpeed i = Just (knots 52.837096) --- fmap interceptTime i = Just (seconds 5947.698) +-- fmap interceptorSpeed i = Just (knots 52.633367756059) +-- fmap interceptTime i = Just (seconds 5993.831) -- @ -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 p r = interceptByTime t p (seconds (timeToIntercept t p r)) 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 - +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 @@ -186,17 +184,19 @@ -- -- * 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 - +-- * interceptor speed is below minimum speed returned by 'intercept' +interceptBySpeed :: (Eq a, NTransform a) => Track a -> a -> Speed -> Length -> Maybe (Intercept a) +interceptBySpeed t p s r + | isNothing minInt = Nothing + | fmap interceptorSpeed minInt == Just s = minInt + | otherwise = interceptByTime t p (seconds (timeToInterceptSpeed t p s r)) r + where + minInt = intercept t p 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 - +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,230 +213,203 @@ -- 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) - +-- +-- Note: contrary to 'intercept' and 'interceptBySpeed' this function handles +-- cases where the interceptor has to catch up the target. +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 + a = toMetresPerSecond s / toMetres r * sec + v1 = vadd (vscale v0 (cos a)) (vscale c (sin a)) + -- | 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 - +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 = vec3d 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 - +timeToIntercept :: (NTransform a) => Track a -> a -> Length -> Double +timeToIntercept (Track p2 b2 s2) p1 r = intMinNrRec v10v20 v10c2 w2 (sep v10 v20 c2 s2 r) t0 0 + where + v10 = vec3d p1 + v20 = vec3d p2 + c2 = vec (course p2 b2) + v10v20 = vdot v10 v20 + v10c2 = vdot v10 c2 + s2mps = toMetresPerSecond s2 + rm = toMetres r + w2 = s2mps / rm + s0 = ad v10 v20 -- initial angular distance between target and interceptor + t0 = rm * s0 / s2mps -- assume target is travelling towards interceptor + -- | 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 - +timeToInterceptSpeed :: (NTransform a) => Track a -> a -> Speed -> Length -> Double +timeToInterceptSpeed (Track p2 b2 s2) p1 s1 r = + intSpdNrRec v10v20 v10c2 w1 w2 (sep v10 v20 c2 s2 r) t0 0 + where + v10 = vec3d p1 + v20 = vec3d p2 + c2 = vec (course p2 b2) + v10v20 = vdot v10 v20 + v10c2 = vdot v10 c2 + rm = toMetres r + w1 = toMetresPerSecond s1 / rm + w2 = toMetresPerSecond s2 / rm + t0 = 0.1 + +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-11 = 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 :: Double -> Double -> Double -> (Double -> Double) -> Double -> Int -> Double +intMinNrRec v10v20 v10c2 w2 st 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 - + | abs fi < 1e-11 = ti1 + | otherwise = intMinNrRec v10v20 v10c2 w2 st ti1 (i + 1) + where + cosw2t = cos (w2 * ti) + sinw2t = sin (w2 * ti) + v10dv2dt = (-w2) * (v10v20 * sinw2t - v10c2 * cosw2t) + v10d2v2dt2 = (-1.0 * w2 * w2) * (v10v20 * cosw2t + v10c2 * sinw2t) + si = st ti + sinS = sin si + a = (-1.0) / sinS + b = cos si / (sinS * sinS) + f = ti * a * v10dv2dt - si + d2sdt2 = a * (b * v10dv2dt * v10dv2dt + v10d2v2dt2) + df = ti * d2sdt2 + fi = f / df + ti1 = ti - fi + -- | 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 +intSpdNrRec :: Double -> Double -> Double -> Double -> (Double -> Double) -> Double -> Int -> Double +intSpdNrRec v10v20 v10c2 w1 w2 st 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-11 = ti1 + | otherwise = intSpdNrRec v10v20 v10c2 w1 w2 st ti1 (i + 1) + where + cosw2t = cos (w2 * ti) + sinw2t = sin (w2 * ti) + si = st ti + f = si / ti - w1 + dsdt = (w2 * (v10v20 * sinw2t - v10c2 * cosw2t)) / sin si + df = (dsdt - (si / ti)) / ti + fi = f / df + ti1 = ti - fi +-- | angular separation in radians at ti between v10 and track with initial position v20, +-- course c2 and speed s2. +sep :: Vector3d -> Vector3d -> Vector3d -> Speed -> Length -> Double -> Double +sep v10 v20 c2 s2 r ti = ad v10 (position'' v20 s2 c2 ti r) + -- | 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) + +vec3d :: (NTransform a) => a -> Vector3d +vec3d = vec . pos . toNVector diff --git a/src/Data/Geo/Jord/NVector.hs b/src/Data/Geo/Jord/NVector.hs --- a/src/Data/Geo/Jord/NVector.hs +++ b/src/Data/Geo/Jord/NVector.hs @@ -6,7 +6,7 @@ -- Stability: experimental -- Portability: portable -- --- Types and functions for working with n-vectors. +-- Types and functions for working with /n/-vectors. -- module Data.Geo.Jord.NVector ( NVector diff --git a/src/Data/Geo/Jord/Transformation.hs b/src/Data/Geo/Jord/Transformation.hs --- a/src/Data/Geo/Jord/Transformation.hs +++ b/src/Data/Geo/Jord/Transformation.hs @@ -35,17 +35,17 @@ import Data.Geo.Jord.Quantity import Data.Geo.Jord.Vector3d --- | Transformation between positions and 'AngularPosition' of 'NVector'. +-- | Transformation between position and /n/-vector and height. class NTransform a where toNVector :: a -> AngularPosition NVector -- ^ position to 'AngularPosition' of 'NVector'. fromNVector :: AngularPosition NVector -> a -- ^ 'AngularPosition' of 'NVector' and height to position. --- | 'NVector' <-> 'AngularPosition' of 'NVector'. +-- | 'NVector' to, from 'AngularPosition' of 'NVector'. instance NTransform NVector where toNVector nv = AngularPosition nv zero fromNVector = pos --- | 'LatLong' <-> 'AngularPosition' of 'NVector'. +-- | 'LatLong' to, from 'AngularPosition' of 'NVector'. instance NTransform LatLong where toNVector ll = AngularPosition (latLongToNVector ll) zero fromNVector = nvectorToLatLong . pos @@ -55,32 +55,32 @@ toNVector = id fromNVector = id --- | 'AngularPosition' of 'LatLong' <-> 'AngularPosition' of 'NVector'. +-- | 'AngularPosition' of 'LatLong' to, from 'AngularPosition' of 'NVector'. instance NTransform (AngularPosition LatLong) where toNVector (AngularPosition ll h) = AngularPosition (latLongToNVector ll) h fromNVector (AngularPosition nv h) = AngularPosition (nvectorToLatLong nv) h --- | Transformation between 'EcefPosition' and angular or n-vector positions. +-- | Transformation between 'EcefPosition' and angular or /n/-vector positions. class ETransform a where toEcef :: a -> Earth -> EcefPosition -- ^ position and earth model to to 'EcefPosition'. fromEcef :: EcefPosition -> Earth -> a -- ^ 'EcefPosition' and earth model to position. --- | 'NVector' <-> 'EcefPosition'. +-- | 'NVector' to, from 'EcefPosition'. instance ETransform NVector where fromEcef p e = pos (ecefToNVector p e) toEcef v = nvectorToEcef (nvectorHeight v zero) --- | 'LatLong' <-> 'EcefPosition'. +-- | 'LatLong' to, from 'EcefPosition'. instance ETransform LatLong where fromEcef p e = fromNVector (nvectorHeight (fromEcef p e :: NVector) zero) toEcef = toEcef . toNVector --- | 'AngularPosition' of 'NVector' <-> 'EcefPosition'. +-- | 'AngularPosition' of 'NVector' to, from 'EcefPosition'. instance ETransform (AngularPosition NVector) where fromEcef = ecefToNVector toEcef = nvectorToEcef --- | 'AngularPosition' of 'LatLong' <-> 'EcefPosition'. +-- | 'AngularPosition' of 'LatLong' to, from 'EcefPosition'. instance ETransform (AngularPosition LatLong) where fromEcef p e = fromNVector (ecefToNVector p e) toEcef = nvectorToEcef . toNVector diff --git a/test/Data/Geo/Jord/KinematicsSpec.hs b/test/Data/Geo/Jord/KinematicsSpec.hs --- a/test/Data/Geo/Jord/KinematicsSpec.hs +++ b/test/Data/Geo/Jord/KinematicsSpec.hs @@ -15,10 +15,11 @@ 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 + it "handles poles" $ -- 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 @@ -85,13 +86,9 @@ (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) + it "returns Nothing if interceptor is behing target" $ do + let t = Track (decimalLatLong 45 67) (decimalDegrees 54) (knots 400) + let ip = decimalLatLong 44 66 intercept84 t ip `shouldBe` Nothing it "handles interceptor on the great circle of target and in front" $ do let tp = decimalLatLong 20 30 @@ -103,12 +100,25 @@ fmap interceptorSpeed i `shouldBe` Just zero fmap interceptPosition i `shouldBe` Just ip fmap interceptTime i `shouldBe` Just (seconds 1.215) + it "returns Nothing if interceptor is behing target" $ do + let t = Track (decimalLatLong 45 67) (decimalDegrees 181) (knots 400) + let ip = decimalLatLong 44 66 + let i = intercept84 t ip + fmap interceptorSpeed i `shouldBe` Just (knots 228.5538171521) + fmap interceptTime i `shouldBe` Just (seconds 808.770) + let interceptor = + Track + ip + (fromJust (fmap interceptorBearing i)) + (fromJust (fmap interceptorSpeed i)) + fmap interceptPosition i `shouldBe` + Just (position84 interceptor (fromJust (fmap interceptTime i))) 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) + fmap interceptorSpeed i `shouldBe` Just (knots 52.633367756059) + fmap interceptTime i `shouldBe` Just (seconds 5993.831) let interceptor = Track ip @@ -127,14 +137,6 @@ 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) @@ -177,10 +179,11 @@ 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 + it "handles the poles" $ -- 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. + do let t = Track (decimalLatLong 90 0) zero (kilometresPerHour 500) let ip = decimalLatLong (-90) 0 let i = interceptByTime84 t ip (seconds 3600)