module Geom2D.CubicBezier.Curvature
       (curvature, radiusOfCurvature, curvatureExtrema, findRadius)
where
import Geom2D
import Geom2D.CubicBezier.Basic
import Geom2D.CubicBezier.Intersection
import Math.BernsteinPoly

-- | Curvature of the Bezier curve.  A negative curvature means the curve
-- curves to the right.
curvature :: CubicBezier Double -> Double -> Double
curvature :: CubicBezier Double -> Double -> Double
curvature CubicBezier Double
b Double
t
  | Double
t forall a. Eq a => a -> a -> Bool
== Double
0 = CubicBezier Double -> Double
curvature' CubicBezier Double
b
  | Double
t forall a. Eq a => a -> a -> Bool
== Double
1 = forall a. Num a => a -> a
negate forall a b. (a -> b) -> a -> b
$ CubicBezier Double -> Double
curvature' forall a b. (a -> b) -> a -> b
$ forall (b :: * -> *) a. (GenericBezier b, Unbox a) => b a -> b a
reorient CubicBezier Double
b
  | Double
t forall a. Ord a => a -> a -> Bool
< Double
0.5 = CubicBezier Double -> Double
curvature' forall a b. (a -> b) -> a -> b
$ forall a b. (a, b) -> b
snd forall a b. (a -> b) -> a -> b
$ forall a (b :: * -> *).
(Unbox a, Fractional a, GenericBezier b) =>
b a -> a -> (b a, b a)
splitBezier CubicBezier Double
b Double
t
  | Bool
otherwise = forall a. Num a => a -> a
negate forall a b. (a -> b) -> a -> b
$ CubicBezier Double -> Double
curvature' forall a b. (a -> b) -> a -> b
$ forall (b :: * -> *) a. (GenericBezier b, Unbox a) => b a -> b a
reorient forall a b. (a -> b) -> a -> b
$ forall a b. (a, b) -> a
fst forall a b. (a -> b) -> a -> b
$ forall a (b :: * -> *).
(Unbox a, Fractional a, GenericBezier b) =>
b a -> a -> (b a, b a)
splitBezier CubicBezier Double
b Double
t

curvature' :: CubicBezier Double -> Double
curvature' :: CubicBezier Double -> Double
curvature' (CubicBezier Point Double
c0 Point Double
c1 Point Double
c2 Point Double
_c3) = Double
2forall a. Fractional a => a -> a -> a
/Double
3 forall a. Num a => a -> a -> a
* Double
bforall a. Fractional a => a -> a -> a
/Double
aforall a b. (Num a, Integral b) => a -> b -> a
^(Int
3::Int)
  where 
    a :: Double
a = forall a. Floating a => Point a -> Point a -> a
vectorDistance Point Double
c1 Point Double
c0
    b :: Double
b = (Point Double
c1forall v. AdditiveGroup v => v -> v -> v
^-^Point Double
c0) forall a. Num a => Point a -> Point a -> a
`vectorCross` (Point Double
c2forall v. AdditiveGroup v => v -> v -> v
^-^Point Double
c1)

-- | Radius of curvature of the Bezier curve.  This
-- is the reciprocal of the curvature.
radiusOfCurvature :: CubicBezier Double -> Double -> Double
radiusOfCurvature :: CubicBezier Double -> Double -> Double
radiusOfCurvature CubicBezier Double
b Double
t = Double
1 forall a. Fractional a => a -> a -> a
/ CubicBezier Double -> Double -> Double
curvature CubicBezier Double
b Double
t

extrema :: CubicBezier Double -> BernsteinPoly Double
extrema :: CubicBezier Double -> BernsteinPoly Double
extrema CubicBezier Double
bez = 
  let (BernsteinPoly Double
x, BernsteinPoly Double
y) = forall (b :: * -> *) a.
(GenericBezier b, Unbox a) =>
b a -> (BernsteinPoly a, BernsteinPoly a)
bezierToBernstein CubicBezier Double
bez
      x' :: BernsteinPoly Double
x' = forall a. (Unbox a, Num a) => BernsteinPoly a -> BernsteinPoly a
bernsteinDeriv BernsteinPoly Double
x
      y' :: BernsteinPoly Double
y' = forall a. (Unbox a, Num a) => BernsteinPoly a -> BernsteinPoly a
bernsteinDeriv BernsteinPoly Double
y
      x'' :: BernsteinPoly Double
x'' = forall a. (Unbox a, Num a) => BernsteinPoly a -> BernsteinPoly a
bernsteinDeriv BernsteinPoly Double
x'
      y'' :: BernsteinPoly Double
