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)