module Data.Geometry.Polygon.Convex( ConvexPolygon(..), simplePolygon
, merge
, lowerTangent, upperTangent
, isLeftOf, isRightOf
, extremes
, maxInDirection
, leftTangent, rightTangent
, minkowskiSum
, bottomMost
) where
import Control.Lens hiding ((:<), (:>))
import Data.CircularSeq (focus,CSeq)
import qualified Data.CircularSeq as C
import Data.Ext
import qualified Data.Foldable as F
import Data.Function (on, )
import Data.Geometry
import Data.Geometry.Box (IsBoxable(..))
import Data.Geometry.Polygon (fromPoints, cmpExtreme)
import Data.Geometry.Properties
import Data.Maybe (fromJust)
import Data.Ord (comparing)
import Data.Sequence (viewl,viewr, ViewL(..), ViewR(..))
import qualified Data.Sequence as S
import Debug.Trace
newtype ConvexPolygon p r = ConvexPolygon {_simplePolygon :: SimplePolygon p r }
deriving (Show,Eq,PointFunctor)
makeLenses ''ConvexPolygon
type instance Dimension (ConvexPolygon p r) = 2
type instance NumType (ConvexPolygon p r) = r
instance Num r => IsTransformable (ConvexPolygon p r) where
transformBy = transformPointFunctor
instance IsBoxable (ConvexPolygon p r) where
boundingBox = boundingBox . _simplePolygon
extremes :: (Num r, Ord r) => Vector 2 r -> ConvexPolygon p r
-> (Point 2 r :+ p, Point 2 r :+ p)
extremes u p = (maxInDirection ((1) *^ u) p, maxInDirection u p)
maxInDirection :: (Num r, Ord r) => Vector 2 r -> ConvexPolygon p r -> Point 2 r :+ p
maxInDirection u = findMaxWith (cmpExtreme u)
findMaxWith :: (Point 2 r :+ p -> Point 2 r :+ p -> Ordering)
-> ConvexPolygon p r -> Point 2 r :+ p
findMaxWith cmp p = findMaxStart . C.rightElements . getVertices $ p
where
p' >=. q = (p' `cmp` q) /= LT
findMaxStart s@(viewl -> (a:<r))
| isLocalMax r a r = a
| otherwise = findMax s
findMaxStart _ = error "absurd"
findMax s = let i = F.length s `div` 2
(ac,cb') = S.splitAt i s
(c :< cb) = viewl cb'
in findMax' ac c cb
findMax' ac c cb
| isLocalMax ac c cb = c
| otherwise = binSearch ac c cb
binSearch ac@(viewl -> a:<r) c cb = case (isUpwards a r, isUpwards c cb, a >=. c) of
(True,False,_) -> findMax (ac |> c)
(True,True,True) -> findMax (ac |> c)
(True,True,False) -> findMax (c <| cb)
(False,True,_) -> findMax (c <| cb)
(False,False,False) -> findMax (ac |> c)
(False,False,True) -> findMax (c <| cb)
binSearch _ _ _ = error "maxInDirection, binSearch: empty chain"
isLocalMax (viewr -> _ :> l) c (viewl -> r :< _) = c >=. l && c >=. r
isLocalMax (viewr -> _ :> l) c _ = c >=. l
isLocalMax _ c (viewl -> r :< _) = c >=. r
isLocalMax _ _ _ = True
isUpwards a (viewl -> b :< _) = (a `cmp` b) /= GT
isUpwards _ _ = error "isUpwards: no edge endpoint"
tangentCmp :: (Num r, Ord r)
=> Point 2 r -> Point 2 r :+ p -> Point 2 r :+ q -> Ordering
tangentCmp o p q = case ccw o (p^.core) (q^.core) of
CCW -> LT
CoLinear -> EQ
CW -> GT
leftTangent :: (Ord r, Num r) => ConvexPolygon p r -> Point 2 r -> Point 2 r :+ p
leftTangent poly q = findMaxWith (tangentCmp q) poly
rightTangent :: (Ord r, Num r) => ConvexPolygon p r -> Point 2 r -> Point 2 r :+ p
rightTangent poly q = findMaxWith (flip $ tangentCmp q) poly
merge :: (Num r, Ord r) => ConvexPolygon p r -> ConvexPolygon p r
-> (ConvexPolygon p r, LineSegment 2 p r, LineSegment 2 p r)
merge lp rp = (ConvexPolygon . fromPoints $ r' ++ l', lt, ut)
where
lt@(ClosedLineSegment a b) = lowerTangent lp rp
ut@(ClosedLineSegment c d) = upperTangent lp rp
takeUntil p xs = let (xs',x:_) = break p xs in xs' ++ [x]
rightElems = F.toList . C.rightElements
takeAndRotate x y = takeUntil (coreEq x) . rightElems . rotateTo' y . getVertices
r' = takeAndRotate b d rp
l' = takeAndRotate c a lp
rotateTo' :: Eq a => (a :+ b) -> CSeq (a :+ b) -> CSeq (a :+ b)
rotateTo' x = fromJust . C.findRotateTo (coreEq x)
coreEq :: Eq a => (a :+ b) -> (a :+ b) -> Bool
coreEq = (==) `on` (^.core)
lowerTangent :: (Num r, Ord r)
=> ConvexPolygon p r
-> ConvexPolygon p r
-> LineSegment 2 p r
lowerTangent (getVertices -> l) (getVertices -> r) = rotate xx yy zz zz''
where
xx = rightMost l
yy = leftMost r
zz = pred' yy
zz'' = succ' xx
rotate x y z z''
| focus z `isRightOf` (focus x, focus y) = rotate x z (pred' z) z''
| focus z'' `isRightOf` (focus x, focus y) = rotate z'' y z (succ' z'')
| otherwise = ClosedLineSegment (focus x)
(focus y)
succ' :: CSeq a -> CSeq a
succ' = C.rotateR
pred' :: CSeq a -> CSeq a
pred' = C.rotateL
upperTangent :: (Num r, Ord r)
=> ConvexPolygon p r
-> ConvexPolygon p r
-> LineSegment 2 p r
upperTangent (getVertices -> l) (getVertices -> r) = rotate xx yy zz zz'
where
xx = rightMost l
yy = leftMost r
zz = succ' yy
zz' = pred' xx
rotate x y z z'
| focus z `isLeftOf` (focus x, focus y) = rotate x z (succ' z) z'
| focus z' `isLeftOf` (focus x, focus y) = rotate z' y z (pred' z')
| otherwise = ClosedLineSegment (focus x)
(focus y)
isRightOf :: (Num r, Ord r)
=> Point 2 r :+ p -> (Point 2 r :+ p', Point 2 r :+ p'') -> Bool
a `isRightOf` (b,c) = ccw (b^.core) (c^.core) (a^.core) == CW
isLeftOf :: (Num r, Ord r)
=> Point 2 r :+ p -> (Point 2 r :+ p', Point 2 r :+ p'') -> Bool
a `isLeftOf` (b,c) = ccw (b^.core) (c^.core) (a^.core) == CCW
minkowskiSum :: (Ord r, Num r)
=> ConvexPolygon p r -> ConvexPolygon q r -> ConvexPolygon (p,q) r
minkowskiSum p q = ConvexPolygon . fromPoints $ merge' (f p) (f q)
where
f p' = let xs@(S.viewl -> (v :< _)) = C.asSeq . bottomMost . getVertices $ p'
in F.toList $ xs |> v
(v :+ ve) .+. (w :+ we) = v .+^ (toVec w) :+ (ve,we)
cmpAngle v v' w w' =
ccwCmpAround (ext $ origin) (ext . Point $ v' .-. v) (ext . Point $ w' .-. w)
merge' [_] [_] = []
merge' vs@[v] (w:ws) = v .+. w : merge' vs ws
merge' (v:vs) ws@[w] = v .+. w : merge' vs ws
merge' (v:v':vs) (w:w':ws) = v .+. w :
case cmpAngle (v^.core) (v'^.core) (w^.core) (w'^.core) of
LT -> merge' (v':vs) (w:w':ws)
GT -> merge' (v:v':vs) (w':ws)
EQ -> merge' (v':vs) (w':ws)
merge' _ _ = error $ "minkowskiSum: Should not happen"
rightMost :: Ord r => CSeq (Point 2 r :+ p) -> CSeq (Point 2 r :+ p)
rightMost xs = let m = F.maximumBy (comparing (^.core)) xs in rotateTo' m xs
leftMost :: Ord r => CSeq (Point 2 r :+ p) -> CSeq (Point 2 r :+ p)
leftMost xs = let m = F.minimumBy (comparing (^.core)) xs in rotateTo' m xs
bottomMost :: Ord r => CSeq (Point 2 r :+ p) -> CSeq (Point 2 r :+ p)
bottomMost xs = let f p = (p^.core.yCoord,p^.core.xCoord)
m = F.minimumBy (comparing f) xs
in rotateTo' m xs
getVertices :: ConvexPolygon p r -> CSeq (Point 2 r :+ p)
getVertices = view (simplePolygon.outerBoundary)
test1 :: Num r => ConvexPolygon () r
test1 = ConvexPolygon . fromPoints . map ext . reverse $ [origin, point2 1 4, point2 5 6, point2 10 3]
test2 :: Num r => ConvexPolygon () r
test2 = ConvexPolygon . fromPoints . map ext . reverse $ [point2 11 6, point2 10 10, point2 15 18, point2 12 5]
testA :: Num r => ConvexPolygon () r
testA = ConvexPolygon . fromPoints . map ext $ [origin, point2 5 1, point2 2 2]
testB :: Num r => ConvexPolygon () r
testB = ConvexPolygon . fromPoints . map ext $ [origin, point2 5 3, point2 (2) 2, point2 (2) 1]