--------------------------------------------------------------------------------
-- |
-- Module      :  Algorithms.Geometry.ConvexHull.QuickHull
-- Copyright   :  (C) Frank Staals
-- License     :  see the LICENSE file
-- Maintainer  :  Frank Staals
--------------------------------------------------------------------------------
module Algorithms.Geometry.ConvexHull.QuickHull( convexHull ) where

import           Control.Lens ((^.))
import           Data.Ext
import qualified Data.Foldable as F
import           Data.Geometry.Line
import           Data.Geometry.Point
import           Data.Geometry.Polygon
import           Data.Geometry.Polygon.Convex (ConvexPolygon(..))
import           Data.Geometry.Triangle
import qualified Data.List as List
import           Data.List.NonEmpty (NonEmpty(..))
import           Data.Ord (comparing)
import           Data.Util


-- import Data.Ratio
-- import qualified Data.List.NonEmpty as NonEmpty
-- import qualified Algorithms.Geometry.ConvexHull.GrahamScan as GC
-- import Debug.Trace
--------------------------------------------------------------------------------

-- | ConvexHull using Quickhull. The resulting polygon is given in
-- clockwise order.
--
-- running time: \(O(n^2)\)
convexHull            :: (Ord r, Fractional r, Show r, Show p)
                      => NonEmpty (Point 2 r :+ p) -> ConvexPolygon p r
convexHull :: NonEmpty (Point 2 r :+ p) -> ConvexPolygon p r
convexHull (Point 2 r :+ p
p :| []) = SimplePolygon p r -> ConvexPolygon p r
forall p r. SimplePolygon p r -> ConvexPolygon p r
ConvexPolygon (SimplePolygon p r -> ConvexPolygon p r)
-> ([Point 2 r :+ p] -> SimplePolygon p r)
-> [Point 2 r :+ p]
-> ConvexPolygon p r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. [Point 2 r :+ p] -> SimplePolygon p r
forall r p. [Point 2 r :+ p] -> SimplePolygon p r
unsafeFromPoints ([Point 2 r :+ p] -> ConvexPolygon p r)
-> [Point 2 r :+ p] -> ConvexPolygon p r
forall a b. (a -> b) -> a -> b
$ [Point 2 r :+ p
p]
convexHull NonEmpty (Point 2 r :+ p)
ps        = SimplePolygon p r -> ConvexPolygon p r
forall p r. SimplePolygon p r -> ConvexPolygon p r
ConvexPolygon (SimplePolygon p r -> ConvexPolygon p r)
-> ([Point 2 r :+ p] -> SimplePolygon p r)
-> [Point 2 r :+ p]
-> ConvexPolygon p r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. [Point 2 r :+ p] -> SimplePolygon p r
forall r p. [Point 2 r :+ p] -> SimplePolygon p r
unsafeFromPoints
                     ([Point 2 r :+ p] -> ConvexPolygon p r)
-> [Point 2 r :+ p] -> ConvexPolygon p r
forall a b. (a -> b) -> a -> b
$ [Point 2 r :+ p
l] [Point 2 r :+ p] -> [Point 2 r :+ p] -> [Point 2 r :+ p]
forall a. Semigroup a => a -> a -> a
<> (Point 2 r :+ p)
-> (Point 2 r :+ p) -> [Point 2 r :+ p] -> [Point 2 r :+ p]
forall r p.
(Fractional r, Ord r) =>
(Point 2 r :+ p)
-> (Point 2 r :+ p) -> [Point 2 r :+ p] -> [Point 2 r :+ p]
hull Point 2 r :+ p
l Point 2 r :+ p
r [Point 2 r :+ p]
above [Point 2 r :+ p] -> [Point 2 r :+ p] -> [Point 2 r :+ p]
forall a. Semigroup a => a -> a -> a
<> [Point 2 r :+ p
r] [Point 2 r :+ p] -> [Point 2 r :+ p] -> [Point 2 r :+ p]
forall a. Semigroup a => a -> a -> a
<> [Point 2 r :+ p] -> [Point 2 r :+ p]
forall a. [a] -> [a]
reverse ((Point 2 r :+ p)
-> (Point 2 r :+ p) -> [Point 2 r :+ p] -> [Point 2 r :+ p]
forall r p.
(Fractional r, Ord r) =>
(Point 2 r :+ p)
-> (Point 2 r :+ p) -> [Point 2 r :+ p] -> [Point 2 r :+ p]
hull Point 2 r :+ p
l Point 2 r :+ p
r [Point 2 r :+ p]
below)
  where
    STR Point 2 r :+ p
