--------------------------------------------------------------------------------
-- |
-- Module      :  Algorithms.Geometry.ConvexHull.Naive
-- Copyright   :  (C) Frank Staals
-- License     :  see the LICENSE file
-- Maintainer  :  Frank Staals
--------------------------------------------------------------------------------
module Algorithms.Geometry.ConvexHull.Naive( ConvexHull
                                           , lowerHull', lowerHullAll

                                           , isValidTriangle, upperHalfSpaceOf
                                           ) where

import           Control.Lens
import           Data.Ext
import           Data.Foldable (toList)
import           Data.Geometry.HalfSpace
import           Data.Geometry.HyperPlane
import           Data.Geometry.Line
import           Data.Geometry.Point
import           Data.Geometry.Triangle
import           Data.Geometry.Vector
import           Data.Intersection(intersects)
import           Data.List.NonEmpty (NonEmpty(..))
import           Data.List (find)
import           Data.Maybe (isNothing)
import           Data.Util
--------------------------------------------------------------------------------

type ConvexHull d p r = [Triangle 3 p r]

-- | Computes the lower hull without its vertical triangles.
--
-- pre: The points are in general position. In particular, no four
-- points should be coplanar.
--
-- running time: \(O(n^4)\)
lowerHull' :: forall r p. (Ord r, Fractional r, Show r)
           => NonEmpty (Point 3 r :+ p) -> ConvexHull 3 p r
lowerHull' :: NonEmpty (Point 3 r :+ p) -> ConvexHull 3 p r
lowerHull' = (Triangle 3 p r -> Bool) -> ConvexHull 3 p r -> ConvexHull 3 p r
forall a. (a -> Bool) -> [a] -> [a]
filter (Bool -> Bool
not (Bool -> Bool)
-> (Triangle 3 p r -> Bool) -> Triangle 3 p r -> Bool
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Triangle 3 p r -> Bool
forall (d :: Nat) r c.
(ImplicitPeano (Peano d), Num r, Ord r,
 ArityPeano (Peano (FromPeano (Peano d))),
 KnownNat (FromPeano (Peano d)), KnownNat d, (2 <=? d) ~ 'True,
 Peano (FromPeano (Peano d) + 1)
 ~ 'S (Peano (FromPeano (Peano d)))) =>
Triangle d c r -> Bool
isVertical) (ConvexHull 3 p r -> ConvexHull 3 p r)
-> (NonEmpty (Point 3 r :+ p) -> ConvexHull 3 p r)
-> NonEmpty (Point 3 r :+ p)
-> ConvexHull 3 p r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. NonEmpty (Point 3 r :+ p) -> ConvexHull 3 p r
forall r p.
(Ord r, Fractional r, Show r) =>
NonEmpty (Point 3 r :+ p) -> ConvexHull 3 p r
lowerHullAll
  where
    isVertical :: Triangle d c r -> Bool
isVertical (Triangle Point d r :+ c
p Point d r :+ c
q Point d r :+ c
r) =
      (Point 2 r :+ c) -> (Point 2 r :+ c) -> (Point 2 r :+ c) -> CCW
forall r a b c.
(Ord r, Num r) =>
(Point 2 r :+ a) -> (Point 2 r :+ b) -> (Point 2 r :+ c) -> CCW
ccw' (Point d r :+ c
p(Point d r :+ c)
-> ((Point d r :+ c) -> Point 2 r :+ c) -> Point 2 r :+ c
forall a b. a -> (a -> b) -> b
&(Point d r -> Identity (Point 2 r))
-> (Point d r :+ c) -> Identity (Point 2 r :+ c)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core ((Point d r -> Identity (Point 2 r))
 -> (Point d r :+ c) -> Identity (Point 2 r :+ c))
-> (Point d r -> Point 2 r) -> (Point d r :+ c) -> Point 2 r :+ c
forall s t a b. ASetter s t a b -> (a -> b) -> s -> t
%~ Point d r -> Point 2 r
forall (i :: Nat) (d :: Nat) r.
(Arity i, Arity d, i <= d) =>
Point d r -> Point i r
projectPoint) (Point d r :+ c
q(Point d r :+ c)
-> ((Point d r :+ c) -> Point 2 r :+ c) -> Point 2 r :+ c
forall a b. a -> (a -> b) -> b
&(Point d r -> Identity (Point 2 r))
-> (Point d r :+ c) -> Identity (Point 2 r :+ c)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core ((Point d r -> Identity (Point 2 r))
 -> (Point d r :+ c) -> Identity (Point 2 r :+ c))
-> (Point d r -> Point 2 r) -> (Point d r :+ c) -> Point 2 r :+ c
forall s t a b. ASetter s t a b -> (a -> b) -> s -> t
%~ Point d r -> Point 2 r
forall (i :: Nat) (d :: Nat) r.
(Arity i, Arity d, i <= d) =>
Point d r -> Point i r
projectPoint) (Point d r :+ c
r(Point d r :+ c)
-> ((Point d r :+ c) -> Point 2 r :+ c) -> Point 2 r :+ c
forall a b. a -> (a -> b) -> b
&(Point d r -> Identity (Point 2 r))
-> (Point d r :+ c) -> Identity (Point 2 r :+ c)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core ((Point d r -> Identity (Point 2 r))
 -> (Point d r :+ c) -> Identity (Point 2 r :+ c))
-> (Point d r -> Point 2 r) -> (Point d r :+ c) -> Point 2 r :+ c
forall s t a b. ASetter s t a b -> (a -> b) -> s -> t
%~ Point d r -> Point 2 r
forall (i :: Nat) (d :: Nat) r.
(Arity i, Arity d, i <= d) =>
Point d r -> Point i r
projectPoint) CCW -> CCW -> Bool
forall a. Eq a => a -> a -> Bool
== CCW
CoLinear

