def do(self):
#Start pathfinding with backtracking
logging.debug("Start method do, class PathBackTracking")
logging.debug("Group partially destroyed: %s", str([p.prim.number() for p in self.partDes]))
logging.debug("Group totally destroyed: %s", str([p.prim.number() for p in self.totDes]))
global TimeExecutionFirst
global TimeExecutionCurrent
global MAXTIMEFORALLPATHS
count = 1
pathAchieved = False
TimeExecutionFirst = time.time()
while(not pathAchieved and count < 2):
self.max_iterations_exceeded = False
path = []
pathAchieved = False
if(count == 1):
refPrim = self.getBestPrimReference(self.firstPrim)
angle = GeoMath.angleBetweenPointsByPrim(GeoMath.primBoundingBox(self.lastPrim.prim).center(), GeoMath.primBoundingBox(self.firstPrim.prim).center(), refPrim.prim)
logging.debug("Main angle, which determine direction: %s", str(angle))
self.clockWise = angle < 0
else:
self.clockWise = not self.clockWise
path.append(self.firstPrim)
pathAchieved = self.backTracking(self.firstPrim, path)
count += 1
self.path = path
logging.debug("Last prim: %s, First prim: %s", str(self.lastPrim.prim.number()), str(self.firstPrim.prim.number()))
if(pathAchieved):
self.goodPath = True
logging.debug("End method do, class PathBackTracking. State: good")
else:
self.goodPath = False
logging.debug("End method do, class PathBackTracking. State: No path achieved")
def do(self):
angleToSum = abs(GeoMath.angleBetweenPointsByPrim(GeoMath.primBoundingBox(self.curPrim.prim).center(), \
GeoMath.primBoundingBox(self.nextPrim.prim).center(), self.refPrim.prim))
heuristic = self.curPrim.F + 1
angle = self.curPrim.sumAngle + angleToSum
logging.debug("Heuristic for prim %s with next prim %s and ref prim %s, angle to sum: %s", str(self.curPrim.prim.number()), str(self.nextPrim.prim.number()), str(self.refPrim.prim.number()), str(angle))
self.nextPrim.setHeuristic(heuristic, 0)
self.nextPrim.setSumAngle(angle)
return self.nextPrim
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