module Geom2D.CubicBezier.Curvature
(curvature, radiusOfCurvature, curvatureExtrema, findRadius)
where
import Geom2D
import Geom2D.CubicBezier.Basic
import Geom2D.CubicBezier.Intersection
import Math.BernsteinPoly
curvature :: CubicBezier -> 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 c0 c1 c2 c3) = 2/3 * b/a^3
where
a = vectorDistance c1 c0
b = (c1^-^c0) `vectorCross` (c2^-^c1)
radiusOfCurvature :: CubicBezier -> Double -> Double
radiusOfCurvature b t = 1 / curvature b t
extrema :: CubicBezier -> BernsteinPoly
extrema (CubicBezier p0 p1 p2 p3) =
let bez = [p0, p1, p2, p3]
x' = bernsteinDeriv $ listToBernstein $ map pointX bez
y' = bernsteinDeriv $ listToBernstein $ map pointY bez
x'' = bernsteinDeriv x'
y'' = bernsteinDeriv y'
x''' = bernsteinDeriv x''
y''' = bernsteinDeriv y''
in
(y'~*y' ~+ x'~*x') ~* (x'~*y''' ~- y'~*x''') ~-
3 *~ (x'~*y'' ~- y'~*x'') ~* (y'~*y'' ~+ x'~*x'')
curvatureExtrema :: CubicBezier -> Double -> [Double]
curvatureExtrema b eps = bezierFindRoot (extrema b) 0 1 $
bezierParamTolerance b eps
radiusSquareEq :: CubicBezier -> Double -> BernsteinPoly
radiusSquareEq (CubicBezier p0 p1 p2 p3) d =
let bez = [p0, p1, p2, p3]
x' = bernsteinDeriv $ listToBernstein $ map pointX bez
y' = bernsteinDeriv $ listToBernstein $ map pointY bez
x'' = bernsteinDeriv x'
y'' = bernsteinDeriv y'
a = x'~*x' ~+ y'~*y'
b = x'~*y'' ~- x''~*y'
in (a~*a~*a) ~- (d*d) *~ b~*b
findRadius :: CubicBezier
-> Double
-> Double
-> [Double]
findRadius b d eps = bezierFindRoot (radiusSquareEq b d) 0 1 $
bezierParamTolerance b eps