diff --git a/src/splines/Math/Bezier/Stroke.hs b/src/splines/Math/Bezier/Stroke.hs index b47bdb7..6781fbf 100644 --- a/src/splines/Math/Bezier/Stroke.hs +++ b/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 ) cusps + OutlineData ( fmap fst -> TwoSided fwdStartCap bwdStartCap ) _ = 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 = 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 ) + !( a, b ) = precondition precondMethod + ( f_t_mid, f_s_mid ) + ( f_t, f_s ) ( neg f_mid ) + + !gsGuesses = gaussSeidel a b ( 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