l Point 2 r :+ p
r [Point 2 r :+ p]
mids  = NonEmpty (Point 2 r :+ p)
-> STR (Point 2 r :+ p) (Point 2 r :+ p) [Point 2 r :+ p]
forall r q.
Ord r =>
NonEmpty (Point 2 r :+ q)
-> STR (Point 2 r :+ q) (Point 2 r :+ q) [Point 2 r :+ q]
findExtremes NonEmpty (Point 2 r :+ p)
ps
    m :: Line 2 r
m             = 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 :+ p
l(Point 2 r :+ p)
-> Getting (Point 2 r) (Point 2 r :+ p) (Point 2 r) -> Point 2 r
forall s a. s -> Getting a s a -> a
^.Getting (Point 2 r) (Point 2 r :+ p) (Point 2 r)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core) (Point 2 r :+ p
r(Point 2 r :+ p)
-> Getting (Point 2 r) (Point 2 r :+ p) (Point 2 r) -> Point 2 r
forall s a. s -> Getting a s a -> a
^.Getting (Point 2 r) (Point 2 r :+ p) (Point 2 r)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core)
    ([Point 2 r :+ p]
above,[Point 2 r :+ p]
below) = ((Point 2 r :+ p) -> Bool)
-> [Point 2 r :+ p] -> ([Point 2 r :+ p], [Point 2 r :+ p])
forall a. (a -> Bool) -> [a] -> ([a], [a])
List.partition (\(Point 2 r
p :+ p
_) -> Point 2 r
p Point 2 r -> Line 2 r -> Bool
forall r. (Ord r, Num r) => Point 2 r -> Line 2 r -> Bool
`liesAbove` Line 2 r
m) [Point 2 r :+ p]
mids

-- | Finds the leftmost and rightmost point in the list
findExtremes            :: Ord r
                        => NonEmpty (Point 2 r :+ q)
                        -> STR (Point 2 r :+ q) (Point 2 r :+ q) [Point 2 r :+ q]
findExtremes :: NonEmpty (Point 2 r :+ q)
-> STR (Point 2 r :+ q) (Point 2 r :+ q) [Point 2 r :+ q]
findExtremes (Point 2 r :+ q
p :| [Point 2 r :+ q]
pts ) = ((Point 2 r :+ q)
 -> STR (Point 2 r :+ q) (Point 2 r :+ q) [Point 2 r :+ q]
 -> STR (Point 2 r :+ q) (Point 2 r :+ q) [Point 2 r :+ q])
-> STR (Point 2 r :+ q) (Point 2 r :+ q) [Point 2 r :+ q]
-> [Point 2 r :+ q]
-> STR (Point 2 r :+ q) (Point 2 r :+ q) [Point 2 r :+ q]
forall (t :: * -> *) a b.
Foldable t =>
(a -> b -> b) -> b -> t a -> b
foldr (Point 2 r :+ q)
-> STR (Point 2 r :+ q) (Point 2 r :+ q) [Point 2 r :+ q]
-> STR (Point 2 r :+ q) (Point 2 r :+ q) [Point 2 r :+ q]
forall r extra.
Ord r =>
(Point 2 r :+ extra)
-> STR
     (Point 2 r :+ extra) (Point 2 r :+ extra) [Point 2 r :+ extra]
