module Data.Geo.Jord.Tx
(
Tx(..)
, inverse
, Params(..)
, Params7
, Rates
, Params15(..)
, params7
, rates
, paramsAt
, Graph
, graph
, paramsBetween
, apply
) where
import Data.List (find, foldl', sortOn)
import Data.Maybe (mapMaybe)
import qualified Data.Geo.Jord.Math3d as Math3d
import Data.Geo.Jord.Model (Epoch(..), ModelId)
data Tx a =
Tx
{ Tx a -> ModelId
modelA :: ModelId
, Tx a -> ModelId
modelB :: ModelId
, Tx a -> a
params :: a
}
inverse :: (Params a) => Tx a -> Tx a
inverse :: Tx a -> Tx a
inverse Tx a
t = ModelId -> ModelId -> a -> Tx a
forall a. ModelId -> ModelId -> a -> Tx a
Tx (Tx a -> ModelId
forall a. Tx a -> ModelId
modelB Tx a
t) (Tx a -> ModelId
forall a. Tx a -> ModelId
modelA Tx a
t) (a -> a
forall a. Params a => a -> a
inverseParams (Tx a -> a
forall a. Tx a -> a
params Tx a
t))
class Params a where
idParams :: a
inverseParams :: a -> a
data Params7 =
Params7 !Math3d.V3 !Double !Math3d.V3
deriving (Int -> Params7 -> ShowS
[Params7] -> ShowS
Params7 -> String
(Int -> Params7 -> ShowS)
-> (Params7 -> String) -> ([Params7] -> ShowS) -> Show Params7
forall a.
(Int -> a -> ShowS) -> (a -> String) -> ([a] -> ShowS) -> Show a
showList :: [Params7] -> ShowS
$cshowList :: [Params7] -> ShowS
show :: Params7 -> String
$cshow :: Params7 -> String
showsPrec :: Int -> Params7 -> ShowS
$cshowsPrec :: Int -> Params7 -> ShowS
Show)
instance Params Params7 where
idParams :: Params7
idParams = V3 -> Double -> V3 -> Params7
Params7 V3
Math3d.zero Double
0 V3
Math3d.zero
inverseParams :: Params7 -> Params7
inverseParams (Params7 V3
c Double
s V3
r) = V3 -> Double -> V3 -> Params7
Params7 (V3 -> Double -> V3
Math3d.scale V3
c (-Double
1.0)) (-Double
s) (V3 -> Double -> V3
Math3d.scale V3
r (-Double
1.0))
instance Params Params15 where
idParams :: Params15
idParams = Epoch -> Params7 -> Rates -> Params15
Params15 (Double -> Epoch
Epoch Double
0) Params7
forall a. Params a => a
idParams (V3 -> Double -> V3 -> Rates
Rates V3
Math3d.zero Double
0 V3
Math3d.zero)
inverseParams :: Params15 -> Params15
inverseParams (Params15 Epoch
e Params7
p (Rates V3
c Double
s V3
r)) =
Epoch -> Params7 -> Rates -> Params15
Params15 Epoch
e (Params7 -> Params7
forall a. Params a => a -> a
inverseParams Params7
p) (V3 -> Double -> V3 -> Rates
Rates (V3 -> Double -> V3
Math3d.scale V3
c (-Double
1.0)) (-Double
s) (V3 -> Double -> V3
Math3d.scale V3
r (-Double
1.0)))
data Rates =
Rates !Math3d.V3 !Double !Math3d.V3
deriving (Int -> Rates -> ShowS
[Rates] -> ShowS
Rates -> String
(Int -> Rates -> ShowS)
-> (Rates -> String) -> ([Rates] -> ShowS) -> Show Rates
forall a.
(Int -> a -> ShowS) -> (a -> String) -> ([a] -> ShowS) -> Show a
showList :: [Rates] -> ShowS
$cshowList :: [Rates] -> ShowS
show :: Rates -> String
$cshow :: Rates -> String
showsPrec :: Int -> Rates -> ShowS
$cshowsPrec :: Int -> Rates -> ShowS
Show)
data Params15 =
Params15 Epoch Params7 Rates
deriving (Int -> Params15 -> ShowS
[Params15] -> ShowS
Params15 -> String
(Int -> Params15 -> ShowS)
-> (Params15 -> String) -> ([Params15] -> ShowS) -> Show Params15
forall a.
(Int -> a -> ShowS) -> (a -> String) -> ([a] -> ShowS) -> Show a
showList :: [Params15] -> ShowS
$cshowList :: [Params15] -> ShowS
show :: Params15 -> String
$cshow :: Params15 -> String
showsPrec :: Int -> Params15 -> ShowS
$cshowsPrec :: Int -> Params15 -> ShowS
Show)
params7 ::
(Double, Double, Double)
-> Double
-> (Double, Double, Double)
-> Params7
params7 :: (Double, Double, Double)
-> Double -> (Double, Double, Double) -> Params7
params7 (Double, Double, Double)
c Double
s (Double, Double, Double)
r = V3 -> Double -> V3 -> Params7
Params7 ((Double, Double, Double) -> V3
mmToMetres (Double, Double, Double)
c) (Double
s Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
1e9) ((Double, Double, Double) -> V3
masToRadians (Double, Double, Double)
r)
rates ::
(Double, Double, Double)
-> Double
-> (Double, Double, Double)
-> Rates
rates :: (Double, Double, Double)
-> Double -> (Double, Double, Double) -> Rates
rates (Double, Double, Double)
c Double
s (Double, Double, Double)
r = V3 -> Double -> V3 -> Rates
Rates ((Double, Double, Double) -> V3
mmToMetres (Double, Double, Double)
c) (Double
s Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
1e9) ((Double, Double, Double) -> V3
masToRadians (Double, Double, Double)
r)
mmToMetres :: (Double, Double, Double) -> Math3d.V3
mmToMetres :: (Double, Double, Double) -> V3
mmToMetres (Double
cx, Double
cy, Double
cz) = V3 -> Double -> V3
Math3d.scale (Double -> Double -> Double -> V3
Math3d.vec3 Double
cx Double
cy Double
cz) (Double
1.0 Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
1000.0)
masToRadians :: (Double, Double, Double) -> Math3d.V3
masToRadians :: (Double, Double, Double) -> V3
masToRadians (Double
rx, Double
ry, Double
rz) = V3 -> Double -> V3
Math3d.scale (Double -> Double -> Double -> V3
Math3d.vec3 Double
rx Double
ry Double
rz) (Double
forall a. Floating a => a
pi Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ (Double
3600.0 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
1000.0 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
180.0))
paramsAt :: Epoch -> Params15 -> Params7
paramsAt :: Epoch -> Params15 -> Params7
paramsAt (Epoch Double
e) (Params15 (Epoch Double
pe) (Params7 V3
c Double
s V3
r) (Rates V3
rc Double
rs V3
rr)) = V3 -> Double -> V3 -> Params7
Params7 V3
c' Double
s' V3
r'
where
de :: Double
de = Double
e Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
pe
c' :: V3
c' = V3 -> V3 -> V3
Math3d.add V3
c (V3 -> Double -> V3
Math3d.scale V3
rc Double
de)
s' :: Double
s' = Double
s Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
de Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
rs
r' :: V3
r' = V3 -> V3 -> V3
Math3d.add V3
r (V3 -> Double -> V3
Math3d.scale V3
rr Double
de)
data Connection =
Connection
{ Connection -> ModelId
node :: !ModelId
, Connection -> [ModelId]
adjacents :: ![ModelId]
}
data Edge a =
Edge ModelId a ModelId
type Path = [ModelId]
data State =
State [ModelId] [Path]
data Graph a =
Graph ![Connection] ![Edge a]
graph :: (Params a) => [Tx a] -> Graph a
graph :: [Tx a] -> Graph a
graph = (Graph a -> Tx a -> Graph a) -> Graph a -> [Tx a] -> Graph a
forall (t :: * -> *) b a.
Foldable t =>
(b -> a -> b) -> b -> t a -> b
foldl' Graph a -> Tx a -> Graph a
forall a. Params a => Graph a -> Tx a -> Graph a
addTx Graph a
forall a. Graph a
emptyGraph
paramsBetween :: (Params a) => ModelId -> ModelId -> Graph a -> [a]
paramsBetween :: ModelId -> ModelId -> Graph a -> [a]
paramsBetween ModelId
m0 ModelId
m1 Graph a
g
| ModelId
m0 ModelId -> ModelId -> Bool
forall a. Eq a => a -> a -> Bool
== ModelId
m1 = [a
forall a. Params a => a
idParams]
| [ModelId] -> Bool
forall (t :: * -> *) a. Foldable t => t a -> Bool
null [ModelId]
ms = []
| Bool
otherwise = [ModelId] -> Graph a -> [a]
forall a. [ModelId] -> Graph a -> [a]
findParams [ModelId]
ms Graph a
g
where
ms :: [ModelId]
ms = State -> ModelId -> Graph a -> [ModelId]
forall a. State -> ModelId -> Graph a -> [ModelId]
dijkstra ([ModelId] -> [[ModelId]] -> State
State [ModelId
m0] []) ModelId
m1 Graph a
g
emptyGraph :: Graph a
emptyGraph :: Graph a
emptyGraph = [Connection] -> [Edge a] -> Graph a
forall a. [Connection] -> [Edge a] -> Graph a
Graph [] []
addTx :: (Params a) => Graph a -> Tx a -> Graph a
addTx :: Graph a -> Tx a -> Graph a
addTx (Graph [Connection]
cs [Edge a]
es) Tx a
t = [Connection] -> [Edge a] -> Graph a
forall a. [Connection] -> [Edge a] -> Graph a
Graph [Connection]
cs' [Edge a]
es'
where
ma :: ModelId
ma = Tx a -> ModelId
forall a. Tx a -> ModelId
modelA Tx a
t
mb :: ModelId
mb = Tx a -> ModelId
forall a. Tx a -> ModelId
modelB Tx a
t
cs1 :: [Connection]
cs1 = [Connection] -> ModelId -> ModelId -> [Connection]
addConnection [Connection]
cs ModelId
ma ModelId
mb
cs' :: [Connection]
cs' = [Connection] -> ModelId -> ModelId -> [Connection]
addConnection [Connection]
cs1 ModelId
mb ModelId
ma
txp :: a
txp = Tx a -> a
forall a. Tx a -> a
params Tx a
t
es' :: [Edge a]
es' = ModelId -> a -> ModelId -> Edge a
forall a. ModelId -> a -> ModelId -> Edge a
Edge ModelId
ma a
txp ModelId
mb Edge a -> [Edge a] -> [Edge a]
forall a. a -> [a] -> [a]
: ModelId -> a -> ModelId -> Edge a
forall a. ModelId -> a -> ModelId -> Edge a
Edge ModelId
mb (a -> a
forall a. Params a => a -> a
inverseParams a
txp) ModelId
ma Edge a -> [Edge a] -> [Edge a]
forall a. a -> [a] -> [a]
: [Edge a]
es
addConnection :: [Connection] -> ModelId -> ModelId -> [Connection]
addConnection :: [Connection] -> ModelId -> ModelId -> [Connection]
addConnection [Connection]
cs ModelId
m1 ModelId
m2
| [Connection] -> Bool
forall (t :: * -> *) a. Foldable t => t a -> Bool
null [Connection]
filtered = ModelId -> [ModelId] -> Connection
Connection ModelId
m1 [ModelId
m2] Connection -> [Connection] -> [Connection]
forall a. a -> [a] -> [a]
: [Connection]
cs
| Bool
otherwise =
(Connection -> Connection) -> [Connection] -> [Connection]
forall a b. (a -> b) -> [a] -> [b]
map
(\Connection
c' ->
if Connection -> ModelId
node Connection
c' ModelId -> ModelId -> Bool
forall a. Eq a => a -> a -> Bool
== ModelId
m1
then Connection
updated
else Connection
c')
[Connection]
cs
where
filtered :: [Connection]
filtered = (Connection -> Bool) -> [Connection] -> [Connection]
forall a. (a -> Bool) -> [a] -> [a]
filter (\Connection
c -> Connection -> ModelId
node Connection
c ModelId -> ModelId -> Bool
forall a. Eq a => a -> a -> Bool
== ModelId
m1) [Connection]
cs
cur :: Connection
cur = [Connection] -> Connection
forall a. [a] -> a
head [Connection]
filtered
updated :: Connection
updated = Connection
cur {adjacents :: [ModelId]
adjacents = ModelId
m2 ModelId -> [ModelId] -> [ModelId]
forall a. a -> [a] -> [a]
: Connection -> [ModelId]
adjacents Connection
cur}
successors :: ModelId -> Graph a -> [ModelId]
successors :: ModelId -> Graph a -> [ModelId]
successors ModelId
m (Graph [Connection]
cs [Edge a]
_) = (Connection -> [ModelId]) -> [Connection] -> [ModelId]
forall (t :: * -> *) a b. Foldable t => (a -> [b]) -> t a -> [b]
concatMap Connection -> [ModelId]
adjacents ((Connection -> Bool) -> [Connection] -> [Connection]
forall a. (a -> Bool) -> [a] -> [a]
filter (\Connection
c -> Connection -> ModelId
node Connection
c ModelId -> ModelId -> Bool
forall a. Eq a => a -> a -> Bool
== ModelId
m) [Connection]
cs)
visit :: ModelId -> [ModelId] -> State -> State
visit :: ModelId -> [ModelId] -> State -> State
visit ModelId
f [ModelId]
ms (State [ModelId]
q0 [[ModelId]]
v0) = [ModelId] -> [[ModelId]] -> State
State [ModelId]
q1 [[ModelId]]
v1
where
toVisit :: [ModelId]
toVisit = (ModelId -> Bool) -> [ModelId] -> [ModelId]
forall a. (a -> Bool) -> [a] -> [a]
filter (ModelId -> [ModelId] -> Bool
forall (t :: * -> *) a. (Foldable t, Eq a) => a -> t a -> Bool
`notElem` [[ModelId]] -> [ModelId]
forall (t :: * -> *) a. Foldable t => t [a] -> [a]
concat [[ModelId]]
v0) [ModelId]
ms
fs :: [[ModelId]]
fs = ([ModelId] -> Bool) -> [[ModelId]] -> [[ModelId]]
forall a. (a -> Bool) -> [a] -> [a]
filter (\[ModelId]
v -> [ModelId] -> ModelId
forall a. [a] -> a
head [ModelId]
v ModelId -> ModelId -> Bool
forall a. Eq a => a -> a -> Bool
== ModelId
f) [[ModelId]]
v0
q1 :: [ModelId]
q1 = [ModelId]
q0 [ModelId] -> [ModelId] -> [ModelId]
forall a. [a] -> [a] -> [a]
++ [ModelId]
toVisit
updatedPaths :: [[ModelId]]
updatedPaths = ([ModelId] -> [[ModelId]]) -> [[ModelId]] -> [[ModelId]]
forall (t :: * -> *) a b. Foldable t => (a -> [b]) -> t a -> [b]
concatMap (\[ModelId]
x -> (ModelId -> [ModelId]) -> [ModelId] -> [[ModelId]]
forall a b. (a -> b) -> [a] -> [b]
map (ModelId -> [ModelId] -> [ModelId]
forall a. a -> [a] -> [a]
: [ModelId]
x) [ModelId]
toVisit) [[ModelId]]
fs
v1 :: [[ModelId]]
v1 = [[ModelId]]
updatedPaths [[ModelId]] -> [[ModelId]] -> [[ModelId]]
forall a. [a] -> [a] -> [a]
++ ([ModelId] -> Bool) -> [[ModelId]] -> [[ModelId]]
forall a. (a -> Bool) -> [a] -> [a]
filter (\[ModelId]
v -> [ModelId] -> ModelId
forall a. [a] -> a
head [ModelId]
v ModelId -> ModelId -> Bool
forall a. Eq a => a -> a -> Bool
/= ModelId
f) [[ModelId]]
v0
shortest :: ModelId -> ModelId -> [Path] -> [ModelId]
shortest :: ModelId -> ModelId -> [[ModelId]] -> [ModelId]
shortest ModelId
c ModelId
m [[ModelId]]
ps = [ModelId] -> [ModelId]
forall a. [a] -> [a]
reverse (ModelId
m ModelId -> [ModelId] -> [ModelId]
forall a. a -> [a] -> [a]
: [ModelId]
s)
where
fs :: [[ModelId]]
fs = ([ModelId] -> Bool) -> [[ModelId]] -> [[ModelId]]
forall a. (a -> Bool) -> [a] -> [a]
filter (\[ModelId]
v -> [ModelId] -> ModelId
forall a. [a] -> a
head [ModelId]
v ModelId -> ModelId -> Bool
forall a. Eq a => a -> a -> Bool
== ModelId
c) [[ModelId]]
ps
s :: [ModelId]
s = [[ModelId]] -> [ModelId]
forall a. [a] -> a
head (([ModelId] -> Int) -> [[ModelId]] -> [[ModelId]]
forall b a. Ord b => (a -> b) -> [a] -> [a]
sortOn [ModelId] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [[ModelId]]
fs)
dijkstra :: State -> ModelId -> Graph a -> [ModelId]
dijkstra :: State -> ModelId -> Graph a -> [ModelId]
dijkstra (State [] [[ModelId]]
_) ModelId
_ Graph a
_ = []
dijkstra (State [ModelId
c] []) ModelId
t Graph a
g = State -> ModelId -> Graph a -> [ModelId]
forall a. State -> ModelId -> Graph a -> [ModelId]
dijkstra ([ModelId] -> [[ModelId]] -> State
State [ModelId
c] [[ModelId
c]]) ModelId
t Graph a
g
dijkstra (State (ModelId
c:[ModelId]
r) [[ModelId]]
v) ModelId
t Graph a
g
| ModelId
t ModelId -> [ModelId] -> Bool
forall (t :: * -> *) a. (Foldable t, Eq a) => a -> t a -> Bool
`elem` [ModelId]
succs = ModelId -> ModelId -> [[ModelId]] -> [ModelId]
shortest ModelId
c ModelId
t [[ModelId]]
v
| Bool
otherwise = State -> ModelId -> Graph a -> [ModelId]
forall a. State -> ModelId -> Graph a -> [ModelId]
dijkstra State
s'' ModelId
t Graph a
g
where
s' :: State
s' = [ModelId] -> [[ModelId]] -> State
State [ModelId]
r [[ModelId]]
v
succs :: [ModelId]
succs = ModelId -> Graph a -> [ModelId]
forall a. ModelId -> Graph a -> [ModelId]
successors ModelId
c Graph a
g
s'' :: State
s'' = ModelId -> [ModelId] -> State -> State
visit ModelId
c [ModelId]
succs State
s'
findParams :: [ModelId] -> Graph a -> [a]
findParams :: [ModelId] -> Graph a -> [a]
findParams [ModelId]
ms (Graph [Connection]
_ [Edge a]
es)
| [(ModelId, ModelId)] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [(ModelId, ModelId)]
ps Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [a]
r = [a]
r
| Bool
otherwise = []
where
ps :: [(ModelId, ModelId)]
ps = [ModelId] -> [ModelId] -> [(ModelId, ModelId)]
forall a b. [a] -> [b] -> [(a, b)]
zip [ModelId]
ms ([ModelId] -> [ModelId]
forall a. [a] -> [a]
tail [ModelId]
ms)
r :: [a]
r = ((ModelId, ModelId) -> Maybe a) -> [(ModelId, ModelId)] -> [a]
forall a b. (a -> Maybe b) -> [a] -> [b]
mapMaybe ((ModelId, ModelId) -> [Edge a] -> Maybe a
forall a. (ModelId, ModelId) -> [Edge a] -> Maybe a
`findParam` [Edge a]
es) [(ModelId, ModelId)]
ps
findParam :: (ModelId, ModelId) -> [Edge a] -> Maybe a
findParam :: (ModelId, ModelId) -> [Edge a] -> Maybe a
findParam (ModelId, ModelId)
p [Edge a]
es = (Edge a -> a) -> Maybe (Edge a) -> Maybe a
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap (\(Edge ModelId
_ a
pa ModelId
_) -> a
pa) ((Edge a -> Bool) -> [Edge a] -> Maybe (Edge a)
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Maybe a
find ((ModelId, ModelId) -> Edge a -> Bool
forall a. (ModelId, ModelId) -> Edge a -> Bool
edgeEq (ModelId, ModelId)
p) [Edge a]
es)
edgeEq :: (ModelId, ModelId) -> Edge a -> Bool
edgeEq :: (ModelId, ModelId) -> Edge a -> Bool
edgeEq (ModelId
m1, ModelId
m2) (Edge ModelId
m1' a
_ ModelId
m2') = ModelId
m1 ModelId -> ModelId -> Bool
forall a. Eq a => a -> a -> Bool
== ModelId
m1' Bool -> Bool -> Bool
&& ModelId
m2 ModelId -> ModelId -> Bool
forall a. Eq a => a -> a -> Bool
== ModelId
m2'
apply :: Math3d.V3 -> Params7 -> Math3d.V3
apply :: V3 -> Params7 -> V3
apply V3
gc (Params7 V3
c Double
s V3
r) = V3 -> V3 -> V3
Math3d.add V3
c (V3 -> Double -> V3
Math3d.scale (V3 -> [V3] -> V3
Math3d.multM V3
gc (V3 -> [V3]
rotation V3
r)) (Double
1.0 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
s))
rotation :: Math3d.V3 -> [Math3d.V3]
rotation :: V3 -> [V3]
rotation V3
v = [Double -> Double -> Double -> V3
Math3d.vec3 Double
1.0 (-Double
z) Double
y, Double -> Double -> Double -> V3
Math3d.vec3 Double
z Double
1.0 (-Double
x), Double -> Double -> Double -> V3
Math3d.vec3 (-Double
y) Double
x Double
1.0]
where
x :: Double
x = V3 -> Double
Math3d.v3x V3
v
y :: Double
y = V3 -> Double
Math3d.v3y V3
v
z :: Double
z = V3 -> Double
Math3d.v3z V3
v