-- | Generates a set of triangles to be used to construct a complete
-- convex hull. In particular, it may contain vertical triangles.
--
-- pre: The points are in general position. In particular, no four
-- points should be coplanar.
--
-- running time: \(O(n^4)\)
lowerHullAll                 :: forall r p. (Ord r, Fractional r, Show r)
                             => NonEmpty (Point 3 r :+ p) -> ConvexHull 3 p r
lowerHullAll :: NonEmpty (Point 3 r :+ p) -> ConvexHull 3 p r
lowerHullAll (NonEmpty (Point 3 r :+ p) -> [Point 3 r :+ p]
forall (t :: * -> *) a. Foldable t => t a -> [a]
toList -> [Point 3 r :+ p]
pts) = let mkT :: Three (Point d r :+ p) -> Triangle d p r
mkT (Three Point d r :+ p
p Point d r :+ p
q Point d r :+ p
r) = (Point d r :+ p)
-> (Point d r :+ p) -> (Point d r :+ p) -> Triangle d 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 d r :+ p
p Point d r :+ p
q Point d r :+ p
r in
    [ Triangle 3 p r
t | Triangle 3 p r
t <- Three (Point 3 r :+ p) -> Triangle 3 p r
forall (d :: Nat) r p. Three (Point d r :+ p) -> Triangle d p r
mkT (Three (Point 3 r :+ p) -> Triangle 3 p r)
-> [Three (Point 3 r :+ p)] -> ConvexHull 3 p r
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> [Point 3 r :+ p] -> [Three (Point 3 r :+ p)]
forall a. [a] -> [Three a]
uniqueTriplets [Point 3 r :+ p]
pts, Maybe (Point 3 r :+ p) -> Bool
forall a. Maybe a -> Bool
isNothing (Triangle 3 p r -> [Point 3 r :+ p] -> Maybe (Point 3 r :+ p)
forall r p q.
(Num r, Ord r) =>
Triangle 3 p r -> [Point 3 r :+ q] -> Maybe (Point 3 r :+ q)
isValidTriangle Triangle 3 p r
t [Point 3 r :+ p]
pts) ]



_killOverlapping :: (Ord r, Fractional r) => [Triangle 3 p r] -> [Triangle 3 p r]
_killOverlapping :: [Triangle 3 p r] -> [Triangle 3 p r]
_killOverlapping = (Triangle 3 p r -> [Triangle 3 p r] -> [Triangle 3 p r])
-> [Triangle 3 p r] -> [Triangle 3 p r] -> [Triangle 3 p r]
forall (t :: * -> *) a b.
Foldable t =>
(a -> b -> b) -> b -> t a -> b
foldr Triangle 3 p r -> [Triangle 3 p r] -> [Triangle 3 p r]
forall r p1.
(Fractional r, Ord r) =>
Triangle 3 p1 r -> [Triangle 3 p1 r] -> [Triangle 3 p1 r]
keepIfNotOverlaps []
  where
    keepIfNotOverlaps :: Triangle 3 p1 r -> [Triangle 3 p1 r] -> [Triangle 3 p1 r]