-> STR
     (Point 2 r :+ extra) (Point 2 r :+ extra) [Point 2 r :+ extra]
f ((Point 2 r :+ q)
-> (Point 2 r :+ q)
-> [Point 2 r :+ q]
-> STR (Point 2 r :+ q) (Point 2 r :+ q) [Point 2 r :+ q]
forall a b c. a -> b -> c -> STR a b c
STR Point 2 r :+ q
p Point 2 r :+ q
p []) [Point 2 r :+ q]
pts
  where
    f :: (Point 2 r :+ extra)
-> STR
     (Point 2 r :+ extra) (Point 2 r :+ extra) [Point 2 r :+ extra]
-> STR
     (Point 2 r :+ extra) (Point 2 r :+ extra) [Point 2 r :+ extra]
f Point 2 r :+ extra
q (STR Point 2 r :+ extra
l Point 2 r :+ extra
r [Point 2 r :+ extra]
ms) = case ((Point 2 r :+ extra) -> (Point 2 r :+ extra) -> Ordering
forall r p q.
Ord r =>
(Point 2 r :+ p) -> (Point 2 r :+ q) -> Ordering
incXdecY Point 2 r :+ extra
q Point 2 r :+ extra
l, (Point 2 r :+ extra) -> (Point 2 r :+ extra) -> Ordering
forall r p q.
Ord r =>
(Point 2 r :+ p) -> (Point 2 r :+ q) -> Ordering
incXdecY Point 2 r :+ extra
q Point 2 r :+ extra
r) of
                         (Ordering
LT,Ordering
_)  -> (Point 2 r :+ extra)
-> (Point 2 r :+ extra)
-> [Point 2 r :+ extra]
-> STR
     (Point 2 r :+ extra) (Point 2 r :+ extra) [Point 2 r :+ extra]
forall a b c. a -> b -> c -> STR a b c
STR Point 2 r :+ extra
q Point 2 r :+ extra
r ((Point 2 r :+ extra)
-> (Point 2 r :+ extra)
-> [Point 2 r :+ extra]
-> [Point 2 r :+ extra]
forall a extra extra.
Eq a =>
(a :+ extra) -> (a :+ extra) -> [a :+ extra] -> [a :+ extra]
addIfNot Point 2 r :+ extra
r Point 2 r :+ extra
l [Point 2 r :+ extra]
ms)
                         (Ordering
EQ,Ordering
_)  -> (Point 2 r :+ extra)
-> (Point 2 r :+ extra)
-> [Point 2 r :+ extra]
-> STR
     (Point 2 r :+ extra) (Point 2 r :+ extra) [Point 2 r :+ extra]
forall a b c. a -> b -> c -> STR a b c
STR Point 2 r :+ extra
l Point 2 r :+ extra
r [Point 2 r :+ extra]
ms -- ditch q; it is the same as l
                         (Ordering
GT,Ordering
GT) -> (Point 2 r :+ extra)
-> (Point 2 r :+ extra)
-> [Point 2 r :+ extra]
-> STR
     (Point 2 r :+ extra) (Point 2 r :+ extra) [Point 2 r :+ extra]
forall a b c. a -> b -> c -> STR a b c
STR Point 2 r :+ extra
l Point 2 r :+ extra
q ((Point 2 r :+ extra)
-> (Point 2 r :+ extra)
-> [Point 2 r :+ extra]
-> [Point 2 r :+ extra]
forall a extra extra.
Eq a =>
(a :+ extra) -> (a :+ extra) -> [a :+ extra] -> [a :+ extra]
addIfNot Point 2 r :+ extra
l Point 2 r :+ extra
r [Point 2 r :+ extra]
ms)
                         (Ordering
GT,Ordering
EQ) -> (Point 2 r :+ extra)
-> (Point 2 r :+ extra)
-> [Point 2 r :+ extra]
-> STR
     (Point 2 r :+ extra) (Point 2 r :+ extra) [Point 2 r :+ extra]
