{-# LANGUAGE UndecidableInstances #-}
{-# LANGUAGE TemplateHaskell #-}
--------------------------------------------------------------------------------
-- |
-- Module      :  Data.Geometry.HalfSpace
-- Copyright   :  (C) Frank Staals
-- License     :  see the LICENSE file
-- Maintainer  :  Frank Staals
--
-- \(d\)-dimensional HalfSpaces
--
--------------------------------------------------------------------------------
module Data.Geometry.HalfSpace where

import Control.Lens
import Data.Geometry.HalfLine
import Data.Geometry.HyperPlane
import Data.Geometry.Line
import Data.Geometry.Point
import Data.Geometry.Properties
import Data.Geometry.Transformation
import Data.Geometry.Boundary
import Data.Geometry.Vector
import Data.Vinyl.CoRec
import Data.Vinyl.Core
import GHC.Generics (Generic)
import GHC.TypeLits

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

-- $setup
-- >>> :{
-- let myVector :: Vector 3 Int
--     myVector = Vector3 1 2 3
--     myPoint = Point myVector
-- :}


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

-- | A Halfspace in \(d\) dimensions. Note that the intended side of
-- the halfspace is already indicated by the normal vector of the
-- bounding plane.
newtype HalfSpace d r = HalfSpace { HalfSpace d r -> HyperPlane d r
_boundingPlane :: HyperPlane d  r }
                       deriving (forall x. HalfSpace d r -> Rep (HalfSpace d r) x)
-> (forall x. Rep (HalfSpace d r) x -> HalfSpace d r)
-> Generic (HalfSpace d r)
forall x. Rep (HalfSpace d r) x -> HalfSpace d r
forall x. HalfSpace d r -> Rep (HalfSpace d r) x
forall a.
(forall x. a -> Rep a x) -> (forall x. Rep a x -> a) -> Generic a
forall (d :: Nat) r x. Rep (HalfSpace d r) x -> HalfSpace d r
forall (d :: Nat) r x. HalfSpace d r -> Rep (HalfSpace d r) x
$cto :: forall (d :: Nat) r x. Rep (HalfSpace d r) x -> HalfSpace d r
$cfrom :: forall (d :: Nat) r x. HalfSpace d r -> Rep (HalfSpace d r) x
Generic
makeLenses ''HalfSpace

deriving instance (Arity d, Show r)   => Show    (HalfSpace d r)
-- deriving instance (NFData r, Arity d) => NFData  (HalfSpace d r)
deriving instance Arity d => Functor     (HalfSpace d)
deriving instance Arity d => Foldable    (HalfSpace d)
deriving instance Arity d => Traversable (HalfSpace d)

type instance NumType (HalfSpace d r)   = r
type instance Dimension (HalfSpace d r) = d

deriving instance (Arity d, Arity (d + 1), Fractional r) => IsTransformable (HalfSpace d r)

instance (Arity d, Eq r, Fractional r) => Eq (HalfSpace d r) where
  (HalfSpace HyperPlane d r
h) == :: HalfSpace d r -> HalfSpace d r -> Bool
== (HalfSpace HyperPlane d r
h') = let u :: Vector d r
u = HyperPlane d r
hHyperPlane d r
-> Getting (Vector d r) (HyperPlane d r) (Vector d r) -> Vector d r
forall s a. s -> Getting a s a -> a
^.Getting (Vector d r) (HyperPlane d r) (Vector d r)
forall (d :: Nat) r. Lens' (HyperPlane d r) (Vector d r)
normalVec
                                        v :: Vector d r
v = HyperPlane d r
h'HyperPlane d r
-> Getting (Vector d r) (HyperPlane d r) (Vector d r) -> Vector d r
forall s a. s -> Getting a s a -> a
^.Getting (Vector d r) (HyperPlane d r) (Vector d r)
forall (d :: Nat) r. Lens' (HyperPlane d r) (Vector d r)
normalVec
                                        d :: r
d = Vector d r -> r
forall (f :: * -> *) a. (Metric f, Num a) => f a -> a
quadrance (Vector d r
u Vector d r -> Vector d r -> Vector d r
forall (f :: * -> *) a. (Additive f, Num a) => f a -> f a -> f a
^+^ Vector d r
v) r -> r -> r
forall a. Num a => a -> a -> a
- Vector d r -> r
forall (f :: * -> *) a. (Metric f, Num a) => f a -> a
quadrance Vector d r
u
                                    in HyperPlane d r
h HyperPlane d r -> HyperPlane d r -> Bool
forall a. Eq a => a -> a -> Bool
== HyperPlane d r
h' Bool -> Bool -> Bool
&& r -> r
forall a. Num a => a -> a
signum r
d r -> r -> Bool
forall a. Eq a => a -> a -> Bool
== r
1

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

type HalfPlane = HalfSpace 2


{- HLINT ignore leftOf -}
-- | Get the halfplane left of a line (i.e. "above") a line
--
-- >>> leftOf $ horizontalLine 4
-- HalfSpace {_boundingPlane = HyperPlane {_inPlane = Point2 0 4, _normalVec = Vector2 0 1}}
leftOf   :: Num r => Line 2 r -> HalfPlane r
leftOf :: Line 2 r -> HalfPlane r
leftOf Line 2 r
l = (Line 2 r -> HalfPlane r
forall r. Num r => Line 2 r -> HalfPlane r
rightOf Line 2 r
l)HalfPlane r -> (HalfPlane r -> HalfPlane r) -> HalfPlane r
forall a b. a -> (a -> b) -> b
&(HyperPlane 2 r -> Identity (HyperPlane 2 r))
-> HalfPlane r -> Identity (HalfPlane r)
forall (d :: Nat) r (d :: Nat) r.
Iso
  (HalfSpace d r) (HalfSpace d r) (HyperPlane d r) (HyperPlane d r)
boundingPlane((HyperPlane 2 r -> Identity (HyperPlane 2 r))
 -> HalfPlane r -> Identity (HalfPlane r))
-> ((Vector 2 r -> Identity (Vector 2 r))
    -> HyperPlane 2 r -> Identity (HyperPlane 2 r))
-> (Vector 2 r -> Identity (Vector 2 r))
-> HalfPlane r
-> Identity (HalfPlane r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(Vector 2 r -> Identity (Vector 2 r))
-> HyperPlane 2 r -> Identity (HyperPlane 2 r)
forall (d :: Nat) r. Lens' (HyperPlane d r) (Vector d r)
normalVec ((Vector 2 r -> Identity (Vector 2 r))
 -> HalfPlane r -> Identity (HalfPlane r))
-> (Vector 2 r -> Vector 2 r) -> HalfPlane r -> HalfPlane r
forall s t a b. ASetter s t a b -> (a -> b) -> s -> t
%~ ((-r
1) r -> Vector 2 r -> Vector 2 r
forall (f :: * -> *) a. (Functor f, Num a) => a -> f a -> f a
*^)

-- | Get the halfplane right of a line (i.e. "below") a line
--
-- >>> rightOf $ horizontalLine 4
-- HalfSpace {_boundingPlane = HyperPlane {_inPlane = Point2 0 4, _normalVec = Vector2 0 (-1)}}
rightOf   :: Num r => Line 2 r -> HalfPlane r
rightOf :: Line 2 r -> HalfPlane r
rightOf Line 2 r
l = HyperPlane 2 r -> HalfPlane r
forall (d :: Nat) r. HyperPlane d r -> HalfSpace d r
HalfSpace (HyperPlane 2 r -> HalfPlane r) -> HyperPlane 2 r -> HalfPlane r
forall a b. (a -> b) -> a -> b
$ Line 2 r
lLine 2 r
-> Getting (HyperPlane 2 r) (Line 2 r) (HyperPlane 2 r)
-> HyperPlane 2 r
forall s a. s -> Getting a s a -> a
^.AReview (HyperPlane 2 r) (Line 2 r)
-> Getter (Line 2 r) (HyperPlane 2 r)
forall t b. AReview t b -> Getter b t
re AReview (HyperPlane 2 r) (Line 2 r)
forall r. Num r => Iso' (HyperPlane 2 r) (Line 2 r)
_asLine

above :: Num r => Line 2 r -> HalfPlane r
above :: Line 2 r -> HalfPlane r
above = Line 2 r -> HalfPlane r
forall r. Num r => Line 2 r -> HalfPlane r
leftOf

below :: Num r => Line 2 r -> HalfPlane r
below :: Line 2 r -> HalfPlane r
below = Line 2 r -> HalfPlane r
forall r. Num r => Line 2 r -> HalfPlane r
rightOf

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

-- type HalfPlane r = GHalfSpace (Line 2 r)

-- type HalfSpace d r = GHalfSpace (HyperPlane d r)

-- TODO: Property test that in 2d this is the same as CCW

type instance IntersectionOf (Point d r) (HalfSpace d r) = [NoIntersection, Point d r]

instance (Num r, Ord r, Arity d) => Point d r `IsIntersectableWith` HalfSpace d r where
  nonEmptyIntersection :: proxy (Point d r)
-> proxy (HalfSpace d r)
-> Intersection (Point d r) (HalfSpace d r)
-> Bool
nonEmptyIntersection = proxy (Point d r)
-> proxy (HalfSpace d r)
-> Intersection (Point d r) (HalfSpace d r)
-> Bool
forall g h (proxy :: * -> *).
(NoIntersection ∈ IntersectionOf g h,
 RecApplicative (IntersectionOf g h)) =>
proxy g -> proxy h -> Intersection g h -> Bool
defaultNonEmptyIntersection

  Point d r
q intersects :: Point d r -> HalfSpace d r -> Bool
`intersects` HalfSpace d r
h = Point d r
q Point d r -> HalfSpace d r -> PointLocationResult
forall r (d :: Nat).
(Num r, Ord r, Arity d) =>
Point d r -> HalfSpace d r -> PointLocationResult
`inHalfSpace` HalfSpace d r
h PointLocationResult -> PointLocationResult -> Bool
forall a. Eq a => a -> a -> Bool
/= PointLocationResult
Outside

  Point d r
q intersect :: Point d r
-> HalfSpace d r -> Intersection (Point d r) (HalfSpace d r)
`intersect` HalfSpace d r
h | Point d r
q Point d r -> HalfSpace d r -> Bool
forall g h. IsIntersectableWith g h => g -> h -> Bool
`intersects` HalfSpace d r
h = Point d r -> CoRec Identity '[NoIntersection, Point d r]
forall a (as :: [*]). (a ∈ as) => a -> CoRec Identity as
coRec Point d r
q
                  | Bool
otherwise        = NoIntersection -> CoRec Identity '[NoIntersection, Point d r]
forall a (as :: [*]). (a ∈ as) => a -> CoRec Identity as
coRec NoIntersection
NoIntersection



type instance IntersectionOf (Line d r) (HalfSpace d r) =
    [NoIntersection, HalfLine d r, Line d r]


instance (Fractional r, Ord r) => Line 2 r `IsIntersectableWith` HalfSpace 2 r where
  nonEmptyIntersection :: proxy (Line 2 r)
-> proxy (HalfSpace 2 r)
-> Intersection (Line 2 r) (HalfSpace 2 r)
-> Bool
nonEmptyIntersection = proxy (Line 2 r)
-> proxy (HalfSpace 2 r)
-> Intersection (Line 2 r) (HalfSpace 2 r)
-> Bool
forall g h (proxy :: * -> *).
(NoIntersection ∈ IntersectionOf g h,
 RecApplicative (IntersectionOf g h)) =>
proxy g -> proxy h -> Intersection g h -> Bool
defaultNonEmptyIntersection

  l :: Line 2 r
l@(Line Point 2 r
o Vector 2 r
v) intersect :: Line 2 r
-> HalfSpace 2 r -> Intersection (Line 2 r) (HalfSpace 2 r)
`intersect` HalfSpace 2 r
h = CoRec Identity '[NoIntersection, Point 2 r, Line 2 r]
-> Handlers
     '[NoIntersection, Point 2 r, Line 2 r]
     (CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r])
-> CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r]
forall (ts :: [*]) b. CoRec Identity ts -> Handlers ts b -> b
match (Line 2 r
l Line 2 r -> Line 2 r -> Intersection (Line 2 r) (Line 2 r)
forall g h. IsIntersectableWith g h => g -> h -> Intersection g h
`intersect` Line 2 r
m) (Handlers
   '[NoIntersection, Point 2 r, Line 2 r]
   (CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r])
 -> CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r])
