jord 0.4.1.0 → 0.4.2.0
raw patch · 9 files changed
+441/−460 lines, 9 filesnew-component:exe:jord-repl
Files
- ChangeLog.md +5/−0
- README.md +6/−6
- jord.cabal +97/−97
- src/Data/Geo/Jord.hs +1/−1
- src/Data/Geo/Jord/EcefPosition.hs +1/−1
- src/Data/Geo/Jord/Kinematics.hs +299/−326
- src/Data/Geo/Jord/NVector.hs +1/−1
- src/Data/Geo/Jord/Transformation.hs +9/−9
- test/Data/Geo/Jord/KinematicsSpec.hs +22/−19
ChangeLog.md view
@@ -1,3 +1,8 @@+### 0.4.2.0 + +- Fixed intercept +- jord-exe renamed jord-repl + ### 0.4.1.0 - Fixed interceptBySpeed
README.md view
@@ -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?
jord.cabal view
@@ -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 <https://github.com/ofmooseandmen/jord#readme> -category: Geography -stability: experimental -homepage: https://github.com/ofmooseandmen/jord -bug-reports: https://github.com/ofmooseandmen/jord/issues -author: Cedric Liegeois -maintainer: Cedric Liegeois <ofmooseandmen@yahoo.com> -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 <https://github.com/ofmooseandmen/jord#readme>+category: Geography+stability: experimental+homepage: https://github.com/ofmooseandmen/jord+bug-reports: https://github.com/ofmooseandmen/jord/issues+author: Cedric Liegeois+maintainer: Cedric Liegeois <ofmooseandmen@yahoo.com>+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
src/Data/Geo/Jord.hs view
@@ -55,4 +55,4 @@ -- | version. jordVersion :: String -jordVersion = "0.4.1.0" +jordVersion = "0.4.2.0"
src/Data/Geo/Jord/EcefPosition.hs view
@@ -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)
src/Data/Geo/Jord/Kinematics.hs view
@@ -12,98 +12,99 @@ -- <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+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
src/Data/Geo/Jord/NVector.hs view
@@ -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
src/Data/Geo/Jord/Transformation.hs view
@@ -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
test/Data/Geo/Jord/KinematicsSpec.hs view
@@ -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)