Cleanup: broken pathfinding

src/Dodge/Path.hs

@@ -3,18 +3,24 @@ module Dodge.Path
     --, pointTowardsImpulse'
     , makePathBetween
     , makePathBetweenPs
+    , removePathsCrossing
+    , pairsToGraph
     )
     where
 import Dodge.Data
 import Dodge.Base.Collide
 import Dodge.Zone
 import Geometry.Data
+import Dodge.Base
+import Geometry
 
-import Data.List
+import Control.Lens
 import Data.Maybe
+import Data.List
+import qualified Data.IntMap.Strict as IM
+import Data.Graph.Inductive hiding ((&))
 --import Data.Graph.Inductive.PatriciaTree
-import Data.Graph.Inductive.Query.SP
-import Data.Graph.Inductive.Graph hiding ((&))
+--import Data.Graph.Inductive.Graph hiding ((&))
 
 makePathBetween :: Point2 -> Point2 -> World -> Maybe [Int]
 makePathBetween a b w = do -- join $ sp <$> a' <*> b' <*> return (_pathGraph w)
@@ -73,3 +79,19 @@ pointTowardsImpulse a b w = find (flip (isWalkable a) w) =<< makePathBetweenPs a
 --    case ns of 
 --        [] -> return Nothing
 --        _  -> return $ Just $ ns !! i
+--
+pairsToGraph :: (Ord a, Eq b) => (a -> a -> b) -> [(a,a)] -> Gr a b
+pairsToGraph f pairs = 
+    let nodes' = nub (map fst pairs ++ map snd pairs)
+        pairs' = map (\(x,y)->(x,y,f x y)) pairs
+    in undir $ run_ Data.Graph.Inductive.empty $ insMapNodesM nodes' >> insMapEdgesM pairs'
+
+removePathsCrossing :: Point2 -> Point2 -> World -> World
+removePathsCrossing a b w = w
+    & pathGraph  .~ newGraph 
+    & pathGraphP .~ pg'
+    & pathPoints .~ foldr insertPoint IM.empty (labNodes newGraph)
+  where 
+    pg' = filter (isNothing . uncurry (intersectSegSeg a b)) $ _pathGraphP w
+    insertPoint pp@(_,V2 x y) = insertInZoneWith (floorHun x) (floorHun y) (++) [pp]
+    newGraph = pairsToGraph dist pg'