def __init__(self, adjLst, startPt, goalPt):
self.adjLst = adjLst
closest = adjLst[0].selfInd
closestG = adjLst[0].selfInd
for i in adjLst:
dist = getDistance(startPt, i.getCenter())
distG = getDistance(goalPt, i.getCenter())
if dist < getDistance(adjLst[closest].getCenter(), startPt):
closest = i.selfInd
if distG < getDistance(adjLst[closestG].getCenter(), goalPt):
closestG = i.selfInd
self.start = adjLst[closest]
self.start.g = 0
self.start.f = 0
self.goal = adjLst[closestG]
self.open = PriorityQueue()
self.open.put(self.start, 0)
self.closed = dict()
self.closed[str(self.start.selfInd)] = self.start
self.curr = self.start
self.bestPath = None
self.bestPathDist = 10000
def getWidthThrough(self, tri1, tri2):
"""Returns the width of tri2 when crossed from tr1 to tri2 on to tri2's parent (if it has a parent)."""
# the following gives the edge on the 1st triangle that the 2nd passed triangle lies on
edgeIn = getSharedEdgeStr(tri2, tri1)
if tri2.par is not None:
edgeOut = getSharedEdgeStr(tri2, self.adjLst[tri2.par])
else:
# TODO: take the least width between the two possible exit edges
# print "parent == None"
if edgeIn == '12':
return getDistance(tri2.getPoint1(), tri2.getPoint2())
elif edgeIn == '23':
return getDistance(tri2.getPoint2(), tri2.getPoint3())
else:
return getDistance(tri2.getPoint1(), tri2.getPoint3())
# print "parent does NOT equal None"
# 12 always comes first. 13 always comes last. Possibilities are 12, 23, 13 mutually exclusive (not duplicates)
crossedEdges = ''
if edgeIn == '12':
crossedEdges = edgeIn + edgeOut
elif edgeOut == '12':
crossedEdges = edgeOut + edgeIn
elif edgeIn == '13':
crossedEdges = edgeOut + edgeIn
elif edgeOut == '13':
crossedEdges = edgeIn + edgeOut
else:
msg = "getWidthThrough defaulted edge match-up edgeIn: " + edgeIn + " edgeOut: " + edgeOut
raise StandardError(msg)
# crossedEdges = 'w' + crossedEdges
#print crossedEdges + " ===================== crossedEdges"
if crossedEdges == '1223':
return tri2.w1223
elif crossedEdges == '2313':
return tri2.w2313
elif crossedEdges == '1213':
return tri2.w1213
else:
msg = "getWidthThrough defaulted return value crossedEdges: " + crossedEdges
raise StandardError(msg)
def getWidthThrough(self, tri1, tri2):
"""Returns the width of tri2 when crossed from tr1 to tri2 on to tri2's parent (if it has a parent)."""
# the following gives the edge on the 1st triangle that the 2nd passed triangle lies on
edgeIn = getSharedEdgeStr(tri2, tri1)
if tri2.par is not None:
edgeOut = getSharedEdgeStr(tri2, self.adjLst[tri2.par])
else:
# TODO: take the least width between the two possible exit edges
# print "parent == None"
if edgeIn == '12':
return getDistance(tri2.getPoint1(), tri2.getPoint2())
elif edgeIn == '23':
return getDistance(tri2.getPoint2(), tri2.getPoint3())
else:
return getDistance(tri2.getPoint1(), tri2.getPoint3())
# print "parent does NOT equal None"
# 12 always comes first. 13 always comes last. Possibilities are 12, 23, 13 mutually exclusive (not duplicates)
crossedEdges = ''
if edgeIn == '12':
crossedEdges = edgeIn + edgeOut
elif edgeOut == '12':
crossedEdges = edgeOut + edgeIn
elif edgeIn == '13':
crossedEdges = edgeOut + edgeIn
elif edgeOut == '13':
crossedEdges = edgeIn + edgeOut
else:
msg = "getWidthThrough defaulted edge match-up edgeIn: " + edgeIn + " edgeOut: " + edgeOut
raise StandardError(msg)
# crossedEdges = 'w' + crossedEdges
#print crossedEdges + " ===================== crossedEdges"
if crossedEdges == '1223':
return tri2.w1223
elif crossedEdges == '2313':
return tri2.w2313
elif crossedEdges == '1213':
return tri2.w1213
else:
msg = "getWidthThrough defaulted return value crossedEdges: " + crossedEdges
raise StandardError(msg)
def calculateG(self, chldInd, h, n):
child = self.adjLst[chldInd]
# region Calculate g using makeChannel PROBLEMATIC!!!
# child = copyAdjLstElement(child)
# # child.par = n.selfInd
# g = 0
#
# # swap out the goal so that the funnel algorithm will calculate based on this temporary goal
# print " child goal ", child
# tmpG = self.goal
# self.goal = child
# tmpGPt = self.goalPt
# self.goalPt = self.getNearestTrianglePtToStartOrGoal(child, target='start')
#
# path = self.makeChannel(child, n)
# # just for printing
# pt = self.getNearestTrianglePtToStartOrGoal(child, target='start')
# print "nearest point to start ", pt, " goalPt ", self.goalPt, " tmpGPt ", tmpGPt, "\npath", path
# for p in range(0, len(path) - 1):
# # The goal pt is the last point but we need to use a point in this triangle
# g += getDistance(path[p], path[p + 1])
# print "g counter ", g, " point p", path[p], " point p + 1", path[p + 1]
#
# self.goal = tmpG
# self.goalPt = tmpGPt
# return g
# endregion
# 1
shPts = n.getSharedPoints(child)
closeToSDist = getDistance(shPts[0], self.startPt)
if chldInd == self.start.selfInd:
closeToSDist = 0
else:
for p in shPts:
if closeToSDist > getDistance(p, self.startPt):
closeToSDist = getDistance(p, self.startPt)
# 2 ??? Why isn't this used
closeToGDist = getDistance(child.tri[0], self.goalPt)
for p in child.tri:
if closeToGDist > getDistance(p, self.goalPt):
closeToGDist = getDistance(p, self.goalPt)
startGoalH = getDistance(self.startPt, self.goalPt) - h
# 3
if child.par is not None:
parGdiffHH = self.adjLst[child.par].g + (self.adjLst[child.par].g - closeToGDist)
else:
parGdiffHH = 0
return max(closeToSDist, startGoalH, parGdiffHH)
def calculateG(self, chldInd, h, n):
child = self.adjLst[chldInd]
# region Calculate g using makeChannel PROBLEMATIC!!!
# child = copyAdjLstElement(child)
# # child.par = n.selfInd
# g = 0
#
# # swap out the goal so that the funnel algorithm will calculate based on this temporary goal
# print " child goal ", child
# tmpG = self.goal
# self.goal = child
# tmpGPt = self.goalPt
# self.goalPt = self.getNearestTrianglePtTo(child, target='start')
#
# path = self.makeChannel(child, n)
# # just for printing
# pt = self.getNearestTrianglePtTo(child, target='start')
# print "nearest point to start ", pt, " goalPt ", self.goalPt, " tmpGPt ", tmpGPt, "\npath", path
# for p in range(0, len(path) - 1):
# # The goal pt is the last point but we need to use a point in this triangle
# g += getDistance(path[p], path[p + 1])
# print "g counter ", g, " point p", path[p], " point p + 1", path[p + 1]
#
# self.goal = tmpG
# self.goalPt = tmpGPt
# return g
# endregion
# 1
shPts = n.getSharedPoints(child)
closeToSDist = getDistance(shPts[0], self.startPt)
if chldInd == self.start.selfInd:
closeToSDist = 0
else:
for p in shPts:
if closeToSDist > getDistance(p, self.startPt):
closeToSDist = getDistance(p, self.startPt)
# 2
closeToGDist = getDistance(child.tri[0], self.goalPt)
for p in child.tri:
if closeToGDist > getDistance(p, self.goalPt):
closeToGDist = getDistance(p, self.goalPt)
startGoalH = getDistance(self.startPt, self.goalPt) - h
# 3
if child.par is not None:
parGdiffHH = self.adjLst[child.par].g + (self.adjLst[child.par].g - closeToGDist)
else:
parGdiffHH = 0
return max(closeToSDist, startGoalH, parGdiffHH)
def getNearestTrianglePtTo(self, tri, target='goal'):
"""Gets the nearest point on the triangle to the target triangle."""
if target == 'goal':
# print "get nearest to goal"
target = self.goalPt
elif target == 'start':
# print "get nearest to start"
target = self.startPt
clst12 = getNearestPointOnLine(target, [tri.tri[0], tri.tri[1]], asLineSeg=True)
clst23 = getNearestPointOnLine(target, [tri.tri[1], tri.tri[2]], asLineSeg=True)
clst13 = getNearestPointOnLine(target, [tri.tri[0], tri.tri[2]], asLineSeg=True)
minDist = getDistance(target, clst12)
clstPt = clst12
# print "getNearestPt\n12 ", clst12, " 23 ", clst23, " 13 ", clst13
if minDist > getDistance(target, clst23):
minDist = getDistance(target, clst23)
clstPt = clst23
if minDist > getDistance(target, clst13):
clstPt = clst13
# print "clstPt ", clstPt
return clstPt
def getNearestTrianglePtToStartOrGoal(self, tri, target='goal'):
"""Gets the nearest point on the triangle to the target triangle."""
if target == 'goal':
# print "get nearest to goal"
target = self.goalPt
elif target == 'start':
# print "get nearest to start"
target = self.startPt
clst12 = getNearestPointOnLine(target, [tri.tri[0], tri.tri[1]], asLineSeg=True)
clst23 = getNearestPointOnLine(target, [tri.tri[1], tri.tri[2]], asLineSeg=True)
clst13 = getNearestPointOnLine(target, [tri.tri[0], tri.tri[2]], asLineSeg=True)
minDist = getDistance(target, clst12)
clstPt = clst12
# print "getNearestPt\n12 ", clst12, " 23 ", clst23, " 13 ", clst13
if minDist > getDistance(target, clst23):
minDist = getDistance(target, clst23)
clstPt = clst23
if minDist > getDistance(target, clst13):
clstPt = clst13
# print "clstPt ", clstPt
return clstPt
def getWidthAcrossEdges(self, searchTri, edge1, edge2):
"""Calculates the path width through this triangle. Edge1 and edge2 are the edges being crossed."""
# this calculates the distance from the point shared by edge1 and edge2 to the nearest obstacle
# 1st it sets the width of the triangle to the shortest edge being crossed
# then it searches across the third edge to see if there is an obstacle closer than its own vertices
# yes that can happen!!!
for p in edge1:
if p in edge2:
pt = p # get the point that both edges share. This is the point we are measuring the distance to.
if edge2 == searchTri.getEdge12() and searchTri.n12 is None\
or edge2 == searchTri.getEdge23() and searchTri.n23 is None\
or edge2 == searchTri.getEdge13() and searchTri.n13 is None:
# if edge2 is on a constrained side swap it for edge1
# doint this makes it so we only have to check edge1. It cuts our code for the next step in half.
tmp = edge2
edge2 = edge1
edge1 = tmp
# TODO make this work with edge 1, 2, & 3 and local vars nayb 1, 2, & 3 so it's not sooo much code
if edge1 == searchTri.getEdge12() and searchTri.n12 is None:
if edge2 == searchTri.getEdge23() and searchTri.n23 is None:
# Both search edges are constrained, so the width of the triangle is the width of the third edge.
return getDistance(searchTri.getPoint1(), searchTri.getPoint3())
elif edge2 == searchTri.getEdge13() and searchTri.n13 is None:
# ditto
return getDistance(searchTri.getPoint2(), searchTri.getPoint3())
else:
# the other edge is not constrained, so the initial width
# should be the shortest of either the length of this unconstrained edge
# or the distance from its non-shared point to the constrained side
if edge2 == searchTri.getEdge23():
minWidth = getDistance(searchTri.getPoint2(), searchTri.getPoint3())
# we also need to find the end point for the next step
if pt != edge2[0]:
otherPt = edge2[0]
else:
otherPt = edge2[1]
else: # the other (non-constrained) edge is 13
minWidth = getDistance(searchTri.getPoint1(), searchTri.getPoint3())
# we also need to find the end point for the next step
if pt != edge2[0]:
otherPt = edge2[0]
else:
otherPt = edge2[1]
# so now get the distance from the end to the other (constrained) edge
debugEdgeConstrained = edge1 ############################# DEBUG
distAcrossTri = getDistance(getNearestPointOnLine(otherPt, edge1),
otherPt)
if distAcrossTri < minWidth:
minWidth = distAcrossTri
# ########################### this next bit seems off
else:
minWidth = getDistance(searchTri.getPoint1(), searchTri.getPoint3())
elif edge1 == searchTri.getEdge13() and searchTri.n13 is None:
if edge2 == searchTri.getEdge23() and searchTri.n23 is None:
# both are constrained, so the width of the triangle is the length of the unconstrained edge
return getDistance(searchTri.getPoint1(), searchTri.getPoint2())
elif edge2 == searchTri.getEdge12() and searchTri.n12 is None:
return getDistance(searchTri.getPoint2(), searchTri.getPoint3())
else:
# the other edge is not constrained, so the initial width
# should be either the length of this unconstrained edge
# or the distance from its non-shared point to the constrained side, whichever is shortest
if edge2 == searchTri.getEdge23():
minWidth = getDistance(searchTri.getPoint2(), searchTri.getPoint3())
# we also need to find the end point for the next step
if pt != edge2[0]:
otherPt = edge2[0]
else:
otherPt = edge2[1]
else: # the other (non-constrained) edge is 12
minWidth = getDistance(searchTri.getPoint1(), searchTri.getPoint2())
# we also need to find the end point for the next step
if pt != edge2[0]:
otherPt = edge2[0]
else:
otherPt = edge2[1]
# so now get the distance from the end to the other (constrained) edge
debugEdgeConstrained = edge1 ############################# DEBUG
distAcrossTri = getDistance(getNearestPointOnLine(otherPt, edge1),
otherPt)
if distAcrossTri < minWidth:
minWidth = distAcrossTri
# ########################### this next bit seems off
else:
minWidth = getDistance(searchTri.getPoint1(), searchTri.getPoint3())
elif edge1 == searchTri.getEdge23() and searchTri.n23 is None:
if edge2 == searchTri.getEdge13() and searchTri.n13 is None:
# both are constrained, so the width of the triangle is the length of the unconstrained edge
return getDistance(searchTri.getPoint1(), searchTri.getPoint2())
elif edge2 == searchTri.getEdge12() and searchTri.n12 is None:
return getDistance(searchTri.getPoint1(), searchTri.getPoint2())
else:
# the other edge is not constrained, so the initial width
# should be either the length of this unconstrained edge
# or the distance from its non-shared point to the constrained side, whichever is shortest
if edge2 == searchTri.getEdge12():
minWidth = getDistance(searchTri.getPoint1(), searchTri.getPoint2())
# we also need to find the end point for the next step
if pt != edge2[0]:
otherPt = edge2[0]
else:
otherPt = edge2[1]
else: # the other (non-constrained) edge is 13
minWidth = getDistance(searchTri.getPoint1(), searchTri.getPoint3())
# we also need to find the end point for the next step
if pt != edge2[0]:
otherPt = edge2[0]
else:
otherPt = edge2[1]
# so now get the distance from the end to the other (constrained) edge
debugEdgeConstrained = edge1 ############################# DEBUG
distAcrossTri = getDistance(getNearestPointOnLine(otherPt, edge1),
otherPt)
if distAcrossTri < minWidth:
minWidth = distAcrossTri
# ########################### this next bit seems off
else:
minWidth = getDistance(searchTri.getPoint1(), searchTri.getPoint3())
else: # edge1 and edge2 are not constrained
# Get the width of the shortest of the these two edges
minWidth = min((edge1[0] - edge1[1]).length(), (edge2[0] - edge2[1]).length())
# if minWidth < 1:
# print "minWidth < 1 pt = ", pt, " || otherPt = ", otherPt, " || debugConstrained = ", debugEdgeConstrained
# save these so we don't consider them as nearest points later, else every triangle's width will be 0
edgePts = [edge1[0], edge1[1]]
edgePts.extend([edge2[0], edge2[1]])
# FINALLY search across the third edge for a constrained edge
# that's closer (to the shared point) than this triangle's vertices
# print self.adjLst
# ###################################################
counter = 0
# ###################################################
for t in range(0, len(self.adjLst)):
# if the edge is constrained, check to see if it narrows the width of this path
tri = self.adjLst[t]
# print tri
if tri.selfInd != searchTri.selfInd:
if tri.n12 is None:
# if the constrained edge, is on the opposite side
# from the point shared between the shared edges i.e. for point C check across edge c
nearest = getNearestPointOnLine(pt, [tri.tri[0], tri.tri[1]], True)
# print tri.selfInd, " 12 is none nearest", nearest
if isPointInWedge(nearest, edge1, edge2) and nearest not in edgePts:
# and it's in the wedge, check the distance against the current minimum width
newW = getDistance(pt, nearest)
# print "in wedge newW", newW
if newW < minWidth:
# ####################################################
from panda3d.core import LineSegs
# print "pt = ", pt, " || nearest = ", nearest
counter += 1
linesegs2 = LineSegs("lines" + str(counter))
linesegs2.setColor(0, 1, 1, 1)
linesegs2.setThickness(5)
linesegs2.drawTo(pt)
linesegs2.drawTo(nearest)
node2 = linesegs2.create(False)
nodePath = render.attachNewNode(node2)
nodePath.setZ(.25)
# ####################################################
minWidth = newW
# do likewise for the other edges
if tri.n23 is None:
nearest = getNearestPointOnLine(pt, [tri.tri[1], tri.tri[2]], True)
# print tri.selfInd, " 23 is none nearest", nearest
if isPointInWedge(nearest, edge1, edge2) and nearest not in edgePts:
newW = getDistance(pt, nearest)
# print "in wedge newW", newW
if newW < minWidth:
# ####################################################
from panda3d.core import LineSegs
# print "pt = ", pt, " || nearest = ", nearest
counter += 1
linesegs2 = LineSegs("lines" + str(counter))
linesegs2.setColor(0, 1, 1, 1)
linesegs2.setThickness(5)
linesegs2.drawTo(pt)
linesegs2.drawTo(nearest)
node2 = linesegs2.create(False)
nodePath = render.attachNewNode(node2)
nodePath.setZ(.25)
# ####################################################
minWidth = newW
if tri.n13 is None:
nearest = getNearestPointOnLine(pt, [tri.tri[0], tri.tri[2]], True)
# print tri.selfInd, " 13 is none nearest", nearest
if isPointInWedge(nearest, edge1, edge2) and nearest not in edgePts:
newW = getDistance(pt, nearest)
# print "in wedge newW", newW
if newW < minWidth:
# ####################################################
from panda3d.core import LineSegs
# print "pt = ", pt, " || nearest = ", nearest
counter += 1
linesegs2 = LineSegs("lines" + str(counter))
linesegs2.setColor(0, 1, 1, 1)
linesegs2.setThickness(5)
linesegs2.drawTo(pt)
linesegs2.drawTo(nearest)
node2 = linesegs2.create(False)
nodePath = render.attachNewNode(node2)
nodePath.setZ(.25)
# ####################################################
minWidth = newW
return minWidth
def AStar(self):
print "start AStar start: ", self.start.selfInd, " startPt ", self.startPt,\
" goal: ", self.goal.selfInd, " goalPt ", self.goalPt
bestPath = path = []
bestPathCost = 100000
if self.start == self.goal:
return [self.startPt, self.goalPt]
while self.open != []:
n = heapq.heappop(self.open)[1]
print "tri ind " + str(n.selfInd), " f: ", n.f
isFirst = True
bestF = 100000
bestInd = -1
# resolve ties in favor of best path
for chldInd in n.getNaybs():
if str(chldInd) in self.closed and n.selfInd != self.closed[str(chldInd)].par:
w = self.getWidthThrough(n, self.closed[str(chldInd)])
print "ind " + str(chldInd) + " is in closed." + " Width: " + str(w)
if isFirst and self.getWidthThrough(n, self.closed[str(chldInd)]) > 2*self.radius:
print "first and width good"
bestF = self.closed[str(chldInd)].f
bestInd = self.closed[str(chldInd)].selfInd
isFirst = False
elif self.closed[str(chldInd)].f < bestF\
and self.getWidthThrough(n, self.closed[str(chldInd)]) > 2*self.radius:
print "better f and width good"
bestF = self.closed[str(chldInd)].f
bestInd = self.closed[str(chldInd)].selfInd
if bestInd != -1: # we found a legal parent
print "parented to ", bestInd
n.par = bestInd
if n.f > bestPathCost:
print "ind " + str(n.selfInd), " n.f ", n.f, " bestPathCost ", bestPathCost
break
# once the nodes we're getting from open are costlier than our path, we've found the best path
if n == self.goal:# and self.goal.par is not None: # commented code is a reminder in case of another "bug"
print "################ FOUND GOAL ####################"
path = self.makeChannel(self.goal, self.closed[str(self.goal.par)])
cost = 0
for c in range(0, len(path) - 1):
cost += getDistance(path[c], path[c + 1])
if bestPathCost == -1: # this is the first path
bestPath = path
bestPathCost = cost
elif cost < bestPathCost:
bestPath = path
bestPathCost = cost
# put n in closed
self.closed[str(n.selfInd)] = n
# terminate algorithm
# if n == self.goal:
# break
for chldInd in n.getNaybs():
sChl = str(chldInd)
# print "child ", sChl, "++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++"
# get the width of the path through each side
if self.adjLst[chldInd].n12 is None:
w12 = getDistToLine(self.adjLst[chldInd].tri[2], self.adjLst[chldInd].tri[0], self.adjLst[chldInd].tri[1])
else:
w12 = self.getWidthAcrossEdges(self.adjLst[chldInd],
[self.adjLst[chldInd].tri[1], self.adjLst[chldInd].tri[2]],
[self.adjLst[chldInd].tri[0], self.adjLst[chldInd].tri[2]])
# print " w2313: ", w2313, "<<<<<<<<<<<<<<<<<<<<<<<<<<"
if self.adjLst[chldInd].n23 is None:
w23 = getDistToLine(self.adjLst[chldInd].tri[0], self.adjLst[chldInd].tri[1], self.adjLst[chldInd].tri[2])
else:
w23 = self.getWidthAcrossEdges(self.adjLst[chldInd],
[self.adjLst[chldInd].tri[0], self.adjLst[chldInd].tri[1]],
[self.adjLst[chldInd].tri[0], self.adjLst[chldInd].tri[2]])
# print "w2313: ", w2313, " w1213: ", w1213, "<<<<<<<<<<<<<<<<<<<<<<<<<<"
if self.adjLst[chldInd].n13 is None:
w13 = getDistToLine(self.adjLst[chldInd].tri[1], self.adjLst[chldInd].tri[0], self.adjLst[chldInd].tri[2])
else:
w13 = self.getWidthAcrossEdges(self.adjLst[chldInd],
[self.adjLst[chldInd].tri[0], self.adjLst[chldInd].tri[1]],
[self.adjLst[chldInd].tri[1], self.adjLst[chldInd].tri[2]])
# region Tampering with the width base on the degree of the triangle
# # if there are two unconstrained edges, then there's only one way through the triangle.
# # The width calculation misses the true width. So, this *SHOULD* fix it.
# constrainedEdges = 0
# if self.adjLst[chldInd].n12 is None:
# constrainedEdges += 1
# if self.adjLst[chldInd].n23 is None:
# constrainedEdges += 1
# if self.adjLst[chldInd].n13 is None:
# constrainedEdges += 1
#
# if constrainedEdges == 1:
# w12 = w23 = w13 = min(w12, w23, w13)
# elif constrainedEdges == 2:
# # if the triangle has only one unconstrained side, that side is the only width that can be crossed
# chl = self.adjLst[chldInd]
# if chl.n12 is not None:
# wOfUnconstrained = getDistance(chl.getPoint1(), chl.getPoint2())
# elif chl.n23 is not None:
# wOfUnconstrained = getDistance(chl.getPoint2(), chl.getPoint3())
# else:
# wOfUnconstrained = getDistance(chl.getPoint1(), chl.getPoint3())
#
# w12 = w23 = w13 = wOfUnconstrained
# endregion
print "n ind", n.selfInd, "chl ind " + sChl + " nw12: ", w12, " w1213: ", w23, " w1223: ", w13, "<<<<<<<<<<<<<<<<<<<<<<<<<<"
nrToG = self.getNearestTrianglePtTo(self.adjLst[chldInd])
h = getDistance(self.goalPt, nrToG)
# TODO: handle their MAX( g1, g2, g3,...) or leave it to my shortened version
g = self.calculateG(chldInd, h, n)
f = h + g
print "g: ", g, " h: ", h
if sChl not in self.closed:# or f < self.closed[sChl].f:
if self.getWidthThrough(self.adjLst[chldInd], n) <= 2*self.radius:
print "ignore child width <= r width ", self.getWidthThrough(self.adjLst[chldInd], n),\
" from ", self.adjLst[chldInd].selfInd, " to ", n.selfInd
continue
# print "put in closed and open"
# self.closed[sChl] = self.adjLst[chldInd]
# self.adjLst[chldInd].par = n.selfInd # double parenting!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
self.adjLst[chldInd].f = f
self.adjLst[chldInd].g = g
# 12 is the edge that was searched across. 2313 are the edges being traveled over by the seeker.
self.adjLst[chldInd].w2313 = w12
self.adjLst[chldInd].w1213 = w23
self.adjLst[chldInd].w1223 = w13
w = str(self.getWidthThrough(self.adjLst[chldInd], n))
print "ind " + sChl + " child width = ", w
print "put in open j = ", f
heapq.heappush(self.open, (f, self.adjLst[chldInd]))
elif sChl in self.closed and f < self.closed[sChl].f:# or chldInd == self.goal.selfInd: ## and self.getWidthThrough(self.closed[sChl], n) > 2*self.radius:
print "ind " + sChl + " in closed w/ better f. bestPathCost ", bestPathCost, " chldInd.f ", f
self.closed[sChl] = self.adjLst[chldInd]
self.closed[sChl].f = f
self.closed[sChl].g = g
# self.closed[sChl].w2313 = w12
# self.closed[sChl].w1213 = w23
# self.closed[sChl].w1223 = w13
heapq.heappush(self.open, (f, self.adjLst[chldInd]))
# print "end child ", self.adjLst[chldInd]
print "end AStar loop #####################################################################\n\n"
print "best path? ", bestPathCost, " f ", n.f
for i in range(0, len(self.open)):
opn = self.open[i][1]
print "open ind", opn.selfInd, "f", opn.f
# if the start and goal are not in the same triangle
# if self.goal.par is not None:
return bestPath
def getWidthAcrossEdges(self, searchTri, edge1, edge2):
"""Calculates the path width through this triangle. Edge1 and edge2 are the edges being crossed."""
# this calculates the distance from the point shared by edge1 and edge2 to the nearest obstacle
# 1st it sets the width of the triangle to the shortest edge being crossed
# then it searches across the third edge to see if there is an obstacle closer than its own vertices
# yes that can happen!!!
for p in edge1:
if p in edge2:
pt = p # get the point that both edges share. This is the point we are measuring the distance to.
if edge2 == searchTri.getEdge12() and searchTri.n12 is None\
or edge2 == searchTri.getEdge23() and searchTri.n23 is None\
or edge2 == searchTri.getEdge13() and searchTri.n13 is None:
# if edge2 is on a constrained side swap it for edge1
# doint this makes it so we only have to check edge1. It cuts our code for the next step in half.
tmp = edge2
edge2 = edge1
edge1 = tmp
# TODO make this work with edge 1, 2, & 3 and local vars nayb 1, 2, & 3 so it's not sooo much code
if edge1 == searchTri.getEdge12() and searchTri.n12 is None:
if edge2 == searchTri.getEdge23() and searchTri.n23 is None:
# Both search edges are constrained, so the width of the triangle is the width of the third edge.
return getDistance(searchTri.getPoint1(), searchTri.getPoint3())
elif edge2 == searchTri.getEdge13() and searchTri.n13 is None:
# ditto
return getDistance(searchTri.getPoint2(), searchTri.getPoint3())
else:
# the other edge is not constrained, so the initial width
# should be the shortest of either the length of this unconstrained edge
# or the distance from its non-shared point to the constrained side
if edge2 == searchTri.getEdge23():
minWidth = getDistance(searchTri.getPoint2(), searchTri.getPoint3())
# we also need to find the end point for the next step
if pt != edge2[0]:
otherPt = edge2[0]
else:
otherPt = edge2[1]
else: # the other (non-constrained) edge is 13
minWidth = getDistance(searchTri.getPoint1(), searchTri.getPoint3())
# we also need to find the end point for the next step
if pt != edge2[0]:
otherPt = edge2[0]
else:
otherPt = edge2[1]
# so now get the distance from the end to the other (constrained) edge
debugEdgeConstrained = edge1 ############################# DEBUG
distAcrossTri = getDistance(getNearestPointOnLine(otherPt, edge1),
otherPt)
if distAcrossTri < minWidth:
minWidth = distAcrossTri
# ########################### this next bit seems off
else:
minWidth = getDistance(searchTri.getPoint1(), searchTri.getPoint3())
elif edge1 == searchTri.getEdge13() and searchTri.n13 is None:
if edge2 == searchTri.getEdge23() and searchTri.n23 is None:
# both are constrained, so the width of the triangle is the length of the unconstrained edge
return getDistance(searchTri.getPoint1(), searchTri.getPoint2())
elif edge2 == searchTri.getEdge12() and searchTri.n12 is None:
return getDistance(searchTri.getPoint2(), searchTri.getPoint3())
else:
# the other edge is not constrained, so the initial width
# should be either the length of this unconstrained edge
# or the distance from its non-shared point to the constrained side, whichever is shortest
if edge2 == searchTri.getEdge23():
minWidth = getDistance(searchTri.getPoint2(), searchTri.getPoint3())
# we also need to find the end point for the next step
if pt != edge2[0]:
otherPt = edge2[0]
else:
otherPt = edge2[1]
else: # the other (non-constrained) edge is 12
minWidth = getDistance(searchTri.getPoint1(), searchTri.getPoint2())
# we also need to find the end point for the next step
if pt != edge2[0]:
otherPt = edge2[0]
else:
otherPt = edge2[1]
# so now get the distance from the end to the other (constrained) edge
debugEdgeConstrained = edge1 ############################# DEBUG
distAcrossTri = getDistance(getNearestPointOnLine(otherPt, edge1),
otherPt)
if distAcrossTri < minWidth:
minWidth = distAcrossTri
# ########################### this next bit seems off
else:
minWidth = getDistance(searchTri.getPoint1(), searchTri.getPoint3())
elif edge1 == searchTri.getEdge23() and searchTri.n23 is None:
if edge2 == searchTri.getEdge13() and searchTri.n13 is None:
# both are constrained, so the width of the triangle is the length of the unconstrained edge
return getDistance(searchTri.getPoint1(), searchTri.getPoint2())
elif edge2 == searchTri.getEdge12() and searchTri.n12 is None:
return getDistance(searchTri.getPoint1(), searchTri.getPoint2())
else:
# the other edge is not constrained, so the initial width
# should be either the length of this unconstrained edge
# or the distance from its non-shared point to the constrained side, whichever is shortest
if edge2 == searchTri.getEdge12():
minWidth = getDistance(searchTri.getPoint1(), searchTri.getPoint2())
# we also need to find the end point for the next step
if pt != edge2[0]:
otherPt = edge2[0]
else:
otherPt = edge2[1]
else: # the other (non-constrained) edge is 13
minWidth = getDistance(searchTri.getPoint1(), searchTri.getPoint3())
# we also need to find the end point for the next step
if pt != edge2[0]:
otherPt = edge2[0]
else:
otherPt = edge2[1]
# so now get the distance from the end to the other (constrained) edge
debugEdgeConstrained = edge1 ############################# DEBUG
distAcrossTri = getDistance(getNearestPointOnLine(otherPt, edge1),
otherPt)
if distAcrossTri < minWidth:
minWidth = distAcrossTri
# ########################### this next bit seems off
else:
minWidth = getDistance(searchTri.getPoint1(), searchTri.getPoint3())
else: # edge1 and edge2 are not constrained
# Get the width of the shortest of the these two edges
minWidth = min((edge1[0] - edge1[1]).length(), (edge2[0] - edge2[1]).length())
# if minWidth < 1:
# print "minWidth < 1 pt = ", pt, " || otherPt = ", otherPt, " || debugConstrained = ", debugEdgeConstrained
# save these so we don't consider them as nearest points later, else every triangle's width will be 0
edgePts = [edge1[0], edge1[1]]
edgePts.extend([edge2[0], edge2[1]])
# FINALLY search across the third edge for a constrained edge
# that's closer (to the shared point) than this triangle's vertices
# print self.adjLst
# ###################################################
counter = 0
# ###################################################
for t in range(0, len(self.adjLst)):
# if the edge is constrained, check to see if it narrows the width of this path
tri = self.adjLst[t]
# print tri
if tri.selfInd != searchTri.selfInd:
if tri.n12 is None:
# if the constrained edge, is on the opposite side
# from the point shared between the shared edges i.e. for point C check across edge c
nearest = getNearestPointOnLine(pt, [tri.tri[0], tri.tri[1]], True)
# print tri.selfInd, " 12 is none nearest", nearest
if isPointInWedge(nearest, edge1, edge2) and nearest not in edgePts:
# and it's in the wedge, check the distance against the current minimum width
newW = getDistance(pt, nearest)
# print "in wedge newW", newW
if newW < minWidth:
# ####################################################
from panda3d.core import LineSegs
# print "pt = ", pt, " || nearest = ", nearest
counter += 1
linesegs2 = LineSegs("lines" + str(counter))
linesegs2.setColor(0, 1, 1, 1)
linesegs2.setThickness(5)
linesegs2.drawTo(pt)
linesegs2.drawTo(nearest)
node2 = linesegs2.create(False)
nodePath = render.attachNewNode(node2)
nodePath.setZ(.25)
# ####################################################
minWidth = newW
# do likewise for the other edges
if tri.n23 is None:
nearest = getNearestPointOnLine(pt, [tri.tri[1], tri.tri[2]], True)
# print tri.selfInd, " 23 is none nearest", nearest
if isPointInWedge(nearest, edge1, edge2) and nearest not in edgePts:
newW = getDistance(pt, nearest)
# print "in wedge newW", newW
if newW < minWidth:
# ####################################################
from panda3d.core import LineSegs
# print "pt = ", pt, " || nearest = ", nearest
counter += 1
linesegs2 = LineSegs("lines" + str(counter))
linesegs2.setColor(0, 1, 1, 1)
linesegs2.setThickness(5)
linesegs2.drawTo(pt)
linesegs2.drawTo(nearest)
node2 = linesegs2.create(False)
nodePath = render.attachNewNode(node2)
nodePath.setZ(.25)
# ####################################################
minWidth = newW
if tri.n13 is None:
nearest = getNearestPointOnLine(pt, [tri.tri[0], tri.tri[2]], True)
# print tri.selfInd, " 13 is none nearest", nearest
if isPointInWedge(nearest, edge1, edge2) and nearest not in edgePts:
newW = getDistance(pt, nearest)
# print "in wedge newW", newW
if newW < minWidth:
# ####################################################
from panda3d.core import LineSegs
# print "pt = ", pt, " || nearest = ", nearest
counter += 1
linesegs2 = LineSegs("lines" + str(counter))
linesegs2.setColor(0, 1, 1, 1)
linesegs2.setThickness(5)
linesegs2.drawTo(pt)
linesegs2.drawTo(nearest)
node2 = linesegs2.create(False)
nodePath = render.attachNewNode(node2)
nodePath.setZ(.25)
# ####################################################
minWidth = newW
return minWidth
def AStar(self):
print "start AStar start: ", self.start.selfInd, " startPt ", self.startPt,\
" goal: ", self.goal.selfInd, " goalPt ", self.goalPt
bestPath = path = []
bestPathCost = 100000
if self.start == self.goal:
return [self.startPt, self.goalPt]
pathsVisited = []
while self.open != []:
n = heapq.heappop(self.open)[1] # open = [(]
print "tri ind " + str(n.selfInd), " f: ", n.f
isFirst = True
bestF = 100000
bestInd = -1
# resolve ties in favor of best path
for chldInd in n.getNaybs():
if str(chldInd) in self.closed and n.selfInd != self.closed[str(chldInd)].par:
print "#### find parent in closed ####"
# making the print work below
w = self.getWidthThrough(n, self.closed[str(chldInd)])
print "ind " + str(chldInd) + " is in closed." + " Width: " + str(w)
print "bestF = ", bestF, " f to check ", self.closed[str(chldInd)].f
if isFirst and self.getWidthThrough(n, self.closed[str(chldInd)]) > 2*self.radius:
# do not parent the goal to a path that has already been through he funnel
if chldInd not in pathsVisited:
print "first and width good"
bestF = self.closed[str(chldInd)].f
bestInd = self.closed[str(chldInd)].selfInd
isFirst = False
elif self.closed[str(chldInd)].f < bestF\
and self.getWidthThrough(n, self.closed[str(chldInd)]) > 2*self.radius:
# do not parent the goal to a path that has already been through he funnel
if chldInd not in pathsVisited:
print "better f and width good"
bestF = self.closed[str(chldInd)].f
bestInd = self.closed[str(chldInd)].selfInd
if bestInd != -1: # we found a legal parent
print "parented to ", bestInd
n.par = bestInd
# once the nodes we're getting from open are costlier than our path, we've found the best path
if n.f > bestPathCost:
print "break ind " + str(n.selfInd), " n.f ", n.f, " bestPathCost ", bestPathCost
break
if n == self.goal:
print "################ FOUND GOAL ####################"
path = self.makeChannel(self.goal, self.closed[str(self.goal.par)])
cost = 0
for c in range(0, len(path) - 1):
cost += getDistance(path[c], path[c + 1])
# keep track of what paths we've traversed, so they don't get re-traversed.
pathsVisited.append(self.goal.par)
# # Reset the goal so we can recalculate f, g, and h for other potential paths
self.goal.par = None
self.goal.g = 100000
self.goal.f = 100000
if bestPathCost == -1: # this is the first path
bestPath = path
bestPathCost = cost
elif cost < bestPathCost:
bestPath = path
bestPathCost = cost
if n != self.goal:
# put n in closed as long as it's not the goal
self.closed[str(n.selfInd)] = n
for chldInd in n.getNaybs():
print "#### EXPAND N ####"
# Never expand the goal. We cannot have one of the goal's children parented to the goal.
# That'd be backwards.
if n == self.goal:
break
sChl = str(chldInd)
# print "child ", sChl, "++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++"
# get the width of the path through each side
if self.adjLst[chldInd].n12 is None:
w12 = getDistToLine(self.adjLst[chldInd].tri[2], self.adjLst[chldInd].tri[0], self.adjLst[chldInd].tri[1])
else:
w12 = self.getWidthAcrossEdges(self.adjLst[chldInd],
[self.adjLst[chldInd].tri[1], self.adjLst[chldInd].tri[2]],
[self.adjLst[chldInd].tri[0], self.adjLst[chldInd].tri[2]])
# print " w2313: ", w2313, "<<<<<<<<<<<<<<<<<<<<<<<<<<"
if self.adjLst[chldInd].n23 is None:
w23 = getDistToLine(self.adjLst[chldInd].tri[0], self.adjLst[chldInd].tri[1], self.adjLst[chldInd].tri[2])
else:
w23 = self.getWidthAcrossEdges(self.adjLst[chldInd],
[self.adjLst[chldInd].tri[0], self.adjLst[chldInd].tri[1]],
[self.adjLst[chldInd].tri[0], self.adjLst[chldInd].tri[2]])
# print "w2313: ", w2313, " w1213: ", w1213, "<<<<<<<<<<<<<<<<<<<<<<<<<<"
if self.adjLst[chldInd].n13 is None:
w13 = getDistToLine(self.adjLst[chldInd].tri[1], self.adjLst[chldInd].tri[0], self.adjLst[chldInd].tri[2])
else:
w13 = self.getWidthAcrossEdges(self.adjLst[chldInd],
[self.adjLst[chldInd].tri[0], self.adjLst[chldInd].tri[1]],
[self.adjLst[chldInd].tri[1], self.adjLst[chldInd].tri[2]])
print "n ind", n.selfInd, "chl ind " + sChl + " nw12: ", w12, " w1213: ", w23, " w1223: ", w13, "<<<<<<<<<<<<<<<<<<<<<<<<<<"
nrToG = self.getNearestTrianglePtToStartOrGoal(self.adjLst[chldInd])
h = getDistance(self.goalPt, nrToG)
# TODO: handle their MAX( g1, g2, g3,...) OR don't and leave it to my shortened version
g = self.calculateG(chldInd, h, n)
f = h + g
print "g: ", g, " h: ", h
if sChl not in self.closed:# or f < self.closed[sChl].f:
if self.getWidthThrough(self.adjLst[chldInd], n) <= 2*self.radius:
print "ignore child width <= r width ", self.getWidthThrough(self.adjLst[chldInd], n),\
" from ", self.adjLst[chldInd].selfInd, " to ", n.selfInd
continue
self.adjLst[chldInd].f = f
self.adjLst[chldInd].g = g
self.adjLst[chldInd].w2313 = w12
self.adjLst[chldInd].w1213 = w23
self.adjLst[chldInd].w1223 = w13
w = str(self.getWidthThrough(self.adjLst[chldInd], n)) # make the print below work (print bug)
print "ind " + sChl + " child width = ", w
print "put in open f = ", f
heapq.heappush(self.open, (f, self.adjLst[chldInd]))
elif sChl in self.closed and f < self.closed[sChl].f:# or chldInd == self.goal.selfInd: ## and self.getWidthThrough(self.closed[sChl], n) > 2*self.radius:
print "ind " + sChl + " in closed w/ better f. bestPathCost ", bestPathCost, " chldInd.f ", f
self.closed[sChl] = self.adjLst[chldInd]
self.closed[sChl].f = f
self.closed[sChl].g = g
# self.closed[sChl].w2313 = w12
# self.closed[sChl].w1213 = w23
# self.closed[sChl].w1223 = w13
heapq.heappush(self.open, (f, self.adjLst[chldInd]))
# print "end child ", self.adjLst[chldInd]
print "end AStar loop #####################################################################\n\n"
print "best path? ", bestPathCost, " f ", n.f
for i in range(0, len(self.open)):
opn = self.open[i][1]
print "open ind", opn.selfInd, "f", opn.f
# if the start and goal are not in the same triangle
# if self.goal.par is not None:
return bestPath