def processReducedVisibilityGraph(debug=False) -> None:
"""
Note that reduced visibility graph is unidirectional. That is, there might be and edge v -> u but not the other way around
"""
for v in model.allVertexObjects:
for u in model.allVertexObjects:
if v.name == u.name and (v.name == "D1" or v.name == "D2"):
v.gaps.add(u)
continue
if v.loc == u.loc:
# v.gaps.add(u)
continue
# The below 2 edge cases rarely happen, but it happens when the robot or destination are exactly at a vertex of an obstacle
if _isRobotOrDestinationAndOnObstacleButNotAdjacent(v, u): continue
if _isRobotOrDestinationAndOnObstacleButNotAdjacent(u, v): continue
# If they belong to the same obstacle but are not adjacent, they aren't u is not visible
if v.ownerObs and u.ownerObs and v.ownerObs.name == u.ownerObs.name and u not in v.adjacentOnObstacle:
continue
if _isGap(v, u):
v.gaps.add(u)
if not debug: return
counter = 0
for v in model.allVertexObjects:
for u in v.gaps:
e = DebugEdge(
"%s=>%s" % (v.name, u.name),
[convertToPoint(v), convertToPoint(u)],
isDirected=True)
model.entities[e.name] = e
e.createShape(model.canvas)
counter += 1
print("Edges: %d" % counter)
def _addFirstTriangleToFunnel(self, mid, pt1, pt2):
if Geom.isToTheRight(self.apex(), mid, pt1):
self._funnelRight.append(convertToPoint(pt1))
self._funnelLeft.append(convertToPoint(pt2))
else:
self._funnelLeft.append(convertToPoint(pt1))
self._funnelRight.append(convertToPoint(pt2))
def getEpsilonVector(frm, to) -> Vector:
"""
Returns a Vector which represents an epsilon vector
"""
toPt = convertToPoint(to)
frmPt = convertToPoint(frm)
return getEpsilonVectorFromVect(toPt - frmPt)
def getPointOnLineSegment(v1, v2, frac) -> Point:
"""
given frac in [0,1] find the point that falls an frac of the distance from v1 to v2
"""
v1 = convertToPoint(v1)
v2 = convertToPoint(v2)
vect = v2 - v1
return v1 + (vect * frac)
def isUndoingLastMove(node, v, index):
if not node.parent: return False
if v.name == "D1" or v.name == "D2": return False
if convertToPoint(node.parent.cable[index]) != convertToPoint(v):
return False
path = node._getPath(index == 0)
path = removeRepeatedVertsOrdered(path)
if convertToPoint(node.parent.cable[-2]) != convertToPoint(v): return False
return True
def getLineSegAndRayIntersection(ls1, ls2, rayFrom, rayTo):
lsPt1 = convertToPoint(ls1)
lsPt2 = convertToPoint(ls2)
ryPt1 = convertToPoint(rayFrom)
ryPt2 = convertToPoint(rayTo)
l = Segment(lsPt1, lsPt2)
r = Ray(ryPt1, ryPt2)
pt = intersection(l, r)
pass
def preprocessTheCable(cable: VertList, destA: Vertex, destB: Vertex) -> tuple: """ If moves are happening along the current cable """ if convertToPoint(destB) == convertToPoint(cable[-2]) or Geom.isCollinear(cable[-2], cable[-1], destB): cable[-1] = destB if convertToPoint(destA) == convertToPoint(cable[1]) or Geom.isCollinear(cable[1], cable[0], destA): cable[0] = destA cable = removeRepeatedVertsOrdered(cable) return (cable, destA, destB)
def rightHandRuleCrossProduct(common, v1, v2):
"""
Calculate the cross product of common -> v1, common -> v2
"""
ptCommon = convertToPoint(common)
pt1 = convertToPoint(v1)
pt2 = convertToPoint(v2)
vec1 = pt1 - ptCommon
vec2 = pt2 - ptCommon
return crossProduct(vec1, vec2)
def pointAndLineDistance(pt, lPt1, lPt2):
"""https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_two_points"""
pt = convertToPoint(pt)
(x0, y0) = (pt.x(), pt.y())
lPt1 = convertToPoint(lPt1)
(x1, y1) = (lPt1.x(), lPt1.y())
lPt2 = convertToPoint(lPt2)
(x2, y2) = (lPt2.x(), lPt2.y())
d = fabs(((y2 - y1) * x0) - (
(x2 - x1) * y0) + x2 * y1 - y2 * x1) / vertexDistance(lPt1, lPt2)
return d
def doPops(cable, dest, isZeroEndMoving):
src = cable[0] if isZeroEndMoving else cable[-1]
movement = Segment(convertToPoint(src), convertToPoint(dest))
stitches = getPoppingStitchLines(cable, isZeroEndMoving)
deleteInd = 1 if isZeroEndMoving else -2
for stitch in stitches:
inter = intersection(movement, stitch)
if inter:
del cable[deleteInd]
else:
break
return cable
def getPoppingStitchLines(cable: list, isZeroEndMoving: bool) -> list:
rays = []
start = len(cable) - 1 if isZeroEndMoving else 0
stop = 1 if isZeroEndMoving else len(cable) - 2
step = -1 if isZeroEndMoving else 1
for i in range(start, stop, step):
pt1 = convertToPoint(cable[i])
pt2 = convertToPoint(cable[i + step])
direction = pt2 - pt1
r = Ray(pt2, direction)
rays.append(r)
rays.reverse()
return rays
def pushCableAwayFromObstacles(cable: VertList, destA: Vertex, destB: Vertex) -> tuple:
transformed = [destA] + cable + [destB]
for i in range(len(transformed)):
c = transformed[i]
for o in model.obstacles:
for v in o.vertices:
if convertToPoint(v) != convertToPoint(c): continue
vects = [Geom.getEpsilonVector(n, v) for n in v.adjacentOnObstacle]
if len(vects) != 2: raise RuntimeError("There should only be 2 adjacent vertices to any vertex")
epsVect = Geom.getEpsilonVectorFromVect(vects[0] + vects[1])
pushedC = convertToPoint(c) + epsVect
transformed[i] = pushedC
return (transformed[1:-1], transformed[0], transformed[-1])
def _isFaceSurroundedByCable(self, face: TriangulationFaceHandle):
pts = [face.vertex(i).point() for i in range(3)]
for i in range(-1, 2):
isOnCable = False
longCable = [self.dest1] + self.cable + [self.dest2]
for j in range(len(longCable) - 1):
if VertexUtils.convertToPoint(
longCable[j]) == pts[i] and VertexUtils.convertToPoint(
longCable[j + 1]) == pts[i + 1]:
isOnCable = True
break
if not isOnCable:
return False
return True
def _insertCableConstraints(self) -> None:
for i in range(len(self.cable) - 1):
(_, h1) = self._addPtToTriangulation(self.cable[i])
(_, h2) = self._addPtToTriangulation(self.cable[i + 1])
self.cgalTri.insert_constraint(h1, h2)
if VertexUtils.convertToPoint(self.src1) != VertexUtils.convertToPoint(
self.dest1):
(_, h1) = self._addPtToTriangulation(self.src1)
(_, h2) = self._addPtToTriangulation(self.dest1)
self.cgalTri.insert_constraint(h1, h2)
if VertexUtils.convertToPoint(self.src2) != VertexUtils.convertToPoint(
self.dest2):
(_, h1) = self._addPtToTriangulation(self.src2)
(_, h2) = self._addPtToTriangulation(self.dest2)
self.cgalTri.insert_constraint(h1, h2)
def getPointOnLineSegmentFraction(ls1, ls2, pt):
"""
Given a line segment (that has direction) from ls1 to ls2, what fraction of the line does pt fall on
"""
ls1 = convertToPoint(ls1)
ls2 = convertToPoint(ls2)
segment = Segment(ls1, ls2)
if not segment.has_on(pt): return nan
v1 = segment.to_vector()
pt = convertToPoint(pt)
v2 = pt - ls1
areInSameDirection = True if (v2 * v1) >= 0 else False
d1 = sqrt(v1.squared_length())
d2 = sqrt(v2.squared_length())
return d1 / d2 if areInSameDirection else -1 * (d1 / d2)
def _getOriginalBoundary(self) -> None:
extendedCable = [self.dest1] + self.cable + [self.dest2]
extendedCable = VertexUtils.removeRepeatedVertsOrdered(extendedCable)
self.boundaryPts = [
VertexUtils.convertToPoint(vert) for vert in extendedCable
]
self.originalPolygon = Polygon(self.boundaryPts)
def isThereCrossMovement(cable, dest1, dest2):
# I've also included the case where the polygon is not simple
cable = getLongCable(cable, dest1, dest2)
cable = removeRepeatedVertsOrdered(cable)
if len(cable) < 3: return False
p = Polygon([convertToPoint(v) for v in cable])
return not p.is_simple()
def _updateFunnelSide(self, candidate, isUpdatingLeft: bool):
# The terminology in this function is weird because this function is agnostic to which side of the funnel it is updating
candidate = convertToPoint(candidate)
mySideOfFunnel = self._funnelLeft if isUpdatingLeft else self._funnelRight
otherSideOfFunnel = self._funnelRight if isUpdatingLeft else self._funnelLeft
# If the point is already on the funnel side then skip it
if mySideOfFunnel[0] == candidate or mySideOfFunnel[-1] == candidate:
return (mySideOfFunnel, otherSideOfFunnel)
index = self._walkFunnelSide(candidate, mySideOfFunnel, isUpdatingLeft)
if index > 0:
mySideOfFunnel = mySideOfFunnel[:index]
mySideOfFunnel.append(candidate)
return (mySideOfFunnel, otherSideOfFunnel)
# At this point we are starting to walk the other side of the funnel
# check if the candidate should be placed at index 0
isStrictlyOutsideFunnel = Geom.isToTheRight if isUpdatingLeft else Geom.isToTheLeft
isStrictlyInsideFunnel = Geom.isToTheLeft if isUpdatingLeft else Geom.isToTheRight
pt1 = self._others[-1]
pt2 = otherSideOfFunnel[0]
if isStrictlyInsideFunnel(pt1, pt2, candidate):
return ([candidate], otherSideOfFunnel)
# At this point we are past the apex, so the apex will have to be changed
for i in range(len(otherSideOfFunnel) - 1):
pt1 = otherSideOfFunnel[i]
pt2 = otherSideOfFunnel[i + 1]
if isStrictlyInsideFunnel(pt1, pt2, candidate):
mySideOfFunnel = [candidate]
self._others = self._others + otherSideOfFunnel[:i + 1]
otherSideOfFunnel = otherSideOfFunnel[i + 1:]
return (mySideOfFunnel, otherSideOfFunnel)
mySideOfFunnel = [candidate]
self._others = self._others + otherSideOfFunnel
otherSideOfFunnel = [self._others[-1]]
return (mySideOfFunnel, otherSideOfFunnel)
def _getCoordinateList(verts):
coords = []
for v in verts:
pt = convertToPoint(v)
coord = (pt.x(), pt.y())
coords.append(coord)
return coords
def pushVertEpsilonInside(self, vert,
faceHandle: TriangulationFaceHandle) -> Point:
edges = self.getIncidentEdges(vert, faceHandle)
toBePushedPt = VertexUtils.convertToPoint(vert)
if not edges or len(edges) != 2:
raise RuntimeError("There must be 2 incident edges")
vects = []
for e in edges:
for v in e:
pt = VertexUtils.convertToPoint(v)
if pt != toBePushedPt:
vects.append(pt - toBePushedPt)
break
summed = vects[0] + vects[1]
epsilon = Geom.getEpsilonVectorFromVect(summed)
return VertexUtils.convertToPoint(vert) + epsilon
def getAllIntersectingObstacles(vertices):
"""
Given the vertices of a polygon, get all the Obstacles that fall intersect (fully or partially) the polygon
Params
===
vertices: `Vertex[]`
:returns: A tuple of two arrays, First array are fully inside, second array is partially inside
"""
result = ([], [])
poly = Polygon([convertToPoint(v) for v in vertices])
for obs in model.obstacles:
isIn = False
isOut = False
for pt in obs.vertices:
# if isInsidePoly(poly, pt) or isOnPoly(poly, pt):
if isInsidePoly(poly, pt):
isIn = True
else:
isOut = True
if isIn:
if not isOut:
result[0].append(obs)
else:
result[1].append(obs)
return result
def getShortestPath(self, dest):
# If sleeve is a single triangle, the path is from apex to destination
if len(self.sleeve) == 1: return self._others + [convertToPoint(dest)]
mid = Geom.midpoint(self._funnelLeft[0], self._funnelRight[0])
shouldGoLeft = Geom.isToTheLeft(self.apex(), mid, dest)
if shouldGoLeft:
return self._findCandidatePath(dest, self._funnelLeft, True)
return self._findCandidatePath(dest, self._funnelRight, False)
def extrudeVertsWithEpsilonVect(verts) -> List[Point]:
centroidPt = centroid(verts)
extruded = []
for v in verts:
pt = convertToPoint(v)
vec = getEpsilonVector(centroidPt, pt)
extruded.append(pt + vec)
return extruded
def __init__(self, src: Point, triangulation: Triangulation, sleeve: list):
self.tri = triangulation
self.src = self.tri.pushVertEpsilonInside(src, sleeve[0])
self.sleeve = sleeve
self._funnelLeft = []
self._funnelRight = []
self._others = [self.src]
self._build()
self._others[0] = convertToPoint(src)
def isVisible(v1, v2):
pt1 = convertToPoint(v1)
pt2 = convertToPoint(v2)
l = Segment(pt1, pt2)
for o in model.obstacles:
intersections = o.intersection(l)
# if vertices are visible, the intersection is either empty or it is a line segment
# whose at least one of the end points is one of the two vertices
if (len(intersections) == 0):
continue
# If obstacles are concave, we might have more than 2 intersections
for intersection in intersections:
if isinstance(intersection, Point):
if not (intersection == pt1 or intersection == pt2):
return False
else: # intersection is a Segment
# Segments show that an edge is tangent to the visibility ray
# They don't block visibility
pass
return True
def circleAndLineSegmentIntersection(pt1, pt2, center, radius):
"""
https://stackoverflow.com/a/30998492/750567
"""
linePts = _getCoordinateList([pt1, pt2])
seg = SHLineString(linePts)
cPt = convertToPoint(center)
circle = SHPoint(cPt.x(), cPt.y()).buffer(radius).boundary
SHintersection = circle.intersection(seg)
if not isinstance(SHintersection, SHPoint):
raise RuntimeError("Haven't debugged this yet")
return Point(SHintersection.x, SHintersection.y)
def doPushes(cable, dest, isZeroEndMoving):
src = cable[0] if isZeroEndMoving else cable[-1]
movement = Segment(convertToPoint(src), convertToPoint(dest))
base = cable[1] if isZeroEndMoving else cable[-2]
closestBase = None
closestDist = 1000000000
for v in model.vertices:
if v.loc == base.loc: continue
if _isGap(base, v):
basePt = convertToPoint(base)
vPt = convertToPoint(v)
stitch = Ray(vPt, vPt - basePt)
inter = intersection(movement, stitch)
if inter:
d = Geom.vertexDistance(src, inter)
if d < closestDist:
closestDist = d
closestBase = v
insertionIndex = -1 if isZeroEndMoving else 1
if closestDist:
cable.insert(insertionIndex, closestBase)
return doPushes(cable, dest, isZeroEndMoving)
else:
return cable
def __init__(self, name, pts: list, color):
"""
params
===
color: color string
name: str
pts: A list of at least 2 utils.cgal.types.Point
"""
super().__init__(color=color, name=name)
if isinstance(pts, list):
if len(pts) < 2:
raise RuntimeError("at least 2 Points are needed for a Path")
self.pts = [convertToPoint(v) for v in pts]
self._ptsCanvasIds = []
def __init__(self, name, pts: list, isOrigin=False):
"""
params
===
color: color string
name: str
pts: A list of at least 2 utils.cgal.types.Point
"""
super().__init__(
color=CABLE_ORIGIN_COLOR if isOrigin else CABLE_FINAL_COLOR,
name=name)
if isinstance(pts, list):
if len(pts) < 2:
raise RuntimeError("at least 2 Points are needed for a Cable")
self.pts = [convertToPoint(v) for v in pts]
def isPointInsideTriangle(self, faceHandle: TriangulationFaceHandle,
pt) -> bool:
'''
see [this](https://stackoverflow.com/a/2049593/750567)
'''
sign = lambda p1, p2, p3: (p1.x() - p3.x()) * (p2.y() - p3.y()) - (
p2.x() - p3.x()) * (p1.y() - p3.y())
pt = VertexUtils.convertToPoint(pt)
d1 = sign(pt,
faceHandle.vertex(1).point(),
faceHandle.vertex(2).point())
d2 = sign(pt,
faceHandle.vertex(2).point(),
faceHandle.vertex(3).point())
d3 = sign(pt,
faceHandle.vertex(3).point(),
faceHandle.vertex(1).point())
has_neg = (d1 < 0) or (d2 < 0) or (d3 < 0)
has_pos = (d1 > 0) or (d2 > 0) or (d3 > 0)
return not (has_neg and has_pos)
