From 62bb1451c54b90aa0577711ad5b245b040083f69 Mon Sep 17 00:00:00 2001 From: sheaf Date: Sat, 29 Aug 2020 18:26:11 +0200 Subject: [PATCH] curve fitting: implement recursive subdivision --- src/lib/Math/Bezier/Cubic/Fit.hs | 79 +++++++++++++++--- src/lib/Math/Bezier/Stroke.hs | 138 +++++++++++++++---------------- 2 files changed, 135 insertions(+), 82 deletions(-) diff --git a/src/lib/Math/Bezier/Cubic/Fit.hs b/src/lib/Math/Bezier/Cubic/Fit.hs index d66599a..5e00d11 100644 --- a/src/lib/Math/Bezier/Cubic/Fit.hs +++ b/src/lib/Math/Bezier/Cubic/Fit.hs @@ -3,7 +3,9 @@ {-# LANGUAGE ScopedTypeVariables #-} {-# LANGUAGE TypeApplications #-} -module Math.Bezier.Cubic.Fit where +module Math.Bezier.Cubic.Fit + ( fitSpline, fitPiece ) + where -- base import Control.Arrow @@ -27,6 +29,12 @@ import Data.Act ( (•) ) ) +-- containers +import Data.Sequence + ( Seq ) +import qualified Data.Sequence as Seq + ( singleton ) + -- transformers import Control.Monad.Trans.State.Strict ( execStateT, modify' ) @@ -42,7 +50,7 @@ import qualified Data.Vector.Unboxed as Unboxed.Vector ( unsafeThaw, generate ) -- MetaBrush -import Math.Bezier.Cubic +import qualified Math.Bezier.Cubic as Cubic ( Bezier(..), bezier, ddist ) import Math.Epsilon ( epsilon ) @@ -57,8 +65,55 @@ import Math.Roots import Math.Vector2D ( Mat22(..), Point2D(..), Vector2D(..) ) +import Debug.Trace + -------------------------------------------------------------------------------- +-- | Fits a cubic Bézier spline to the given curve \( t \mapsto C(t), 0 \leqslant t \leqslant 1 \), +-- assumed to be G1-continuous. +-- +-- Subdivides the given curve into the specified number of segments \( \texttt{nbSegments} \), +-- and tries to fit the resulting points with a cubic Bézier curve using 'fitPiece'. +-- +-- When the fit is too poor: subdivide at the worst fitting point, and recurse. +-- \( \texttt{maxSubdiv} \) controls the maximum recursion depth for subdivision. +-- +-- See 'fitPiece' for more information on the fitting process, +-- including the meaning of \( \texttt{t_tol} \) and \( \texttt{maxIters} \). +fitSpline + :: Int -- ^ \( \texttt{maxSubdiv} \), the maximum subdivision recursion number + -> Int -- ^ \( \texttt{nbSegments} \), number of segments to split the curve into for fitting, + -> Double -- ^ \( \texttt{dist_tol} \), tolerance for the distance + -> Double -- ^ \( \texttt{t_tol} \), the tolerance for the Bézier parameter (for the fitting process) + -> Int -- ^ \( \texttt{maxIters} \), maximum number of iterations (for the fitting process) + -> ( Double -> ( Point2D Double, Vector2D Double ) ) -- ^ curve \( t \mapsto ( C(t), C'(t) ) \) to fit + -> Seq ( Cubic.Bezier ( Point2D Double ) ) +fitSpline maxSubdiv nbSegments dist_tol t_tol maxIters = go 0 + where + dt :: Double + dt = recip ( fromIntegral nbSegments ) + go + :: Int + -> ( Double -> ( Point2D Double, Vector2D Double ) ) + -> Seq ( Cubic.Bezier ( Point2D Double ) ) + go subdiv curve = + let + p, r :: Point2D Double + tp, tr :: Vector2D Double + qs :: [ Point2D Double ] + (p, tp) = curve 0 + (r, tr) = curve 1 + qs = map ( fst . curve ) [ dt * fromIntegral j | j <- [ 1 .. nbSegments - 1 ] ] + in + case fitPiece dist_tol t_tol maxIters p tp qs r tr of + ( bez, Max ( Arg t_split sq_d ) ) + | subdiv >= maxSubdiv + || sq_d <= dist_tol ^ ( 2 :: Int ) + -> Seq.singleton bez + | otherwise + -> go ( subdiv + 1 ) ( \ t -> curve ( t * t_split ) ) + <> go ( subdiv + 1 ) ( \ t -> curve ( t_split + t * ( 1 - t_split ) ) ) + -- | Fits a single cubic Bézier curve to the given data. -- -- This will consist of a cubic Bézier curve which: @@ -80,7 +135,7 @@ import Math.Vector2D -- \[ \sum_{i=1}^n \Big \| B(t_i) - q_i \Big \|^2. \] -- -- The values of the parameters \( \left ( t_i \right )_{i=1}^n \) are recursively estimated, --- starting from uniform parametrisation (this will be the fit if `maxIters` is 0). +-- starting from uniform parametrisation (this will be the fit when \( \texttt{maxIters} = 0 \)). -- -- The iteration ends when any of the following conditions are satisfied: -- @@ -90,16 +145,16 @@ import Math.Vector2D -- of its corresponding point to fit \( q_i \), -- * the maximum iteration limit \( \texttt{maxIters} \) has been reached. fitPiece - :: Double -- ^ \( \texttt{t_tol} \), the tolerance for the Bézier parameter - -> Double -- ^ \( \texttt{dist_tol} \), tolerance for the distance + :: Double -- ^ \( \texttt{dist_tol} \), tolerance for the distance + -> Double -- ^ \( \texttt{t_tol} \), the tolerance for the Bézier parameter -> Int -- ^ \( \texttt{maxIters} \), 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 ), ArgMax Double Double ) -fitPiece t_tol dist_tol maxIters p tp qs r tr = + -> ( Cubic.Bezier ( Point2D Double ), ArgMax Double Double ) +fitPiece dist_tol t_tol maxIters p tp qs r tr = runST do -- Initialise the parameter values to a uniform subdivision. ts <- Unboxed.Vector.unsafeThaw ( Unboxed.Vector.generate n uniform ) @@ -116,7 +171,7 @@ fitPiece t_tol dist_tol maxIters p tp qs r tr = f2 t = h0 t *^ ( MkVector2D p ) f3 t = h3 t *^ ( MkVector2D r ) - loop :: forall s. Unboxed.MVector s Double -> Int -> ST s ( Bezier ( Point2D Double ), ArgMax Double Double ) + loop :: forall s. Unboxed.MVector s Double -> Int -> ST s ( Cubic.Bezier ( Point2D Double ), ArgMax Double Double ) loop ts count = do let hermiteParameters :: Mat22 Double -> Vector2D Double -> Int -> [ Point2D Double ] -> ST s ( Vector2D Double ) @@ -147,8 +202,8 @@ fitPiece t_tol dist_tol maxIters p tp qs r tr = cp1 = ( ( s1 / 3 ) *^ tp ) • p cp2 = ( ( (-s2) / 3 ) *^ tr ) • r - bez :: Bezier ( Point2D Double ) - bez = Bezier p cp1 cp2 r + bez :: Cubic.Bezier ( Point2D Double ) + bez = Cubic.Bezier p cp1 cp2 r -- 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 @@ -157,7 +212,7 @@ fitPiece t_tol dist_tol maxIters p tp qs r tr = ti <- lift ( Unboxed.MVector.unsafeRead ts i ) let poly :: [ Complex Double ] - poly = map (:+ 0) $ ddist @( Vector2D Double ) bez q + poly = map (:+ 0) $ Cubic.ddist @( Vector2D Double ) bez q ti' <- case laguerre epsilon 1 poly ( ti :+ 0 ) of x :+ y | isNaN x @@ -170,7 +225,7 @@ fitPiece t_tol dist_tol maxIters p tp qs r tr = $> x let sq_dist :: Double - sq_dist = quadrance @( Vector2D Double ) q ( bezier @( Vector2D Double ) bez ti' ) + sq_dist = quadrance @( Vector2D Double ) q ( Cubic.bezier @( Vector2D Double ) bez ti' ) modify' ( second ( <> Max ( Arg ti' sq_dist ) ) ) lift ( Unboxed.MVector.unsafeWrite ts i ti' ) diff --git a/src/lib/Math/Bezier/Stroke.hs b/src/lib/Math/Bezier/Stroke.hs index a3e51de..eb8bba4 100644 --- a/src/lib/Math/Bezier/Stroke.hs +++ b/src/lib/Math/Bezier/Stroke.hs @@ -58,7 +58,7 @@ import Data.GenericLens.Internal -- MetaBrush import qualified Math.Bezier.Cubic as Cubic import Math.Bezier.Cubic.Fit - ( fitPiece ) + ( fitSpline ) import qualified Math.Bezier.Quadratic as Quadratic import Math.Epsilon ( epsilon ) @@ -113,9 +113,9 @@ stroke Empty = Left Empty stroke ( spt0 :<| Empty ) = Left . removePointData $ ( Point2D 0 0 --> coords spt0 :: Vector2D Double ) • brushShape @x spt0 stroke allPts@( spt0 :<| spt1 :<| spts ) | isClosed - = Right ( fwd, bwd ) + = Right ( fwdPts, bwdPts ) | otherwise - = Left ( fwd <> bwd ) + = Left ( fwdPts <> bwdPts ) where startOffset, endOffset :: Vector2D Double @@ -136,8 +136,8 @@ stroke allPts@( spt0 :<| spt1 :<| spts ) -> True _ -> False - fwd, bwd :: Seq ( StrokePoint () ) - ( fwd, bwd ) = go spt0 ( spt1 :<| spts ) + fwdPts, bwdPts :: Seq ( StrokePoint () ) + ( fwdPts, bwdPts ) = go spt0 ( spt1 :<| spts ) (<~>) :: ( Monoid a, Monoid b ) @@ -149,7 +149,6 @@ stroke allPts@( spt0 :<| spt1 :<| spts ) -- Connecting paths at a point of discontinuity of the tangent vector direction (G1 discontinuity). -- This happens at corners of the brush path (including endpoints of an open brush path, where the tangent flips direction). joinAndContinue :: Vector2D Double -> StrokePoint d -> Seq ( StrokePoint d ) -> ( Seq ( StrokePoint () ), Seq ( StrokePoint () ) ) - --joinAndContinue tgt sp sps = go sp sps joinAndContinue _ _ Empty -- Closed curve. | isClosed @@ -185,63 +184,60 @@ stroke allPts@( spt0 :<| spt1 :<| spts ) go sp0 ( sp1 :<| sps ) | PathPoint {} <- sp1 , let - p0, p1, fop0, fop1, bop0, bop1 :: Point2D Double + p0, p1 :: Point2D Double p0 = coords sp0 p1 = coords sp1 - fop0 = offset ( withTangent tgt ( brushShape @x sp0 ) ) • p0 - fop1 = offset ( withTangent tgt ( brushShape @x sp1 ) ) • p1 - bop0 = offset ( withTangent ( (-1) *^ tgt ) ( brushShape @x sp0 ) ) • p0 - bop1 = offset ( withTangent ( (-1) *^ tgt ) ( brushShape @x sp1 ) ) • p1 tgt :: Vector2D Double tgt = p0 --> p1 brush :: Double -> Seq ( StrokePoint () ) brush t = lerpBrush t ( brushShape @x sp0 ) ( brushShape @x sp1 ) - fwdPts, bwdPts :: [ Point2D Double ] - fwdPts - = [ ( offset $ withTangent tgt $ brush t ) - • ( lerp @( Vector2D Double ) t p0 p1 ) - | t <- [0.1,0.2..0.9] - ] - bwdPts - = [ ( offset $ withTangent ( (-1) *^ tgt ) $ brush t ) • - ( lerp @( Vector2D Double ) t p0 p1 ) - | t <- [0.9,0.8..0.1] - ] - = ( fitCurve fop0 tgt fwdPts fop1 tgt, fitCurve bop1 ( (-1) *^ tgt ) bwdPts bop0 ( (-1) *^ tgt ) ) + fwd, bwd :: Double -> ( Point2D Double, Vector2D Double ) + fwd t + = ( offset ( withTangent tgt ( brush t ) ) + • lerp @( Vector2D Double ) t p0 p1 + , tgt -- wrong + ) + bwd t + = ( offset ( withTangent ( (-1) *^ tgt ) ( brush s ) ) + • lerp @( Vector2D Double ) s p0 p1 + , (-1) *^ tgt -- wrong + ) + where + s :: Double + s = 1 - t + = ( fitCurve fwd, fitCurve bwd ) <~> joinAndContinue tgt sp1 sps -- Quadratic Bézier curve. go sp0 ( sp1 :<| sp2 :<| sps ) | ControlPoint {} <- sp1 , PathPoint {} <- sp2 , let - p0, p1, p2, fop0, fop2, bop0, bop2 :: Point2D Double + p0, p1, p2 :: Point2D Double p0 = coords sp0 p1 = coords sp1 p2 = coords sp2 - fop0 = offset ( withTangent tgt0 ( brushShape @x sp0 ) ) • p0 - fop2 = offset ( withTangent tgt2 ( brushShape @x sp2 ) ) • p2 - bop0 = offset ( withTangent ( (-1) *^ tgt0 ) ( brushShape @x sp0 ) ) • p0 - bop2 = offset ( withTangent ( (-1) *^ tgt2 ) ( brushShape @x sp2 ) ) • p2 - tgt0, tgt2 :: Vector2D Double - tgt0 = p0 --> p1 + tgt2 :: Vector2D Double tgt2 = p1 --> p2 bez :: Quadratic.Bezier ( Point2D Double ) bez = Quadratic.Bezier { .. } brush :: Double -> Seq ( StrokePoint () ) brush t = quadraticBezierBrush t ( Quadratic.Bezier ( brushShape @x sp0 ) ( brushShape @x sp1 ) ( brushShape @x sp2 ) ) - fwdPts, bwdPts :: [ Point2D Double ] - fwdPts - = [ ( offset $ withTangent ( Quadratic.bezier' bez t ) $ brush t ) • - ( Quadratic.bezier @( Vector2D Double ) bez t ) - | t <- [0.1,0.2..0.9] - ] - bwdPts - = [ ( offset $ withTangent ( (-1) *^ Quadratic.bezier' bez t ) $ brush t ) • - ( Quadratic.bezier @( Vector2D Double ) bez t ) - | t <- [0.9,0.8..0.1] - ] - = ( fitCurve fop0 tgt0 fwdPts fop2 tgt2, fitCurve bop2 ( (-1) *^ tgt2 ) bwdPts bop0 ( (-1) *^ tgt0 ) ) + fwd, bwd :: Double -> ( Point2D Double, Vector2D Double ) + fwd t + = ( offset ( withTangent ( Quadratic.bezier' bez t ) ( brush t ) ) + • Quadratic.bezier @( Vector2D Double ) bez t + , Quadratic.bezier' bez t -- wrong + ) + bwd t + = ( offset ( withTangent ( (-1) *^ Quadratic.bezier' bez s ) ( brush s ) ) + • Quadratic.bezier @( Vector2D Double ) bez s + , (-1) *^ Quadratic.bezier' bez s -- wrong + ) + where + s :: Double + s = 1 - t + = ( fitCurve fwd, fitCurve bwd ) <~> joinAndContinue tgt2 sp2 sps -- Cubic Bézier curve. go sp0 ( sp1 :<| sp2 :<| sp3 :<| sps ) @@ -249,35 +245,33 @@ stroke allPts@( spt0 :<| spt1 :<| spts ) , ControlPoint {} <- sp2 , PathPoint {} <- sp3 , let - p0, p1, p2, p3, fop0, fop3, bop0, bop3 :: Point2D Double + p0, p1, p2, p3 :: Point2D Double p0 = coords sp0 p1 = coords sp1 p2 = coords sp2 p3 = coords sp3 - fop0 = offset ( withTangent tgt0 ( brushShape @x sp0 ) ) • p0 - fop3 = offset ( withTangent tgt3 ( brushShape @x sp3 ) ) • p3 - bop0 = offset ( withTangent ( (-1) *^ tgt0 ) ( brushShape @x sp0 ) ) • p0 - bop3 = offset ( withTangent ( (-1) *^ tgt3 ) ( brushShape @x sp3 ) ) • p3 - tgt0, tgt3 :: Vector2D Double - tgt0 = p0 --> p1 + tgt3 :: Vector2D Double tgt3 = p2 --> p3 bez :: Cubic.Bezier ( Point2D Double ) bez = Cubic.Bezier { .. } brush :: Double -> Seq ( StrokePoint () ) brush t = cubicBezierBrush t ( Cubic.Bezier ( brushShape @x sp0 ) ( brushShape @x sp1 ) ( brushShape @x sp2 ) ( brushShape @x sp3 ) ) - fwdPts, bwdPts :: [ Point2D Double ] - fwdPts - = [ ( offset $ withTangent ( Cubic.bezier' bez t ) $ brush t ) • - ( Cubic.bezier @( Vector2D Double ) bez t ) - | t <- [0.1,0.2..0.9] - ] - bwdPts - = [ ( offset $ withTangent ( (-1) *^ Cubic.bezier' bez t ) $ brush t ) • - ( Cubic.bezier @( Vector2D Double ) bez t ) - | t <- [0.9,0.8..0.1] - ] - = ( fitCurve fop0 tgt0 fwdPts fop3 tgt3, fitCurve bop3 ( (-1) *^ tgt3 ) bwdPts bop0 ( (-1) *^ tgt0 ) ) + fwd, bwd :: Double -> ( Point2D Double, Vector2D Double ) + fwd t + = ( offset ( withTangent ( Cubic.bezier' bez t ) ( brush t ) ) + • Cubic.bezier @( Vector2D Double ) bez t + , Cubic.bezier' bez t -- wrong + ) + bwd t + = ( offset ( withTangent ( (-1) *^ Cubic.bezier' bez s ) ( brush s ) ) + • Cubic.bezier @( Vector2D Double ) bez s + , (-1) *^ Cubic.bezier' bez s -- wrong + ) + where + s :: Double + s = 1 - t + = ( fitCurve fwd, fitCurve bwd ) -- fitCurve bwd ) <~> joinAndContinue tgt3 sp3 sps go p0 ps = error $ "stroke: unrecognised stroke type\n" <> show ( p0 :<| ps ) @@ -292,7 +286,7 @@ brushShape = view typed . pointData removePointData :: Seq ( StrokePoint d ) -> Seq ( StrokePoint () ) removePointData = fmap ( set ( field @"pointData" ) () ) -lerpBrush :: forall d. Double -> Seq ( StrokePoint d ) -> Seq ( StrokePoint d ) -> Seq ( StrokePoint () ) +lerpBrush :: forall d. Show d => Double -> Seq ( StrokePoint d ) -> Seq ( StrokePoint d ) -> Seq ( StrokePoint () ) lerpBrush t p0s p1s = f <$> p0s <*> p1s where f :: StrokePoint d -> StrokePoint d -> StrokePoint () @@ -302,9 +296,9 @@ lerpBrush t p0s p1s = f <$> p0s <*> p1s f ( ControlPoint { coords = p0 } ) ( ControlPoint { coords = p1 } ) = CP $ lerp @( Vector2D Double ) t p0 p1 - f _ _ = error "stroke: incompatible brushes" + f p1 p2 = error $ "stroke: incompatible brushes " <> show [ p1, p2 ] -quadraticBezierBrush :: forall d. Double -> Quadratic.Bezier ( Seq ( StrokePoint d ) ) -> Seq ( StrokePoint () ) +quadraticBezierBrush :: forall d. Show d => Double -> Quadratic.Bezier ( Seq ( StrokePoint d ) ) -> Seq ( StrokePoint () ) quadraticBezierBrush t ( Quadratic.Bezier p0s p1s p2s ) = Seq.zipWith3 f p0s p1s p2s where f :: StrokePoint d -> StrokePoint d -> StrokePoint d -> StrokePoint () @@ -316,7 +310,7 @@ quadraticBezierBrush t ( Quadratic.Bezier p0s p1s p2s ) = Seq.zipWith3 f p0s p1s ( ControlPoint { coords = p1 } ) ( ControlPoint { coords = p2 } ) = CP $ Quadratic.bezier @( Vector2D Double ) ( Quadratic.Bezier { .. } ) t - f _ _ _ = error "stroke: incompatible brushes" + f p1 p2 p3 = error $ "stroke: incompatible brushes " <> show [ p1, p2, p3 ] cubicBezierBrush :: forall d. Show d => Double -> Cubic.Bezier ( Seq ( StrokePoint d ) ) -> Seq ( StrokePoint () ) cubicBezierBrush t ( Cubic.Bezier p0s p1s p2s p3s ) = Seq.zipWith4 f p0s p1s p2s p3s @@ -335,13 +329,17 @@ cubicBezierBrush t ( Cubic.Bezier p0s p1s p2s p3s ) = Seq.zipWith4 f p0s p1s p2s f p1 p2 p3 p4 = error $ "stroke: incompatible brushes " <> show [ p1, p2, p3, p4 ] fitCurve - :: Point2D Double -> Vector2D Double -> [ Point2D Double ] -> Point2D Double -> Vector2D Double + :: ( Double -> ( Point2D Double, Vector2D Double ) ) -> Seq ( StrokePoint () ) -fitCurve p tp qs r tr = case fitPiece 1e-4 1e-3 100 p tp qs r tr of - ( Cubic.Bezier p0 p1 p2 p3, _ ) -> - -- TODO: don't duplicate endpoints - PP p0 :<| CP p1 :<| CP p2 :<| PP p3 :<| Empty +fitCurve curve = splinePoints $ fitSpline 2 10 1e-4 1e-5 80 curve +splinePoints :: Seq ( Cubic.Bezier ( Point2D Double ) ) -> Seq ( StrokePoint () ) +splinePoints Empty = Empty +splinePoints ps@( Cubic.Bezier p0 _ _ _ :<| _ ) = PP p0 :<| go ps + where + go :: Seq ( Cubic.Bezier ( Point2D Double ) ) -> Seq ( StrokePoint () ) + go Empty = Empty + go ( Cubic.Bezier _ p1 p2 p3 :<| pts ) = CP p1 :<| CP p2 :<| PP p3 :<| go pts --------------------------------------------------------------------------------