keepIfNotOverlaps Triangle 3 p1 r
t [Triangle 3 p1 r]
ts | (Triangle 3 p1 r -> Bool) -> [Triangle 3 p1 r] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
any (Triangle 3 p1 r
t Triangle 3 p1 r -> Triangle 3 p1 r -> Bool
forall r p1 p2.
(Fractional r, Ord r) =>
Triangle 3 p1 r -> Triangle 3 p2 r -> Bool
`overlaps`) [Triangle 3 p1 r]
ts = [Triangle 3 p1 r]
ts
                           | Bool
otherwise             = Triangle 3 p1 r
tTriangle 3 p1 r -> [Triangle 3 p1 r] -> [Triangle 3 p1 r]
forall a. a -> [a] -> [a]
:[Triangle 3 p1 r]
ts

overlaps :: (Fractional r, Ord r) => Triangle 3 p1 r -> Triangle 3 p2 r -> Bool
Triangle 3 p1 r
t1 overlaps :: Triangle 3 p1 r -> Triangle 3 p2 r -> Bool
`overlaps` Triangle 3 p2 r
t2 = Triangle 3 p1 r -> HalfSpace 3 r
forall r p. (Ord r, Num r) => Triangle 3 p r -> HalfSpace 3 r
upperHalfSpaceOf Triangle 3 p1 r
t1 HalfSpace 3 r -> HalfSpace 3 r -> Bool
forall a. Eq a => a -> a -> Bool
== Triangle 3 p2 r -> HalfSpace 3 r
forall r p. (Ord r, Num r) => Triangle 3 p r -> HalfSpace 3 r
upperHalfSpaceOf Triangle 3 p2 r
t2 Bool -> Bool -> Bool
&& Bool
False



-- | Tests if this is a valid triangle for the lower envelope. That
-- is, if all point lie above the plane through these points. Returns
-- a Maybe; if the result is a Nothing the triangle is valid, if not
-- it returns a counter example.
--
-- >>> let t = (Triangle (ext origin) (ext $ Point3 1 0 0) (ext $ Point3 0 1 0))
-- >>> isValidTriangle t [ext $ Point3 5 5 0]
-- Nothing
-- >>> let t = (Triangle (ext origin) (ext $ Point3 1 0 0) (ext $ Point3 0 1 0))
-- >>> isValidTriangle t [ext $ Point3 5 5 (-10)]
-- Just (Point3 5 5 (-10) :+ ())
isValidTriangle   :: (Num r, Ord r)
                  => Triangle 3 p r -> [Point 3 r :+ q] -> Maybe (Point 3 r :+ q)
isValidTriangle :: Triangle 3 p r -> [Point 3 r :+ q] -> Maybe (Point 3 r :+ q)
isValidTriangle Triangle 3 p r
t = ((Point 3 r :+ q) -> Bool)
-> [Point 3 r :+ q] -> Maybe (Point 3 r :+ q)
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Maybe a
find (\Point 3 r :+ q
a -> Bool -> Bool
not (Bool -> Bool) -> Bool -> Bool
forall a b. (a -> b) -> a -> b
$ (Point 3 r :+ q
a(Point 3 r :+ q)
-> Getting (Point 3 r) (Point 3 r :+ q) (Point 3 r) -> Point 3 r
forall s a. s -> Getting a s a -> a
^.Getting (Point 3 r) (Point 3 r :+ q) (Point 3 r)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core) Point 3 r -> HalfSpace 3 r -> Bool
forall g h. IsIntersectableWith g h => g -> h -> Bool
`intersects` HalfSpace 3 r
h)
  where
    h :: HalfSpace 3 r
h = Triangle 3 p r -> HalfSpace 3 r
forall r p. (Ord r, Num r) => Triangle 3 p r -> HalfSpace 3 r
upperHalfSpaceOf Triangle 3 p r
t


-- | Computes the halfspace above the triangle.
--
-- >>> upperHalfSpaceOf (Triangle (ext $ origin) (ext $ Point3 10 0 0) (ext $ Point3 0 10 0))
-- HalfSpace {_boundingPlane = HyperPlane {_inPlane = Point3 0 0 0, _normalVec = Vector3 0 0 100}}
upperHalfSpaceOf                  :: (Ord r, Num r) => Triangle 3 p r -> HalfSpace 3 r
upperHalfSpaceOf :: Triangle 3 p r -> HalfSpace 3 r
upperHalfSpaceOf (Triangle Point 3 r :+ p
p Point 3 r :+ p
q Point 3 r :+ p
r) = HyperPlane 3 r -> HalfSpace 3 r
forall (d :: Nat) r. HyperPlane d r -> HalfSpace d r
HalfSpace HyperPlane 3 r
h
  where
    h' :: HyperPlane 3 r
