done implementing functor to search through quad tree, adding to priority queue (for Dijkstra's algorithm)

fcore/bin-vec.sml

@@ -19,6 +19,7 @@ sig
   val findInsPos: elem * elem vector -> int
   val insert: elem vector * elem * int -> elem vector
   val delete: elem vector * elem -> elem vector
+  val updateAtIdx: elem vector * elem * int -> elem vector
 end
 
 functor MakeBinVec(Fn: MAKE_BIN_VEC): BIN_VEC =
@@ -136,6 +137,10 @@ struct
           VectorSlice.concat [slice1, slice2]
         end
     end
+
+  fun updateAtIdx (vec, elem, idx) =
+    Vector.mapi
+      (fn (curIdx, curElem) => if curIdx <> idx then curElem else elem) vec
 end
 
 structure IntSet =
@@ -151,20 +156,17 @@ structure IntSet =
 structure ValSet =
   MakeBinVec
     (struct
-      type elem = {distance: int, from: char}
+       type elem = {distance: int, from: char}
 
-      (* l, e and q functions are not actually used in the ValSet
+       (* l, e and q functions are not actually used in the ValSet
-      * because the IntSet is meant to contain keys while the ValSet
+       * because the IntSet is meant to contain keys while the ValSet
-      * is meant to contain corresponding values, like in a Map structure.
+       * is meant to contain corresponding values, like in a Map structure.
-      * However, it's required by the functor, 
+       * However, it's required by the functor, 
-      * and it is actually easy to implement so no issue. *)
+       * and it is actually easy to implement so no issue. *)
 
-      fun l ({distance = a, ...}: elem, {distance = b, ...}: elem) =
+       fun l ({distance = a, ...}: elem, {distance = b, ...}: elem) = a < b
-        a < b
 
-      fun eq ({distance = a, ...}: elem, {distance = b, ...}: elem) =
+       fun eq ({distance = a, ...}: elem, {distance = b, ...}: elem) = a = b
-        a = b
 
-      fun g ({distance = a, ...}: elem, {distance = b, ...}: elem) =
+       fun g ({distance = a, ...}: elem, {distance = b, ...}: elem) = a > b
-        a > b
      end)

fcore/path-finding.sml

@@ -1,14 +1,233 @@
 structure PathFinding =
 struct
+  (* functor for adding reachable platforms to queue *)
+  structure FindReachable =
+    MakeQuadFolder
+      (struct
+         open GameType
+
+         type env =
+           { platforms: GameType.platform vector
+           , currentPlat: GameType.platform
+           , eKeys: IntSet.elem vector
+           }
+
+         type state = ValSet.elem vector * DistHeap.t
+
+         fun isBetween (p1, check, p2) = check >= p1 andalso check <= p2
+
+         fun canJumpUpTo (prevPlat: platform, currentPlat: platform) =
+           let
+             val {x = prevX, y = prevY, width = prevWidth, ...} = prevPlat
+             val {x = curX, y = curY, width = curWidth, ...} = currentPlat
+
+             val prevFinishX = prevX + prevWidth
+             val curFinishX = curX + curWidth
+           in
+             (isBetween (prevX, curX, prevFinishX)
+              orelse
+              isBetween (prevX, curFinishX, prevFinishX)
+              andalso prevY + Constants.jumpLimit >= curY)
+           end
+
+         fun canDropDownTo (prevPlat: platform, currentPlat: platform) =
+           let
+             val {x = prevX, y = prevY, width = prevWidth, ...} = prevPlat
+             val {x = curX, y = curY, width = curWidth, ...} = currentPlat
+
+             val prevFinishX = prevX + prevWidth
+             val curFinishX = curX + curWidth
+           in
+             (isBetween (prevX, curX, prevFinishX)
+              orelse
+              isBetween (prevX, curFinishX, prevFinishX) andalso prevY <= curY)
+           end
+
+         fun isReachableFromLeft (prevPlat, currentPlat) =
+           (* prev = right/from, current = left/to *)
+           let
+             val {x = prevX, y = prevY, width = prevWidth, ...} = prevPlat
+             val {x = curX, y = curY, width = curWidth, ...} = currentPlat
+
+             val enemyX = prevX
+             val xDiff = prevX - curX
+           in
+             if xDiff <= Constants.jumpLimit then
+               true
+             else
+               let
+                 val enemyApexX = enemyX - Constants.jumpLimit
+                 val enemyApexY = prevY + Constants.jumpLimit
+
+                 val curFinishX = curX + curWidth
+
+                 val diffApexY = enemyApexY - curY
+                 val diffApexX = enemyApexX - curFinishX
+               in
+                 diffApexX <= diffApexY orelse diffApexY <= 0
+               end
+           end
+
+         fun isReachableFromRight (prevPlat, currentPlat) =
+           (* prev = left/from, current = right/to *)
+           let
+             val {x = prevX, y = prevY, width = prevWidth, ...} = prevPlat
+             val {x = curX, y = curY, width = curWidth, ...} = currentPlat
+
+             (* last x coordinate where enemy can fully fit on prevPlat *)
+             val enemyX = prevX + prevWidth - Constants.enemySize
+
+             val xDiff = curX - prevX
+           in
+             if xDiff <= Constants.jumpLimit then
+               (* platform is possible to jump to without falling *)
+               true
+             else
+               let
+                 val enemyApexX = enemyX + Constants.jumpLimit
+                 val enemyApexY = prevY + Constants.jumpLimit
+
+                 val diffApexY = enemyApexY - curY
+                 val diffApexX = enemyApexX - curX
+               in
+                 diffApexY <= 0 orelse diffApexX <= diffApexY
+               end
+           end
+
+         fun insertIfNotExistsOrShorter (dist, eKeys, eVals, foldPlatID, q) =
+           let
+             val pos = IntSet.findInsPos (foldPlatID, eKeys)
+           in
+             if pos <> ~1 andalso pos <> Vector.length eKeys then
+               let
+                 val key = IntSet.sub (eKeys, pos)
+               in
+                 if pos = key then
+                   (* may need to update record in eVals if it is shorter *)
+                   let
+                     val {distance = oldDist, ...} = ValSet.sub (eVals, pos)
+                   in
+                     if dist < oldDist then
+                       (* update values as we found a shorter path *)
+                       let
+                         val eVals = ValSet.updateAtIdx
+                           ( eVals
+                           , {distance = dist, from = Char.chr foldPlatID}
+                           , pos
+                           )
+                       in
+                         (eVals, q)
+                       end
+                     else
+                       (* return existing *)
+                       (eVals, q)
+                   end
+                 else
+                   (* key not explored, so add to queue *)
+                   let
+                     val q =
+                       DistHeap.insert ({distance = dist, id = foldPlatID}, q)
+                   in
+                     (eVals, q)
+                   end
+               end
+             else
+               (* key not explored, so add to queue *)
+               let
+                 val q = DistHeap.insert ({distance = dist, id = foldPlatID}, q)
+               in
+                 (eVals, q)
+               end
+           end
+
+         fun fState ((eVals, q), env, foldPlatID) =
+           let
+             val {platforms, currentPlat, eKeys} = env
+             val foldPlat = Platform.find (foldPlatID, platforms)
+           in
+             if
+               canJumpUpTo (currentPlat, foldPlat)
+               orelse canDropDownTo (currentPlat, foldPlat)
+             then
+               let
+                 (* only need to calculate vertical distance *)
+                 val {y = py, ...} = currentPlat
+                 val {y = cy, ...} = foldPlat
+                 val dist = abs (py - cy)
+               in
+                 insertIfNotExistsOrShorter (dist, eKeys, eVals, foldPlatID, q)
+               end
+             else if
+               isReachableFromLeft (currentPlat, foldPlat)
+               orelse isReachableFromRight (currentPlat, foldPlat)
+             then
+               let
+                 val {x = px, y = py, width = pw, ...} = currentPlat
+                 val {x = cx, y = cy, width = cw, ...} = foldPlat
+
+                 val pFinishX = px + pw
+                 val cFinishX = cx + cw
+
+                 val dist =
+                   if py = cy then
+                     let
+                       (* if on same y coordinate, 
+                        * only need to calculate horizontal distance *)
+                       val d1 = abs (px - cx)
+                       val d2 = abs (px - cFinishX)
+                       val d3 = abs (pFinishX - cx)
+                       val d4 = abs (pFinishX - cFinishX)
+
+                       val min = Int.min (d1, d2)
+                       val min = Int.min (min, d3)
+                     in
+                       Int.min (min, d4)
+                     end
+                   else
+                     let
+                       (* if they have different y coordinate,
+                        * need to calculate diagonal length/hypotenuse by pythagoras
+                        * *)
+                       val x1 = abs (px - cx)
+                       val x2 = abs (px - cFinishX)
+                       val x3 = abs (pFinishX - cx)
+                       val x4 = abs (pFinishX - cFinishX)
+
+                       val x = Int.min (x1, x2)
+                       val x = Int.min (x, x3)
+                       val x = Int.min (x, x4)
+
+                       (* there is only one y coordinate for platform
+                        * so don't need to 'minimise' it
+                        * *)
+                       val y = abs (py - cy)
+
+                       (* pythagoras *)
+                       val xsq = x * x
+                       val ysq = y * y
+                       val hypsq = xsq + ysq
+
+                       (* square root to find diagonal length *)
+                       val dg = Real.fromInt hypsq
+                       val dg = Math.sqrt dg
+                     in
+                       Real.toInt IEEEReal.TO_NEAREST dg
+                     end
+               in
+                 insertIfNotExistsOrShorter (dist, eKeys, eVals, foldPlatID, q)
+               end
+             else
+               (eVals, q)
+           end
+       end)
+
   fun filterMinDuplicates (q, eKeys) =
     let
       val min = DistHeap.findMin q
     in
       if IntSet.contains (min, eKeys) then
-        let
+        let val q = DistHeap.deleteMin q
-          val q = DistHeap.deleteMin q
+        in filterMinDuplicates (q, eKeys)
-        in
-          filterMinDuplicates (q, eKeys)
         end
       else
         q
@@ -69,7 +288,7 @@ struct
        * If the key matching this value is the same as the "from" node,
        * then we're done reconstructing the path and can return the path list.
        * *)