-> Handlers
     '[NoIntersection, Point 2 r, Line 2 r]
     (CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r])
-> CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r]
forall a b. (a -> b) -> a -> b
$
         (NoIntersection
 -> CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r])
-> Handler
     (CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r])
     NoIntersection
forall b a. (a -> b) -> Handler b a
H (\NoIntersection
NoIntersection -> if Point 2 r
o Point 2 r -> HalfSpace 2 r -> Bool
forall g h. IsIntersectableWith g h => g -> h -> Bool
`intersects` HalfSpace 2 r
h
                                   then Line 2 r
-> CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r]
forall a (as :: [*]). (a ∈ as) => a -> CoRec Identity as
coRec Line 2 r
l
                                   else NoIntersection
-> CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r]
forall a (as :: [*]). (a ∈ as) => a -> CoRec Identity as
coRec NoIntersection
NoIntersection)
      Handler
  (CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r])
  NoIntersection
-> Rec
     (Handler
        (CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r]))
     '[Point 2 r, Line 2 r]
-> Handlers
     '[NoIntersection, Point 2 r, Line 2 r]
     (CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r])
forall u (a :: u -> *) (r :: u) (rs :: [u]).
a r -> Rec a rs -> Rec a (r : rs)
:& (Point 2 r
 -> CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r])
