Kinda working A* - however, it doesn't yet give a path, for some reason, it only shows all explored nodes, before finding the end o.o

Author: Daxode

Blobtory/Scripts/planet_former/AStar.py

@@ -1,30 +1,79 @@
+from copy import deepcopy
 from typing import Dict, Set, List, Tuple
 from Blobtory.Scripts.planet_former.NodeRef import NodeRef, NodeKey
+from queue import PriorityQueue
+from multipledispatch import dispatch
+
+
+def GetDist(pFrom: Tuple[float, float, float, float],
+            pTo: Tuple[float, float, float, float]):
+    return (abs(pFrom[0] - pTo[0]) +
+            abs(pFrom[1] - pTo[1]) +
+            abs(pFrom[2] - pTo[2]))
+
+
+def ReconstructPath(cameFrom: Dict[NodeRef, NodeKey], current: NodeKey):
+    totalPath: List[Tuple[float, float, float, float]] = [current.point]
+    for key in cameFrom.keys():
+        current = cameFrom[key]
+        totalPath.insert(0, current.point)
+
+    return totalPath
 
 
 class AStar:
     nodeDict: Dict
+    keyList: List[NodeKey]
 
     def __init__(self, nodeDict: Dict):
         self.nodeDict = nodeDict
+        self.keyList = list(self.nodeDict.keys())
+
+    def GetKeyNodeFromRef(self, p: NodeRef) -> NodeKey:
+        return self.keyList[self.keyList.index(p.point)]
+
+    def GetKeyNodeFromPoint(self, p: Tuple[float, float, float, float]) -> NodeKey:
+        return self.keyList[self.keyList.index(p)]
+
+    def GetH(self, p: NodeKey) -> float:
+        return 1/p.weight
 
     def GetPathFromTo(self,
                       pFrom: Tuple[float, float, float, float],
                       pTo: Tuple[float, float, float, float]
                       ) -> List[Tuple[float, float, float, float]]:
-        neighboursNeighbourPoints = []
+        startNode = self.GetKeyNodeFromPoint(pFrom)
+
+        openSet = PriorityQueue()
+        openSet.put((0, startNode))
+        cameFrom: Dict[NodeRef, NodeKey] = {}
+
+        gScore = {nodeKey: float("inf") for nodeKey in self.nodeDict.keys()}
+        gScore[pFrom] = 0
+
+        fScore = deepcopy(gScore)
+        fScore[pFrom] = self.GetH(startNode)
+
+        openSetTable = {startNode}
+
+        while not openSet.empty():
+            setVal = openSet.get()
+            current: NodeKey = setVal[1]
+            openSetTable.remove(current)
 
-        neighbourPointsTo: Set[NodeRef] = self.nodeDict[pTo]
-        neighbourPointsFrom: Set[NodeRef] = self.nodeDict[pFrom]
+            if current == pTo:
+                return ReconstructPath(cameFrom, current)
 
-        for point in neighbourPointsTo:
-            # keyList = list(self.nodeDict.keys())
-            # print(keyList[keyList.index(point)].point)
-            points = self.nodeDict[point]
-            neighboursNeighbourPoints.extend([point.point for point in points])
+            neighbourPoints: Set[NodeRef] = self.nodeDict[current]
+            for neighbourPoint in neighbourPoints:
+                tentativeGScore = gScore[current] + GetDist(current.point, neighbourPoint.point)
 
-        for point in neighbourPointsFrom:
-            points = self.nodeDict[point]
-            neighboursNeighbourPoints.extend([point.point for point in points])
+                if tentativeGScore < gScore[neighbourPoint]:
+                    cameFrom[neighbourPoint] = current
+                    gScore[neighbourPoint] = tentativeGScore
+                    fScore[neighbourPoint] = gScore[neighbourPoint] + self.GetH(self.GetKeyNodeFromRef(neighbourPoint))
+                    if neighbourPoint not in openSetTable:
+                        openSet.put((fScore[neighbourPoint], neighbourPoint))
+                        openSetTable.add(neighbourPoint)
 
-        return neighboursNeighbourPoints
+        return []

Blobtory/Scripts/planet_former/NodeRef.py

