Rotation matrix to angle is fun

Also arrows can point two different ways,
so need to correct for that in TilePositioner.

package.yaml

@@ -33,6 +33,7 @@ dependencies:
 - mtl
 - data-default
 - text
+- lens
 
 library:
   source-dirs: src

src/TilePositioner.hs

@@ -47,22 +47,25 @@ positionTile frame segment =
 mean :: Fractional a => V.Vector a -> a
 mean xs = V.sum xs / realToFrac (V.length xs)
 
-angleFromPoints :: V2 Double -> V2 Double-> Double
-angleFromPoints (V2 x0 y0) (V2 x1 y1) = atan2 (y1 - y0) (x1 - x0)
+angleFromPoints :: V2 (V2 Double) -> Double
+angleFromPoints (V2 (V2 x0 y0) (V2 x1 y1)) =
+  let a = atan2 (y1 - y0) (x1 - x0)
+  in if a >= 0 then a else (pi*2) + a
 
-pointsFromLineSegment :: LineSegment Int32 -> (V2 Double, V2 Double)
-pointsFromLineSegment (LineSegment p0 p1) = ((realToFrac <$>) p0, (realToFrac <$>) p1)
+pointsFromLineSegment :: LineSegment Int32 -> V2 (V2 Double)
+pointsFromLineSegment (LineSegment p0 p1) = V2 ((realToFrac <$>) p0) ((realToFrac <$>) p1)
 
+-- clockwise rotation matrix, aka left handed, aka y axes goes down
 transformFromAngle :: Double -> Transform
 transformFromAngle angle = V2 (V2 (cos angle) (sin angle)) (V2 (-(sin angle)) (cos angle))
 
 transformTile :: FrameMat -> Segment -> Transform
 transformTile frame s@(Segment Straight p t) =
   let lines = candidateLines s frame
-      angle = traceShowId $ angleFromTransform t
-      lineAngle = traceShowId $ uncurry angleFromPoints $ pointsFromLineSegment $ fst $ V.head lines
-      angle' = if abs(angle - lineAngle) < pi/2 then lineAngle else (lineAngle + pi) `mod'` 2*pi
-      t' = (^* trackWidth t) <$> transformFromAngle angle'
+      angle = angleFromTransform t
+      lineAngle = angleFromPoints $ pointsFromLineSegment $ fst $ V.head lines
+      -- angle' = if abs(angle - lineAngle) < pi/2 then lineAngle else (lineAngle + pi) `mod'` 2*pi
+      t' = (^* trackWidth t) <$> transformFromAngle lineAngle
     in if V.null lines then t else t'
 
 transformTile frame (Segment tile p t) = t

src/TilePositionerDebug.hs

@@ -1,18 +1,20 @@
 module TilePositionerDebug where
 
 import Prelude hiding (Left, lines)
+import Control.Lens
 import Control.Monad.Except(MonadError, void)
 import Control.Monad.Primitive
 import Data.Foldable
 import Data.Int
 import Data.Proxy
 import Data.Word
-import Data.Vector as V
+import Data.Vector as V hiding ((++))
 import Linear
 import OpenCV as CV
 import OpenCV.Extra.XFeatures2d
 import OpenCV.Internal.C.Types
 import OpenCV.ImgProc.FeatureDetection
+import qualified Data.Text as T
 
 import TilePositioner
 import Track
@@ -28,8 +30,11 @@ positionLineDebug frame (Segment tile p t) = exceptError $ do
   withMatM (h ::: w ::: Z) (Proxy :: Proxy 3) (Proxy :: Proxy Word8) white $ \imgM -> do
     void $ matCopyToM imgM zero frame Nothing
     let dot = round $ trackWidth t / 32.0
-    for_ (candidateLines (Segment tile p t) frame) $
-      \(lineSegment, edge) -> line imgM (lineSegmentStart lineSegment) (lineSegmentStop  lineSegment) (edgeColor edge) 2 LineType_8 0
+        putText' str pos color = putText imgM (T.pack str) pos (Font FontHersheySimplex NotSlanted 0.3) color 1 LineType_AA False
+        showAngle angle = show (round $ angle / (2*pi) * 360)
+    for_ (candidateLines (Segment tile p t) frame) $ \(lineSegment, edge) -> do
+      arrowedLine imgM (lineSegmentStart lineSegment) (lineSegmentStop  lineSegment) (edgeColor edge) 1 LineType_AA 0 0.15
+      putText' (showAngle $ angleFromPoints $ pointsFromLineSegment lineSegment) ((round <$>) <$> (^._x) $ pointsFromLineSegment lineSegment :: V2 Int32) (edgeColor edge)
 
     circle imgM  (round <$> p) dot white (-1) LineType_AA 0
     drawSegmentArrow imgM white (Segment tile p t)

src/Track.hs

@@ -27,13 +27,14 @@ data Segment = Segment
 
 angleFromTransform :: Transform -> Double
 angleFromTransform t =
-  let V2 t_x t_y = (t !* V2 (-1) 0)
-  in atan2 t_y t_x
+  let V2 t_x t_y = (t !* V2 1 0)
+      a = -atan2 t_y t_x
+  in if a >= 0 then a else (pi*2) + a
 
 instance Show Segment where
   show (Segment tile p t) =
     let V2 x y = p
-        angle = round $ 180 + angleFromTransform t / pi * 180
+        angle = round $ angleFromTransform t / (2*pi) * 360
       in show tile ++ " " ++ show (round x) ++ "×" ++ show (round y) ++ " " ++ show angle ++ "°"
 
 type Track = Loop.Loop Segment

test/Spec.hs

@@ -302,6 +302,7 @@ trackTests = testGroup "Track tests"
   , testCase "shows" trackShow
   , testCase "moves" trackMoves
   , testCase "loops" trackLoops
+  , testCase "transform" trackTransform
   ]
 
 testTrack = fromJust $ Track.parseTrack Track.start "srrsrr"
@@ -365,3 +366,16 @@ trackLoops = let
   Track.Segment tile p t = end
   -- TODO handle rounding error
   in True @?= True -- Track.nextSegment end Track.Straight @?= Track.start
+
+trackTransform :: Assertion
+trackTransform = do
+  transformFromAngle 0 @?= V2 (V2 1 0) (V2 0 1)
+  (round <$>) <$> transformFromAngle (pi / 2) @?= V2 (V2 0 1) (V2 (-1) 0)
+  (round <$>) <$> transformFromAngle pi @?= V2 (V2 (-1) 0) (V2 0 (-1))
+  (round <$>) <$> transformFromAngle (3* (pi/2)) @?= V2 (V2 0 (-1)) (V2 1 0)
+
+  Track.angleFromTransform (V2 (V2 1 0) (V2 0 1)) @?= 0
+  Track.angleFromTransform (V2 (V2 0 1) (V2 (-1) 0)) @?= (pi/2)
+  Track.angleFromTransform (transformFromAngle 0) @?= 0
+  Track.angleFromTransform (transformFromAngle (pi/2)) @?= (pi/2)
+  Track.angleFromTransform (transformFromAngle 2) @?= 2