Interval Newton method for cusp isolation

src/splines/Math/Bezier/Stroke.hs

@@ -31,11 +31,13 @@ import Control.Monad.ST
 import Data.Bifunctor
   ( Bifunctor(bimap) )
 import Data.Coerce
-  ( Coercible )
+  ( Coercible, coerce )
 import Data.Foldable
   ( for_, toList )
 import Data.Functor.Identity
   ( Identity(..) )
+import Data.List
+  ( nub, partition )
 import Data.List.NonEmpty
   ( unzip )
 import Data.Maybe
@@ -548,14 +550,20 @@ outlineFunction ptParams toBrushParams brushFromParams sp0 crv =
                              $ runD ( brushFromParams @Point proxy# id )
                              $ toBrushParams params_t
 
-    bisSols = bisection 0.0001 curvesI
+    ( newtDunno, newtSols ) = intervalNewtonGS 0.0001 curvesI
 
   in --trace
-     --    ( unlines $
+     --   ( unlines $
-     --      ( "bisectionMethod: #(possible zeroes) = " ++ show ( length bisSols ) ) :
+     --     [ "newtonMethod: #(definite zeroes) = " ++ show ( length newtSols  )
-     --      "" :
+     --     , "newtonMethod: #(unknown)         = " ++ show ( length newtDunno )
-     --      map show bisSols )
+     --     , ""
-         fwdBwd
+     --     , "definite solutions:"
+     --     , if null newtSols then "[]" else unlines $ map show newtSols
+     --     , ""
+     --     , "unknown:"
+     --     , if null newtDunno then "[]" else unlines $ map show newtDunno ]
+     --   ) $
+     fwdBwd
 
 -----------------------------------
 -- Various utility functions
@@ -1056,75 +1064,154 @@ brushStrokeData path params brush =
 
 --------------------------------------------------------------------------------
 
-bisection :: Double
+-- Take one interval Gauss–Seidel step for the equation \( A X = B \),
-          -> ( 𝕀ℝ 1 -> Seq ( 𝕀ℝ 1 -> StrokeDatum 'Interval ) )
+-- refining the initial guess box for \( X \) into up to four (disjoint) new boxes.
-          -> [ ( 𝕀ℝ 1, Int, 𝕀ℝ 1, 𝕀ℝ 1, 𝕀ℝ 2 ) ]
+--
-bisection minWidth eqs =
+-- The boolean indicates whether the Gauss–Seidel step was a contraction.
- bisect initialCands [] []
+gaussSeidel :: ( 𝕀ℝ 2, 𝕀ℝ 2 ) -- ^ columns of \( A \)
+            -> 𝕀ℝ 2           -- ^ \( B \)
+            -> ( 𝕀ℝ 1, 𝕀ℝ 1 ) -- ^ initial box \( X \)
+            -> [ ( ( 𝕀ℝ 1, 𝕀ℝ 1 ), Bool ) ]
+gaussSeidel
+  ( 𝕀 ( ℝ2 a11_lo a21_lo ) ( ℝ2 a11_hi a21_hi )
+  , 𝕀 ( ℝ2 a12_lo a22_lo ) ( ℝ2 a12_hi a22_hi ) )
+  ( 𝕀 ( ℝ2 b1_lo b2_lo ) ( ℝ2 b1_hi b2_hi ) )
+  ( 𝕀 ( ℝ1 t0_lo ) ( ℝ1 t0_hi ), 𝕀 ( ℝ1 s0_lo ) ( ℝ1 s0_hi ) )
+    = let !a11 = 𝕀 a11_lo a11_hi
+          !a12 = 𝕀 a12_lo a12_hi
+          !a21 = 𝕀 a21_lo a21_hi
+          !a22 = 𝕀 a22_lo a22_hi
+          !b1  = 𝕀  b1_lo  b1_hi
+          !b2  = 𝕀  b2_lo  b2_hi
+          !t0  = 𝕀  t0_lo  t0_hi
+          !s0  = 𝕀  s0_lo  s0_hi
+      in nub $ do
+
+        t' <- ( b1 - a12 * s0 ) `extendedDivide` a11
+        ( t@( 𝕀 t_lo t_hi ), sub_t ) <- t' `intersect` t0
+        s' <- ( b2 - a21 * t  ) `extendedDivide` a22
+        ( 𝕀 s_lo s_hi, sub_s ) <- s' `intersect` s0
+
+        return ( ( 𝕀 ( ℝ1 t_lo ) ( ℝ1 t_hi ), 𝕀 ( ℝ1 s_lo ) ( ℝ1 s_hi ) )
+               , sub_t && sub_s )
+
+intersect :: 𝕀 Double -> 𝕀 Double -> [ ( 𝕀 Double, Bool ) ]
+intersect ( 𝕀 lo1 hi1 ) ( 𝕀 lo2 hi2 )
+  | lo > hi
+  = [ ]
+  | otherwise
+  = [ ( 𝕀 lo hi, lo == lo1 && hi == hi1 ) ]
+  where
+    lo = max lo1 lo2
+    hi = min hi1 hi2
+
+extendedDivide :: 𝕀 Double -> 𝕀 Double -> [ 𝕀 Double ]
+extendedDivide x y = map ( x * ) ( extendedRecip y )
+
+extendedRecip :: 𝕀 Double -> [ 𝕀 Double ]
+extendedRecip x@( 𝕀 lo hi )
+  | lo == 0 && hi == 0
+  = [ 𝕀 ( -1 / 0 ) ( 1 / 0 ) ]
+  | lo >= 0 || hi <= 0
+  = [ recip x ]
+  | otherwise
+  = [ recip ( 𝕀 lo 0 ), recip ( 𝕀 0 hi ) ]
+
+-- | Interval Newton method with Gauss–Seidel step for inversion
+-- of the interval Jacobian.
+--
+-- Returns @(dunno, sols)@ where @sols@ are boxes that contain a unique solution
+-- (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
+                 -> ( 𝕀ℝ 1 -> Seq ( 𝕀ℝ 1 -> StrokeDatum 'Interval ) )
+                 -> ( [ ( 𝕀ℝ 1, Int, 𝕀ℝ 1 ) ], [ ( 𝕀ℝ 1, Int, 𝕀ℝ 1 ) ] )
+intervalNewtonGS minWidth eqs =
+  go [ ( 𝕀 ( ℝ1 0 ) ( ℝ1 1 ), i, 𝕀 ( ℝ1 0 ) ( ℝ1 1 ) )
+     | i <- [ 0 .. length ( eqs ( 𝕀 ( ℝ1 0 ) ( ℝ1 1 ) ) ) - 1 ]
+     ]
+     []
+     []
+
   where
 
-    bisect :: [ ( 𝕀ℝ 1, Int, 𝕀ℝ 1, 𝕀ℝ 1, 𝕀ℝ 2 ) ] -- have solutions, need bisection to refine
+    go :: [ ( 𝕀ℝ 1, Int, 𝕀ℝ 1 ) ] -- boxes to work on
-           -> [ ( 𝕀ℝ 1, Int, 𝕀ℝ 1 ) ] -- have been bisected, don't know if they contain solutions yet
+       -> [ ( 𝕀ℝ 1, Int, 𝕀ℝ 1 ) ] -- too small: don't shrink further
-           -> [ ( 𝕀ℝ 1, Int, 𝕀ℝ 1, 𝕀ℝ 1, 𝕀ℝ 2 ) ] -- have solutions, don't bisect further
+       -> [ ( 𝕀ℝ 1, Int, 𝕀ℝ 1 ) ] -- found solutions
-           -> [ ( 𝕀ℝ 1, Int, 𝕀ℝ 1, 𝕀ℝ 1, 𝕀ℝ 2 ) ]
+       -> ( [ ( 𝕀ℝ 1, Int, 𝕀ℝ 1 ) ], [ ( 𝕀ℝ 1, Int, 𝕀ℝ 1 ) ] )
-    bisect [] [] sols = sols
+    go [] giveUp sols = ( giveUp, sols )
-    bisect cands ( ( t, i, s ) : toTry ) sols
+    go ( cand@( t@( 𝕀 ( ℝ1 t_lo ) ( ℝ1 t_hi ) )
-      | Just ( ee, 𝛿E𝛿sdcdt ) <- isCand t i s
+              , i
-      = bisect ( ( t, i, s, ee, 𝛿E𝛿sdcdt ) : cands ) toTry sols
+              , s@( 𝕀 ( ℝ1 s_lo ) ( ℝ1 s_hi ) )
-      | otherwise
+              ) : cands ) giveUp sols
-      = bisect cands toTry sols
+      -- Box is small: stop processing it.
-
-    bisect ( cand@( t@( 𝕀 ( ℝ1 t_lo ) ( ℝ1 t_hi ) )
-                  , i
-                  , s@( 𝕀 ( ℝ1 s_lo ) ( ℝ1 s_hi ) )
-                  , _, _
-                  ) : cands )
-           toTry
-           sols
-      -- If the box is small, don't bisect it further, and store it as a candidate solution.
       | t_hi - t_lo < minWidth && s_hi - s_lo < minWidth
-      = trace ( "bisection sol: " ++ show cand ++ "\nnbCands = " ++ show ( length cands ) ++ "\nnbToTry = " ++ show ( length toTry ) )
+      = go cands ( cand : giveUp ) sols
-      $ bisect cands toTry ( cand : sols )
-      -- Otherwise, bisect in its longest direction and add the two resulting
-      -- boxes to the list of boxes to try.
-      | otherwise
-      = let newToTry
-             | t_hi - t_lo > s_hi - s_lo
-             , let t_mid = 0.5 * ( t_lo + t_hi )
-             =   ( 𝕀 ( ℝ1 t_lo  ) ( ℝ1 t_mid ), i, s )
-               : ( 𝕀 ( ℝ1 t_mid ) ( ℝ1 t_hi  ), i, s )
-               : toTry
-             | let s_mid = 0.5 * ( s_lo + s_hi )
-             =   ( t, i, 𝕀 ( ℝ1 s_lo  ) ( ℝ1 s_mid ) )
-               : ( t, i, 𝕀 ( ℝ1 s_mid ) ( ℝ1 s_hi  ) )
-               : toTry
-        in bisect cands newToTry sols
 
-    initialCands =
+      | StrokeDatum { ee = D22 ee _ _ _ _ _
-      getCands
+                    , 𝛿E𝛿sdcdt = D12 ( T f ) ( T ( T f_t ) ) ( T ( T f_s ) ) }
-        ( 𝕀 ( ℝ1 0 ) ( ℝ1 1 ) )
+        <- ( eqs t `Seq.index` i ) s
-        ( 𝕀 ( ℝ1 0 ) ( ℝ1 1 ) )
 
-    getCands t s =
+      , StrokeDatum { 𝛿E𝛿sdcdt = D12 ( T f_mid ) ( T ( T f_t_mid ) ) ( T ( T f_s_mid ) ) }
-      [ (t, i, s, ee, 𝛿E𝛿sdcdt )
+        <- ( eqs i_t_mid `Seq.index` i ) i_s_mid
-      | let !eqs_t = eqs t
+      = if | Interval.inf ( ival ee ) < Rounded ( ℝ1 0 )
-      , ( eq_t, i ) <- zip ( toList eqs_t ) ( [0,1..] :: [Int] )
+           , Interval.sup ( ival ee ) > Rounded ( ℝ1 0 )
-      , let !( StrokeDatum { ee = D22 ee _ _ _ _ _, 𝛿E𝛿sdcdt = D12 ( T 𝛿E𝛿sdcdt ) _ _ } ) = eq_t s
+           , cmpℝ2 (<) ( getRounded ( Interval.inf $ ival f ) ) ( ℝ2 0 0 )
-      , Interval.inf ( ival ee ) < Rounded ( ℝ1 0 )
+           , cmpℝ2 (>) ( getRounded ( Interval.sup $ ival f ) ) ( ℝ2 0 0 )
-      , Interval.sup ( ival ee ) > Rounded ( ℝ1 0 )
+           -> let -- Interval Newton method: take one Gauss–Seidel step
-      , cmpℝ2 (<) ( getRounded ( Interval.inf $ ival 𝛿E𝛿sdcdt ) ) ( ℝ2 0 0 )
+                  -- for the equation f'(X) v = - f(x_mid).
-      , cmpℝ2 (>) ( getRounded ( Interval.sup $ ival 𝛿E𝛿sdcdt ) ) ( ℝ2 0 0 )
+                  -- !precond   = matInverse ( f_t_mid, f_s_mid )
-      ]
+                  -- !a         = matMul precond ( f_t, f_s )
+                  -- !b         = matMulVec precond ( neg f_mid )
+                  -- !gsGuesses = gaussSeidel a b ( t, s )
+                  !gsGuesses = gaussSeidel
+                                 ( 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
+                 then
+                   -- If the Gauss–Seidel step was a contraction, then the box
+                   -- contains a unique solution (by the Banach fixed point theorem).
+                   --
+                   -- These boxes can thus be directly added to the solution set:
+                   -- Newton's method is guaranteed to converge to the unique solution.
+                   let !(done, todo) = bimap ( map ( mkGuess . fst ) ) ( map ( mkGuess . fst ) )
+                                     $ partition snd gsGuesses
+                   in go ( todo ++ cands ) giveUp ( done ++ sols )
+                 else
+                    -- Gauss–Seidel failed to shrink the boxes.
+                    -- Bisect along the widest dimension instead.
+                   let bisGuesses
+                         | t_hi - t_lo > s_hi - s_lo
+                         = [ ( 𝕀 ( ℝ1 t_lo  ) ( ℝ1 t_mid ), i, s )
+                           , ( 𝕀 ( ℝ1 t_mid ) ( ℝ1 t_hi  ), i, s ) ]
+                         | otherwise
+                         = [ ( t, i, 𝕀 ( ℝ1 s_lo  ) ( ℝ1 s_mid ) )
+                           , ( t, i, 𝕀 ( ℝ1 s_mid ) ( ℝ1 s_hi  ) ) ]
+                   in go ( bisGuesses ++ cands ) giveUp sols
 
-    isCand :: 𝕀ℝ 1 -> Int -> 𝕀ℝ 1 -> Maybe ( 𝕀ℝ 1, 𝕀ℝ 2 )
+           -- Box doesn't contain a solution: discard it.
-    isCand t i s = case ( ( eqs t ) `Seq.index` i ) s of
+           | otherwise
-      StrokeDatum { ee = D22 ee _ _ _ _ _, 𝛿E𝛿sdcdt = D12 ( T 𝛿E𝛿sdcdt ) _ _ } ->
+           -> go cands giveUp sols
-        do guard $
+      where
-                 Interval.inf ( ival ee ) < Rounded ( ℝ1 0 )
+        t_mid = 0.5 * ( t_lo + t_hi )
-              && Interval.sup ( ival ee ) > Rounded ( ℝ1 0 )
+        s_mid = 0.5 * ( s_lo + s_hi )
-              && cmpℝ2 (<) ( getRounded ( Interval.inf $ ival 𝛿E𝛿sdcdt ) ) ( ℝ2 0 0 )
+        i_t_mid = 𝕀 ( ℝ1 t_mid ) ( ℝ1 t_mid )
-              && cmpℝ2 (>) ( getRounded ( Interval.sup $ ival 𝛿E𝛿sdcdt ) ) ( ℝ2 0 0 )
+        i_s_mid = 𝕀 ( ℝ1 s_mid ) ( ℝ1 s_mid )
-           return ( ee, 𝛿E𝛿sdcdt )
+        mkGuess ( t0, s0 ) = ( coerce ( (+) @( 𝕀 Double ) ) t0 i_t_mid
+                             , i
+                             , coerce ( (+) @( 𝕀 Double ) ) s0 i_s_mid )
+        smaller ( 𝕀 ( ℝ1 t0_lo ) ( ℝ1 t0_hi ), 𝕀 ( ℝ1 s0_lo ) ( ℝ1 s0_hi ) )
+          =  ( t0_lo + t_mid ) > t_lo + 0.25 * minWidth
+          || ( t0_hi + t_mid ) < t_hi - 0.25 * minWidth
+          || ( s0_lo + s_mid ) > s_lo + 0.25 * minWidth
+          || ( s0_hi + s_mid ) < s_hi - 0.25 * minWidth
+        neg ( 𝕀 ( ℝ2 x_lo y_lo ) ( ℝ2 x_hi y_hi ) )
+          = let !( 𝕀 x'_lo x'_hi ) = negate $ 𝕀 x_lo x_hi
+                !( 𝕀 y'_lo y'_hi ) = negate $ 𝕀 y_lo y_hi
+            in 𝕀 ( ℝ2 x'_lo y'_lo ) ( ℝ2 x'_hi y'_hi )
 
 cmpℝ2 :: ( Double -> Double -> Bool ) -> ℝ 2 -> ℝ 2 -> Bool
 cmpℝ2 cmp ( ℝ2 x1 y1 ) ( ℝ2 x2 y2 )

src/splines/Math/Interval/Internal.hs

@@ -41,6 +41,10 @@ import Math.Ring
 newtype 𝕀 a = MkI { ival :: Interval a }
   deriving newtype ( Prelude.Num, Prelude.Fractional, Prelude.Floating )
 
+instance Eq a => Eq ( 𝕀 a ) where
+  𝕀 a b == 𝕀 c d =
+    a == c && b == d
+
 {-# COMPLETE 𝕀 #-}
 pattern 𝕀 :: a -> a -> 𝕀 a
 pattern 𝕀 x y = MkI ( Interval.I ( Rounded x ) ( Rounded y ) )