forall a b c. a -> b -> c -> STR a b c
STR Point 2 r :+ extra
l Point 2 r :+ extra
r [Point 2 r :+ extra]
ms -- ditch q; it is the same as r
                         (Ordering
GT,Ordering
LT) -> (Point 2 r :+ extra)
-> (Point 2 r :+ extra)
-> [Point 2 r :+ extra]
-> STR
     (Point 2 r :+ extra) (Point 2 r :+ extra) [Point 2 r :+ extra]
forall a b c. a -> b -> c -> STR a b c
STR Point 2 r :+ extra
l Point 2 r :+ extra
r (Point 2 r :+ extra
q(Point 2 r :+ extra)
-> [Point 2 r :+ extra] -> [Point 2 r :+ extra]
forall a. a -> [a] -> [a]
:[Point 2 r :+ extra]
ms)

    addIfNot :: (a :+ extra) -> (a :+ extra) -> [a :+ extra] -> [a :+ extra]
addIfNot a :+ extra
y a :+ extra
x [a :+ extra]
xs | (a :+ extra
x(a :+ extra) -> Getting a (a :+ extra) a -> a
forall s a. s -> Getting a s a -> a
^.Getting a (a :+ extra) a
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core) a -> a -> Bool
forall a. Eq a => a -> a -> Bool
/= (a :+ extra
y(a :+ extra) -> Getting a (a :+ extra) a -> a
forall s a. s -> Getting a s a -> a
^.Getting a (a :+ extra) a
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core) = a :+ extra
x(a :+ extra) -> [a :+ extra] -> [a :+ extra]
forall a. a -> [a] -> [a]
:[a :+ extra]
xs
                    | Bool
otherwise              = [a :+ extra]
xs

-- findExtremesBy         :: (a -> a -> Ordering)
--                        -> NonEmpty a
--                        -> STR a a [a]
-- findExtremesBy cmp pts = let l = F.minimumBy cmp pts
--                              r = F.maximumBy cmp pts
--                              a /=. b = a `cmp` b /= EQ
--                          in STR l r [p | p <- F.toList pts, p /=. l, p /=. r]


incXdecY  :: Ord r => Point 2 r :+ p -> Point 2 r :+ q -> Ordering
incXdecY :: (Point 2 r :+ p) -> (Point 2 r :+ q) -> Ordering
incXdecY (Point2 r
px r
py :+ p
_) (Point2 r
qx r
qy :+ q
_) =
  r -> r -> Ordering
forall a. Ord a => a -> a -> Ordering
compare r
px r
qx Ordering -> Ordering -> Ordering
forall a. Semigroup a => a -> a -> a
<> r -> r -> Ordering
forall a. Ord a => a -> a -> Ordering
compare r
qy r
py

-- | include neigher left or right
--
hull         :: (Fractional r, Ord r)
             => Point 2 r :+ p -> Point 2 r :+ p -> [Point 2 r :+ p] -> [Point 2 r :+ p]
hull :: (Point 2 r :+ p)
-> (Point 2 r :+ p) -> [Point 2 r :+ p] -> [Point 2 r :+ p]
hull Point 2 r :+ p
_ Point 2 r :+ p
_ []  = []
hull Point 2 r :+ p
l Point 2 r :+ p
r [Point 2 r :+ p]
pts = (Point 2 r :+ p)
-> (Point 2 r :+ p) -> [Point 2 r :+ p] -> [Point 2 r :+ p]
forall r p.
(Fractional r, Ord r) =>
(Point 2 r :+ p)
-> (Point 2 r :+ p) -> [Point 2 r :+ p] -> [Point 2 r :+ p]
hull Point 2 r :+ p
l Point 2 r :+ p
mid [Point 2 r :+ p]
ls [Point 2 r :+ p] -> [Point 2 r :+ p] -> [Point 2 r :+ p]
forall a. Semigroup a => a -> a -> a
<> [Point 2 r :+ p
mid] [Point 2 r :+ p] -> [Point 2 r :+ p] -> [Point 2 r :+ p]
forall a. Semigroup a => a -> a -> a
<> (Point 2 r :+ p)
-> (Point 2 r :+ p) -> [Point 2 r :+ p] -> [Point 2 r :+ p]
forall r p.
(Fractional r, Ord r) =>
(Point 2 r :+ p)
-> (Point 2 r :+ p) -> [Point 2 r :+ p] -> [Point 2 r :+ p]
hull Point 2 r :+ p
mid Point 2 r :+ p
r [Point 2 r :+ p]
rs
  where
    m :: Line 2 r
