curve fitting: implement recursive subdivision

This commit is contained in:
sheaf 2020-08-29 18:26:11 +02:00
parent d4561ad549
commit 62bb1451c5
2 changed files with 135 additions and 82 deletions

View file

@ -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' )

View file

@ -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
--------------------------------------------------------------------------------