-> Handler
     (CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r])
     (Point 2 r)
forall b a. (a -> b) -> Handler b a
H (\Point 2 r
p              -> if (Point 2 r
p Point 2 r -> Diff (Point 2) r -> Point 2 r
forall (p :: * -> *) a. (Affine p, Num a) => p a -> Diff p a -> p a
.+^ Diff (Point 2) r
Vector 2 r
v) Point 2 r -> HalfSpace 2 r -> Bool
forall g h. IsIntersectableWith g h => g -> h -> Bool
`intersects` HalfSpace 2 r
h
                                   then HalfLine 2 r
-> CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r]
forall a (as :: [*]). (a ∈ as) => a -> CoRec Identity as
coRec (HalfLine 2 r
 -> CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r])
-> HalfLine 2 r
-> CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r]
forall a b. (a -> b) -> a -> b
$ Point 2 r -> Vector 2 r -> HalfLine 2 r
forall (d :: Nat) r. Point d r -> Vector d r -> HalfLine d r
HalfLine Point 2 r
p Vector 2 r
v
                                   else HalfLine 2 r
-> CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r]
forall a (as :: [*]). (a ∈ as) => a -> CoRec Identity as
coRec (HalfLine 2 r
 -> CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r])
-> HalfLine 2 r
-> CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r]
forall a b. (a -> b) -> a -> b
$ Point 2 r -> Vector 2 r -> HalfLine 2 r
forall (d :: Nat) r. Point d r -> Vector d r -> HalfLine d r
HalfLine Point 2 r
p ((-r
1) r -> Vector 2 r -> Vector 2 r
forall (f :: * -> *) a. (Functor f, Num a) => a -> f a -> f a
*^ Vector 2 r
v))
      Handler
  (CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r])
  (Point 2 r)
