def getPrimsSharingGroupOfEdges(self, prim, setNotDestroyed, setPartiallyDestroyed, setTotDestroyed):
validPrim1 = None
validPrim2 = None
if(self.isThisPrimMaybeValid(prim)):
primsSharedInPathSet = GeoMath.getConnectedPrims(prim, self.path)
primsSharedInGeneral = GeoMath.getConnectedPrims(prim, setPartiallyDestroyed)
primsSharedInGeneral.extend(GeoMath.getConnectedPrims(prim, setNotDestroyed))
primsSharedInGeneral.extend(GeoMath.getConnectedPrims(prim, setTotDestroyed))
setOfEdgesToTrack = []
for primSharedInGeneral in primsSharedInGeneral:
setOfEdgesToTrack.extend(GeoMath.getEdgesBetweenPrims(prim, primSharedInGeneral))
excluded = []
for primSharedPath in primsSharedInPathSet:
edgesSharedWithPrim = GeoMath.getEdgesBetweenPrims(primSharedPath, prim)
excluded.extend(edgesSharedWithPrim)
# We need some first edge and last edge for track edges between this edges, excluding edges
# (included in group "excluded") in other prims that share some edge with central prim
indexPrim = self.path.index(prim)
if(indexPrim == 0):
prevIndexPrim = len(self.path) - 1
else:
prevIndexPrim = indexPrim - 1
edgesSharedWithPrim1 = GeoMath.getEdgesBetweenPrims(self.path[(indexPrim + 1) % len(self.path)], prim)
edgesSharedWithPrim2 = GeoMath.getEdgesBetweenPrims(self.path[prevIndexPrim], prim)
logging.debug("edges shared curPrim1: %s with prim: %s, edges: %s", str(self.path[(indexPrim + 1) % len(self.path)].number()), str(prim.number()), str(edgesSharedWithPrim1))
logging.debug("edges shared curPrim2: %s with prim: %s, edges: %s", str(self.path[prevIndexPrim].number()), str(prim.number()), str(edgesSharedWithPrim2))
groupEdges1, groupEdges2 = GeoMath.trackEdges(edgesSharedWithPrim1[0], setOfEdgesToTrack, edgesSharedWithPrim2[0], excluded)
# Find two good prims containing one of the group of edges
index = 0
while(index < len(primsSharedInGeneral) and not (validPrim1 and validPrim2)):
curPrim = primsSharedInGeneral[index]
conEdges = GeoMath.getEdgesBetweenPrims(curPrim, prim)
if(not validPrim1):
for edge in groupEdges1:
if(GeoMath.sameEdge(conEdges[0], edge)):
validPrim1 = curPrim
break
if (not validPrim2):
for edge in groupEdges2:
if(GeoMath.sameEdge(conEdges[0], edge)):
validPrim2 = curPrim
break
index += 1
return validPrim1, validPrim2
def getPrimsSharingGroupOfEdges(self, prim, setNotDestroyed,
setPartiallyDestroyed, setTotDestroyed):
validPrim1 = None
validPrim2 = None
if (self.isThisPrimMaybeValid(prim)):
primsSharedInPathSet = GeoMath.getConnectedPrims(prim, self.path)
primsSharedInGeneral = GeoMath.getConnectedPrims(
prim, setPartiallyDestroyed)
primsSharedInGeneral.extend(
GeoMath.getConnectedPrims(prim, setNotDestroyed))
primsSharedInGeneral.extend(
GeoMath.getConnectedPrims(prim, setTotDestroyed))
setOfEdgesToTrack = []
for primSharedInGeneral in primsSharedInGeneral:
setOfEdgesToTrack.extend(
GeoMath.getEdgesBetweenPrims(prim, primSharedInGeneral))
excluded = []
for primSharedPath in primsSharedInPathSet:
edgesSharedWithPrim = GeoMath.getEdgesBetweenPrims(
primSharedPath, prim)
excluded.extend(edgesSharedWithPrim)
# We need some first edge and last edge for track edges between this edges, excluding edges
# (included in group "excluded") in other prims that share some edge with central prim
indexPrim = self.path.index(prim)
if (indexPrim == 0):
prevIndexPrim = len(self.path) - 1
else:
prevIndexPrim = indexPrim - 1
edgesSharedWithPrim1 = GeoMath.getEdgesBetweenPrims(
self.path[(indexPrim + 1) % len(self.path)], prim)
edgesSharedWithPrim2 = GeoMath.getEdgesBetweenPrims(
self.path[prevIndexPrim], prim)
logging.debug(
"edges shared curPrim1: %s with prim: %s, edges: %s",
str(self.path[(indexPrim + 1) % len(self.path)].number()),
str(prim.number()), str(edgesSharedWithPrim1))
logging.debug("edges shared curPrim2: %s with prim: %s, edges: %s",
str(self.path[prevIndexPrim].number()),
str(prim.number()), str(edgesSharedWithPrim2))
groupEdges1, groupEdges2 = GeoMath.trackEdges(
edgesSharedWithPrim1[0], setOfEdgesToTrack,
edgesSharedWithPrim2[0], excluded)
# Find two good prims containing one of the group of edges
index = 0
while (index < len(primsSharedInGeneral)
and not (validPrim1 and validPrim2)):
curPrim = primsSharedInGeneral[index]
conEdges = GeoMath.getEdgesBetweenPrims(curPrim, prim)
if (not validPrim1):
for edge in groupEdges1:
if (GeoMath.sameEdge(conEdges[0], edge)):
validPrim1 = curPrim
break
if (not validPrim2):
for edge in groupEdges2:
if (GeoMath.sameEdge(conEdges[0], edge)):
validPrim2 = curPrim
break
index += 1
return validPrim1, validPrim2
def getExtremPrims(self,
Ipoint,
primOfIpoint,
partDes,
refPrim,
notDes,
volume=None):
'''
We can't ensure that the primitives have a posible path, but we ensure that last primitive
have at least 2 adjacent primitives and first primitive is connected with the last primitive
'''
logging.debug("Start method getExtremPrims, class DefPath")
firstPrim = None
lastPrim = None
if (primOfIpoint and Ipoint):
# NOT YET TOTALLY IMPLEMENTED
edge = GeoMath.getEdgeWithPointInPrim(primOfIpoint, Ipoint)
lastPrim = primOfIpoint
for prim in partDes:
if (prim != lastPrim):
sharedEdges = GeoMath.getSharedEdgesPrims(lastPrim, prim)
rs_lP_fP = False
if (volume):
for edge in sharedEdges:
rs = RejectionSampling.RejectionSampling(
edge, volume)
rs.do()
point = rs.getValue()
if (point):
rs_lP_fP = True
break
if (len(sharedEdges >= 1) and (edge in sharedEdges)
and (volume == None or rs_lP_fP)):
firstPrim = prim
else:
# Automatically decision of extrem prims.
# Ensure that 2 prims is connected to another primitive in
# group of partially destroyed.
# Didn't use "getConnectedPrims" because need ramdonless in choice of prims.
stopSearch = False
tempList1 = list(partDes)
# minimum of 4 prims to get a path
while (len(tempList1) > 4 and not stopSearch):
numPrim1 = random.randint(0, len(tempList1) - 1)
prim1 = tempList1[numPrim1]
del tempList1[numPrim1]
# We have to ensure that first prim has at least two conected prims
if (True): # prim1.number()>17 and prim1.number()<27
while (
(len(GeoMath.getConnectedPrims(prim1, list(partDes),
2)) < 2)
and (len(tempList1) > 4)):
numPrim1 = random.randint(0, len(tempList1) - 1)
prim1 = tempList1[numPrim1]
del tempList1[numPrim1]
# If prim1 has at least two conected prims
if (len(tempList1) > 4):
conectedToPrim1 = GeoMath.getConnectedPrims(
prim1, list(tempList1))
while (len(conectedToPrim1) > 0 and not stopSearch):
numPrim2 = random.randint(0,
len(conectedToPrim1) - 1)
prim2 = conectedToPrim1[numPrim2]
if (prim2 != prim1):
# If prim2 has at least 2 conected prims
if (len(
GeoMath.getConnectedPrims(
prim2, list(tempList1), 2)) >= 2):
stopSearch = True
if (volume):
rs_lP_fP = False
for edge in GeoMath.getEdgesBetweenPrims(
prim1, prim2):
logging.debug(
"Edge: %s", str(edge))
rs = RejectionSampling.RejectionSampling(
edge, volume)
rs.do()
point = rs.getValue()
if (point):
rs_lP_fP = True
break
if (not rs_lP_fP):
stopSearch = False
if (stopSearch):
# Assign the last evaluate because we have it now in a variable.
firstPrim = InfoPathPrim.InfoPathPrim(
prim2)
# Last prim sure has two adjacent primitives.
lastPrim = InfoPathPrim.InfoPathPrim(
prim1)
firstPrim.setiPoint(list(point))
lastPrim.setfPoint(list(point))
del conectedToPrim1[numPrim2]
if (firstPrim and lastPrim):
logging.debug(
"End method getExtremPrims, class DefPath. State: good")
else:
logging.debug(
"End method getExtremPrims, class DefPath. State: no extrem prims"
)
return firstPrim, lastPrim
def getExtremPrims(self, Ipoint, primOfIpoint, partDes, refPrim, notDes, volume=None):
'''
We can't ensure that the primitives have a posible path, but we ensure that last primitive
have at least 2 adjacent primitives and first primitive is connected with the last primitive
'''
logging.debug("Start method getExtremPrims, class DefPath")
firstPrim = None
lastPrim = None
if(primOfIpoint and Ipoint):
# NOT YET TOTALLY IMPLEMENTED
edge = GeoMath.getEdgeWithPointInPrim(primOfIpoint, Ipoint)
lastPrim = primOfIpoint
for prim in partDes:
if(prim != lastPrim):
sharedEdges = GeoMath.getSharedEdgesPrims(lastPrim, prim)
rs_lP_fP = False
if(volume):
for edge in sharedEdges:
rs = RejectionSampling.RejectionSampling(edge, volume)
rs.do()
point = rs.getValue()
if(point):
rs_lP_fP = True
break
if (len(sharedEdges >= 1) and (edge in sharedEdges) and (volume == None or rs_lP_fP)):
firstPrim = prim
else:
# Automatically decision of extrem prims.
# Ensure that 2 prims is connected to another primitive in
# group of partially destroyed.
# Didn't use "getConnectedPrims" because need ramdonless in choice of prims.
stopSearch = False
tempList1 = list(partDes)
# minimum of 4 prims to get a path
while (len(tempList1) > 4 and not stopSearch):
numPrim1 = random.randint(0, len(tempList1) - 1)
prim1 = tempList1[numPrim1]
del tempList1[numPrim1]
# We have to ensure that first prim has at least two conected prims
if(True): # prim1.number()>17 and prim1.number()<27
while((len(GeoMath.getConnectedPrims(prim1, list(partDes), 2)) < 2) and (len(tempList1) > 4)):
numPrim1 = random.randint(0, len(tempList1) - 1)
prim1 = tempList1[numPrim1]
del tempList1[numPrim1]
# If prim1 has at least two conected prims
if(len(tempList1) > 4):
conectedToPrim1 = GeoMath.getConnectedPrims(prim1, list(tempList1))
while (len(conectedToPrim1) > 0 and not stopSearch):
numPrim2 = random.randint(0, len(conectedToPrim1) - 1)
prim2 = conectedToPrim1[numPrim2]
if(prim2 != prim1):
# If prim2 has at least 2 conected prims
if(len(GeoMath.getConnectedPrims(prim2, list(tempList1), 2)) >= 2):
stopSearch = True
if(volume):
rs_lP_fP = False
for edge in GeoMath.getEdgesBetweenPrims(prim1, prim2):
logging.debug("Edge: %s", str(edge))
rs = RejectionSampling.RejectionSampling(edge, volume)
rs.do()
point = rs.getValue()
if(point):
rs_lP_fP = True
break
if(not rs_lP_fP):
stopSearch = False
if(stopSearch):
# Assign the last evaluate because we have it now in a variable.
firstPrim = InfoPathPrim.InfoPathPrim(prim2)
# Last prim sure has two adjacent primitives.
lastPrim = InfoPathPrim.InfoPathPrim(prim1)
firstPrim.setiPoint(list(point))
lastPrim.setfPoint(list(point))
del conectedToPrim1[numPrim2]
if(firstPrim and lastPrim):
logging.debug("End method getExtremPrims, class DefPath. State: good")
else:
logging.debug("End method getExtremPrims, class DefPath. State: no extrem prims")
return firstPrim, lastPrim
def backTracking(self, curPrim, path):
global TimeExecutionFirst
global TimeExecutionCurrent
global MAXTIMEFORONEPATH
global DEBUG
logging.debug("Start method backTracking, class PathBackTracking")
logging.debug("Current prim from parm: %s", str(curPrim.prim.number()))
conPrims = GeoMath.getConnectedInfoPrims(curPrim, self.partDes)
indexPrims = 0
pathAchieved = False
startPoint = None
max_iterations_exceeded = False
while (not pathAchieved and indexPrims < len(conPrims) and not max_iterations_exceeded):
logging.debug("Current iteration: " + str(self.currentIteration))
self.currentIteration += 1
nextPrim = conPrims[indexPrims]
#Now, choose the best prim reference
refPrim = self.getBestPrimReference(curPrim)
logging.debug("Current prim: %s. Next prim: %s", str(curPrim.prim.number()), str(nextPrim.prim.number()))
logging.debug("Conected prims: %s. Count: %s", str([p.prim.number() for p in conPrims]), str(indexPrims))
logging.debug("Reference prim: %s", str(refPrim.prim.number()))
if(nextPrim not in path):
if(self.volume):
edges = GeoMath.getEdgesBetweenPrims(curPrim.prim, nextPrim.prim)
for edge in edges:
rs = RejectionSampling.RejectionSampling(edge, self.volume)
rs.do()
startPoint = rs.getValue()
if(startPoint):
break
logging.debug("Inicial point: %s", str(startPoint))
if(startPoint):
angleMin, angleMax = GeoMath.getMinMaxAngleBetweenPointsInPrim(curPrim.prim, nextPrim.prim, refPrim.prim)
logging.debug("Current prim: %s. Next prim: s", str(curPrim.prim.number()), str(nextPrim.prim.number()))
logging.debug("Min angle: %s. Max angle: %s", str(angleMin), str(angleMax))
if(self.clockWise and (angleMin > 0 or angleMin < -(math.pi - math.pi * 0.1))):
logging.debug("ignorada por clockwise y revolverse")
if(not self.clockWise and (angleMax < 0 and angleMax < (math.pi - math.pi * 0.1))):
logging.debug("ignorada por not clockwise y revolverse")
if(nextPrim == self.lastPrim and curPrim.sumAngle < (1.4 * math.pi)):
logging.debug("ignorada por ultima y angulo no suficiente")
if((nextPrim == self.lastPrim and curPrim.sumAngle > (1.4 * math.pi))):
logging.debug("aceptada por ultima y angulo suficiente")
if((not((self.clockWise and (angleMin > 0 or angleMin < -(math.pi - math.pi * 0.01))) or \
(not self.clockWise and (angleMax < 0 or angleMax > (math.pi - math.pi * 0.01))) or \
(nextPrim == self.lastPrim and curPrim.sumAngle < (1.4 * math.pi))) or \
(nextPrim == self.lastPrim and curPrim.sumAngle > (1.4 * math.pi)))):
ch = CalculateHeuristic.CalculateHeuristic(curPrim, nextPrim, refPrim)
ch.do()
curPrim.next = nextPrim
curPrim.setfPoint(list(startPoint))
nextPrim.setiPoint(list(startPoint))
path.append(nextPrim)
logging.debug("Path: %s", str([p.number() for p in InfoPathPrim.convertListFromInfoPrimToPrim(path)]))
if(nextPrim == self.lastPrim):
#BASE CASE
logging.debug("Last prim achieved")
pathAchieved = True
if((self.currentIteration >= self.max_interations / 2) and not pathAchieved):
self.max_iterations_exceeded = True
logging.error('Max iterations, no path achieved in the maximum iterations')
#path.remove(nextPrim)
pathAchieved = False
if(not pathAchieved and not self.max_iterations_exceeded and self.backTracking(nextPrim, path)):
pathAchieved = True
elif (not pathAchieved and not self.max_iterations_exceeded):
path.remove(nextPrim)
logging.debug("Path: %s", str([p.number() for p in InfoPathPrim.convertListFromInfoPrimToPrim(path)]))
indexPrims += 1
if(pathAchieved):
logging.debug("End ireration of while, method backTracking, class PathBackTracking. State: good")
else:
logging.debug("End ireration of while, method backTracking, class PathBackTracking. State: no path achieved")
return pathAchieved
def do(self):
epsilon = 0.001
if (self.DEBUG):
print "REF PRIM"
print self.refPrim.prim.number()
print "########## START PATH ###############"
"""
Construct a path around refPrim with start prim "firstPrim" and goal prim "lastPrim"
if parameter minimum is true, que path is the minimum path, otherwise is the "maximum"
path (inverted heuristic, but not maximum path)
"""
count = 0
path = []
while (not path and count < 2):
count += 1
openList = []
closedList = []
connectedPrims = []
if (count == 1):
angleMin, angleMax = GeoMath.getMinMaxAngleBetweenPointsInPrim(
self.lastPrim.prim, self.firstPrim.prim, self.refPrim.prim)
clockWise = max(math.fabs(angleMin),
math.fabs(angleMax)) == math.fabs(angleMin)
else:
clockWise = not clockWise
if (self.DEBUG):
print "Angulo min max"
print angleMin, angleMax, clockWise
openList.append(self.firstPrim)
# Start A* search
while (len(openList) > 0 and (self.lastPrim not in closedList)):
# Get the node with more or less heuristic depending of parm minimum
if (self.minimum):
curPrim = openList[0]
del openList[0]
else:
curPrim = openList.pop()
# Switch the current prim to closest list
closedList.append(curPrim)
# Get connected primitives
connectedPrims = GeoMath.getConnectedInfoPrims(
curPrim, self.partDes)
if (self.DEBUG):
print "CLOSE PRIM"
print curPrim.prim.number()
print "CONNECTED PRIMS"
print[conp.prim.number() for conp in connectedPrims]
# Clean not possible primitives(because we are go around refPrim)
for index in range(len(connectedPrims)):
conPrim = connectedPrims[index]
# angleMin, angleMax = GeoMath.getMinMaxAngleBetweenPointsInPrim(curPrim.prim, conPrim.prim, refPrim)
angleMin = angleMax = GeoMath.angleBetweenPointsByPrim(
GeoMath.primBoundingBox(curPrim.prim).center(),
GeoMath.primBoundingBox(conPrim.prim).center(),
self.refPrim)
dot = GeoMath.vecDotProduct(self.refPrim.normal(),
conPrim.prim.normal())
if (dot > 1 - epsilon):
# precision error
dot = 1
# math.acos(dot) > aperture
if (self.volume):
edges = GeoMath.getEdgesBetweenPrims(
curPrim.prim, curPrim.parent.prim)
for edge in edges:
rs = RejectionSampling.RejectionSampling(
edge, self.volume)
rs.do()
inicialPoint = rs.getValue()
if (inicialPoint):
break
if((not((math.acos(dot) > self.aperture) or \
(clockWise and (angleMin > 0 or angleMin < -(math.pi - math.pi * 0.1))) or \
(not clockWise and (angleMax < 0 or angleMax > (math.pi - math.pi * 0.1))) or \
(conPrim in closedList) or \
(conPrim == self.lastPrim and curPrim.sumAngle < (1.4 * math.pi))) or \
(conPrim == self.lastPrim and curPrim.sumAngle > (1.4 * math.pi))) and \
(inicialPoint or not self.volume)):
# If prim is already in openList
if (conPrim in openList):
heuristic = 1
if ((curPrim.G + heuristic > conPrim.G
and not self.minimum)
or (curPrim.G + heuristic < conPrim.G
and self.minimum)):
# If this path is better than the path with the current parent
conPrim.setParent(curPrim)
conPrim = self.calculateHeuristic(
curPrim, conPrim, self.refPrim)
if (self.volume):
conPrim.fPoint = list(inicialPoint)
curPrim.iPoint = list(inicialPoint)
if (self.DEBUG):
print "Prim aceptada y ya estaba en openlist"
print curPrim.prim.number(
), conPrim.prim.number()
else:
conPrim.setParent(curPrim)
conPrim = self.calculateHeuristic(
curPrim, conPrim, self.refPrim)
if (self.volume):
conPrim.fPoint = list(inicialPoint)
curPrim.iPoint = list(inicialPoint)
openList.append(conPrim)
if (self.DEBUG):
print "Prim aceptada y no estaba en openlist"
print curPrim.prim.number(
), conPrim.prim.number()
# Sort nodes by heuristic
openList.sort(key=lambda infoPrim: infoPrim.F)
if (self.lastPrim in closedList):
if (self.DEBUG):
print "Last prim Angle", self.lastPrim.sumAngle
curPrim = closedList.pop()
while (curPrim != self.firstPrim):
path.append(curPrim)
curPrim = curPrim.parent
path.append(curPrim)
else:
path = []
path.reverse()
print "########## FINALIZE PATH ###############"
self.path = path
def do(self):
epsilon = 0.001
if (self.DEBUG):
print "REF PRIM"
print self.refPrim.prim.number()
print "########## START PATH ###############"
"""
Construct a path around refPrim with start prim "firstPrim" and goal prim "lastPrim"
if parameter minimum is true, que path is the minimum path, otherwise is the "maximum"
path (inverted heuristic, but not maximum path)
"""
count = 0
path = []
while(not path and count < 2):
count += 1
openList = []
closedList = []
connectedPrims = []
if(count == 1):
angleMin, angleMax = GeoMath.getMinMaxAngleBetweenPointsInPrim(self.lastPrim.prim, self.firstPrim.prim, self.refPrim.prim)
clockWise = max(math.fabs(angleMin), math.fabs(angleMax)) == math.fabs(angleMin)
else:
clockWise = not clockWise
if(self.DEBUG):
print "Angulo min max"
print angleMin, angleMax, clockWise
openList.append(self.firstPrim)
# Start A* search
while(len(openList) > 0 and (self.lastPrim not in closedList)):
# Get the node with more or less heuristic depending of parm minimum
if(self.minimum):
curPrim = openList[0]
del openList[0]
else:
curPrim = openList.pop()
# Switch the current prim to closest list
closedList.append(curPrim)
# Get connected primitives
connectedPrims = GeoMath.getConnectedInfoPrims(curPrim, self.partDes)
if(self.DEBUG):
print "CLOSE PRIM"
print curPrim.prim.number()
print "CONNECTED PRIMS"
print [conp.prim.number() for conp in connectedPrims]
# Clean not possible primitives(because we are go around refPrim)
for index in range(len(connectedPrims)):
conPrim = connectedPrims[index]
# angleMin, angleMax = GeoMath.getMinMaxAngleBetweenPointsInPrim(curPrim.prim, conPrim.prim, refPrim)
angleMin = angleMax = GeoMath.angleBetweenPointsByPrim(GeoMath.primBoundingBox(curPrim.prim).center(), GeoMath.primBoundingBox(conPrim.prim).center(), self.refPrim)
dot = GeoMath.vecDotProduct(self.refPrim.normal(), conPrim.prim.normal())
if(dot > 1 - epsilon):
# precision error
dot = 1
# math.acos(dot) > aperture
if(self.volume):
edges = GeoMath.getEdgesBetweenPrims(curPrim.prim, curPrim.parent.prim)
for edge in edges:
rs = RejectionSampling.RejectionSampling(edge, self.volume)
rs.do()
inicialPoint = rs.getValue()
if(inicialPoint):
break
if((not((math.acos(dot) > self.aperture) or \
(clockWise and (angleMin > 0 or angleMin < -(math.pi - math.pi * 0.1))) or \
(not clockWise and (angleMax < 0 or angleMax > (math.pi - math.pi * 0.1))) or \
(conPrim in closedList) or \
(conPrim == self.lastPrim and curPrim.sumAngle < (1.4 * math.pi))) or \
(conPrim == self.lastPrim and curPrim.sumAngle > (1.4 * math.pi))) and \
(inicialPoint or not self.volume)):
# If prim is already in openList
if(conPrim in openList):
heuristic = 1
if((curPrim.G + heuristic > conPrim.G and not self.minimum) or
(curPrim.G + heuristic < conPrim.G and self.minimum)):
# If this path is better than the path with the current parent
conPrim.setParent(curPrim)
conPrim = self.calculateHeuristic(curPrim, conPrim, self.refPrim)
if(self.volume):
conPrim.fPoint = list(inicialPoint)
curPrim.iPoint = list(inicialPoint)
if(self.DEBUG):
print "Prim aceptada y ya estaba en openlist"
print curPrim.prim.number(), conPrim.prim.number()
else:
conPrim.setParent(curPrim)
conPrim = self.calculateHeuristic(curPrim, conPrim, self.refPrim)
if(self.volume):
conPrim.fPoint = list(inicialPoint)
curPrim.iPoint = list(inicialPoint)
openList.append(conPrim)
if(self.DEBUG):
print "Prim aceptada y no estaba en openlist"
print curPrim.prim.number(), conPrim.prim.number()
# Sort nodes by heuristic
openList.sort(key=lambda infoPrim: infoPrim.F)
if(self.lastPrim in closedList):
if(self.DEBUG):
print "Last prim Angle", self.lastPrim.sumAngle
curPrim = closedList.pop()
while(curPrim != self.firstPrim):
path.append(curPrim)
curPrim = curPrim.parent
path.append(curPrim)
else:
path = []
path.reverse()
print "########## FINALIZE PATH ###############"
self.path = path