@@ -14,6 +14,12 @@ def __getitem__(self, key):
     def __eq__(self, other):
         return hash(self) == hash(other)
 
+    def __str__(self):
+        return str(self.point)
+
+    def __repr__(self):
+        return str(self.point)
+
 
 class NodeKey:
     point: (float, float, float, float)
@@ -29,4 +35,10 @@ def __getitem__(self, key):
         return self.point[key]
 
     def __eq__(self, other):
-        return hash(self) == hash(other)
+        return hash(self) == hash(other)
+
+    def __str__(self):
+        return str(self.point)
+
+    def __repr__(self):
+        return str(self.point)

Blobtory/Scripts/planet_former/PlanetGenerator.py

@@ -17,6 +17,8 @@
 class PlanetGenerator:
     shouldUpdatePhysicsMeshes = False
     aStarHandler: AStar
+    listOfItems = None
+    examplePointFrom: NodeKey
 
     def __init__(self, winCreator: WindowCreator, gridSize: int, radius: float):
         self.radius = radius
@@ -58,6 +60,18 @@ def __init__(self, winCreator: WindowCreator, gridSize: int, radius: float):
         self.RegenPlanet()
         self.winCreator.base.taskMgr.doMethodLater(1, self.UpdatePhysicsMesh, "Planet Physics Updater")
 
+        self.winCreator.base.accept("r", self.NextPoint)
+
+    def NextPoint(self):
+        examplePointTo: NodeKey = next(self.listOfItems)
+
+        self.sphere3.setPos(examplePointTo[0], examplePointTo[1], examplePointTo[2])
+        PipelineInstancing.RenderThisModelAtVertexes(self.sphere1,
+                                                     self.aStarHandler.GetPathFromTo(
+                                                         self.examplePointFrom.point,
+                                                         examplePointTo.point),
+                                                     self.winCreator)
+
     def RegenPlanet(self):
         self.winCreator.baseData.debuggerPlanetFormer.Inform("Regenerating planet")
         self.cubeformerNav.GenerateNoiseSphere(self.radius)
@@ -72,67 +86,61 @@ def UpdatePlanet(self):
 
     def UpdatePhysicsMesh(self, task):
         if self.shouldUpdatePhysicsMeshes:
-            # Generate marching
-            self.marchingCubesNav.EdgeGenerator()
-            self.marchingCubesNav.MarchCube()
-
-            # Extract Mesh Data (Tri Indexes and Vertexes)
-            self.winCreator.base.graphicsEngine.extractTextureData(self.marchingCubesNav.edgeVertexBuffer,
-                                                                   self.winCreator.base.win.gsg)
-            ramImageVertex = self.marchingCubesNav.edgeVertexBuffer.getRamImage()
-            output = np.frombuffer(ramImageVertex, dtype=np.float32)
-            output: np.ndarray = output.reshape((self.marchingCubesNav.size[2],
-                                     self.marchingCubesNav.size[1],
-                                     self.marchingCubesNav.size[0]*3, 4))
-
-            self.winCreator.base.graphicsEngine.extractTextureData(self.marchingCubesNav.triangleBuffer,
-                                                                   self.winCreator.base.win.gsg)
-            ramImage = self.marchingCubesNav.triangleBuffer.getRamImage()
-            outputTriangle = memoryview(ramImage).cast("i")
-
-            # Restructure that data to be a node network instead using a dictionary
-            outputR = map(tuple, output.reshape((self.marchingCubesNav.size[0]*3 *
-                                     self.marchingCubesNav.size[1] *
-                                     self.marchingCubesNav.size[2], 4)))
-            nodeDict = dict((NodeKey(el), set([])) for el in outputR)
-            del outputR
-
-            buffer = np.empty(12, dtype=int)
-
-            triagIndexCount = self.marchingCubesNav.vertexCount * 4
-            for count, x in enumerate(outputTriangle):
-                buffer[count % 12] = x
-                if count % 12 == 11:
-                    v1: NodeRef = NodeRef(tuple(output[buffer[2], buffer[1], buffer[0]]))
-                    v2: NodeRef = NodeRef(tuple(output[buffer[6], buffer[5], buffer[4]]))
-                    v3: NodeRef = NodeRef(tuple(output[buffer[10], buffer[9], buffer[8]]))
-
-                    nodeDict[v1].add(v2)
-                    nodeDict[v1].add(v3)
-
-                    nodeDict[v2].add(v1)
-                    nodeDict[v2].add(v3)
-
-                    nodeDict[v3].add(v2)
-                    nodeDict[v3].add(v1)
-
-                if count > triagIndexCount:
-                    break
-            listOfItems = (item[0] for item in nodeDict.items() if len(item[1]) > 0)
-
-            examplePointFrom: NodeKey = next(listOfItems)
-            for i in range(128): next(listOfItems)
-            examplePointTo: NodeKey = next(listOfItems)
-
-            self.sphere2.setPos(examplePointFrom[0], examplePointFrom[1], examplePointFrom[2])
-            self.sphere3.setPos(examplePointTo[0], examplePointTo[1], examplePointTo[2])
-
-            self.aStarHandler = AStar(nodeDict)
-            PipelineInstancing.RenderThisModelAtVertexes(self.sphere1,
-                                                         self.aStarHandler.GetPathFromTo(
-                                                             examplePointFrom.point,
-                                                             examplePointTo.point),
-                                                         self.winCreator)
+            self.GenerateAStarPather()
 
             self.shouldUpdatePhysicsMeshes = False
         return Task.again
