module Data.Geo.Jord.Local
(
Frame(..)
, FrameB
, yaw
, pitch
, roll
, bOrigin
, frameB
, FrameL
, wanderAzimuth
, lOrigin
, frameL
, FrameN
, nOrigin
, frameN
, Delta(..)
, deltaMetres
, Ned(..)
, nedMetres
, bearing
, elevation
, slantRange
, deltaBetween
, nedBetween
, destination
, destinationN
) where
import Data.Geo.Jord.Angle (Angle)
import qualified Data.Geo.Jord.Angle as Angle
import qualified Data.Geo.Jord.Geocentric as Geocentric
import qualified Data.Geo.Jord.Geodetic as Geodetic
import Data.Geo.Jord.Length (Length)
import qualified Data.Geo.Jord.Length as Length (metres, toMetres)
import qualified Data.Geo.Jord.Math3d as Math3d
import Data.Geo.Jord.Model (Model)
import Data.Geo.Jord.Positions
import Data.Geo.Jord.Rotation
class Frame a where
rEF :: a -> [Math3d.V3]
data FrameB a =
FrameB
{ FrameB a -> Angle
yaw :: Angle
, FrameB a -> Angle
pitch :: Angle
, FrameB a -> Angle
roll :: Angle
, FrameB a -> Position a
bOrigin :: Geodetic.Position a
, FrameB a -> V3
bNorth :: Math3d.V3
}
deriving (FrameB a -> FrameB a -> Bool
(FrameB a -> FrameB a -> Bool)
-> (FrameB a -> FrameB a -> Bool) -> Eq (FrameB a)
forall a. Model a => FrameB a -> FrameB a -> Bool
forall a. (a -> a -> Bool) -> (a -> a -> Bool) -> Eq a
/= :: FrameB a -> FrameB a -> Bool
$c/= :: forall a. Model a => FrameB a -> FrameB a -> Bool
== :: FrameB a -> FrameB a -> Bool
$c== :: forall a. Model a => FrameB a -> FrameB a -> Bool
Eq, Int -> FrameB a -> ShowS
[FrameB a] -> ShowS
FrameB a -> String
(Int -> FrameB a -> ShowS)
-> (FrameB a -> String) -> ([FrameB a] -> ShowS) -> Show (FrameB a)
forall a. Model a => Int -> FrameB a -> ShowS
forall a. Model a => [FrameB a] -> ShowS
forall a. Model a => FrameB a -> String
forall a.
(Int -> a -> ShowS) -> (a -> String) -> ([a] -> ShowS) -> Show a
showList :: [FrameB a] -> ShowS
$cshowList :: forall a. Model a => [FrameB a] -> ShowS
show :: FrameB a -> String
$cshow :: forall a. Model a => FrameB a -> String
showsPrec :: Int -> FrameB a -> ShowS
$cshowsPrec :: forall a. Model a => Int -> FrameB a -> ShowS
Show)
frameB :: (Model a) => Angle -> Angle -> Angle -> Geodetic.Position a -> FrameB a
frameB :: Angle -> Angle -> Angle -> Position a -> FrameB a
frameB Angle
y Angle
p Angle
r Position a
o = Angle -> Angle -> Angle -> Position a -> V3 -> FrameB a
forall a. Angle -> Angle -> Angle -> Position a -> V3 -> FrameB a
FrameB Angle
y Angle
p Angle
r Position a
o (Position a -> V3
forall a. Model a => Position a -> V3
northPole Position a
o)
instance Frame (FrameB a) where
rEF :: FrameB a -> [V3]
rEF (FrameB Angle
y Angle
p Angle
r Position a
o V3
np) = [V3]
rm
where
rNB :: [V3]
rNB = Angle -> Angle -> Angle -> [V3]
zyx2r Angle
y Angle
p Angle
r
n :: FrameN a
n = Position a -> V3 -> FrameN a
forall a. Position a -> V3 -> FrameN a
FrameN Position a
o V3
np
rEN :: [V3]
rEN = FrameN a -> [V3]
forall a. Frame a => a -> [V3]
rEF FrameN a
n
rm :: [V3]
rm = [V3] -> [V3] -> [V3]
Math3d.dotM [V3]
rEN [V3]
rNB
data FrameL a =
FrameL
{ FrameL a -> Angle
wanderAzimuth :: Angle
, FrameL a -> Position a
lOrigin :: Geodetic.Position a
}
deriving (FrameL a -> FrameL a -> Bool
(FrameL a -> FrameL a -> Bool)
-> (FrameL a -> FrameL a -> Bool) -> Eq (FrameL a)
forall a. Model a => FrameL a -> FrameL a -> Bool
forall a. (a -> a -> Bool) -> (a -> a -> Bool) -> Eq a
/= :: FrameL a -> FrameL a -> Bool
$c/= :: forall a. Model a => FrameL a -> FrameL a -> Bool
== :: FrameL a -> FrameL a -> Bool
$c== :: forall a. Model a => FrameL a -> FrameL a -> Bool
Eq, Int -> FrameL a -> ShowS
[FrameL a] -> ShowS
FrameL a -> String
(Int -> FrameL a -> ShowS)
-> (FrameL a -> String) -> ([FrameL a] -> ShowS) -> Show (FrameL a)
forall a. Model a => Int -> FrameL a -> ShowS
forall a. Model a => [FrameL a] -> ShowS
forall a. Model a => FrameL a -> String
forall a.
(Int -> a -> ShowS) -> (a -> String) -> ([a] -> ShowS) -> Show a
showList :: [FrameL a] -> ShowS
$cshowList :: forall a. Model a => [FrameL a] -> ShowS
show :: FrameL a -> String
$cshow :: forall a. Model a => FrameL a -> String
showsPrec :: Int -> FrameL a -> ShowS
$cshowsPrec :: forall a. Model a => Int -> FrameL a -> ShowS
Show)
instance Frame (FrameL m) where
rEF :: FrameL m -> [V3]
rEF (FrameL Angle
w Position m
o) = [V3]
rm
where
lat :: Angle
lat = Position m -> Angle
forall a. HasCoordinates a => a -> Angle
Geodetic.latitude Position m
o
lon :: Angle
lon = Position m -> Angle
forall a. HasCoordinates a => a -> Angle
Geodetic.longitude Position m
o
r :: [V3]
r = Angle -> Angle -> Angle -> [V3]
xyz2r Angle
lon (Angle -> Angle
Angle.negate Angle
lat) Angle
w
rEe' :: [V3]
rEe' = [Double -> Double -> Double -> V3
Math3d.vec3 Double
0 Double
0 (-Double
1), Double -> Double -> Double -> V3
Math3d.vec3 Double
0 Double
1 Double
0, Double -> Double -> Double -> V3
Math3d.vec3 Double
1 Double
0 Double
0]
rm :: [V3]
rm = [V3] -> [V3] -> [V3]
Math3d.dotM [V3]
rEe' [V3]
r
frameL :: (Model a) => Angle -> Geodetic.Position a -> FrameL a
frameL :: Angle -> Position a -> FrameL a
frameL = Angle -> Position a -> FrameL a
forall a. Angle -> Position a -> FrameL a
FrameL
data FrameN a =
FrameN
{ FrameN a -> Position a
nOrigin :: Geodetic.Position a
, FrameN a -> V3
nNorth :: Math3d.V3
}
deriving (FrameN a -> FrameN a -> Bool
(FrameN a -> FrameN a -> Bool)
-> (FrameN a -> FrameN a -> Bool) -> Eq (FrameN a)
forall a. Model a => FrameN a -> FrameN a -> Bool
forall a. (a -> a -> Bool) -> (a -> a -> Bool) -> Eq a
/= :: FrameN a -> FrameN a -> Bool
$c/= :: forall a. Model a => FrameN a -> FrameN a -> Bool
== :: FrameN a -> FrameN a -> Bool
$c== :: forall a. Model a => FrameN a -> FrameN a -> Bool
Eq, Int -> FrameN a -> ShowS
[FrameN a] -> ShowS
FrameN a -> String
(Int -> FrameN a -> ShowS)
-> (FrameN a -> String) -> ([FrameN a] -> ShowS) -> Show (FrameN a)
forall a. Model a => Int -> FrameN a -> ShowS
forall a. Model a => [FrameN a] -> ShowS
forall a. Model a => FrameN a -> String
forall a.
(Int -> a -> ShowS) -> (a -> String) -> ([a] -> ShowS) -> Show a
showList :: [FrameN a] -> ShowS
$cshowList :: forall a. Model a => [FrameN a] -> ShowS
show :: FrameN a -> String
$cshow :: forall a. Model a => FrameN a -> String
showsPrec :: Int -> FrameN a -> ShowS
$cshowsPrec :: forall a. Model a => Int -> FrameN a -> ShowS
Show)
instance Frame (FrameN a) where
rEF :: FrameN a -> [V3]
rEF (FrameN Position a
o V3
np) = [V3] -> [V3]
Math3d.transposeM [V3]
rm
where
vo :: V3
vo = Position a -> V3
forall a. HasCoordinates a => a -> V3
Geodetic.nvector Position a
o
rd :: V3
rd = V3 -> Double -> V3
Math3d.scale V3
vo (-Double
1.0)
re :: V3
re = V3 -> V3
Math3d.unit (V3 -> V3 -> V3
Math3d.cross V3
np V3
vo)
rn :: V3
rn = V3 -> V3 -> V3
Math3d.cross V3
re V3
rd
rm :: [V3]
rm = [V3
rn, V3
re, V3
rd]
frameN :: (Model a) => Geodetic.Position a -> FrameN a
frameN :: Position a -> FrameN a
frameN Position a
p = Position a -> V3 -> FrameN a
forall a. Position a -> V3 -> FrameN a
FrameN Position a
p (Position a -> V3
forall a. Model a => Position a -> V3
northPole Position a
p)
data Delta =
Delta
{ Delta -> Length
dx :: Length
, Delta -> Length
dy :: Length
, Delta -> Length
dz :: Length
}
deriving (Delta -> Delta -> Bool
(Delta -> Delta -> Bool) -> (Delta -> Delta -> Bool) -> Eq Delta
forall a. (a -> a -> Bool) -> (a -> a -> Bool) -> Eq a
/= :: Delta -> Delta -> Bool
$c/= :: Delta -> Delta -> Bool
== :: Delta -> Delta -> Bool
$c== :: Delta -> Delta -> Bool
Eq, Int -> Delta -> ShowS
[Delta] -> ShowS
Delta -> String
(Int -> Delta -> ShowS)
-> (Delta -> String) -> ([Delta] -> ShowS) -> Show Delta
forall a.
(Int -> a -> ShowS) -> (a -> String) -> ([a] -> ShowS) -> Show a
showList :: [Delta] -> ShowS
$cshowList :: [Delta] -> ShowS
show :: Delta -> String
$cshow :: Delta -> String
showsPrec :: Int -> Delta -> ShowS
$cshowsPrec :: Int -> Delta -> ShowS
Show)
deltaMetres :: Double -> Double -> Double -> Delta
deltaMetres :: Double -> Double -> Double -> Delta
deltaMetres Double
x Double
y Double
z = Length -> Length -> Length -> Delta
Delta (Double -> Length
Length.metres Double
x) (Double -> Length
Length.metres Double
y) (Double -> Length
Length.metres Double
z)
data Ned =
Ned
{ Ned -> Length
north :: Length
, Ned -> Length
east :: Length
, Ned -> Length
down :: Length
}
deriving (Ned -> Ned -> Bool
(Ned -> Ned -> Bool) -> (Ned -> Ned -> Bool) -> Eq Ned
forall a. (a -> a -> Bool) -> (a -> a -> Bool) -> Eq a
/= :: Ned -> Ned -> Bool
$c/= :: Ned -> Ned -> Bool
== :: Ned -> Ned -> Bool
$c== :: Ned -> Ned -> Bool
Eq, Int -> Ned -> ShowS
[Ned] -> ShowS
Ned -> String
(Int -> Ned -> ShowS)
-> (Ned -> String) -> ([Ned] -> ShowS) -> Show Ned
forall a.
(Int -> a -> ShowS) -> (a -> String) -> ([a] -> ShowS) -> Show a
showList :: [Ned] -> ShowS
$cshowList :: [Ned] -> ShowS
show :: Ned -> String
$cshow :: Ned -> String
showsPrec :: Int -> Ned -> ShowS
$cshowsPrec :: Int -> Ned -> ShowS
Show)
nedMetres :: Double -> Double -> Double -> Ned
nedMetres :: Double -> Double -> Double -> Ned
nedMetres Double
n Double
e Double
d = Length -> Length -> Length -> Ned
Ned (Double -> Length
Length.metres Double
n) (Double -> Length
Length.metres Double
e) (Double -> Length
Length.metres Double
d)
bearing :: Ned -> Angle
bearing :: Ned -> Angle
bearing (Ned Length
n Length
e Length
_) =
let a :: Angle
a = Double -> Double -> Angle
Angle.atan2 (Length -> Double
Length.toMetres Length
e) (Length -> Double
Length.toMetres Length
n)
in Angle -> Angle -> Angle
Angle.normalise Angle
a (Double -> Angle
Angle.decimalDegrees Double
360.0)
elevation :: Ned -> Angle
elevation :: Ned -> Angle
elevation Ned
n = Angle -> Angle
Angle.negate (Double -> Angle
Angle.asin (V3 -> Double
Math3d.v3z V3
v Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ V3 -> Double
Math3d.norm V3
v))
where
v :: V3
v = Ned -> V3
nedV3 Ned
n
slantRange :: Ned -> Length
slantRange :: Ned -> Length
slantRange = Double -> Length
Length.metres (Double -> Length) -> (Ned -> Double) -> Ned -> Length
forall b c a. (b -> c) -> (a -> b) -> a -> c
. V3 -> Double
Math3d.norm (V3 -> Double) -> (Ned -> V3) -> Ned -> Double
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Ned -> V3
nedV3
deltaBetween ::
(Frame a, Model b)
=> Geodetic.Position b
-> Geodetic.Position b
-> (Geodetic.Position b -> a)
-> Delta
deltaBetween :: Position b -> Position b -> (Position b -> a) -> Delta
deltaBetween Position b
p1 Position b
p2 Position b -> a
f = Double -> Double -> Double -> Delta
deltaMetres (V3 -> Double
Math3d.v3x V3
d) (V3 -> Double
Math3d.v3y V3
d) (V3 -> Double
Math3d.v3z V3
d)
where
g1 :: V3
g1 = Position b -> V3
forall a. Model a => Position a -> V3
Geocentric.metresCoords (Position b -> V3)
-> (Position b -> Position b) -> Position b -> V3
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Position b -> Position b
forall m. Model m => Position m -> Position m
toGeocentric (Position b -> V3) -> Position b -> V3
forall a b. (a -> b) -> a -> b
$ Position b
p1
g2 :: V3
g2 = Position b -> V3
forall a. Model a => Position a -> V3
Geocentric.metresCoords (Position b -> V3)
-> (Position b -> Position b) -> Position b -> V3
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Position b -> Position b
forall m. Model m => Position m -> Position m
toGeocentric (Position b -> V3) -> Position b -> V3
forall a b. (a -> b) -> a -> b
$ Position b
p2
de :: V3
de = V3 -> V3 -> V3
Math3d.subtract V3
g2 V3
g1
rm :: [V3]
rm = [V3] -> [V3]
Math3d.transposeM (a -> [V3]
forall a. Frame a => a -> [V3]
rEF (Position b -> a
f Position b
p1))
d :: V3
d = V3 -> [V3] -> V3
Math3d.multM V3
de [V3]
rm
nedBetween :: (Model a) => Geodetic.Position a -> Geodetic.Position a -> Ned
nedBetween :: Position a -> Position a -> Ned
nedBetween Position a
p1 Position a
p2 = Length -> Length -> Length -> Ned
Ned Length
n Length
e Length
d
where
(Delta Length
n Length
e Length
d) = Position a -> Position a -> (Position a -> FrameN a) -> Delta
forall a b.
(Frame a, Model b) =>
Position b -> Position b -> (Position b -> a) -> Delta
deltaBetween Position a
p1 Position a
p2 Position a -> FrameN a
forall a. Model a => Position a -> FrameN a
frameN
destination ::
(Frame a, Model b)
=> Geodetic.Position b
-> (Geodetic.Position b -> a)
-> Delta
-> Geodetic.Position b
destination :: Position b -> (Position b -> a) -> Delta -> Position b
destination Position b
p0 Position b -> a
f Delta
d = Position b -> Position b
forall m. Model m => Position m -> Position m
toGeodetic Position b
gt
where
g0 :: V3
g0 = Position b -> V3
forall a. Model a => Position a -> V3
Geocentric.metresCoords (Position b -> V3)
-> (Position b -> Position b) -> Position b -> V3
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Position b -> Position b
forall m. Model m => Position m -> Position m
toGeocentric (Position b -> V3) -> Position b -> V3
forall a b. (a -> b) -> a -> b
$ Position b
p0
rm :: [V3]
rm = a -> [V3]
forall a. Frame a => a -> [V3]
rEF (Position b -> a
f Position b
p0)
c :: V3
c = V3 -> [V3] -> V3
Math3d.multM (Delta -> V3
deltaV3 Delta
d) [V3]
rm
v :: V3
v = V3 -> V3 -> V3
Math3d.add V3
g0 V3
c
gt :: Position b
gt = V3 -> b -> Position b
forall a. Model a => V3 -> a -> Position a
Geocentric.metresPos' V3
v (Position b -> b
forall a. Model a => Position a -> a
Geodetic.model' Position b
p0)
destinationN :: (Model a) => Geodetic.Position a -> Ned -> Geodetic.Position a
destinationN :: Position a -> Ned -> Position a
destinationN Position a
p0 (Ned Length
n Length
e Length
d) = Position a -> (Position a -> FrameN a) -> Delta -> Position a
forall a b.
(Frame a, Model b) =>
Position b -> (Position b -> a) -> Delta -> Position b
destination Position a
p0 Position a -> FrameN a
forall a. Model a => Position a -> FrameN a
frameN (Length -> Length -> Length -> Delta
Delta Length
n Length
e Length
d)
nedV3 :: Ned -> Math3d.V3
nedV3 :: Ned -> V3
nedV3 (Ned Length
n Length
e Length
d) = Double -> Double -> Double -> V3
Math3d.vec3 (Length -> Double
Length.toMetres Length
n) (Length -> Double
Length.toMetres Length
e) (Length -> Double
Length.toMetres Length
d)
deltaV3 :: Delta -> Math3d.V3
deltaV3 :: Delta -> V3
deltaV3 (Delta Length
x' Length
y' Length
z') =
Double -> Double -> Double -> V3
Math3d.vec3 (Length -> Double
Length.toMetres Length
x') (Length -> Double
Length.toMetres Length
y') (Length -> Double
Length.toMetres Length
z')
northPole :: (Model a) => Geodetic.Position a -> Math3d.V3
northPole :: Position a -> V3
northPole = HorizontalPosition a -> V3
forall a. HasCoordinates a => a -> V3
Geodetic.nvector (HorizontalPosition a -> V3)
-> (Position a -> HorizontalPosition a) -> Position a -> V3
forall b c a. (b -> c) -> (a -> b) -> a -> c
. a -> HorizontalPosition a
forall a. Model a => a -> HorizontalPosition a
Geodetic.northPole (a -> HorizontalPosition a)
-> (Position a -> a) -> Position a -> HorizontalPosition a
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Position a -> a
forall a. Model a => Position a -> a
Geodetic.model'