diff --git a/Geom2D.hs b/Geom2D.hs --- a/Geom2D.hs +++ b/Geom2D.hs @@ -1,63 +1,123 @@ -{-# LANGUAGE BangPatterns #-} +{-# LANGUAGE TypeFamilies, MultiParamTypeClasses, FlexibleInstances, DeriveFunctor, FunctionalDependencies #-} -- | Basic 2 dimensional geometry functions. module Geom2D where +import qualified Data.Vector.Generic.Mutable as M +import qualified Data.Vector.Generic as G +import qualified Data.Vector.Unboxed as V +import Control.Monad + infixl 6 ^+^, ^-^ infixl 7 *^, ^*, ^/ infixr 5 $* -data Point = Point { - pointX :: {-# UNPACK #-} !Double, - pointY :: {-# UNPACK #-} !Double} - deriving Eq +data Point a = Point { + pointX :: !a, + pointY :: !a} + deriving (Eq, Functor) -instance Show Point where +type DPoint = Point Double + +instance Show a => Show (Point a) where show (Point x y) = "Point " ++ show x ++ " " ++ show y -- | A transformation (x, y) -> (ax + by + c, dx + ey + d) -data Transform = Transform { - xformA :: {-# UNPACK #-} !Double, - xformB :: {-# UNPACK #-} !Double, - xformC :: {-# UNPACK #-} !Double, - xformD :: {-# UNPACK #-} !Double, - xformE :: {-# UNPACK #-} !Double, - xformF :: {-# UNPACK #-} !Double } - deriving Show +data Transform a = Transform { + xformA :: !a, + xformB :: !a, + xformC :: !a, + xformD :: !a, + xformE :: !a, + xformF :: !a } + deriving (Eq, Show, Functor) -data Line = Line Point Point -data Polygon = Polygon [Point] +data Line a = Line (Point a) (Point a) + deriving (Show, Eq, Functor) +data Polygon a = Polygon [Point a] + deriving (Show, Eq, Functor) -class AffineTransform a where - transform :: Transform -> a -> a +class AffineTransform a b | a -> b where + transform :: Transform b -> a -> a -instance AffineTransform Transform where +instance Num a => AffineTransform (Transform a) a where + {-# INLINE transform #-} transform (Transform a' b' c' d' e' f') (Transform a b c d e f) = Transform (a*a'+b'*d) (a'*b + b'*e) (a'*c+b'*f +c') (d'*a+e'*d) (d'*b+e'*e) (d'*c+e'*f+f') -instance AffineTransform Point where +instance Num a => AffineTransform (Point a) a where + {-# INLINE transform #-} transform (Transform a b c d e f) (Point x y) = Point (a*x + b*y + c) (d*x + e*y + f) -instance AffineTransform Polygon where +instance Num a => AffineTransform (Polygon a) a where + {-# INLINE transform #-} transform t (Polygon p) = Polygon $ map (transform t) p +newtype instance V.MVector s (Point a) = MV_Point (V.MVector s (a, a)) +newtype instance V.Vector (Point a) = V_Point (V.Vector (a, a)) + +instance V.Unbox a => V.Unbox (Point a) +instance V.Unbox a => M.MVector V.MVector (Point a) where + {-# INLINE basicLength #-} + {-# INLINE basicUnsafeSlice #-} + {-# INLINE basicOverlaps #-} + {-# INLINE basicUnsafeNew #-} + {-# INLINE basicUnsafeReplicate #-} + {-# INLINE basicUnsafeRead #-} + {-# INLINE basicUnsafeWrite #-} + {-# INLINE basicClear #-} + {-# INLINE basicSet #-} + {-# INLINE basicUnsafeCopy #-} + {-# INLINE basicUnsafeGrow #-} + basicLength (MV_Point v) = M.basicLength v + basicUnsafeSlice i n (MV_Point v) = MV_Point $ M.basicUnsafeSlice i n v + basicOverlaps (MV_Point v1) (MV_Point v2) = M.basicOverlaps v1 v2 + basicUnsafeNew n = MV_Point `liftM` M.basicUnsafeNew n + basicUnsafeReplicate n (Point x y) = MV_Point `liftM` M.basicUnsafeReplicate n (x,y) + basicUnsafeRead (MV_Point v) i = uncurry Point `liftM` M.basicUnsafeRead v i + basicUnsafeWrite (MV_Point v) i (Point x y) = M.basicUnsafeWrite v i (x,y) + basicClear (MV_Point v) = M.basicClear v + basicSet (MV_Point v) (Point x y) = M.basicSet v (x,y) + basicUnsafeCopy (MV_Point v1) (MV_Point v2) = M.basicUnsafeCopy v1 v2 + basicUnsafeGrow (MV_Point v) n = MV_Point `liftM` M.basicUnsafeGrow v n + +instance V.Unbox a => G.Vector V.Vector (Point a) where + {-# INLINE basicUnsafeFreeze #-} + {-# INLINE basicUnsafeThaw #-} + {-# INLINE basicLength #-} + {-# INLINE basicUnsafeSlice #-} + {-# INLINE basicUnsafeIndexM #-} + {-# INLINE elemseq #-} + basicUnsafeFreeze (MV_Point v) = V_Point `liftM` G.basicUnsafeFreeze v + basicUnsafeThaw (V_Point v) = MV_Point `liftM` G.basicUnsafeThaw v + basicLength (V_Point v) = G.basicLength v + basicUnsafeSlice i n (V_Point v) = V_Point $ G.basicUnsafeSlice i n v + basicUnsafeIndexM (V_Point v) i + = uncurry Point `liftM` G.basicUnsafeIndexM v i + basicUnsafeCopy (MV_Point mv) (V_Point v) + = G.basicUnsafeCopy mv v + elemseq _ (Point x y) z = G.elemseq (undefined :: V.Vector a) x + $ G.elemseq (undefined :: V.Vector a) y z + -- | Operator for applying a transformation. -($*) :: AffineTransform a => Transform -> a -> a +($*) :: AffineTransform a b => Transform b -> a -> a t $* p = transform t p +{-# INLINE ($*) #-} -- | Calculate the inverse of a transformation. -inverse :: Transform -> Maybe Transform +inverse :: (Eq a, Num a, Fractional a) => Transform a -> Maybe (Transform a) inverse (Transform a b c d e f) = case a*e - b*d of 0 -> Nothing det -> Just $! Transform (a/det) (d/det) (-(a*c + d*f)/det) (b/det) (e/det) (-(b*c + e*f)/det) +{-# SPECIALIZE inverse :: Transform Double -> Maybe (Transform Double) #-} -- | Return the parameters (a, b, c) for the normalised equation -- of the line: @a*x + b*y + c = 0@. -lineEquation :: Line -> (Double, Double, Double) +lineEquation :: Floating t => Line t -> (t, t, t) lineEquation (Line (Point x1 y1) (Point x2 y2)) = (a, b, c) where a = a' / d b = b' / d @@ -65,100 +125,129 @@ a' = y1 - y2 b' = x2 - x1 d = sqrt(a'*a' + b'*b') +{-# SPECIALIZE lineEquation :: Line Double -> (Double, Double, Double) #-} -- | Return the signed distance from a point to the line. If the -- distance is negative, the point lies to the right of the line - -lineDistance :: Line -> Point -> Double +lineDistance :: Floating a => Line a -> Point a -> a lineDistance l = \(Point x y) -> a*x + b*y + c where (a, b, c) = lineEquation l +{-# SPECIALIZE lineDistance :: Line Double -> DPoint -> Double #-} +-- | Return the point on the line closest to the given point. +closestPoint :: Fractional a => Line a -> Point a -> Point a +closestPoint (Line p1 p2) p3 = Point px py + where + (Point dx dy) = p2 ^-^ p1 + u = dy*pointY p3 + dx*pointX p3 + v = pointX p1*pointY p2 - pointX p2*pointY p1 + m = dx*dx + dy*dy + px = (dx*u + dy*v) / m + py = (dy*u - dx*v) / m +{-# specialize closestPoint :: Line Double -> Point Double -> Point Double #-} + -- | The lenght of the vector. -vectorMag :: Point -> Double +vectorMag :: Floating a => Point a -> a vectorMag (Point x y) = sqrt(x*x + y*y) {-# INLINE vectorMag #-} -- | The angle of the vector, in the range @(-'pi', 'pi']@. -vectorAngle :: Point -> Double +vectorAngle :: RealFloat a => Point a -> a vectorAngle (Point 0.0 0.0) = 0.0 vectorAngle (Point x y) = atan2 y x {-# INLINE vectorAngle #-} -- | The unitvector with the given angle. -dirVector :: Double -> Point +dirVector :: Floating a => a -> Point a dirVector angle = Point (cos angle) (sin angle) {-# INLINE dirVector #-} -- | The unit vector with the same direction. -normVector :: Point -> Point +normVector :: Floating a => Point a -> Point a normVector p@(Point x y) = Point (x/l) (y/l) where l = vectorMag p +{-# INLINE normVector #-} -- | Scale vector by constant. -(*^) :: Double -> Point -> Point +(*^) :: Num a => a -> Point a -> Point a s *^ (Point x y) = Point (s*x) (s*y) {-# INLINE (*^) #-} -- | Scale vector by reciprocal of constant. -(^/) :: Point -> Double -> Point +(^/) :: Fractional a => Point a -> a -> Point a (Point x y) ^/ s = Point (x/s) (y/s) {-# INLINE (^/) #-} -- | Scale vector by constant, with the arguments swapped. -(^*) :: Point -> Double -> Point +(^*) :: Num a => Point a -> a -> Point a p ^* s = s *^ p {-# INLINE (^*) #-} -- | Add two vectors. -(^+^) :: Point -> Point -> Point +(^+^) :: Num a => Point a -> Point a -> Point a (Point x1 y1) ^+^ (Point x2 y2) = Point (x1+x2) (y1+y2) {-# INLINE (^+^) #-} -- | Subtract two vectors. -(^-^) :: Point -> Point -> Point +(^-^) :: Num a => Point a -> Point a -> Point a (Point x1 y1) ^-^ (Point x2 y2) = Point (x1-x2) (y1-y2) {-# INLINE (^-^) #-} -- | Dot product of two vectors. -(^.^) :: Point -> Point -> Double +(^.^) :: Num a => Point a -> Point a -> a (Point x1 y1) ^.^ (Point x2 y2) = x1*x2 + y1*y2 {-# INLINE (^.^) #-} -- | Cross product of two vectors. -vectorCross :: Point -> Point -> Double +vectorCross :: Num a => Point a -> Point a -> a vectorCross (Point x1 y1) (Point x2 y2) = x1*y2 - y1*x2 {-# INLINE vectorCross #-} -- | Distance between two vectors. -vectorDistance :: Point -> Point -> Double +vectorDistance :: Floating a => Point a -> Point a -> a vectorDistance p q = vectorMag (p^-^q) {-# INLINE vectorDistance #-} -- | Interpolate between two vectors. -interpolateVector :: Point -> Point -> Double -> Point +interpolateVector :: (Num a) => Point a -> Point a -> a -> Point a interpolateVector a b t = t*^b ^+^ (1-t)*^a {-# INLINE interpolateVector #-} -- | Create a transform that rotates by the angle of the given vector +-- and multiplies with the magnitude of the vector. +rotateScaleVec :: Num a => Point a -> Transform a +rotateScaleVec (Point x y) = Transform x (-y) 0 y x 0 +{-# INLINE rotateScaleVec #-} + +-- | reflect the vector over the X-axis. +flipVector :: (Num a) => Point a -> Point a +flipVector (Point x y) = Point x (-y) +{-# INLINE flipVector #-} + +-- | Create a transform that rotates by the angle of the given vector -- with the x-axis -rotateVec :: Point -> Transform +rotateVec :: Floating a => Point a -> Transform a rotateVec v = Transform x (-y) 0 y x 0 where Point x y = normVector v +{-# INLINE rotateVec #-} -- | Create a transform that rotates by the given angle (radians). -rotate :: Double -> Transform +rotate :: Floating s => s -> Transform s rotate a = Transform (cos a) (negate $ sin a) 0 (sin a) (cos a) 0 +{-# INLINE rotate #-} -- | Rotate vector 90 degrees left. -rotate90L :: Transform +rotate90L :: Floating s => Transform s rotate90L = rotateVec (Point 0 1) +{-# INLINE rotate90L #-} -- | Rotate vector 90 degrees right. -rotate90R :: Transform +rotate90R :: Floating s => Transform s rotate90R = rotateVec (Point 0 (-1)) +{-# INLINE rotate90R #-} -- | Create a transform that translates by the given vector. -translate :: Point -> Transform +translate :: Num a => Point a -> Transform a translate (Point x y) = Transform 1 0 x 0 1 y - +{-# INLINE translate #-} diff --git a/Geom2D/CubicBezier.hs b/Geom2D/CubicBezier.hs --- a/Geom2D/CubicBezier.hs +++ b/Geom2D/CubicBezier.hs @@ -3,6 +3,7 @@ module Geom2D.CubicBezier (module Geom2D.CubicBezier.Basic, module Geom2D.CubicBezier.Approximate, + module Geom2D.CubicBezier.Overlap, module Geom2D.CubicBezier.Outline, module Geom2D.CubicBezier.Curvature, module Geom2D.CubicBezier.Intersection, @@ -15,6 +16,7 @@ import Geom2D.CubicBezier.Approximate import Geom2D.CubicBezier.Outline import Geom2D.CubicBezier.Curvature +import Geom2D.CubicBezier.Overlap import Geom2D.CubicBezier.Intersection import Geom2D.CubicBezier.MetaPath diff --git a/Geom2D/CubicBezier/Approximate.hs b/Geom2D/CubicBezier/Approximate.hs --- a/Geom2D/CubicBezier/Approximate.hs +++ b/Geom2D/CubicBezier/Approximate.hs @@ -1,16 +1,17 @@ -{-# LANGUAGE BangPatterns #-} -module Geom2D.CubicBezier.Approximate ( - approximatePath, approximatePathMax, approximateCurve, approximateCurveWithParams) +{-# LANGUAGE BangPatterns, MultiWayIf #-} +module Geom2D.CubicBezier.Approximate +-- (approximatePath, approximateQuadPath, approximatePathMax, approximateCubic) where import Geom2D -import Geom2D.CubicBezier.Numeric import Geom2D.CubicBezier.Basic -import Data.Function -import Data.List +import Geom2D.CubicBezier.Numeric import Data.Maybe +import Data.List +import qualified Data.Vector.Unboxed as V import qualified Data.Map as M +import Data.Function -interpolate :: Double -> Double -> Double -> Double +interpolate :: (Num a) => a -> a -> a -> a interpolate a b x = (1-x)*a + x*b -- | Approximate a function with piecewise cubic bezier splines using @@ -18,128 +19,294 @@ -- approximated by using a finite number of samples. It is recommended -- to avoid changes in direction by subdividing the original function -- at points of inflection. +approximatePath :: (V.Unbox a, Ord a, Floating a) => + (a -> (Point a, Point a)) -- ^ The function to approximate and it's derivative + -> Int + -- ^ The number of discrete samples taken to + -- approximate each subcurve. More samples are + -- more precise but take more time to calculate. + -- For good precision 16 is a good candidate. + -> a -- ^ The tolerance + -> a -- ^ The lower parameter of the function + -> a -- ^ The upper parameter of the function + -> Bool + -- ^ Calculate the result faster, but with more + -- subcurves. Runs typically 10 times faster, but + -- generates 50% more subcurves. Useful for interactive use. + -> [CubicBezier a] +approximatePath f n tol tmin tmax fast + | err < tol = [curve] + | otherwise = approximatePath' f n tol tmin tmax fast + where + (curve, err) = approx1cubic n f tmin tmax (if fast then 0 else 5) +{-# SPECIALIZE approximatePath :: (Double -> (DPoint, DPoint)) -> Int -> Double + -> Double -> Double -> Bool -> [CubicBezier Double] #-} -approximatePath :: (Double -> (Point, Point)) -- ^ The function to approximate and it's derivative - -> Double -- ^ The number of discrete samples taken to approximate each subcurve - -> Double -- ^ The tolerance - -> Double -- ^ The lower parameter of the function - -> Double -- ^ The upper parameter of the function - -> [CubicBezier] -approximatePath f n tol tmin tmax - | err <= tol = [cb_out] - | otherwise = approximatePath f n tol tmin terr ++ - approximatePath f n tol terr tmax +-- | Approximate a function with piecewise quadratic bezier splines +-- using a least-squares fit, within the given tolerance. It is +-- recommended to avoid changes in direction by subdividing the +-- original function at points of inflection. +approximateQuadPath :: (Show a, V.Unbox a, Ord a, Floating a) => + (a -> (Point a, Point a)) -- ^ The function to approximate and it's derivative + -> a -- ^ The tolerance + -> a -- ^ The lower parameter of the function + -> a -- ^ The upper parameter of the function + -> Bool + -- ^ Calculate the result faster, but with more + -- subcurves. + -> [QuadBezier a] +approximateQuadPath f tol tmin tmax fast + | err < tol = [curve] + | otherwise = approximateQuad' f tol tmin tmax fast where - (cb_out, terr', err) = approximateCurveWithParams curveCb - points ts tol - terr = interpolate tmin tmax terr' - ts = [i/(n+1) | i <- [1..n]] - points = map (fst . f . interpolate tmin tmax) ts - (t0, t0') = f tmin - (t1, t1') = f tmax - curveCb = CubicBezier t0 (t0^+^t0') (t1^-^t1') t1 + curve = approx1quad f tmin tmax + err = maxDist f curve tmin tmax +{-# SPECIALIZE approximateQuadPath :: (Double -> (DPoint, DPoint)) -> Double -> + Double -> Double -> Bool -> [QuadBezier Double] #-} + +-- find the distance between the function at t and the quadratic bezier. +-- calculate the value and derivative at t, and improve the closeness of t. +quadDist :: (V.Unbox a, Floating a) => + (a -> (Point a, Point a)) -> QuadBezier a -> a -> a -> a -> a +quadDist f qb tmin tmax t = + let p = fst (f $ interpolate tmin tmax t) + (b, b') = evalBezierDeriv qb t + -- distance from p to the normal at b(t) / velocity + nd = ((p ^-^ b) ^.^ b') / (b'^.^b') + in vectorDistance p $ evalBezier qb (t + nd) +phi :: (Floating a) => a +phi = (-1 + sqrt 5) / 2 +goldSearch :: (Ord a, Floating a) => (a -> a) -> a +goldSearch f = + goldSearch' f 0 x1 x2 1 (f 0) + (f x1) (f x2) (f 1) 4 + where x1 = 1 - phi + x2 = phi + +goldSearch' :: (Ord a, Floating a) => + (a -> a) -> a -> a -> a -> + a -> a -> a -> a -> a -> Int -> a +goldSearch' f x0 x1 x2 x3 y0 y1 y2 y3 maxiter + | maxiter < 1 = maximum [y0, y1, y2, y3] + | y1 < y2 = + let x25 = x1 + phi*(x3-x1) + y25 = f x25 + in goldSearch' f x1 x2 x25 x3 y1 y2 y25 y3 (maxiter-1) + | otherwise = + let x05 = x2 + phi*(x0-x2) + y05 = f x05 + in goldSearch' f x0 x05 x1 x2 y0 y05 y1 y2 (maxiter-1) + +-- find maximum distance using golden section search +maxDist :: (V.Unbox a, Ord a, Floating a) => + (a -> (Point a, Point a)) -> + QuadBezier a -> a -> a -> a +maxDist f qb tmin tmax = goldSearch (quadDist f qb tmin tmax) + +approxquad :: (Ord a, Floating a) => + Point a -> Point a -> Point a -> Point a -> QuadBezier a +approxquad p0 p0' p1' p1 + | abs (pointY q') < abs (pointX q'*1e-3) = + QuadBezier p0 (interpolateVector p0 p1 0.5) p1 + | otherwise = QuadBezier p0 (p1^+^p1'^*t) p1 + where + q = rotateVec (flipVector p0') $* p1^-^p0 + q' = rotateVec (flipVector p0') $* p1' + t = - pointY q / pointY q' + +approx1quad :: (Ord a, Floating a) => + (a -> (Point a, Point a)) -> a -> a -> QuadBezier a +approx1quad f tmin tmax = + approxquad p0 p0' p1' p1 + where (p0, p0') = f tmin + (p1, p1') = f tmax + +splitQuad :: (Show a, V.Unbox a, Ord a, Floating a) => + a -> a -> (a -> (Point a, Point a)) + -> a -> a -> Int -> (a, a, QuadBezier a, a, QuadBezier a) +splitQuad node offset f tmin tmax maxiter + | maxiter < 1 || (err0 < 2*err1 && err0 > err1/2) = + (tmid, err0, curve0, err1, curve1) + | otherwise = + splitQuad (if err0 < err1 then node+offset else node-offset) + (offset/2) f tmin tmax (maxiter-1) + where + tmid = interpolate tmin tmax node + curve0 = approx1quad f tmin tmid + err0 = maxDist f curve0 tmin tmid + curve1 = approx1quad f tmid tmax + err1 = maxDist f curve1 tmid tmax + +approximateQuad' :: (Show a, V.Unbox a, Ord a, Floating a) => + (a -> (Point a, Point a)) -> + a -> a -> a -> Bool -> + [QuadBezier a] +approximateQuad' f tol tmin tmax fast = + (if err0 <= tol + then [curve0] + else approximateQuad' f tol tmin tmid fast) ++ + (if err1 <= tol + then [curve1] + else approximateQuad' f tol tmid tmax fast) + where + (tmid, err0, curve0, err1, curve1) = + splitQuad 0.5 0.25 f tmin tmax (if fast then 0 else 5) + +approximatePath' :: (V.Unbox a, Ord a, Floating a) => + (a -> (Point a, Point a)) -> Int -> + a -> a -> a -> Bool -> + [CubicBezier a] +approximatePath' f n tol tmin tmax fast = + (if err0 <= tol + then [curve0] + else approximatePath' f n tol tmin tmid fast) ++ + (if err1 <= tol + then [curve1] + else approximatePath' f n tol tmid tmax fast) + where + (tmid, err0, curve0, err1, curve1) = + splitCubic 0.5 0.25 n f tmin tmax (if fast then 0 else 5) +--{-# SPECIALIZE approximatePath' :: (Double -> (Point Double, Point Double)) -> Int -> Double -> Double -> Double -> [CubicBezier Double] #-} + -- | Like approximatePath, but limit the number of subcurves. -approximatePathMax :: Int -- ^ The maximum number of subcurves - -> (Double -> (Point, Point)) -- ^ The function to approximate and it's derivative - -> Double -- ^ The number of discrete samples taken to approximate each subcurve - -> Double -- ^ The tolerance - -> Double -- ^ The lower parameter of the function - -> Double -- ^ The upper parameter of the function - -> [CubicBezier] -approximatePathMax m f n tol tmin tmax = - approxMax f tol m ts segments - where segments = M.singleton err (FunctionSegment tmin tmax t_err outline) +approximatePathMax :: (V.Unbox a, Floating a, Ord a) => + Int -- ^ The maximum number of subcurves + -> (a -> (Point a, Point a)) -- ^ The function to approximate and it's derivative + -> Int + -- ^ The number of discrete samples taken to + -- approximate each subcurve. More samples are + -- more precise but take more time to calculate. + -- For good precision 16 is a good candidate. + -> a -- ^ The tolerance + -> a -- ^ The lower parameter of the function + -> a -- ^ The upper parameter of the function + -> Bool + -- ^ Calculate the result faster, but with more + -- subcurves. Runs typically 10 times faster, but + -- generates 50% more subcurves. Useful for interactive use. + -> [CubicBezier a] +approximatePathMax m f n tol tmin tmax fast = + approxMax f tol m ts fast segments + where segments = M.singleton err (FunctionSegment tmin tmax outline) (p0, p0') = f tmin (p1, p1') = f tmax - ts = [i/(n+1) | i <- [1..n]] - points = map (fst . f . interpolate tmin tmax) ts + ts = V.map (\i -> fromIntegral i/(fromIntegral n+1) `asTypeOf` tmin) $ + V.enumFromN (1::Int) n + points = V.map (fst . f . interpolate tmin tmax) ts curveCb = CubicBezier p0 (p0^+^p0') (p1^-^p1') p1 - (outline, t_err', err) = approximateCurveWithParams curveCb - points ts tol - t_err = interpolate tmin tmax t_err' - -data FunctionSegment = FunctionSegment { - fs_t_min :: {-# UNPACK #-} !Double, -- the least t param of the segment in the original curve - _fs_t_max :: {-# UNPACK #-} !Double, -- the max t param of the segment in the original curve - _fs_t_err :: {-# UNPACK #-} !Double, -- the param where the error is maximal - fs_curve :: CubicBezier -- the curve segment + (outline, err) = + approximateCubic curveCb points (Just ts) (if fast then 0 else 5) +{-# SPECIALIZE approximatePathMax :: + Int -> (Double -> (Point Double, Point Double)) -> Int + -> Double -> Double -> Double -> Bool -> [CubicBezier Double] #-} +data FunctionSegment a = FunctionSegment { + fsTmin :: !a, -- the least t param of the segment in the original curve + _fsTmax :: !a, -- the max t param of the segment in the original curve + fsCurve :: CubicBezier a -- the curve segment } -- Keep a map from maxError to FunctionSegment for each subsegment to keep -- track of the segment with the maximum error. This ensures a n -- log(n) execution time, rather than n^2 when a list is used. -approxMax :: (Double -> (Point, Point)) -> Double -> Int - -> [Double] -> M.Map Double FunctionSegment -> [CubicBezier] -approxMax f tol n ts segments - | n < 1 = error "Minimum number of segments is one." - | (n == 1) || (err < tol) = - map fs_curve $ sortBy (compare `on` fs_t_min) $ map snd $ M.toList segments - | otherwise = approxMax f tol (n-1) ts $ - M.insert err_l (FunctionSegment t_min t_err t_err_l curve_l) $ - M.insert err_r (FunctionSegment t_err t_max t_err_r curve_r) +approxMax :: (V.Unbox a, Ord a, Floating a) => + (a -> (Point a, Point a)) -> a -> Int + -> V.Vector a -> Bool -> M.Map a (FunctionSegment a) -> + [CubicBezier a] +approxMax f tol n ts fast segments + | (n <= 1) || (err < tol) = + map fsCurve $ sortBy (compare `on` fsTmin) $ + map snd $ M.toList segments + | otherwise = approxMax f tol (n-1) ts fast $ + M.insert err_l (FunctionSegment t_min t_mid curve_l) $ + M.insert err_r (FunctionSegment t_mid t_max curve_r) newSegments where - ((err, FunctionSegment t_min t_max t_err _), newSegments) = M.deleteFindMax segments - (fmin, fmin') = f t_min - (fmid, fmid') = f t_err - (fmax, fmax') = f t_max - fcurve_l = CubicBezier fmin (fmin^+^fmin') (fmid^-^fmid') fmid - fcurve_r = CubicBezier fmid (fmid^+^fmid') (fmax^-^fmax') fmax - pointsl = map (fst . f . interpolate t_min t_err) ts - pointsr = map (fst . f . interpolate t_err t_max) ts - t_err_l = interpolate t_min t_err t_err_l' - t_err_r = interpolate t_err t_max t_err_r' - (curve_l, t_err_l', err_l) = approximateCurveWithParams fcurve_l pointsl ts tol - (curve_r, t_err_r', err_r) = approximateCurveWithParams fcurve_r pointsr ts tol + ((err, FunctionSegment t_min t_max _), newSegments) = + M.deleteFindMax segments + (t_mid, err_l, curve_l, err_r, curve_r) = + splitCubic 0.5 0.25 n f t_min t_max (if fast then 0 else 5) +{-# SPECIALIZE approxMax :: (Double -> (Point Double, Point Double)) -> Double -> Int + -> V.Vector Double -> Bool -> M.Map Double (FunctionSegment Double) -> [CubicBezier Double] #-} + +splitCubic :: (V.Unbox a, Ord a, Floating a) => + a -> a -> Int -> (a -> (Point a, Point a)) + -> a -> a -> Int -> (a, a, CubicBezier a, a, CubicBezier a) +splitCubic node offset n f tmin tmax maxiter + | maxiter < 1 || (err0 < 2*err1 && err0 > err1/2) = + (tmid, err0, curve0, err1, curve1) + | otherwise = + splitCubic (if err0 < err1 then node+offset else node-offset) + (offset/2) n f tmin tmax (maxiter-1) + where + tmid = interpolate tmin tmax node + (curve0, err0) = approx1cubic n f tmin tmid maxiter + (curve1, err1) = approx1cubic n f tmid tmax maxiter +{-# SPECIALIZE splitCubic :: Double -> Double -> Int -> (Double -> (Point Double, Point Double)) + -> Double -> Double -> Int -> (Double, Double, CubicBezier Double, Double, CubicBezier Double) #-} + +approx1cubic :: (V.Unbox a, Ord a, Floating a) => + Int -> (a -> (Point a, Point a)) -> a -> a -> + Int -> (CubicBezier a, a) +approx1cubic n f t0 t1 maxiter = + approximateCubic curveCb points (Just ts) maxiter + where (p0, p0') = f t0 + (p1, p1') = f t1 + ts = V.map (\i -> fromIntegral i/(fromIntegral n+1)) + (V.enumFromN 1 n :: V.Vector Int) + points = V.map (fst . f . interpolate t0 t1) ts + curveCb = CubicBezier p0 (p0^+^p0') (p1^+^p1') p1 +{-# SPECIALIZE approx1cubic :: Int -> (Double -> (Point Double, Point Double)) -> Double -> Double -> Int -> (CubicBezier Double, Double) #-} --- | @approximateCurve b pts eps@ finds the least squares fit of a bezier +-- | @approximateCubic b pts maxiter@ finds the least squares fit of a bezier -- curve to the points @pts@. The resulting bezier has the same first -- and last control point as the curve @b@, and have tangents colinear with @b@. --- return the curve, the parameter with maximum error, and maximum error. --- Calculate to withing eps tolerance. - -approximateCurve :: CubicBezier -> [Point] -> Double -> (CubicBezier, Double, Double) -approximateCurve curve@(CubicBezier p1 _ _ p4) pts eps = - approximateCurveWithParams curve pts (approximateParams curve p1 p4 pts) eps - --- | Like approximateCurve, but also takes an initial guess of the --- parameters closest to the points. This might be faster if a good --- guess can be made. - -approximateCurveWithParams :: CubicBezier -> [Point] -> [Double] -> Double -> (CubicBezier, Double, Double) -approximateCurveWithParams curve pts ts eps = - let (c, newTs) = fromMaybe (curve, ts) $ - approximateCurve' curve pts ts 40 (bezierParamTolerance curve eps) 1 - curvePts = map (evalBezier c) newTs - distances = zipWith vectorDistance pts curvePts - (t, maxError) = maximumBy (compare `on` snd) (zip ts distances) - in (c, t, maxError) - -data LSParams = LSParams {-# UNPACK #-} !Double - {-# UNPACK #-} !Double - {-# UNPACK #-} !Double - {-# UNPACK #-} !Double - {-# UNPACK #-} !Double - {-# UNPACK #-} !Double +approximateCubic :: (V.Unbox a, Ord a, Floating a) => + CubicBezier a -- ^ Curve + -> V.Vector (Point a) -- ^ Points + -> Maybe (V.Vector a) -- ^ Params. Approximate if Nothing + -> Int -- ^ Maximum iterations + -> (CubicBezier a, a) -- ^ result curve and maximum error +approximateCubic curve pts mbTs maxiter = + let ts = fromMaybe (approximateParams (cubicC0 curve) (cubicC3 curve) pts) mbTs + curve2 = fromMaybe curve $ lsqDist curve pts ts + (bt, bt') = V.unzip $ V.map (evalBezierDeriv curve2) ts + err = V.maximum $ V.zipWith vectorDistance pts bt + (c, _, _, err2, _) = + fromMaybe (curve2, ts, undefined, err, undefined) $ + approximateCubic' curve2 pts ts maxiter err bt bt' + in (c, err2) +{-# SPECIALIZE approximateCubic :: CubicBezier Double -> V.Vector (Point Double) + -> Maybe (V.Vector Double) -> Int -> (CubicBezier Double, Double) #-} -addParams :: LSParams -> LSParams -> LSParams -addParams (LSParams a b c d e f) (LSParams a' b' c' d' e' f') = - LSParams (a+a') (b+b') (c+c') (d+d') (e+e') (f+f') +-- find (a, b) which minimises ∑ᵢ(a*aᵢ + b*bᵢ + epsᵢ)² +leastSquares :: (V.Unbox a, Fractional a, Eq a) => + V.Vector a -> V.Vector a -> V.Vector a -> Maybe (a, a) +leastSquares as bs epses = solveLinear2x2 a b c d e f + where + square x = x*x + a = V.sum $ V.map square as + b = V.sum $ V.zipWith (*) as bs + c = V.sum $ V.zipWith (*) as epses + d = b + e = V.sum $ V.map square bs + f = V.sum $ V.zipWith (*) bs epses +{-# SPECIALIZE leastSquares ::V.Vector Double -> V.Vector Double -> V.Vector Double -> Maybe (Double, Double) #-} --- find the least squares between the points p_i and B(t_i) for --- bezier curve B, where pts contains the points p_i and ts --- the values of t_i . +-- find the least squares between the points pᵢ and B(tᵢ) for +-- bezier curve B, where pts contains the points pᵢ and ts +-- the values of tᵢ . -- The tangent at the beginning and end is maintained. -- Since the start and end point remains the same, --- we need to find the new value of p2' = p1 + alpha1 * (p2 - p1) --- and p3' = p4 + alpha2 * (p3 - p4) --- minimizing (sum |B(t_i) - p_i|^2) gives a linear equation --- with two unknown values (alpha1 and alpha2), which can be --- solved easily -leastSquares :: CubicBezier -> [Point] -> [Double] -> Maybe CubicBezier -leastSquares (CubicBezier (Point !p1x !p1y) (Point !p2x !p2y) (Point !p3x !p3y) (Point !p4x !p4y)) pts ts = let +-- we need to find the new value of p2' = p1 + α₁ * (p2 - p1) +-- and p₃' = p4 + α2 * (p3 - p4) +-- minimizing (∑|B(tᵢ) - pᵢ|²) gives a linear equation +-- with two unknown values (α₁ and α₂) +lsqDist :: (V.Unbox a, Fractional a, Eq a) => + CubicBezier a + -> V.Vector (Point a) -> V.Vector a -> Maybe (CubicBezier a) +lsqDist (CubicBezier (Point !p1x !p1y) (Point !p2x !p2y) (Point !p3x !p3y) (Point !p4x !p4y)) pts ts = let calcParams t (Point px py) = let t2 = t * t; t3 = t2 * t ax = 3 * (p2x - p1x) * (t3 - 2 * t2 + t) @@ -148,76 +315,88 @@ by = 3 * (p3y - p4y) * (t2 - t3) cx = (p4x - p1x) * (3 * t2 - 2 * t3) + p1x - px cy = (p4y - p1y) * (3 * t2 - 2 * t3) + p1y - py - in LSParams - (ax * ax + ay * ay) - (ax * bx + ay * by) - (ax * cx + ay * cy) - (bx * ax + by * ay) - (bx * bx + by * by) - (bx * cx + by * cy) - LSParams !a !b !c !d !e !f = foldl1' addParams (zipWith calcParams ts pts) - in do (alpha1, alpha2) <- solveLinear2x2 a b c d e f + in (ax * ax + ay * ay, + ax * bx + ay * by, + ax * cx + ay * cy, + bx * ax + by * ay, + bx * bx + by * by, + bx * cx + by * cy) + add6 (!a,!b,!c,!d,!e,!f) (!a',!b',!c',!d',!e',!f') = + (a+a',b+b',c+c',d+d',e+e',f+f') + ( as, bs, cs, ds, es, fs ) = V.foldl1' add6 $ V.zipWith calcParams ts pts + in do (alpha1, alpha2) <- solveLinear2x2 as bs cs ds es fs let cp1 = Point (alpha1 * (p2x - p1x) + p1x) (alpha1 * (p2y - p1y) + p1y) cp2 = Point (alpha2 * (p3x - p4x) + p4x) (alpha2 * (p3y - p4y) + p4y) Just $ CubicBezier (Point p1x p1y) cp1 cp2 (Point p4x p4y) +{-# SPECIALIZE lsqDist :: CubicBezier Double + -> V.Vector (Point Double) -> V.Vector Double -> Maybe (CubicBezier Double) #-} -- calculate the least Squares bezier curve by choosing approximate values -- of t, and iterating again with an improved estimate of t, by taking the -- the values of t for which the points are closest to the curve +approximateCubic' :: (V.Unbox a, Ord a, Floating a) => + CubicBezier a + -> V.Vector (Point a) -> V.Vector a + -> Int -> a -> V.Vector (Point a) + -> V.Vector (Point a) + -> Maybe (CubicBezier a, V.Vector a, V.Vector a, a, V.Vector (Point a)) +approximateCubic' (CubicBezier p1 p2 p3 p4) pts ts maxiter err bt bt' = do + let dir1 = V.map (($* (p2^-^p1)) . rotateVec . flipVector) bt' + dir2 = V.map (($* (p3^-^p4)) . rotateVec . flipVector) bt' + ps = V.zipWith3 (\b b' p -> + rotateVec (flipVector b') $* + (p^-^b)) bt bt' pts + errs = V.map (negate.pointY) ps + as = V.zipWith (\d t -> 3*pointY d*(1-t)*(1-t)*t) + dir1 ts + bs = V.zipWith (\d t -> 3*pointY d*(1-t)*t*t) + dir2 ts + (a,b) <- leastSquares as bs errs + let newTs = V.zipWith5 (\t p d1 d2 b' -> + max 0 $ min 1 $ + t + (pointX p - 3*(1-t)*t*(a*pointX d1*(1-t) + + b*pointX d2*t)) / + vectorMag b') + ts ps dir1 dir2 bt' + newCurve = CubicBezier p1 (p2 ^+^ a*^(p2^-^p1)) (p3 ^+^ b*^(p3^-^p4)) p4 + (bt2,bt2') = V.unzip $ V.map (evalBezierDeriv newCurve) newTs + err2 = V.zipWith vectorDistance pts bt2 + maxErr = V.maximum err2 + -- alternative method for finding the t values: + -- newTs = V.zipWith (-) ts (V.zipWith (calcDeltaT newCurve) pts ts) + if maxiter < 1 || abs(err - maxErr) <= err/8 + then return (newCurve, newTs, err2, maxErr, bt2) + else approximateCubic' newCurve pts newTs (maxiter-1) maxErr bt2 bt2' +{-# SPECIALIZE approximateCubic' :: + CubicBezier Double -> V.Vector (Point Double) -> V.Vector Double + -> Int -> Double -> V.Vector (Point Double) + -> V.Vector (Point Double) + -> Maybe (CubicBezier Double, V.Vector Double, V.Vector Double, Double, V.Vector (Point Double)) #-} -approximateCurve' :: CubicBezier -> [Point] -> [Double] -> Int -> Double -> Double -> Maybe (CubicBezier, [Double]) -approximateCurve' curve pts ts maxiter eps prevDeltaT = do - newCurve <- leastSquares curve pts ts - let deltaTs = zipWith (calcDeltaT newCurve) pts ts - ts' = map (max 0 . min 1) $ zipWith (-) ts deltaTs - newerCurve <- leastSquares curve pts ts' - let deltaTs' = zipWith (calcDeltaT newerCurve) pts ts' - newTs = interpolateTs ts ts' deltaTs deltaTs' - thisDeltaT = maximum $ map abs $ zipWith (-) newTs ts - if maxiter < 1 || - -- Because convergence may be slow initially, make sure it is converging: - (prevDeltaT < eps/2 && thisDeltaT < prevDeltaT / 2) - then do c <- leastSquares curve pts newTs - return (c, newTs) - else approximateCurve' curve pts newTs (maxiter - 1) eps thisDeltaT --- improve convergence by making a better estimate for t --- it is based on the observation that the ratio --- r = dt_2 / dt_1, with dt_2 = t_2 - t_1 and dt_1 = t_1 - t_0 --- for successive approximations of t changes little. --- The infinite sum (dt_1 + dt_1 * r + dt_1 * r^2 + dt_1 * r^3 ...) --- can easily be calculated by dt_1 * (1 / (1 - r)) --- which becomes dt_1^2 / (dt_1 - dt_2) --- Only do this if it appears to converge for all values of t --- If the value of t changes too much keep the old value. --- This improves the convergence by a factor of about 10 -interpolateTs :: [Double] -> [Double] -> [Double] -> [Double] -> [Double] -interpolateTs ts ts' deltaTs deltaTs' = - map (max 0 . min 1) ( - if all id $ zipWith (\dT dT' -> dT * dT' > 0 && dT' / dT < 1) deltaTs deltaTs' - then zipWith3 (\t dT dT' -> let - newDt = (dT * dT / (dT - dT')) - in t - (if abs newDt > 0.2 then dT' else newDt)) ts deltaTs deltaTs' - else zipWith (-) ts' deltaTs') - -- approximate t by calculating the distances between all points -- and dividing by the total sum -approximateParams :: CubicBezier -> Point -> Point -> [Point] -> [Double] -approximateParams cb start end pts = let - segments = start : (pts ++ [end]) - dists = zipWith vectorDistance segments (tail segments) - total = sum dists - improve p t = t - calcDeltaT cb p t - in zipWith improve pts $ map (/ total) $ scanl1 (+) dists +approximateParams :: (V.Unbox a, Floating a) => + Point a -> Point a -> V.Vector (Point a) -> V.Vector a +approximateParams start end pts + | V.null pts = V.empty + | otherwise = + let dists = V.generate (V.length pts) + (\i -> if i == 0 + then vectorDistance start (V.unsafeIndex pts 0) + else vectorDistance (V.unsafeIndex pts (i-1)) (V.unsafeIndex pts i)) + total = V.sum dists + vectorDistance (V.last pts) end + in V.map (/ total) $ V.scanl1 (+) dists +{-# SPECIALIZE approximateParams :: + Point Double -> Point Double -> V.Vector (Point Double) -> V.Vector Double #-} --- find a value of t where B(t) is closer between the bezier curve and --- the point (ptx, pty), by solving f' = 0 where --- f(t) = (X(t) - x)^2 + (Y(t) - y)^2, the square of the distance between the bezier and the point --- the reduction of t is one iteration of Newton Raphson: f'(t)/f''(t) --- using more iterations doesn't appear to give an improvement --- See Curve Fitting with Piecewise Parametric Cubics by Stone & Plass -calcDeltaT :: CubicBezier -> Point -> Double -> Double -calcDeltaT curve (Point !ptx !pty) t = let - [Point bezx bezy, Point dbezx dbezy, Point ddbezx ddbezy, _] = evalBezierDerivs curve t - in ((bezx - ptx) * dbezx + (bezy - pty) * dbezy) / - (dbezx * dbezx + dbezy * dbezy + (bezx - ptx) * ddbezx + (bezy - pty) * ddbezy) +-- Alternative method for finding the next t values, using +-- Newton-Rafphson. There is no noticable difference in speed or +-- efficiency. + +-- calcDeltaT :: CubicBezier -> Point -> Double -> Double +-- calcDeltaT curve (Point !ptx !pty) t = let +-- (Point bezx bezy, Point dbezx dbezy, Point ddbezx ddbezy, _) = evalBezierDerivs curve t +-- in ((bezx - ptx) * dbezx + (bezy - pty) * dbezy) / +-- (dbezx * dbezx + dbezy * dbezy + (bezx - ptx) * ddbezx + (bezy - pty) * ddbezy) + diff --git a/Geom2D/CubicBezier/Basic.hs b/Geom2D/CubicBezier/Basic.hs --- a/Geom2D/CubicBezier/Basic.hs +++ b/Geom2D/CubicBezier/Basic.hs @@ -1,8 +1,10 @@ -{-# LANGUAGE BangPatterns #-} +{-# LANGUAGE BangPatterns, FlexibleInstances, MultiParamTypeClasses, DeriveFunctor, ViewPatterns #-} module Geom2D.CubicBezier.Basic - (CubicBezier (..), PathJoin (..), Path (..), AffineTransform (..), + (CubicBezier (..), QuadBezier (..), AnyBezier (..), GenericBezier(..), + PathJoin (..), ClosedPath(..), OpenPath (..), AffineTransform (..), anyToCubic, anyToQuad, + openPathCurves, closedPathCurves, curvesToOpen, curvesToClosed, bezierParam, bezierParamTolerance, reorient, bezierToBernstein, - evalBezier, evalBezierDeriv, evalBezierDerivs, findBezierTangent, + evalBezierDerivs, evalBezier, evalBezierDeriv, findBezierTangent, quadToCubic, bezierHoriz, bezierVert, findBezierInflection, findBezierCusp, arcLength, arcLengthParam, splitBezier, bezierSubsegment, splitBezierN, colinear) @@ -11,84 +13,249 @@ import Geom2D.CubicBezier.Numeric import Math.BernsteinPoly import Numeric.Integration.TanhSinh +import qualified Data.Vector.Unboxed as V +import qualified Data.Vector.Unboxed.Mutable as MV -data CubicBezier = CubicBezier { - bezierC0 :: Point, - bezierC1 :: Point, - bezierC2 :: Point, - bezierC3 :: Point} deriving Show +-- | A cubic bezier curve. +data CubicBezier a = CubicBezier { + cubicC0 :: !(Point a), + cubicC1 :: !(Point a), + cubicC2 :: !(Point a), + cubicC3 :: !(Point a)} + deriving (Eq, Show, Functor) -data PathJoin = JoinLine | JoinCurve Point Point - deriving Show -data Path = OpenPath [(Point, PathJoin)] Point - | ClosedPath [(Point, PathJoin)] - deriving Show +-- | A quadratic bezier curve. +data QuadBezier a = QuadBezier { + quadC0 :: !(Point a), + quadC1 :: !(Point a), + quadC2 :: !(Point a)} + deriving (Eq, Show, Functor) -instance AffineTransform CubicBezier where +-- Use a tuple, because it has 0(1) unzip when using unboxed vectors. +-- | A bezier curve of any degree. +data AnyBezier a = AnyBezier (V.Vector (a, a)) + +class GenericBezier b where + degree :: (V.Unbox a) => b a -> Int + toVector :: (V.Unbox a) => b a -> V.Vector (a, a) + unsafeFromVector :: (V.Unbox a) => V.Vector (a, a) -> b a + +instance GenericBezier CubicBezier where + degree _ = 3 + toVector (CubicBezier (Point ax ay) (Point bx by) + (Point cx cy) (Point dx dy)) = + V.create $ do + v <- MV.new 4 + MV.write v 0 (ax, ay) + MV.write v 1 (bx, by) + MV.write v 2 (cx, cy) + MV.write v 3 (dx, dy) + return v + unsafeFromVector v = CubicBezier + (uncurry Point $ v `V.unsafeIndex` 0) + (uncurry Point $ v `V.unsafeIndex` 1) + (uncurry Point $ v `V.unsafeIndex` 2) + (uncurry Point $ v `V.unsafeIndex` 3) + +instance GenericBezier QuadBezier where + degree _ = 2 + toVector (QuadBezier (Point ax ay) (Point bx by) + (Point cx cy)) = + V.create $ do + v <- MV.new 3 + MV.write v 0 (ax, ay) + MV.write v 1 (bx, by) + MV.write v 2 (cx, cy) + return v + unsafeFromVector v = QuadBezier + (uncurry Point $ v `V.unsafeIndex` 0) + (uncurry Point $ v `V.unsafeIndex` 1) + (uncurry Point $ v `V.unsafeIndex` 2) + +instance GenericBezier AnyBezier where + degree (AnyBezier b) = V.length b + toVector (AnyBezier v) = v + unsafeFromVector = AnyBezier + +data PathJoin a = JoinLine | + JoinCurve (Point a) (Point a) + deriving (Show, Functor) +data OpenPath a = OpenPath [(Point a, PathJoin a)] (Point a) + deriving (Show, Functor) +data ClosedPath a = ClosedPath [(Point a, PathJoin a)] + deriving (Show, Functor) + +instance (Num a) => AffineTransform (CubicBezier a) a where + {-# SPECIALIZE transform :: Transform Double -> CubicBezier Double -> CubicBezier Double #-} transform t (CubicBezier c0 c1 c2 c3) = CubicBezier (transform t c0) (transform t c1) (transform t c2) (transform t c3) +-- | Return the open path as a list of curves. +openPathCurves :: Fractional a => OpenPath a -> [CubicBezier a] +openPathCurves (OpenPath curves p) = go curves p + where + go [] _ = [] + go [(p0, jn)] p = [makeCB p0 jn p] + go ((p0, jn):rest@((p1,_):_)) p = + makeCB p0 jn p1 : go rest p + makeCB p0 (JoinLine) p1 = + CubicBezier p0 (interpolateVector p0 p1 (1/3)) + (interpolateVector p0 p1 (2/3)) p1 + makeCB p0 (JoinCurve p1 p2) p3 = + CubicBezier p0 p1 p2 p3 +-- | Return the closed path as a list of curves +closedPathCurves :: Fractional a => ClosedPath a -> [CubicBezier a] +closedPathCurves (ClosedPath []) = [] +closedPathCurves (ClosedPath (cs@((p1, _):_))) = + openPathCurves (OpenPath cs p1) +-- | Make an open path from a list of curves. The last control point +-- of each curve except the last is ignored. +curvesToOpen :: [CubicBezier a] -> OpenPath a +curvesToOpen [] = OpenPath [] undefined +curvesToOpen [CubicBezier p0 p1 p2 p3] = + OpenPath [(p0, JoinCurve p1 p2)] p3 +curvesToOpen (CubicBezier p0 p1 p2 _:cs) = + OpenPath ((p0, JoinCurve p1 p2):rest) lastP + where + OpenPath rest lastP = curvesToOpen cs + +-- | Make an open path from a list of curves. The last control point +-- of each curve is ignored. +curvesToClosed :: [CubicBezier a] -> ClosedPath a +curvesToClosed cs = ClosedPath cs2 + where + OpenPath cs2 _ = curvesToOpen cs + + +-- | safely convert from `AnyBezier' to `CubicBezier` +anyToCubic :: (V.Unbox a) => AnyBezier a -> Maybe (CubicBezier a) +anyToCubic b@(AnyBezier v) + | degree b == 3 = Just $ unsafeFromVector v + | otherwise = Nothing + +-- | safely convert from `AnyBezier' to `QuadBezier` +anyToQuad :: (V.Unbox a) => AnyBezier a -> Maybe (QuadBezier a) +anyToQuad b@(AnyBezier v) + | degree b == 2 = Just $ unsafeFromVector v + | otherwise = Nothing + +evalBezierDerivsCubic :: Fractional a => + CubicBezier a -> a -> [Point a] +evalBezierDerivsCubic (CubicBezier a b c d) t = + [p, p', p'', p''', Point 0 0] + where + u = 1-t + t2 = t*t + t3 = t2*t + da = 3*^(b^-^a) + db = 3*^(c^-^b) + dc = 3*^(d^-^c) + p = u*^(u*^(u*^a ^+^ 3*t*^b) ^+^ 3*t2*^c) ^+^ t3*^d + p' = u*^(u*^da ^+^ 2*t*^db) ^+^ t2*^dc + p'' = 2*u*^(db^-^da) ^+^ 2*t*^(dc^-^db) + p''' = 2*^(dc^-^2*^db^+^da) +{-# SPECIALIZE evalBezierDerivsCubic :: CubicBezier Double -> Double -> [DPoint] #-} + +evalBezierDerivsQuad :: Fractional a => + QuadBezier a -> a -> [Point a] +evalBezierDerivsQuad (QuadBezier a b c) t = [p, p', p'', Point 0 0] + where + u = 1-t + t2 = t*t + p = u*^(u*^a ^+^ 2*t*^b) ^+^ t2*^c + p' = 2*^(u*^(b^-^a) ^+^ t*^(c^-^b)) + p'' = 2*^(c^-^ 2*^b ^+^ a) +{-# SPECIALIZE evalBezierDerivsQuad :: QuadBezier Double -> Double -> [DPoint] #-} + +-- | Evaluate the bezier and all its derivatives using the modified horner algorithm. +evalBezierDerivs :: (GenericBezier b, V.Unbox a, Fractional a) => + b a -> a -> [Point a] +evalBezierDerivs b t = + zipWith Point (bernsteinEvalDerivs (BernsteinPoly x) t) + (bernsteinEvalDerivs (BernsteinPoly y) t) + where (x, y) = V.unzip $ toVector b +{-# SPECIALIZE evalBezierDerivs :: AnyBezier Double -> Double -> [DPoint] #-} +{-# NOINLINE [2] evalBezierDerivs #-} +{-# RULES "evalBezierDerivs/cubic" evalBezierDerivs = evalBezierDerivsCubic #-} +{-# RULES "evalBezierDerivs/quad" evalBezierDerivs = evalBezierDerivsQuad #-} + -- | Return True if the param lies on the curve, iff it's in the interval @[0, 1]@. -bezierParam :: Double -> Bool +bezierParam :: (Ord a, Num a) => a -> Bool bezierParam t = t >= 0 && t <= 1 --- | Convert a tolerance from the codomain to the domain of the bezier curve. --- Should be good enough, but may not hold for high very tolerance values. - --- The magnification of error from the domain to the codomain of the --- curve approaches the length of the tangent for small errors. We --- can use the maximum of the convex hull of the derivative, and double it to --- have some margin for larger values. -bezierParamTolerance :: CubicBezier -> Double -> Double -bezierParamTolerance (CubicBezier !p1 !p2 !p3 !p4) eps = eps / maxDist +-- | Convert a tolerance from the codomain to the domain of the bezier +-- curve, by dividing by the maximum velocity on the curve. The +-- estimate is conservative, but holds for any value on the curve. +bezierParamTolerance :: (GenericBezier b) => b Double -> Double -> Double +bezierParamTolerance (toVector -> v) eps = eps / maxVel where - maxDist = 6 * (max (vectorDistance p1 p2) $ - max (vectorDistance p2 p3) - (vectorDistance p3 p4)) + maxVel = 3 * V.maximum (V.zipWith vectorDistance (V.map (uncurry Point) v) + (V.map (uncurry Point) $ V.tail v)) -- | Reorient to the curve B(1-t). -reorient :: CubicBezier -> CubicBezier -reorient (CubicBezier p0 p1 p2 p3) = CubicBezier p3 p2 p1 p0 +reorient :: (GenericBezier b, V.Unbox a) => b a -> b a +reorient = unsafeFromVector . V.reverse . toVector +{-# SPECIALIZE reorient :: (V.Unbox a) => AnyBezier a -> AnyBezier a #-} +{-# NOINLINE [2] reorient #-} +reorientCubic :: CubicBezier a -> CubicBezier a +reorientCubic (CubicBezier a b c d) = CubicBezier d c b a + +reorientQuad :: QuadBezier a -> QuadBezier a +reorientQuad (QuadBezier a b c) = QuadBezier c b a +{-# RULES "reorient/cubic" reorient = reorientCubic #-} +{-# RULES "reorient/quad" reorient = reorientQuad #-} + -- | Give the bernstein polynomial for each coordinate. -bezierToBernstein :: CubicBezier -> (BernsteinPoly, BernsteinPoly) -bezierToBernstein (CubicBezier a b c d) = (listToBernstein $ map pointX coeffs, - listToBernstein $ map pointY coeffs) - where coeffs = [a, b, c, d] +bezierToBernstein :: (GenericBezier b, MV.Unbox a) => + b a -> (BernsteinPoly a, BernsteinPoly a) +bezierToBernstein b = (BernsteinPoly x, BernsteinPoly y) + where (x, y) = V.unzip $ toVector b --- | Calculate a value on the curve. -evalBezier :: CubicBezier -> Double -> Point -evalBezier b = fst . evalBezierDeriv b +-- | Calculate a value on the bezier curve. +evalBezier :: (GenericBezier b, MV.Unbox a, Fractional a) => + b a -> a -> Point a +evalBezier bc t = head $ evalBezierDerivs bc t +{-# SPECIALIZE evalBezier :: AnyBezier Double -> Double -> DPoint #-} +{-# NOINLINE [2] evalBezier #-} --- | Calculate a value and the first derivative on the curve. -evalBezierDeriv :: CubicBezier -> Double -> (Point, Point) -evalBezierDeriv (CubicBezier !p0 !p1 !p2 !p3) t = (bt, bt') +evalBezierCubic :: Fractional a => + CubicBezier a -> a -> Point a +evalBezierCubic (CubicBezier a b c d) t = + u*^(u*^(u*^a ^+^ 3*t*^b) ^+^ 3*t2*^c) ^+^ t3*^d where - b0' = 3*^(p1^-^p0) - b0'' = 2*^(3*^(p2^-^p1) ^-^ b0') - b0''' = 6*^(p3^-^ 2*^p2 ^+^ p1) ^-^ b0'' - bt' = b0'^+^(b0''^+^ t*^b0'''^/2)^*t - bt = p0 ^+^ t*^(b0' ^+^ t*^(b0''^/2 ^+^ t*^(b0'''^/6))) + u = 1-t + t2 = t*t + t3 = t2*t +{-# SPECIALIZE evalBezierCubic :: CubicBezier Double -> Double -> DPoint #-} --- | Calculate a value and all derivatives on the curve. -evalBezierDerivs :: CubicBezier -> Double -> [Point] -evalBezierDerivs (CubicBezier !p0 !p1 !p2 !p3) t = [bt, bt', bt'', b0'''] +evalBezierQuad :: Fractional a => + QuadBezier a -> a -> Point a +evalBezierQuad (QuadBezier a b c) t = + u*^(u*^a ^+^ 2*t*^b) ^+^ t2*^c where - b0' = 3*^(p1^-^p0) - b0'' = 2*^(3*^(p2^-^p1) ^-^ b0') - b0''' = 6*^(p3^-^ 2*^p2 ^+^ p1) ^-^ b0'' - bt'' = b0''^+^ b0'''^*t - bt' = b0'^+^(b0''^+^ t*^b0'''^/2)^*t - bt = p0 ^+^ t*^(b0' ^+^ t*^(b0''^/2 ^+^ t*^(b0'''^/6))) + u = 1-t + t2 = t*t +{-# SPECIALIZE evalBezierQuad :: QuadBezier Double -> Double -> DPoint #-} +{-# RULES "evalBezier/cubic" evalBezier = evalBezierCubic #-} +{-# RULES "evalBezier/quad" evalBezier = evalBezierQuad #-} + +-- | Calculate a value and the first derivative on the curve. +evalBezierDeriv :: (V.Unbox a, Fractional a) => + GenericBezier b => b a -> a -> (Point a, Point a) +evalBezierDeriv bc t = (b,b') + where + (b:b':_) = evalBezierDerivs bc t + -- | @findBezierTangent p b@ finds the parameters where -- the tangent of the bezier curve @b@ has the same direction as vector p. -- Use the formula tx * B'y(t) - ty * B'x(t) = 0 where -- B'x is the x value of the derivative of the Bezier curve. -findBezierTangent :: Point -> CubicBezier -> [Double] +findBezierTangent :: DPoint -> CubicBezier Double -> [Double] findBezierTangent (Point tx ty) (CubicBezier (Point x0 y0) (Point x1 y1) (Point x2 y2) (Point x3 y3)) = filter bezierParam $ quadraticRoot a b c where @@ -97,19 +264,18 @@ c = tx*(y1 - y0) - ty*(x1 - x0) -- | Find the parameter where the bezier curve is horizontal. -bezierHoriz :: CubicBezier -> [Double] +bezierHoriz :: CubicBezier Double -> [Double] bezierHoriz = findBezierTangent (Point 1 0) -- | Find the parameter where the bezier curve is vertical. -bezierVert :: CubicBezier -> [Double] +bezierVert :: CubicBezier Double -> [Double] bezierVert = findBezierTangent (Point 0 1) -- | Find inflection points on the curve. - --- Use the formula B''x(t) * B'y(t) - B''y(t) * B'x(t) = 0 --- with B'x(t) the x value of the first derivative at t, --- B''y(t) the y value of the second derivative at t -findBezierInflection :: CubicBezier -> [Double] +-- Use the formula B_x''(t) * B_y'(t) - B_y''(t) * B_x'(t) = 0 with +-- B_x'(t) the x value of the first derivative at t, B_y''(t) the y +-- value of the second derivative at t +findBezierInflection :: CubicBezier Double -> [Double] findBezierInflection (CubicBezier (Point x0 y0) (Point x1 y1) (Point x2 y2) (Point x3 y3)) = filter bezierParam $ quadraticRoot a b c where @@ -127,13 +293,13 @@ -- find a cusp. We look for points where the tangent is both horizontal -- and vertical, which is only true for the zero vector. -findBezierCusp :: CubicBezier -> [Double] +findBezierCusp :: CubicBezier Double -> [Double] findBezierCusp b = filter vertical $ bezierHoriz b where vertical = (== 0) . pointY . snd . evalBezierDeriv b -- | @arcLength c t tol finds the arclength of the bezier c at t, within given tolerance tol. -arcLength :: CubicBezier -> Double -> Double -> Double +arcLength :: CubicBezier Double -> Double -> Double -> Double arcLength b@(CubicBezier c0 c1 c2 c3) t eps = if eps / maximum [vectorDistance c0 c1, vectorDistance c1 c2, @@ -142,19 +308,19 @@ arcLengthEstimate (fst $ splitBezier b t) eps else arcLengthQuad b t eps -arcLengthQuad :: CubicBezier -> Double -> Double -> Double +arcLengthQuad :: CubicBezier Double -> Double -> Double -> Double arcLengthQuad b t eps = result $ absolute eps $ trap distDeriv 0 t where distDeriv t' = vectorMag $ snd $ evalD t' - evalD = evalBezierDeriv b + evalD = evalBezierDeriv b -outline :: CubicBezier -> Double +outline :: CubicBezier Double -> Double outline (CubicBezier c0 c1 c2 c3) = vectorDistance c0 c1 + vectorDistance c1 c2 + vectorDistance c2 c3 -arcLengthEstimate :: CubicBezier -> Double -> (Double, (Double, Double)) +arcLengthEstimate :: CubicBezier Double -> Double -> (Double, (Double, Double)) arcLengthEstimate b eps = (arclen, (estimate, ol)) where estimate = (4*(olL+olR) - ol) / 3 @@ -167,14 +333,14 @@ -- | arcLengthParam c len tol finds the parameter where the curve c has the arclength len, -- within tolerance tol. -arcLengthParam :: CubicBezier -> Double -> Double -> Double +arcLengthParam :: CubicBezier Double -> Double -> Double -> Double arcLengthParam b len eps = arcLengthP b len ol (len/ol) 1 eps where ol = outline b -- Use the Newton rootfinding method. Start with large tolerance -- values, and decrease tolerance as we go closer to the root. -arcLengthP :: CubicBezier -> Double -> Double -> +arcLengthP :: CubicBezier Double -> Double -> Double -> Double -> Double -> Double -> Double arcLengthP !b !len !tot !t !dt !eps | abs diff < eps = t - newDt @@ -182,9 +348,28 @@ where diff = arcLength b t (max (abs (dt*tot/50)) (eps/2)) - len newDt = diff / vectorMag (snd $ evalBezierDeriv b t) +-- | Convert a quadratic bezier to a cubic bezier. +quadToCubic :: (Fractional a) => + QuadBezier a -> CubicBezier a +quadToCubic (QuadBezier a b c) = + CubicBezier a (1/3*^(a ^+^ 2*^b)) (1/3*^(2*^b ^+^ c)) c + -- | Split a bezier curve into two curves. -splitBezier :: CubicBezier -> Double -> (CubicBezier, CubicBezier) -splitBezier (CubicBezier a b c d) t = +splitBezier :: (V.Unbox a, Fractional a) => + GenericBezier b => b a -> a -> (b a, b a) +splitBezier b t = + (unsafeFromVector $ V.zip (bernsteinCoeffs x1) (bernsteinCoeffs y1), + unsafeFromVector $ V.zip (bernsteinCoeffs x2) (bernsteinCoeffs y2)) + where + (x, y) = bezierToBernstein b + (x1, x2) = bernsteinSplit x t + (y1, y2) = bernsteinSplit y t +{-# NOINLINE [2] splitBezier #-} +{-# SPECIALIZE splitBezier :: AnyBezier Double -> Double -> (AnyBezier Double, AnyBezier Double) #-} + +-- | Split a bezier curve into two curves. +splitBezierCubic :: (Fractional a) => CubicBezier a -> a -> (CubicBezier a, CubicBezier a) +splitBezierCubic (CubicBezier a b c d) t = let ab = interpolateVector a b t bc = interpolateVector b c t cd = interpolateVector c d t @@ -192,18 +377,36 @@ bccd = interpolateVector bc cd t mid = interpolateVector abbc bccd t in (CubicBezier a ab abbc mid, CubicBezier mid bccd cd d) +{-# SPECIALIZE splitBezierCubic :: CubicBezier Double -> Double -> (CubicBezier Double, CubicBezier Double) #-} +-- | Split a bezier curve into two curves. +splitBezierQuad :: (Fractional a) => QuadBezier a -> a -> (QuadBezier a, QuadBezier a) +splitBezierQuad (QuadBezier a b c) t = + let ab = interpolateVector a b t + bc = interpolateVector b c t + mid = interpolateVector ab bc t + in (QuadBezier a ab mid, QuadBezier mid bc c) +{-# SPECIALIZE splitBezierQuad :: QuadBezier Double -> Double -> (QuadBezier Double, QuadBezier Double) #-} +{-# RULES "splitBezier/cubic" splitBezier = splitBezierCubic #-} +{-# RULES "splitBezier/quad" splitBezier = splitBezierQuad #-} + + -- | Return the subsegment between the two parameters. -bezierSubsegment :: CubicBezier -> Double -> Double -> CubicBezier +bezierSubsegment :: (Ord a, V.Unbox a, Fractional a) => GenericBezier b => + b a -> a -> a -> b a bezierSubsegment b t1 t2 | t1 > t2 = bezierSubsegment b t2 t1 + | t2 == 0 = fst $ splitBezier b t1 | otherwise = snd $ flip splitBezier (t1/t2) $ fst $ splitBezier b t2 +{-# SPECIALIZE bezierSubsegment :: CubicBezier Double -> Double -> Double -> CubicBezier Double #-} +{-# SPECIALIZE bezierSubsegment :: QuadBezier Double -> Double -> Double -> QuadBezier Double #-} -- | Split a bezier curve into a list of beziers -- The parameters should be in ascending order or -- the result is unpredictable. -splitBezierN :: CubicBezier -> [Double] -> [CubicBezier] +splitBezierN :: (Ord a, V.Unbox a, Fractional a) => + GenericBezier b => b a -> [a] -> [b a] splitBezierN c [] = [c] splitBezierN c [t] = [a, b] where (a, b) = splitBezier c t @@ -211,11 +414,13 @@ bezierSubsegment c 0 t : bezierSubsegment c t u : tail (splitBezierN c $ u:rest) +{-# SPECIALIZE splitBezierN :: CubicBezier Double -> [Double] -> [CubicBezier Double] #-} +{-# SPECIALIZE splitBezierN :: QuadBezier Double -> [Double] -> [QuadBezier Double] #-} -- | Return False if some points fall outside a line with a thickness of the given tolerance. -- fat line calculation taken from the bezier-clipping algorithm (Sederberg) -colinear :: CubicBezier -> Double -> Bool +colinear :: CubicBezier Double -> Double -> Bool colinear (CubicBezier !a !b !c !d) eps = dmax - dmin < eps where ld = lineDistance (Line a d) d1 = ld b @@ -224,5 +429,3 @@ 3/4 * maximum [0, d1, d2]) | otherwise = (4/9 * minimum [0, d1, d2], 4/9 * maximum [0, d1, d2]) - - diff --git a/Geom2D/CubicBezier/Curvature.hs b/Geom2D/CubicBezier/Curvature.hs --- a/Geom2D/CubicBezier/Curvature.hs +++ b/Geom2D/CubicBezier/Curvature.hs @@ -8,14 +8,14 @@ -- | Curvature of the Bezier curve. A negative curvature means the curve -- curves to the right. -curvature :: CubicBezier -> Double -> Double +curvature :: CubicBezier Double -> Double -> Double curvature b t | t == 0 = curvature' b | t == 1 = negate $ curvature' $ reorient b | t < 0.5 = curvature' $ snd $ splitBezier b t | otherwise = negate $ curvature' $ reorient $ fst $ splitBezier b t -curvature' :: CubicBezier -> Double +curvature' :: CubicBezier Double -> Double curvature' (CubicBezier c0 c1 c2 _c3) = 2/3 * b/a^(3::Int) where a = vectorDistance c1 c0 @@ -23,10 +23,10 @@ -- | Radius of curvature of the Bezier curve. This -- is the reciprocal of the curvature. -radiusOfCurvature :: CubicBezier -> Double -> Double +radiusOfCurvature :: CubicBezier Double -> Double -> Double radiusOfCurvature b t = 1 / curvature b t -extrema :: CubicBezier -> BernsteinPoly +extrema :: CubicBezier Double -> BernsteinPoly Double extrema bez = let (x, y) = bezierToBernstein bez x' = bernsteinDeriv x @@ -39,13 +39,13 @@ 3 *~ (x'~*y'' ~- y'~*x'') ~* (y'~*y'' ~+ x'~*x'') -- | Find extrema of the curvature, but not inflection points. -curvatureExtrema :: CubicBezier -> Double -> [Double] +curvatureExtrema :: CubicBezier Double -> Double -> [Double] curvatureExtrema b eps | colinear b eps = [] | otherwise = bezierFindRoot (extrema b) 0 1 $ bezierParamTolerance b eps -radiusSquareEq :: CubicBezier -> Double -> BernsteinPoly +radiusSquareEq :: CubicBezier Double -> Double -> BernsteinPoly Double radiusSquareEq bez d = let (x, y) = bezierToBernstein bez x' = bernsteinDeriv x @@ -58,7 +58,7 @@ -- | Find points on the curve that have a certain radius of curvature. -- Values to the left and to the right of the curve are returned. -findRadius :: CubicBezier -- ^ the curve +findRadius :: CubicBezier Double -- ^ the curve -> Double -- ^ distance -> Double -- ^ tolerance -> [Double] -- ^ result diff --git a/Geom2D/CubicBezier/Intersection.hs b/Geom2D/CubicBezier/Intersection.hs --- a/Geom2D/CubicBezier/Intersection.hs +++ b/Geom2D/CubicBezier/Intersection.hs @@ -7,25 +7,26 @@ import Geom2D.CubicBezier.Basic import Math.BernsteinPoly import Data.Maybe - +import qualified Data.Vector.Unboxed as V +import Debug.Trace -- find the convex hull by comparing the angles of the vectors with -- the cross product and backtracking if necessary. -findOuter' :: Bool -> Point -> Point -> [Point] -> Either [Point] [Point] +findOuter' :: Bool -> DPoint -> DPoint -> [DPoint] -> Either [DPoint] [DPoint] findOuter' !upper !dir !p1 l@(p2:rest) -- backtrack if the direction is outward | if upper then dir `vectorCross` (p2^-^p1) > 0 -- left turn - else dir `vectorCross` (p2^-^p1) < 0 = Left $! l + else dir `vectorCross` (p2^-^p1) < 0 = Left l -- succeed | otherwise = case findOuter' upper (p2^-^p1) p2 rest of Left m -> findOuter' upper dir p1 m Right m -> Right (p1:m) -findOuter' _ _ p1 p = Right $! (p1:p) +findOuter' _ _ p1 p = Right (p1:p) -- find the outermost point. It doesn't look at the x values. -findOuter :: Bool -> [Point] -> [Point] +findOuter :: Bool -> [DPoint] -> [DPoint] findOuter upper (p1:p2:rest) = case findOuter' upper (p2^-^p1) p2 rest of Right l -> p1:l @@ -34,7 +35,7 @@ -- take the y values and turn it in into a convex hull with upper en -- lower points separated. -makeHull :: [Double] -> ([Point], [Point]) +makeHull :: [Double] -> ([DPoint], [DPoint]) makeHull ds = let n = fromIntegral $ length ds - 1 points = zipWith Point [i/n | i <- [0..n]] ds @@ -43,34 +44,36 @@ -- test if the chords cross the fat line -- return the continuation if above the line -testBelow :: Double -> [Point] -> Maybe Double -> Maybe Double +testBelow :: Double -> [DPoint] -> Maybe Double -> Maybe Double testBelow _ [] _ = Nothing testBelow _ [_] _ = Nothing testBelow !dmin (p:q:rest) cont | pointY p >= dmin = cont | pointY p > pointY q = Nothing | pointY q < dmin = testBelow dmin (q:rest) cont - | otherwise = Just $! intersectPt dmin p q + | otherwise = Just $ intersectPt dmin p q -testBetween :: Double -> Point -> Maybe Double -> Maybe Double +testBetween :: Double -> DPoint -> Maybe Double -> Maybe Double testBetween !dmax (Point !x !y) cont | y <= dmax = Just x | otherwise = cont -- test if the chords cross the line y=dmax somewhere -testAbove :: Double -> [Point] -> Maybe Double +testAbove :: Double -> [DPoint] -> Maybe Double testAbove _ [] = Nothing testAbove _ [_] = Nothing testAbove dmax (p:q:rest) | pointY p < pointY q = Nothing | pointY q > dmax = testAbove dmax (q:rest) - | otherwise = Just $! intersectPt dmax p q + | otherwise = Just $ intersectPt dmax p q -- find the x value where the line through the two points -- intersect the line y=d -intersectPt :: Double -> Point -> Point -> Double -intersectPt d (Point x1 y1) (Point x2 y2) = - x1 + (d - y1) * (x2 - x1) / (y2 - y1) +intersectPt :: Double -> DPoint -> DPoint -> Double +intersectPt d (Point x1 y1) (Point x2 y2) + | y1 == y2 = x1 + | otherwise = + x1 + (d - y1) * (x2 - x1) / (y2 - y1) -- make a hull and test over which interval the -- curve is garuanteed to lie inside the fat line @@ -85,12 +88,11 @@ testAbove dmax (reverse lower) Just (left_t, right_t) -bezierClip :: CubicBezier -> CubicBezier -> Double -> Double +bezierClip :: CubicBezier Double -> CubicBezier Double -> Double -> Double -> Double -> Double -> Double -> Double -> Bool -> [(Double, Double)] bezierClip p@(CubicBezier !p0 !p1 !p2 !p3) q@(CubicBezier !q0 !q1 !q2 !q3) tmin tmax umin umax prevClip eps revCurves - -- no intersection | isNothing chop_interval = [] @@ -105,23 +107,36 @@ -- not enough reduction, so split the curve in case we have -- multiple intersections | prevClip > 0.8 && newClip > 0.8 = - if new_tmax - new_tmin > umax - umin -- split the longest segment - then let - (pl, pr) = splitBezier newP 0.5 - half_t = new_tmin + (new_tmax - new_tmin) / 2 - in bezierClip q pl umin umax new_tmin half_t newClip eps (not revCurves) ++ - bezierClip q pr umin umax half_t new_tmax newClip eps (not revCurves) - else let - (ql, qr) = splitBezier q 0.5 - half_t = umin + (umax - umin) / 2 - in bezierClip ql newP umin half_t new_tmin new_tmax newClip eps (not revCurves) ++ - bezierClip qr newP half_t umax new_tmin new_tmax newClip eps (not revCurves) - + if | abs (dmax - dmin) < eps * vectorDistance p0 p3 -> + -- fat line is smaller than tolerance. + if revCurves + then [(umin, tmin), (umax, tmax)] + else [(tmin, umin), (umin, tmin)] + | new_tmax - new_tmin > umax - umin -> + -- split the longest segment + let (pl, pr) = splitBezier newP 0.5 + half_t = new_tmin + (new_tmax - new_tmin) / 2 + in bezierClip q pl umin umax new_tmin half_t + newClip eps (not revCurves) ++ + bezierClip q pr umin umax half_t new_tmax + newClip eps (not revCurves) + | otherwise -> + let (ql, qr) = splitBezier q 0.5 + half_t = umin + (umax - umin) / 2 + in bezierClip ql newP umin half_t + new_tmin new_tmax newClip eps (not revCurves) ++ + bezierClip qr newP half_t umax new_tmin new_tmax + newClip eps (not revCurves) -- iterate with the curves reversed. - | otherwise = bezierClip q newP umin umax new_tmin new_tmax newClip eps (not revCurves) + | otherwise = + bezierClip q newP umin umax new_tmin + new_tmax newClip eps (not revCurves) where - d = lineDistance (Line q0 q3) + q3' | q0 == q3 = + q0 ^+^ (rotate90L $* p3 ^-^ p0) + | otherwise = q3 + d = lineDistance (Line q0 q3') d1 = d q1 d2 = d q2 (dmin, dmax) | d1*d2 > 0 = (3/4 * minimum [0, d1, d2], @@ -136,11 +151,19 @@ new_tmin = tmax * chop_tmin + tmin * (1 - chop_tmin) new_tmax = tmax * chop_tmax + tmin * (1 - chop_tmax) +maxEps = 1e-8 + -- | Find the intersections between two Bezier curves, using the -- Bezier Clip algorithm. Returns the parameters for both curves. -bezierIntersection :: CubicBezier -> CubicBezier -> Double -> [(Double, Double)] -bezierIntersection p q eps = bezierClip p q 0 1 0 1 0 eps False +bezierIntersection :: CubicBezier Double -> CubicBezier Double -> Double -> [(Double, Double)] +bezierIntersection p q eps = bezierClip p q 0 1 0 1 0 eps2 False + where eps2 = max eps maxEps +-- TODO: +-- following curve generate very large list of intersections +-- let b1 = CubicBezier {cubicC0 = Point 365.70000000000005 477.40000000000003, cubicC1 = Point 373.3 476.70000000000005, cubicC2 = Point 381.1 476.3, cubicC3 = Point 389.20000000000005 476.3}; +-- b2 = CubicBezier {cubicC0 = Point 365.70000000000005 477.40000000000003, cubicC1 = Point 365.70000000000005 476.6, cubicC2 = Point 365.70000000000005 475.8, cubicC3 = Point 365.70000000000005 475.0} + ------------------------ Line intersection ------------------------------------- -- Clipping a line uses a simplified version of the Bezier Clip algorithm, -- and uses the (thin) line itself instead of the fat line. @@ -148,14 +171,14 @@ -- | Find the zero of a 1D bezier curve of any degree. Note that this -- can be used as a bernstein polynomial root solver by converting from -- the power basis to the bernstein basis. -bezierFindRoot :: BernsteinPoly -- ^ the bernstein coefficients of the polynomial +bezierFindRoot :: BernsteinPoly Double -- ^ the bernstein coefficients of the polynomial -> Double -- ^ The lower bound of the interval -> Double -- ^ The upper bound of the interval -> Double -- ^ The accuracy -> [Double] -- ^ The roots found bezierFindRoot p tmin tmax eps -- no intersection - | chop_interval == Nothing = [] + | isNothing chop_interval = [] -- not enough reduction, so split the curve in case we have -- multiple intersections @@ -174,7 +197,7 @@ bezierFindRoot newP new_tmin new_tmax eps where - chop_interval = chopHull 0 0 (bernsteinCoeffs p) + chop_interval = chopHull 0 0 (V.toList $ bernsteinCoeffs p) Just (chop_tmin, chop_tmax) = chop_interval newP = bernsteinSubsegment p chop_tmin chop_tmax clip = chop_tmax - chop_tmin @@ -185,7 +208,7 @@ -- Apply a transformation to the bezier that maps the line onto the -- X-axis. Then we only need to test the Y-values for a zero. -bezierLineIntersections :: CubicBezier -> Line -> Double -> [Double] +bezierLineIntersections :: CubicBezier Double -> Line Double -> Double -> [Double] bezierLineIntersections b (Line p q) eps = bezierFindRoot (listToBernstein $ map pointY [p0, p1, p2, p3]) 0 1 $ bezierParamTolerance b eps @@ -193,7 +216,7 @@ fromJust (inverse $ translate p $* rotateVec (q ^-^ p)) $* b -- | Find the closest value(s) on the bezier to the given point, within tolerance. -closest :: CubicBezier -> Point -> Double -> [Double] +closest :: CubicBezier Double -> DPoint -> Double -> [Double] closest cb p@(Point px py) eps = map fst $ filter (\(_, d) -> abs (d - closestDist) < eps/2) $ zip tVals dists diff --git a/Geom2D/CubicBezier/MetaPath.hs b/Geom2D/CubicBezier/MetaPath.hs --- a/Geom2D/CubicBezier/MetaPath.hs +++ b/Geom2D/CubicBezier/MetaPath.hs @@ -1,4 +1,4 @@ -{-# LANGUAGE BangPatterns #-} +{-# LANGUAGE BangPatterns, DeriveFunctor #-} -- | This module implements an extension to paths as used in -- D.E.Knuth's /Metafont/. Metafont gives an alternate way -- to specify paths using bezier curves. I'll give a brief overview of @@ -54,40 +54,76 @@ -- as the first point. module Geom2D.CubicBezier.MetaPath - (unmeta, MetaPath (..), MetaJoin (..), MetaNodeType (..), Tension (..)) + (unmetaOpen, unmetaClosed, ClosedMetaPath(..), OpenMetaPath (..), + MetaJoin (..), MetaNodeType (..), Tension (..)) where import Geom2D import Geom2D.CubicBezier.Basic import Data.List import Text.Printf - -data MetaPath = OpenMetaPath [(Point, MetaJoin)] Point - | CyclicMetaPath [(Point, MetaJoin)] +import qualified Data.Vector as V -data MetaJoin = MetaJoin { metaTypeL :: MetaNodeType - , tensionL :: Tension - , tensionR :: Tension - , metaTypeR :: MetaNodeType - } - | Controls Point Point - deriving Show +data OpenMetaPath a = OpenMetaPath [(Point a, MetaJoin a)] (Point a) + -- ^ A metapath with endpoints +data ClosedMetaPath a = ClosedMetaPath [(Point a, MetaJoin a)] + -- ^ A metapath with cycles. The last join + -- joins the last point with the first. + deriving (Eq, Functor) -data MetaNodeType = Open +data MetaJoin a = MetaJoin { metaTypeL :: MetaNodeType a + -- ^ The nodetype going out of the + -- previous point. The metafont default is + -- @Open@. + , tensionL :: Tension + -- ^ The tension going out of the previous point. + -- The metafont default is 1. + , tensionR :: Tension + -- ^ The tension going into the next point. + -- The metafont default is 1. + , metaTypeR :: MetaNodeType a + -- ^ The nodetype going into the next point. + -- The metafont default is @Open@. + } + | Controls (Point a) (Point a) + -- ^ Specify the control points explicitly. + deriving (Show, Eq, Functor) + +data MetaNodeType a = Open + -- ^ An open node has no direction specified. If + -- it is an internal node, the curve will keep the + -- same direction going into and going out from + -- the node. If it is an endpoint or corner + -- point, it will have curl of 1. | Curl {curlgamma :: Double} - | Direction {nodedir :: Point} - deriving (Eq, Show) + -- ^ The node becomes and endpoint or a corner + -- point. The curl specifies how much the segment + -- `curves`. A curl of `gamma` means that the + -- curvature is `gamma` times that of the + -- following node. + | Direction {nodedir :: Point a} + -- ^ The node has a given direction. + deriving (Eq, Show, Functor) data Tension = Tension {tensionValue :: Double} + -- ^ The tension value specifies how /tense/ the curve is. + -- A higher value means the curve approaches a line + -- segment, while a lower value means the curve is more + -- round. Metafont doesn't allow values below 3/4. | TensionAtLeast {tensionValue :: Double} + -- ^ Like @Tension@, but keep the segment inside the + -- bounding triangle defined by the control points, if + -- there is one. deriving (Eq, Show) -instance Show MetaPath where - show (CyclicMetaPath nodes) = +instance Show a => Show (ClosedMetaPath a) where + show (ClosedMetaPath nodes) = showPath nodes ++ "cycle" + +instance Show a => Show (OpenMetaPath a) where show (OpenMetaPath nodes lastpoint) = showPath nodes ++ showPoint lastpoint -showPath :: [(Point, MetaJoin)] -> String +showPath :: Show a => [(Point a, MetaJoin a)] -> String showPath = concatMap showNodes where showNodes (p, Controls u v) = @@ -106,15 +142,26 @@ typename (Curl g) = printf "{curl %.3f}" g :: String typename (Direction dir) = printf "{%s}" (showPoint dir) :: String -showPoint :: Point -> String -showPoint (Point x y) = printf "(%.3f, %.3f)" x y +showPoint :: Show a => Point a -> String +showPoint (Point x y) = "(" ++ show x ++ ", " ++ show y ++ ")" -- | Create a normal path from a metapath. -unmeta :: MetaPath -> Path -unmeta (OpenMetaPath nodes endpoint) = - unmetaOpen (flip sanitize endpoint $ removeEmptyDirs nodes) endpoint +unmetaOpen :: OpenMetaPath Double -> OpenPath Double +unmetaOpen (OpenMetaPath nodes endpoint) = + unmetaOpen' (flip sanitize endpoint $ + removeEmptyDirs nodes) + endpoint -unmeta (CyclicMetaPath nodes) = +unmetaOpen' :: [(DPoint, MetaJoin Double)] -> DPoint -> OpenPath Double +unmetaOpen' nodes endpoint = + let subsegs = openSubSegments nodes endpoint + path = joinSegments $ map unmetaSubSegment subsegs + in OpenPath path endpoint + + + +unmetaClosed :: ClosedMetaPath Double -> ClosedPath Double +unmetaClosed (ClosedMetaPath nodes) = case spanList bothOpen (removeEmptyDirs nodes) of ([], []) -> error "empty metapath" (l, []) -> if fst (last l) == fst (head l) @@ -128,21 +175,15 @@ -- solve a cyclic metapath as an open path if possible. -- rotate to the defined node, and rotate back after -- solving the path. -unmetaAsOpen :: [(Point, MetaJoin)] -> [(Point, MetaJoin)] -> Path +unmetaAsOpen :: [(DPoint, MetaJoin Double)] -> [(DPoint, MetaJoin Double)] -> ClosedPath Double unmetaAsOpen l m = ClosedPath (l'++m') where n = length m OpenPath o _ = - unmetaOpen (sanitizeCycle (m++l)) (fst $ head (m ++l)) + unmetaOpen' (sanitizeCycle (m++l)) (fst $ head (m ++l)) (m',l') = splitAt n o -unmetaOpen :: [(Point, MetaJoin)] -> Point -> Path -unmetaOpen nodes endpoint = - let subsegs = openSubSegments nodes endpoint - path = joinSegments $ map unmetaSubSegment subsegs - in OpenPath path endpoint - -- decompose into a list of subsegments that need to be solved. -openSubSegments :: [(Point, MetaJoin)] -> Point -> [MetaPath] +openSubSegments :: [(DPoint, MetaJoin Double)] -> DPoint -> [OpenMetaPath Double] openSubSegments [] _ = [] openSubSegments l lastPoint = case spanList (not . breakPoint) l of @@ -155,13 +196,13 @@ _ -> error "openSubSegments': unexpected end of segments" -- join subsegments into one segment -joinSegments :: [Path] -> [(Point, PathJoin)] +joinSegments :: [OpenPath Double] -> [(DPoint, PathJoin Double)] joinSegments = concatMap nodes where nodes (OpenPath n _) = n - nodes (ClosedPath n) = n + --nodes (ClosedPath n) = n -- solve a cyclic metapath where all angles depend on the each other. -unmetaCyclic :: [(Point, MetaJoin)] -> Path +unmetaCyclic :: [(DPoint, MetaJoin Double)] -> ClosedPath Double unmetaCyclic nodes = let points = map fst nodes chords = zipWith (^-^) (tail $ cycle points) points @@ -179,7 +220,7 @@ thetas phis tensionsA tensionsB -- solve a subsegment -unmetaSubSegment :: MetaPath -> Path +unmetaSubSegment :: OpenMetaPath Double -> OpenPath Double -- the simple case where the control points are already given. unmetaSubSegment (OpenMetaPath [(p, Controls u v)] q) = @@ -202,32 +243,30 @@ zipWith6 unmetaJoin points (tail points) thetas phis tensionsA tensionsB in OpenPath (zip points pathjoins) lastpoint -unmetaSubSegment _ = error "unmetaSubSegment: subsegment should not be cyclic" - -removeEmptyDirs :: [(Point, MetaJoin)] -> [(Point, MetaJoin)] +removeEmptyDirs :: [(DPoint, MetaJoin Double)] -> [(DPoint, MetaJoin Double)] removeEmptyDirs = map remove where remove (p, MetaJoin (Direction (Point 0 0)) tl tr jr) = remove (p, MetaJoin Open tl tr jr) remove (p, MetaJoin jl tl tr (Direction (Point 0 0))) = (p, MetaJoin jl tl tr Open) remove j = j -- if p == q, it will become a control point -bothOpen :: [(Point, MetaJoin)] -> Bool +bothOpen :: [(DPoint, MetaJoin Double)] -> Bool bothOpen ((p, MetaJoin Open _ _ Open):(q, _):_) = p /= q bothOpen [(_, MetaJoin Open _ _ Open)] = True bothOpen _ = False -leftOpen :: [(Point, MetaJoin)] -> Bool +leftOpen :: [(DPoint, MetaJoin Double)] -> Bool leftOpen ((p, MetaJoin Open _ _ _):(q, _):_) = p /= q leftOpen [(_, MetaJoin Open _ _ _)] = True leftOpen _ = False -sanitizeCycle :: [(Point, MetaJoin)] -> [(Point, MetaJoin)] +sanitizeCycle :: [(DPoint, MetaJoin Double)] -> [(DPoint, MetaJoin Double)] sanitizeCycle [] = [] sanitizeCycle l = take n $ tail $ sanitize (drop (n-1) $ cycle l) (fst $ head l) where n = length l -sanitize :: [(Point, MetaJoin)] -> Point -> [(Point, MetaJoin)] +sanitize :: [(DPoint, MetaJoin Double)] -> DPoint -> [(DPoint, MetaJoin Double)] sanitize [] _ = [] -- ending open => curl @@ -284,7 +323,7 @@ | otherwise = ([],xs) -- break the subsegment if the angle to the left or the right is defined or a curl. -breakPoint :: [(Point, MetaJoin)] -> Bool +breakPoint :: [(DPoint, MetaJoin Double)] -> Bool breakPoint ((_, MetaJoin _ _ _ Open):(_, MetaJoin Open _ _ _):_) = False breakPoint _ = True @@ -298,28 +337,22 @@ -- see metafont the program: ¶ 283 solveTriDiagonal :: [(Double, Double, Double, Double)] -> [Double] solveTriDiagonal [] = error "solveTriDiagonal: not enough equations" -solveTriDiagonal ((_, b0, c0, d0): rows) = solutions +solveTriDiagonal ((_, b0, c0, d0): rows) = + V.toList $ solveTriDiagonal2 (b0, c0, d0) (V.fromList rows) + +solveTriDiagonal2 :: (Double, Double, Double) -> V.Vector (Double, Double, Double, Double) -> V.Vector Double +solveTriDiagonal2 (!b0, !c0, !d0) rows = solutions where - ((_, vn): twovars) = - reverse $ scanl nextrow (c0/b0, d0/b0) rows + twovars = V.scanl nextrow (c0/b0, d0/b0) rows + solutions = V.scanr nextsol vn (V.unsafeInit twovars) + vn = snd $ V.unsafeLast twovars + nextsol (u, v) ti = v - u*ti nextrow (u, v) (ai, bi, ci, di) = (ci/(bi - u*ai), (di - v*ai)/(bi - u*ai)) - solutions = reverse $ scanl nextsol vn twovars - nextsol ti (u, v) = v - u*ti --- solveTriDiagonal2 :: (Double, Double, Double) -> V.Vector (Double, Double, Double, Double) -> V.Vector Double --- solveTriDiagonal2 (!b0, !c0, !d0) rows = solutions --- where --- solutions = undefined --- twovars = V.scanl nextrow (c0/b0, d0/b0) rows --- solutions = scanr V.unsafeInit --- nextrow (u, v) (ai, bi, ci, di) = --- (ci/(bi - u*ai), (di - v*ai)/(bi - u*ai)) - -- test = ((80.0,58.0,51.0),[(-432.0,78.0,102.0,503.0),(71.0,-82.0,20.0,2130.0),(52.39,-10.43,4.0,56.0),(34.0,38.0,0.0,257.0)]) -- [-15.726940528143576,22.571642107784243,-78.93751365259996,-297.27313545829384,272.74438435742667] - -- solve the cyclic tridiagonal system. -- see metafont the program: ¶ 286 solveCyclicTriD :: [(Double, Double, Double, Double)] -> [Double] @@ -337,7 +370,7 @@ ((un, vn, wn) : reverse (tail threevars)) nextsol t (!u, !v, !w) = (v + w*t0 - t)/u -turnAngle :: Point -> Point -> Double +turnAngle :: DPoint -> DPoint -> Double turnAngle (Point 0 0) _ = 0 turnAngle (Point x y) q = vectorAngle $ rotateVec p $* q where p = Point x (-y) @@ -347,7 +380,7 @@ zipNext l = zip l (tail $ cycle l) -- find the equations for a cycle containing only open points -eqsCycle :: [Tension] -> [Point] -> [Tension] +eqsCycle :: [Tension] -> [DPoint] -> [Tension] -> [Double] -> [(Double, Double, Double, Double)] eqsCycle tensionsA points tensionsB turnAngles = zipWith4 eqTension @@ -361,7 +394,7 @@ -- find the equations for an path with open points. -- The first and last node should be a curl or a given angle -eqsOpen :: [Point] -> [MetaJoin] -> [Point] -> [Double] +eqsOpen :: [DPoint] -> [MetaJoin Double] -> [DPoint] -> [Double] -> [Double] -> [Double] -> [(Double, Double, Double, Double)] eqsOpen _ [MetaJoin mt1 t1 t2 mt2] [delta] _ _ _ = let replaceType Open = Curl 1 @@ -433,7 +466,7 @@ chi = gamma*tensionA*tensionA / (tensionB*tensionB) -- getting the control points -unmetaJoin :: Point -> Point -> Double -> Double -> Tension -> Tension -> PathJoin +unmetaJoin :: DPoint -> DPoint -> Double -> Double -> Tension -> Tension -> PathJoin Double unmetaJoin !z0 !z1 !theta !phi !alpha !beta | abs phi < 1e-4 && abs theta < 1e-4 = JoinLine | otherwise = JoinCurve u v diff --git a/Geom2D/CubicBezier/Numeric.hs b/Geom2D/CubicBezier/Numeric.hs --- a/Geom2D/CubicBezier/Numeric.hs +++ b/Geom2D/CubicBezier/Numeric.hs @@ -1,6 +1,17 @@ -- | Some numerical computations used by the cubic bezier functions module Geom2D.CubicBezier.Numeric where +import Data.Vector.Unboxed as V +import Data.Vector.Unboxed.Mutable as MV +import Data.Matrix.Unboxed as M +import qualified Data.Matrix.Generic as G +import qualified Data.Matrix.Unboxed.Mutable as MM +import Control.Monad.ST +import Control.Monad + +sign x | x < 0 = -1 + | otherwise = 1 + -- | @quadraticRoot a b c@ find the real roots of the quadratic equation -- @a x^2 + b x + c = 0@. It will return one, two or zero roots. @@ -11,7 +22,7 @@ | otherwise = result where d = b*b - 4*a*c - q = - (b + signum b * sqrt d) / 2 + q = - (b + sign b * sqrt d) / 2 x1 = q/a x2 = c/q result | d < 0 = [] @@ -24,9 +35,233 @@ -- >d x + e y + f = 0 -- -- Returns @Nothing@ if no solution is found. -solveLinear2x2 :: Double -> Double -> Double -> Double -> Double -> Double -> Maybe (Double, Double) +solveLinear2x2 :: (Eq a, Fractional a) => a -> a -> a -> a -> a -> a -> Maybe (a, a) solveLinear2x2 a b c d e f = case det of 0 -> Nothing _ -> Just ((c * e - b * f) / det, (a * f - c * d) / det) where det = d * b - a * e +{-# SPECIALIZE solveLinear2x2 :: Double -> Double -> Double -> Double -> Double -> Double -> Maybe (Double, Double) #-} +data SparseMatrix a = + SparseMatrix (V.Vector Int) + (V.Vector (Int, Int)) (M.Matrix a) + +makeSparse :: Unbox a => Vector Int + -- ^ The column index of the first element of each row. + -- Should be ascending in order. + -> M.Matrix a + -- ^ The adjacent coefficients in each row + -> SparseMatrix a + -- ^ A sparse matrix. +makeSparse v m = SparseMatrix v (sparseRanges v vars width) m + where + width = cols m + vars = V.last v + width + +-- give the range of (possibly) nonzero coefficients for each column. +-- The column indices are those of the dense matrix of which the +-- sparse is a representation. +sparseRanges :: V.Vector Int -> Int -> Int -> V.Vector (Int, Int) +sparseRanges v vars width = ranges + where + height = V.length v + ranges = V.scanl' nextRange (nextRange (0,0) 0) $ + V.enumFromN 1 (vars-1) + nextRange (s,e) i = (nextStart s i, nextEnd e i) + nextStart s i + | s >= height = height + | v `V.unsafeIndex` s + width <= i = + nextStart (s+1) i + | otherwise = s + nextEnd e i + | e >= height = height + | v `V.unsafeIndex` e > i = e + | otherwise = nextEnd (e+1) i + +-- | Given a rectangular matrix M, calculate the symmetric square +-- matrix MᵀM which can be used to find a least squares solution to +-- the overconstrained system. +lsqMatrix :: (Num a, Unbox a) => + SparseMatrix a + -- ^ The input system. + -> Matrix a + -- ^ The resulting symmetric matrix as a sparse matrix. + -- The first element of each row is the element on the + -- diagonal. + +lsqMatrix (SparseMatrix rowStart ranges m) + | V.length rowStart /= height = + error "lsqMatrix: lengths don't match." + | otherwise = M.generate (vars, width) coeff + where + (height, width) = dim m + vars = V.last rowStart + width + overlap (s1,e1) (s2, e2) = + (max s1 s2, min e1 e2) + realIndex (r, c) = + (r, c - rowStart `V.unsafeIndex` r) + coeff (r,c) = let + (s, e) | r+c >= vars = (0, 0) + | otherwise = + overlap (ranges `V.unsafeIndex` r) (ranges `V.unsafeIndex` (r+c)) + in V.foldl' (\acc i -> acc + m `M.unsafeIndex` realIndex (i, r) * + m `M.unsafeIndex` realIndex (i, r+c)) 0 $ + V.enumFromN s (e-s) +{-# SPECIALIZE lsqMatrix :: SparseMatrix Double -> M.Matrix Double #-} + +addMatrix :: (Num a, Unbox a) => M.Matrix a -> M.Matrix a -> M.Matrix a +addMatrix = M.zipWith (+) +{-# SPECIALIZE addMatrix :: M.Matrix Double -> M.Matrix Double -> M.Matrix Double #-} + +addVec :: (Num a, Unbox a) => V.Vector a -> V.Vector a -> V.Vector a +addVec = V.zipWith (+) +{-# SPECIALIZE addVec :: V.Vector Double -> V.Vector Double -> V.Vector Double #-} + +-- | Multiply the vector by the transpose of the sparse matrix. +sparseMulT :: (Num a, Unbox a) => + V.Vector a + -> SparseMatrix a + -> V.Vector a +sparseMulT v (SparseMatrix rowStart ranges m) + | V.length v /= height = + error "sparseMulT: lengths don't match." + | otherwise = V.generate vars coeff + where (height, width) = dim m + vars | V.null rowStart = 0 + | otherwise = V.unsafeLast rowStart + width + realIndex (r, c) = + (r, c - rowStart `V.unsafeIndex` r) + coeff i = + let (s, e) = ranges `V.unsafeIndex` i + in V.foldl' (\acc j -> + acc + m `M.unsafeIndex` realIndex (j, i) * + v `V.unsafeIndex` j) 0 $ + V.enumFromN s (e-s) +{-# SPECIALIZE sparseMulT :: V.Vector Double -> SparseMatrix Double -> V.Vector Double #-} + +-- | Sparse matrix * vector multiplication. +sparseMul :: (Num a, Unbox a) => + SparseMatrix a + -> V.Vector a + -> V.Vector a +sparseMul (SparseMatrix rowStart _ranges m) v + | V.length v /= vars = + error "sparseMulT: lengths don't match." + | otherwise = V.generate height coeff + where (height, width) = dim m + vars | V.null rowStart = 0 + | otherwise = V.unsafeLast rowStart + width + coeff i = V.sum $ V.zipWith (*) + (V.unsafeSlice (rowStart V.! i) width v) + (G.unsafeTakeRow m i) +{-# SPECIALIZE sparseMul :: SparseMatrix Double -> V.Vector Double -> V.Vector Double #-} + +-- | LDL* decomposition of the sparse hermitian matrix. The +-- first element of each row is the diagonal component of the D +-- matrix. The following elements are the elements next to the +-- diagonal in the L* matrix (the diagonal components in L* are 1). +-- For efficiency it mutates the matrix inplace. +decompLDL :: (Fractional a, Unbox a) => M.Matrix a -> M.Matrix a +decompLDL m = runST $ do + m2 <- M.thaw m + let (vars, width) = dim m + V.forM_ (V.enumFromN 0 $ vars-1) $ + \startr -> do + pivot <- MM.unsafeRead m2 (startr, 0) + V.forM_ (V.enumFromN 1 $ width-1) $ + \c -> do + el <- MM.unsafeRead m2 (startr, c) + MM.unsafeWrite m2 (startr, c) (el/pivot) + V.forM_ (V.enumFromN 0 $ min (width-1) $ vars-startr-1) $ + \r -> do + r0 <- MM.unsafeRead m2 (startr, r+1) + V.forM_ (V.enumFromN 0 (width-r-1)) $ + \c -> do r1 <- MM.unsafeRead m2 (startr, r+c+1) + el <- MM.unsafeRead m2 (r+startr+1, c) + MM.unsafeWrite m2 (r+startr+1, c) + (el - r0*r1*pivot) + M.unsafeFreeze m2 +{-# SPECIALIZE decompLDL :: Matrix Double -> Matrix Double #-} + +solveLDL :: (Fractional a, Unbox a) => + M.Matrix a -> V.Vector a -> V.Vector a +solveLDL m v + | rows m /= V.length v = error "solveLDL: lengths don't match" + | otherwise = runST $ do + let (vars, width) = M.dim m + sol1 <- MV.new vars + -- forward substitution on the first (width) rows + V.forM_ (V.enumFromN 0 $ min vars width) $ + \i -> do + let vi = v `V.unsafeIndex` i + s <- liftM (V.foldl' (-) vi) $ + V.forM (enumFromN 0 i) $ + \j -> liftM ((m `M.unsafeIndex` (j, i-j)) *) + (MV.unsafeRead sol1 j) + MV.unsafeWrite sol1 i s + + -- forward substitution on the next (height-width) rows + V.forM_ (V.enumFromN width $ vars - width) $ + \i -> do + let vi = v `V.unsafeIndex` i + s <- liftM (V.foldl' (-) vi) $ + V.forM (enumFromN 1 (width-1)) $ + \j -> liftM ((m `M.unsafeIndex` (i-j, j)) *) + (MV.unsafeRead sol1 $ i-j) + MV.unsafeWrite sol1 i s + + -- backward substitution on the last (width) rows + V.forM_ (V.enumFromN 0 $ min vars width) $ + \i -> do + solI <- MV.unsafeRead sol1 (vars-i-1) + let d = m `M.unsafeIndex` (vars-i-1, 0) + s <- liftM (V.foldl' (-) (solI/d)) $ + V.forM (enumFromN 0 i) $ + \j -> liftM ((m `M.unsafeIndex` (vars-i-1, j+1)) *) + (MV.unsafeRead sol1 $ vars-i+j) + MV.unsafeWrite sol1 (vars-i-1) s + + -- backward substitution on the prevous (vars-width) rows + V.forM_ (V.enumFromN width $ vars - width) $ + \i -> do + solI <- MV.unsafeRead sol1 (vars-i-1) + let d = m `M.unsafeIndex` (vars-i-1, 0) + s <- liftM (V.foldl' (-) (solI/d)) $ + V.forM (enumFromN 0 (width-1)) $ + \j -> liftM ((m `M.unsafeIndex` (vars-i-1, j+1)) *) + (MV.unsafeRead sol1 $ vars-i+j) + MV.unsafeWrite sol1 (vars-i-1) s + + V.unsafeFreeze sol1 +{-# SPECIALIZE solveLDL :: M.Matrix Double -> V.Vector Double -> V.Vector Double #-} + +-- | @lsqSolve rowStart M y@ Find a least squares solution x to the +-- system xM = y. +lsqSolve :: (Fractional a, Unbox a) => + SparseMatrix a -- ^ sparse matrix + -> V.Vector a -- ^ Right hand side vector. + -> V.Vector a -- ^ Solution vector +lsqSolve m@(SparseMatrix _ _ m') v + | rows m' /= V.length v = error "lsqSolve: lengths don't match" + | otherwise = solveLDL m2 v2 + where + v2 = sparseMulT v m + m2 = decompLDL $ lsqMatrix m +{-# SPECIALIZE lsqSolve :: SparseMatrix Double -> V.Vector Double -> V.Vector Double #-} + +-- | @lsqSolveDist rowStart M y@ Find a least squares solution of the distance between the points. +lsqSolveDist :: (Fractional a, Unbox a) => + SparseMatrix (a, a) -- ^ sparse matrix + -> V.Vector (a, a) -- ^ Right hand side vector. + -> V.Vector a -- ^ Solution vector +lsqSolveDist (SparseMatrix r s m') v + | rows m' /= V.length v = error "lsqSolve: lengths don't match" + | otherwise = solveLDL m3 v3 + where + v3 = sparseMulT v1 m1 `addVec` sparseMulT v2 m2 + m3 = decompLDL $ lsqMatrix m1 `addMatrix` lsqMatrix m2 + (v1, v2) = V.unzip v + (m1', m2') = M.unzip m' + m1 = SparseMatrix r s m1' + m2 = SparseMatrix r s m2' +{-# SPECIALIZE lsqSolveDist :: SparseMatrix (Double, Double) -> V.Vector (Double, Double) -> V.Vector Double #-} diff --git a/Geom2D/CubicBezier/Outline.hs b/Geom2D/CubicBezier/Outline.hs --- a/Geom2D/CubicBezier/Outline.hs +++ b/Geom2D/CubicBezier/Outline.hs @@ -1,50 +1,31 @@ -- | Offsetting bezier curves and stroking curves. module Geom2D.CubicBezier.Outline - (bezierOffset, bezierOffsetMax) + (bezierOffset) where import Geom2D import Geom2D.CubicBezier.Basic import Geom2D.CubicBezier.Approximate -import Geom2D.CubicBezier.Curvature -offsetPoint :: Double -> Point -> Point -> Point +offsetPoint :: (Floating a) => a -> Point a -> Point a -> Point a offsetPoint dist start tangent = start ^+^ (rotate90L $* dist *^ normVector tangent) -bezierOffsetPoint :: CubicBezier -> Double -> Double -> (Point, Point) +bezierOffsetPoint :: CubicBezier Double -> Double -> Double -> (DPoint, DPoint) bezierOffsetPoint cb dist t = (offsetPoint dist p p', p') where (p, p') = evalBezierDeriv cb t --- Approximate the bezier curve offset by dist. A positive value --- means to the left, a negative to the right. -offsetSegment :: Double -> Double -> CubicBezier -> [CubicBezier] -offsetSegment dist tol cb = - approximatePath (bezierOffsetPoint cb dist) 15 tol 0 1 - -offsetSegmentMax :: Int -> Double -> Double -> CubicBezier -> [CubicBezier] -offsetSegmentMax m dist tol cb = - approximatePathMax m (bezierOffsetPoint cb dist) 15 tol 0 1 - -- | Calculate an offset path from the bezier curve to within -- tolerance. If the distance is positive offset to the left, -- otherwise to the right. A smaller tolerance may require more bezier -- curves in the path to approximate the offset curve -bezierOffset :: CubicBezier -- ^ The curve +bezierOffset :: CubicBezier Double -- ^ The curve -> Double -- ^ Offset distance. + -> Maybe Int -- ^ maximum subcurves -> Double -- ^ Tolerance. - -> [CubicBezier] -- ^ The offset curve -bezierOffset cb dist tol = - --Path $ map BezierSegment $ - concatMap (offsetSegment dist tol) $ - splitBezierN cb $ - findRadius cb dist tol + -> [CubicBezier Double] -- ^ The offset curve +bezierOffset cb dist (Just m) tol = + approximatePathMax m (bezierOffsetPoint cb dist) 15 tol 0 1 False --- | Like bezierOffset, but limit the number of subpaths for each --- smooth subsegment. The number should not be smaller than one. -bezierOffsetMax :: Int -> CubicBezier -> Double -> Double -> [CubicBezier] -bezierOffsetMax n cb dist tol = - -- Path $ map BezierSegment $ - concatMap (offsetSegmentMax n dist tol) $ - splitBezierN cb $ - findRadius cb dist tol +bezierOffset cb dist Nothing tol = + approximatePath (bezierOffsetPoint cb dist) 15 tol 0 1 False diff --git a/Geom2D/CubicBezier/Overlap.lhs b/Geom2D/CubicBezier/Overlap.lhs new file mode 100644 --- /dev/null +++ b/Geom2D/CubicBezier/Overlap.lhs @@ -0,0 +1,980 @@ +> {-# LANGUAGE MultiWayIf, PatternGuards, TemplateHaskell, BangPatterns #-} + +Removing overlap from bezier paths in haskell +============================================= + +This document describes an algorithm for removing overlap and +performing set operations on bezier paths, but at the same time it is +a working module for the haskell `cubicbezier` package. This way it +can serve two purposes at once: someone who wants to implement this +algorithm can use this as an explanation of the algorithm, while at +the same time it is a working version of the described algorithm. + +**Note on porting**: Porting this code to another language should +present no difficulties. However some care must be taken with regards +to lazyness. Often many variables inside the `where` statement aren't +evaluated in all guards, so it's important to evaluate only those +which appear in the guards. The main state can be modified using +mutation instead of copying without any troubles. For modifying the +state, the `lens` library is used. The lens functions can be +interpreted in a mutable language as follows: + + * reading state: + - `view field struct`: `struct.field` + - `get`: `state` (implicit state, usually sweepstate) + - `use field`: `state.field` + + * writing state: + - `set field value struct`: `struct.field = value` + - `field .= value`: `state.field = value` (typically sweepstate) + + * modifying state: + - `over field fun struct`: `struct.field = fun (struct.field)` + - `modify fun`: `state = fun state` + - `field %= fun`: `state.field = fun (state.field)` + +Let's begin with declaring the module and library imports: + +> module Geom2D.CubicBezier.Overlap +> (boolPathOp, union, intersection, difference, +> exclusion, FillRule (..)) +> where +> import Prelude hiding (mapM) +> import Geom2D +> import Geom2D.CubicBezier.Basic +> import Geom2D.CubicBezier.Intersection +> import Math.BernsteinPoly +> import Data.Traversable (mapM) +> import Data.Functor ((<$>)) +> import Data.List (sortBy, sort) +> import Control.Monad.State hiding (mapM) +> import Lens.Micro +> import Lens.Micro.TH +> import Lens.Micro.Mtl +> import qualified Data.Map.Strict as M +> import qualified Data.Set as S + +So what does it mean to remove overlap? Basicly we want to keep +curves where one side is inside the filled region, and the other side +is outside, and discard the rest.Since that could be true only of a +part of the curve, we also need to split each curve when it intersects +another curve. How do you know which side is the inside, and which +side the outside? There are two methods which are use the most: the +[*even-odd rule*](https://en.wikipedia.org/wiki/Even%E2%80%93odd_rule) +and the [*nonzero rule*](https://en.wikipedia.org/wiki/Nonzero-rule). +Instead of hardwiring it, I use higher-order functions to determine +when a turnratio is inside the region to be filled, and how the +turnratio changes with each curve. + +Checking each pair of curves for intersections would work, but is +rather inefficient. We only need to check for overlap when two curves +are adjacent. Fortunately there exist a good method from +*computational geometry*, called a *sweep line algorithm*. The basic +idea is to sweep a vertical line over the input, starting from +leftmost point to the right (of course the opposite direction is also +possible), and to update the input dynamically. We keep track of each +curve that intersects the sweepline by using a balanced tree of +curves. When adding a new curve, it's only necessary to check for +intersections with the curve above and below. Since searching on the +tree takes only `O(log n)` operations, this will save a lot of +computation. + +The input is processed in horizontal order, and after splitting curves +the order must be preserved, so an ordered structure is needed. The +standard map library from `Data.Map` is ideal, and has all the +operations needed. This structure is called the *X-structure*, +since the data is ordered by X coordinate.: + +> type XStruct = M.Map PointEvent [Curve] + +Why `PointEvent`, and not just `Point`? We need to have a `Ord` +instance for the map, which much match our horizontal ordering. A +newtype is ideal, since it has no extra cost, and allows us to define +a Ord instance for defining the relative order. The value from the +map is a list, since there can be many curves starting from the same +point. + +> newtype PointEvent = PointEvent DPoint +> deriving Show + +When the x-coordinates are equal, I use the y-coordinate to determine +the order. + +> instance Eq PointEvent where +> (PointEvent (Point x1 y1)) == (PointEvent (Point x2 y2)) = +> (x1, y1) == (x2, y2) +> +> instance Ord PointEvent where +> compare (PointEvent (Point x1 y1)) (PointEvent (Point x2 y2)) = +> compare (x1, y2) (x2, y1) + +All curves are kept left to right, so we need to remember the +direction for the output: + +The curves intersecting the sweepline are kept in another balanced +Tree, called the *Y-structure*. *These curves are not allowed to +overlap*, except in the endpoints, and will be ordered vertically. +I'll use the `Curve` datatype to define the ordering of the curves, +and to add additional information. The `turnRatio` field is the +turnRatio of the area to the left for a left to right curve, and to +the right for a right to left curve. The `changeTurn` function +determines how the turnRatio will change from up to down. This +together with a test for the *insideness* of a certain turnratio, +allows for more flexibility. Using this, it is possible to generalize +this algorithm to boolean operations! + +The FillRule datatype is used for the exported API: + +> data FillRule = EvenOdd | NonZero + +> data Curve = Curve { +> _bezier :: !(CubicBezier Double), +> _turnRatio :: !(Int, Int), +> _changeTurn :: !((Int, Int) -> (Int, Int))} +> +> trOne :: (Int, Int) +> trOne = (0,0) +> +> makeLenses ''Curve +> +> instance Show Curve where +> show (Curve b a _) = +> "Curve " ++ show b ++ " " ++ show a +> +> type YStruct = S.Set Curve + +The total state for the algorithm consists of the X-structure, the +Y-structure, and the output found so far. I use a trick to make +access to curves above and below the current pointevent more +convenient. I use two sets to represent a focus point into the +Y-structure, where the left set are the elements less than the +pointEvent (above), and the right set the elements greater (below): + + +> data SweepState = SweepState { +> _output :: !(M.Map PointEvent [CubicBezier Double]), +> _yStructLeft :: !YStruct, +> _yStructRight :: !YStruct, +> _xStruct :: !XStruct} +> deriving Show +> +> makeLenses ''SweepState + +Changing the focus point can be done efficiently in `O(log n)` by +mering and splitting again: + +> changeFocus :: DPoint -> SweepState -> SweepState +> changeFocus p sweep = +> let (lStr, rStr) = +> S.split (Curve (CubicBezier p p p p) trOne id) $ +> S.union (view yStructLeft sweep) (view yStructRight sweep) +> in set yStructLeft lStr $ +> set yStructRight rStr +> sweep + +This handy helper function will pass the first curve above to the +given function, and if it doesn't return `Nothing`, remove it from the +state. It does nothing when there is no curve above. + +> withAbove :: (Curve -> Maybe a) -> State SweepState (Maybe a) +> withAbove f = do +> lStr <- use yStructLeft +> if S.null lStr +> then return Nothing +> else let (c, lStr') = S.deleteFindMax lStr +> in case f c of +> Nothing -> +> return Nothing +> Just x -> do +> yStructLeft .= lStr' +> return $ Just x + +The same with the curve below. + +> withBelow :: (Curve -> Maybe a) -> State SweepState (Maybe a) +> withBelow f = do +> rStr <- use yStructRight +> if S.null rStr +> then return Nothing +> else let (c, rStr') = S.deleteFindMin rStr +> in case f c of +> Nothing -> +> return Nothing +> Just x -> do +> yStructRight .= rStr' +> return $ Just x + +`splitYStruct` changes the focus and returns and removes any curves which end in +the current pointEvent: + +> splitYStruct :: DPoint -> State SweepState [Curve] +> splitYStruct p = do +> modify $ changeFocus p +> let go = do +> mbC <- withAbove $ \c -> +> -- remove and return c if it ends in point p +> +> guard (cubicC3 (_bezier c) == p) >> Just c +> case mbC of +> Just c -> +> (c:) <$> go +> Nothing -> return [] +> go + + +=== Some functions on the Sweep state: + +Adding and removing curves from the X structure. + +> insertX :: PointEvent -> [Curve] -> SweepState -> SweepState +> insertX p c = +> over xStruct $ M.insertWith (++) p c +> +> xStructAdd :: Curve -> SweepState -> SweepState +> xStructAdd c = +> insertX (PointEvent $ cubicC0 $ +> view bezier c) [c] +> +> xStructRemove :: State SweepState (PointEvent, [Curve]) +> xStructRemove = zoom xStruct $ state M.deleteFindMin + +To compare curves vertically, take the the curve which starts the +rightmost, and see if it falls below or above the curve. If the first control points are coincident, test the +last control points instead. The curves in the Y-structure shouldn't +intersect (except in the endpoints), so these cases don't have to be +handled. To lookup a single point, I use a singular bezier curve. + +> instance Eq Curve where +> Curve c1 t1 ct1 == Curve c2 t2 ct2 = +> c1 == c2 && t1 == t2 && ct1 (ct2 t1) == t1 +> +> instance Ord Curve where +> compare (Curve c1@(CubicBezier p0 p1 p2 p3) tr1 _) +> (Curve c2@(CubicBezier q0 q1 q2 q3) tr2 _) +> | p0 == q0 = if +> | p3 == q3 -> +> -- compare the midpoint +> case (compVert (evalBezier c1 0.5) c2) of +> LT -> LT +> GT -> GT +> EQ -> +> -- otherwise arbitrary +> compare (tr1, PointEvent p1, PointEvent p2) +> (tr2, PointEvent q1, PointEvent q2) +> | pointX p3 < pointX q3 -> +> case (compVert p3 c2) of +> LT -> LT +> EQ -> LT +> GT -> GT +> | otherwise -> +> case compVert q3 c1 of +> LT -> GT +> EQ -> GT +> GT -> LT +> | pointX p0 < pointX q0 = +> case compVert q0 c1 of +> LT -> GT +> EQ -> LT +> GT -> LT +> | otherwise = +> case (compVert p0 c2) of +> LT -> LT +> EQ -> GT +> GT -> GT + +Compare a point with a curve. See if it falls below or above the hull +first. Otherwise find the point on the curve with the same +X-coordinate by solving a cubic equation. + +> compVert :: DPoint -> CubicBezier Double -> Ordering +> compVert p c +> | p == cubicC0 c || +> p == cubicC3 c = EQ +> | compH /= EQ = compH +> | otherwise = comparePointCurve p c +> where +> compH = compareHull p c + +=== Test if the point is above or below the curve {#comparePC} + +> comparePointCurve :: Point Double -> CubicBezier Double -> Ordering +> comparePointCurve (Point x1 y1) c1@(CubicBezier p0 p1 p2 p3) +> | pointX p0 == x1 && +> pointX p0 == pointX p1 && +> pointX p0 == pointX p2 && +> pointX p0 == pointX p3 = +> compare (pointY p0) y1 +> | otherwise = compare y2 y1 +> where +> t = findX x1 c1 $ +> maximum (map maxp [p0, p1, p2, p3])*1e-12 +> maxp (Point x y) = max (abs x) (abs y) +> y2 = pointY $ evalBezier c1 t + +=== Comparing against the hull {#hull} + +Compare a point against the convex hull of the bezier. `EQ` means the +point is inside the hull, `LT` below and `GT` above. I am currently +only testing against the control points, some testing needs to be done +to see what is faster. + +> belowLine :: DPoint -> DPoint -> DPoint -> Bool +> belowLine (Point px py) (Point lx ly) (Point rx ry) +> | lx == rx = True +> | (px >= lx && px <= rx) || +> (px <= lx && px >= rx) = py < midY +> | otherwise = True +> where midY = ly + (ry-ly) * (rx-lx) / (px-lx) +> +> aboveLine :: DPoint -> DPoint -> DPoint -> Bool +> aboveLine (Point px py) (Point lx ly) (Point rx ry) +> | lx == rx = True +> | (px >= lx && px <= rx) || +> (px <= lx && px >= rx) = py > midY +> | otherwise = True +> where midY = ly + (ry-ly) * (rx-lx) / (px-lx) +> +> compareHull :: DPoint -> CubicBezier Double -> Ordering +> compareHull p (CubicBezier c0 c1 c2 c3) +> | pointY p > pointY c0 && +> pointY p > pointY c1 && +> pointY p > pointY c2 && +> pointY p > pointY c3 = LT +> | pointY p < pointY c0 && +> pointY p < pointY c1 && +> pointY p < pointY c2 && +> pointY p < pointY c3 = GT +> | otherwise = EQ + +Preprocessing +------------- + +Since the algorithm assumes curves are increasing in the horizontal +direction they have to be preprocessed first. I split each curve +where the tangent is vertical. If the resulting subsegment is too +small however, I just adjust the control point to make the curve +vertical at the endpoint. + +I also do snaprounding to prevent points closer than the tolerance. + +> makeXStruct :: ((Int, Int) -> (Int, Int)) -> ((Int, Int) -> (Int, Int)) -> Double -> [CubicBezier Double] -> XStruct +> makeXStruct chTr chTrBack tol = +> M.fromListWith (++) . +> concatMap (toCurve . snapRoundBezier tol) . +> concatMap (splitVert tol) +> where toCurve c@(CubicBezier p0 _ _ p3) = +> case compare (pointX p0) (pointX p3) of +> LT -> [(PointEvent p0, [Curve c trOne chTr])] +> GT -> [(PointEvent p3, [Curve (reorient c) trOne chTrBack]), +> (PointEvent p0, [])] +> -- vertical curve +> EQ | pointY p0 > pointY p3 -> +> [(PointEvent p0, [Curve c trOne chTr])] +> | otherwise -> +> [(PointEvent p3, [Curve (reorient c) trOne chTrBack]), +> (PointEvent p0, [])] +> +> splitVert :: Double -> CubicBezier Double -> [CubicBezier Double] +> splitVert tol curve@(CubicBezier c0 c1 c2 c3) = +> uncurry splitBezierN $ +> adjustLast $ +> adjustFirst (curve, vert) +> where vert +> | pointX c0 == pointX c1 && +> pointX c0 == pointX c2 && +> pointX c0 == pointX c3 = [] +> | otherwise = +> sort $ bezierVert curve +> -- adjust control points to avoid small curve fragments +> -- near the endpoints +> adjustFirst (c@(CubicBezier p0 p1 p2 p3), t:ts) +> | vectorDistance p0 (evalBezier c t) < tol = +> (CubicBezier p0 (Point (pointX p0) (pointY p1)) p2 p3, +> ts) +> adjustFirst x = x +> adjustLast (c@(CubicBezier p0 p1 p2 p3), ts@(_:_)) +> | vectorDistance p3 (evalBezier c $ last ts) < tol = +> (CubicBezier p0 p1 (Point (pointX p3) (pointY p2)) p3, +> init ts) +> adjustLast x = x + +main loop +--------- + +For the main loop, we remove the leftmost point from the +X-structure, and do the following steps: + + 1. Split any curves which come near the current pointEvent. + + 2. Send all curves to the left of the sweepline to the output, after + filtering them based on the turning number. + + 3. For each curve starting at the point, split if it intersects with +the curve above or the curve below. Sort resulting curves vertically. +If there are no curves starting from point, test the curves above and +below instead. Adjust the turnRatios for each curve. + + 4. Insert the points in the Y structure. + + 5. Loop until the X-structure is empty + +> loopEvents :: ((Int, Int) -> Bool) -> Double -> SweepState -> SweepState +> loopEvents isInside tol sweep +> | M.null $ view xStruct sweep = sweep +> | otherwise = +> loopEvents isInside tol $! +> flip execState sweep $ do +> -- remove leftmost point from X structure +> (PointEvent p, curves) <- xStructRemove +> -- change focus, and remove curves ending at current +> -- pointevent from Y structure +> ending <- splitYStruct p +> -- split near curves +> (ending2, rightSubCurves) <- splitNearPoints p tol +> -- output curves to the left of the sweepline. +> modify $ filterOutput (ending ++ ending2) isInside +> let allCurves = rightSubCurves ++ curves +> if null allCurves +> -- split surrounding curves +> then splitSurround tol +> else do +> -- sort curves +> sorted <- splitAndOrder tol allCurves +> -- split curve above +> curves2 <- splitAbove sorted tol +> -- add curves to Y structure +> addMidCurves curves2 tol + +Send curves to output +--------------------- + +> outputPaths :: (M.Map PointEvent [CubicBezier Double]) -> [ClosedPath Double] +> outputPaths m +> | M.null m = [] +> | otherwise = outputNext m +> where +> lookupDelete p m = +> case M.lookup (PointEvent p) m of +> Nothing -> Nothing +> Just (x:xs) -> Just (x, m') +> where m' | null xs = M.delete (PointEvent p) m +> | otherwise = M.insert (PointEvent p) xs m +> _ -> error "outputPaths: empty list inside map." +> outputNext !m +> | M.null m = [] +> | otherwise = +> let ((PointEvent p0, (c0:cs)), m0) = +> M.deleteFindMin m +> m0' | null cs = m0 +> | otherwise = M.insert (PointEvent p0) cs m0 +> in go m0' c0 [] p0 +> go !m !next !prev !start +> | p == start = +> curvesToPath (reverse $ next:prev): +> outputNext m +> | otherwise = +> case lookupDelete p m of +> Nothing -> outputNext m +> Just (x, m') -> go m' x (next:prev) start +> where p = cubicC3 next +> +> curvesToPath :: [CubicBezier Double] -> ClosedPath Double +> curvesToPath = +> ClosedPath . +> map (\(CubicBezier p0 p1 p2 _) -> +> (p0, JoinCurve p1 p2)) + +Filter and output the given curves. The `isInside` function +determines the *insideness* of a give turnratio. For example for the +nonzero-rule, this would be `(> 0)`. This inserts the curve into the +output map. + +> filterOutput :: [Curve] -> ((Int, Int) -> Bool) -> SweepState -> SweepState +> filterOutput curves isInside sweep = +> foldl (flip $ outputCurve isInside) sweep curves +> +> outputCurve :: ((Int, Int) -> Bool) -> Curve -> SweepState -> SweepState +> outputCurve isInside (Curve c tr op) +> | isInside (op tr) /= isInside tr = +> let c' | isInside tr = reorient c +> | otherwise = c +> in over output (M.insertWith (++) (PointEvent $ cubicC0 c') [c']) +> | otherwise = id + +Test for intersections and split: +--------------------------------- + +Since the curves going out of the current pointEvent in the X-structure are +unordered, they need to be ordered first. First they are ordered by +first derivative. Since it's easier to compare two curves when they +don't overlap, remove overlap, and then sort again by comparing the +whole curve. + +To do this, I implemented a monadic insertion sort. First the curves are split +in the statemonad, then they are compared. + +> splitAndOrder :: Double -> [Curve] -> State SweepState [Curve] +> splitAndOrder tol curves = +> sortSplit tol $ +> sortBy compDeriv curves +> +> compDeriv :: Curve -> Curve -> Ordering +> compDeriv (Curve (CubicBezier p0 p1 _ _) _ _) +> (Curve (CubicBezier q0 q1 _ _) _ _) = +> compare (vectorCross (p1^-^p0) (q1^-^ q0)) 0 + +Insertion sort, by splitting and comparing. This should be efficient +enough, since ordering by derivative should mostly order the curves. + +> sortSplit :: Double -> [Curve] -> State SweepState [Curve] +> sortSplit _ [] = return [] +> sortSplit tol (x:xs) = +> insertM x tol =<< +> sortSplit tol xs +> +> insertM :: Curve -> Double -> [Curve] -> State SweepState [Curve] +> insertM x _ [] = return [x] +> insertM x tol (y:ys) = +> case curveOverlap x y tol of +> Just (c1, c2) -> do +> mapM (modify . xStructAdd) c2 +> insertM c1 tol ys +> Nothing -> do +> (x', y') <- splitM x y tol +> if x' < y' +> then return (x':y':ys) +> else (y':) <$> insertM x' tol ys +> +> splitM :: Curve -> Curve -> Double -> State SweepState (Curve, Curve) +> splitM x y tol = +> case splitMaybe x y tol of +> (Just (a, b), Just (c, d)) -> do +> modify $ insertX (PointEvent $ cubicC0 $ view bezier b) [b, d] +> return (a, c) +> (Nothing, Just (c, d)) -> do +> modify $ insertX (PointEvent $ cubicC0 $ view bezier d) [d] +> return (x, c) +> (Just (a, b), Nothing) -> do +> modify $ insertX (PointEvent $ cubicC0 $ view bezier b) [b] +> return (a, y) +> (Nothing, Nothing) -> +> return (x, y) + +Handle intersections of the first curve at point and the curve +above. Return the curves with updated turnratios. Some care is needed +when one of the curves is intersected at the endpoints, in order not +to create singular curves. + +> updateTurnRatio :: Curve -> Curve -> Curve +> updateTurnRatio (Curve _ tr chTr) = +> set turnRatio (chTr tr) +> +> propagateTurnRatio :: Curve -> [Curve] -> [Curve] +> propagateTurnRatio cAbove l = +> tail $ scanl updateTurnRatio cAbove l +> +> splitAbove :: [Curve] -> Double -> State SweepState [Curve] +> splitAbove [] _ = return [] +> splitAbove (c:cs) tol = do +> lStr <- use yStructLeft +> if S.null lStr +> then let c' = set turnRatio trOne c +> in return $ c':propagateTurnRatio c' cs +> else do +> let (cAbove, lStr') = S.deleteFindMax lStr +> case splitMaybe c cAbove tol of +> (Nothing, Nothing) -> +> return $ propagateTurnRatio cAbove $ c:cs +> (Just (c1, c2), Nothing) -> +> if cubicC3 (_bezier c1) == cubicC0 (_bezier cAbove) +> then do +> modify $ xStructAdd cAbove . xStructAdd c2 +> yStructLeft .= lStr' +> return $ propagateTurnRatio cAbove $ c1:cs +> else do +> modify $ xStructAdd c2 +> return $ propagateTurnRatio cAbove $ c1:cs +> (Nothing, Just (c3, c4)) -> +> if cubicC3 (_bezier c3) == cubicC0 (_bezier c) +> then error "curve intersecting pointevent" +> else do +> modify $ xStructAdd c4 +> yStructLeft .= S.insert c3 lStr' +> return $ propagateTurnRatio cAbove $ c:cs +> (Just (c1, c2), Just (c3, c4)) -> do +> modify $ xStructAdd c2 . xStructAdd c4 +> yStructLeft .= S.insert c3 lStr' +> return $ propagateTurnRatio cAbove $ c1:cs + +Split curves near the point. Return the curves starting from this point, and the index of the last split point + +> splitNearPoints :: DPoint -> Double -> State SweepState ([Curve], [Curve]) +> splitNearPoints p tol = do +> curves1 <- splitNearDir withAbove p tol +> curves2 <- splitNearDir withBelow p tol +> return (map fst curves1 ++ map fst curves2, +> map snd curves1 ++ map snd curves2) +> +> splitNearDir :: ((Curve -> Maybe (Curve, Double)) +> -> State SweepState (Maybe (Curve, Double))) +> -> DPoint -> Double +> -> State SweepState [(Curve, Curve)] +> splitNearDir dir p tol = do +> mbSplit <- dir $ \curve -> +> (,) curve <$> +> pointOnCurve tol p +> (view bezier curve) +> case mbSplit of +> Nothing -> return [] +> Just (curve, t) -> do +> let (c1, c2) = splitBezier (view bezier curve) t +> c1' = adjust curve $ adjustC3 p $ +> snapRound tol <$> c1 +> c2' = adjust curve $ adjustC0 p $ +> snapRound tol <$> c2 +> ((c1', c2'):) <$> splitNearDir dir p tol + +Add the sorted curves starting at point to the Y-structure, and test +last curve with curve below. + +> addMidCurves :: [Curve] -> Double -> State SweepState () +> addMidCurves [] _ = return () +> addMidCurves [c] tol = +> splitBelow c tol +> addMidCurves (c:cs) tol = do +> yStructLeft %= S.insert c +> addMidCurves cs tol +> +> splitBelow :: Curve -> Double -> State SweepState () +> splitBelow c tol = do +> rStr <- use yStructRight +> let (cBelow, rStr') = S.deleteFindMin rStr +> if S.null rStr +> then yStructLeft %= S.insert c +> else +> case splitMaybe c cBelow tol of +> (Nothing, Nothing) -> +> yStructLeft %= S.insert c +> (Nothing, Just (c3, c4)) -> +> if cubicC3 (_bezier c3) == cubicC0 (_bezier c) +> then error "internal error: splitBelow: curve starting in future" +> else do +> modify $ xStructAdd c4 +> yStructLeft %= S.insert c . S.insert c3 +> yStructRight .= rStr' +> (Just (c1, c2), Nothing) -> +> if cubicC3 (_bezier c1) == cubicC0 (_bezier cBelow) +> then error "internal error: splitBelow: curve intersecting pointevent." +> else do +> modify $ xStructAdd c2 +> yStructLeft %= S.insert c1 +> (Just (c1, c2), Just (c3, c4)) -> do +> modify $ xStructAdd c2 . xStructAdd c4 +> yStructLeft %= S.insert c1 . S.insert c3 +> yStructRight .= rStr' + +If no curves start from the point, we have to check if the surrounding +curves overlap. + +> splitSurround :: Double -> State SweepState () +> splitSurround tol = do +> lStr <- use yStructLeft +> rStr <- use yStructRight +> if S.null lStr || S.null rStr +> then return () +> else do +> let (cAbove, lStr') = S.deleteFindMax lStr +> (cBelow, rStr') = S.deleteFindMin rStr +> case splitMaybe cAbove cBelow tol of +> (Just (c1, c2), Just (c3, c4)) -> do +> modify $ xStructAdd c2 . +> xStructAdd c4 +> yStructLeft .= S.insert c1 lStr' +> yStructRight .= S.insert c3 rStr' +> (Just (c1, c2), Nothing) -> do +> modify $ xStructAdd c2 +> yStructLeft .= S.insert c1 lStr' +> (Nothing, Just (c1, c2)) -> do +> modify $ xStructAdd c2 +> yStructRight .= S.insert c1 rStr' +> (Nothing, Nothing) -> +> return () + + +=== Find curve intersections + +Test if both curves intersect. Split one or both of the curves when +they intersect. Also snapround each point, and make sure the point +of overlap is the same in both curves. + +> splitMaybe :: Curve -> Curve -> Double -> +> (Maybe (Curve, Curve), +> Maybe (Curve, Curve)) +> splitMaybe c1 c2 tol = +> (adjustSplit c1 <$> fst n, +> adjustSplit c2 <$> snd n) +> where +> n = nextIntersection b1 b2 tol $ +> bezierIntersection b1 b2 pTol +> pTol = min (bezierParamTolerance b1 tol) +> (bezierParamTolerance b2 tol) +> b1 = view bezier c1 +> b2 = view bezier c2 +> +> adjustSplit :: Curve -> (CubicBezier Double, CubicBezier Double) -> (Curve, Curve) +> adjustSplit curve (b1, b2) = +> (set bezier b1 curve, +> set bezier b2 curve) +> +> adjust :: Curve -> CubicBezier Double -> Curve +> adjust curve curve2 = set bezier curve2 curve +> +> snapRoundBezier :: Double -> CubicBezier Double -> CubicBezier Double +> snapRoundBezier tol = fmap (snapRound tol) +> + +Given a list of intersection parameters, split at the next +intersection, but don't split at the first or last control point, or +when the two curves are (nearly) coincident. Note that list of +intersections is read lazily, in order not to evaluate more +intersections that necessary. + +> nextIntersection :: CubicBezier Double -> CubicBezier Double -> Double -> [(Double, Double)] +> -> (Maybe (CubicBezier Double, CubicBezier Double), +> Maybe (CubicBezier Double, CubicBezier Double)) +> nextIntersection _ _ _ [] = (Nothing, Nothing) +> nextIntersection b1@(CubicBezier p0 _ _ p3) b2@(CubicBezier q0 _ _ q3) tol ((t1, t2): ts) +> | atStart1 && atStart2 = +> nextIntersection b1 b2 tol ts +> | atStart1 = +> (Nothing, +> Just (adjustC3 p0 $ snapRoundBezier tol b2l, +> adjustC0 p0 $ snapRoundBezier tol b2r)) +> | atStart2 = +> (Just (adjustC3 q0 $ snapRoundBezier tol b1l, +> adjustC0 q0 $ snapRoundBezier tol b1r), +> Nothing) +> | atEnd1 && atEnd2 = (Nothing, Nothing) +> | atEnd1 = +> (Nothing, +> Just (adjustC3 p3 $ snapRoundBezier tol b2l, +> adjustC0 p3 $ snapRoundBezier tol b2r)) +> | atEnd2 = +> (Just (adjustC3 q3 $ snapRoundBezier tol b1l, +> adjustC0 q3 $ snapRoundBezier tol b1r), +> Nothing) +> | bezierEqual b1l b2l tol = +> nextIntersection b1 b2 tol ts +> | otherwise = +> let pMid = snapRound tol <$> cubicC3 b1l +> in (Just (snapRoundBezier tol b1l, +> snapRoundBezier tol b1r), +> Just (adjustC3 pMid $ snapRoundBezier tol b2l, +> adjustC0 pMid $ snapRoundBezier tol b2r)) +> where +> x1 = evalBezier b1 t1 +> x2 = evalBezier b2 t2 +> atStart1 = vectorDistance (cubicC0 b1) x1 < tol +> atStart2 = vectorDistance (cubicC0 b2) x2 < tol +> atEnd1 = vectorDistance (cubicC3 b1) x1 < tol +> atEnd2 = vectorDistance (cubicC3 b2) x2 < tol +> (b1l, b1r) = splitBezier b1 t1 +> (b2l, b2r) = splitBezier b2 t2 +> +> adjustC0 :: Point a -> CubicBezier a -> CubicBezier a +> adjustC0 p (CubicBezier _ p1 p2 p3) = CubicBezier p p1 p2 p3 +> +> adjustC3 :: Point a -> CubicBezier a -> CubicBezier a +> adjustC3 p (CubicBezier p0 p1 p2 _) = CubicBezier p0 p1 p2 p + +=== Check if curves overlap. + +If the curves overlap, combine the overlapping part into one curve. +To compare the curves, I first split the longest curve so that the +velocities in the first control point match, then compare those curves +for equality. + +> curveOverlap :: Curve -> Curve -> Double +> -> Maybe (Curve, Maybe Curve) +> curveOverlap c1 c2 tol +> -- starting in the same point +> | p0 /= q0 = Nothing +> | colinear (view bezier c1) tol = if +> | not $ colinear (view bezier c2) tol -> +> Nothing +> | vectorDistance (p3^-^p0) +> ((q3^-^q0) ^* (d1/d2)) > tol -> +> Nothing +> | p3 == q3 -> +> Just (combineCurves c2 c1, +> Nothing) +> | d1 > d2 -> +> Just (combineCurves c2 c1, +> Just $ adjust c1 $ +> CubicBezier q3 +> (snapRound tol <$> interpolateVector q3 p3 (1/3)) +> (snapRound tol <$> interpolateVector q3 p3 (2/3)) +> p3) +> | otherwise -> +> Just (combineCurves c1 c2, +> Just $ adjust c2 $ +> CubicBezier p3 +> (snapRound tol <$> interpolateVector p3 q3 (1/3)) +> (snapRound tol <$> interpolateVector p3 q3 (2/3)) +> q3) +> -- equalize velocities, and compare +> | v1 == 0 || +> v2 == 0 = Nothing +> | v1 > v2 = if bezierEqual b2 b1l tol +> then Just (combineCurves c2 c1, +> if checkEmpty b1r tol +> then Nothing +> else Just $ adjust c1 $ +> adjustC0 (cubicC3 b2) $ +> snapRoundBezier tol b1r) +> else Nothing +> +> | otherwise = +> if bezierEqual b1 b2l tol +> then Just (combineCurves c1 c2, +> if checkEmpty b2r tol +> then Nothing +> else Just $ adjust c2 $ +> adjustC0 (cubicC3 b1) $ +> snapRoundBezier tol b2r) +> else Nothing +> where +> (b1l, b1r) = splitBezier b1 (v2/v1) +> (b2l, b2r) = splitBezier b2 (v1/v2) +> b1@(CubicBezier p0 p1 _ p3) = view bezier c1 +> b2@(CubicBezier q0 q1 _ q3) = view bezier c2 +> d1 = vectorDistance p0 p3 +> d2 = vectorDistance q0 q3 +> v1 = vectorDistance p0 p1 +> v2 = vectorDistance q0 q1 +> +> checkEmpty :: CubicBezier Double -> Double -> Bool +> checkEmpty (CubicBezier p0 p1 p2 p3) tol = +> p0 == p3 && +> vectorDistance p0 p1 < tol && +> vectorDistance p0 p2 < tol + +Curves can be combined if they are equal, just by composing their +changeTurn functions. + +> combineCurves :: Curve -> Curve -> Curve +> combineCurves c1 c2 = +> over changeTurn (view changeTurn c2 .) c1 + +=== Snaprounding + +> snapRound :: Double -> Double -> Double +> snapRound tol v = +> fromInteger (round (v/tol)) * tol + +=== Test if the point is on the curve (within tolerance) {#oncurve} + +> pointOnCurve :: Double -> DPoint -> CubicBezier Double -> Maybe Double +> pointOnCurve tol p c1 +> | (t:_) <- +> closest c1 p tol, +> p2 <- evalBezier c1 t, +> vectorDistance p p2 < tol = Just t +> | otherwise = Nothing + +=== Testing beziers for approximate equality {#eq} + +If the control points of two bezier curves are within a distance `eps` +from each other, then both curves will all so be at least within +distance `eps` from each other. This can be proven easily: +subtracting both curves gives the distance curve. Since each control +point of this curve lies within a circle of radius `eps`, by the +convex hull property, the curve will also be inside the circle, so the +distances between each point will never exceed `eps`. + +> bezierEqual :: CubicBezier Double -> CubicBezier Double -> Double -> Bool +> bezierEqual cb1@(CubicBezier a0 a1 a2 a3) cb2@(CubicBezier b0 b1 b2 b3) tol +> -- controlpoints equal within tol +> | vectorDistance a1 b1 < tol && +> vectorDistance a2 b2 < tol && +> vectorDistance a3 b3 < tol && +> vectorDistance a0 b0 < tol = True +> -- compare if both are colinear and close together +> | dist < tol && +> colinear cb1 ((tol-dist)/2) && +> colinear cb2 ((tol-dist)/2) = True +> | otherwise = False +> where dist = max (abs $ ld b0) (abs $ ld b3) +> ld = lineDistance (Line a0 a3) + +=== Finding the on curve point at the X coordinate {#findx} + +Solve a cubic equation to find the X coordinate. This should be +converted to a closed form solver for efficiency. + +> findX :: Double -> CubicBezier Double -> Double -> Double +> findX x c@(CubicBezier p0 p1 p2 p3) eps = +> head $ bezierFindRoot bez 0 1 $ +> bezierParamTolerance c (eps/10) +> where bez = listToBernstein +> (map pointX [p0, p1, p2, p3]) ~- +> listToBernstein [x, x, x, x] + +Higher level functions +---------------------- + +> fillFunction :: FillRule -> Int -> Bool +> fillFunction NonZero = (>0) +> fillFunction EvenOdd = odd + +> -- | Union of paths, removing overlap and rounding to the given +> -- tolerance. +> union :: [ClosedPath Double] -- ^ Paths +> -> FillRule -- ^ input fillrule +> -> Double -- ^ Tolerance +> -> [ClosedPath Double] +> union p fill tol = +> outputPaths out +> where +> out = view output $ +> loopEvents (fillFunction fill . fst) tol sweep +> sweep = SweepState M.empty S.empty S.empty xStr +> xStr = makeXStruct (over _1 $ subtract 1) (over _1 (+1)) tol $ +> concatMap closedPathCurves p +> +> -- | combine paths using the given boolean operation +> boolPathOp :: (Bool -> Bool -> Bool) -- ^ operation +> -> [ClosedPath Double] -- ^ first path (merged with union) +> -> [ClosedPath Double] -- ^ second path (merged with union) +> -> FillRule -- ^ input fillrule +> -> Double -- ^ tolerance +> -> [ClosedPath Double] +> boolPathOp op p1 p2 fill tol = +> outputPaths $ view output $ +> loopEvents isInside tol sweep +> where +> isInside (a, b) = fillFunction fill a `op` +> fillFunction fill b +> sweep = SweepState M.empty S.empty S.empty xStr +> xStr = M.unionWith (++) +> (makeXStruct +> (over _1 (subtract 1)) +> (over _1 (+1)) tol $ +> concatMap closedPathCurves p1) +> (makeXStruct +> (over _2 (subtract 1)) +> (over _2 (+1)) tol $ +> concatMap closedPathCurves p2) +> +> intersection, difference, exclusion :: +> [ClosedPath Double] -> [ClosedPath Double] -> +> FillRule -> Double -> [ClosedPath Double] +> +> -- | path intersection +> intersection = boolPathOp (&&) +> +> -- | path difference +> difference = boolPathOp (\a b -> a && not b) +> +> -- | path exclusion +> exclusion = boolPathOp (\a b -> if a then not b else b) diff --git a/Math/BernsteinPoly.hs b/Math/BernsteinPoly.hs --- a/Math/BernsteinPoly.hs +++ b/Math/BernsteinPoly.hs @@ -1,140 +1,221 @@ -{-# LANGUAGE BangPatterns #-} +{-# LANGUAGE BangPatterns, ViewPatterns #-} +-- | Algebra on polynomials in the Bernstein form. It is based on the +-- paper /Algebraic manipulation in the Bernstein form made simple via +-- convolutions/ by J. Sanchez-Reyes. It's an efficient +-- implementation using the scaled basis, and using ghc rewrite rules +-- to eliminate intermediate polynomials. + module Math.BernsteinPoly - (BernsteinPoly(..), bernsteinSubsegment, listToBernstein, zeroPoly, (~*), (*~), (~+), - (~-), degreeElevate, bernsteinSplit, bernsteinEval, - bernsteinEvalDerivs, bernsteinDeriv) + (BernsteinPoly(..), bernsteinSubsegment, listToBernstein, zeroPoly, + (~*), (*~), (~+), (~-), degreeElevate, bernsteinSplit, bernsteinEval, + bernsteinEvalDeriv, binCoeff, convolve, bernsteinEvalDerivs, bernsteinDeriv) where - -import Data.List +import Data.Vector.Unboxed as V +import qualified Data.Vector as B -data BernsteinPoly = BernsteinPoly { - bernsteinDegree :: !Int, - bernsteinCoeffs :: ![Double] } +data BernsteinPoly a = BernsteinPoly { + bernsteinCoeffs :: V.Vector a} deriving Show +data ScaledPoly a = ScaledPoly { + scaledCoeffs :: V.Vector a } + deriving Show infixl 7 ~*, *~ infixl 6 ~+, ~- +{-# RULES "toScaled/fromScaled" forall a. toScaled (fromScaled a) = a; + "fromScaled/toScaled" forall a. fromScaled (toScaled a) = a; #-} + +toScaled :: (Unbox a, Num a) => BernsteinPoly a -> ScaledPoly a +toScaled (BernsteinPoly v) = + ScaledPoly $ + V.zipWith (*) v $ binCoeff $ V.length v - 1 +{-# NOINLINE[2] toScaled #-} + +fromScaled :: (Unbox a, Fractional a) => ScaledPoly a -> BernsteinPoly a +fromScaled (ScaledPoly v) = + BernsteinPoly $ + V.zipWith (/) v $ binCoeff $ V.length v - 1 +{-# NOINLINE[2] fromScaled #-} + -- | Create a bernstein polynomail from a list of coëfficients. -listToBernstein :: [Double] -> BernsteinPoly +listToBernstein :: (Unbox a, Num a) => [a] -> BernsteinPoly a listToBernstein [] = zeroPoly -listToBernstein l = BernsteinPoly (length l - 1) l +listToBernstein l = BernsteinPoly $ V.fromList l +{-# INLINE listToBernstein #-} -- | The constant zero. -zeroPoly :: BernsteinPoly -zeroPoly = BernsteinPoly 0 [0] +zeroPoly :: (Num a, Unbox a) => BernsteinPoly a +zeroPoly = BernsteinPoly $ V.fromList [0] +{-# SPECIALIZE zeroPoly :: BernsteinPoly Double #-} -- | Return the subsegment between the two parameters. -bernsteinSubsegment :: BernsteinPoly -> Double -> Double -> BernsteinPoly +bernsteinSubsegment :: (Unbox a, Ord a, Fractional a) => + BernsteinPoly a -> a -> a -> BernsteinPoly a bernsteinSubsegment b t1 t2 | t1 > t2 = bernsteinSubsegment b t2 t1 | otherwise = snd $ flip bernsteinSplit (t1/t2) $ fst $ bernsteinSplit b t2 +{-# INLINE bernsteinSubsegment #-} --- multiply two bezier curves --- control point i from the product of beziers P * Q --- is sum (P_j * Q_k) where j + k = i+1 +-- | Calculate the convolution of two vectors. +convolve :: (Unbox a, Num a) => Vector a -> Vector a -> Vector a +convolve a b = + V.map (\i -> V.sum $ + V.zipWith (*) a $ + V.reverse $ + V.unsafeTake i b) + (V.enumFromN 1 $ V.length b) + V.++ V.map (\i -> V.sum $ + V.zipWith (*) + (V.unsafeDrop i a) + (V.reverse b)) + (V.enumFromN 1 $ V.length a-1) +{-# SPECIALIZE convolve :: Vector Double -> Vector Double -> Vector Double #-} --- | Multiply two bernstein polynomials. The final degree --- will be the sum of either degrees. This operation takes O((n+m)^2) --- with n and m the degree of the beziers. +-- | Multiply two bernstein polynomials using convolution. The final +-- degree will be the sum of either degrees. This operation takes +-- O((n+m)^2) with n and m the degree of the beziers. Note that +-- convolution can be done in O(n log n) using the FFT, which may be +-- faster for large polynomials. -(~*) :: BernsteinPoly -> BernsteinPoly -> BernsteinPoly -(BernsteinPoly la a) ~* (BernsteinPoly lb b) = - BernsteinPoly (la+lb) $ - zipWith (flip (/)) (binCoeff (la + lb)) $ - init $ map sum $ - zipWith (zipWith (*)) (repeat a') (down b') ++ - zipWith (zipWith (*)) (tail $ tails a') (repeat $ reverse b') - where down l = tail $ scanl (flip (:)) [] l -- [[1], [2, 1], [3, 2, 1], ... - a' = zipWith (*) a (binCoeff la) - b' = zipWith (*) b (binCoeff lb) +(~*) :: (Unbox a, Fractional a) => + BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a +(toScaled -> a) ~* (toScaled -> b) = + fromScaled $ mulScaled a b +{-# INLINE (~*) #-} +mulScaled :: (Unbox a, Num a) => ScaledPoly a -> ScaledPoly a -> ScaledPoly a +mulScaled (ScaledPoly a) (ScaledPoly b) = + ScaledPoly $ convolve a b +{-# INLINE mulScaled #-} --- find the binomial coefficients of degree n. -binCoeff :: Int -> [Double] -binCoeff n = map fromIntegral $ - scanl (\x m -> x * (n-m+1) `quot` m) 1 [1..n] +-- | Give the binomial coefficients of degree n. +binCoeff :: (Num a, Unbox a) => Int -> V.Vector a +binCoeff n = V.map fromIntegral $ + V.scanl (\x m -> x * (n-m+1) `quot` m) + 1 (V.enumFromN 1 n) +{-# INLINE binCoeff #-} --- | Degree elevate a bernstein polynomail a number of times. -degreeElevate :: BernsteinPoly -> Int -> BernsteinPoly -degreeElevate b 0 = b -degreeElevate (BernsteinPoly lp p) times = - degreeElevate (BernsteinPoly (lp+1) (head p:inner p 1)) (times-1) - where - n = fromIntegral lp - inner [] _ = error "empty bernstein coefficients" - inner [a] _ = [a] - inner (a:b:rest) i = - (i*a/(n+1) + b*(1 - i/(n+1))) - : inner (b:rest) (i+1) +-- | Degree elevate a bernstein polynomial a number of times. +degreeElevateScaled :: (Unbox a, Num a) + => ScaledPoly a -> Int -> ScaledPoly a +degreeElevateScaled b@(ScaledPoly p) times + | times <= 0 = b + | otherwise = ScaledPoly $ convolve (binCoeff times) p +{-# SPECIALIZE degreeElevateScaled :: ScaledPoly Double -> + Int -> ScaledPoly Double #-} +degreeElevate :: (Unbox a, Fractional a) + => BernsteinPoly a -> Int -> BernsteinPoly a +degreeElevate (toScaled -> b) times = + fromScaled (degreeElevateScaled b times) +{-# INLINE degreeElevate #-} --- | Evaluate the bernstein polynomial. -bernsteinEval :: BernsteinPoly -> Double -> Double -bernsteinEval (BernsteinPoly _ []) _ = error "illegal bernstein polynomial" -bernsteinEval (BernsteinPoly _ [b]) _ = b -bernsteinEval (BernsteinPoly lp (b':bs)) t = go t n (b'*u) 2 bs +-- | Evaluate the bernstein polynomial using the horner rule adapted +-- for bernstein polynomials. + +bernsteinEval :: (Unbox a, Fractional a) + => BernsteinPoly a -> a -> a +bernsteinEval (BernsteinPoly v) _ + | V.length v == 0 = 0 +bernsteinEval (BernsteinPoly v) _ + | V.length v == 1 = V.unsafeHead v +bernsteinEval (BernsteinPoly v) t = + go t (fromIntegral n) (V.unsafeIndex v 0 * u) 1 where u = 1-t - n = fromIntegral lp - go !tn !bc !tmp _ [b] = tmp + tn*bc*b - go !tn !bc !tmp !i (b:rest) = - go (tn*t) -- tn - (bc*(n-i+1)/i) -- bc - ((tmp + tn*bc*b)*u) -- tmp - (i+1) -- i - rest - go _ _ _ _ [] = error "impossible" - + n = fromIntegral $ V.length v - 1 + go !tn !bc !tmp !i + | i == n = tmp + tn*V.unsafeIndex v n + | otherwise = + go (tn*t) -- tn + (bc*fromIntegral (n-i)/(fromIntegral i + 1)) -- bc + ((tmp + tn*bc*V.unsafeIndex v i)*u) -- tmp + (i+1) -- i +{-# SPECIALIZE bernsteinEval :: BernsteinPoly Double -> Double -> Double #-} + +-- | Evaluate the bernstein polynomial and first derivative +bernsteinEvalDeriv :: (Unbox t, Fractional t) => BernsteinPoly t -> t -> (t,t) +bernsteinEvalDeriv b@(BernsteinPoly v) t + | V.length v <= 1 = (V.unsafeHead v, 0) + | otherwise = (bernsteinEval b t, bernsteinEval (bernsteinDeriv b) t) +{-# INLINE bernsteinEvalDeriv #-} + -- | Evaluate the bernstein polynomial and its derivatives. -bernsteinEvalDerivs :: BernsteinPoly -> Double -> [Double] -bernsteinEvalDerivs b t - | bernsteinDegree b == 0 = [bernsteinEval b t] +bernsteinEvalDerivs :: (Unbox t, Fractional t) => BernsteinPoly t -> t -> [t] +bernsteinEvalDerivs b@(BernsteinPoly v) t + | V.length v <= 1 = [V.unsafeHead v, 0] | otherwise = bernsteinEval b t : bernsteinEvalDerivs (bernsteinDeriv b) t +{-# INLINE bernsteinEvalDerivs #-} -- | Find the derivative of a bernstein polynomial. -bernsteinDeriv :: BernsteinPoly -> BernsteinPoly -bernsteinDeriv (BernsteinPoly 0 _) = zeroPoly -bernsteinDeriv (BernsteinPoly lp p) = - BernsteinPoly (lp-1) $ - map (* fromIntegral lp) $ zipWith (-) (tail p) p +bernsteinDeriv :: (Unbox a, Num a) => BernsteinPoly a -> BernsteinPoly a +bernsteinDeriv (BernsteinPoly v) + | V.length v == 0 = zeroPoly +bernsteinDeriv (BernsteinPoly v) = + BernsteinPoly $ + V.map (* fromIntegral (V.length v - 1)) $ + V.zipWith (-) (V.tail v) v +{-# SPECIALIZE bernsteinDeriv :: BernsteinPoly Double -> + BernsteinPoly Double #-} -- | Split a bernstein polynomial -bernsteinSplit :: BernsteinPoly -> Double -> (BernsteinPoly, BernsteinPoly) -bernsteinSplit (BernsteinPoly lp p) t = - (BernsteinPoly lp $ map head controls, - BernsteinPoly lp $ reverse $ map last controls) +bernsteinSplit :: (Unbox a, Num a) => + BernsteinPoly a -> a -> (BernsteinPoly a, BernsteinPoly a) +bernsteinSplit (BernsteinPoly v) t = + (BernsteinPoly $ convert $ + B.map V.head interpVecs, + BernsteinPoly $ V.reverse $ convert $ + B.map V.last $ convert interpVecs) where interp a b = (1-t)*a + t*b - terp [_] = [] - terp l = let ctrs = zipWith interp l (tail l) - in ctrs : terp ctrs - controls = p:terp p + interpVecs = B.iterateN (V.length v) interpVec v + interpVec v2 = V.zipWith interp v2 (V.tail v2) +{-# SPECIALIZE bernsteinSplit :: BernsteinPoly Double -> Double -> + (BernsteinPoly Double, BernsteinPoly Double) #-} --- | Sum two bernstein polynomials. The final degree will be the maximum of either --- degrees. -(~+) :: BernsteinPoly -> BernsteinPoly -> BernsteinPoly -ba@(BernsteinPoly la a) ~+ bb@(BernsteinPoly lb b) - | la < lb = BernsteinPoly lb $ - zipWith (+) (bernsteinCoeffs $ degreeElevate ba $ lb-la) b - | la > lb = BernsteinPoly la $ - zipWith (+) a (bernsteinCoeffs $ degreeElevate bb $ la-lb) - | otherwise = BernsteinPoly la $ - zipWith (+) a b +addScaled :: (Unbox a, Num a) => ScaledPoly a -> ScaledPoly a -> ScaledPoly a +addScaled ba@(ScaledPoly a) bb@(ScaledPoly b) + | la < lb = ScaledPoly $ + V.zipWith (+) (scaledCoeffs $ degreeElevateScaled ba $ lb-la) b + | la > lb = ScaledPoly $ + V.zipWith (+) a (scaledCoeffs $ degreeElevateScaled bb $ la-lb) + | otherwise = ScaledPoly $ V.zipWith (+) a b + where la = V.length a + lb = V.length b +{-# SPECIALIZE addScaled :: ScaledPoly Double -> ScaledPoly Double -> + ScaledPoly Double #-} --- | Subtract two bernstein polynomials. The final degree will be the maximum of either --- degrees. -(~-) :: BernsteinPoly -> BernsteinPoly -> BernsteinPoly -ba@(BernsteinPoly la a) ~- bb@(BernsteinPoly lb b) - | la < lb = BernsteinPoly lb $ - zipWith (-) (bernsteinCoeffs $ degreeElevate ba (lb-la)) b - | la > lb = BernsteinPoly la $ - zipWith (-) a (bernsteinCoeffs $ degreeElevate bb (la-lb)) - | otherwise = BernsteinPoly la $ - zipWith (-) a b +-- | Sum two bernstein polynomials. The final degree will be the +-- maximum of either degrees. +(~+) :: (Unbox a, Fractional a) => + BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a +(toScaled -> a) ~+ (toScaled -> b) = fromScaled $ addScaled a b +{-# INLINE (~+) #-} +subScaled :: (Unbox a, Num a) => ScaledPoly a -> ScaledPoly a -> ScaledPoly a +subScaled ba@(ScaledPoly a) bb@(ScaledPoly b) + | la < lb = ScaledPoly $ + V.zipWith (-) (scaledCoeffs $ degreeElevateScaled ba $ lb-la) b + | la > lb = ScaledPoly $ + V.zipWith (-) a (scaledCoeffs $ degreeElevateScaled bb $ la-lb) + | otherwise = ScaledPoly $ V.zipWith (-) a b + where la = V.length a + lb = V.length b +{-# SPECIALIZE subScaled :: ScaledPoly Double -> ScaledPoly Double -> + ScaledPoly Double #-} + +-- | Subtract two bernstein polynomials. The final degree will be the +-- maximum of either degrees. +(~-) :: (Unbox a, Fractional a) => + BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a + +(toScaled -> a) ~- (toScaled -> b) = fromScaled $ subScaled a b +{-# INLINE (~-) #-} + -- | Scale a bernstein polynomial by a constant. -(*~) :: Double -> BernsteinPoly -> BernsteinPoly -a *~ (BernsteinPoly lb b) = BernsteinPoly lb (map (*a) b) +(*~) :: (Unbox a, Num a) => a -> BernsteinPoly a -> BernsteinPoly a +a *~ (BernsteinPoly v) = BernsteinPoly (V.map (*a) v) +{-# INLINE (*~) #-} diff --git a/cubicbezier.cabal b/cubicbezier.cabal --- a/cubicbezier.cabal +++ b/cubicbezier.cabal @@ -1,5 +1,5 @@ Name: cubicbezier -Version: 0.3.0 +Version: 0.4.0.1 Synopsis: Efficient manipulating of 2D cubic bezier curves. Category: Graphics, Geometry, Typography Copyright: Kristof Bastiaensen (2014) @@ -11,8 +11,7 @@ Bug-Reports: https://github.com/kuribas/cubicbezier/issues Build-type: Simple Cabal-version: >=1.8 -Description: This library supports efficient manipulating of 2D cubic bezier curves. The original goal - is to support typography, but it is useful for general graphics. Supported features are: +Description: This library supports efficient manipulating of 2D cubic bezier curves, for use in graphics or typography. Supported features are: . Evaluating bezier curves and derivatives, affine transformations on bezier curves, arclength and inverse arclength, intersections between two curves, intersection between a curve and a line, curvature and radius of curvature, finding tangents parallel to a vector, finding inflection points and cusps. . @@ -27,7 +26,8 @@ Library Ghc-options: -Wall - Build-depends: base >= 3 && < 5, containers > 0.4, integration >= 0.1.1 + Build-depends: base >= 3 && < 5, containers >= 0.5.3, integration >= 0.1.1, vector >= 0.10, + matrices >= 0.4.1, microlens >= 0.1.2, microlens-th >= 0.1.2, microlens-mtl >= 0.1.2, mtl >= 2.1.1 Exposed-Modules: Geom2D Geom2D.CubicBezier @@ -35,10 +35,10 @@ Geom2D.CubicBezier.Approximate Geom2D.CubicBezier.Outline Geom2D.CubicBezier.Curvature + Geom2D.CubicBezier.Overlap Geom2D.CubicBezier.Intersection Geom2D.CubicBezier.MetaPath Math.BernsteinPoly - Other-Modules: Geom2D.CubicBezier.Numeric test-suite test diff --git a/tests/test.hs b/tests/test.hs --- a/tests/test.hs +++ b/tests/test.hs @@ -1,5 +1,3 @@ -{-# Language ViewPatterns #-} - import Test.Tasty import Test.Tasty.HUnit import Geom2D.CubicBezier @@ -7,9 +5,11 @@ import Text.Parsec import Text.Parsec.String import Text.Parsec.Error +import MPTest +import NumTest tests :: TestTree -tests = testGroup "Tests" [unitTests] +tests = testGroup "Tests" [mfTests, numTests] num :: Parser Double num = @@ -186,8 +186,8 @@ -- These tests were created by running mf, typing expr after the -- prompt, and entering the metapaths. -unitTests :: TestTree -unitTests = testGroup "Metafont" [ +mfTests :: TestTree +mfTests = testGroup "Metafont" [ testPath "(0,0)..(4,3)" "(0,0)..controls (1.33333,1) and (2.66667,2) ..(4,3)",