Initial replacement of FGL with incidence representation, use A*

src/Dodge/Path.hs

@@ -13,10 +13,12 @@ module Dodge.Path (
     pairsToIncGraph,
 ) where
 
+import Control.Lens
+import qualified Algorithm.Search as AS
+import Dodge.Zoning.Common
 import qualified Data.Vector as V
 import qualified Data.Vector.Unboxed as UV
-import Control.Lens
-import Data.Bifunctor
+--import Data.Bifunctor
 import Data.Foldable
 import Data.Graph.Inductive hiding ((&))
 import Data.List (sortOn)
@@ -36,24 +38,52 @@ import qualified IntMapHelp as IM
 getNodePos :: Int -> World -> Maybe Point2
 getNodePos i w = (w ^. cWorld . pathGraph) `lab` i
 
-makePathUsing :: (PathEdge -> Bool) -> Point2 -> Point2 -> World -> Maybe [Int]
+--makePathUsing' :: (PathEdge -> Bool) -> Point2 -> Point2 -> World -> Maybe [Int]
+--makePathUsing' t s e w = do
+--    na <- walkableNodeNear w s
+--    nb <- walkableNodeNear w e
+--    sp na nb . second _peDist . efilter (^. _3 . to t) $ w ^. cWorld . pathGraph
+
+makePathUsing :: (Set.Set EdgeObstacle -> Bool) -> Point2 -> Point2 -> World -> Maybe [Int]
 makePathUsing t s e w = do
     na <- walkableNodeNear w s
     nb <- walkableNodeNear w e
-    sp na nb . second _peDist . efilter (^. _3 . to t) $ w ^. cWorld . pathGraph
+    let f i = getes i
+        h i = distance (getn nb) (getn i)
+    (na :) . snd <$> AS.aStarAssoc f h (==nb) na
+  where
+    g (i,SimpleEdge c o)
+        | t o = Just (i,c)
+        | otherwise = Nothing
+    getes i = mapMaybe g $ w ^?! cWorld . incGraph . ix i
+    getn i = w ^?! cWorld . incNode . ix i
+  
+--    sp na nb . second _peDist . efilter (^. _3 . to t) $ w ^. cWorld . pathGraph
 
 makePathBetween :: Point2 -> Point2 -> World -> Maybe [Int]
 makePathBetween = makePathUsing $ not . pathEdgeObstructed
 
-pathEdgeObstructed :: PathEdge -> Bool
-pathEdgeObstructed pe =
+pathEdgeObstructed' :: PathEdge -> Bool
+pathEdgeObstructed' pe =
     any
         (`Set.member` _peObstacles pe)
         [DoorObstacle, BlockObstacle, ChasmObstacle]
 
+pathEdgeObstructed :: Set.Set EdgeObstacle -> Bool
+pathEdgeObstructed pe =
+    any
+        (`Set.member` pe)
+        [DoorObstacle, BlockObstacle, ChasmObstacle]
+
 walkableNodeNear :: World -> Point2 -> Maybe Int
 {-# INLINE walkableNodeNear #-}
 walkableNodeNear w p = fmap fst . find (flip (isWalkable p) w . snd) $ nodesNear
+  where
+    nodesNear = zonesExtract (w ^. incNodeZoning) . snailAround $ zoneOfPoint pnZoneSize p
+
+walkableNodeNear' :: World -> Point2 -> Maybe Int
+{-# INLINE walkableNodeNear' #-}
+walkableNodeNear' w p = fmap fst . find (flip (isWalkable p) w . snd) $ nodesNear
   where
     nodesNear = zonesExtract (w ^. pnZoning) . snailAround $ zoneOfPoint pnZoneSize p
 
@@ -130,7 +160,8 @@ pairsToIncGraph :: Set.Set (Point2,Point2)
         , V.Vector [(Int,SimpleEdge)]
         , [(Int,Int)]
     )
-pairsToIncGraph pairs = (inodes,incgraph,undefined)
+pairsToIncGraph pairs = (inodes,incgraph
+    ,Set.toList pairs & each . each %~ (\i -> pstons ^?! ix i . _head))
   where
     incgraph = V.generate (length im) (\i -> im ^?! ix i)
     im = IM.fromListWith (<>) . fmap toedge $ Set.toList pairs
@@ -178,9 +209,18 @@ obstructPathsCrossing ::
     (World, Set PathEdgeNodes)
 obstructPathsCrossing obstacletype sp' ep w =
     ( w & cWorld . pathGraph %~ updateedges
+        & cWorld . incGraph %~ updateincedges
     , es
     )
   where
+    updateincedge :: V.Vector [(Int,SimpleEdge)] -> (Int,Int) -> V.Vector [(Int,SimpleEdge)]
+    updateincedge gr (i,j) = gr & ix i . each %~ g j
+    g j (k,o) | j == k = (k,o & seObstacles . at obstacletype ?~ ())
+        | otherwise = (k,o)
+    updateincedges gr = foldl' updateincedge gr inces
+    inces = filter inedgecrosses $ nearSeg peZoneSize _incEdgeZoning sp' ep w
+    inedgecrosses (i,j) = isJust $ intersectSegSeg sp' ep (f i) (f j)
+    f i = w ^?! cWorld . incNode . ix i
     es = Set.filter edgecrosses $ pesNearSeg sp' ep w
     edgecrosses (PathEdgeNodes _ _ pe) =
         isJust $ intersectSegSeg sp' ep (_peStart pe) (_peEnd pe)