-> Rec
     (Handler
        (CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r]))
     '[Line 2 r]
-> Rec
     (Handler
        (CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r]))
     '[Point 2 r, Line 2 r]
forall u (a :: u -> *) (r :: u) (rs :: [u]).
a r -> Rec a rs -> Rec a (r : rs)
:& (Line 2 r
 -> CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r])
-> Handler
     (CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r])
     (Line 2 r)
forall b a. (a -> b) -> Handler b a
H (\Line 2 r
_l             -> Line 2 r
-> CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r]
forall a (as :: [*]). (a ∈ as) => a -> CoRec Identity as
coRec Line 2 r
l)
      Handler
  (CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r])
  (Line 2 r)
-> Rec
     (Handler
        (CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r]))
     '[]
-> Rec
     (Handler
        (CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r]))
     '[Line 2 r]
forall u (a :: u -> *) (r :: u) (rs :: [u]).
a r -> Rec a rs -> Rec a (r : rs)
:& Rec
  (Handler
     (CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r]))
  '[]
forall u (a :: u -> *). Rec a '[]
RNil
    where
      m :: Line 2 r
m = HalfSpace 2 r
hHalfSpace 2 r
-> Getting (Line 2 r) (HalfSpace 2 r) (Line 2 r) -> Line 2 r
forall s a. s -> Getting a s a -> a
^.(HyperPlane 2 r -> Const (Line 2 r) (HyperPlane 2 r))
-> HalfSpace 2 r -> Const (Line 2 r) (HalfSpace 2 r)
forall (d :: Nat) r (d :: Nat) r.
Iso
  (HalfSpace d r) (HalfSpace d r) (HyperPlane d r) (HyperPlane d r)
boundingPlane((HyperPlane 2 r -> Const (Line 2 r) (HyperPlane 2 r))
 -> HalfSpace 2 r -> Const (Line 2 r) (HalfSpace 2 r))
-> ((Line 2 r -> Const (Line 2 r) (Line 2 r))
    -> HyperPlane 2 r -> Const (Line 2 r) (HyperPlane 2 r))
-> Getting (Line 2 r) (HalfSpace 2 r) (Line 2 r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(Line 2 r -> Const (Line 2 r) (Line 2 r))
-> HyperPlane 2 r -> Const (Line 2 r) (HyperPlane 2 r)
forall r. Num r => Iso' (HyperPlane 2 r) (Line 2 r)
_asLine


-- | Test if a point lies in a halfspace
inHalfSpace                                  :: (Num r, Ord r, Arity d)
                                             => Point d r -> HalfSpace d r
                                             -> PointLocationResult
Point d r
q inHalfSpace :: Point d r -> HalfSpace d r -> PointLocationResult
`inHalfSpace` (HalfSpace (HyperPlane Point d r
p Vector d r
n)) = case Vector d r
n Vector d r -> Vector d r -> r
forall (f :: * -> *) a. (Metric f, Num a) => f a -> f a -> a
`dot` (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) r -> r -> Ordering
forall a. Ord a => a -> a -> Ordering
`compare` r
0 of
                                                 Ordering
LT -> PointLocationResult
Outside
                                                 Ordering
EQ -> PointLocationResult
OnBoundary
                                                 Ordering
GT -> PointLocationResult
Inside