+
+    def GenerateAStarPather(self):
+        # Generate marching
+        self.marchingCubesNav.EdgeGenerator()
+        self.marchingCubesNav.MarchCube()
+
+        # Extract Mesh Data (Tri Indexes and Vertexes)
+        self.winCreator.base.graphicsEngine.extractTextureData(self.marchingCubesNav.edgeVertexBuffer,
+                                                               self.winCreator.base.win.gsg)
+        ramImageVertex = self.marchingCubesNav.edgeVertexBuffer.getRamImage()
+        output = np.frombuffer(ramImageVertex, dtype=np.float32)
+        output: np.ndarray = output.reshape((self.marchingCubesNav.size[2],
+                                             self.marchingCubesNav.size[1],
+                                             self.marchingCubesNav.size[0] * 3, 4))
+
+        self.winCreator.base.graphicsEngine.extractTextureData(self.marchingCubesNav.triangleBuffer,
+                                                               self.winCreator.base.win.gsg)
+        ramImage = self.marchingCubesNav.triangleBuffer.getRamImage()
+        outputTriangle = memoryview(ramImage).cast("i")
+
+        # Restructure that data to be a node network instead using a dictionary
+        outputR = map(tuple, output.reshape((self.marchingCubesNav.size[0] * 3 *
+                                             self.marchingCubesNav.size[1] *
+                                             self.marchingCubesNav.size[2], 4)))
+        nodeDict = dict((NodeKey(el), set([])) for el in outputR)
+        del outputR
+
+        buffer = np.empty(12, dtype=int)
+
+        triagIndexCount = self.marchingCubesNav.vertexCount * 4
+        for count, x in enumerate(outputTriangle):
+            buffer[count % 12] = x
+            if count % 12 == 11:
+                v1: NodeRef = NodeRef(tuple(output[buffer[2], buffer[1], buffer[0]]))
+                v2: NodeRef = NodeRef(tuple(output[buffer[6], buffer[5], buffer[4]]))
+                v3: NodeRef = NodeRef(tuple(output[buffer[10], buffer[9], buffer[8]]))
+
+                nodeDict[v1].add(v2)
+                nodeDict[v1].add(v3)
+
+                nodeDict[v2].add(v1)
+                nodeDict[v2].add(v3)
+
+                nodeDict[v3].add(v2)
+                nodeDict[v3].add(v1)
+
+            if count > triagIndexCount:
+                break
+        self.listOfItems = (item[0] for item in nodeDict.items() if len(item[1]) > 0)
+
+        self.examplePointFrom: NodeKey = next(self.listOfItems)
+        self.sphere2.setPos(self.examplePointFrom[0], self.examplePointFrom[1], self.examplePointFrom[2])
+
+        self.aStarHandler = AStar(nodeDict)