h' = Point 3 r -> Point 3 r -> Point 3 r -> HyperPlane 3 r
forall r.
Num r =>
Point 3 r -> Point 3 r -> Point 3 r -> HyperPlane 3 r
from3Points (Point 3 r :+ p
p(Point 3 r :+ p)
-> Getting (Point 3 r) (Point 3 r :+ p) (Point 3 r) -> Point 3 r
forall s a. s -> Getting a s a -> a
^.Getting (Point 3 r) (Point 3 r :+ p) (Point 3 r)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core) (Point 3 r :+ p
q(Point 3 r :+ p)
-> Getting (Point 3 r) (Point 3 r :+ p) (Point 3 r) -> Point 3 r
forall s a. s -> Getting a s a -> a
^.Getting (Point 3 r) (Point 3 r :+ p) (Point 3 r)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core) (Point 3 r :+ p
r(Point 3 r :+ p)
-> Getting (Point 3 r) (Point 3 r :+ p) (Point 3 r) -> Point 3 r
forall s a. s -> Getting a s a -> a
^.Getting (Point 3 r) (Point 3 r :+ p) (Point 3 r)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core)
    c :: Point 3 r :+ p
c  = Point 3 r :+ p
p(Point 3 r :+ p)
-> ((Point 3 r :+ p) -> Point 3 r :+ p) -> Point 3 r :+ p
forall a b. a -> (a -> b) -> b
&(Point 3 r -> Identity (Point 3 r))
-> (Point 3 r :+ p) -> Identity (Point 3 r :+ p)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core((Point 3 r -> Identity (Point 3 r))
 -> (Point 3 r :+ p) -> Identity (Point 3 r :+ p))
-> ((r -> Identity r) -> Point 3 r -> Identity (Point 3 r))
-> (r -> Identity r)
-> (Point 3 r :+ p)
-> Identity (Point 3 r :+ p)
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(r -> Identity r) -> Point 3 r -> Identity (Point 3 r)
forall (d :: Nat) (point :: Nat -> * -> *) r.
(3 <= d, Arity d, AsAPoint point) =>
Lens' (point d r) r
zCoord ((r -> Identity r)
 -> (Point 3 r :+ p) -> Identity (Point 3 r :+ p))
-> r -> (Point 3 r :+ p) -> Point 3 r :+ p
forall a s t. Num a => ASetter s t a a -> a -> s -> t
-~ r
1
    h :: HyperPlane 3 r
h  = if (Point 3 r :+ p
c(Point 3 r :+ p)
-> Getting (Point 3 r) (Point 3 r :+ p) (Point 3 r) -> Point 3 r
forall s a. s -> Getting a s a -> a
^.Getting (Point 3 r) (Point 3 r :+ p) (Point 3 r)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core) Point (Dimension (HyperPlane 3 r)) (NumType (HyperPlane 3 r))
-> HyperPlane 3 r -> Bool
forall t.
(OnSideUpDownTest t, Ord (NumType t), Num (NumType t)) =>
Point (Dimension t) (NumType t) -> t -> Bool
`liesBelow` HyperPlane 3 r
h' then HyperPlane 3 r
h' else HyperPlane 3 r
h'HyperPlane 3 r
-> (HyperPlane 3 r -> HyperPlane 3 r) -> HyperPlane 3 r
forall a b. a -> (a -> b) -> b
&(Vector 3 r -> Identity (Vector 3 r))
-> HyperPlane 3 r -> Identity (HyperPlane 3 r)
forall (d :: Nat) r. Lens' (HyperPlane d r) (Vector d r)
normalVec ((Vector 3 r -> Identity (Vector 3 r))
 -> HyperPlane 3 r -> Identity (HyperPlane 3 r))
-> (Vector 3 r -> Vector 3 r) -> HyperPlane 3 r -> HyperPlane 3 r
forall s t a b. ASetter s t a b -> (a -> b) -> s -> t
%~ ((-r
1) r -> Vector 3 r -> Vector 3 r
forall (f :: * -> *) a. (Functor f, Num a) => a -> f a -> f a
*^)
    Point (Dimension t) (NumType t)
a liesBelow :: Point (Dimension t) (NumType t) -> t -> Bool
`liesBelow` t
plane = (Point (Dimension t) (NumType t)
a Point (Dimension t) (NumType t) -> t -> SideTestUpDown
forall t (d :: Nat) r.
(OnSideUpDownTest t, d ~ Dimension t, r ~ NumType t, Ord r,
 Num r) =>
Point d r -> t -> SideTestUpDown
`onSideUpDown` t
plane) SideTestUpDown -> SideTestUpDown -> Bool
forall a. Eq a => a -> a -> Bool
== SideTestUpDown
Below