y'' = forall a. (Unbox a, Num a) => BernsteinPoly a -> BernsteinPoly a
bernsteinDeriv BernsteinPoly Double
y'
      x''' :: BernsteinPoly Double
x''' = forall a. (Unbox a, Num a) => BernsteinPoly a -> BernsteinPoly a
bernsteinDeriv BernsteinPoly Double
x''
      y''' :: BernsteinPoly Double
y''' = forall a. (Unbox a, Num a) => BernsteinPoly a -> BernsteinPoly a
bernsteinDeriv BernsteinPoly Double
y''
  in (BernsteinPoly Double
y'forall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~*BernsteinPoly Double
y' forall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~+ BernsteinPoly Double
x'forall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~*BernsteinPoly Double
x') forall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~* (BernsteinPoly Double
x'forall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~*BernsteinPoly Double
y''' forall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~- BernsteinPoly Double
y'forall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~*BernsteinPoly Double
x''') forall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~-
     Double
3 forall a.
(Unbox a, Num a) =>
a -> BernsteinPoly a -> BernsteinPoly a
*~ (BernsteinPoly Double
x'forall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~*BernsteinPoly Double
y'' forall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~- BernsteinPoly Double
y'forall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~*BernsteinPoly Double
x'') forall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~* (BernsteinPoly Double
y'forall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~*BernsteinPoly Double
y'' forall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~+ BernsteinPoly Double
x'forall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~*BernsteinPoly Double
x'')

-- | Find extrema of the curvature, but not inflection points.
curvatureExtrema :: CubicBezier Double -> Double -> [Double]
curvatureExtrema :: CubicBezier Double -> Double -> [Double]
curvatureExtrema CubicBezier Double
b Double
eps
  | CubicBezier Double -> Double -> Bool
colinear CubicBezier Double
b Double
eps = []
  | Bool
otherwise = BernsteinPoly Double -> Double -> Double -> Double -> [Double]
bezierFindRoot (CubicBezier Double -> BernsteinPoly Double
extrema CubicBezier Double
b) Double
0 Double
1 forall a b. (a -> b) -> a -> b
$
                forall (b :: * -> *).
GenericBezier b =>
b Double -> Double -> Double
bezierParamTolerance CubicBezier Double
b Double
eps

radiusSquareEq :: CubicBezier Double -> Double -> BernsteinPoly Double
radiusSquareEq :: CubicBezier Double -> Double -> BernsteinPoly Double
radiusSquareEq CubicBezier Double
bez Double
d =
  let (BernsteinPoly Double
x, BernsteinPoly Double
y) = forall (b :: * -> *) a.
(GenericBezier b, Unbox a) =>
b a -> (BernsteinPoly a, BernsteinPoly a)
bezierToBernstein CubicBezier Double
bez
      x' :: BernsteinPoly Double
x' = forall a. (Unbox a, Num a) => BernsteinPoly a -> BernsteinPoly a
bernsteinDeriv BernsteinPoly Double
x
      y' :: BernsteinPoly Double
y' = forall a. (Unbox a, Num a) => BernsteinPoly a -> BernsteinPoly a
bernsteinDeriv BernsteinPoly Double
y
      x'' :: BernsteinPoly Double
x'' = forall a. (Unbox a, Num a) => BernsteinPoly a -> BernsteinPoly a
bernsteinDeriv BernsteinPoly Double
x'
      y'' :: BernsteinPoly Double
y'' = forall a. (Unbox a, Num a) => BernsteinPoly a -> BernsteinPoly a
bernsteinDeriv BernsteinPoly Double
y'
      a :: BernsteinPoly Double
a =  BernsteinPoly Double
x'forall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~*BernsteinPoly Double
x' forall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~+  BernsteinPoly Double
y'forall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~*BernsteinPoly Double
y'
      b :: BernsteinPoly Double
b =  BernsteinPoly Double
x'forall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~*BernsteinPoly Double
y'' forall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~-  BernsteinPoly Double
x''forall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~*BernsteinPoly Double
y'
  in (BernsteinPoly Double
aforall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~*BernsteinPoly Double
aforall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~*BernsteinPoly Double
a) forall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~- (Double
dforall a. Num a => a -> a -> a
*Double
d) forall a.
(Unbox a, Num a) =>
a -> BernsteinPoly a -> BernsteinPoly a
*~ BernsteinPoly Double
bforall a.
(Unbox a, Fractional a) =>
BernsteinPoly a -> BernsteinPoly a -> BernsteinPoly a
~*BernsteinPoly Double
b

-- | 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 Double  -- ^ the curve
           -> Double       -- ^ distance
           -> Double       -- ^ tolerance
           -> [Double]     -- ^ result
findRadius :: CubicBezier Double -> Double -> Double -> [Double]
findRadius CubicBezier Double
b Double
d Double
eps
  | CubicBezier Double -> Double -> Bool
colinear CubicBezier Double
b Double
eps = []  -- either empty or a huge list of t's
  | Bool
otherwise = BernsteinPoly Double -> Double -> Double -> Double -> [Double]
bezierFindRoot (CubicBezier Double -> Double -> BernsteinPoly Double
radiusSquareEq CubicBezier Double
b Double
d) Double
0 Double
1 forall a b. (a -> b) -> a -> b
$
                forall (b :: * -> *).
GenericBezier b =>
b Double -> Double -> Double
bezierParamTolerance CubicBezier Double
b Double
eps