{-# LANGUAGE FlexibleContexts #-}
{-# LANGUAGE GADTs            #-}
-----------------------------------------------------------------------------
-- |
-- Module      :  Diagrams.TwoD.Curvature
-- Copyright   :  (c) 2013 diagrams-lib team (see LICENSE)
-- License     :  BSD-style (see LICENSE)
-- Maintainer  :  diagrams-discuss@googlegroups.com
--
-- Compute curvature for segments in two dimensions.
--
-----------------------------------------------------------------------------

module Diagrams.TwoD.Curvature
    (
      curvature
    , radiusOfCurvature
    , squaredCurvature
    , squaredRadiusOfCurvature
    ) where

import           Control.Lens        (over)
import           Control.Monad
import           Data.Monoid.Inf

import           Diagrams.Segment
import           Diagrams.Tangent
import           Diagrams.TwoD.Types

import           Linear.Vector

-- | Curvature measures how curved the segment is at a point.  One intuition
-- for the concept is how much you would turn the wheel when driving a car
-- along the curve.  When the wheel is held straight there is zero curvature.
-- When turning a corner to the left we will have positive curvature.  When
-- turning to the right we will have negative curvature.
--
-- Another way to measure this idea is to find the largest circle that we can
-- push up against the curve and have it touch (locally) at exactly the point
-- and not cross the curve.  This is a tangent circle.  The radius of that
-- circle is the \"Radius of Curvature\" and it is the reciprocal of curvature.
-- Note that if the circle is on the \"left\" of the curve, we have a positive
-- radius, and if it is to the right we have a negative radius.  Straight
-- segments have an infinite radius which leads us to our representation.  We
-- result in a pair of numerator and denominator so we can include infinity and
-- zero for both the radius and the curvature.
--
--
-- Lets consider the following curve:
--
-- <<diagrams/src_Diagrams_TwoD_Curvature_diagramA.svg#diagram=diagramA&height=200&width=400>>
--
-- The curve starts with positive curvature,
--
-- <<diagrams/src_Diagrams_TwoD_Curvature_diagramPos.svg#diagram=diagramPos&height=200&width=400>>
--
-- approaches zero curvature
--
-- <<diagrams/src_Diagrams_TwoD_Curvature_diagramZero.svg#diagram=diagramZero&height=200&width=400>>
--
-- then has negative curvature
--
-- <<diagrams/src_Diagrams_TwoD_Curvature_diagramNeg.svg#diagram=diagramNeg&height=200&width=400>>
--
-- > {-# LANGUAGE GADTs #-}
-- >
-- > import Diagrams.TwoD.Curvature
-- > import Data.Monoid.Inf
-- > import Diagrams.Coordinates
-- >
-- > segmentA :: Segment Closed V2 Double
-- > segmentA = Cubic (12 ^& 0) (8 ^& 10) (OffsetClosed (20 ^& 8))
-- >
-- > curveA = lw thick . strokeP . fromSegments $ [segmentA]
-- >
-- > diagramA = pad 1.1 . centerXY $ curveA
-- >
-- > diagramPos = diagramWithRadius 0.2
-- >
-- > diagramZero = diagramWithRadius 0.45
-- >
-- > diagramNeg = diagramWithRadius 0.8
-- >
-- > diagramWithRadius t = pad 1.1 . centerXY
-- >          $ curveA
-- >         <> showCurvature segmentA t
-- >          # withEnvelope (curveA :: D V2 Double)
-- >          # lc red
-- >
-- > showCurvature :: Segment Closed V2 Double -> Double -> Diagram SVG
-- > showCurvature bez@(Cubic b c (OffsetClosed d)) t
-- >   | v == (0,0) = mempty
-- >   | otherwise  = go (radiusOfCurvature bez t)
-- >   where
-- >     v@(x,y) = unr2 $ firstDerivative b c d t
-- >     vp = (-y) ^& x
-- >
-- >     firstDerivative b c d t = let tt = t*t in (3*(3*tt-4*t+1))*^b + (3*(2-3*t)*t)*^c + (3*tt)*^d
-- >
-- >     go Infinity   = mempty
-- >     go (Finite r) = (circle (abs r) # translate vpr
-- >                  <> strokeP (origin ~~ (origin .+^ vpr)))
-- >                   # moveTo (origin .+^ atParam bez t)
-- >       where
-- >         vpr = signorm vp ^* r
-- >
--
curvature :: RealFloat n
          => Segment Closed V2 n  -- ^ Segment to measure on.
          -> n                    -- ^ Parameter to measure at.
          -> PosInf n             -- ^ Result is a @PosInf@ value where @PosInfty@ represents
                                  --   infinite curvature or zero radius of curvature.
curvature :: forall n. RealFloat n => Segment Closed V2 n -> n -> PosInf n
curvature Segment Closed V2 n
s = V2 n -> PosInf n
forall a. RealFloat a => V2 a -> PosInf a
toPosInf (V2 n -> PosInf n) -> (n -> V2 n) -> n -> PosInf n
forall b c a. (b -> c) -> (a -> b) -> a -> c
. ASetter (V2 n) (V2 n) n n -> (n -> n) -> V2 n -> V2 n
forall s t a b. ASetter s t a b -> (a -> b) -> s -> t
over ASetter (V2 n) (V2 n) n n
forall a. Lens' (V2 a) a
forall (t :: * -> *) a. R2 t => Lens' (t a) a
_y n -> n
forall a. Floating a => a -> a
sqrt (V2 n -> V2 n) -> (n -> V2 n) -> n -> V2 n
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Segment Closed V2 n -> n -> V2 n
forall n. Num n => Segment Closed V2 n -> n -> V2 n
curvaturePair Segment Closed V2 n
s

-- | With @squaredCurvature@ we can compute values in spaces that do not support
-- 'sqrt' and it is just as useful for relative ordering of curvatures or looking
-- for zeros.
squaredCurvature :: RealFloat n => Segment Closed V2 n -> n -> PosInf n
squaredCurvature :: forall n. RealFloat n => Segment Closed V2 n -> n -> PosInf n
squaredCurvature Segment Closed V2 n
s = V2 n -> PosInf n
forall a. RealFloat a => V2 a -> PosInf a
toPosInf (V2 n -> PosInf n) -> (n -> V2 n) -> n -> PosInf n
forall b c a. (b -> c) -> (a -> b) -> a -> c
. ASetter (V2 n) (V2 n) n n -> (n -> n) -> V2 n -> V2 n
forall s t a b. ASetter s t a b -> (a -> b) -> s -> t
over ASetter (V2 n) (V2 n) n n
forall a. Lens' (V2 a) a
forall (t :: * -> *) a. R1 t => Lens' (t a) a
_x ((n -> n -> n) -> n -> n
forall (m :: * -> *) a. Monad m => m (m a) -> m a
join n -> n -> n
forall a. Num a => a -> a -> a
(*)) (V2 n -> V2 n) -> (n -> V2 n) -> n -> V2 n
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Segment Closed V2 n -> n -> V2 n
forall n. Num n => Segment Closed V2 n -> n -> V2 n
curvaturePair Segment Closed V2 n
s

-- | Reciprocal of @curvature@.
radiusOfCurvature :: RealFloat n
                  => Segment Closed V2 n -- ^ Segment to measure on.
                  -> n                   -- ^ Parameter to measure at.
                  -> PosInf n            -- ^ Result is a @PosInf@ value where @PosInfty@ represents
                                         --   infinite radius of curvature or zero curvature.
radiusOfCurvature :: forall n. RealFloat n => Segment Closed V2 n -> n -> PosInf n
radiusOfCurvature Segment Closed V2 n
s = V2 n -> PosInf n
forall a. RealFloat a => V2 a -> PosInf a
toPosInf (V2 n -> PosInf n) -> (n -> V2 n) -> n -> PosInf n
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (\(V2 n
p n
q) -> n -> n -> V2 n
forall a. a -> a -> V2 a
V2 (n -> n
forall a. Floating a => a -> a
sqrt n
q) n
p) (V2 n -> V2 n) -> (n -> V2 n) -> n -> V2 n
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Segment Closed V2 n -> n -> V2 n
forall n. Num n => Segment Closed V2 n -> n -> V2 n
curvaturePair Segment Closed V2 n
s

-- | Reciprocal of @squaredCurvature@
squaredRadiusOfCurvature :: RealFloat n => Segment Closed V2 n -> n -> PosInf n
squaredRadiusOfCurvature :: forall n. RealFloat n => Segment Closed V2 n -> n -> PosInf n
squaredRadiusOfCurvature Segment Closed V2 n
s = V2 n -> PosInf n
forall a. RealFloat a => V2 a -> PosInf a
toPosInf (V2 n -> PosInf n) -> (n -> V2 n) -> n -> PosInf n
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (\(V2 n
p n
q) -> (n -> n -> V2 n
forall a. a -> a -> V2 a
V2 n
q (n
p n -> n -> n
forall a. Num a => a -> a -> a
* n
p))) (V2 n -> V2 n) -> (n -> V2 n) -> n -> V2 n
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Segment Closed V2 n -> n -> V2 n
forall n. Num n => Segment Closed V2 n -> n -> V2 n
curvaturePair Segment Closed V2 n
s

-- Package up problematic values with the appropriate infinity.
toPosInf :: RealFloat a => V2 a -> PosInf a
toPosInf :: forall a. RealFloat a => V2 a -> PosInf a
toPosInf (V2 a
_ a
0) = Inf Pos a
forall p a. Inf p a
Infinity
toPosInf (V2 a
p a
q)
  | a -> Bool
forall a. RealFloat a => a -> Bool
isInfinite a
r Bool -> Bool -> Bool
|| a -> Bool
forall a. RealFloat a => a -> Bool
isNaN a
r = Inf Pos a
forall p a. Inf p a
Infinity
  | Bool
otherwise               = a -> Inf Pos a
forall p a. a -> Inf p a
Finite a
r
  where r :: a
r = a
p a -> a -> a
forall a. Fractional a => a -> a -> a
/ a
q

-- Internal function that is not quite curvature or squaredCurvature but lets
-- us get there by either taking the square root of the numerator or squaring
-- the denominator respectively.
curvaturePair :: Num n
    => Segment Closed V2 n -> n -> V2 n
curvaturePair :: forall n. Num n => Segment Closed V2 n -> n -> V2 n
curvaturePair (Linear Offset Closed V2 n
_) n
_ = n -> n -> V2 n
forall a. a -> a -> V2 a
V2 n
0 n
1 -- Linear segments always have zero curvature (infinite radius).
curvaturePair seg :: Segment Closed V2 n
seg@(Cubic V2 n
b V2 n
c (OffsetClosed V2 n
d)) n
t
  = n -> n -> V2 n
forall a. a -> a -> V2 a
V2 (n
x'n -> n -> n
forall a. Num a => a -> a -> a
*n
y'' n -> n -> n
forall a. Num a => a -> a -> a
- n
y'n -> n -> n
forall a. Num a => a -> a -> a
*n
x'') ((n
x'n -> n -> n
forall a. Num a => a -> a -> a
*n
x' n -> n -> n
forall a. Num a => a -> a -> a
+ n
y'n -> n -> n
forall a. Num a => a -> a -> a
*n
y')n -> Int -> n
forall a b. (Num a, Integral b) => a -> b -> a
^(Int
3 :: Int))
  where
    (V2 n
x'  n
y' )     = Segment Closed V2 n
seg Segment Closed V2 n
-> N (Segment Closed V2 n) -> Vn (Segment Closed V2 n)
forall t. Parametric (Tangent t) => t -> N t -> Vn t
`tangentAtParam` n
N (Segment Closed V2 n)
t
    (V2 n
x'' n
y'')     = V2 n
secondDerivative
    secondDerivative :: V2 n
secondDerivative = (n
6n -> n -> n
forall a. Num a => a -> a -> a
*(n
3n -> n -> n
forall a. Num a => a -> a -> a
*n
tn -> n -> n
forall a. Num a => a -> a -> a
-n
2))n -> V2 n -> V2 n
forall (f :: * -> *) a. (Functor f, Num a) => a -> f a -> f a
*^V2 n
b V2 n -> V2 n -> V2 n
forall a. Num a => V2 a -> V2 a -> V2 a
forall (f :: * -> *) a. (Additive f, Num a) => f a -> f a -> f a
^+^ (n
6n -> n -> n
forall a. Num a => a -> a -> a
-n
18n -> n -> n
forall a. Num a => a -> a -> a
*n
t)n -> V2 n -> V2 n
forall (f :: * -> *) a. (Functor f, Num a) => a -> f a -> f a
*^V2 n
c V2 n -> V2 n -> V2 n
forall a. Num a => V2 a -> V2 a -> V2 a
forall (f :: * -> *) a. (Additive f, Num a) => f a -> f a -> f a
^+^ (n
6n -> n -> n
forall a. Num a => a -> a -> a
*n
t)n -> V2 n -> V2 n
forall (f :: * -> *) a. (Functor f, Num a) => a -> f a -> f a
*^V2 n
d

-- TODO: We should be able to generalize this to higher dimensions.  See
-- <http://en.wikipedia.org/wiki/Curvature>
--
-- TODO: I'm not sure what the best way to generalize squaredCurvature to other spaces is.

-- curvaturePair :: (Num t, Num (Scalar t), VectorSpace t)
--     => Segment Closed (t, t) -> Scalar t -> (t, t)
-- curvaturePair (Linear _) _ = (0,1) -- Linear segments always have zero curvature (infinite radius).
-- curvaturePair seg@(Cubic b c (OffsetClosed d)) t = ((x'*y'' - y'*x''), (x'*x' + y'*y')^(3 :: Integer))
--   where
--     (x' ,y' ) = seg `tangentAtParam` t
--     (x'',y'') = secondDerivative
--     secondDerivative = (6*(3*t-2))*^b ^+^ (6-18*t)*^c ^+^ (6*t)*^d