m       = 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 :+ p
l(Point 2 r :+ p)
-> Getting (Point 2 r) (Point 2 r :+ p) (Point 2 r) -> Point 2 r
forall s a. s -> Getting a s a -> a
^.Getting (Point 2 r) (Point 2 r :+ p) (Point 2 r)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core) (Point 2 r :+ p
r(Point 2 r :+ p)
-> Getting (Point 2 r) (Point 2 r :+ p) (Point 2 r) -> Point 2 r
forall s a. s -> Getting a s a -> a
^.Getting (Point 2 r) (Point 2 r :+ p) (Point 2 r)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core)
    mid :: Point 2 r :+ p
mid     = ((Point 2 r :+ p) -> (Point 2 r :+ p) -> Ordering)
-> [Point 2 r :+ p] -> Point 2 r :+ p
forall (t :: * -> *) a.
Foldable t =>
(a -> a -> Ordering) -> t a -> a
F.maximumBy (((Point 2 r :+ p) -> r)
-> (Point 2 r :+ p) -> (Point 2 r :+ p) -> Ordering
forall a b. Ord a => (b -> a) -> b -> b -> Ordering
comparing (Point 2 r :+ p) -> r
dist) [Point 2 r :+ p]
pts

    dist :: (Point 2 r :+ p) -> r
dist (Point 2 r
p :+ p
_) = Point 2 r
p Point 2 r -> Line 2 r -> r
forall r (d :: Nat).
(Fractional r, Arity d) =>
Point d r -> Line d r -> r
`sqDistanceTo` Line 2 r
m
    t :: Triangle 2 p r
t       = (Point 2 r :+ p)
-> (Point 2 r :+ p) -> (Point 2 r :+ p) -> Triangle 2 p r
forall (d :: Nat) p r.
(Point d r :+ p)
-> (Point d r :+ p) -> (Point d r :+ p) -> Triangle d p r
Triangle Point 2 r :+ p
l Point 2 r :+ p
mid Point 2 r :+ p
r
    -- line through l and mid, which splits the remaining points in a left half and a right half.
    splitL :: Line 2 r
splitL   = 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 :+ p
l(Point 2 r :+ p)
-> Getting (Point 2 r) (Point 2 r :+ p) (Point 2 r) -> Point 2 r
forall s a. s -> Getting a s a -> a
^.Getting (Point 2 r) (Point 2 r :+ p) (Point 2 r)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core) (Point 2 r :+ p
mid(Point 2 r :+ p)
-> Getting (Point 2 r) (Point 2 r :+ p) (Point 2 r) -> Point 2 r
forall s a. s -> Getting a s a -> a
^.Getting (Point 2 r) (Point 2 r :+ p) (Point 2 r)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core)
    rightSide :: SideTest
