fitting a cubic Bézier to a sequence of points

* WIP: current code exhibits convergence issues / instability

MetaBrush.cabal

@@ -36,6 +36,8 @@ common common
           >= 0.8.0.0 && < 0.8.4.0
       , groups
           ^>= 0.4.1.0
+      , transformers
+          ^>= 0.5.6.2
 
     default-language:
         Haskell2010
@@ -62,17 +64,20 @@ library
 
     exposed-modules:
         Math.Bezier.Cubic
+      , Math.Bezier.Cubic.Fit
       , Math.Bezier.Quadratic
       , Math.Bezier.Stroke
-      , Math.Bezier.Subdivision
       , Math.Epsilon
+      , Math.Linear.SVD
       , Math.Module
-      , Math.RealRoots
+      , Math.Roots
       , Math.Vector2D
 
     build-depends:
         groups-generic
           ^>= 0.1.0.0
+      , vector
+          ^>= 0.12.1.2
 
 executable MetaBrush
 
@@ -147,6 +152,4 @@ executable MetaBrush
       , tardis
           ^>= 0.4.1.0
       , text
-          ^>= 1.2.3.1 && < 1.2.5
-      , transformers
-          ^>= 0.5.6.2
+          ^>= 1.2.3.1 && < 1.2.5

src/app/MetaBrush/Asset/Colours.hs

@@ -30,31 +30,14 @@ import Data.Text
 
 data ColourRecord a
   = Colours
