precondition before taking Gauss-Seidel step

src/splines/Math/Bezier/Stroke.hs

@@ -25,7 +25,7 @@ import Control.Arrow
 import Control.Applicative
   ( Applicative(..) )
 import Control.Monad
-  ( guard, unless )
+  ( unless )
 import Control.Monad.ST
   ( RealWorld, ST )
 import Data.Bifunctor
@@ -33,7 +33,7 @@ import Data.Bifunctor
 import Data.Coerce
   ( Coercible, coerce )
 import Data.Foldable
-  ( for_, toList )
+  ( for_ )
 import Data.Functor.Identity
   ( Identity(..) )
 import Data.List
@@ -88,6 +88,8 @@ import qualified Control.Parallel.Strategies as Strats
 -- rounded-hw
 import Numeric.Rounded.Hardware
   ( Rounded(..) )
+import Numeric.Rounded.Hardware.Interval.NonEmpty
+  ( Interval(..) )
 import qualified Numeric.Rounded.Hardware.Interval.NonEmpty as Interval
 
 -- transformers
@@ -122,7 +124,7 @@ import Math.Differentiable
   , type ExtentOrder
   )
 import Math.Epsilon
-  ( epsilon )
+  ( epsilon, nearZero )
 import Math.Interval
 import Math.Linear
 import Math.Module