rightSide = (Point 2 r :+ p
r(Point 2 r :+ p)
-> Getting (Point 2 r) (Point 2 r :+ p) (Point 2 r) -> Point 2 r
forall s a. s -> Getting a s a -> a
^.Getting (Point 2 r) (Point 2 r :+ p) (Point 2 r)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core) Point 2 r -> Line 2 r -> SideTest
forall r. (Ord r, Num r) => Point 2 r -> Line 2 r -> SideTest
`onSide` Line 2 r
splitL -- define the side containing r the right side

    ([Point 2 r :+ p]
ls,[Point 2 r :+ p]
rs) = ((Point 2 r :+ p) -> Bool)
-> [Point 2 r :+ p] -> ([Point 2 r :+ p], [Point 2 r :+ p])
forall a. (a -> Bool) -> [a] -> ([a], [a])
List.partition (\(Point 2 r
p :+ p
_) -> Point 2 r
p Point 2 r -> Line 2 r -> SideTest
forall r. (Ord r, Num r) => Point 2 r -> Line 2 r -> SideTest
`onSide` Line 2 r
splitL SideTest -> SideTest -> Bool
forall a. Eq a => a -> a -> Bool
/= SideTest
rightSide)
            ([Point 2 r :+ p] -> ([Point 2 r :+ p], [Point 2 r :+ p]))
-> ([Point 2 r :+ p] -> [Point 2 r :+ p])
-> [Point 2 r :+ p]
-> ([Point 2 r :+ p], [Point 2 r :+ p])
forall b c a. (b -> c) -> (a -> b) -> a -> c
. ((Point 2 r :+ p) -> Bool) -> [Point 2 r :+ p] -> [Point 2 r :+ p]
forall a. (a -> Bool) -> [a] -> [a]
filter (\(Point 2 r
p :+ p
_) -> Bool -> Bool
not (Bool -> Bool) -> Bool -> Bool
forall a b. (a -> b) -> a -> b
$ Point 2 r
p Point 2 r -> Triangle 2 p r -> Bool
forall r p.
(Ord r, Fractional r) =>
Point 2 r -> Triangle 2 p r -> Bool
`onTriangle` Triangle 2 p r
t) ([Point 2 r :+ p] -> ([Point 2 r :+ p], [Point 2 r :+ p]))
-> [Point 2 r :+ p] -> ([Point 2 r :+ p], [Point 2 r :+ p])
forall a b. (a -> b) -> a -> b
$ [Point 2 r :+ p]
pts


-- mPoint2 [x,y] = Point2 x y

-- -- testPoints = NonEmpty.fromList
-- --   [ mPoint2 [22536303956634 % 7570647828779,(-5816376064439) % 1228319866920] :+ 1
-- --   , mPoint2 [(-3136920648983) % 824638230353,(-14583744643665) % 9604445576558] :+ 2
-- --   , mPoint2 [(-11653462784667) % 6525086575987,(-598434515815) % 1364557986096] :+ 3
-- --   , mPoint2 [(-7841595901661) % 3282967141364,(-207167076115) % 482378191549] :+ 4
-- --   ]


-- testPoints :: NonEmpty (Point 2 Rational :+ Int)
-- testPoints = read "(Point2 [(-11199966464450) % 1365514034959,4065659138075 % 2296468530516] :+ 1) :| [Point2 [86686001553073 % 2736621704548,(-63774454571048) % 1880665081093] :+ 2,Point2 [(-77322324895231) % 8260610289790,(-41165682123514) % 2291705829063] :+ 3,Point2 [2292642905947 % 2659329735076,87045289726355 % 2752214350419] :+ 4]"


-- toDouble          :: Point 2 Rational :+ a -> Point 2 Double :+ a
-- toDouble (p :+ x) = (realToFrac <$> p) :+ x


--        -- Falsified (after 36 tests and 4 shrinks):



-- testPoints2 :: NonEmpty (Point 2 Rational :+ Int)
-- testPoints2 = read "(Point2 [(-9876468593885) % 9254762894818,(-34982972348716) % 7450362538495] :+ 1) :| [Point2 [(-11974177119403) % 7705693443554,(-37634868551543) % 9311528788922] :+ 2,Point2 [(-32383659855458) % 9565531378857,20253950785876 % 8268868939819] :+ 3,Point2 [42425655100996 % 8786996213535,(-7972873491283) % 1604043452399] :+ 4]"