-  { bg                  :: !a
-  , active              :: !a
-  , highlight           :: !a
-  , cursor              :: !a
-  , cursorOutline       :: !a
-  , plain               :: !a
-  , base                :: !a
-  , splash              :: !a
-  , pathPoint           :: !a
-  , pathPointOutline    :: !a
-  , controlPoint        :: !a
-  , controlPointOutline :: !a
-  , path                :: !a
-  , brush               :: !a
-  , brushStroke         :: !a
-  , brushCenter         :: !a
-  , pointHover          :: !a
-  , pointSelected       :: !a
-  , viewport            :: !a
-  , viewportScrollbar   :: !a
-  , tabScrollbar        :: !a
-  , magnifier           :: !a
-  , glass               :: !a
-  , selected            :: !a
-  , selectedOutline     :: !a
+  { bg, active, highlight, cursor, cursorOutline      
+  , plain, base, splash             
+  , pathPoint, pathPointOutline, controlPoint, controlPointOutline
+  , path, brush, brushStroke, brushCenter        
+  , pointHover, pointSelected      
+  , viewport, viewportScrollbar, tabScrollbar       
+  , magnifier, glass
+  , selected, selectedOutline :: !a
   }
   deriving stock ( Show, Functor, Foldable, Traversable )
 

src/app/MetaBrush/Document.hs

@@ -58,9 +58,7 @@ import MetaBrush.Unique
 
 data AABB
   = AABB
-  { topLeft  :: !( Point2D Double )
-  , botRight :: !( Point2D Double )
-  }
+  { topLeft, botRight :: !( Point2D Double ) }
   deriving stock Show
 
 data Document

src/app/MetaBrush/Event.hs

@@ -257,13 +257,13 @@ handleScrollEvent
               | dx == 0 && any ( \ key -> key == Shift_L || key == Shift_R ) pressedKeys
               = let
                   newCenter :: Point2D Double
-                  newCenter = ( ( 25 / oldZoomFactor ) *^ Vector2D ( Point2D dy 0 ) ) • oldCenter
+                  newCenter = ( ( 25 / oldZoomFactor ) *^ Vector2D dy 0 ) • oldCenter
                 in doc { viewportCenter = newCenter }
               -- Vertical scrolling.
               | otherwise
               = let
                   newCenter :: Point2D Double
-                  newCenter = ( ( 25 / oldZoomFactor ) *^ Vector2D ( Point2D dx dy ) ) • oldCenter
+                  newCenter = ( ( 25 / oldZoomFactor ) *^ Vector2D dx dy ) • oldCenter
                 in doc { viewportCenter = newCenter }
             docs' :: IntMap Document
             docs' = IntMap.insert i newDoc docs

src/app/MetaBrush/Render/Document.hs

@@ -89,12 +89,8 @@ import MetaBrush.UI.ToolBar
 
 data Renders a
   = Renders
-  { renderPath    :: a
-  , renderStrokes :: a
-  , renderBrushes :: a
-  , renderCPts    :: a
-  , renderCLines  :: a
-  , renderPPts    :: a
+  { renderPath, renderStrokes, renderBrushes
+  , renderCPts, renderCLines, renderPPts :: a
   }
   deriving stock ( Show, Functor, Foldable, Traversable, Generic, Generic1 )
   deriving Applicative

src/app/MetaBrush/UI/InfoBar.hs

@@ -51,18 +51,14 @@ import MetaBrush.Render.Util
 
 data InfoBar
   = InfoBar
-  { zoomText        :: !GTK.Label -- make this editable
-  , cursorPosText   :: !GTK.Label
-  , topLeftPosText  :: !GTK.Label
-  , botRightPosText :: !GTK.Label
+  { zoomText :: !GTK.Label -- make this editable
+  , cursorPosText, topLeftPosText, botRightPosText :: !GTK.Label
   }
 
 data InfoData
   = InfoData
-  { zoom        :: !Double
-  , mousePos    :: !( Point2D Double )
-  , topLeftPos  :: !( Point2D Double )
-  , botRightPos :: !( Point2D Double )
+  { zoom :: !Double
+  , mousePos, topLeftPos, botRightPos :: !( Point2D Double )
   }
   deriving stock Show
 

src/app/MetaBrush/UI/Menu.hs

@@ -110,40 +110,26 @@ data Menu ( rt :: ResourceType )
 
 data FileMenu ( rt :: ResourceType )
   = FileMenu
-  { new    :: !( MenuItem NoSubresource rt )
-  , open   :: !( MenuItem NoSubresource rt )
-  , save   :: !( MenuItem NoSubresource rt )
-  , saveAs :: !( MenuItem NoSubresource rt )
-  , close  :: !( MenuItem NoSubresource rt )
-  , quit   :: !( MenuItem NoSubresource rt )
-  }
+  { new, open, save, saveAs, close, quit :: !( MenuItem NoSubresource rt ) }
   deriving stock Generic
 
 data EditMenu ( rt :: ResourceType )
   = EditMenu
-  { undo        :: !( MenuItem  NoSubresource rt )
-  , redo        :: !( MenuItem  NoSubresource rt )
-  , history     :: !( MenuItem  NoSubresource rt )
-  , editSep1    :: !( Separator rt )
-  , cut         :: !( MenuItem  NoSubresource rt )
-  , copy        :: !( MenuItem  NoSubresource rt )
-  , paste       :: !( MenuItem  NoSubresource rt )
-  , duplicate   :: !( MenuItem  NoSubresource rt )
-  , delete      :: !( MenuItem  NoSubresource rt )
-  , editSep2    :: !( Separator rt )
+  { undo, redo, history :: !( MenuItem  NoSubresource rt )
+  , editSep1 :: !( Separator rt )
+  , cut, copy, paste, duplicate, delete :: !( MenuItem  NoSubresource rt )
+  , editSep2 :: !( Separator rt )
   , preferences :: !( MenuItem  NoSubresource rt )
   }
   deriving stock Generic
 
 data ViewMenu ( rt :: ResourceType )
   = ViewMenu
-  { navigator      :: !( MenuItem  NoSubresource rt )
-  , viewSep1       :: !( Separator rt )
-  , strokes        :: !( MenuItem  NoSubresource rt )
-  , brushes        :: !( MenuItem  NoSubresource rt )
-  , metaparameters :: !( MenuItem  NoSubresource rt )
-  , viewSep2       :: !( Separator rt )
-  , transform      :: !( MenuItem  NoSubresource rt )
+  { navigator :: !( MenuItem  NoSubresource rt )
+  , viewSep1  :: !( Separator rt )
+  , strokes, brushes, metaparameters :: !( MenuItem  NoSubresource rt )
+  , viewSep2  :: !( Separator rt )
+  , transform :: !( MenuItem  NoSubresource rt )
   }
   deriving stock Generic
 

src/app/MetaBrush/UI/ToolBar.hs

@@ -54,12 +54,7 @@ data Mode
 
 data ToolBar
   = ToolBar
-  { selectionTool :: !GTK.RadioButton
-  , penTool       :: !GTK.RadioButton
-  , pathTool      :: !GTK.RadioButton
-  , brushTool     :: !GTK.RadioButton
-  , metaTool      :: !GTK.RadioButton
-  }
+  { selectionTool, penTool, pathTool, brushTool, metaTool :: !GTK.RadioButton }
 
 createToolBar :: STM.TVar Tool -> STM.TVar Mode -> Colours -> GTK.DrawingArea -> GTK.Box -> IO ToolBar
 createToolBar toolTVar modeTVar colours drawingArea toolBar = do

src/lib/Math/Bezier/Cubic.hs

@@ -13,7 +13,7 @@ module Math.Bezier.Cubic
   ( Bezier(..)
   , bezier, bezier'
   , subdivide
-  , closestPoint
+  , ddist, closestPoint
   )
   where
 
@@ -39,21 +39,17 @@ import Math.Module
   , lerp
   , Inner(..), squaredNorm
   )
-import Math.RealRoots
+import Math.Roots
   ( realRoots )
 
 --------------------------------------------------------------------------------
 
--- | Points defining a cubic Bézier curve.
+-- | Points defining a cubic Bézier curve (Bernstein form).
 --
 -- @ p0 @ and @ p3 @ are endpoints, whereas @ p1 @ and @ p2 @ are control points.
 data Bezier p
   = Bezier
-    { p0 :: !p
-    , p1 :: !p
-    , p2 :: !p
-    , p3 :: !p
-    }
+    { p0, p1, p2, p3 :: !p }
   deriving stock ( Show, Generic, Functor, Foldable, Traversable )
 
 -- | Cubic Bézier curve.
@@ -83,13 +79,10 @@ subdivide ( Bezier { .. } ) t = ( Bezier p0 q1 q2 pt, Bezier pt r1 r2 p3 )
     r1 = lerp @v t s  r2
     pt = lerp @v t q2 r1
 
--- | Finds the closest point to a given point on a cubic Bézier curve.
-closestPoint :: forall v r p. ( Torsor v p, Inner r v, RealFloat r ) => Bezier p -> p -> ArgMin r ( r, p )
-closestPoint pts@( Bezier { .. } ) c = pickClosest ( 0 :| 1 : roots )
+-- | Polynomial coefficients of the derivative of the distance to a cubic Bézier curve.
+ddist :: forall v r p. ( Torsor v p, Inner r v, RealFloat r ) => Bezier p -> p -> [ r ]
+ddist ( Bezier { .. } ) c = [ a0, a1, a2, a3, a4, a5 ]
   where
-    roots :: [ r ]
-    roots = filter ( \ r -> r > 0 && r < 1 ) ( realRoots [ a0, a1, a2, a3, a4, a5 ] )
-
     v, v', v'', v''' :: v
     v    = c  --> p0
     v'   = p0 --> p1
@@ -99,11 +92,18 @@ closestPoint pts@( Bezier { .. } ) c = pickClosest ( 0 :| 1 : roots )
     a0, a1, a2, a3, a4, a5 :: r
     a0 = v ^.^ v'
     a1 = 3 * squaredNorm v' + 2 * v ^.^ v''
-    a2 = 9 * v' ^.^ v'' + 3 * v ^.^ v'''
+    a2 = 9 * v' ^.^ v'' + v ^.^ v'''
     a3 = 6 * squaredNorm v'' + 4 * v' ^.^ v'''
     a4 = 5 * v'' ^.^ v'''
     a5 = squaredNorm v'''
 
+-- | Finds the closest point to a given point on a cubic Bézier curve.
+closestPoint :: forall v r p. ( Torsor v p, Inner r v, RealFloat r ) => Bezier p -> p -> ArgMin r ( r, p )
+closestPoint pts@( Bezier { .. } ) c = pickClosest ( 0 :| 1 : roots )
+  where
+    roots :: [ r ]
+    roots = filter ( \ r -> r > 0 && r < 1 ) ( realRoots $ ddist @v pts c )
+
     pickClosest :: NonEmpty r -> ArgMin r ( r, p )
     pickClosest ( s :| ss ) = go s q nm0 ss
       where

src/lib/Math/Bezier/Cubic/Fit.hs

@@ -0,0 +1,181 @@
+{-# LANGUAGE BlockArguments      #-}
+{-# LANGUAGE NamedFieldPuns      #-}
+{-# LANGUAGE ScopedTypeVariables #-}
+{-# LANGUAGE TypeApplications    #-}
+
+module Math.Bezier.Cubic.Fit where
+
+-- base
+import Control.Arrow
+  ( first, second )
+import Control.Monad
+  ( when )
+import Control.Monad.ST
+  ( ST, runST )
+import Data.Complex
+  ( Complex(..) )
+import Data.Foldable
+  ( for_ )
+
+-- acts
+import Data.Act
+  ( Act
+    ( (•) )
+  )
+
+-- transformers
+import Control.Monad.Trans.State.Strict
+  ( execStateT, modify' )
+import Control.Monad.Trans.Class
+  ( lift )
+
+-- vector
+import qualified Data.Vector.Unboxed.Mutable as Unboxed
+  ( MVector )
+import qualified Data.Vector.Unboxed.Mutable as Unboxed.MVector
+  ( unsafeRead, unsafeWrite )
+import qualified Data.Vector.Unboxed         as Unboxed.Vector
+  ( unsafeThaw, generate )
+
+-- MetaBrush
+import Math.Bezier.Cubic
+  ( Bezier(..), bezier, ddist )
+import Math.Epsilon
+  ( epsilon )
+import Math.Linear.SVD
+  ( lsolve )
+import Math.Module
+  ( Module((*^), (^-^))
+  , Inner((^.^)), quadrance
+  )
+import Math.Roots
+  ( laguerre ) --, eval, derivative )
+import Math.Vector2D
+  ( Mat22(..), Point2D(..), Vector2D(..) )
+
+--------------------------------------------------------------------------------
+
+-- | Fits a single cubic Bézier curve to the given data.
+--
+-- This will consist of a cubic Bézier curve which:
+--
+--   * starts at \( p \) with tangent \( \textrm{t}_p \),
+--   * ends at \( r \) with tangent \( \textrm{t}_r \),
+--   * best fits the intermediate sequence of points \( \left ( q_i \right )_{i=1}^n \).
+--
+-- /Note/: the order of the intermediate points is important.
+--
+-- Proceeds by fitting a cubic Bézier curve \( B(t) \), \( 0 \leqslant t \leqslant 1 \),
+-- with given endpoints and tangents, which minimises the sum of squares functional
+-- 
+-- \[ \sum_{i=1}^n \left \| C(t_i) - q_i \right \|^2. \]
+--
+-- The values of the parameters \( \left ( t_i \right )_{i=1}^n \) are recursively estimated,
+-- starting from uniform parametrisation.
+--
+-- The iteration ends when any of the following conditions are satisfied:
+--
+--   * each new estimated parameter values \( t_i' \) differs from
+--     its previous value \( t_i \) by less than \( \texttt{t_tol} \),
+--   * each point \( C(t_i) \) is within squared distance \( \texttt{sq_dist_tol} \)
+--     of the point \( q_i \) it is associated with,
+--   * the maximum iteration limit \( \texttt{maxCount} \) has been reached.
+fitPiece
+  :: Double             -- ^ \( \texttt{t_tol} \), the tolerance for the Bézier parameter
+  -> Double             -- ^ \( \texttt{sq_dist_tol} \), tolerance for the squared distance
+  -> Int                -- ^ \( \texttt{maxCount} \), maximum number of iterations
+  -> Point2D Double     -- ^ \( p \), start point
+  -> Vector2D Double    -- ^ \( \textrm{t}_p \), start tangent vector (length is ignored)
+  -> [ Point2D Double ] -- ^ \( \left ( q_i \right )_{i=1}^n \), points to fit
+  -> Point2D Double     -- ^ \( r \), end point
+  -> Vector2D Double    -- ^ \( \textrm{t}_r \), end tangent vector (length is ignored)
+  -> Bezier ( Point2D Double )
+fitPiece t_tol sq_dist_tol maxCount p tp qs r tr = piece
+  where
+    n :: Int
+    n = length qs
+    uniform :: Int -> Double
+    uniform i = fromIntegral ( i + 1 ) / fromIntegral ( n + 1 )
+
+    f0, f1, f2, f3 :: Double -> Vector2D Double
+    f0 t = h1 t *^ tp
+    f1 t = h2 t *^ tr
+    f2 t = h0 t *^ ( MkVector2D p )
+    f3 t = h3 t *^ ( MkVector2D r )
+    
+    piece :: Bezier ( Point2D Double )
+    piece = runST do
+      ts <- Unboxed.Vector.unsafeThaw ( Unboxed.Vector.generate n uniform )
+      loop ts 0
+
+    loop :: forall s. Unboxed.MVector s Double -> Int -> ST s ( Bezier ( Point2D Double ) )
+    loop ts count = do
+      let
+        hermiteParameters :: Mat22 Double -> Vector2D Double -> Int -> [ Point2D Double ] -> ST s ( Vector2D Double )
+        hermiteParameters ( Mat22 a11 a12 _ a22 ) ( Vector2D b1 b2 ) i ( q : rest ) = do
+          ti <- Unboxed.MVector.unsafeRead ts i
+          let
+            f0i, f1i, f2i, f3i :: Vector2D Double
+            f0i = f0 ti
+            f1i = f1 ti
+            f2i = f2 ti
+            f3i = f3 ti
+            q'  = MkVector2D q ^-^ f2i ^-^ f3i
+            a11', a12', a21', a22', b1', b2' :: Double
+            a11' = a11 + ( f0i ^.^ f0i )
+            a12' = a12 + ( f1i ^.^ f0i )
+            a21' = a12'
+            a22' = a22 + ( f1i ^.^ f1i )
+            b1'  = b1  + ( q'  ^.^ f0i )
+            b2'  = b2  + ( q'  ^.^ f1i )
+          hermiteParameters ( Mat22 a11' a12' a21' a22' ) ( Vector2D b1' b2' ) ( i + 1 ) rest
+        hermiteParameters a b _ [] = pure ( lsolve a b )
+      
+      Vector2D s1 s2 <- hermiteParameters ( Mat22 0 0 0 0 ) ( Vector2D 0 0 ) 0 qs
+
+      let
+        cp1, cp2 :: Point2D Double
+        cp1 = ( (   s1  / 3 ) *^ tp ) • p
+        cp2 = ( ( (-s2) / 3 ) *^ tr ) • r
+
+        bez :: Bezier ( Point2D Double )
+        bez = Bezier p cp1 cp2 r
+
+      if count >= maxCount
+      then pure bez
+      else do
+
+        -- Run one iteration of Laguerre's method to improve the parameter values t_i,
+        -- so that t_i' is a better approximation of the parameter
+        -- at which the curve is closest to q_i.
+        ( ts_ok, pts_ok ) <- ( `execStateT` ( True, True ) ) $ for_ ( zip qs [ 0 .. ] ) \( q, i ) -> do
+          ti <- lift ( Unboxed.MVector.unsafeRead ts i )
+          let
+            poly :: [ Complex Double ]
+            poly = map (:+ 0) $ ddist @( Vector2D Double ) bez q
+            ti' :: Double
+            ti' = case laguerre epsilon 1 poly ( ti :+ 0 ) of
+              x :+ y
+                |  abs y > epsilon
+                || isNaN x
+                || isNaN y
+                -> ti
+                | otherwise
+                -> x
+
+          when ( abs ( ti' - ti ) > t_tol )
+            ( modify' ( first  ( const False ) ) )
+          when ( quadrance @( Vector2D Double ) q ( bezier @( Vector2D Double ) bez ti' ) > sq_dist_tol )
+            ( modify' ( second ( const False ) ) )
+          lift ( Unboxed.MVector.unsafeWrite ts i ti' )
+
+        if ts_ok || pts_ok 
+        then pure bez
+        else loop ts ( count + 1 )
+
+-- | Cubic Hermite polynomial.
+h0, h1, h2, h3 :: Num t => t -> t
+h0 t = ( 1 + 2 * t )    * ( t - 1 ) ^ ( 2 :: Int )
+h1 t = t                * ( t - 1 ) ^ ( 2 :: Int )
+h2 t = t ^ ( 2 :: Int ) * ( t - 1 )
+h3 t = t ^ ( 2 :: Int ) * ( 3 - 2 * t )

src/lib/Math/Bezier/Quadratic.hs

@@ -13,7 +13,7 @@ module Math.Bezier.Quadratic
   ( Bezier(..)
   , bezier, bezier'
   , subdivide
-  , closestPoint
+  , ddist, closestPoint
   )
   where
 
@@ -37,20 +37,17 @@ import Math.Module
   , lerp
   , Inner(..), squaredNorm
   )
-import Math.RealRoots
+import Math.Roots
   ( realRoots )
 
 --------------------------------------------------------------------------------
 
--- | Points defining a quadratic Bézier curve.
+-- | Points defining a quadratic Bézier curve (Bernstein form).
 --
 -- @ p0 @ and @ p2 @ are endpoints, whereas @ p1 @ is a control point.
 data Bezier p
   = Bezier
-    { p0 :: !p
-    , p1 :: !p
-    , p2 :: !p
-    }
+    { p0, p1, p2 :: !p }
   deriving stock ( Show, Generic, Functor, Foldable, Traversable )
 
 -- | Quadratic Bézier curve.
@@ -70,13 +67,10 @@ subdivide ( Bezier { .. } ) t = ( Bezier p0 q1 pt, Bezier pt r1 p2 )
     r1 = lerp @v t p1 p2
     pt = lerp @v t q1 r1
 
--- | Finds the closest point to a given point on a quadratic Bézier curve.
-closestPoint :: forall v r p. ( Torsor v p, Inner r v, RealFloat r ) => Bezier p -> p -> ArgMin r ( r, p )
-closestPoint pts@( Bezier { .. } ) c = pickClosest ( 0 :| 1 : roots )
+-- | Polynomial coefficients of the derivative of the distance to a quadratic Bézier curve.
+ddist :: forall v r p. ( Torsor v p, Inner r v, RealFloat r ) => Bezier p -> p -> [ r ]
+ddist ( Bezier { .. } ) c = [ a0, a1, a2, a3 ]
   where
-    roots :: [ r ]
-    roots = filter ( \ r -> r > 0 && r < 1 ) ( realRoots [ a0, a1, a2, a3 ] )
-
     v, v', v'' :: v
     v   = c  --> p0
     v'  = p0 --> p1
@@ -88,6 +82,13 @@ closestPoint pts@( Bezier { .. } ) c = pickClosest ( 0 :| 1 : roots )
     a2 = 3 * v' ^.^ v''
     a3 = squaredNorm v''
 
+-- | Finds the closest point to a given point on a quadratic Bézier curve.
+closestPoint :: forall v r p. ( Torsor v p, Inner r v, RealFloat r ) => Bezier p -> p -> ArgMin r ( r, p )
+closestPoint pts@( Bezier { .. } ) c = pickClosest ( 0 :| 1 : roots )
+  where
+    roots :: [ r ]
+    roots = filter ( \ r -> r > 0 && r < 1 ) ( realRoots $ ddist @v pts c ) 
+
     pickClosest :: NonEmpty r -> ArgMin r ( r, p )
     pickClosest ( s :| ss ) = go s q nm0 ss
       where

src/lib/Math/Bezier/Stroke.hs

@@ -39,7 +39,7 @@ import Math.Epsilon
   ( epsilon )
 import Math.Module
   ( Module((^-^)), Inner((^.^)), lerp )
-import Math.RealRoots
+import Math.Roots
   ( realRoots )
 import Math.Vector2D
   ( Point2D(..), Vector2D(..), cross )

src/lib/Math/Bezier/Subdivision.hs

@@ -1,3 +0,0 @@
-module Math.Bezier.Subdivision where
-
---------------------------------------------------------------------------------

src/lib/Math/Linear/SVD.hs

@@ -0,0 +1,107 @@
+{-# LANGUAGE DerivingStrategies  #-}
+{-# LANGUAGE NamedFieldPuns      #-}
+{-# LANGUAGE RecordWildCards     #-}
+{-# LANGUAGE ScopedTypeVariables #-}
+
+module Math.Linear.SVD
+  ( SVD(..), svd
+  , pinv
+  , lsolve
+  )
+  where
+
+-- base
+import Data.Complex
+  ( Complex(..), magnitude )
+
+-- MetaBrush
+import Math.Epsilon
+  ( epsilon )
+import Math.Vector2D
+  ( Vector2D(..), Mat22(..) )
+
+--------------------------------------------------------------------------------
+
+data SVD a
+  = SVD
+  { u   :: !( Complex a )
+  , sv1 :: !a
+  , sv2 :: !a
+  , v   :: !( Complex a )
+  }
+  deriving stock Show
+
+-- | Singular value decomposition of a real 2x2 matrix.
+svd :: forall a. ( RealFloat a, Show a ) => Mat22 a -> SVD a
+svd ( Mat22 a b c d )
+  | abs f < tol
+  || any isNaN [ c1, c2, s1, s2 ]
+  || magnitude u < 1 - tol
+  || magnitude v < 1 - tol
+  = SVD { u = 1, v = 1, .. }
+  | otherwise
+  = SVD { .. }
+  where
+    tol = sqrt epsilon
+
+    det = a * d - b * c
+    q = a * a + b * b + c * c + d * d
+
+    n1 = q + 2 * det
+    n2 = q - 2 * det
+
+    r1 = sqrt n1
+    r2 = sqrt n2
+
+    sv1 = 0.5 * ( r1 + r2 )
+    sv2 = 0.5 * ( r1 - r2 )
+
+    k = a * a - d * d
+    l = b * b - c * c
+    f = n1 * n2
+
+
+    i = 0.5 / sqrt f
+
+    ip = ( k + l ) * i
+    im = ( k - l ) * i
+
+    c1 =                            sqrt ( 0.5 + ip )
+    c2 =                            sqrt ( 0.5 + im )
+    s1 = signum ( a * c + b * d ) * sqrt ( 0.5 - ip )
+    s2 = signum ( a * b + c * d ) * sqrt ( 0.5 - im )
+
+    u = c1 :+ s1
+    v = c2 :+ s2
+
+-- | Pseudo-inverse of a real 2x2 matrix.
+pinv :: forall a. ( RealFloat a, Show a ) => Mat22 a -> Mat22 a
+pinv mat = case svd mat of
+  SVD { u = c1 :+ s1, sv1, sv2, v = c2 :+ s2 } ->
+    Mat22
+      ( c1 * c2 * rsv1 + s1 * s2 * rsv2 ) ( s1 * c2 * rsv1 - c1 * s2 * rsv2 )
+      ( c1 * s2 * rsv1 - s1 * c2 * rsv2 ) ( s1 * s2 * rsv1 + c1 * c2 * rsv2 )
+    where
+      rsv1, rsv2 :: a
+      rsv1
+        | sv1 < epsilon
+        = sv1
+        | otherwise
+        = recip sv1
+      rsv2
+        | abs sv2 < epsilon
+        = sv2
+        | otherwise
+        = recip sv2
+
+-- | Solve a 2x2 system of linear equations.
+lsolve :: forall a. ( RealFloat a, Show a ) => Mat22 a -> Vector2D a -> Vector2D a
+lsolve mat ( Vector2D x y ) = Vector2D x' y'
+  where
+    x', y', a11, a12, a21, a22 :: a
+    Mat22
+      a11 a12
+      a21 a22
+        = pinv mat
+    x' = a11 * x + a12 * y
+    y' = a21 * x + a22 * y

src/lib/Math/Module.hs

@@ -2,10 +2,12 @@
 {-# LANGUAGE FlexibleInstances      #-}
 {-# LANGUAGE FunctionalDependencies #-}
 {-# LANGUAGE ScopedTypeVariables    #-}
+{-# LANGUAGE TypeApplications       #-}
 
 module Math.Module
   ( Module(..), lerp
-  , Inner(..), squaredNorm
+  , Inner(..)
+  , squaredNorm, quadrance, distance
   , proj, projC, closestPointToLine
   )
   where
@@ -48,6 +50,14 @@ class Module r m => Inner r m where
 squaredNorm :: forall m r. Inner r m => m -> r
 squaredNorm v = v ^.^ v
 
+-- | Squared distance between two points.
+quadrance :: forall v r p. ( Inner r v, Torsor v p ) => p -> p -> r
+quadrance p1 p2 = squaredNorm ( p1 --> p2 :: v )
+
+-- | Distance between two points.
+distance :: forall v r p. ( Floating r, Inner r v, Torsor v p ) => p -> p -> r
+distance p1 p2 = sqrt ( quadrance @v p1 p2 )
+
 -- | Projects the first argument onto the second.
 proj :: forall m r. ( Inner r m, Fractional r ) => m -> m -> m
 proj x y = projC x y *^ y

src/lib/Math/Roots.hs

@@ -1,7 +1,7 @@
 {-# LANGUAGE ScopedTypeVariables #-}
 
-module Math.RealRoots
-  ( realRoots )
+module Math.Roots
+
   where
 
 -- base
@@ -75,9 +75,9 @@ laguerre eps maxIters p = go maxIters
     p'' = derivative p'
     go :: Int -> Complex a -> Complex a
     go iterationsLeft x
-      | iterationsLeft <= 0        = x
-      | magnitude ( x' - x ) < eps = x'
-      | otherwise                  = go ( iterationsLeft - 1 ) x'
+      | iterationsLeft <= 1
+      || magnitude ( x' - x ) < eps = x'
+      | otherwise                   = go ( iterationsLeft - 1 ) x'
       where
         x' :: Complex a
         x' = laguerreStep eps p p' p'' x

src/lib/Math/Vector2D.hs

@@ -3,9 +3,10 @@
 {-# LANGUAGE DerivingVia           #-}
 {-# LANGUAGE FlexibleInstances     #-}
 {-# LANGUAGE MultiParamTypeClasses #-}
+{-# LANGUAGE PatternSynonyms       #-}
 
 module Math.Vector2D
-  ( Point2D(..), Vector2D(..)
+  ( Point2D(..), Vector2D(.., Vector2D), Mat22(..)
   , cross
   )
   where
@@ -43,11 +44,15 @@ data Point2D a = Point2D !a !a
   deriving ( Act ( Vector2D a ), Torsor ( Vector2D a ) )
     via Vector2D a
 
-newtype Vector2D a = Vector2D { tip :: Point2D a }
+newtype Vector2D a = MkVector2D { tip :: Point2D a }
   deriving stock ( Show, Eq, Functor, Foldable, Traversable )
   deriving ( Semigroup, Monoid, Group )
     via GenericProduct ( Point2D ( Sum a ) )
 
+{-# COMPLETE Vector2D #-}
+pattern Vector2D :: a -> a -> Vector2D a
+pattern Vector2D x y = MkVector2D ( Point2D x y )
+
 instance Num a => Module a ( Vector2D a ) where
   (^+^) = (<>)
   p ^-^ q = p <> invert q
@@ -56,9 +61,13 @@ instance Num a => Module a ( Vector2D a ) where
   p ^* c = fmap ( * c ) p
 
 instance Num a => Inner a ( Vector2D a ) where
-  ( Vector2D ( Point2D x1 y1 ) ) ^.^ ( Vector2D ( Point2D x2 y2 ) )
+  ( Vector2D x1 y1 ) ^.^ ( Vector2D x2 y2 )
     = x1 * x2 + y1 * y2
 
 cross :: Num a => Vector2D a -> Vector2D a -> a
-cross ( Vector2D ( Point2D x1 y1 ) ) ( Vector2D ( Point2D x2 y2 ) )
+cross ( Vector2D x1 y1 ) ( Vector2D x2 y2 )
   = x1 * y2 - x2 * y1
+
+data Mat22 a
+  = Mat22 !a !a !a !a
+  deriving stock ( Show, Eq, Generic, Functor, Foldable, Traversable )