module Geom2D.CubicBezier.Basic
(CubicBezier (..), QuadBezier (..), AnyBezier (..), GenericBezier(..),
PathJoin (..), ClosedPath(..), OpenPath (..), AffineTransform (..), anyToCubic, anyToQuad,
openPathCurves, closedPathCurves, curvesToOpen, curvesToClosed,
consOpenPath, consClosedPath, openClosedPath, closeOpenPath,
bezierParam, bezierParamTolerance, reorient, bezierToBernstein,
evalBezierDerivs, evalBezier, evalBezierDeriv, findBezierTangent, quadToCubic,
bezierHoriz, bezierVert, findBezierInflection, findBezierCusp,
bezierArc, arcLength, arcLengthParam, splitBezier, bezierSubsegment,
splitBezierN, colinear, closest, findX)
where
import Geom2D
import Geom2D.CubicBezier.Numeric
import Math.BernsteinPoly
import Numeric.Integration.TanhSinh
import Data.Monoid ()
import Data.List (minimumBy)
import Data.Function (on)
import Data.VectorSpace
import Debug.Trace
import qualified Data.Vector.Unboxed as V
import qualified Data.Vector.Unboxed.Mutable as MV
data CubicBezier a = CubicBezier {
cubicC0 :: !(Point a),
cubicC1 :: !(Point a),
cubicC2 :: !(Point a),
cubicC3 :: !(Point a)}
deriving (Eq, Show, Functor, Foldable, Traversable)
data QuadBezier a = QuadBezier {
quadC0 :: !(Point a),
quadC1 :: !(Point a),
quadC2 :: !(Point a)}
deriving (Eq, Show, Functor, Foldable, Traversable)
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, Foldable, Traversable)
data OpenPath a = OpenPath [(Point a, PathJoin a)] (Point a)
deriving (Show, Functor, Foldable, Traversable)
data ClosedPath a = ClosedPath [(Point a, PathJoin a)]
deriving (Show, Functor, Foldable, Traversable)
instance Monoid (OpenPath a) where
mempty = OpenPath [] (error "empty path")
mappend p1 (OpenPath [] _) = p1
mappend (OpenPath [] _) p2 = p2
mappend (OpenPath joins1 _) (OpenPath joins2 p) =
OpenPath (joins1 ++ joins2) p
instance (Num a) => AffineTransform (PathJoin a) a where
transform _ JoinLine = JoinLine
transform t (JoinCurve p q) = JoinCurve (transform t p) (transform t q)
instance (Num a) => AffineTransform (OpenPath a) a where
transform t (OpenPath s p) = OpenPath (map (transformSeg t) s) (transform t p)
transformSeg :: (Num a) => Transform a -> (Point a, PathJoin a) -> (Point a, PathJoin a)
transformSeg t (p, jn) = (transform t p, transform t jn)
instance (Num a) => AffineTransform (ClosedPath a) a where
transform t (ClosedPath s) = ClosedPath (map (transformSeg t) s)
instance (Num a) => AffineTransform (CubicBezier a) a where
transform t (CubicBezier c0 c1 c2 c3) =
CubicBezier (transform t c0) (transform t c1) (transform t c2) (transform t c3)
instance (Num a) => AffineTransform (QuadBezier a) a where
transform t (QuadBezier c0 c1 c2) =
QuadBezier (transform t c0) (transform t c1) (transform t c2)
consOpenPath :: Point a -> PathJoin a -> OpenPath a -> OpenPath a
consOpenPath p join (OpenPath joins q) =
OpenPath ((p, join):joins) q
consClosedPath :: Point a -> PathJoin a -> ClosedPath a -> ClosedPath a
consClosedPath p join (ClosedPath joins) =
ClosedPath ((p, join):joins)
openPathCurves :: Fractional a => OpenPath a -> [CubicBezier a]
openPathCurves (OpenPath curves p) = go curves p
where
go [] _ = []
go [(p0, jn)] q = [makeCB p0 jn q]
go ((p0, jn):rest@((p1,_):_)) q =
makeCB p0 jn p1 : go rest q
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
closedPathCurves :: Fractional a => ClosedPath a -> [CubicBezier a]
closedPathCurves (ClosedPath []) = []
closedPathCurves (ClosedPath (cs@((p1, _):_))) =
openPathCurves (OpenPath cs p1)
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
curvesToClosed :: [CubicBezier a] -> ClosedPath a
curvesToClosed cs = ClosedPath cs2
where
OpenPath cs2 _ = curvesToOpen cs
closeOpenPath :: OpenPath a -> ClosedPath a
closeOpenPath (OpenPath j p) = ClosedPath j
openClosedPath :: ClosedPath a -> OpenPath a
openClosedPath (ClosedPath []) = OpenPath [] (error "empty path")
openClosedPath (ClosedPath j@((p,_):_)) = OpenPath j p
anyToCubic :: (V.Unbox a) => AnyBezier a -> Maybe (CubicBezier a)
anyToCubic b@(AnyBezier v)
| degree b == 3 = Just $ unsafeFromVector v
| otherwise = Nothing
anyToQuad :: (V.Unbox a) => AnyBezier a -> Maybe (QuadBezier a)
anyToQuad b@(AnyBezier v)
| degree b == 2 = Just $ unsafeFromVector v
| otherwise = Nothing
evalBezierDerivsCubic :: Num a =>
CubicBezier a -> a -> [Point a]
evalBezierDerivsCubic (CubicBezier a b c d) t =
p `seq` p' `seq` p'' `seq` p''' `seq`
[p, p', p'', p''', Point 0 0]
where
u = 1t
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)
evalBezierDerivCubic :: Num a =>
CubicBezier a -> a -> (Point a, Point a)
evalBezierDerivCubic (CubicBezier a b c d) t = p `seq` p' `seq` (p, p')
where
u = 1t
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
evalBezierDerivsQuad :: Num a =>
QuadBezier a -> a -> [Point a]
evalBezierDerivsQuad (QuadBezier a b c) t = [p, p', p'', Point 0 0]
where
u = 1t
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)
evalBezierDerivQuad :: Num a =>
QuadBezier a -> a -> (Point a, Point a)
evalBezierDerivQuad (QuadBezier a b c) t = p `seq` p' `seq` (p, p')
where
u = 1t
t2 = t*t
p = u*^(u*^a ^+^ (2*t)*^b) ^+^ t2*^c
p' = 2*^(u*^(b^-^a) ^+^ t*^(c^-^b))
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
bezierParam :: (Ord a, Num a) => a -> Bool
bezierParam t = t >= 0 && t <= 1
bezierParamTolerance :: (GenericBezier b) => b Double -> Double -> Double
bezierParamTolerance (toVector -> v) eps = eps / maxVel
where
maxVel = 3 * V.maximum (V.zipWith vectorDistance (V.map (uncurry Point) v)
(V.map (uncurry Point) $ V.tail v))
bezierParamToleranceCubic :: (Ord a, Floating a) => CubicBezier a -> a -> a
bezierParamToleranceCubic (CubicBezier p0 p1 p2 p3) eps = eps / maxVel
where maxVel = (3 *) $ max (vectorDistance p0 p1) $
max (vectorDistance p1 p2)
(vectorDistance p2 p3)
bezierParamToleranceQuad :: (Ord a, Floating a) => QuadBezier a -> a -> a
bezierParamToleranceQuad (QuadBezier p0 p1 p2) eps = eps / maxVel
where maxVel = (3 *) $ max (vectorDistance p0 p1) (vectorDistance p1 p2)
reorient :: (GenericBezier b, V.Unbox a) => b a -> b a
reorient = unsafeFromVector . V.reverse . toVector
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
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
evalBezier :: (GenericBezier b, MV.Unbox a, Fractional a) =>
b a -> a -> Point a
evalBezier bc t = head $ evalBezierDerivs bc t
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
u = 1t
t2 = t*t
t3 = t2*t
evalBezierQuad :: Fractional a =>
QuadBezier a -> a -> Point a
evalBezierQuad (QuadBezier a b c) t =
u*^(u*^a ^+^ (2*t)*^b) ^+^ t2*^c
where
u = 1t
t2 = t*t
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 :: 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
a = tx*((y3 y0) + 3*(y1 y2)) ty*((x3 x0) + 3*(x1 x2))
b = 2*(tx*((y2 + y0) 2*y1) ty*((x2 + x0) 2*x1))
c = tx*(y1 y0) ty*(x1 x0)
bezierHoriz :: CubicBezier Double -> [Double]
bezierHoriz = findBezierTangent (Point 1 0)
bezierVert :: CubicBezier Double -> [Double]
bezierVert = findBezierTangent (Point 0 1)
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
ax = x1 x0
bx = x3 x1 ax
cx = x3 x2 ax 2*bx
ay = y1 y0
by = y2 y1 ay
cy = y3 y2 ay 2*by
a = bx*cy by*cx
b = ax*cy ay*cx
c = ax*by ay*bx
findBezierCusp :: CubicBezier Double -> [Double]
findBezierCusp b = filter vertical $ bezierHoriz b
where vertical = (== 0) . pointY . snd . evalBezierDeriv b
bezierArc :: Double -> Double -> CubicBezier Double
bezierArc start end = CubicBezier p0 p1 p2 p3
where
p0 = dirVector start
p3 = dirVector end
p1 = p0 ^+^ k *^ (rotate90L $* p0)
p2 = p1 ^+^ k *^ (rotate90R $* p3)
k = 4/3 * tan((endstart)/4)
arcLength :: CubicBezier Double -> Double -> Double -> Double
arcLength b@(CubicBezier c0 c1 c2 c3) t eps =
if eps / maximum [vectorDistance c0 c1,
vectorDistance c1 c2,
vectorDistance c2 c3] > 1e-10
then (signum t *) $ fst $
arcLengthEstimate (fst $ splitBezier b t) eps
else arcLengthQuad b t eps
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
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, Double))
arcLengthEstimate b eps = (arclen, (estimate, ol))
where
estimate = (4*(olL+olR) ol) / 3
(bl, br) = splitBezier b 0.5
ol = outline b
(arcL, (estL, olL)) = arcLengthEstimate bl eps
(arcR, (estR, olR)) = arcLengthEstimate br eps
arclen | abs(estL + estR estimate) < eps = estL + estR
| otherwise = arcL + arcR
arcLengthParam :: CubicBezier Double -> Double -> Double -> Double
arcLengthParam b len eps =
arcLengthP b len ol (len/ol) 1 eps
where ol = outline b
arcLengthP :: CubicBezier Double -> Double -> Double ->
Double -> Double -> Double -> Double
arcLengthP !b !len !tot !t !dt !eps
| abs diff < eps = t newDt
| otherwise = arcLengthP b len tot (t newDt) newDt eps
where diff = arcLength b t (max (abs (dt*tot/50)) (eps/2)) len
newDt = diff / vectorMag (snd $ evalBezierDeriv b t)
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
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
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
abbc = interpolateVector ab bc t
bccd = interpolateVector bc cd t
mid = interpolateVector abbc bccd t
in mid `seq` (CubicBezier a ab abbc mid, CubicBezier mid bccd cd d)
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)
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
bezierSubsegmentCubic :: (V.Unbox a, Fractional a) => CubicBezier a -> a -> a -> CubicBezier a
bezierSubsegmentCubic b t1 t2 =
CubicBezier b1 (b1 ^+^ b1' ^* ((t2t1)/3))
(b2 ^-^ (b2' ^* ((t2t1)/3))) b2
where (b1, b1') = evalBezierDeriv b t1
(b2, b2') = evalBezierDeriv b t2
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
splitBezierN c (t:u:rest) =
bezierSubsegment c 0 t :
bezierSubsegment c t u :
tail (splitBezierN c $ u:rest)
evalBezierDerivs2Cubic :: CubicBezier Double -> Double -> (DPoint, DPoint, DPoint)
evalBezierDerivs2Cubic (CubicBezier a b c d) t =
p `seq` p' `seq` p'' `seq`(p, p', p'')
where
u = 1t
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)
closestBezierCurve :: CubicBezier Double -> DPoint -> Double -> Double
closestBezierCurve cb p@(Point px py) t
| vSqr == 0 =
let (Point dx dy) = cubicC3 cb ^-^ cubicC0 cb
in signum ((px bx)*dx + (py by)*dy)
| vectorMagSquare (p ^-^ b) * 100 < vSqr*r_v*r_v =
closestLinePt
| otherwise =
(fastVectorAngle (rotateScaleVec (c ^-^ p) $* ((Point by' bx') ^* signum r_v))) * r_v
where
closestLinePt :: Double
closestLinePt = ((px bx)*bx' + (py by)*by')/vSqr
(b@(Point bx by), b'@(Point bx' by'), Point bx'' by'') = evalBezierDerivs2Cubic cb t
r_v = vSqr/denom
vSqr = bx'*bx' + by'*by'
denom = bx''*by' bx'*by''
c = b ^+^ ((rotate90R $* b') ^* r_v)
fastVectorAngle :: DPoint -> Double
fastVectorAngle (Point x y)
| abs y < abs x = y/x + if x < 0 then signum y*pi else 0
| otherwise = signum y*pi/2(x/y)
closest :: CubicBezier Double -> DPoint -> Double -> Double
closest cb p eps =
iter (closestBezierCurve cb p) (bezierParamTolerance cb eps) 0 1 0.5
iter :: (Double -> Double) -> Double -> Double -> Double -> Double -> Double
iter f eps mint maxt curt
| abs dt <= eps = curt + dt
| dt < 0 =
if | curt + dt <= mint ->
if mint == 0 && f 0 <= 0
then 0
else
let dT = (curt mint)/2
in if dT < eps then mint+dT
else iter f eps mint curt (mint+dT)
| otherwise ->
iter f eps mint curt (curt+dt)
| otherwise =
if | curt + dt >= maxt ->
if maxt == 1 && f 1 >= 0
then 1
--bisect
else
let dT = (maxt curt)/2
in if dT < eps then curt +dT
else iter f eps curt maxt (curt+dT)
| otherwise ->
iter f eps curt maxt (curt+dt)
where
dt = f curt
findX :: CubicBezier Double -> Double -> Double -> Double
findX (CubicBezier (Point p0 _) (Point p1 _) (Point p2 _) (Point p3 _)) x =
find0 (p0x) (p1x) (p2x) (p3x)
find0 :: Double -> Double -> Double -> Double -> Double -> Double
find0 a b c d eps =
iter bezierZero eps 0 1 0.5
where
bezierZero t
| bx == 0 = t
| otherwise = bx/bx'
where
u = 1t
t2 = t*t
t3 = t2*t
da = 3*^(ba)
db = 3*^(cb)
dc = 3*^(dc)
bx = u*(u*(u*a + (3*t)*b) + (3*t2)*c) + t3*d
bx' = u*(u*da + (2*t)*db) + t2*dc
colinear :: CubicBezier Double -> Double -> Bool
colinear (CubicBezier !a !b !c !d) eps = dmax dmin < eps
where ld = lineDistance (Line a d)
d1 = ld b
d2 = ld c
(dmin, dmax) | d1*d2 > 0 = (3/4 * minimum [0, d1, d2],
3/4 * maximum [0, d1, d2])
| otherwise = (4/9 * minimum [0, d1, d2],
4/9 * maximum [0, d1, d2])