Reorganise geometry modules slightly

2021-12-15 10:59:06 +00:00
parent e3402bacf1
commit 9333d9497e
10 changed files with 198 additions and 160 deletions
@@ -5,7 +5,7 @@ module Geometry.ConvexPoly
    , cpPoints
    , cpCen
    , cpRad
-    , centroid
+--    , centroid
    , pointsToPoly
    , convexPolysOverlap
    , pointInPolyPoints
@@ -14,9 +14,10 @@ import Geometry.Data
 import Geometry.Vector
 import Geometry.LHS
 import Geometry.Intersect
+import Geometry.Polygon

 import Control.Lens
-import qualified Control.Foldl as L
+--import qualified Control.Foldl as L
 data ConvexPoly = ConvexPoly
    { _cpPoints :: [Point2]
    , _cpCen    :: Point2
@@ -68,7 +69,5 @@ pairPolyPointsIntersect a' b' (c':d':xs') = go c' a' b' (c':d':xs')
    go _ _ _ _ = False
 pairPolyPointsIntersect _ _ _ = False

-centroid :: Foldable t => t Point2 -> Point2
-centroid = L.fold $ (/) <$> L.Fold (+.+) (V2 0 0) id <*> L.genericLength

 makeLenses ''ConvexPoly
@@ -1,13 +1,14 @@
+{-# LANGUAGE BangPatterns #-}
 --{-# LANGUAGE TupleSections #-}
-{-
-Testing for and finding intersection points.
-}
+{- Testing for and finding intersection points.  -}
 module Geometry.Intersect where
 import Geometry.Data
 import Geometry.Vector
 import Geometry.LHS
 import Control.Applicative

+import Data.List
+
 import Data.Maybe (isNothing)
 -- | If two lines intersect, return 'Just' that point.
 intersectLineLine' :: Point2 -> Point2 -> Point2 -> Point2 -> Maybe Point2
@@ -212,3 +213,51 @@ intersectSegPolyFirst :: Point2 -> Point2 -> [Point2] -> Maybe Point2
 intersectSegPolyFirst a b xs = foldr (<|>) Nothing $ zipWith lineColl xs (tail xs ++ [head xs])
  where
    lineColl = intersectSegSeg a b
+
+-- | Given a line and a point return the point on the line closest to the
+-- point.
+closestPointOnLine 
+    :: Point2 -- ^ First line point. 
+    -> Point2 -- ^ Second line point.
+    -> Point2 -- ^ Point not on line.
+    -> Point2
+{-# INLINE closestPointOnLine #-}
+closestPointOnLine !a  !b  !p  = a +.+ u *.* (b -.- a)
+  where u = closestPointOnLineParam a b p
+-- | Given a line and a point return a value corresponding to how far along the
+-- line the point is.
+closestPointOnLineParam 
+    :: Point2   -- ^ First line point. 
+    -> Point2   -- ^ Second line point.
+    -> Point2   -- ^ Point not on line.
+    -> Float
+{-# INLINE closestPointOnLineParam #-}
+closestPointOnLineParam !a !b !p
+    = (p -.- a) `dotV` (b -.- a) / (b -.- a) `dotV` (b -.- a)
+
+-- | Given a segment and external point, find the closest point on the segment.
+-- clamps to the end of the segment
+closestPointOnSeg :: Point2 -> Point2 -> Point2 -> Point2
+closestPointOnSeg segP1 segP2 p
+    | closestPointOnLineParam segP1 segP2 p <= 0 = segP1
+    | closestPointOnLineParam segP1 segP2 p >= 1 = segP2
+    | otherwise                                  = closestPointOnLine segP1 segP2 p
+
+-- | Given a segment and external point, find the closest point on the segment.
+-- does not return closest points beyond the segment
+orthogonalPointOnSeg :: Point2 -> Point2 -> Point2 -> Maybe Point2
+orthogonalPointOnSeg a b p
+    | param < 0 || param > 1 = Nothing
+    | otherwise              = Just $ a +.+ param *.* (normalizeV $ b -.- a)
+  where
+    param = closestPointOnLineParam a b p
+intersectCircSeg :: Point2 -> Float -> Point2 -> Point2 -> [Point2]
+intersectCircSeg c r a b 
+    | y < 0 = []
+    | otherwise = nub $ filter ( (< dist a b) . dist a ) [ d -.- v, d +.+ c ]
+  where
+    d = closestPointOnLine a b c
+    x = dist d c
+    y = r ** 2 - x ** 2
+    z = sqrt y
+    v = z *.* normalizeV (b -.- a)
@@ -0,0 +1,102 @@
+{-# LANGUAGE BangPatterns #-}
+module Geometry.Polygon where
+import Geometry.Data
+import Geometry.LHS
+import Geometry.Vector
+
+import Data.Maybe
+import Data.List
+import qualified Control.Foldl as L
+
+-- | Draw a rectangle based on maximal N E S W values.
+rectNESW :: Float -> Float -> Float -> Float -> [Point2]
+rectNESW !a !b !c !d = [V2 b a,V2 b c,V2 d c,V2 d a]
+-- | Draw a rectangle based on maximal N S E W values.
+rectNSEW :: Float -> Float -> Float -> Float -> [Point2]
+rectNSEW !n !s !e !w = rectNESW n e s w
+-- | Draw a rectangle based on maximal N S W E values.
+rectNSWE :: Float -> Float -> Float -> Float -> [Point2]
+rectNSWE !n !s !w !e = [V2 w n, V2 w s, V2 e s, V2 e n]
+-- | Draw a rectangle around the origin with given height and width
+rectWH :: Float -> Float -> [Point2]
+rectWH w h = rectNSWE h (-h) (-w) w
+
+rectXH :: Float -> Float -> [Point2]
+rectXH x h = rectNSWE h (-h) 0 x
+
+rectXY :: Float -> Float -> [Point2]
+rectXY x y = rectNSWE y 0 0 x
+
+square :: Float -> [Point2]
+square n = rectWH n n
+
+mirrorXAxis :: [Point2] -> [Point2]
+mirrorXAxis ps = orderPolygon $ ps ++ mapMaybe f ps
+  where
+    f (V2 _ 0) = Nothing
+    f (V2 x y) = Just $ V2 x (-y)
+
+-- | Test whether a point is in a polygon or on the polygon border.
+-- Supposes the points in the
+-- polygon are listed in anticlockwise order.
+pointInOrOnPolygon :: Point2 -> [Point2] -> Bool
+pointInOrOnPolygon !p (x:xs) = all (\l -> not (uncurry isRHS l p)) $ zip (x:xs) (xs ++ [x])
+pointInOrOnPolygon _ _ = undefined
+-- | Test whether a point is strictly inside a polygon.
+-- Supposes the points in the polygon are listed in anticlockwise order.
+pointInPolygon :: Point2 -> [Point2] -> Bool
+pointInPolygon !p (x:xs) = all (\l -> uncurry isLHS l p) $ zip (x:xs) (xs ++ [x])
+pointInPolygon _ [] = False
+
+orderPolygonAround 
+    :: Point2 -- ^ point to order around
+    -> [Point2]
+    -> [Point2]
+orderPolygonAround _ [] = []
+orderPolygonAround cen ps = sortOn (\p -> argV (p -.- cen)) ps
+
+orderAroundFirstReverse :: [Point2] -> [Point2]
+orderAroundFirstReverse [] = []
+orderAroundFirstReverse (a:as) = a : reverse (orderPolygonAround a as)
+
+orderAroundFirst :: [Point2] -> [Point2]
+orderAroundFirst [] = []
+orderAroundFirst (a:as) = a : orderPolygonAround a as
+-- | Reorder points to be anticlockwise around their center.
+orderPolygon :: [Point2] -> [Point2]
+orderPolygon [] = []
+--orderPolygon ps = orderPolygonAround (1/ fromIntegral (length ps) *.* foldr1 (+.+) ps) ps
+orderPolygon ps = orderPolygonAround (centroid ps) ps
+-- | Adds a point to a convex polygon.
+-- If the point is inside, returns the original.
+-- Points ordered anticlockwise, input not checked.
+addPointPolygon :: Point2 -> [Point2] -> [Point2]
+addPointPolygon p ps
+    | pointInOrOnPolygon p ps = ps
+    | otherwise = orderPolygon $ p : ps
+-- | Creates the convex hull of a set of points.
+-- Need to verify whether or not this is ordered
+convexHull :: [Point2] -> [Point2]
+convexHull (x:y:z:xs) = grahamScan $ orderAroundFirst $ sortOn (\(V2 a b) -> (b,a)) (x:y:z:xs)
+convexHull _ = error "Tried to create the convex hull of two or fewer points"
+-- | Creates the convex hull of a set of points.
+-- assumes no repetition of points: try nubbing!
+convexHullSafe :: [Point2] -> [Point2]
+--convexHullSafe (x:y:z:xs) = grahamScan $ orderAroundFirst $ sortOn (\(V2 a b) -> (b,a)) (x:y:z:xs)
+convexHullSafe (x:y:z:xs) = grahamScan $ orderAroundFirst $ sortOn (\(V2 a b) -> (b,a)) (x:y:z:xs)
+convexHullSafe _ = []
+
+grahamScan :: [Point2] -> [Point2]
+grahamScan = foldr push []
+  where
+    push point stack = grahamEliminate (point:stack)
+-- | Remove second element if top three elements are not counterclockwise.
+-- Repeat if necessary. See 
+-- https://codereview.stackexchange.com/questions/206019/graham-scan-algorithm-in-haskell
+grahamEliminate :: [Point2] -> [Point2]
+grahamEliminate (x:y:z:xs)
+    | not $ isLHS x y z = grahamEliminate (x:z:xs)
+grahamEliminate xs = xs
+
+centroid :: Foldable t => t Point2 -> Point2
+centroid = L.fold $ (/) <$> L.Fold (+.+) (V2 0 0) id <*> L.genericLength