@@ -339,8 +341,7 @@ computeStrokeOutline fitParams ptParams toBrushParams brushFn spline@( Spline {
         ( ( _, startTgtFwd), ( _, startTgtBwd ) ) = outlineFn firstOutlineFn $ ℝ1 0
         ( ( _, endTgtFwd  ), ( _, endTgtBwd   ) ) = outlineFn  lastOutlineFn $ ℝ1 1
         fwdStartCap, bwdStartCap :: SplinePts Open
-        cusps :: [ Cusp ]
+        OutlineData ( fmap fst -> TwoSided fwdStartCap bwdStartCap ) _
-        OutlineData ( fmap fst -> TwoSided fwdStartCap bwdStartCap ) cusps
           = snd . runWriter
           $ tellBrushJoin ( endTgt, endTgtFwd, endTgtBwd ) spt0 ( startTgt, startTgtFwd, startTgtBwd )
     -> do
@@ -596,7 +597,7 @@ outlineFunction ptParams toBrushParams brushFromParams sp0 crv =
                              $ runD ( brushFromParams @Point proxy# id )
                              $ toBrushParams params_t
 
-    ( newtDunno, newtSols ) = intervalNewtonGS 0.0001 curvesI
+    ( newtDunno, newtSols ) = intervalNewtonGS InverseMidJacobian 0.0001 curvesI
 
   in --trace
      --   ( unlines $
@@ -1184,9 +1185,14 @@ cuspCoords eqs ( 𝕀 ( ℝ1 t_lo ) ( ℝ1 t_hi ), i, 𝕀 ( ℝ1 s_lo ) ( ℝ1
   }
   where
     t_mid = 0.5 * ( t_lo + t_hi )
-    s_mid = 0.5 * ( t_lo + t_hi )
+    s_mid = 0.5 * ( s_lo + s_hi )
 
 
+data Preconditioner
+  = NoPreconditioning
+  | InverseMidJacobian
+  deriving stock Show
+
 -- | Interval Newton method with Gauss–Seidel step for inversion
 -- of the interval Jacobian.
 --
@@ -1194,10 +1200,11 @@ cuspCoords eqs ( 𝕀 ( ℝ1 t_lo ) ( ℝ1 t_hi ), i, 𝕀 ( ℝ1 s_lo ) ( ℝ1
 -- (and to which Newton's method will converge starting from anywhere inside
 -- the box), and @dunno@ which are small boxes which might or might not
 -- contain solutions.
-intervalNewtonGS :: Double
+intervalNewtonGS :: Preconditioner
+                 -> Double
                  -> ( 𝕀ℝ 1 -> Seq ( 𝕀ℝ 1 -> StrokeDatum 'Interval ) )
                  -> ( [ ( 𝕀ℝ 1, Int, 𝕀ℝ 1 ) ], [ ( 𝕀ℝ 1, Int, 𝕀ℝ 1 ) ] )
-intervalNewtonGS minWidth eqs =
+intervalNewtonGS precondMethod minWidth eqs =
   go [ ( 𝕀 ( ℝ1 0 ) ( ℝ1 1 ), i, 𝕀 ( ℝ1 0 ) ( ℝ1 1 ) )
      | i <- [ 0 .. length ( eqs ( 𝕀 ( ℝ1 0 ) ( ℝ1 1 ) ) ) - 1 ]
      ]
@@ -1231,13 +1238,11 @@ intervalNewtonGS minWidth eqs =
            , cmpℝ2 (>) ( getRounded ( Interval.sup $ ival f ) ) ( ℝ2 0 0 )
            -> let -- Interval Newton method: take one Gauss–Seidel step
                   -- for the equation f'(X) v = - f(x_mid).
-                  -- !precond   = matInverse ( f_t_mid, f_s_mid )
+                  !( a, b ) = precondition precondMethod
-                  -- !a         = matMul precond ( f_t, f_s )
+                                ( f_t_mid, f_s_mid )
-                  -- !b         = matMulVec precond ( neg f_mid )
+                                ( f_t, f_s ) ( neg f_mid )
-                  -- !gsGuesses = gaussSeidel a b ( t, s )
+
-                  !gsGuesses = gaussSeidel
+                  !gsGuesses = gaussSeidel a b
-                                 ( f_t, f_s )
-                                 ( neg f_mid )
                                  ( coerce ( (-) @( 𝕀 Double ) ) t i_t_mid
                                  , coerce ( (-) @( 𝕀 Double ) ) s i_s_mid )
               in if all ( smaller . fst ) gsGuesses
@@ -1283,6 +1288,50 @@ intervalNewtonGS minWidth eqs =
                 !( 𝕀 y'_lo y'_hi ) = negate $ 𝕀 y_lo y_hi
             in 𝕀 ( ℝ2 x'_lo y'_lo ) ( ℝ2 x'_hi y'_hi )
 
+precondition :: Preconditioner
+             -> ( 𝕀ℝ 2, 𝕀ℝ 2 )
+             -> ( 𝕀ℝ 2, 𝕀ℝ 2 )
+             -> 𝕀ℝ 2
+             -> ( ( 𝕀ℝ 2, 𝕀ℝ 2 ), 𝕀ℝ 2 )
+precondition meth jac_mid a@( a1, a2 ) b =
+  case meth of
+    NoPreconditioning
+      -> ( a, b )
+    InverseMidJacobian
+      | ( 𝕀 ( ℝ2 a11_lo a21_lo ) ( ℝ2 a11_hi a21_hi )
+        , 𝕀 ( ℝ2 a12_lo a22_lo ) ( ℝ2 a12_hi a22_hi ) ) <- jac_mid
+      , let !a11 = 0.5 * ( a11_lo + a11_hi )
+            !a12 = 0.5 * ( a12_lo + a12_hi )
+            !a21 = 0.5 * ( a21_lo + a21_hi )
+            !a22 = 0.5 * ( a22_lo + a22_hi )
+            !d = a11 * a22 - a12 * a21
+      , not ( nearZero d )
+      , let !precond = ( ℝ2 a22 -a21, ℝ2 -a12 a11 )
+            !inv = recip d
+            f x = scale inv $ matMulVec precond x
+      -> ( ( f a1, f a2 ), f b )
+      | otherwise
+      -> ( a, b )
+
+scale :: Double -> 𝕀ℝ 2 -> 𝕀ℝ 2
+scale s ( 𝕀 ( ℝ2 a1_lo a2_lo ) ( ℝ2 a1_hi a2_hi ) )
+  | I ( Rounded b1_lo ) ( Rounded b1_hi )
+     <- I ( Rounded s ) ( Rounded s ) * I ( Rounded a1_lo ) ( Rounded a1_hi )
+  , I ( Rounded b2_lo ) ( Rounded b2_hi )
+     <- I ( Rounded s ) ( Rounded s ) * I ( Rounded a2_lo ) ( Rounded a2_hi )
+  = 𝕀 ( ℝ2 b1_lo b2_lo ) ( ℝ2 b1_hi b2_hi )
+
+matMulVec :: ( ℝ 2, ℝ 2 ) -> 𝕀ℝ 2 -> 𝕀ℝ 2
+matMulVec ( ℝ2 a11 a21, ℝ2 a12 a22 ) ( 𝕀 ( ℝ2 u_lo v_lo ) ( ℝ2 u_hi v_hi ) ) =
+  let !( I ( Rounded u'_lo ) ( Rounded u'_hi ) ) =
+         I ( Rounded a11 ) ( Rounded a11 ) * I ( Rounded u_lo ) ( Rounded u_hi )
+       + I ( Rounded a12 ) ( Rounded a12 ) * I ( Rounded v_lo ) ( Rounded v_hi )
+      !( I ( Rounded v'_lo ) ( Rounded v'_hi ) ) =
+         I ( Rounded a21 ) ( Rounded a21 ) * I ( Rounded u_lo ) ( Rounded u_hi )
+       + I ( Rounded a22 ) ( Rounded a22 ) * I ( Rounded v_lo ) ( Rounded v_hi )
+  in 𝕀 ( ℝ2 u'_lo v'_lo ) ( ℝ2 u'_hi v'_hi )
+
+
 cmpℝ2 :: ( Double -> Double -> Bool ) -> ℝ 2 -> ℝ 2 -> Bool
 cmpℝ2 cmp ( ℝ2 x1 y1 ) ( ℝ2 x2 y2 )
   = cmp x1 x2 && cmp y1 y2