diff --git a/src/splines/Math/Bezier/Stroke.hs b/src/splines/Math/Bezier/Stroke.hs index c8c82bc..e77d1f9 100644 --- a/src/splines/Math/Bezier/Stroke.hs +++ b/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 $ - -- ( "bisectionMethod: #(possible zeroes) = " ++ show ( length bisSols ) ) : - -- "" : - -- map show bisSols ) - fwdBwd + -- ( unlines $ + -- [ "newtonMethod: #(definite zeroes) = " ++ show ( length newtSols ) + -- , "newtonMethod: #(unknown) = " ++ show ( length newtDunno ) + -- , "" + -- , "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 - -> ( 𝕀ℝ 1 -> Seq ( 𝕀ℝ 1 -> StrokeDatum 'Interval ) ) - -> [ ( 𝕀ℝ 1, Int, 𝕀ℝ 1, 𝕀ℝ 1, 𝕀ℝ 2 ) ] -bisection minWidth eqs = - bisect initialCands [] [] +-- Take one interval Gauss–Seidel step for the equation \( A X = B \), +-- refining the initial guess box for \( X \) into up to four (disjoint) new boxes. +-- +-- The boolean indicates whether the Gauss–Seidel step was a contraction. +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 - -> [ ( 𝕀ℝ 1, Int, 𝕀ℝ 1 ) ] -- have been bisected, don't know if they contain solutions yet - -> [ ( 𝕀ℝ 1, Int, 𝕀ℝ 1, 𝕀ℝ 1, 𝕀ℝ 2 ) ] -- have solutions, don't bisect further - -> [ ( 𝕀ℝ 1, Int, 𝕀ℝ 1, 𝕀ℝ 1, 𝕀ℝ 2 ) ] - bisect [] [] sols = sols - bisect cands ( ( t, i, s ) : toTry ) sols - | Just ( ee, 𝛿E𝛿sdcdt ) <- isCand t i s - = bisect ( ( t, i, s, ee, 𝛿E𝛿sdcdt ) : cands ) toTry sols - | otherwise - = bisect cands toTry sols - - 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. + go :: [ ( 𝕀ℝ 1, Int, 𝕀ℝ 1 ) ] -- boxes to work on + -> [ ( 𝕀ℝ 1, Int, 𝕀ℝ 1 ) ] -- too small: don't shrink further + -> [ ( 𝕀ℝ 1, Int, 𝕀ℝ 1 ) ] -- found solutions + -> ( [ ( 𝕀ℝ 1, Int, 𝕀ℝ 1 ) ], [ ( 𝕀ℝ 1, Int, 𝕀ℝ 1 ) ] ) + go [] giveUp sols = ( giveUp, sols ) + go ( cand@( t@( 𝕀 ( ℝ1 t_lo ) ( ℝ1 t_hi ) ) + , i + , s@( 𝕀 ( ℝ1 s_lo ) ( ℝ1 s_hi ) ) + ) : cands ) giveUp sols + -- Box is small: stop processing it. | t_hi - t_lo < minWidth && s_hi - s_lo < minWidth - = trace ( "bisection sol: " ++ show cand ++ "\nnbCands = " ++ show ( length cands ) ++ "\nnbToTry = " ++ show ( length toTry ) ) - $ 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 + = go cands ( cand : giveUp ) sols - initialCands = - getCands - ( 𝕀 ( ℝ1 0 ) ( ℝ1 1 ) ) - ( 𝕀 ( ℝ1 0 ) ( ℝ1 1 ) ) + | StrokeDatum { ee = D22 ee _ _ _ _ _ + , 𝛿E𝛿sdcdt = D12 ( T f ) ( T ( T f_t ) ) ( T ( T f_s ) ) } + <- ( eqs t `Seq.index` i ) s - getCands t s = - [ (t, i, s, ee, 𝛿E𝛿sdcdt ) - | let !eqs_t = eqs t - , ( eq_t, i ) <- zip ( toList eqs_t ) ( [0,1..] :: [Int] ) - , let !( StrokeDatum { ee = D22 ee _ _ _ _ _, 𝛿E𝛿sdcdt = D12 ( T 𝛿E𝛿sdcdt ) _ _ } ) = eq_t s - , Interval.inf ( ival ee ) < Rounded ( ℝ1 0 ) - , Interval.sup ( ival ee ) > Rounded ( ℝ1 0 ) - , cmpℝ2 (<) ( getRounded ( Interval.inf $ ival 𝛿E𝛿sdcdt ) ) ( ℝ2 0 0 ) - , cmpℝ2 (>) ( getRounded ( Interval.sup $ ival 𝛿E𝛿sdcdt ) ) ( ℝ2 0 0 ) - ] + , StrokeDatum { 𝛿E𝛿sdcdt = D12 ( T f_mid ) ( T ( T f_t_mid ) ) ( T ( T f_s_mid ) ) } + <- ( eqs i_t_mid `Seq.index` i ) i_s_mid + = if | Interval.inf ( ival ee ) < Rounded ( ℝ1 0 ) + , Interval.sup ( ival ee ) > Rounded ( ℝ1 0 ) + , cmpℝ2 (<) ( getRounded ( Interval.inf $ ival f ) ) ( ℝ2 0 0 ) + , 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 = 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 ) - isCand t i s = case ( ( eqs t ) `Seq.index` i ) s of - StrokeDatum { ee = D22 ee _ _ _ _ _, 𝛿E𝛿sdcdt = D12 ( T 𝛿E𝛿sdcdt ) _ _ } -> - do guard $ - Interval.inf ( ival ee ) < Rounded ( ℝ1 0 ) - && Interval.sup ( ival ee ) > Rounded ( ℝ1 0 ) - && cmpℝ2 (<) ( getRounded ( Interval.inf $ ival 𝛿E𝛿sdcdt ) ) ( ℝ2 0 0 ) - && cmpℝ2 (>) ( getRounded ( Interval.sup $ ival 𝛿E𝛿sdcdt ) ) ( ℝ2 0 0 ) - return ( ee, 𝛿E𝛿sdcdt ) + -- Box doesn't contain a solution: discard it. + | otherwise + -> go cands giveUp sols + where + t_mid = 0.5 * ( t_lo + t_hi ) + s_mid = 0.5 * ( s_lo + s_hi ) + i_t_mid = 𝕀 ( ℝ1 t_mid ) ( ℝ1 t_mid ) + i_s_mid = 𝕀 ( ℝ1 s_mid ) ( ℝ1 s_mid ) + 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 ) diff --git a/src/splines/Math/Interval/Internal.hs b/src/splines/Math/Interval/Internal.hs index 1ab4b77..244cecd 100644 --- a/src/splines/Math/Interval/Internal.hs +++ b/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 ) )