{-# LANGUAGE BangPatterns         #-}
{-# LANGUAGE UndecidableInstances #-}
--------------------------------------------------------------------------------
-- |
-- Module      :  Data.Geometry.BezierSpline
-- Copyright   :  (C) Frank Staals
-- License     :  see the LICENSE file
-- Maintainer  :  Frank Staals
--------------------------------------------------------------------------------
module Data.Geometry.BezierSpline(
    BezierSpline (BezierSpline, Bezier2, Bezier3)
  , controlPoints
  , fromPointSeq
  , endPoints
  , Data.Geometry.BezierSpline.reverse

  , evaluate
  , split
  , splitMany
  , splitMonotone
  , splitByPoints
  , extension
  , extend
  , growTo
  , merge
  , subBezier
  , tangent
  , approximate
  , parameterOf
  , snap
  , intersectB
  , colinear
  , quadToCubic
  ) where

import           Algorithms.Geometry.ConvexHull.GrahamScan
import           Algorithms.Geometry.SmallestEnclosingBall.RIC
import           Algorithms.Geometry.SmallestEnclosingBall.Types
import           Control.Lens hiding (Empty)
import           Data.Ext
import qualified Data.Foldable as F
import           Data.Geometry.Ball
import           Data.Geometry.Box.Internal
import           Data.Geometry.Line
import           Data.Geometry.LineSegment hiding (endPoints)
import           Data.Geometry.Point
import           Data.Geometry.PolyLine (PolyLine(..))
import           Data.Geometry.Polygon
import           Data.Geometry.Polygon.Convex hiding (merge)
import           Data.Geometry.Properties
import           Data.Geometry.Transformation
import           Data.Geometry.Vector hiding (init)
import           Data.LSeq (LSeq)
import qualified Data.LSeq as LSeq
import           Data.List (sort)
import qualified Data.List.NonEmpty as NonEmpty
import           Data.Sequence (Seq(..))
import qualified Data.Sequence as Seq
import           Data.Traversable (fmapDefault,foldMapDefault)
import           GHC.TypeNats
import qualified Test.QuickCheck as QC

-- import Debug.Trace

--------------------------------------------------------------------------------

-- | Datatype representing a Bezier curve of degree \(n\) in \(d\)-dimensional space.
newtype BezierSpline n d r = BezierSpline { BezierSpline n d r -> LSeq (1 + n) (Point d r)
_controlPoints :: LSeq (1+n) (Point d r) }
-- makeLenses ''BezierSpline

-- | Bezier control points. With n degrees, there are n+1 control points.
controlPoints :: Iso (BezierSpline n1 d1 r1)     (BezierSpline n2 d2 r2)
                     (LSeq (1+n1) (Point d1 r1)) (LSeq (1+n2) (Point d2 r2))
controlPoints :: p (LSeq (1 + n1) (Point d1 r1)) (f (LSeq (1 + n2) (Point d2 r2)))
-> p (BezierSpline n1 d1 r1) (f (BezierSpline n2 d2 r2))
controlPoints = (BezierSpline n1 d1 r1 -> LSeq (1 + n1) (Point d1 r1))
-> (LSeq (1 + n2) (Point d2 r2) -> BezierSpline n2 d2 r2)
-> Iso
     (BezierSpline n1 d1 r1)
     (BezierSpline n2 d2 r2)
     (LSeq (1 + n1) (Point d1 r1))
     (LSeq (1 + n2) (Point d2 r2))
forall s a b t. (s -> a) -> (b -> t) -> Iso s t a b
iso BezierSpline n1 d1 r1 -> LSeq (1 + n1) (Point d1 r1)
forall (n :: Nat) (d :: Nat) r.
BezierSpline n d r -> LSeq (1 + n) (Point d r)
_controlPoints LSeq (1 + n2) (Point d2 r2) -> BezierSpline n2 d2 r2
forall (n :: Nat) (d :: Nat) r.
LSeq (1 + n) (Point d r) -> BezierSpline n d r
BezierSpline

-- | Quadratic Bezier Spline
pattern Bezier2      :: Point d r -> Point d r -> Point d r -> BezierSpline 2 d r
pattern $bBezier2 :: Point d r -> Point d r -> Point d r -> BezierSpline 2 d r
$mBezier2 :: forall r (d :: Nat) r.
BezierSpline 2 d r
-> (Point d r -> Point d r -> Point d r -> r) -> (Void# -> r) -> r
Bezier2 p q r <- (F.toList . LSeq.take 3 . _controlPoints -> [p,q,r])
  where
    Bezier2 Point d r
p Point d r
q Point d r
r = Seq (Point d r) -> BezierSpline 2 d r
forall (d :: Nat) r (n :: Nat).
Seq (Point d r) -> BezierSpline n d r
fromPointSeq (Seq (Point d r) -> BezierSpline 2 d r)
-> ([Point d r] -> Seq (Point d r))
-> [Point d r]
-> BezierSpline 2 d r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. [Point d r] -> Seq (Point d r)
forall a. [a] -> Seq a
Seq.fromList ([Point d r] -> BezierSpline 2 d r)
-> [Point d r] -> BezierSpline 2 d r
forall a b. (a -> b) -> a -> b
$ [Point d r
p,Point d r
q,Point d r
r]
{-# COMPLETE Bezier2 #-}

-- | Cubic Bezier Spline
pattern Bezier3         :: Point d r -> Point d r -> Point d r -> Point d r -> BezierSpline 3 d r
pattern $bBezier3 :: Point d r
-> Point d r -> Point d r -> Point d r -> BezierSpline 3 d r
$mBezier3 :: forall r (d :: Nat) r.
BezierSpline 3 d r
-> (Point d r -> Point d r -> Point d r -> Point d r -> r)
-> (Void# -> r)
-> r
Bezier3 p q r s <- (F.toList . LSeq.take 4 . _controlPoints -> [p,q,r,s])
  where
    Bezier3 Point d r
p Point d r
q Point d r
r Point d r
s = Seq (Point d r) -> BezierSpline 3 d r
forall (d :: Nat) r (n :: Nat).
Seq (Point d r) -> BezierSpline n d r
fromPointSeq (Seq (Point d r) -> BezierSpline 3 d r)
-> ([Point d r] -> Seq (Point d r))
-> [Point d r]
-> BezierSpline 3 d r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. [Point d r] -> Seq (Point d r)
forall a. [a] -> Seq a
Seq.fromList ([Point d r] -> BezierSpline 3 d r)
-> [Point d r] -> BezierSpline 3 d r
forall a b. (a -> b) -> a -> b
$ [Point d r
p,Point d r
q,Point d r
r,Point d r
s]
{-# COMPLETE Bezier3 #-}

-- | Constructs the Bezier Spline from a given sequence of points.
fromPointSeq :: Seq (Point d r) -> BezierSpline n d r
fromPointSeq :: Seq (Point d r) -> BezierSpline n d r
fromPointSeq = LSeq (1 + n) (Point d r) -> BezierSpline n d r
forall (n :: Nat) (d :: Nat) r.
LSeq (1 + n) (Point d r) -> BezierSpline n d r
BezierSpline (LSeq (1 + n) (Point d r) -> BezierSpline n d r)
-> (Seq (Point d r) -> LSeq (1 + n) (Point d r))
-> Seq (Point d r)
-> BezierSpline n d r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. LSeq 0 (Point d r) -> LSeq (1 + n) (Point d r)
forall (n :: Nat) (m :: Nat) a. LSeq m a -> LSeq n a
LSeq.promise (LSeq 0 (Point d r) -> LSeq (1 + n) (Point d r))
-> (Seq (Point d r) -> LSeq 0 (Point d r))
-> Seq (Point d r)
-> LSeq (1 + n) (Point d r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Seq (Point d r) -> LSeq 0 (Point d r)
forall a. Seq a -> LSeq 0 a
LSeq.fromSeq


deriving instance (Arity d, Eq r) => Eq (BezierSpline n d r)

type instance Dimension (BezierSpline n d r) = d
type instance NumType   (BezierSpline n d r) = r

instance (Arity n, Arity d, QC.Arbitrary r) => QC.Arbitrary (BezierSpline n d r) where
  arbitrary :: Gen (BezierSpline n d r)
arbitrary = Seq (Point d r) -> BezierSpline n d r
forall (d :: Nat) r (n :: Nat).
Seq (Point d r) -> BezierSpline n d r
fromPointSeq (Seq (Point d r) -> BezierSpline n d r)
-> ([Point d r] -> Seq (Point d r))
-> [Point d r]
-> BezierSpline n d r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. [Point d r] -> Seq (Point d r)
forall a. [a] -> Seq a
Seq.fromList ([Point d r] -> BezierSpline n d r)
-> Gen [Point d r] -> Gen (BezierSpline n d r)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Int -> Gen [Point d r]
forall a. Arbitrary a => Int -> Gen [a]
QC.vector (Natural -> Int
forall a b. (Integral a, Num b) => a -> b
fromIntegral (Natural -> Int) -> (C n -> Natural) -> C n -> Int
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (Natural
1Natural -> Natural -> Natural
forall a. Num a => a -> a -> a
+) (Natural -> Natural) -> (C n -> Natural) -> C n -> Natural
forall b c a. (b -> c) -> (a -> b) -> a -> c
. C n -> Natural
forall (n :: Nat) (proxy :: Nat -> *).
KnownNat n =>
proxy n -> Natural
natVal (C n -> Int) -> C n -> Int
forall a b. (a -> b) -> a -> b
$ C n
forall (n :: Nat). C n
C @n)

{-
instance (Arity n, Arity d, QC.Arbitrary r, Ord r) => QC.Arbitrary (BezierSpline n d r) where
  arbitrary = fromPointSeq . Seq.fromList <$> allDifferent (fromIntegral . (1+) . natVal $ C @n)

-- | Generates a set of unique items.
allDifferent   :: (Ord a, QC.Arbitrary  a) => Int -> QC.Gen [a]
allDifferent n = take n . Set.toList . go maxattempts mempty <$> QC.infiniteList
  where
    maxattempts = 100
    go 0 s _                        = s -- too many attempts
    go t s (x:xs) | Set.size s == n = s
                  | otherwise       = go (t-1) (Set.insert x s) xs
-}

instance (Arity d, Show r) => Show (BezierSpline n d r) where
  show :: BezierSpline n d r -> String
show (BezierSpline LSeq (1 + n) (Point d r)
ps) =
    [String] -> String
forall a. Monoid a => [a] -> a
mconcat [ String
"BezierSpline", Int -> String
forall a. Show a => a -> String
show (Int -> String) -> Int -> String
forall a b. (a -> b) -> a -> b
$ LSeq (1 + n) (Point d r) -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length LSeq (1 + n) (Point d r)
ps Int -> Int -> Int
forall a. Num a => a -> a -> a
- Int
1, String
" ", [Point d r] -> String
forall a. Show a => a -> String
show (LSeq (1 + n) (Point d r) -> [Point d r]
forall (t :: * -> *) a. Foldable t => t a -> [a]
F.toList LSeq (1 + n) (Point d r)
ps) ]

instance Arity d => Functor (BezierSpline n d) where
  fmap :: (a -> b) -> BezierSpline n d a -> BezierSpline n d b
fmap = (a -> b) -> BezierSpline n d a -> BezierSpline n d b
forall (t :: * -> *) a b. Traversable t => (a -> b) -> t a -> t b
fmapDefault

instance Arity d => Foldable (BezierSpline n d) where
  foldMap :: (a -> m) -> BezierSpline n d a -> m
foldMap = (a -> m) -> BezierSpline n d a -> m
forall (t :: * -> *) m a.
(Traversable t, Monoid m) =>
(a -> m) -> t a -> m
foldMapDefault

instance Arity d => Traversable (BezierSpline n d) where
  traverse :: (a -> f b) -> BezierSpline n d a -> f (BezierSpline n d b)
traverse a -> f b
f (BezierSpline LSeq (1 + n) (Point d a)
ps) = LSeq (1 + n) (Point d b) -> BezierSpline n d b
forall (n :: Nat) (d :: Nat) r.
LSeq (1 + n) (Point d r) -> BezierSpline n d r
BezierSpline (LSeq (1 + n) (Point d b) -> BezierSpline n d b)
-> f (LSeq (1 + n) (Point d b)) -> f (BezierSpline n d b)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> (Point d a -> f (Point d b))
-> LSeq (1 + n) (Point d a) -> f (LSeq (1 + n) (Point d b))
forall (t :: * -> *) (f :: * -> *) a b.
(Traversable t, Applicative f) =>
(a -> f b) -> t a -> f (t b)
traverse ((a -> f b) -> Point d a -> f (Point d b)
forall (t :: * -> *) (f :: * -> *) a b.
(Traversable t, Applicative f) =>
(a -> f b) -> t a -> f (t b)
traverse a -> f b
f) LSeq (1 + n) (Point d a)
ps

instance (Fractional r, Arity d, Arity (d + 1), Arity n)
          => IsTransformable (BezierSpline n d r) where
  transformBy :: Transformation
  (Dimension (BezierSpline n d r)) (NumType (BezierSpline n d r))
-> BezierSpline n d r -> BezierSpline n d r
transformBy = Transformation
  (Dimension (BezierSpline n d r)) (NumType (BezierSpline n d r))
-> BezierSpline n d r -> BezierSpline n d r
forall (g :: * -> *) r (d :: Nat).
(PointFunctor g, Fractional r, d ~ Dimension (g r), Arity d,
 Arity (d + 1)) =>
Transformation d r -> g r -> g r
transformPointFunctor

instance PointFunctor (BezierSpline n d) where
  pmap :: (Point (Dimension (BezierSpline n d r)) r
 -> Point (Dimension (BezierSpline n d s)) s)
-> BezierSpline n d r -> BezierSpline n d s
pmap Point (Dimension (BezierSpline n d r)) r
-> Point (Dimension (BezierSpline n d s)) s
f = ASetter
  (BezierSpline n d r)
  (BezierSpline n d s)
  (LSeq (1 + n) (Point d r))
  (LSeq (1 + n) (Point d s))
-> (LSeq (1 + n) (Point d r) -> LSeq (1 + n) (Point d s))
-> BezierSpline n d r
-> BezierSpline n d s
forall s t a b. ASetter s t a b -> (a -> b) -> s -> t
over ASetter
  (BezierSpline n d r)
  (BezierSpline n d s)
  (LSeq (1 + n) (Point d r))
  (LSeq (1 + n) (Point d s))
forall (n1 :: Nat) (d1 :: Nat) r1 (n2 :: Nat) (d2 :: Nat) r2.
Iso
  (BezierSpline n1 d1 r1)
  (BezierSpline n2 d2 r2)
  (LSeq (1 + n1) (Point d1 r1))
  (LSeq (1 + n2) (Point d2 r2))
controlPoints ((Point d r -> Point d s)
-> LSeq (1 + n) (Point d r) -> LSeq (1 + n) (Point d s)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Point d r -> Point d s
Point (Dimension (BezierSpline n d r)) r
-> Point (Dimension (BezierSpline n d s)) s
f)

--------------------------------------------------------------------------------

-- | Convert a quadratic bezier to a cubic bezier.
quadToCubic :: Fractional r => BezierSpline 2 2 r -> BezierSpline 3 2 r
quadToCubic :: BezierSpline 2 2 r -> BezierSpline 3 2 r
quadToCubic (Bezier2 Point 2 r
a (Point Vector 2 r
b) Point 2 r
c) =
  Point 2 r
-> Point 2 r -> Point 2 r -> Point 2 r -> BezierSpline 3 2 r
forall (d :: Nat) r.
Point d r
-> Point d r -> Point d r -> Point d r -> BezierSpline 3 d r
Bezier3 Point 2 r
a (Vector 2 r -> Point 2 r
forall (d :: Nat) r. Vector d r -> Point d r
Point (Vector 2 r -> Point 2 r) -> Vector 2 r -> Point 2 r
forall a b. (a -> b) -> a -> b
$ (r
1r -> r -> r
forall a. Fractional a => a -> a -> a
/r
3)r -> Vector 2 r -> Vector 2 r
forall (f :: * -> *) a. (Functor f, Num a) => a -> f a -> f a
*^ (Point 2 r -> Vector 2 r
forall (d :: Nat) r. Point d r -> Vector d r
toVec Point 2 r
a Vector 2 r -> Vector 2 r -> Vector 2 r
forall (f :: * -> *) a. (Additive f, Num a) => f a -> f a -> f a
^+^ r
2r -> Vector 2 r -> Vector 2 r
forall (f :: * -> *) a. (Functor f, Num a) => a -> f a -> f a
*^Vector 2 r
b)) (Vector 2 r -> Point 2 r
forall (d :: Nat) r. Vector d r -> Point d r
Point (Vector 2 r -> Point 2 r) -> Vector 2 r -> Point 2 r
forall a b. (a -> b) -> a -> b
$ (r
1r -> r -> r
forall a. Fractional a => a -> a -> a
/r
3)r -> Vector 2 r -> Vector 2 r
forall (f :: * -> *) a. (Functor f, Num a) => a -> f a -> f a
*^ (r
2r -> Vector 2 r -> Vector 2 r
forall (f :: * -> *) a. (Functor f, Num a) => a -> f a -> f a
*^ Vector 2 r
b Vector 2 r -> Vector 2 r -> Vector 2 r
forall (f :: * -> *) a. (Additive f, Num a) => f a -> f a -> f a
^+^ Point 2 r -> Vector 2 r
forall (d :: Nat) r. Point d r -> Vector d r
toVec Point 2 r
c)) Point 2 r
c

--------------------------------------------------------------------------------

-- | Reverse a BezierSpline
reverse :: (Arity d, Ord r, Num r) => BezierSpline n d r -> BezierSpline n d r
reverse :: BezierSpline n d r -> BezierSpline n d r
reverse = (LSeq (1 + n) (Point d r) -> Identity (LSeq (1 + n) (Point d r)))
-> BezierSpline n d r -> Identity (BezierSpline n d r)
forall (n1 :: Nat) (d1 :: Nat) r1 (n2 :: Nat) (d2 :: Nat) r2.
Iso
  (BezierSpline n1 d1 r1)
  (BezierSpline n2 d2 r2)
  (LSeq (1 + n1) (Point d1 r1))
  (LSeq (1 + n2) (Point d2 r2))
controlPoints ((LSeq (1 + n) (Point d r) -> Identity (LSeq (1 + n) (Point d r)))
 -> BezierSpline n d r -> Identity (BezierSpline n d r))
-> (LSeq (1 + n) (Point d r) -> LSeq (1 + n) (Point d r))
-> BezierSpline n d r
-> BezierSpline n d r
forall s t a b. ASetter s t a b -> (a -> b) -> s -> t
%~ LSeq (1 + n) (Point d r) -> LSeq (1 + n) (Point d r)
forall (n :: Nat) a. LSeq n a -> LSeq n a
LSeq.reverse


-- | Evaluate a BezierSpline curve at time t in [0, 1]
--
-- pre: \(t \in [0,1]\)
evaluate     :: (Arity d, Eq r, Num r) => BezierSpline n d r -> r -> Point d r
evaluate :: BezierSpline n d r -> r -> Point d r
evaluate BezierSpline n d r
b r
0 = (Point d r, Point d r) -> Point d r
forall a b. (a, b) -> a
fst ((Point d r, Point d r) -> Point d r)
-> (Point d r, Point d r) -> Point d r
forall a b. (a -> b) -> a -> b
$ BezierSpline n d r -> (Point d r, Point d r)
forall (n :: Nat) (d :: Nat) r.
BezierSpline n d r -> (Point d r, Point d r)
endPoints BezierSpline n d r
b
evaluate BezierSpline n d r
b r
1 = (Point d r, Point d r) -> Point d r
forall a b. (a, b) -> b
snd ((Point d r, Point d r) -> Point d r)
-> (Point d r, Point d r) -> Point d r
forall a b. (a -> b) -> a -> b
$ BezierSpline n d r -> (Point d r, Point d r)
forall (n :: Nat) (d :: Nat) r.
BezierSpline n d r -> (Point d r, Point d r)
endPoints BezierSpline n d r
b
evaluate BezierSpline n d r
b r
t = Seq (Point d r) -> Point d r
evaluate' (BezierSpline n d r
bBezierSpline n d r
-> Getting (Seq (Point d r)) (BezierSpline n d r) (Seq (Point d r))
-> Seq (Point d r)
forall s a. s -> Getting a s a -> a
^.(LSeq (1 + n) (Point d r)
 -> Const (Seq (Point d r)) (LSeq (1 + n) (Point d r)))
-> BezierSpline n d r
-> Const (Seq (Point d r)) (BezierSpline n d r)
forall (n1 :: Nat) (d1 :: Nat) r1 (n2 :: Nat) (d2 :: Nat) r2.
Iso
  (BezierSpline n1 d1 r1)
  (BezierSpline n2 d2 r2)
  (LSeq (1 + n1) (Point d1 r1))
  (LSeq (1 + n2) (Point d2 r2))
controlPoints((LSeq (1 + n) (Point d r)
  -> Const (Seq (Point d r)) (LSeq (1 + n) (Point d r)))
 -> BezierSpline n d r
 -> Const (Seq (Point d r)) (BezierSpline n d r))
-> ((Seq (Point d r) -> Const (Seq (Point d r)) (Seq (Point d r)))
    -> LSeq (1 + n) (Point d r)
    -> Const (Seq (Point d r)) (LSeq (1 + n) (Point d r)))
-> Getting (Seq (Point d r)) (BezierSpline n d r) (Seq (Point d r))
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(LSeq (1 + n) (Point d r) -> Seq (Point d r))
-> (Seq (Point d r) -> Const (Seq (Point d r)) (Seq (Point d r)))
-> LSeq (1 + n) (Point d r)
-> Const (Seq (Point d r)) (LSeq (1 + n) (Point d r))
forall (p :: * -> * -> *) (f :: * -> *) s a.
(Profunctor p, Contravariant f) =>
(s -> a) -> Optic' p f s a
to LSeq (1 + n) (Point d r) -> Seq (Point d r)
forall (n :: Nat) a. LSeq n a -> Seq a
LSeq.toSeq)
  where
    evaluate' :: Seq (Point d r) -> Point d r
evaluate' = \case
      (Point d r
p :<| Seq (Point d r)
Empty)  -> Point d r
p
      pts :: Seq (Point d r)
pts@(Point d r
_ :<| Seq (Point d r)
tl) -> let (Seq (Point d r)
ini :|> Point d r
_) = Seq (Point d r)
pts in Seq (Point d r) -> Point d r
evaluate' (Seq (Point d r) -> Point d r) -> Seq (Point d r) -> Point d r
forall a b. (a -> b) -> a -> b
$ (Point d r -> Point d r -> Point d r)
-> Seq (Point d r) -> Seq (Point d r) -> Seq (Point d r)
forall a b c. (a -> b -> c) -> Seq a -> Seq b -> Seq c
Seq.zipWith Point d r -> Point d r -> Point d r
blend Seq (Point d r)
ini Seq (Point d r)
tl
      Seq (Point d r)
_              -> String -> Point d r
forall a. HasCallStack => String -> a
error String
"evaluate: absurd"
    blend :: Point d r -> Point d r -> Point d r
blend Point d r
p Point d r
q = Point d r
p Point d r -> Diff (Point d) r -> Point d r
forall (p :: * -> *) a. (Affine p, Num a) => p a -> Diff p a -> p a
.+^ r
t r -> Vector d r -> Vector d r
forall (f :: * -> *) a. (Functor f, Num a) => a -> f a -> f a
*^ (Point d r
q Point d r -> Point d r -> Diff (Point d) r
forall (p :: * -> *) a. (Affine p, Num a) => p a -> p a -> Diff p a
.-. Point d r
p)

-- | Extract a tangent vector from the first to the second control point.
tangent   :: (Arity d, Num r, 1 <= n) => BezierSpline n d r -> Vector d r
tangent :: BezierSpline n d r -> Vector d r
tangent BezierSpline n d r
b = BezierSpline n d r
bBezierSpline n d r
-> Getting (Endo (Point d r)) (BezierSpline n d r) (Point d r)
-> Point d r
forall s a. HasCallStack => s -> Getting (Endo a) s a -> a
^?!(LSeq (1 + n) (Point d r)
 -> Const (Endo (Point d r)) (LSeq (1 + n) (Point d r)))
-> BezierSpline n d r
-> Const (Endo (Point d r)) (BezierSpline n d r)
forall (n1 :: Nat) (d1 :: Nat) r1 (n2 :: Nat) (d2 :: Nat) r2.
Iso
  (BezierSpline n1 d1 r1)
  (BezierSpline n2 d2 r2)
  (LSeq (1 + n1) (Point d1 r1))
  (LSeq (1 + n2) (Point d2 r2))
controlPoints((LSeq (1 + n) (Point d r)
  -> Const (Endo (Point d r)) (LSeq (1 + n) (Point d r)))
 -> BezierSpline n d r
 -> Const (Endo (Point d r)) (BezierSpline n d r))
-> ((Point d r -> Const (Endo (Point d r)) (Point d r))
    -> LSeq (1 + n) (Point d r)
    -> Const (Endo (Point d r)) (LSeq (1 + n) (Point d r)))
-> Getting (Endo (Point d r)) (BezierSpline n d r) (Point d r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
.Index (LSeq (1 + n) (Point d r))
-> Traversal'
     (LSeq (1 + n) (Point d r)) (IxValue (LSeq (1 + n) (Point d r)))
forall m. Ixed m => Index m -> Traversal' m (IxValue m)
ix Index (LSeq (1 + n) (Point d r))
1 Point d r -> Point d r -> Diff (Point d) r
forall (p :: * -> *) a. (Affine p, Num a) => p a -> p a -> Diff p a
.-. BezierSpline n d r
bBezierSpline n d r
-> Getting (Endo (Point d r)) (BezierSpline n d r) (Point d r)
-> Point d r
forall s a. HasCallStack => s -> Getting (Endo a) s a -> a
^?!(LSeq (1 + n) (Point d r)
 -> Const (Endo (Point d r)) (LSeq (1 + n) (Point d r)))
-> BezierSpline n d r
-> Const (Endo (Point d r)) (BezierSpline n d r)
forall (n1 :: Nat) (d1 :: Nat) r1 (n2 :: Nat) (d2 :: Nat) r2.
Iso
  (BezierSpline n1 d1 r1)
  (BezierSpline n2 d2 r2)
  (LSeq (1 + n1) (Point d1 r1))
  (LSeq (1 + n2) (Point d2 r2))
controlPoints((LSeq (1 + n) (Point d r)
  -> Const (Endo (Point d r)) (LSeq (1 + n) (Point d r)))
 -> BezierSpline n d r
 -> Const (Endo (Point d r)) (BezierSpline n d r))
-> ((Point d r -> Const (Endo (Point d r)) (Point d r))
    -> LSeq (1 + n) (Point d r)
    -> Const (Endo (Point d r)) (LSeq (1 + n) (Point d r)))
-> Getting (Endo (Point d r)) (BezierSpline n d r) (Point d r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
.Index (LSeq (1 + n) (Point d r))
-> Traversal'
     (LSeq (1 + n) (Point d r)) (IxValue (LSeq (1 + n) (Point d r)))
forall m. Ixed m => Index m -> Traversal' m (IxValue m)
ix Index (LSeq (1 + n) (Point d r))
0

-- | Return the endpoints of the Bezier spline.
endPoints   :: BezierSpline n d r -> (Point d r, Point d r)
endPoints :: BezierSpline n d r -> (Point d r, Point d r)
endPoints BezierSpline n d r
b = let (Point d r
p LSeq.:<| LSeq n (Point d r)
_) = BezierSpline n d r
bBezierSpline n d r
-> Getting
     (LSeq (1 + n) (Point d r))
     (BezierSpline n d r)
     (LSeq (1 + n) (Point d r))
-> LSeq (1 + n) (Point d r)
forall s a. s -> Getting a s a -> a
^.Getting
  (LSeq (1 + n) (Point d r))
  (BezierSpline n d r)
  (LSeq (1 + n) (Point d r))
forall (n1 :: Nat) (d1 :: Nat) r1 (n2 :: Nat) (d2 :: Nat) r2.
Iso
  (BezierSpline n1 d1 r1)
  (BezierSpline n2 d2 r2)
  (LSeq (1 + n1) (Point d1 r1))
  (LSeq (1 + n2) (Point d2 r2))
controlPoints
                  (LSeq n (Point d r)
_ LSeq.:|> Point d r
q) = BezierSpline n d r
bBezierSpline n d r
-> Getting
     (LSeq (1 + n) (Point d r))
     (BezierSpline n d r)
     (LSeq (1 + n) (Point d r))
-> LSeq (1 + n) (Point d r)
forall s a. s -> Getting a s a -> a
^.Getting
  (LSeq (1 + n) (Point d r))
  (BezierSpline n d r)
  (LSeq (1 + n) (Point d r))
forall (n1 :: Nat) (d1 :: Nat) r1 (n2 :: Nat) (d2 :: Nat) r2.
Iso
  (BezierSpline n1 d1 r1)
  (BezierSpline n2 d2 r2)
  (LSeq (1 + n1) (Point d1 r1))
  (LSeq (1 + n2) (Point d2 r2))
controlPoints
              in (Point d r
p,Point d r
q)




-- | Restrict a Bezier curve to the piece between parameters t < u in [0, 1].
subBezier     :: (KnownNat n, Arity d, Ord r, Num r)
              => r -> r -> BezierSpline n d r -> BezierSpline n d r
subBezier :: r -> r -> BezierSpline n d r -> BezierSpline n d r
subBezier r
t r
u = (BezierSpline n d r, BezierSpline n d r) -> BezierSpline n d r
forall a b. (a, b) -> a
fst ((BezierSpline n d r, BezierSpline n d r) -> BezierSpline n d r)
-> (BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r))
-> BezierSpline n d r
-> BezierSpline n d r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
forall (n :: Nat) (d :: Nat) r.
(KnownNat n, Arity d, Ord r, Num r) =>
r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
split r
u (BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r))
-> (BezierSpline n d r -> BezierSpline n d r)
-> BezierSpline n d r
-> (BezierSpline n d r, BezierSpline n d r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (BezierSpline n d r, BezierSpline n d r) -> BezierSpline n d r
forall a b. (a, b) -> b
snd ((BezierSpline n d r, BezierSpline n d r) -> BezierSpline n d r)
-> (BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r))
-> BezierSpline n d r
-> BezierSpline n d r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
forall (n :: Nat) (d :: Nat) r.
(KnownNat n, Arity d, Ord r, Num r) =>
r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
split r
t


-- | Compute the convex hull of the control polygon of a 2-dimensional Bezier curve.
--   Should also work in any dimension, but convex hull is not yet implemented.
convexHullB :: (Ord r, Fractional r) => BezierSpline n 2 r -> ConvexPolygon () r
convexHullB :: BezierSpline n 2 r -> ConvexPolygon () r
convexHullB = NonEmpty (Point 2 r :+ ()) -> ConvexPolygon () r
forall r p.
(Ord r, Num r) =>
NonEmpty (Point 2 r :+ p) -> ConvexPolygon p r
convexHull (NonEmpty (Point 2 r :+ ()) -> ConvexPolygon () r)
-> (BezierSpline n 2 r -> NonEmpty (Point 2 r :+ ()))
-> BezierSpline n 2 r
-> ConvexPolygon () r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. [Point 2 r :+ ()] -> NonEmpty (Point 2 r :+ ())
forall a. [a] -> NonEmpty a
NonEmpty.fromList ([Point 2 r :+ ()] -> NonEmpty (Point 2 r :+ ()))
-> (BezierSpline n 2 r -> [Point 2 r :+ ()])
-> BezierSpline n 2 r
-> NonEmpty (Point 2 r :+ ())
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (Point 2 r -> Point 2 r :+ ()) -> [Point 2 r] -> [Point 2 r :+ ()]
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Point 2 r -> Point 2 r :+ ()
forall a. a -> a :+ ()
ext ([Point 2 r] -> [Point 2 r :+ ()])
-> (BezierSpline n 2 r -> [Point 2 r])
-> BezierSpline n 2 r
-> [Point 2 r :+ ()]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. LSeq (1 + n) (Point 2 r) -> [Point 2 r]
forall (t :: * -> *) a. Foldable t => t a -> [a]
F.toList (LSeq (1 + n) (Point 2 r) -> [Point 2 r])
-> (BezierSpline n 2 r -> LSeq (1 + n) (Point 2 r))
-> BezierSpline n 2 r
-> [Point 2 r]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. BezierSpline n 2 r -> LSeq (1 + n) (Point 2 r)
forall (n :: Nat) (d :: Nat) r.
BezierSpline n d r -> LSeq (1 + n) (Point d r)
_controlPoints

--------------------------------------------------------------------------------

-- | Split a Bezier curve at time t in [0, 1] into two pieces.
split                 :: forall n d r. (KnownNat n, Arity d, Ord r, Num r)
                      => r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
split :: r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
split r
t BezierSpline n d r
b | r
t r -> r -> Bool
forall a. Ord a => a -> a -> Bool
< r
0     = String -> (BezierSpline n d r, BezierSpline n d r)
forall a. HasCallStack => String -> a
error String
"split: t < 0" -- ++ show t ++ " < 0"
          | r
t r -> r -> Bool
forall a. Ord a => a -> a -> Bool
> r
1     = String -> (BezierSpline n d r, BezierSpline n d r)
forall a. HasCallStack => String -> a
error String
"split: t > 1" -- ++ show t ++ " > 1"
          | Bool
otherwise = r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
forall (n :: Nat) (d :: Nat) r.
(KnownNat n, Arity d, Ord r, Num r) =>
r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
splitRaw r
t BezierSpline n d r
b


-- | Split without parameter check. If t outside [0,1], will actually extend the curve
--   rather than split it.
splitRaw     :: forall n d r. (KnownNat n, Arity d, Ord r, Num r)
             => r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
splitRaw :: r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
splitRaw r
t BezierSpline n d r
b = let n :: Int
n  = Natural -> Int
forall a b. (Integral a, Num b) => a -> b
fromIntegral (Natural -> Int) -> Natural -> Int
forall a b. (a -> b) -> a -> b
$ C n -> Natural
forall (n :: Nat) (proxy :: Nat -> *).
KnownNat n =>
proxy n -> Natural
natVal (C n
forall (n :: Nat). C n
C @n)
                   ps :: Seq (Point d r)
ps = r -> LSeq (1 + n) (Point d r) -> Seq (Point d r)
forall (d :: Nat) r (n :: Nat).
(Arity d, Ord r, Num r) =>
r -> LSeq n (Point d r) -> Seq (Point d r)
collect r
t (LSeq (1 + n) (Point d r) -> Seq (Point d r))
-> LSeq (1 + n) (Point d r) -> Seq (Point d r)
forall a b. (a -> b) -> a -> b
$ BezierSpline n d r
bBezierSpline n d r
-> Getting
     (LSeq (1 + n) (Point d r))
     (BezierSpline n d r)
     (LSeq (1 + n) (Point d r))
-> LSeq (1 + n) (Point d r)
forall s a. s -> Getting a s a -> a
^.Getting
  (LSeq (1 + n) (Point d r))
  (BezierSpline n d r)
  (LSeq (1 + n) (Point d r))
forall (n1 :: Nat) (d1 :: Nat) r1 (n2 :: Nat) (d2 :: Nat) r2.
Iso
  (BezierSpline n1 d1 r1)
  (BezierSpline n2 d2 r2)
  (LSeq (1 + n1) (Point d1 r1))
  (LSeq (1 + n2) (Point d2 r2))
controlPoints
               in ( Seq (Point d r) -> BezierSpline n d r
forall (d :: Nat) r (n :: Nat).
Seq (Point d r) -> BezierSpline n d r
fromPointSeq (Seq (Point d r) -> BezierSpline n d r)
-> (Seq (Point d r) -> Seq (Point d r))
-> Seq (Point d r)
-> BezierSpline n d r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Int -> Seq (Point d r) -> Seq (Point d r)
forall a. Int -> Seq a -> Seq a
Seq.take (Int
n Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
1) (Seq (Point d r) -> BezierSpline n d r)
-> Seq (Point d r) -> BezierSpline n d r
forall a b. (a -> b) -> a -> b
$ Seq (Point d r)
ps
                  , Seq (Point d r) -> BezierSpline n d r
forall (d :: Nat) r (n :: Nat).
Seq (Point d r) -> BezierSpline n d r
fromPointSeq (Seq (Point d r) -> BezierSpline n d r)
-> (Seq (Point d r) -> Seq (Point d r))
-> Seq (Point d r)
-> BezierSpline n d r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Int -> Seq (Point d r) -> Seq (Point d r)
forall a. Int -> Seq a -> Seq a
Seq.drop (Int
n Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
0) (Seq (Point d r) -> BezierSpline n d r)
-> Seq (Point d r) -> BezierSpline n d r
forall a b. (a -> b) -> a -> b
$ Seq (Point d r)
ps
                  )

-- | implementation of splitRaw
collect   :: (Arity d, Ord r, Num r) => r -> LSeq n (Point d r) -> Seq (Point d r)
collect :: r -> LSeq n (Point d r) -> Seq (Point d r)
collect r
t = Seq (Point d r) -> Seq (Point d r)
go (Seq (Point d r) -> Seq (Point d r))
-> (LSeq n (Point d r) -> Seq (Point d r))
-> LSeq n (Point d r)
-> Seq (Point d r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. LSeq n (Point d r) -> Seq (Point d r)
forall (n :: Nat) a. LSeq n a -> Seq a
LSeq.toSeq
  where
    go :: Seq (Point d r) -> Seq (Point d r)
go = \case
      ps :: Seq (Point d r)
ps@(Point d r
_ :<| Seq (Point d r)
Empty) -> Seq (Point d r)
ps
      ps :: Seq (Point d r)
ps@(Point d r
p :<| Seq (Point d r)
tl)    -> let (Seq (Point d r)
ini :|> Point d r
q) = Seq (Point d r)
ps in (Point d r
p Point d r -> Seq (Point d r) -> Seq (Point d r)
forall a. a -> Seq a -> Seq a
:<| Seq (Point d r) -> Seq (Point d r)
go ((Point d r -> Point d r -> Point d r)
-> Seq (Point d r) -> Seq (Point d r) -> Seq (Point d r)
forall a b c. (a -> b -> c) -> Seq a -> Seq b -> Seq c
Seq.zipWith Point d r -> Point d r -> Point d r
blend Seq (Point d r)
ini Seq (Point d r)
tl)) Seq (Point d r) -> Point d r -> Seq (Point d r)
forall a. Seq a -> a -> Seq a
:|> Point d r
q
      Seq (Point d r)
_                -> String -> Seq (Point d r)
forall a. HasCallStack => String -> a
error String
"collect: absurd"

    blend :: Point d r -> Point d r -> Point d r
blend Point d r
p Point d r
q = Point d r
p Point d r -> Diff (Point d) r -> Point d r
forall (p :: * -> *) a. (Affine p, Num a) => p a -> Diff p a -> p a
.+^ r
t r -> Vector d r -> Vector d r
forall (f :: * -> *) a. (Functor f, Num a) => a -> f a -> f a
*^ (Point d r
q Point d r -> Point d r -> Diff (Point d) r
forall (p :: * -> *) a. (Affine p, Num a) => p a -> p a -> Diff p a
.-. Point d r
p)

-- | Split a Bezier curve into many pieces.
--   Todo: filter out duplicate parameter values!
splitMany :: forall n d r. (KnownNat n, Arity d, Ord r, Fractional r)
          => [r] -> BezierSpline n d r -> [BezierSpline n d r]
splitMany :: [r] -> BezierSpline n d r -> [BezierSpline n d r]
splitMany = [r] -> BezierSpline n d r -> [BezierSpline n d r]
splitManySorted ([r] -> BezierSpline n d r -> [BezierSpline n d r])
-> ([r] -> [r])
-> [r]
-> BezierSpline n d r
-> [BezierSpline n d r]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. [r] -> [r]
forall a. Ord a => [a] -> [a]
sort ([r] -> [r]) -> ([r] -> [r]) -> [r] -> [r]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (r -> r) -> [r] -> [r]
forall a b. (a -> b) -> [a] -> [b]
map (String -> r -> r -> r -> r
forall r. Ord r => String -> r -> r -> r -> r
restrict String
"splitMany" r
0 r
1)

  where splitManySorted :: [r] -> BezierSpline n d r -> [BezierSpline n d r]
splitManySorted []       BezierSpline n d r
b' = [BezierSpline n d r
b']
        splitManySorted (r
t : [r]
ts) BezierSpline n d r
b' = let (BezierSpline n d r
a,BezierSpline n d r
c) = r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
forall (n :: Nat) (d :: Nat) r.
(KnownNat n, Arity d, Ord r, Num r) =>
r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
split r
t BezierSpline n d r
b'
                                      in BezierSpline n d r
a BezierSpline n d r -> [BezierSpline n d r] -> [BezierSpline n d r]
forall a. a -> [a] -> [a]
: [r] -> BezierSpline n d r -> [BezierSpline n d r]
splitManySorted ((r -> r) -> [r] -> [r]
forall a b. (a -> b) -> [a] -> [b]
map (r -> r -> r
rescale r
t) [r]
ts) BezierSpline n d r
c
        rescale :: r -> r -> r
        rescale :: r -> r -> r
rescale r
1 r
_ = r
1
        rescale r
t r
u = (r
u r -> r -> r
forall a. Num a => a -> a -> a
- r
t) r -> r -> r
forall a. Fractional a => a -> a -> a
/ (r
1 r -> r -> r
forall a. Num a => a -> a -> a
- r
t)


-- | Cut a Bezier curve into $x_i$-monotone pieces.
--   Can only be solved exactly for degree 4 or smaller.
--   Only gives rational result for degree 2 or smaller.
--   Currentlly implemented for degree 3.
splitMonotone :: (Arity d, Ord r, Enum r, Floating r) => Int -> BezierSpline 3 d r -> [BezierSpline 3 d r]
splitMonotone :: Int -> BezierSpline 3 d r -> [BezierSpline 3 d r]
splitMonotone Int
i BezierSpline 3 d r
b = [r] -> BezierSpline 3 d r -> [BezierSpline 3 d r]
forall (n :: Nat) (d :: Nat) r.
(KnownNat n, Arity d, Ord r, Fractional r) =>
[r] -> BezierSpline n d r -> [BezierSpline n d r]
splitMany (Int -> BezierSpline 3 d r -> [r]
forall (d :: Nat) r.
(Arity d, Ord r, Enum r, Floating r) =>
Int -> BezierSpline 3 d r -> [r]
locallyExtremalParameters Int
i BezierSpline 3 d r
b) BezierSpline 3 d r
b

{-
type family RealTypeConstraint (n :: Nat) (r :: *) :: Constraint where
  RealTypeConstraint 1 r = (Fractional r)
  RealTypeConstraint 2 r = (Fractional r)
  RealTypeConstraint 3 r = (Floating r)
  RealTypeConstraint 4 r = (Floating r)
  RealTypeConstraint 5 r = (Floating r)
  RealTypeConstraint n r = TypeError ""
-}

-- | Report all parameter values at which the derivative of the $i$th coordinate is 0.
locallyExtremalParameters         :: (Arity d, Ord r, Enum r, Floating r)
                                  => Int -> BezierSpline 3 d r -> [r]
locallyExtremalParameters :: Int -> BezierSpline 3 d r -> [r]
locallyExtremalParameters Int
i BezierSpline 3 d r
curve =
  let [r
x1, r
x2, r
x3, r
x4] = (Point d r -> r) -> [Point d r] -> [r]
forall a b. (a -> b) -> [a] -> [b]
map (Getting r (Point d r) r -> Point d r -> r
forall s (m :: * -> *) a. MonadReader s m => Getting a s a -> m a
view (Getting r (Point d r) r -> Point d r -> r)
-> Getting r (Point d r) r -> Point d r -> r
forall a b. (a -> b) -> a -> b
$ Int -> Lens' (Point d r) r
forall (d :: Nat) (p :: Nat -> * -> *) r.
(Arity d, AsAPoint p) =>
Int -> Lens' (p d r) r
unsafeCoord Int
i) ([Point d r] -> [r]) -> [Point d r] -> [r]
forall a b. (a -> b) -> a -> b
$ LSeq 4 (Point d r) -> [Point d r]
forall (t :: * -> *) a. Foldable t => t a -> [a]
F.toList (LSeq 4 (Point d r) -> [Point d r])
-> LSeq 4 (Point d r) -> [Point d r]
forall a b. (a -> b) -> a -> b
$ BezierSpline 3 d r -> LSeq (1 + 3) (Point d r)
forall (n :: Nat) (d :: Nat) r.
BezierSpline n d r -> LSeq (1 + n) (Point d r)
_controlPoints BezierSpline 3 d r
curve
      a :: r
a = r
3 r -> r -> r
forall a. Num a => a -> a -> a
* r
x4 r -> r -> r
forall a. Num a => a -> a -> a
-  r
9 r -> r -> r
forall a. Num a => a -> a -> a
* r
x3 r -> r -> r
forall a. Num a => a -> a -> a
+ r
9 r -> r -> r
forall a. Num a => a -> a -> a
* r
x2 r -> r -> r
forall a. Num a => a -> a -> a
- r
3 r -> r -> r
forall a. Num a => a -> a -> a
* r
x1
      b :: r
b = r
6 r -> r -> r
forall a. Num a => a -> a -> a
* r
x1 r -> r -> r
forall a. Num a => a -> a -> a
- r
12 r -> r -> r
forall a. Num a => a -> a -> a
* r
x2 r -> r -> r
forall a. Num a => a -> a -> a
+ r
6 r -> r -> r
forall a. Num a => a -> a -> a
* r
x3
      c :: r
c = r
3 r -> r -> r
forall a. Num a => a -> a -> a
* r
x2 r -> r -> r
forall a. Num a => a -> a -> a
-  r
3 r -> r -> r
forall a. Num a => a -> a -> a
* r
x1
  in (r -> Bool) -> [r] -> [r]
forall a. (a -> Bool) -> [a] -> [a]
filter (\r
j -> r
0 r -> r -> Bool
forall a. Ord a => a -> a -> Bool
<= r
j Bool -> Bool -> Bool
&& r
j r -> r -> Bool
forall a. Ord a => a -> a -> Bool
<= r
1) ([r] -> [r]) -> [r] -> [r]
forall a b. (a -> b) -> a -> b
$ r -> r -> r -> [r]
forall r. (Ord r, Enum r, Floating r) => r -> r -> r -> [r]
solveQuadraticEquation r
a r
b r
c


-- | Subdivide a curve based on a sequence of points.
--   Assumes these points are all supposed to lie on the curve, and
--   snaps endpoints of pieces to these points.
--   (higher dimensions would work, but depends on convex hull)
splitByPoints :: (KnownNat n, Ord r, RealFrac r)
              => r -> [Point 2 r] -> BezierSpline n 2 r -> [BezierSpline n 2 r]
splitByPoints :: r -> [Point 2 r] -> BezierSpline n 2 r -> [BezierSpline n 2 r]
splitByPoints r
treshold [Point 2 r]
points BezierSpline n 2 r
curve =
  let a :: Point 2 r
a      = (Point 2 r, Point 2 r) -> Point 2 r
forall a b. (a, b) -> a
fst ((Point 2 r, Point 2 r) -> Point 2 r)
-> (Point 2 r, Point 2 r) -> Point 2 r
forall a b. (a -> b) -> a -> b
$ BezierSpline n 2 r -> (Point 2 r, Point 2 r)
forall (n :: Nat) (d :: Nat) r.
BezierSpline n d r -> (Point d r, Point d r)
endPoints BezierSpline n 2 r
curve
      b :: Point 2 r
b      = (Point 2 r, Point 2 r) -> Point 2 r
forall a b. (a, b) -> b
snd ((Point 2 r, Point 2 r) -> Point 2 r)
-> (Point 2 r, Point 2 r) -> Point 2 r
forall a b. (a -> b) -> a -> b
$ BezierSpline n 2 r -> (Point 2 r, Point 2 r)
forall (n :: Nat) (d :: Nat) r.
BezierSpline n d r -> (Point d r, Point d r)
endPoints BezierSpline n 2 r
curve
      intern :: [Point 2 r]
intern = (Point 2 r -> Bool) -> [Point 2 r] -> [Point 2 r]
forall a. (a -> Bool) -> [a] -> [a]
filter (\Point 2 r
p -> Point 2 r
p Point 2 r -> Point 2 r -> Bool
forall a. Eq a => a -> a -> Bool
/= Point 2 r
a Bool -> Bool -> Bool
&& Point 2 r
p Point 2 r -> Point 2 r -> Bool
forall a. Eq a => a -> a -> Bool
/= Point 2 r
b) [Point 2 r]
points
      times :: [r]
times  = (Point 2 r -> r) -> [Point 2 r] -> [r]
forall a b. (a -> b) -> [a] -> [b]
map (r -> BezierSpline n 2 r -> Point 2 r -> r
forall (n :: Nat) r.
(KnownNat n, Ord r, RealFrac r) =>
r -> BezierSpline n 2 r -> Point 2 r -> r
parameterOf r
treshold BezierSpline n 2 r
curve) [Point 2 r]
intern
      tipos :: [(r, Point 2 r)]
tipos  = [(r, Point 2 r)] -> [(r, Point 2 r)]
forall a. Ord a => [a] -> [a]
sort ([(r, Point 2 r)] -> [(r, Point 2 r)])
-> [(r, Point 2 r)] -> [(r, Point 2 r)]
forall a b. (a -> b) -> a -> b
$ [r] -> [Point 2 r] -> [(r, Point 2 r)]
forall a b. [a] -> [b] -> [(a, b)]
zip [r]
times [Point 2 r]
intern
      pieces :: [BezierSpline n 2 r]
pieces = [r] -> BezierSpline n 2 r -> [BezierSpline n 2 r]
forall (n :: Nat) (d :: Nat) r.
(KnownNat n, Arity d, Ord r, Fractional r) =>
[r] -> BezierSpline n d r -> [BezierSpline n d r]
splitMany (((r, Point 2 r) -> r) -> [(r, Point 2 r)] -> [r]
forall a b. (a -> b) -> [a] -> [b]
map (r, Point 2 r) -> r
forall a b. (a, b) -> a
fst [(r, Point 2 r)]
tipos) BezierSpline n 2 r
curve
      stapts :: [Point 2 r]
stapts = Point 2 r
a Point 2 r -> [Point 2 r] -> [Point 2 r]
forall a. a -> [a] -> [a]
: ((r, Point 2 r) -> Point 2 r) -> [(r, Point 2 r)] -> [Point 2 r]
forall a b. (a -> b) -> [a] -> [b]
map (r, Point 2 r) -> Point 2 r
forall a b. (a, b) -> b
snd [(r, Point 2 r)]
tipos
      endpts :: [Point 2 r]
endpts = ((r, Point 2 r) -> Point 2 r) -> [(r, Point 2 r)] -> [Point 2 r]
forall a b. (a -> b) -> [a] -> [b]
map (r, Point 2 r) -> Point 2 r
forall a b. (a, b) -> b
snd [(r, Point 2 r)]
tipos [Point 2 r] -> [Point 2 r] -> [Point 2 r]
forall a. [a] -> [a] -> [a]
++ [Point 2 r
b]
  in (Point 2 r
 -> Point 2 r -> BezierSpline n 2 r -> BezierSpline n 2 r)
-> [Point 2 r]
-> [Point 2 r]
-> [BezierSpline n 2 r]
-> [BezierSpline n 2 r]
forall a b c d. (a -> b -> c -> d) -> [a] -> [b] -> [c] -> [d]
zipWith3 Point 2 r -> Point 2 r -> BezierSpline n 2 r -> BezierSpline n 2 r
forall (n :: Nat) (d :: Nat) r.
(KnownNat n, Arity d, Ord r, Fractional r) =>
Point d r -> Point d r -> BezierSpline n d r -> BezierSpline n d r
snapEndpoints [Point 2 r]
stapts [Point 2 r]
endpts [BezierSpline n 2 r]
pieces

--------------------------------------------------------------------------------

-- | Extend a Bezier curve to a parameter value t outside the interval [0,1].
--   For t < 0, returns a Bezier representation of the section of the underlying curve
--   from parameter value t until paramater value 0. For t > 1, the same from 1 to t.
--
-- pre: t outside [0,1]
extension :: forall n d r. (KnownNat n, Arity d, Ord r, Num r)
      => r -> BezierSpline n d r -> BezierSpline n d r
extension :: r -> BezierSpline n d r -> BezierSpline n d r
extension r
t BezierSpline n d r
b | r
t r -> r -> Bool
forall a. Ord a => a -> a -> Bool
> r
0 Bool -> Bool -> Bool
&& r
t r -> r -> Bool
forall a. Ord a => a -> a -> Bool
< r
1        = String -> BezierSpline n d r
forall a. HasCallStack => String -> a
error String
"extension: 0 < t < 1" -- ++ show t ++ " < 1"
              | r
t r -> r -> Bool
forall a. Ord a => a -> a -> Bool
<= r
0                = (BezierSpline n d r, BezierSpline n d r) -> BezierSpline n d r
forall a b. (a, b) -> a
fst ((BezierSpline n d r, BezierSpline n d r) -> BezierSpline n d r)
-> (BezierSpline n d r, BezierSpline n d r) -> BezierSpline n d r
forall a b. (a -> b) -> a -> b
$ r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
forall (n :: Nat) (d :: Nat) r.
(KnownNat n, Arity d, Ord r, Num r) =>
r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
splitRaw r
t BezierSpline n d r
b
              | Bool
otherwise {- t >= 1-} = (BezierSpline n d r, BezierSpline n d r) -> BezierSpline n d r
forall a b. (a, b) -> b
snd ((BezierSpline n d r, BezierSpline n d r) -> BezierSpline n d r)
-> (BezierSpline n d r, BezierSpline n d r) -> BezierSpline n d r
forall a b. (a -> b) -> a -> b
$ r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
forall (n :: Nat) (d :: Nat) r.
(KnownNat n, Arity d, Ord r, Num r) =>
r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
splitRaw r
t BezierSpline n d r
b

-- | Extend a Bezier curve to a parameter value t outside the interval [0,1].
--   For t < 0, returns a Bezier representation of the section of the underlying curve
--   from parameter value t until paramater value 1. For t > 1, the same from 0 to t.
--
-- pre: t outside [0,1]
extend :: forall n d r. (KnownNat n, Arity d, Ord r, Num r)
      => r -> BezierSpline n d r -> BezierSpline n d r
extend :: r -> BezierSpline n d r -> BezierSpline n d r
extend r
t BezierSpline n d r
b | r
t r -> r -> Bool
forall a. Ord a => a -> a -> Bool
> r
0 Bool -> Bool -> Bool
&& r
t r -> r -> Bool
forall a. Ord a => a -> a -> Bool
< r
1         = String -> BezierSpline n d r
forall a. HasCallStack => String -> a
error String
"extend: 0 < t < 1" -- ++ show t ++ " < 1"
           | r
t r -> r -> Bool
forall a. Ord a => a -> a -> Bool
<= r
0                 = (BezierSpline n d r, BezierSpline n d r) -> BezierSpline n d r
forall a b. (a, b) -> b
snd ((BezierSpline n d r, BezierSpline n d r) -> BezierSpline n d r)
-> (BezierSpline n d r, BezierSpline n d r) -> BezierSpline n d r
forall a b. (a -> b) -> a -> b
$ r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
forall (n :: Nat) (d :: Nat) r.
(KnownNat n, Arity d, Ord r, Num r) =>
r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
splitRaw r
t BezierSpline n d r
b
           | Bool
otherwise {- t >= 1 -} = (BezierSpline n d r, BezierSpline n d r) -> BezierSpline n d r
forall a b. (a, b) -> a
fst ((BezierSpline n d r, BezierSpline n d r) -> BezierSpline n d r)
-> (BezierSpline n d r, BezierSpline n d r) -> BezierSpline n d r
forall a b. (a -> b) -> a -> b
$ r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
forall (n :: Nat) (d :: Nat) r.
(KnownNat n, Arity d, Ord r, Num r) =>
r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
splitRaw r
t BezierSpline n d r
b


-- | Extend a Bezier curve to a point not on the curve, but on / close
--   to the extended underlying curve.
growTo              :: (KnownNat n, Arity d, Ord r, Fractional r)
                    => r -> Point d r -> BezierSpline n d r -> BezierSpline n d r
growTo :: r -> Point d r -> BezierSpline n d r -> BezierSpline n d r
growTo r
treshold Point d r
p BezierSpline n d r
b =
  let t :: r
t = r -> BezierSpline n d r -> Point d r -> r
forall (d :: Nat) (n :: Nat) r.
(Arity d, KnownNat n, Ord r, Fractional r) =>
r -> BezierSpline n d r -> Point d r -> r
extendedParameterOf r
treshold BezierSpline n d r
b Point d r
p
      r :: BezierSpline n d r
r | r
t r -> r -> Bool
forall a. Ord a => a -> a -> Bool
< r
0 = r -> BezierSpline n d r -> BezierSpline n d r
forall (n :: Nat) (d :: Nat) r.
(KnownNat n, Arity d, Ord r, Num r) =>
r -> BezierSpline n d r -> BezierSpline n d r
extend r
t BezierSpline n d r
b
        | r
t r -> r -> Bool
forall a. Ord a => a -> a -> Bool
> r
1 = r -> BezierSpline n d r -> BezierSpline n d r
forall (n :: Nat) (d :: Nat) r.
(KnownNat n, Arity d, Ord r, Num r) =>
r -> BezierSpline n d r -> BezierSpline n d r
extend r
t BezierSpline n d r
b
        | Bool
otherwise = BezierSpline n d r
b
  in BezierSpline n d r
r

{-

-- | Tries to fit a degree n Bezier curve through a list of points, with error parameter eps.
--   Either returns an appropriate curve, or fails.
fit :: r -> [Point 2 r] -> Maybe (Bezier n d r)
fit eps pts

-}


--------------------------------------------------------------------------------

-- | Merge two Bezier pieces. Assumes they can be merged into a single piece of the same degree
--   (as would e.g. be the case for the result of a 'split' operation).
--   Does not test whether this is the case!
merge                :: (KnownNat n, Arity d, Ord r, Fractional r)
                     => r -> BezierSpline n d r -> BezierSpline n d r -> BezierSpline n d r
merge :: r -> BezierSpline n d r -> BezierSpline n d r -> BezierSpline n d r
merge r
treshold BezierSpline n d r
b1 BezierSpline n d r
b2 = let (Point d r
p1, Point d r
q1) = BezierSpline n d r -> (Point d r, Point d r)
forall (n :: Nat) (d :: Nat) r.
BezierSpline n d r -> (Point d r, Point d r)
endPoints BezierSpline n d r
b1
                           (Point d r
p2, Point d r
q2) = BezierSpline n d r -> (Point d r, Point d r)
forall (n :: Nat) (d :: Nat) r.
BezierSpline n d r -> (Point d r, Point d r)
endPoints BezierSpline n d r
b2
                           result :: BezierSpline n d r
result | Point d r
q1 Point d r -> Point d r -> Bool
forall a. Eq a => a -> a -> Bool
/= Point d r
p2 = String -> BezierSpline n d r
forall a. HasCallStack => String -> a
error String
"merge: something is wrong, maybe need to flip one of the curves?"
                                  | Bool
otherwise = Point d r -> Point d r -> BezierSpline n d r -> BezierSpline n d r
forall (n :: Nat) (d :: Nat) r.
(KnownNat n, Arity d, Ord r, Fractional r) =>
Point d r -> Point d r -> BezierSpline n d r -> BezierSpline n d r
snapEndpoints Point d r
p1 Point d r
q2 (BezierSpline n d r -> BezierSpline n d r)
-> BezierSpline n d r -> BezierSpline n d r
forall a b. (a -> b) -> a -> b
$ r -> Point d r -> BezierSpline n d r -> BezierSpline n d r
forall (n :: Nat) (d :: Nat) r.
(KnownNat n, Arity d, Ord r, Fractional r) =>
r -> Point d r -> BezierSpline n d r -> BezierSpline n d r
growTo r
treshold Point d r
p1 BezierSpline n d r
b2
                       in BezierSpline n d r
result

-- need distance function between polyBeziers...


--------------------------------------------------------------------------------


-- | Approximate Bezier curve by Polyline with given resolution.  That
-- is, every point on the approximation will have distance at most res
-- to the Bezier curve.
approximate     :: (KnownNat n, Arity d, Ord r, Fractional r)
                => r -> BezierSpline n d r -> PolyLine d () r
approximate :: r -> BezierSpline n d r -> PolyLine d () r
approximate r
res = LSeq 2 (Point d r :+ ()) -> PolyLine d () r
forall (d :: Nat) p r. LSeq 2 (Point d r :+ p) -> PolyLine d p r
PolyLine (LSeq 2 (Point d r :+ ()) -> PolyLine d () r)
-> (BezierSpline n d r -> LSeq 2 (Point d r :+ ()))
-> BezierSpline n d r
-> PolyLine d () r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (Point d r -> Point d r :+ ())
-> LSeq 2 (Point d r) -> LSeq 2 (Point d r :+ ())
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Point d r -> Point d r :+ ()
forall a. a -> a :+ ()
ext (LSeq 2 (Point d r) -> LSeq 2 (Point d r :+ ()))
-> (BezierSpline n d r -> LSeq 2 (Point d r))
-> BezierSpline n d r
-> LSeq 2 (Point d r :+ ())
forall b c a. (b -> c) -> (a -> b) -> a -> c
. r -> BezierSpline n d r -> LSeq 2 (Point d r)
forall (n :: Nat) (d :: Nat) r.
(KnownNat n, Arity d, Ord r, Fractional r) =>
r -> BezierSpline n d r -> LSeq 2 (Point d r)
approximate' r
res

-- | implementation of approximate; returns the polyline as an LSeq
approximate'     :: (KnownNat n, Arity d, Ord r, Fractional r)
                 => r -> BezierSpline n d r -> LSeq 2 (Point d r)
approximate' :: r -> BezierSpline n d r -> LSeq 2 (Point d r)
approximate' r
res = LSeq 0 (Point d r) -> LSeq 2 (Point d r)
forall (n :: Nat) (m :: Nat) a. LSeq m a -> LSeq n a
LSeq.promise (LSeq 0 (Point d r) -> LSeq 2 (Point d r))
-> (BezierSpline n d r -> LSeq 0 (Point d r))
-> BezierSpline n d r
-> LSeq 2 (Point d r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Seq (Point d r) -> LSeq 0 (Point d r)
forall a. Seq a -> LSeq 0 a
LSeq.fromSeq (Seq (Point d r) -> LSeq 0 (Point d r))
-> (BezierSpline n d r -> Seq (Point d r))
-> BezierSpline n d r
-> LSeq 0 (Point d r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. BezierSpline n d r -> Seq (Point d r)
go
  where
    go :: BezierSpline n d r -> Seq (Point d r)
go BezierSpline n d r
b | r -> BezierSpline n d r -> Bool
forall (n :: Nat) (d :: Nat) r.
(KnownNat n, Arity d, Ord r, Fractional r) =>
r -> BezierSpline n d r -> Bool
flat r
res BezierSpline n d r
b = let (Point d r
p,Point d r
q) = BezierSpline n d r -> (Point d r, Point d r)
forall (n :: Nat) (d :: Nat) r.
BezierSpline n d r -> (Point d r, Point d r)
endPoints BezierSpline n d r
b in [Point d r] -> Seq (Point d r)
forall a. [a] -> Seq a
Seq.fromList [Point d r
p,Point d r
q]
         | Bool
otherwise  = let (BezierSpline n d r
b1, BezierSpline n d r
b2) = r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
forall (n :: Nat) (d :: Nat) r.
(KnownNat n, Arity d, Ord r, Num r) =>
r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
split r
0.5 BezierSpline n d r
b in BezierSpline n d r -> Seq (Point d r)
go BezierSpline n d r
b1 Seq (Point d r) -> Seq (Point d r) -> Seq (Point d r)
forall a. Semigroup a => a -> a -> a
<> Int -> Seq (Point d r) -> Seq (Point d r)
forall a. Int -> Seq a -> Seq a
Seq.drop Int
1 (BezierSpline n d r -> Seq (Point d r)
go BezierSpline n d r
b2)

-- | Test whether a Bezier curve can be approximated by a single line segment,
--   given the resolution parameter.
flat :: (KnownNat n, Arity d, Ord r, Fractional r) => r -> BezierSpline n d r -> Bool
flat :: r -> BezierSpline n d r -> Bool
flat r
r BezierSpline n d r
b = let p :: Point d r
p = (Point d r, Point d r) -> Point d r
forall a b. (a, b) -> a
fst ((Point d r, Point d r) -> Point d r)
-> (Point d r, Point d r) -> Point d r
forall a b. (a -> b) -> a -> b
$ BezierSpline n d r -> (Point d r, Point d r)
forall (n :: Nat) (d :: Nat) r.
BezierSpline n d r -> (Point d r, Point d r)
endPoints BezierSpline n d r
b
               q :: Point d r
q = (Point d r, Point d r) -> Point d r
forall a b. (a, b) -> b
snd ((Point d r, Point d r) -> Point d r)
-> (Point d r, Point d r) -> Point d r
forall a b. (a -> b) -> a -> b
$ BezierSpline n d r -> (Point d r, Point d r)
forall (n :: Nat) (d :: Nat) r.
BezierSpline n d r -> (Point d r, Point d r)
endPoints BezierSpline n d r
b
               s :: LineSegment d () r
s = (Point d r :+ ()) -> (Point d r :+ ()) -> LineSegment d () r
forall (d :: Nat) r p.
(Point d r :+ p) -> (Point d r :+ p) -> LineSegment d p r
ClosedLineSegment (Point d r
p Point d r -> () -> Point d r :+ ()
forall core extra. core -> extra -> core :+ extra
:+ ()) (Point d r
q Point d r -> () -> Point d r :+ ()
forall core extra. core -> extra -> core :+ extra
:+ ())
               e :: r -> Bool
e r
t = Point
  (Dimension (LineSegment d () r)) (NumType (LineSegment d () r))
-> LineSegment d () r -> NumType (LineSegment d () r)
forall g.
(HasSquaredEuclideanDistance g, Num (NumType g),
 Arity (Dimension g)) =>
Point (Dimension g) (NumType g) -> g -> NumType g
squaredEuclideanDistTo (BezierSpline n d r -> r -> Point d r
forall (d :: Nat) r (n :: Nat).
(Arity d, Eq r, Num r) =>
BezierSpline n d r -> r -> Point d r
evaluate BezierSpline n d r
b r
t) LineSegment d () r
s r -> r -> Bool
forall a. Ord a => a -> a -> Bool
< r
r r -> Integer -> r
forall a b. (Num a, Integral b) => a -> b -> a
^ Integer
2
           in Point d r -> Point d r -> r
forall (p :: * -> *) a.
(Affine p, Foldable (Diff p), Num a) =>
p a -> p a -> a
qdA Point d r
p Point d r
q r -> r -> Bool
forall a. Ord a => a -> a -> Bool
< r
r r -> Integer -> r
forall a b. (Num a, Integral b) => a -> b -> a
^ Integer
2 Bool -> Bool -> Bool
|| (r -> Bool) -> [r] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
all r -> Bool
e [r
0.1, r
0.2, r
0.3, r
0.4, r
0.5, r
0.6, r
0.7, r
0.8, r
0.9]

-- seems this is now covered by approximate
--
--
-- -- | Approximate curve as line segments where no point on the curve is further away
-- --   from the nearest line segment than the given tolerance.
-- lineApproximate :: (Ord r, Fractional r) => r -> BezierSpline 3 2 r -> [Point 2 r]
-- lineApproximate eps bezier
--   | colinear eps bezier =
--     [ bezier^.controlPoints.to LSeq.head
--     , bezier^.controlPoints.to LSeq.last ]
--   | otherwise =
--     let (b1, b2) = split 0.5 bezier
--     in lineApproximate eps b1 ++ tail (lineApproximate eps b2)

-- If both control points are on the same side of the straight line from the start and end
-- points then the curve is guaranteed to be within 3/4 of the distance from the straight line
-- to the furthest control point.
-- Otherwise, if the control points are on either side of the straight line, the curve is
-- guaranteed to be within 4/9 of the maximum distance from the straight line to a control
-- point.
-- Also: 3/4 * sqrt(v) = sqrt (9/16 * v)
--       4/9 * sqrt(v) = sqrt (16/81 * v)
-- So: 3/4 * sqrt(v) < eps =>
--     sqrt(9/16 * v) < eps =>
--     9/16*v < eps*eps
-- | Return True if the curve is definitely completely covered by a line of thickness
--   twice the given tolerance. May return false negatives but not false positives.
colinear :: (Ord r, Fractional r) => r -> BezierSpline 3 2 r -> Bool
colinear :: r -> BezierSpline 3 2 r -> Bool
colinear r
eps (Bezier3 !Point 2 r
a !Point 2 r
b !Point 2 r
c !Point 2 r
d) = r
sqBound r -> r -> Bool
forall a. Ord a => a -> a -> Bool
< r
epsr -> r -> r
forall a. Num a => a -> a -> a
*r
eps
  where ld :: Point 2 r -> r
ld = (Point 2 r -> Line 2 r -> r) -> Line 2 r -> Point 2 r -> r
forall a b c. (a -> b -> c) -> b -> a -> c
flip Point 2 r -> Line 2 r -> r
forall g.
(HasSquaredEuclideanDistance g, Num (NumType g),
 Arity (Dimension g)) =>
Point (Dimension g) (NumType g) -> g -> NumType g
squaredEuclideanDistTo (Point 2 r -> Point 2 r -> Line 2 r
forall r (d :: Nat).
(Num r, Arity d) =>
Point d r -> Point d r -> Line d r
lineThrough Point 2 r
a Point 2 r
d)
        sameSide :: Bool
sameSide = Point 2 r -> Point 2 r -> Point 2 r -> CCW
forall r.
(Ord r, Num r) =>
Point 2 r -> Point 2 r -> Point 2 r -> CCW
ccw Point 2 r
a Point 2 r
d Point 2 r
b CCW -> CCW -> Bool
forall a. Eq a => a -> a -> Bool
== Point 2 r -> Point 2 r -> Point 2 r -> CCW
forall r.
(Ord r, Num r) =>
Point 2 r -> Point 2 r -> Point 2 r -> CCW
ccw Point 2 r
a Point 2 r
d Point 2 r
c
        maxDist :: r
maxDist = r -> r -> r
forall a. Ord a => a -> a -> a
max (Point 2 r -> r
ld Point 2 r
b) (Point 2 r -> r
ld Point 2 r
c)
        sqBound :: r
sqBound
          | Bool
sameSide  = r
9r -> r -> r
forall a. Fractional a => a -> a -> a
/r
16  r -> r -> r
forall a. Num a => a -> a -> a
* r
maxDist
          | Bool
otherwise = r
16r -> r -> r
forall a. Fractional a => a -> a -> a
/r
81 r -> r -> r
forall a. Num a => a -> a -> a
* r
maxDist

--------------------------------------------------------------------------------

-- general d depends on convex hull
-- parameterOf :: (Arity d, Ord r, Fractional r) => BezierSpline n d r -> Point d r -> r
--
-- | Given a point on (or within distance treshold to) a Bezier curve, return the parameter value
--   of some point on the curve within distance treshold from p.
--   For points farther than treshold from the curve, the function will attempt to return the
--   parameter value of an approximate locally closest point to the input point, but no guarantees.
parameterOf :: (KnownNat n, Ord r, RealFrac r) => r -> BezierSpline n 2 r -> Point 2 r -> r
parameterOf :: r -> BezierSpline n 2 r -> Point 2 r -> r
parameterOf r
treshold BezierSpline n 2 r
b Point 2 r
p | r -> Point 2 r -> Point 2 r -> Bool
forall (d :: Nat) r.
(Arity d, Ord r, Fractional r) =>
r -> Point d r -> Point d r -> Bool
closeEnough r
treshold Point 2 r
p (Point 2 r -> Bool) -> Point 2 r -> Bool
forall a b. (a -> b) -> a -> b
$ (Point 2 r, Point 2 r) -> Point 2 r
forall a b. (a, b) -> a
fst ((Point 2 r, Point 2 r) -> Point 2 r)
-> (Point 2 r, Point 2 r) -> Point 2 r
forall a b. (a -> b) -> a -> b
$ BezierSpline n 2 r -> (Point 2 r, Point 2 r)
forall (n :: Nat) (d :: Nat) r.
BezierSpline n d r -> (Point d r, Point d r)
endPoints BezierSpline n 2 r
b = r
0
                         | r -> Point 2 r -> Point 2 r -> Bool
forall (d :: Nat) r.
(Arity d, Ord r, Fractional r) =>
r -> Point d r -> Point d r -> Bool
closeEnough r
treshold Point 2 r
p (Point 2 r -> Bool) -> Point 2 r -> Bool
forall a b. (a -> b) -> a -> b
$ (Point 2 r, Point 2 r) -> Point 2 r
forall a b. (a, b) -> b
snd ((Point 2 r, Point 2 r) -> Point 2 r)
-> (Point 2 r, Point 2 r) -> Point 2 r
forall a b. (a -> b) -> a -> b
$ BezierSpline n 2 r -> (Point 2 r, Point 2 r)
forall (n :: Nat) (d :: Nat) r.
BezierSpline n d r -> (Point d r, Point d r)
endPoints BezierSpline n 2 r
b = r
1
                         | Bool
otherwise = r -> BezierSpline n 2 r -> Point 2 r -> r
forall (n :: Nat) r.
(KnownNat n, Ord r, RealFrac r) =>
r -> BezierSpline n 2 r -> Point 2 r -> r
parameterInterior r
treshold BezierSpline n 2 r
b Point 2 r
p

-- parameterInterior is slow, look into algebraic solution?

-- general d depends on convex hull
parameterInterior :: (KnownNat n, Ord r, RealFrac r) => r -> BezierSpline n 2 r -> Point 2 r -> r
parameterInterior :: r -> BezierSpline n 2 r -> Point 2 r -> r
parameterInterior r
treshold BezierSpline n 2 r
b Point 2 r
p | [Point 2 r] -> r
forall r. (Ord r, RealFrac r) => [Point 2 r] -> r
sqrad (LSeq (1 + n) (Point 2 r) -> [Point 2 r]
forall (t :: * -> *) a. Foldable t => t a -> [a]
F.toList (LSeq (1 + n) (Point 2 r) -> [Point 2 r])
-> LSeq (1 + n) (Point 2 r) -> [Point 2 r]
forall a b. (a -> b) -> a -> b
$ Getting
  (LSeq (1 + n) (Point 2 r))
  (BezierSpline n 2 r)
  (LSeq (1 + n) (Point 2 r))
-> BezierSpline n 2 r -> LSeq (1 + n) (Point 2 r)
forall s (m :: * -> *) a. MonadReader s m => Getting a s a -> m a
view Getting
  (LSeq (1 + n) (Point 2 r))
  (BezierSpline n 2 r)
  (LSeq (1 + n) (Point 2 r))
forall (n1 :: Nat) (d1 :: Nat) r1 (n2 :: Nat) (d2 :: Nat) r2.
Iso
  (BezierSpline n1 d1 r1)
  (BezierSpline n2 d2 r2)
  (LSeq (1 + n1) (Point d1 r1))
  (LSeq (1 + n2) (Point d2 r2))
controlPoints BezierSpline n 2 r
b) r -> r -> Bool
forall a. Ord a => a -> a -> Bool
< (r
0.5 r -> r -> r
forall a. Num a => a -> a -> a
* r
treshold)r -> Integer -> r
forall a b. (Num a, Integral b) => a -> b -> a
^Integer
2 = r
0.5
                               | Bool
otherwise =
  let (BezierSpline n 2 r
b1, BezierSpline n 2 r
b2) = r -> BezierSpline n 2 r -> (BezierSpline n 2 r, BezierSpline n 2 r)
forall (n :: Nat) (d :: Nat) r.
(KnownNat n, Arity d, Ord r, Num r) =>
r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
split r
0.5 BezierSpline n 2 r
b
      recurse1 :: r
recurse1 =       r
0.5 r -> r -> r
forall a. Num a => a -> a -> a
* r -> BezierSpline n 2 r -> Point 2 r -> r
forall (n :: Nat) r.
(KnownNat n, Ord r, RealFrac r) =>
r -> BezierSpline n 2 r -> Point 2 r -> r
parameterInterior r
treshold BezierSpline n 2 r
b1 Point 2 r
p
      recurse2 :: r
recurse2 = r
0.5 r -> r -> r
forall a. Num a => a -> a -> a
+ r
0.5 r -> r -> r
forall a. Num a => a -> a -> a
* r -> BezierSpline n 2 r -> Point 2 r -> r
forall (n :: Nat) r.
(KnownNat n, Ord r, RealFrac r) =>
r -> BezierSpline n 2 r -> Point 2 r -> r
parameterInterior r
treshold BezierSpline n 2 r
b2 Point 2 r
p
      chb1 :: SimplePolygon () r
chb1     = ConvexPolygon () r -> SimplePolygon () r
forall p r. ConvexPolygon p r -> SimplePolygon p r
_simplePolygon (ConvexPolygon () r -> SimplePolygon () r)
-> ConvexPolygon () r -> SimplePolygon () r
forall a b. (a -> b) -> a -> b
$ BezierSpline n 2 r -> ConvexPolygon () r
forall r (n :: Nat).
(Ord r, Fractional r) =>
BezierSpline n 2 r -> ConvexPolygon () r
convexHullB BezierSpline n 2 r
b1
      chb2 :: SimplePolygon () r
chb2     = ConvexPolygon () r -> SimplePolygon () r
forall p r. ConvexPolygon p r -> SimplePolygon p r
_simplePolygon (ConvexPolygon () r -> SimplePolygon () r)
-> ConvexPolygon () r -> SimplePolygon () r
forall a b. (a -> b) -> a -> b
$ BezierSpline n 2 r -> ConvexPolygon () r
forall r (n :: Nat).
(Ord r, Fractional r) =>
BezierSpline n 2 r -> ConvexPolygon () r
convexHullB BezierSpline n 2 r
b2
      in1 :: Bool
in1      = Point
  (Dimension (SimplePolygon () r)) (NumType (SimplePolygon () r))
-> SimplePolygon () r -> NumType (SimplePolygon () r)
forall g.
(HasSquaredEuclideanDistance g, Num (NumType g),
 Arity (Dimension g)) =>
Point (Dimension g) (NumType g) -> g -> NumType g
squaredEuclideanDistTo Point 2 r
Point
  (Dimension (SimplePolygon () r)) (NumType (SimplePolygon () r))
p SimplePolygon () r
chb1 r -> r -> Bool
forall a. Ord a => a -> a -> Bool
< r
tresholdr -> Integer -> r
forall a b. (Num a, Integral b) => a -> b -> a
^Integer
2
      in2 :: Bool
in2      = Point
  (Dimension (SimplePolygon () r)) (NumType (SimplePolygon () r))
-> SimplePolygon () r -> NumType (SimplePolygon () r)
forall g.
(HasSquaredEuclideanDistance g, Num (NumType g),
 Arity (Dimension g)) =>
Point (Dimension g) (NumType g) -> g -> NumType g
squaredEuclideanDistTo Point 2 r
Point
  (Dimension (SimplePolygon () r)) (NumType (SimplePolygon () r))
p SimplePolygon () r
chb2 r -> r -> Bool
forall a. Ord a => a -> a -> Bool
< r
tresholdr -> Integer -> r
forall a b. (Num a, Integral b) => a -> b -> a
^Integer
2
      result :: r
result |     Bool
in1 Bool -> Bool -> Bool
&&     Bool
in2 = BezierSpline n 2 r -> Point 2 r -> r -> r -> r
forall (n :: Nat) (d :: Nat) r.
(KnownNat n, Arity d, Ord r, Fractional r) =>
BezierSpline n d r -> Point d r -> r -> r -> r
betterFit BezierSpline n 2 r
b Point 2 r
p r
recurse1 r
recurse2
             |     Bool
in2 Bool -> Bool -> Bool
&& Bool -> Bool
not Bool
in2 = r
recurse1
             | Bool -> Bool
not Bool
in2 Bool -> Bool -> Bool
&&     Bool
in2 = r
recurse2
             | Point
  (Dimension (SimplePolygon () r)) (NumType (SimplePolygon () r))
-> SimplePolygon () r -> NumType (SimplePolygon () r)
forall g.
(HasSquaredEuclideanDistance g, Num (NumType g),
 Arity (Dimension g)) =>
Point (Dimension g) (NumType g) -> g -> NumType g
squaredEuclideanDistTo Point 2 r
Point
  (Dimension (SimplePolygon () r)) (NumType (SimplePolygon () r))
p SimplePolygon () r
chb1 r -> r -> Bool
forall a. Ord a => a -> a -> Bool
< Point
  (Dimension (SimplePolygon () r)) (NumType (SimplePolygon () r))
-> SimplePolygon () r -> NumType (SimplePolygon () r)
forall g.
(HasSquaredEuclideanDistance g, Num (NumType g),
 Arity (Dimension g)) =>
Point (Dimension g) (NumType g) -> g -> NumType g
squaredEuclideanDistTo Point 2 r
Point
  (Dimension (SimplePolygon () r)) (NumType (SimplePolygon () r))
p SimplePolygon () r
chb2 = r
recurse1
             | Bool
otherwise                                                     = r
recurse2
  in r
result

-- | Given a point on (or close to) the extension of a Bezier curve, return the corresponding
--   parameter value, which might also be smaller than 0 or larger than 1.
--   (For points far away from the curve, the function will return the parameter value of
--   an approximate locally closest point to the input point.)
--
--   This implementation is not robust: might return a locally closest point on the curve
--   even though the point lies on another part of the curve. For points on the actual
--   curve, use parameterOf instead.
extendedParameterOf      :: (Arity d, KnownNat n, Ord r, Fractional r)
                         => r -> BezierSpline n d r -> Point d r -> r
extendedParameterOf :: r -> BezierSpline n d r -> Point d r -> r
extendedParameterOf r
treshold BezierSpline n d r
b Point d r
p | Point d r
p Point d r -> Point d r -> Bool
forall a. Eq a => a -> a -> Bool
== (Point d r, Point d r) -> Point d r
forall a b. (a, b) -> a
fst (BezierSpline n d r -> (Point d r, Point d r)
forall (n :: Nat) (d :: Nat) r.
BezierSpline n d r -> (Point d r, Point d r)
endPoints BezierSpline n d r
b) = r
0
                                 | Point d r
p Point d r -> Point d r -> Bool
forall a. Eq a => a -> a -> Bool
== (Point d r, Point d r) -> Point d r
forall a b. (a, b) -> b
snd (BezierSpline n d r -> (Point d r, Point d r)
forall (n :: Nat) (d :: Nat) r.
BezierSpline n d r -> (Point d r, Point d r)
endPoints BezierSpline n d r
b) = r
1
                                 | Bool
otherwise = r -> (r -> r) -> r -> r -> r
forall r. (Ord r, Fractional r) => r -> (r -> r) -> r -> r -> r
binarySearch r
treshold (Point d r -> Point d r -> r
forall (p :: * -> *) a.
(Affine p, Foldable (Diff p), Num a) =>
p a -> p a -> a
qdA Point d r
p (Point d r -> r) -> (r -> Point d r) -> r -> r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. BezierSpline n d r -> r -> Point d r
forall (d :: Nat) r (n :: Nat).
(Arity d, Eq r, Num r) =>
BezierSpline n d r -> r -> Point d r
evaluate BezierSpline n d r
b) (-r
100) r
100

----------------------------------------
-- * Stuff to implement parameterOf and extendedParameterOf

betterFit         :: (KnownNat n, Arity d, Ord r, Fractional r)
                  => BezierSpline n d r -> Point d r -> r -> r -> r
betterFit :: BezierSpline n d r -> Point d r -> r -> r -> r
betterFit BezierSpline n d r
b Point d r
p r
t r
u =
  let q :: Point d r
q = BezierSpline n d r -> r -> Point d r
forall (d :: Nat) r (n :: Nat).
(Arity d, Eq r, Num r) =>
BezierSpline n d r -> r -> Point d r
evaluate BezierSpline n d r
b r
t
      r :: Point d r
r = BezierSpline n d r -> r -> Point d r
forall (d :: Nat) r (n :: Nat).
(Arity d, Eq r, Num r) =>
BezierSpline n d r -> r -> Point d r
evaluate BezierSpline n d r
b r
u
  in if Point d r -> Point d r -> r
forall (p :: * -> *) a.
(Affine p, Foldable (Diff p), Num a) =>
p a -> p a -> a
qdA Point d r
q Point d r
p r -> r -> Bool
forall a. Ord a => a -> a -> Bool
< Point d r -> Point d r -> r
forall (p :: * -> *) a.
(Affine p, Foldable (Diff p), Num a) =>
p a -> p a -> a
qdA Point d r
r Point d r
p then r
t else r
u

--------------------------------------------------------------------------------

-- | Given two Bezier curves, list all intersection points.
--   Not exact, since for degree >= 3 there is no closed form.
--   (In principle, this algorithm works in any dimension
--   but this requires convexHull, area/volume, and intersect.)
intersectB :: (KnownNat n, Ord r, RealFrac r) => r -> BezierSpline n 2 r -> BezierSpline n 2 r -> [Point 2 r]
intersectB :: r -> BezierSpline n 2 r -> BezierSpline n 2 r -> [Point 2 r]
intersectB r
treshold BezierSpline n 2 r
a BezierSpline n 2 r
b
  | BezierSpline n 2 r
a BezierSpline n 2 r -> BezierSpline n 2 r -> Bool
forall a. Eq a => a -> a -> Bool
== BezierSpline n 2 r
b    = [(Point 2 r, Point 2 r) -> Point 2 r
forall a b. (a, b) -> a
fst ((Point 2 r, Point 2 r) -> Point 2 r)
-> (Point 2 r, Point 2 r) -> Point 2 r
forall a b. (a -> b) -> a -> b
$ BezierSpline n 2 r -> (Point 2 r, Point 2 r)
forall (n :: Nat) (d :: Nat) r.
BezierSpline n d r -> (Point d r, Point d r)
endPoints BezierSpline n 2 r
b, (Point 2 r, Point 2 r) -> Point 2 r
forall a b. (a, b) -> b
snd ((Point 2 r, Point 2 r) -> Point 2 r)
-> (Point 2 r, Point 2 r) -> Point 2 r
forall a b. (a -> b) -> a -> b
$ BezierSpline n 2 r -> (Point 2 r, Point 2 r)
forall (n :: Nat) (d :: Nat) r.
BezierSpline n d r -> (Point d r, Point d r)
endPoints BezierSpline n 2 r
b] -- should really return the whole curve
  | Bool
otherwise = let [Point 2 r
a1, Point 2 r
_a2, Point 2 r
_a3, Point 2 r
a4] = LSeq (1 + n) (Point 2 r) -> [Point 2 r]
forall (t :: * -> *) a. Foldable t => t a -> [a]
F.toList (LSeq (1 + n) (Point 2 r) -> [Point 2 r])
-> LSeq (1 + n) (Point 2 r) -> [Point 2 r]
forall a b. (a -> b) -> a -> b
$ BezierSpline n 2 r -> LSeq (1 + n) (Point 2 r)
forall (n :: Nat) (d :: Nat) r.
BezierSpline n d r -> LSeq (1 + n) (Point d r)
_controlPoints BezierSpline n 2 r
a
                    [Point 2 r
b1, Point 2 r
_b2, Point 2 r
_b3, Point 2 r
b4] = LSeq (1 + n) (Point 2 r) -> [Point 2 r]
forall (t :: * -> *) a. Foldable t => t a -> [a]
F.toList (LSeq (1 + n) (Point 2 r) -> [Point 2 r])
-> LSeq (1 + n) (Point 2 r) -> [Point 2 r]
forall a b. (a -> b) -> a -> b
$ BezierSpline n 2 r -> LSeq (1 + n) (Point 2 r)
forall (n :: Nat) (d :: Nat) r.
BezierSpline n d r -> LSeq (1 + n) (Point d r)
_controlPoints BezierSpline n 2 r
b
                in    r -> [Point 2 r] -> [Point 2 r] -> [Point 2 r]
forall r.
(Ord r, Fractional r) =>
r -> [Point 2 r] -> [Point 2 r] -> [Point 2 r]
intersectPointsPoints     r
treshold [Point 2 r
a1, Point 2 r
a4] [Point 2 r
b1, Point 2 r
b4]
                   [Point 2 r] -> [Point 2 r] -> [Point 2 r]
forall a. [a] -> [a] -> [a]
++ r -> [Point 2 r] -> BezierSpline n 2 r -> [Point 2 r]
forall (n :: Nat) r.
(KnownNat n, Ord r, RealFrac r) =>
r -> [Point 2 r] -> BezierSpline n 2 r -> [Point 2 r]
intersectPointsInterior   r
treshold [Point 2 r
a1, Point 2 r
a4] BezierSpline n 2 r
b
                   [Point 2 r] -> [Point 2 r] -> [Point 2 r]
forall a. [a] -> [a] -> [a]
++ r -> [Point 2 r] -> BezierSpline n 2 r -> [Point 2 r]
forall (n :: Nat) r.
(KnownNat n, Ord r, RealFrac r) =>
r -> [Point 2 r] -> BezierSpline n 2 r -> [Point 2 r]
intersectPointsInterior   r
treshold [Point 2 r
b1, Point 2 r
b4] BezierSpline n 2 r
a
                   [Point 2 r] -> [Point 2 r] -> [Point 2 r]
forall a. [a] -> [a] -> [a]
++ r
-> [Point 2 r]
-> BezierSpline n 2 r
-> BezierSpline n 2 r
-> [Point 2 r]
forall (n :: Nat) r.
(KnownNat n, Ord r, RealFrac r) =>
r
-> [Point 2 r]
-> BezierSpline n 2 r
-> BezierSpline n 2 r
-> [Point 2 r]
intersectInteriorInterior r
treshold [Point 2 r
a1, Point 2 r
a4, Point 2 r
b1, Point 2 r
b4] BezierSpline n 2 r
a BezierSpline n 2 r
b


closeEnough :: (Arity d, Ord r, Fractional r) => r -> Point d r -> Point d r -> Bool
closeEnough :: r -> Point d r -> Point d r -> Bool
closeEnough r
treshold Point d r
p Point d r
q = Point d r -> Point d r -> r
forall (p :: * -> *) a.
(Affine p, Foldable (Diff p), Num a) =>
p a -> p a -> a
qdA Point d r
p Point d r
q r -> r -> Bool
forall a. Ord a => a -> a -> Bool
< r
treshold r -> Integer -> r
forall a b. (Num a, Integral b) => a -> b -> a
^ Integer
2

intersectPointsPoints :: (Ord r, Fractional r) => r -> [Point 2 r] -> [Point 2 r] -> [Point 2 r]
intersectPointsPoints :: r -> [Point 2 r] -> [Point 2 r] -> [Point 2 r]
intersectPointsPoints r
treshold [Point 2 r]
ps = (Point 2 r -> Bool) -> [Point 2 r] -> [Point 2 r]
forall a. (a -> Bool) -> [a] -> [a]
filter (\Point 2 r
q -> (Point 2 r -> Bool) -> [Point 2 r] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
any (r -> Point 2 r -> Point 2 r -> Bool
forall (d :: Nat) r.
(Arity d, Ord r, Fractional r) =>
r -> Point d r -> Point d r -> Bool
closeEnough r
treshold Point 2 r
q) [Point 2 r]
ps)

intersectPointsInterior :: (KnownNat n, Ord r, RealFrac r) => r -> [Point 2 r] -> BezierSpline n 2 r -> [Point 2 r]
intersectPointsInterior :: r -> [Point 2 r] -> BezierSpline n 2 r -> [Point 2 r]
intersectPointsInterior r
treshold [Point 2 r]
ps BezierSpline n 2 r
b =
  let [Point 2 r
b1, Point 2 r
_b2, Point 2 r
_b3, Point 2 r
b4] = LSeq (1 + n) (Point 2 r) -> [Point 2 r]
forall (t :: * -> *) a. Foldable t => t a -> [a]
F.toList (LSeq (1 + n) (Point 2 r) -> [Point 2 r])
-> LSeq (1 + n) (Point 2 r) -> [Point 2 r]
forall a b. (a -> b) -> a -> b
$ BezierSpline n 2 r -> LSeq (1 + n) (Point 2 r)
forall (n :: Nat) (d :: Nat) r.
BezierSpline n d r -> LSeq (1 + n) (Point d r)
_controlPoints BezierSpline n 2 r
b
      nearc :: Point 2 r -> Bool
nearc Point 2 r
p = r -> Point 2 r -> Point 2 r -> Bool
forall (d :: Nat) r.
(Arity d, Ord r, Fractional r) =>
r -> Point d r -> Point d r -> Bool
closeEnough r
treshold (r -> BezierSpline n 2 r -> Point 2 r -> Point 2 r
forall (n :: Nat) r.
(KnownNat n, Ord r, RealFrac r) =>
r -> BezierSpline n 2 r -> Point 2 r -> Point 2 r
snap r
treshold BezierSpline n 2 r
b Point 2 r
p) Point 2 r
p
      near1 :: Point 2 r -> Bool
near1 = r -> Point 2 r -> Point 2 r -> Bool
forall (d :: Nat) r.
(Arity d, Ord r, Fractional r) =>
r -> Point d r -> Point d r -> Bool
closeEnough r
treshold Point 2 r
b1
      near4 :: Point 2 r -> Bool
near4 = r -> Point 2 r -> Point 2 r -> Bool
forall (d :: Nat) r.
(Arity d, Ord r, Fractional r) =>
r -> Point d r -> Point d r -> Bool
closeEnough r
treshold Point 2 r
b4
  in (Point 2 r -> Bool) -> [Point 2 r] -> [Point 2 r]
forall a. (a -> Bool) -> [a] -> [a]
filter (\Point 2 r
p -> Point 2 r -> Bool
nearc Point 2 r
p Bool -> Bool -> Bool
&& Bool -> Bool
not (Point 2 r -> Bool
near1 Point 2 r
p) Bool -> Bool -> Bool
&& Bool -> Bool
not (Point 2 r -> Bool
near4 Point 2 r
p)) [Point 2 r]
ps


intersectInteriorInterior :: (KnownNat n, Ord r, RealFrac r) => r -> [Point 2 r] -> BezierSpline n 2 r -> BezierSpline n 2 r -> [Point 2 r]
intersectInteriorInterior :: r
-> [Point 2 r]
-> BezierSpline n 2 r
-> BezierSpline n 2 r
-> [Point 2 r]
intersectInteriorInterior r
treshold [Point 2 r]
forbidden BezierSpline n 2 r
a BezierSpline n 2 r
b =
  let cha :: SimplePolygon () r
cha      = ConvexPolygon () r -> SimplePolygon () r
forall p r. ConvexPolygon p r -> SimplePolygon p r
_simplePolygon (ConvexPolygon () r -> SimplePolygon () r)
-> ConvexPolygon () r -> SimplePolygon () r
forall a b. (a -> b) -> a -> b
$ BezierSpline n 2 r -> ConvexPolygon () r
forall r (n :: Nat).
(Ord r, Fractional r) =>
BezierSpline n 2 r -> ConvexPolygon () r
convexHullB BezierSpline n 2 r
a
      chb :: SimplePolygon () r
chb      = ConvexPolygon () r -> SimplePolygon () r
forall p r. ConvexPolygon p r -> SimplePolygon p r
_simplePolygon (ConvexPolygon () r -> SimplePolygon () r)
-> ConvexPolygon () r -> SimplePolygon () r
forall a b. (a -> b) -> a -> b
$ BezierSpline n 2 r -> ConvexPolygon () r
forall r (n :: Nat).
(Ord r, Fractional r) =>
BezierSpline n 2 r -> ConvexPolygon () r
convexHullB BezierSpline n 2 r
b
      (BezierSpline n 2 r
a1, BezierSpline n 2 r
a2) = r -> BezierSpline n 2 r -> (BezierSpline n 2 r, BezierSpline n 2 r)
forall (n :: Nat) (d :: Nat) r.
(KnownNat n, Arity d, Ord r, Num r) =>
r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
split r
0.5 BezierSpline n 2 r
a
      (BezierSpline n 2 r
b1, BezierSpline n 2 r
b2) = r -> BezierSpline n 2 r -> (BezierSpline n 2 r, BezierSpline n 2 r)
forall (n :: Nat) (d :: Nat) r.
(KnownNat n, Arity d, Ord r, Num r) =>
r -> BezierSpline n d r -> (BezierSpline n d r, BezierSpline n d r)
split r
0.5 BezierSpline n 2 r
b
      points :: [Point 2 r]
points   = LSeq (1 + n) (Point 2 r) -> [Point 2 r]
forall (t :: * -> *) a. Foldable t => t a -> [a]
F.toList (Getting
  (LSeq (1 + n) (Point 2 r))
  (BezierSpline n 2 r)
  (LSeq (1 + n) (Point 2 r))
-> BezierSpline n 2 r -> LSeq (1 + n) (Point 2 r)
forall s (m :: * -> *) a. MonadReader s m => Getting a s a -> m a
view Getting
  (LSeq (1 + n) (Point 2 r))
  (BezierSpline n 2 r)
  (LSeq (1 + n) (Point 2 r))
forall (n1 :: Nat) (d1 :: Nat) r1 (n2 :: Nat) (d2 :: Nat) r2.
Iso
  (BezierSpline n1 d1 r1)
  (BezierSpline n2 d2 r2)
  (LSeq (1 + n1) (Point d1 r1))
  (LSeq (1 + n2) (Point d2 r2))
controlPoints BezierSpline n 2 r
a)
              [Point 2 r] -> [Point 2 r] -> [Point 2 r]
forall a. [a] -> [a] -> [a]
++ LSeq (1 + n) (Point 2 r) -> [Point 2 r]
forall (t :: * -> *) a. Foldable t => t a -> [a]
F.toList (Getting
  (LSeq (1 + n) (Point 2 r))
  (BezierSpline n 2 r)
  (LSeq (1 + n) (Point 2 r))
-> BezierSpline n 2 r -> LSeq (1 + n) (Point 2 r)
forall s (m :: * -> *) a. MonadReader s m => Getting a s a -> m a
view Getting
  (LSeq (1 + n) (Point 2 r))
  (BezierSpline n 2 r)
  (LSeq (1 + n) (Point 2 r))
forall (n1 :: Nat) (d1 :: Nat) r1 (n2 :: Nat) (d2 :: Nat) r2.
Iso
  (BezierSpline n1 d1 r1)
  (BezierSpline n2 d2 r2)
  (LSeq (1 + n1) (Point d1 r1))
  (LSeq (1 + n2) (Point d2 r2))
controlPoints BezierSpline n 2 r
b)
      approx :: Point 2 r
approx   = [Point 2 r] -> Point 2 r
forall (t :: * -> *) (d :: Nat) r.
(Functor t, Foldable t, Arity d, Fractional r) =>
t (Point d r) -> Point d r
average [Point 2 r]
points
      done :: Bool
done | Bool -> Bool
not (SimplePolygon () r
cha SimplePolygon () r -> SimplePolygon () r -> Bool
forall r p.
(Ord r, Fractional r) =>
SimplePolygon p r -> SimplePolygon p r -> Bool
`intersectsP` SimplePolygon () r
chb) = Bool
True
           | [Point 2 r] -> r
forall r. (Ord r, RealFrac r) => [Point 2 r] -> r
sqrad [Point 2 r]
points r -> r -> Bool
forall a. Ord a => a -> a -> Bool
< r
tresholdr -> Integer -> r
forall a b. (Num a, Integral b) => a -> b -> a
^Integer
2   = Bool
True
           | Bool
otherwise                   = Bool
False
      result :: [Point 2 r]
result | Bool -> Bool
not (SimplePolygon () r
cha SimplePolygon () r -> SimplePolygon () r -> Bool
forall r p.
(Ord r, Fractional r) =>
SimplePolygon p r -> SimplePolygon p r -> Bool
`intersectsP` SimplePolygon () r
chb)        = []
             | (Point 2 r -> Bool) -> [Point 2 r] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
any (r -> Point 2 r -> Point 2 r -> Bool
forall (d :: Nat) r.
(Arity d, Ord r, Fractional r) =>
r -> Point d r -> Point d r -> Bool
closeEnough r
treshold Point 2 r
approx) [Point 2 r]
forbidden = []
             | Bool
otherwise                          = [Point 2 r
approx]
      recurse :: [Point 2 r]
recurse = r
-> [Point 2 r]
-> BezierSpline n 2 r
-> BezierSpline n 2 r
-> [Point 2 r]
forall (n :: Nat) r.
(KnownNat n, Ord r, RealFrac r) =>
r
-> [Point 2 r]
-> BezierSpline n 2 r
-> BezierSpline n 2 r
-> [Point 2 r]
intersectInteriorInterior r
treshold [Point 2 r]
forbidden BezierSpline n 2 r
a1 BezierSpline n 2 r
b1
             [Point 2 r] -> [Point 2 r] -> [Point 2 r]
forall a. [a] -> [a] -> [a]
++ r
-> [Point 2 r]
-> BezierSpline n 2 r
-> BezierSpline n 2 r
-> [Point 2 r]
forall (n :: Nat) r.
(KnownNat n, Ord r, RealFrac r) =>
r
-> [Point 2 r]
-> BezierSpline n 2 r
-> BezierSpline n 2 r
-> [Point 2 r]
intersectInteriorInterior r
treshold [Point 2 r]
forbidden BezierSpline n 2 r
a1 BezierSpline n 2 r
b2
             [Point 2 r] -> [Point 2 r] -> [Point 2 r]
forall a. [a] -> [a] -> [a]
++ r
-> [Point 2 r]
-> BezierSpline n 2 r
-> BezierSpline n 2 r
-> [Point 2 r]
forall (n :: Nat) r.
(KnownNat n, Ord r, RealFrac r) =>
r
-> [Point 2 r]
-> BezierSpline n 2 r
-> BezierSpline n 2 r
-> [Point 2 r]
intersectInteriorInterior r
treshold [Point 2 r]
forbidden BezierSpline n 2 r
a2 BezierSpline n 2 r
b1
             [Point 2 r] -> [Point 2 r] -> [Point 2 r]
forall a. [a] -> [a] -> [a]
++ r
-> [Point 2 r]
-> BezierSpline n 2 r
-> BezierSpline n 2 r
-> [Point 2 r]
forall (n :: Nat) r.
(KnownNat n, Ord r, RealFrac r) =>
r
-> [Point 2 r]
-> BezierSpline n 2 r
-> BezierSpline n 2 r
-> [Point 2 r]
intersectInteriorInterior r
treshold [Point 2 r]
forbidden BezierSpline n 2 r
a2 BezierSpline n 2 r
b2
  in if Bool
done then [Point 2 r]
result else [Point 2 r]
recurse

sqrad :: (Ord r, RealFrac r) => [Point 2 r] -> r
sqrad :: [Point 2 r] -> r
sqrad [Point 2 r]
points | [Point 2 r] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [Point 2 r]
points Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
< Int
2 = String -> r
forall a. HasCallStack => String -> a
error String
"sqrad: not enough points"
sqrad [Point 2 r]
points | Bool
otherwise =
  let rationalPoints :: [Point 2 Rational] -- smallestEnclosingDisk fails on Floats
      rationalPoints :: [Point 2 Rational]
rationalPoints = (Point 2 r -> Point 2 Rational)
-> [Point 2 r] -> [Point 2 Rational]
forall a b. (a -> b) -> [a] -> [b]
map ((r -> Identity Rational)
-> Point 2 r -> Identity (Point 2 Rational)
forall (t :: * -> *) (f :: * -> *) a b.
(Traversable t, Applicative f) =>
(a -> f b) -> t a -> f (t b)
traverse ((r -> Identity Rational)
 -> Point 2 r -> Identity (Point 2 Rational))
-> (r -> Rational) -> Point 2 r -> Point 2 Rational
forall s t a b. ASetter s t a b -> (a -> b) -> s -> t
%~ r -> Rational
forall a b. (Real a, Fractional b) => a -> b
realToFrac) [Point 2 r]
points
      (Point 2 Rational :+ ()
a : Point 2 Rational :+ ()
b : [Point 2 Rational :+ ()]
cs) = (Point 2 Rational -> Point 2 Rational :+ ())
-> [Point 2 Rational] -> [Point 2 Rational :+ ()]
forall a b. (a -> b) -> [a] -> [b]
map (Point 2 Rational -> () -> Point 2 Rational :+ ()
forall core extra. core -> extra -> core :+ extra
:+ ()) [Point 2 Rational]
rationalPoints
      diskResult :: DiskResult () Rational
diskResult   = (Point 2 Rational :+ ())
-> (Point 2 Rational :+ ())
-> [Point 2 Rational :+ ()]
-> DiskResult () Rational
forall r p.
(Ord r, Fractional r) =>
(Point 2 r :+ p)
-> (Point 2 r :+ p) -> [Point 2 r :+ p] -> DiskResult p r
smallestEnclosingDisk' Point 2 Rational :+ ()
a Point 2 Rational :+ ()
b [Point 2 Rational :+ ()]
cs
  in Rational -> r
forall a b. (Real a, Fractional b) => a -> b
realToFrac (Rational -> r) -> Rational -> r
forall a b. (a -> b) -> a -> b
$ Getting Rational (Disk () Rational) Rational
-> Disk () Rational -> Rational
forall s (m :: * -> *) a. MonadReader s m => Getting a s a -> m a
view Getting Rational (Disk () Rational) Rational
forall (d :: Nat) p r. Lens' (Ball d p r) r
squaredRadius (Disk () Rational -> Rational) -> Disk () Rational -> Rational
forall a b. (a -> b) -> a -> b
$ Getting
  (Disk () Rational) (DiskResult () Rational) (Disk () Rational)
-> DiskResult () Rational -> Disk () Rational
forall s (m :: * -> *) a. MonadReader s m => Getting a s a -> m a
view Getting
  (Disk () Rational) (DiskResult () Rational) (Disk () Rational)
forall p r. Lens' (DiskResult p r) (Disk () r)
enclosingDisk (DiskResult () Rational -> Disk () Rational)
-> DiskResult () Rational -> Disk () Rational
forall a b. (a -> b) -> a -> b
$ DiskResult () Rational
diskResult

average :: (Functor t, Foldable t, Arity d, Fractional r) => t (Point d r) -> Point d r
average :: t (Point d r) -> Point d r
average t (Point d r)
ps = Point d r
forall (d :: Nat) r. (Arity d, Num r) => Point d r
origin Point d r -> Diff (Point d) r -> Point d r
forall (p :: * -> *) a. (Affine p, Num a) => p a -> Diff p a -> p a
.+^ (Vector d r -> Vector d r -> Vector d r)
-> t (Vector d r) -> Vector d r
forall (t :: * -> *) a. Foldable t => (a -> a -> a) -> t a -> a
foldr1 Vector d r -> Vector d r -> Vector d r
forall (f :: * -> *) a. (Additive f, Num a) => f a -> f a -> f a
(^+^) ((Point d r -> Vector d r) -> t (Point d r) -> t (Vector d r)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Point d r -> Vector d r
forall (d :: Nat) r. Point d r -> Vector d r
toVec t (Point d r)
ps) Vector d r -> r -> Vector d r
forall (f :: * -> *) a.
(Functor f, Fractional a) =>
f a -> a -> f a
^/ Int -> r
forall a b. (Real a, Fractional b) => a -> b
realToFrac (t (Point d r) -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length t (Point d r)
ps)

{-
type instance IntersectionOf (BezierSpline n 2 r) (BezierSpline n 2 r) = [ NoIntersection
                                                                                   , [Point 2 r]
                                                                                   , BezierSpline n 2 r
                                                                                   ]


instance (KnownNat n, Ord r, Fractional r) => (BezierSpline n 2 r) `IsIntersectableWith` (BezierSpline n 2 r) where
  nonEmptyIntersection = defaultNonEmptyIntersection
  a `intersect` b = a `intersectB` b
-}


-- function to test whether two convex polygons intersect
-- for speed, first test bounding boxes
-- maybe would be faster to directly compare bounding boxes of points, rather than
-- call convex hull first?
intersectsP :: (Ord r, Fractional r) => SimplePolygon p r -> SimplePolygon p r -> Bool
intersectsP :: SimplePolygon p r -> SimplePolygon p r -> Bool
intersectsP SimplePolygon p r
p SimplePolygon p r
q | Bool -> Bool
not (Bool -> Bool) -> Bool -> Bool
forall a b. (a -> b) -> a -> b
$ SimplePolygon p r
-> Box
     (Dimension (SimplePolygon p r)) () (NumType (SimplePolygon p r))
forall g.
(IsBoxable g, Ord (NumType g)) =>
g -> Box (Dimension g) () (NumType g)
boundingBox SimplePolygon p r
p Box 2 () r -> Box 2 () r -> Bool
forall g h. HasIntersectionWith g h => g -> h -> Bool
`intersects` SimplePolygon p r
-> Box
     (Dimension (SimplePolygon p r)) () (NumType (SimplePolygon p r))
forall g.
(IsBoxable g, Ord (NumType g)) =>
g -> Box (Dimension g) () (NumType g)
boundingBox SimplePolygon p r
q = Bool
False
                | Bool
otherwise = [Bool] -> Bool
forall (t :: * -> *). Foldable t => t Bool -> Bool
or [LineSegment 2 p r
a LineSegment 2 p r -> LineSegment 2 p r -> Bool
forall g h. HasIntersectionWith g h => g -> h -> Bool
`intersects` LineSegment 2 p r
b | LineSegment 2 p r
a <- SimplePolygon p r
p SimplePolygon p r
-> (SimplePolygon p r -> [LineSegment 2 p r])
-> [LineSegment 2 p r]
forall a b. a -> (a -> b) -> b
& SimplePolygon p r -> [LineSegment 2 p r]
forall (t :: PolygonType) p r. Polygon t p r -> [LineSegment 2 p r]
listEdges, LineSegment 2 p r
b <- SimplePolygon p r
q SimplePolygon p r
-> (SimplePolygon p r -> [LineSegment 2 p r])
-> [LineSegment 2 p r]
forall a b. a -> (a -> b) -> b
& SimplePolygon p r -> [LineSegment 2 p r]
forall (t :: PolygonType) p r. Polygon t p r -> [LineSegment 2 p r]
listEdges]
                           Bool -> Bool -> Bool
|| ((Point 2 r -> Bool) -> [Point 2 r] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
any ((Point 2 r -> SimplePolygon p r -> Bool)
-> SimplePolygon p r -> Point 2 r -> Bool
forall a b c. (a -> b -> c) -> b -> a -> c
flip Point 2 r -> SimplePolygon p r -> Bool
forall r (t :: PolygonType) p.
(Fractional r, Ord r) =>
Point 2 r -> Polygon t p r -> Bool
insidePolygon SimplePolygon p r
p) ([Point 2 r] -> Bool) -> [Point 2 r] -> Bool
forall a b. (a -> b) -> a -> b
$ ((Point 2 r :+ p) -> Point 2 r) -> [Point 2 r :+ p] -> [Point 2 r]
forall a b. (a -> b) -> [a] -> [b]
map (Point 2 r :+ p) -> Point 2 r
forall core extra. (core :+ extra) -> core
_core ([Point 2 r :+ p] -> [Point 2 r])
-> [Point 2 r :+ p] -> [Point 2 r]
forall a b. (a -> b) -> a -> b
$ NonEmpty (Point 2 r :+ p) -> [Point 2 r :+ p]
forall (t :: * -> *) a. Foldable t => t a -> [a]
F.toList (NonEmpty (Point 2 r :+ p) -> [Point 2 r :+ p])
-> NonEmpty (Point 2 r :+ p) -> [Point 2 r :+ p]
forall a b. (a -> b) -> a -> b
$ SimplePolygon p r -> NonEmpty (Point 2 r :+ p)
forall (t :: PolygonType) p r.
Polygon t p r -> NonEmpty (Point 2 r :+ p)
polygonVertices SimplePolygon p r
q)
                           Bool -> Bool -> Bool
|| ((Point 2 r -> Bool) -> [Point 2 r] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
any ((Point 2 r -> SimplePolygon p r -> Bool)
-> SimplePolygon p r -> Point 2 r -> Bool
forall a b c. (a -> b -> c) -> b -> a -> c
flip Point 2 r -> SimplePolygon p r -> Bool
forall r (t :: PolygonType) p.
(Fractional r, Ord r) =>
Point 2 r -> Polygon t p r -> Bool
insidePolygon SimplePolygon p r
q) ([Point 2 r] -> Bool) -> [Point 2 r] -> Bool
forall a b. (a -> b) -> a -> b
$ ((Point 2 r :+ p) -> Point 2 r) -> [Point 2 r :+ p] -> [Point 2 r]
forall a b. (a -> b) -> [a] -> [b]
map (Point 2 r :+ p) -> Point 2 r
forall core extra. (core :+ extra) -> core
_core ([Point 2 r :+ p] -> [Point 2 r])
-> [Point 2 r :+ p] -> [Point 2 r]
forall a b. (a -> b) -> a -> b
$ NonEmpty (Point 2 r :+ p) -> [Point 2 r :+ p]
forall (t :: * -> *) a. Foldable t => t a -> [a]
F.toList (NonEmpty (Point 2 r :+ p) -> [Point 2 r :+ p])
-> NonEmpty (Point 2 r :+ p) -> [Point 2 r :+ p]
forall a b. (a -> b) -> a -> b
$ SimplePolygon p r -> NonEmpty (Point 2 r :+ p)
forall (t :: PolygonType) p r.
Polygon t p r -> NonEmpty (Point 2 r :+ p)
polygonVertices SimplePolygon p r
p)
  -- first test bounding box?


{-

instance (Arity d, Floating r) => IsBoxable (BezierSpline 3 d r) where
  boundingBox b = foldr1 (<>) $ map (\i -> boundingBox (extremal True i b) <> boundingBox (extremal False i b)) [1 .. d]

-- | Find extremal points on curve in the $i$th dimension.
extremal :: Floating r => Bool -> Int -> BezierSpline 3 d r -> Point d r
extremal pos i b =
  let [p1, _, _, p4] = F.toList $ view controlPoints b
      ps = map evaluate $ locallyExtremalParameters i b
      candidates = [p1, p4] ++ ps
      result | pos     = maximumBy (unsafeCoord i . snd) candidates
             | not pos = minimumBy (unsafeCoord i . snd) candidates
  in result

-}


--------------------------------------------------------------------------------

snapEndpoints           :: (KnownNat n, Arity d, Ord r, Fractional r)
                        => Point d r -> Point d r -> BezierSpline n d r -> BezierSpline n d r
snapEndpoints :: Point d r -> Point d r -> BezierSpline n d r -> BezierSpline n d r
snapEndpoints Point d r
p Point d r
q BezierSpline n d r
curve =
  let points :: [Point d r]
points = LSeq (1 + n) (Point d r) -> [Point d r]
forall (t :: * -> *) a. Foldable t => t a -> [a]
F.toList (LSeq (1 + n) (Point d r) -> [Point d r])
-> LSeq (1 + n) (Point d r) -> [Point d r]
forall a b. (a -> b) -> a -> b
$ BezierSpline n d r -> LSeq (1 + n) (Point d r)
forall (n :: Nat) (d :: Nat) r.
BezierSpline n d r -> LSeq (1 + n) (Point d r)
_controlPoints BezierSpline n d r
curve
      middle :: [Point d r]
middle = [Point d r] -> [Point d r]
forall a. [a] -> [a]
tail ([Point d r] -> [Point d r])
-> ([Point d r] -> [Point d r]) -> [Point d r] -> [Point d r]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. [Point d r] -> [Point d r]
forall a. [a] -> [a]
init ([Point d r] -> [Point d r]) -> [Point d r] -> [Point d r]
forall a b. (a -> b) -> a -> b
$ [Point d r]
points
      new :: [Point d r]
new    = [Point d r
p] [Point d r] -> [Point d r] -> [Point d r]
forall a. [a] -> [a] -> [a]
++ [Point d r]
middle [Point d r] -> [Point d r] -> [Point d r]
forall a. [a] -> [a] -> [a]
++ [Point d r
q]
  in  Seq (Point d r) -> BezierSpline n d r
forall (d :: Nat) r (n :: Nat).
Seq (Point d r) -> BezierSpline n d r
fromPointSeq (Seq (Point d r) -> BezierSpline n d r)
-> Seq (Point d r) -> BezierSpline n d r
forall a b. (a -> b) -> a -> b
$ [Point d r] -> Seq (Point d r)
forall a. [a] -> Seq a
Seq.fromList [Point d r]
new


-- | Snap a point close to a Bezier curve to the curve.
snap   :: (KnownNat n, Ord r, RealFrac r) => r -> BezierSpline n 2 r -> Point 2 r -> Point 2 r
snap :: r -> BezierSpline n 2 r -> Point 2 r -> Point 2 r
snap r
treshold BezierSpline n 2 r
b = BezierSpline n 2 r -> r -> Point 2 r
forall (d :: Nat) r (n :: Nat).
(Arity d, Eq r, Num r) =>
BezierSpline n d r -> r -> Point d r
evaluate BezierSpline n 2 r
b (r -> Point 2 r) -> (Point 2 r -> r) -> Point 2 r -> Point 2 r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. r -> BezierSpline n 2 r -> Point 2 r -> r
forall (n :: Nat) r.
(KnownNat n, Ord r, RealFrac r) =>
r -> BezierSpline n 2 r -> Point 2 r -> r
parameterOf r
treshold BezierSpline n 2 r
b

--------------------------------------------------------------------------------
-- * Helper functions

-- | Solve equation of the form ax^2 + bx + c = 0.
--   If there are multiple solutions, report in ascending order.
--   Attempt at a somewhat robust implementation.
solveQuadraticEquation :: (Ord r, Enum r, Floating r) => r -> r -> r -> [r]
solveQuadraticEquation :: r -> r -> r -> [r]
solveQuadraticEquation r
0 r
0 r
0 = [r
0..] -- error "infinite solutions"
solveQuadraticEquation r
_ r
0 r
0 = [r
0]
solveQuadraticEquation r
0 r
_ r
0 = [r
0]
solveQuadraticEquation r
0 r
0 r
_ = []
solveQuadraticEquation r
a r
b r
0 = [r] -> [r]
forall a. Ord a => [a] -> [a]
sort [r
0, -r
b r -> r -> r
forall a. Fractional a => a -> a -> a
/ r
a]
solveQuadraticEquation r
a r
0 r
c | (-r
c r -> r -> r
forall a. Fractional a => a -> a -> a
/ r
a) r -> r -> Bool
forall a. Ord a => a -> a -> Bool
<  r
0 = []
                             | (-r
c r -> r -> r
forall a. Fractional a => a -> a -> a
/ r
a) r -> r -> Bool
forall a. Eq a => a -> a -> Bool
== r
0 = [r
0]
                             | (-r
c r -> r -> r
forall a. Fractional a => a -> a -> a
/ r
a) r -> r -> Bool
forall a. Ord a => a -> a -> Bool
>  r
0 = [r -> r
forall a. Floating a => a -> a
sqrt (-r
c r -> r -> r
forall a. Fractional a => a -> a -> a
/ r
a)]
solveQuadraticEquation r
0 r
b r
c = [-r
c r -> r -> r
forall a. Fractional a => a -> a -> a
/ r
b]
solveQuadraticEquation r
a r
b r
c | r -> Bool
forall r. (Floating r, Ord r) => r -> Bool
almostzero r
a Bool -> Bool -> Bool
|| r -> Bool
forall r. (Floating r, Ord r) => r -> Bool
almostzero (r
a r -> r -> r
forall a. Fractional a => a -> a -> a
/ r
b) Bool -> Bool -> Bool
|| r -> Bool
forall r. (Floating r, Ord r) => r -> Bool
almostzero (r
a r -> r -> r
forall a. Fractional a => a -> a -> a
/ r
c) = r -> r -> r -> [r]
forall r. (Ord r, Enum r, Floating r) => r -> r -> r -> [r]
solveQuadraticEquation r
0 r
b r
c
solveQuadraticEquation r
a r
b r
c =
  let d :: r
d = r
br -> Integer -> r
forall a b. (Num a, Integral b) => a -> b -> a
^Integer
2 r -> r -> r
forall a. Num a => a -> a -> a
- r
4 r -> r -> r
forall a. Num a => a -> a -> a
* r
a r -> r -> r
forall a. Num a => a -> a -> a
* r
c
      result :: [r]
result | r
d r -> r -> Bool
forall a. Eq a => a -> a -> Bool
== r
0 = [-r
b r -> r -> r
forall a. Fractional a => a -> a -> a
/ (r
2 r -> r -> r
forall a. Num a => a -> a -> a
* r
a)]
             | r
d r -> r -> Bool
forall a. Ord a => a -> a -> Bool
>  r
0 = [(-r
b r -> r -> r
forall a. Num a => a -> a -> a
- r -> r
forall a. Floating a => a -> a
sqrt r
d) r -> r -> r
forall a. Fractional a => a -> a -> a
/ (r
2 r -> r -> r
forall a. Num a => a -> a -> a
* r
a), (-r
b r -> r -> r
forall a. Num a => a -> a -> a
+ r -> r
forall a. Floating a => a -> a
sqrt r
d) r -> r -> r
forall a. Fractional a => a -> a -> a
/ (r
2 r -> r -> r
forall a. Num a => a -> a -> a
* r
a)]
             | Bool
otherwise = []
  in [r]
result
  -- trace ("soving equation " ++ show a ++ "x^2 + " ++ show b ++ "x + " ++ show c ++ " = 0") $ result

-- | Test whether a floating point number is close enough to zero, taking rounding errors into account.
almostzero :: (Floating r, Ord r) => r -> Bool
almostzero :: r -> Bool
almostzero r
x = r -> r
forall a. Num a => a -> a
abs r
x r -> r -> Bool
forall a. Ord a => a -> a -> Bool
< r
forall r. Floating r => r
epsilon

-- | Treshold for rounding errors in almostzero test.
--   TODO: Should be different depending on the type.
epsilon :: Floating r => r
epsilon :: r
epsilon = r
0.0001



-- | This function tests whether a value lies within bounds of a given interval.
--   If not, graciously continues with value snapped to interval.
--   This should never happen, but apparently it sometimes does?
restrict :: (Ord r) => String -> r -> r -> r -> r
restrict :: String -> r -> r -> r -> r
restrict String
f r
l r
r r
x | r
l r -> r -> Bool
forall a. Ord a => a -> a -> Bool
> r
r = String -> r
forall a. HasCallStack => String -> a
error (String -> r) -> String -> r
forall a b. (a -> b) -> a -> b
$ String
f String -> ShowS
forall a. Semigroup a => a -> a -> a
<> String
": restrict [l,r] is not an interval" --error $ f ++ ": restrict: [" ++ show l ++ ", " ++ show r ++ "] is not an interval"
                 --   | x < l = trace (f ++ ": restricting " ++ show x ++ " to [" ++ show l ++ ", " ++ show r ++ "]") l
                 --   | x > r = trace (f ++ ": restricting " ++ show x ++ " to [" ++ show l ++ ", " ++ show r ++ "]") r
                 | Bool
otherwise = r
x


binarySearch                                    :: (Ord r, Fractional r)
                                                => r -> (r -> r) -> r -> r -> r
binarySearch :: r -> (r -> r) -> r -> r -> r
binarySearch r
treshold r -> r
f r
l r
r
    | r -> r
forall a. Num a => a -> a
abs (r -> r
f r
l r -> r -> r
forall a. Num a => a -> a -> a
- r -> r
f r
r) r -> r -> Bool
forall a. Ord a => a -> a -> Bool
< r
treshold = String -> r -> r -> r -> r
forall r. Ord r => String -> r -> r -> r -> r
restrict String
"binarySearch" r
l r
r   r
m
    | (r -> r) -> r -> r
forall r. Fractional r => (r -> r) -> r -> r
derivative r -> r
f r
m  r -> r -> Bool
forall a. Ord a => a -> a -> Bool
> r
0        = String -> r -> r -> r -> r
forall r. Ord r => String -> r -> r -> r -> r
restrict String
"binarySearch" r
l r
r (r -> r) -> r -> r
forall a b. (a -> b) -> a -> b
$ r -> (r -> r) -> r -> r -> r
forall r. (Ord r, Fractional r) => r -> (r -> r) -> r -> r -> r
binarySearch r
treshold r -> r
f r
l r
m
    | Bool
otherwise                  = String -> r -> r -> r -> r
forall r. Ord r => String -> r -> r -> r -> r
restrict String
"binarySearch" r
l r
r (r -> r) -> r -> r
forall a b. (a -> b) -> a -> b
$ r -> (r -> r) -> r -> r -> r
forall r. (Ord r, Fractional r) => r -> (r -> r) -> r -> r -> r
binarySearch r
treshold r -> r
f r
m r
r
  where m :: r
m = (r
l r -> r -> r
forall a. Num a => a -> a -> a
+ r
r) r -> r -> r
forall a. Fractional a => a -> a -> a
/ r
2

derivative     :: Fractional r => (r -> r) -> r -> r
derivative :: (r -> r) -> r -> r
derivative r -> r
f r
x = (r -> r
f (r
x r -> r -> r
forall a. Num a => a -> a -> a
+ r
delta) r -> r -> r
forall a. Num a => a -> a -> a
- r -> r
f r
x) r -> r -> r
forall a. Fractional a => a -> a -> a
/ r
delta
  where delta :: r
delta = r
0.0000001