{-# LANGUAGE UndecidableInstances #-}
{-# LANGUAGE PackageImports #-}
--------------------------------------------------------------------------------
-- |
-- Module      :  Algorithms.Geometry.LinearProgramming.LP2DRIC
-- Copyright   :  (C) Frank Staals
-- License     :  see the LICENSE file
-- Maintainer  :  Frank Staals
--
-- 2D Linear programming in expected linear time.
--
--------------------------------------------------------------------------------
module Algorithms.Geometry.LinearProgramming.LP2DRIC( solveBoundedLinearProgram
                                                    , solveBoundedLinearProgram'

                                                    , maximumOn
                                                    , oneDLinearProgramming
                                                    , commonIntersection
                                                    , cmpHalfPlane
                                                    ) where

import           Algorithms.Geometry.LinearProgramming.Types
import           Control.Lens
import           Control.Monad (foldM)
import           Control.Monad.Random.Class
import           Data.Ext
import qualified Data.Foldable as F
import           Data.Geometry.Boundary
import           Data.Geometry.HalfLine
import           Data.Geometry.HalfSpace
import           Data.Geometry.HyperPlane
import           Data.Geometry.Line
import           Data.Geometry.Point
import           Data.Geometry.Properties
import           Data.Geometry.Vector
import qualified Data.List.NonEmpty as NonEmpty
import           Data.Maybe (mapMaybe)
import           Data.Util
import           Data.Vinyl
import           Data.Vinyl.CoRec
import "hgeometry-combinatorial" System.Random.Shuffle

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

-- | Solve a linear program
_solveLinearProgram :: MonadRandom m => LinearProgram 2 r -> m (LPSolution 2 r)
_solveLinearProgram :: LinearProgram 2 r -> m (LPSolution 2 r)
_solveLinearProgram = LinearProgram 2 r -> m (LPSolution 2 r)
forall a. HasCallStack => a
undefined


-- | Solves a bounded linear program in 2d. Returns Nothing if there is no
-- solution.
--
-- pre: The linear program is bounded, meaning that *the first two constraints*
-- m1,m2 make sure th the there is no arbitrarily large/good
-- solution. I..e. these halfspaces bound the solution in the c direction.
--
-- (note that if there is only one constraint, m1, the assumption that the LP
-- is bounded means that the contraint must be perpendicular to the objective
-- direction. Hence, any point on the bounding plane is a solution, and they
-- are all equally good.)
--
-- \(O(n)\) expected time
--
--
solveBoundedLinearProgram                      :: (MonadRandom m, Ord r, Fractional r)
                                               => LinearProgram 2 r -> m (Maybe (Point 2 r))
solveBoundedLinearProgram :: LinearProgram 2 r -> m (Maybe (Point 2 r))
solveBoundedLinearProgram (LinearProgram Vector 2 r
c [HalfSpace 2 r]
hs') = case [HalfSpace 2 r]
hs' of
    []         -> Maybe (Point 2 r) -> m (Maybe (Point 2 r))
forall (f :: * -> *) a. Applicative f => a -> f a
pure Maybe (Point 2 r)
forall a. Maybe a
Nothing
    [HalfSpace 2 r
m1]       -> Maybe (Point 2 r) -> m (Maybe (Point 2 r))
forall (f :: * -> *) a. Applicative f => a -> f a
pure (Maybe (Point 2 r) -> m (Maybe (Point 2 r)))
-> Maybe (Point 2 r) -> m (Maybe (Point 2 r))
forall a b. (a -> b) -> a -> b
$ Point 2 r -> Maybe (Point 2 r)
forall a. a -> Maybe a
Just (HalfSpace 2 r
m1HalfSpace 2 r
-> Getting (Point 2 r) (HalfSpace 2 r) (Point 2 r) -> Point 2 r
forall s a. s -> Getting a s a -> a
^.(HyperPlane 2 r -> Const (Point 2 r) (HyperPlane 2 r))
-> HalfSpace 2 r -> Const (Point 2 r) (HalfSpace 2 r)
forall (d :: Nat) r (d2 :: Nat) r2.
Iso
  (HalfSpace d r)
  (HalfSpace d2 r2)
  (HyperPlane d r)
  (HyperPlane d2 r2)
boundingPlane((HyperPlane 2 r -> Const (Point 2 r) (HyperPlane 2 r))
 -> HalfSpace 2 r -> Const (Point 2 r) (HalfSpace 2 r))
-> ((Point 2 r -> Const (Point 2 r) (Point 2 r))
    -> HyperPlane 2 r -> Const (Point 2 r) (HyperPlane 2 r))
-> Getting (Point 2 r) (HalfSpace 2 r) (Point 2 r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(Point 2 r -> Const (Point 2 r) (Point 2 r))
-> HyperPlane 2 r -> Const (Point 2 r) (HyperPlane 2 r)
forall (d :: Nat) r. Lens' (HyperPlane d r) (Point d r)
inPlane)
    (HalfSpace 2 r
m1:HalfSpace 2 r
m2:[HalfSpace 2 r]
hs) -> LinearProgram 2 r -> Maybe (Point 2 r)
forall r.
(Ord r, Fractional r) =>
LinearProgram 2 r -> Maybe (Point 2 r)
solveBoundedLinearProgram' (LinearProgram 2 r -> Maybe (Point 2 r))
-> (Vector (HalfSpace 2 r) -> LinearProgram 2 r)
-> Vector (HalfSpace 2 r)
-> Maybe (Point 2 r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Vector 2 r -> [HalfSpace 2 r] -> LinearProgram 2 r
forall (d :: Nat) r.
Vector d r -> [HalfSpace d r] -> LinearProgram d r
LinearProgram Vector 2 r
c ([HalfSpace 2 r] -> LinearProgram 2 r)
-> (Vector (HalfSpace 2 r) -> [HalfSpace 2 r])
-> Vector (HalfSpace 2 r)
-> LinearProgram 2 r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. ([HalfSpace 2 r
m1,HalfSpace 2 r
m2] [HalfSpace 2 r] -> [HalfSpace 2 r] -> [HalfSpace 2 r]
forall a. [a] -> [a] -> [a]
++) ([HalfSpace 2 r] -> [HalfSpace 2 r])
-> (Vector (HalfSpace 2 r) -> [HalfSpace 2 r])
-> Vector (HalfSpace 2 r)
-> [HalfSpace 2 r]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Vector (HalfSpace 2 r) -> [HalfSpace 2 r]
forall (t :: * -> *) a. Foldable t => t a -> [a]
F.toList
                  (Vector (HalfSpace 2 r) -> Maybe (Point 2 r))
-> m (Vector (HalfSpace 2 r)) -> m (Maybe (Point 2 r))
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> [HalfSpace 2 r] -> m (Vector (HalfSpace 2 r))
forall (f :: * -> *) (m :: * -> *) a.
(Foldable f, MonadRandom m) =>
f a -> m (Vector a)
shuffle [HalfSpace 2 r]
hs


-- | Solves a bounded linear program (like 'solveBoundedLinearProgram')
-- assuming that the first two constraints [m1,m2] make sure the solutions is
-- bounded, and the other constraints already have been shuffled.
solveBoundedLinearProgram'    :: (Ord r, Fractional r)
                              => LinearProgram 2 r -> Maybe (Point 2 r)
solveBoundedLinearProgram' :: LinearProgram 2 r -> Maybe (Point 2 r)
solveBoundedLinearProgram' LinearProgram 2 r
lp = let (LPState 2 r
s,[HalfSpace 2 r]
hs) = LinearProgram 2 r -> (LPState 2 r, [HalfSpace 2 r])
forall r.
(Ord r, Fractional r) =>
LinearProgram 2 r -> (LPState 2 r, [HalfSpace 2 r])
initialize LinearProgram 2 r
lp
                                in (LPState 2 r
-> Getting (Point 2 r) (LPState 2 r) (Point 2 r) -> Point 2 r
forall s a. s -> Getting a s a -> a
^.Getting (Point 2 r) (LPState 2 r) (Point 2 r)
forall (d :: Nat) r. Lens' (LPState d r) (Point d r)
current) (LPState 2 r -> Point 2 r)
-> Maybe (LPState 2 r) -> Maybe (Point 2 r)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> (LPState 2 r -> HalfSpace 2 r -> Maybe (LPState 2 r))
-> LPState 2 r -> [HalfSpace 2 r] -> Maybe (LPState 2 r)
forall (t :: * -> *) (m :: * -> *) b a.
(Foldable t, Monad m) =>
(b -> a -> m b) -> b -> t a -> m b
foldM LPState 2 r -> HalfSpace 2 r -> Maybe (LPState 2 r)
forall r.
(Fractional r, Ord r) =>
LPState 2 r -> HalfSpace 2 r -> Maybe (LPState 2 r)
step LPState 2 r
s [HalfSpace 2 r]
hs

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

-- | State during the LP algo
data LPState d r = LPState { LPState d r -> Vector d r
_obj     :: !(Vector d r)
                           , LPState d r -> [HalfSpace d r]
_seen    :: [HalfSpace d r]
                           , LPState d r -> Point d r
_current :: !(Point d r)
                           }

deriving instance (Arity d, Show r)             => Show    (LPState d r)
deriving instance (Arity d, Eq r, Fractional r) => Eq      (LPState d r)

obj     :: Lens' (LPState d r) (Vector d r)
obj :: (Vector d r -> f (Vector d r)) -> LPState d r -> f (LPState d r)
obj     = (LPState d r -> Vector d r)
-> (LPState d r -> Vector d r -> LPState d r)
-> Lens (LPState d r) (LPState d r) (Vector d r) (Vector d r)
forall s a b t. (s -> a) -> (s -> b -> t) -> Lens s t a b
lens LPState d r -> Vector d r
forall (d :: Nat) r. LPState d r -> Vector d r
_obj     (\(LPState Vector d r
_ [HalfSpace d r]
s Point d r
p) Vector d r
o -> Vector d r -> [HalfSpace d r] -> Point d r -> LPState d r
forall (d :: Nat) r.
Vector d r -> [HalfSpace d r] -> Point d r -> LPState d r
LPState Vector d r
o [HalfSpace d r]
s Point d r
p)
seen    :: Lens' (LPState d r) [HalfSpace d r]
seen :: ([HalfSpace d r] -> f [HalfSpace d r])
-> LPState d r -> f (LPState d r)
seen    = (LPState d r -> [HalfSpace d r])
-> (LPState d r -> [HalfSpace d r] -> LPState d r)
-> Lens (LPState d r) (LPState d r) [HalfSpace d r] [HalfSpace d r]
forall s a b t. (s -> a) -> (s -> b -> t) -> Lens s t a b
lens LPState d r -> [HalfSpace d r]
forall (d :: Nat) r. LPState d r -> [HalfSpace d r]
_seen    (\(LPState Vector d r
o [HalfSpace d r]
_ Point d r
p) [HalfSpace d r]
s -> Vector d r -> [HalfSpace d r] -> Point d r -> LPState d r
forall (d :: Nat) r.
Vector d r -> [HalfSpace d r] -> Point d r -> LPState d r
LPState Vector d r
o [HalfSpace d r]
s Point d r
p)
current :: Lens' (LPState d r) (Point d r)
current :: (Point d r -> f (Point d r)) -> LPState d r -> f (LPState d r)
current = (LPState d r -> Point d r)
-> (LPState d r -> Point d r -> LPState d r)
-> Lens (LPState d r) (LPState d r) (Point d r) (Point d r)
forall s a b t. (s -> a) -> (s -> b -> t) -> Lens s t a b
lens LPState d r -> Point d r
forall (d :: Nat) r. LPState d r -> Point d r
_current (\(LPState Vector d r
o [HalfSpace d r]
s Point d r
_) Point d r
p -> Vector d r -> [HalfSpace d r] -> Point d r -> LPState d r
forall (d :: Nat) r.
Vector d r -> [HalfSpace d r] -> Point d r -> LPState d r
LPState Vector d r
o [HalfSpace d r]
s Point d r
p)


-- | What we do when we get a new halfspace h
step                                   :: (Fractional r, Ord r)
                                       => LPState 2 r -> HalfSpace 2 r
                                       -> Maybe (LPState 2 r)
step :: LPState 2 r -> HalfSpace 2 r -> Maybe (LPState 2 r)
step LPState 2 r
s HalfSpace 2 r
h | (LPState 2 r
sLPState 2 r
-> Getting (Point 2 r) (LPState 2 r) (Point 2 r) -> Point 2 r
forall s a. s -> Getting a s a -> a
^.Getting (Point 2 r) (LPState 2 r) (Point 2 r)
forall (d :: Nat) r. Lens' (LPState d r) (Point d r)
current) Point 2 r -> HalfSpace 2 r -> Bool
forall g h. IsIntersectableWith g h => g -> h -> Bool
`intersects` HalfSpace 2 r
h = LPState 2 r -> Maybe (LPState 2 r)
forall a. a -> Maybe a
Just (LPState 2 r -> Maybe (LPState 2 r))
-> LPState 2 r -> Maybe (LPState 2 r)
forall a b. (a -> b) -> a -> b
$ LPState 2 r
sLPState 2 r -> (LPState 2 r -> LPState 2 r) -> LPState 2 r
forall a b. a -> (a -> b) -> b
&([HalfSpace 2 r] -> Identity [HalfSpace 2 r])
-> LPState 2 r -> Identity (LPState 2 r)
forall (d :: Nat) r. Lens' (LPState d r) [HalfSpace d r]
seen     (([HalfSpace 2 r] -> Identity [HalfSpace 2 r])
 -> LPState 2 r -> Identity (LPState 2 r))
-> ([HalfSpace 2 r] -> [HalfSpace 2 r])
-> LPState 2 r
-> LPState 2 r
forall s t a b. ASetter s t a b -> (a -> b) -> s -> t
%~ (HalfSpace 2 r
hHalfSpace 2 r -> [HalfSpace 2 r] -> [HalfSpace 2 r]
forall a. a -> [a] -> [a]
:)
         | Bool
otherwise                   = (\Point 2 r
p -> LPState 2 r
sLPState 2 r -> (LPState 2 r -> LPState 2 r) -> LPState 2 r
forall a b. a -> (a -> b) -> b
&([HalfSpace 2 r] -> Identity [HalfSpace 2 r])
-> LPState 2 r -> Identity (LPState 2 r)
forall (d :: Nat) r. Lens' (LPState d r) [HalfSpace d r]
seen     (([HalfSpace 2 r] -> Identity [HalfSpace 2 r])
 -> LPState 2 r -> Identity (LPState 2 r))
-> ([HalfSpace 2 r] -> [HalfSpace 2 r])
-> LPState 2 r
-> LPState 2 r
forall s t a b. ASetter s t a b -> (a -> b) -> s -> t
%~ (HalfSpace 2 r
hHalfSpace 2 r -> [HalfSpace 2 r] -> [HalfSpace 2 r]
forall a. a -> [a] -> [a]
:)
                                                 LPState 2 r -> (LPState 2 r -> LPState 2 r) -> LPState 2 r
forall a b. a -> (a -> b) -> b
&(Point 2 r -> Identity (Point 2 r))
-> LPState 2 r -> Identity (LPState 2 r)
forall (d :: Nat) r. Lens' (LPState d r) (Point d r)
current ((Point 2 r -> Identity (Point 2 r))
 -> LPState 2 r -> Identity (LPState 2 r))
-> Point 2 r -> LPState 2 r -> LPState 2 r
forall s t a b. ASetter s t a b -> b -> s -> t
.~ Point 2 r
p)
                                        (Point 2 r -> LPState 2 r)
-> Maybe (Point 2 r) -> Maybe (LPState 2 r)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> LPState 2 r -> Line 2 r -> Maybe (Point 2 r)
forall r.
(Ord r, Fractional r) =>
LPState 2 r -> Line 2 r -> Maybe (Point 2 r)
maximumOn LPState 2 r
s (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 (d2 :: Nat) r2.
Iso
  (HalfSpace d r)
  (HalfSpace d2 r2)
  (HyperPlane d r)
  (HyperPlane d2 r2)
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)

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

-- | collect all intersecting halflines on the boundary l of h. If we return a Nothing there
-- is no solution. Just [] indicates that somehow this halfspace h is contained in all other
-- halfspaces.
collectOn     :: (Ord r, Fractional r)
              => Line 2 r
              -> [HalfSpace 2 r]
              -> Maybe [HalfLine 2 r]
collectOn :: Line 2 r -> [HalfSpace 2 r] -> Maybe [HalfLine 2 r]
collectOn Line 2 r
l = [Maybe (HalfLine 2 r)] -> Maybe [HalfLine 2 r]
forall (t :: * -> *) (m :: * -> *) a.
(Traversable t, Monad m) =>
t (m a) -> m (t a)
sequence ([Maybe (HalfLine 2 r)] -> Maybe [HalfLine 2 r])
-> ([HalfSpace 2 r] -> [Maybe (HalfLine 2 r)])
-> [HalfSpace 2 r]
-> Maybe [HalfLine 2 r]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (HalfSpace 2 r -> Maybe (Maybe (HalfLine 2 r)))
-> [HalfSpace 2 r] -> [Maybe (HalfLine 2 r)]
forall a b. (a -> Maybe b) -> [a] -> [b]
mapMaybe (CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r]
-> Maybe (Maybe (HalfLine 2 r))
forall r.
Intersection (Line 2 r) (HalfSpace 2 r)
-> Maybe (Maybe (HalfLine 2 r))
collect (CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r]
 -> Maybe (Maybe (HalfLine 2 r)))
-> (HalfSpace 2 r
    -> CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r])
-> HalfSpace 2 r
-> Maybe (Maybe (HalfLine 2 r))
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (Line 2 r
l Line 2 r
-> HalfSpace 2 r -> Intersection (Line 2 r) (HalfSpace 2 r)
forall g h. IsIntersectableWith g h => g -> h -> Intersection g h
`intersect`))
  where
    collect   :: Intersection (Line 2 r) (HalfSpace 2 r) -> Maybe (Maybe (HalfLine 2 r))
    collect :: Intersection (Line 2 r) (HalfSpace 2 r)
-> Maybe (Maybe (HalfLine 2 r))
collect Intersection (Line 2 r) (HalfSpace 2 r)
r = CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r]
-> Handlers
     '[NoIntersection, HalfLine 2 r, Line 2 r]
     (Maybe (Maybe (HalfLine 2 r)))
-> Maybe (Maybe (HalfLine 2 r))
forall (ts :: [*]) b. CoRec Identity ts -> Handlers ts b -> b
match CoRec Identity '[NoIntersection, HalfLine 2 r, Line 2 r]
Intersection (Line 2 r) (HalfSpace 2 r)
r (Handlers
   '[NoIntersection, HalfLine 2 r, Line 2 r]
   (Maybe (Maybe (HalfLine 2 r)))
 -> Maybe (Maybe (HalfLine 2 r)))
-> Handlers
     '[NoIntersection, HalfLine 2 r, Line 2 r]
     (Maybe (Maybe (HalfLine 2 r)))
-> Maybe (Maybe (HalfLine 2 r))
forall a b. (a -> b) -> a -> b
$
         (NoIntersection -> Maybe (Maybe (HalfLine 2 r)))
-> Handler (Maybe (Maybe (HalfLine 2 r))) NoIntersection
forall b a. (a -> b) -> Handler b a
H (Maybe (Maybe (HalfLine 2 r))
-> NoIntersection -> Maybe (Maybe (HalfLine 2 r))
forall a b. a -> b -> a
const (Maybe (Maybe (HalfLine 2 r))
 -> NoIntersection -> Maybe (Maybe (HalfLine 2 r)))
-> Maybe (Maybe (HalfLine 2 r))
-> NoIntersection
-> Maybe (Maybe (HalfLine 2 r))
forall a b. (a -> b) -> a -> b
$ Maybe (HalfLine 2 r) -> Maybe (Maybe (HalfLine 2 r))
forall a. a -> Maybe a
Just Maybe (HalfLine 2 r)
forall a. Maybe a
Nothing) -- NoIntersection
      Handler (Maybe (Maybe (HalfLine 2 r))) NoIntersection
-> Rec
     (Handler (Maybe (Maybe (HalfLine 2 r)))) '[HalfLine 2 r, Line 2 r]
-> Handlers
     '[NoIntersection, HalfLine 2 r, Line 2 r]
     (Maybe (Maybe (HalfLine 2 r)))
forall u (a :: u -> *) (r :: u) (rs :: [u]).
a r -> Rec a rs -> Rec a (r : rs)
:& (HalfLine 2 r -> Maybe (Maybe (HalfLine 2 r)))
-> Handler (Maybe (Maybe (HalfLine 2 r))) (HalfLine 2 r)
forall b a. (a -> b) -> Handler b a
H (Maybe (HalfLine 2 r) -> Maybe (Maybe (HalfLine 2 r))
forall a. a -> Maybe a
Just (Maybe (HalfLine 2 r) -> Maybe (Maybe (HalfLine 2 r)))
-> (HalfLine 2 r -> Maybe (HalfLine 2 r))
-> HalfLine 2 r
-> Maybe (Maybe (HalfLine 2 r))
forall b c a. (b -> c) -> (a -> b) -> a -> c
. HalfLine 2 r -> Maybe (HalfLine 2 r)
forall a. a -> Maybe a
Just)          -- HalfLine
      Handler (Maybe (Maybe (HalfLine 2 r))) (HalfLine 2 r)
-> Rec (Handler (Maybe (Maybe (HalfLine 2 r)))) '[Line 2 r]
-> Rec
     (Handler (Maybe (Maybe (HalfLine 2 r)))) '[HalfLine 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 -> Maybe (Maybe (HalfLine 2 r)))
-> Handler (Maybe (Maybe (HalfLine 2 r))) (Line 2 r)
forall b a. (a -> b) -> Handler b a
H (Maybe (Maybe (HalfLine 2 r))
-> Line 2 r -> Maybe (Maybe (HalfLine 2 r))
forall a b. a -> b -> a
const Maybe (Maybe (HalfLine 2 r))
forall a. Maybe a
Nothing)        -- Line
      Handler (Maybe (Maybe (HalfLine 2 r))) (Line 2 r)
-> Rec (Handler (Maybe (Maybe (HalfLine 2 r)))) '[]
-> Rec (Handler (Maybe (Maybe (HalfLine 2 r)))) '[Line 2 r]
forall u (a :: u -> *) (r :: u) (rs :: [u]).
a r -> Rec a rs -> Rec a (r : rs)
:& Rec (Handler (Maybe (Maybe (HalfLine 2 r)))) '[]
forall u (a :: u -> *). Rec a '[]
RNil


-- | Given a vector v and two points a and b, determine which is smaller in direction v.
cmpHalfPlane       :: (Ord r, Num r, Arity d)
                   => Vector d r -> Point d r -> Point d r -> Ordering
cmpHalfPlane :: Vector d r -> Point d r -> Point d r -> Ordering
cmpHalfPlane Vector d r
v Point d r
a Point d r
b = case Point d r
a 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` HyperPlane d r -> HalfSpace d r
forall (d :: Nat) r. HyperPlane d r -> HalfSpace d r
HalfSpace (Point d r -> Vector d r -> HyperPlane d r
forall (d :: Nat) r. Point d r -> Vector d r -> HyperPlane d r
HyperPlane Point d r
b Vector d r
v) of
                       PointLocationResult
Inside     -> Ordering
GT
                       PointLocationResult
OnBoundary -> Ordering
EQ
                       PointLocationResult
Outside    -> Ordering
LT

type OneOrTwo a = Either a (Two a)

flatten :: OneOrTwo a -> [a]
flatten :: OneOrTwo a -> [a]
flatten = (a -> [a]) -> (Two a -> [a]) -> OneOrTwo a -> [a]
forall a c b. (a -> c) -> (b -> c) -> Either a b -> c
either (a -> [a] -> [a]
forall a. a -> [a] -> [a]
:[]) (\(Two a
a a
b) -> [a
a,a
b])

-- | Computes the common intersection of a nonempty list of halfines that are
-- all colinear with the given line l.
--
-- We return either the two halflines that prove that there is no counter
-- example or we return one or two points that form form the boundary points of
-- the range in which all halflines intersect.
commonIntersection                :: (Ord r, Num r, Arity d)
                                  => Line d r
                                  -> NonEmpty.NonEmpty (HalfLine d r :+ a)
                                  -> Either (Two (HalfLine d r :+ a))
                                            (OneOrTwo (Point d r :+ a))
commonIntersection :: Line d r
-> NonEmpty (HalfLine d r :+ a)
-> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a))
commonIntersection (Line Point d r
_ Vector d r
v) NonEmpty (HalfLine d r :+ a)
hls = case (Maybe (HalfLine d r :+ a)
nh,Maybe (HalfLine d r :+ a)
ph) of
     (Maybe (HalfLine d r :+ a)
Nothing,Maybe (HalfLine d r :+ a)
Nothing) -> String
-> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a))
forall a. HasCallStack => String -> a
error String
"absurd; this case cannot occur"
     (Maybe (HalfLine d r :+ a)
Nothing, Just HalfLine d r :+ a
p) -> OneOrTwo (Point d r :+ a)
-> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a))
forall a b. b -> Either a b
Right (OneOrTwo (Point d r :+ a)
 -> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a)))
-> ((HalfLine d r :+ a) -> OneOrTwo (Point d r :+ a))
-> (HalfLine d r :+ a)
-> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a))
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (Point d r :+ a) -> OneOrTwo (Point d r :+ a)
forall a b. a -> Either a b
Left ((Point d r :+ a) -> OneOrTwo (Point d r :+ a))
-> ((HalfLine d r :+ a) -> Point d r :+ a)
-> (HalfLine d r :+ a)
-> OneOrTwo (Point d r :+ a)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (HalfLine d r :+ a) -> Point d r :+ a
forall (d :: Nat) r extra.
(HalfLine d r :+ extra) -> Point d r :+ extra
extract ((HalfLine d r :+ a)
 -> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a)))
-> (HalfLine d r :+ a)
-> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a))
forall a b. (a -> b) -> a -> b
$ HalfLine d r :+ a
p
     (Just HalfLine d r :+ a
n, Maybe (HalfLine d r :+ a)
Nothing) -> OneOrTwo (Point d r :+ a)
-> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a))
forall a b. b -> Either a b
Right (OneOrTwo (Point d r :+ a)
 -> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a)))
-> ((HalfLine d r :+ a) -> OneOrTwo (Point d r :+ a))
-> (HalfLine d r :+ a)
-> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a))
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (Point d r :+ a) -> OneOrTwo (Point d r :+ a)
forall a b. a -> Either a b
Left ((Point d r :+ a) -> OneOrTwo (Point d r :+ a))
-> ((HalfLine d r :+ a) -> Point d r :+ a)
-> (HalfLine d r :+ a)
-> OneOrTwo (Point d r :+ a)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (HalfLine d r :+ a) -> Point d r :+ a
forall (d :: Nat) r extra.
(HalfLine d r :+ extra) -> Point d r :+ extra
extract ((HalfLine d r :+ a)
 -> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a)))
-> (HalfLine d r :+ a)
-> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a))
forall a b. (a -> b) -> a -> b
$ HalfLine d r :+ a
n
     (Just HalfLine d r :+ a
n, Just HalfLine d r :+ a
p)  -> case Vector d r
-> (HalfLine d r :+ a) -> (HalfLine d r :+ a) -> Ordering
forall (d :: Nat) r extra extra.
(ImplicitPeano (Peano d), Num r, Ord r,
 ArityPeano (Peano (FromPeano (Peano d))),
 KnownNat (FromPeano (Peano d)), KnownNat d,
 Peano (FromPeano (Peano d) + 1)
 ~ 'S (Peano (FromPeano (Peano d)))) =>
Vector d r
-> (HalfLine d r :+ extra) -> (HalfLine d r :+ extra) -> Ordering
cmpHalfPlane' Vector d r
v HalfLine d r :+ a
n HalfLine d r :+ a
p of
                            Ordering
LT -> Two (HalfLine d r :+ a)
-> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a))
forall a b. a -> Either a b
Left (Two (HalfLine d r :+ a)
 -> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a)))
-> Two (HalfLine d r :+ a)
-> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a))
forall a b. (a -> b) -> a -> b
$ (HalfLine d r :+ a)
-> (HalfLine d r :+ a) -> Two (HalfLine d r :+ a)
forall a. a -> a -> Two a
Two HalfLine d r :+ a
n HalfLine d r :+ a
p
                            Ordering
EQ -> OneOrTwo (Point d r :+ a)
-> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a))
forall a b. b -> Either a b
Right (OneOrTwo (Point d r :+ a)
 -> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a)))
-> ((HalfLine d r :+ a) -> OneOrTwo (Point d r :+ a))
-> (HalfLine d r :+ a)
-> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a))
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (Point d r :+ a) -> OneOrTwo (Point d r :+ a)
forall a b. a -> Either a b
Left ((Point d r :+ a) -> OneOrTwo (Point d r :+ a))
-> ((HalfLine d r :+ a) -> Point d r :+ a)
-> (HalfLine d r :+ a)
-> OneOrTwo (Point d r :+ a)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (HalfLine d r :+ a) -> Point d r :+ a
forall (d :: Nat) r extra.
(HalfLine d r :+ extra) -> Point d r :+ extra
extract ((HalfLine d r :+ a)
 -> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a)))
-> (HalfLine d r :+ a)
-> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a))
forall a b. (a -> b) -> a -> b
$ HalfLine d r :+ a
p
                            Ordering
GT -> OneOrTwo (Point d r :+ a)
-> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a))
forall a b. b -> Either a b
Right (OneOrTwo (Point d r :+ a)
 -> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a)))
-> (Two (Point d r :+ a) -> OneOrTwo (Point d r :+ a))
-> Two (Point d r :+ a)
-> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a))
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Two (Point d r :+ a) -> OneOrTwo (Point d r :+ a)
forall a b. b -> Either a b
Right (Two (Point d r :+ a)
 -> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a)))
-> Two (Point d r :+ a)
-> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a))
forall a b. (a -> b) -> a -> b
$ (Point d r :+ a) -> (Point d r :+ a) -> Two (Point d r :+ a)
forall a. a -> a -> Two a
Two ((HalfLine d r :+ a) -> Point d r :+ a
forall (d :: Nat) r extra.
(HalfLine d r :+ extra) -> Point d r :+ extra
extract HalfLine d r :+ a
p) ((HalfLine d r :+ a) -> Point d r :+ a
forall (d :: Nat) r extra.
(HalfLine d r :+ extra) -> Point d r :+ extra
extract HalfLine d r :+ a
n)
  where
    extract :: (HalfLine d r :+ extra) -> Point d r :+ extra
extract = ASetter
  (HalfLine d r :+ extra)
  (Point d r :+ extra)
  (HalfLine d r)
  (Point d r)
-> (HalfLine d r -> Point d r)
-> (HalfLine d r :+ extra)
-> Point d r :+ extra
forall s t a b. ASetter s t a b -> (a -> b) -> s -> t
over ASetter
  (HalfLine d r :+ extra)
  (Point d r :+ extra)
  (HalfLine d r)
  (Point d r)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core (HalfLine d r
-> Getting (Point d r) (HalfLine d r) (Point d r) -> Point d r
forall s a. s -> Getting a s a -> a
^.Getting (Point d r) (HalfLine d r) (Point d r)
forall (d :: Nat) r. Lens' (HalfLine d r) (Point d r)
startPoint)
    ([HalfLine d r :+ a]
pos,[HalfLine d r :+ a]
neg) = ((HalfLine d r :+ a) -> Bool)
-> NonEmpty (HalfLine d r :+ a)
-> ([HalfLine d r :+ a], [HalfLine d r :+ a])
forall a. (a -> Bool) -> NonEmpty a -> ([a], [a])
NonEmpty.partition (\HalfLine d r :+ a
hl -> HalfLine d r :+ a
hl(HalfLine d r :+ a)
-> Getting (Vector d r) (HalfLine d r :+ a) (Vector d r)
-> Vector d r
forall s a. s -> Getting a s a -> a
^.(HalfLine d r -> Const (Vector d r) (HalfLine d r))
-> (HalfLine d r :+ a) -> Const (Vector d r) (HalfLine d r :+ a)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core((HalfLine d r -> Const (Vector d r) (HalfLine d r))
 -> (HalfLine d r :+ a) -> Const (Vector d r) (HalfLine d r :+ a))
-> ((Vector d r -> Const (Vector d r) (Vector d r))
    -> HalfLine d r -> Const (Vector d r) (HalfLine d r))
-> Getting (Vector d r) (HalfLine d r :+ a) (Vector d r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(Vector d r -> Const (Vector d r) (Vector d r))
-> HalfLine d r -> Const (Vector d r) (HalfLine d r)
forall (d :: Nat) r. Lens' (HalfLine d r) (Vector d r)
halfLineDirection Vector d r -> Vector d r -> Bool
forall a. Eq a => a -> a -> Bool
== Vector d r
v) NonEmpty (HalfLine d r :+ a)
hls
    ph :: Maybe (HalfLine d r :+ a)
ph = ((HalfLine d r :+ a) -> (HalfLine d r :+ a) -> Ordering)
-> [HalfLine d r :+ a] -> Maybe (HalfLine d r :+ a)
forall a. (a -> a -> Ordering) -> [a] -> Maybe a
maximumBy' (Vector d r
-> (HalfLine d r :+ a) -> (HalfLine d r :+ a) -> Ordering
forall (d :: Nat) r extra extra.
(ImplicitPeano (Peano d), Num r, Ord r,
 ArityPeano (Peano (FromPeano (Peano d))),
 KnownNat (FromPeano (Peano d)), KnownNat d,
 Peano (FromPeano (Peano d) + 1)
 ~ 'S (Peano (FromPeano (Peano d)))) =>
Vector d r
-> (HalfLine d r :+ extra) -> (HalfLine d r :+ extra) -> Ordering
cmpHalfPlane' Vector d r
v) [HalfLine d r :+ a]
pos
    nh :: Maybe (HalfLine d r :+ a)
nh = ((HalfLine d r :+ a) -> (HalfLine d r :+ a) -> Ordering)
-> [HalfLine d r :+ a] -> Maybe (HalfLine d r :+ a)
forall a. (a -> a -> Ordering) -> [a] -> Maybe a
maximumBy' (((HalfLine d r :+ a) -> (HalfLine d r :+ a) -> Ordering)
-> (HalfLine d r :+ a) -> (HalfLine d r :+ a) -> Ordering
forall a b c. (a -> b -> c) -> b -> a -> c
flip (((HalfLine d r :+ a) -> (HalfLine d r :+ a) -> Ordering)
 -> (HalfLine d r :+ a) -> (HalfLine d r :+ a) -> Ordering)
-> ((HalfLine d r :+ a) -> (HalfLine d r :+ a) -> Ordering)
-> (HalfLine d r :+ a)
-> (HalfLine d r :+ a)
-> Ordering
forall a b. (a -> b) -> a -> b
$ Vector d r
-> (HalfLine d r :+ a) -> (HalfLine d r :+ a) -> Ordering
forall (d :: Nat) r extra extra.
(ImplicitPeano (Peano d), Num r, Ord r,
 ArityPeano (Peano (FromPeano (Peano d))),
 KnownNat (FromPeano (Peano d)), KnownNat d,
 Peano (FromPeano (Peano d) + 1)
 ~ 'S (Peano (FromPeano (Peano d)))) =>
Vector d r
-> (HalfLine d r :+ extra) -> (HalfLine d r :+ extra) -> Ordering
cmpHalfPlane' Vector d r
v) [HalfLine d r :+ a]
neg

    cmpHalfPlane' :: Vector d r
-> (HalfLine d r :+ extra) -> (HalfLine d r :+ extra) -> Ordering
cmpHalfPlane' Vector d r
vv HalfLine d r :+ extra
a HalfLine d r :+ extra
b = Vector d r -> Point d r -> Point d r -> Ordering
forall r (d :: Nat).
(Ord r, Num r, Arity d) =>
Vector d r -> Point d r -> Point d r -> Ordering
cmpHalfPlane Vector d r
vv (HalfLine d r :+ extra
a(HalfLine d r :+ extra)
-> Getting (Point d r) (HalfLine d r :+ extra) (Point d r)
-> Point d r
forall s a. s -> Getting a s a -> a
^.(HalfLine d r -> Const (Point d r) (HalfLine d r))
-> (HalfLine d r :+ extra)
-> Const (Point d r) (HalfLine d r :+ extra)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core((HalfLine d r -> Const (Point d r) (HalfLine d r))
 -> (HalfLine d r :+ extra)
 -> Const (Point d r) (HalfLine d r :+ extra))
-> ((Point d r -> Const (Point d r) (Point d r))
    -> HalfLine d r -> Const (Point d r) (HalfLine d r))
-> Getting (Point d r) (HalfLine d r :+ extra) (Point d r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(Point d r -> Const (Point d r) (Point d r))
-> HalfLine d r -> Const (Point d r) (HalfLine d r)
forall (d :: Nat) r. Lens' (HalfLine d r) (Point d r)
startPoint) (HalfLine d r :+ extra
b(HalfLine d r :+ extra)
-> Getting (Point d r) (HalfLine d r :+ extra) (Point d r)
-> Point d r
forall s a. s -> Getting a s a -> a
^.(HalfLine d r -> Const (Point d r) (HalfLine d r))
-> (HalfLine d r :+ extra)
-> Const (Point d r) (HalfLine d r :+ extra)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core((HalfLine d r -> Const (Point d r) (HalfLine d r))
 -> (HalfLine d r :+ extra)
 -> Const (Point d r) (HalfLine d r :+ extra))
-> ((Point d r -> Const (Point d r) (Point d r))
    -> HalfLine d r -> Const (Point d r) (HalfLine d r))
-> Getting (Point d r) (HalfLine d r :+ extra) (Point d r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(Point d r -> Const (Point d r) (Point d r))
-> HalfLine d r -> Const (Point d r) (HalfLine d r)
forall (d :: Nat) r. Lens' (HalfLine d r) (Point d r)
startPoint)

commonIntersection'               :: (Ord r, Num r, Arity d)
                                  => Line d r
                                  -> NonEmpty.NonEmpty (HalfLine d r)
                                  -> [Point d r]
commonIntersection' :: Line d r -> NonEmpty (HalfLine d r) -> [Point d r]
commonIntersection' Line d r
l NonEmpty (HalfLine d r)
hls = (Two (HalfLine d r :+ ()) -> [Point d r])
-> (OneOrTwo (Point d r :+ ()) -> [Point d r])
-> Either (Two (HalfLine d r :+ ())) (OneOrTwo (Point d r :+ ()))
-> [Point d r]
forall a c b. (a -> c) -> (b -> c) -> Either a b -> c
either ([Point d r] -> Two (HalfLine d r :+ ()) -> [Point d r]
forall a b. a -> b -> a
const []) (((Point d r :+ ()) -> Point d r)
-> [Point d r :+ ()] -> [Point d r]
forall a b. (a -> b) -> [a] -> [b]
map ((Point d r :+ ())
-> Getting (Point d r) (Point d r :+ ()) (Point d r) -> Point d r
forall s a. s -> Getting a s a -> a
^.Getting (Point d r) (Point d r :+ ()) (Point d r)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core) ([Point d r :+ ()] -> [Point d r])
-> (OneOrTwo (Point d r :+ ()) -> [Point d r :+ ()])
-> OneOrTwo (Point d r :+ ())
-> [Point d r]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. OneOrTwo (Point d r :+ ()) -> [Point d r :+ ()]
forall a. OneOrTwo a -> [a]
flatten)
                          (Either (Two (HalfLine d r :+ ())) (OneOrTwo (Point d r :+ ()))
 -> [Point d r])
-> Either (Two (HalfLine d r :+ ())) (OneOrTwo (Point d r :+ ()))
-> [Point d r]
forall a b. (a -> b) -> a -> b
$ Line d r
-> NonEmpty (HalfLine d r :+ ())
-> Either (Two (HalfLine d r :+ ())) (OneOrTwo (Point d r :+ ()))
forall r (d :: Nat) a.
(Ord r, Num r, Arity d) =>
Line d r
-> NonEmpty (HalfLine d r :+ a)
-> Either (Two (HalfLine d r :+ a)) (OneOrTwo (Point d r :+ a))
commonIntersection Line d r
l (HalfLine d r -> HalfLine d r :+ ()
forall a. a -> a :+ ()
ext (HalfLine d r -> HalfLine d r :+ ())
-> NonEmpty (HalfLine d r) -> NonEmpty (HalfLine d r :+ ())
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> NonEmpty (HalfLine d r)
hls)


-- | maximum of a list using a given comparison ; if the list is empty returns Nothing
maximumBy'     :: (a -> a -> Ordering) -> [a] -> Maybe a
maximumBy' :: (a -> a -> Ordering) -> [a] -> Maybe a
maximumBy' a -> a -> Ordering
cmp = \case
  [] -> Maybe a
forall a. Maybe a
Nothing
  [a]
xs -> a -> Maybe a
forall a. a -> Maybe a
Just (a -> Maybe a) -> a -> Maybe a
forall a b. (a -> b) -> a -> b
$ (a -> a -> Ordering) -> [a] -> a
forall (t :: * -> *) a.
Foldable t =>
(a -> a -> Ordering) -> t a -> a
F.maximumBy a -> a -> Ordering
cmp [a]
xs


-- | One dimensional linear programming on lines embedded in \(\mathbb{R}^d\).
--
-- Given an objective vector c, a line l, and a collection of half-lines hls that are all
-- sublines of l (i.e. halfspaces *on* l), compute if there is a point inside
-- all these halflines. If so, we actually return the one that maximizes c.
--
-- running time: \(O(n)\)
oneDLinearProgramming         :: (Ord r, Num r, Arity d)
                              => Vector d r -> Line d r -> [HalfLine d r] -> Maybe (Point d r)
oneDLinearProgramming :: Vector d r -> Line d r -> [HalfLine d r] -> Maybe (Point d r)
oneDLinearProgramming Vector d r
c Line d r
l [HalfLine d r]
hls = do
                                  NonEmpty (HalfLine d r)
hls'       <- [HalfLine d r] -> Maybe (NonEmpty (HalfLine d r))
forall a. [a] -> Maybe (NonEmpty a)
NonEmpty.nonEmpty [HalfLine d r]
hls
                                  let candidates :: [Point d r]
candidates = Line d r -> NonEmpty (HalfLine d r) -> [Point d r]
forall r (d :: Nat).
(Ord r, Num r, Arity d) =>
Line d r -> NonEmpty (HalfLine d r) -> [Point d r]
commonIntersection' Line d r
l NonEmpty (HalfLine d r)
hls'
                                  (Point d r -> Point d r -> Ordering)
-> [Point d r] -> Maybe (Point d r)
forall a. (a -> a -> Ordering) -> [a] -> Maybe a
maximumBy' (Vector d r -> Point d r -> Point d r -> Ordering
forall r (d :: Nat).
(Ord r, Num r, Arity d) =>
Vector d r -> Point d r -> Point d r -> Ordering
cmpHalfPlane Vector d r
c) [Point d r]
candidates


-- | Let l be the boundary of h, and assume that we know that the new point in
-- the common intersection must lie on h, try to find this point. In
-- partiuclar, we find the 'maximum' point in the s^.direction vector. The
-- funtion returns Nothing if no such point exists, i.e. if there is no point
-- on l that is contained in all halfspaces.
--
-- Note that this is essentially one dinsional LP
maximumOn     :: (Ord r, Fractional r) => LPState 2 r -> Line 2 r -> Maybe (Point 2 r)
maximumOn :: LPState 2 r -> Line 2 r -> Maybe (Point 2 r)
maximumOn LPState 2 r
s Line 2 r
l = do [HalfLine 2 r]
hls  <- Line 2 r -> [HalfSpace 2 r] -> Maybe [HalfLine 2 r]
forall r.
(Ord r, Fractional r) =>
Line 2 r -> [HalfSpace 2 r] -> Maybe [HalfLine 2 r]
collectOn Line 2 r
l ([HalfSpace 2 r] -> Maybe [HalfLine 2 r])
-> [HalfSpace 2 r] -> Maybe [HalfLine 2 r]
forall a b. (a -> b) -> a -> b
$ LPState 2 r
sLPState 2 r
-> Getting [HalfSpace 2 r] (LPState 2 r) [HalfSpace 2 r]
-> [HalfSpace 2 r]
forall s a. s -> Getting a s a -> a
^.Getting [HalfSpace 2 r] (LPState 2 r) [HalfSpace 2 r]
forall (d :: Nat) r. Lens' (LPState d r) [HalfSpace d r]
seen
                   Vector 2 r -> Line 2 r -> [HalfLine 2 r] -> Maybe (Point 2 r)
forall r (d :: Nat).
(Ord r, Num r, Arity d) =>
Vector d r -> Line d r -> [HalfLine d r] -> Maybe (Point d r)
oneDLinearProgramming (LPState 2 r
sLPState 2 r
-> Getting (Vector 2 r) (LPState 2 r) (Vector 2 r) -> Vector 2 r
forall s a. s -> Getting a s a -> a
^.Getting (Vector 2 r) (LPState 2 r) (Vector 2 r)
forall (d :: Nat) r. Lens' (LPState d r) (Vector d r)
obj) Line 2 r
l [HalfLine 2 r]
hls
  -- if collectOn returns a Just [] it means there is a point in the common intersection,
  -- however it does not lie on the boundary of h. This violates our input assumption
  -- thus if this would happen we can safely return a Nothing


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

-- | Assumes that the first two constraints somehow bound the solution in direction c.
initialize :: forall r. (Ord r, Fractional r)
           => LinearProgram 2 r -> (LPState 2 r, [HalfSpace 2 r])
initialize :: LinearProgram 2 r -> (LPState 2 r, [HalfSpace 2 r])
initialize (LinearProgram Vector 2 r
c (HalfSpace 2 r
m1:HalfSpace 2 r
m2:[HalfSpace 2 r]
hs)) = (Vector 2 r -> [HalfSpace 2 r] -> Point 2 r -> LPState 2 r
forall (d :: Nat) r.
Vector d r -> [HalfSpace d r] -> Point d r -> LPState d r
LPState Vector 2 r
c [HalfSpace 2 r
m1,HalfSpace 2 r
m2] Point 2 r
p, [HalfSpace 2 r]
hs)
  where
    Just Point 2 r
p = forall (ts :: [*]).
NatToInt (RIndex (Point 2 r) ts) =>
CoRec Identity ts -> Maybe (Point 2 r)
forall t (ts :: [*]).
NatToInt (RIndex t ts) =>
CoRec Identity ts -> Maybe t
asA @(Point 2 r)
           (CoRec Identity '[NoIntersection, Point 2 r, Line 2 r]
 -> Maybe (Point 2 r))
-> CoRec Identity '[NoIntersection, Point 2 r, Line 2 r]
-> Maybe (Point 2 r)
forall a b. (a -> b) -> a -> b
$ (HalfSpace 2 r
m1HalfSpace 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 (d2 :: Nat) r2.
Iso
  (HalfSpace d r)
  (HalfSpace d2 r2)
  (HyperPlane d r)
  (HyperPlane d2 r2)
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) 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` (HalfSpace 2 r
m2HalfSpace 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 (d2 :: Nat) r2.
Iso
  (HalfSpace d r)
  (HalfSpace d2 r2)
  (HyperPlane d r)
  (HyperPlane d2 r2)
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)
initialize LinearProgram 2 r
_ = String -> (LPState 2 r, [HalfSpace 2 r])
forall a. HasCallStack => String -> a
error
  String
"Algorithms.Geometry.LinearProgramming.LP2DRIC.initialize requires \
  \at least two constraints."


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

-- | Let \(n(h)\) denote the normal of the line bounding a halfspace \(h\).
--
-- This function tries to Find an "unbounded direction" \(d\). If such a
-- direction \(d\) exits the LP is unbounded, and we can produce evidence of
-- this in the form of a half-line in direction \(d\).
--
-- More formally, we are looking for a direction \(d\) so that
-- - \(c \cdot d > 0\), and
-- - \(d \cdot n(h) \geq 0\), wherefor every half space \(h\).
--
_findD                      :: (Ord r, Fractional r)
                           => LinearProgram 2 r -> Maybe (Vector 2 r)
_findD :: LinearProgram 2 r -> Maybe (Vector 2 r)
_findD (LinearProgram Vector 2 r
c [HalfSpace 2 r]
hs) = do [HalfLine 2 r]
hls <- Line 2 r -> [HalfSpace 2 r] -> Maybe [HalfLine 2 r]
forall r.
(Ord r, Fractional r) =>
Line 2 r -> [HalfSpace 2 r] -> Maybe [HalfLine 2 r]
collectOn Line 2 r
nl [HalfSpace 2 r]
hs'
                                 Vector 2 r
d   <- Point 2 r -> Vector 2 r
forall (d :: Nat) r. Point d r -> Vector d r
toVec (Point 2 r -> Vector 2 r)
-> Maybe (Point 2 r) -> Maybe (Vector 2 r)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Vector 2 r -> Line 2 r -> [HalfLine 2 r] -> Maybe (Point 2 r)
forall r (d :: Nat).
(Ord r, Num r, Arity d) =>
Vector d r -> Line d r -> [HalfLine d r] -> Maybe (Point d r)
oneDLinearProgramming Vector 2 r
v Line 2 r
nl [HalfLine 2 r]
hls
                                        -- the direction v here does not really matter
                                 if Vector 2 r
c Vector 2 r -> Vector 2 r -> r
forall (f :: * -> *) a. (Metric f, Num a) => f a -> f a -> a
`dot` Vector 2 r
d r -> r -> Bool
forall a. Ord a => a -> a -> Bool
> r
0 then Vector 2 r -> Maybe (Vector 2 r)
forall (f :: * -> *) a. Applicative f => a -> f a
pure Vector 2 r
d
                                                  else Maybe (Vector 2 r)
forall a. Maybe a
Nothing
  where
    -- we interpret the points on nl as directions w.r.t the origin
    nl :: Line 2 r
nl@(Line Point 2 r
_ Vector 2 r
v) = Line 2 r -> Line 2 r
forall r. Num r => Line 2 r -> Line 2 r
perpendicularTo (Point 2 r -> Vector 2 r -> Line 2 r
forall (d :: Nat) r. Point d r -> Vector d r -> Line d r
Line (Point 2 r
forall (d :: Nat) r. (Arity d, Num r) => Point d r
origin 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
c) Vector 2 r
c)
    hs' :: [HalfSpace 2 r]
hs' = (HalfSpace 2 r -> HalfSpace 2 r)
-> [HalfSpace 2 r] -> [HalfSpace 2 r]
forall a b. (a -> b) -> [a] -> [b]
map HalfSpace 2 r -> HalfSpace 2 r
forall (d1 :: Nat) r1 a. HalfSpace d1 r1 -> a
toHL [HalfSpace 2 r]
hs

    -- every halfspace creates an allowed set of directions, modelled by a
    -- half-line on nl
    toHL :: HalfSpace d1 r1 -> a
toHL HalfSpace d1 r1
h = let _n :: Vector d1 r1
_n              = HalfSpace d1 r1
hHalfSpace d1 r1
-> Getting (Vector d1 r1) (HalfSpace d1 r1) (Vector d1 r1)
-> Vector d1 r1
forall s a. s -> Getting a s a -> a
^.(HyperPlane d1 r1 -> Const (Vector d1 r1) (HyperPlane d1 r1))
-> HalfSpace d1 r1 -> Const (Vector d1 r1) (HalfSpace d1 r1)
forall (d :: Nat) r (d2 :: Nat) r2.
Iso
  (HalfSpace d r)
  (HalfSpace d2 r2)
  (HyperPlane d r)
  (HyperPlane d2 r2)
boundingPlane((HyperPlane d1 r1 -> Const (Vector d1 r1) (HyperPlane d1 r1))
 -> HalfSpace d1 r1 -> Const (Vector d1 r1) (HalfSpace d1 r1))
-> ((Vector d1 r1 -> Const (Vector d1 r1) (Vector d1 r1))
    -> HyperPlane d1 r1 -> Const (Vector d1 r1) (HyperPlane d1 r1))
-> Getting (Vector d1 r1) (HalfSpace d1 r1) (Vector d1 r1)
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(Vector d1 r1 -> Const (Vector d1 r1) (Vector d1 r1))
-> HyperPlane d1 r1 -> Const (Vector d1 r1) (HyperPlane d1 r1)
forall (d :: Nat) r. Lens' (HyperPlane d r) (Vector d r)
normalVec
             in a
forall a. HasCallStack => a
undefined


-- | Either finds an unbounded Haflline, or evidence the two halfspaces that provide
-- evidence that no solution exists
_findUnBoundedHalfLine :: LinearProgram 2 r -> Either (Two (HalfSpace 2 r)) (HalfLine 2 r)
_findUnBoundedHalfLine :: LinearProgram 2 r -> Either (Two (HalfSpace 2 r)) (HalfLine 2 r)
_findUnBoundedHalfLine = LinearProgram 2 r -> Either (Two (HalfSpace 2 r)) (HalfLine 2 r)
forall a. HasCallStack => a
undefined -- use findD then find the starting point








    -- ok; we can normalize the "y-coord" of our d-vector by taking the
    -- length of vector c. (I should probably square the c to avoid having to take square roots.)

  -- but how about the "x-coord"; we need to express that as the "lambda coord" along nl



-- ok so the global plan is: Find a vector d



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

-- TODO: Fix this


-- findWith' o h ho hs =


-- findWith o h []
--   | o `isPerpendicularTo` (h^.boundingPlane._asLine) = Single $ h^.boundingPlane.inPlane
--   | otherwise                                        = UnBounded
-- findWith o h hs = case commonSpace oppo of
--                     []     ->
--                     (ho:_) | ho `intersects1` hX -> findWith' o hX ho hs'
--                              -- observe that we can ignore the rest of the planes in
--                              -- parra and oppo from our computation, since they
--                              -- contain hX and hO, respectively anyway.
--                            | otherwise          -> NoSolution
--                              -- appararently ho and hX are disjoint, so there is no
--                              -- point in the common intersection of (all halfspaces)
--   where
--     -- parra: parrallel halfspaces where the space is on the same "side" as h
--     -- oppo: parallel halfspaces with opposite side as h
--     -- hs' : non-parallel
--     STR parra oppo hs' = foldr (extractParallel h) (STR [] [] []) hs
--     Just hX = commonSpace (h:parra)

--     intersects1 a b = (a^.boundingPlane.inPlane) `intersects` b



-- -- | Given two halfspaces a and h, test if the bounding plane of h is parallel to a, with
-- -- the spaces on the same side (first arg of the result), opposite (second), or
-- -- neither (third).
-- extractParallel                    :: (Arity d, Ord r, Fractional r)
--                                    => HalfSpace d r
--                                    -> HalfSpace d r
--                                    -> STR [HalfSpace d r] [HalfSpace d r] [HalfSpace d r]
--                                    -> STR [HalfSpace d r] [HalfSpace d r] [HalfSpace d r]
-- extractParallel a h (STR ps os hs) = case scalarMultiple n v of
--                                         Nothing              -> STR ps     os     (h:hs)
--                                         Just lam | lam < 0   -> STR ps     (h:os) hs
--                                                  | otherwise -> STR (h:ps) os     hs

--   where
--     n = a^.boundingPlane.normalVec
--     v = h^.boundingPlane.normalVec



-- -- | Given a bunch of halfpsaces that have parallel bounding planes and whose
-- -- normal vectors are oriented in the same direciton, reports the common
-- -- halfspace, i.e. the "smallest" common intersection.
-- --
-- -- this function returns a Nothing only if the initial list is empty
-- commonSpace :: forall d r. (Ord r, Fractional r) => [HalfSpace d r] -> Maybe (HalfSpace d r)
-- commonSpace = \case
--     []     -> Nothing
--     (h:hs) -> foldr trim (Just h) hs
--   where
--     trim h acc = acc >>= \h' -> let p = h^.boundingPlane.inPlane in
--                                 if p `intersects` h' then h' else h
--                                 -- We use that the boudning planes of h and h' are parallel
--                                 -- so if h' contains the inPlane point of h then h'
--                                 -- contains h. If not then it is the other way around







--   match (h^.boundingPlane._asLine) `intersect` h' $
--       (H $ \Nothing -> if (h'^.boundingPlane._asLine) `intersects` h
--                       then findWith o h' -- no solution on h ; h is redundant
--                       else insert h' $ findWith o h hs)  -- add h'
--    :& (H $ \hl -> undefined)
--    :& (H $ \_ -> undefined)
--    :& RNil





-- initialize          :: (Ord r, Fractional r)
--                     => Vector 2 r -> [HalfSpace d r]
--                     -> GLPolution (LPState d r, [HalfSpace d r])
-- initialize _ []     = NoSolution
-- initialize o (h:hs) = findWith o h hs


-- -- -- | Given a vector v and a halfplane h, return the extremal point in direction
-- -- -- v that lies in h.
-- -- findBest v h =


--   -- ( LPState o [h,h2] undefined
-- --                         (h^.boundingPlane `intersect` h2^.boundingPlane)
-- --                          , hs)
-- -- initialize _ _         = error "initialize: unbounded LP"



-- -- (\h' -> (h'^.halfSpaceBoundary) `intersect`) h^.halfSpaceBoundary