-      val eVal = {distance = 0, from = Char.fromInt eID}
+      val eVal = {distance = 0, from = Char.chr eID}
       val exploredVals = ValSet.insert (exploredVals, eVal, insPos)
     in
       loop (pID, eID, platforms, platformTree, q, eKeys, eVals)

fcore/quad-tree-fold.sml

@@ -2,8 +2,6 @@ signature QUAD_FOLDER =
 sig
   type env
   type state
-
-  val isReachable: state * env * int -> bool
   val fState: state * env * int -> state
 end
 
@@ -17,11 +15,7 @@ struct
     else
       let
         val {itemID, ...} = Vector.sub (elements, pos)
-        val state =
+        val state = Fn.fState (state, env, itemID)
-          if Fn.isReachable (state, env, itemID) then
-            Fn.fState (state, env, itemID)
-          else
-            state
       in
         foldVec (iX, iY, iW, iH, pos + 1, elements, state, env)
       end

oms.mlb

@@ -11,8 +11,6 @@ fcore/quad-tree-fold.sml
 fcore/bin-search.sml
 fcore/bin-vec.sml
 
-fcore/path-finding.sml
-
 ann
   "allowVectorExps true"
 in
@@ -29,6 +27,8 @@ fcore/player-patch.sml
 fcore/enemy-patch.sml
 fcore/physics.sml
 
+fcore/path-finding.sml
+
 fcore/enemy-behaviour.sml
 fcore/enemy.sml
 fcore/player.sml