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 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