class LinePathSegment(PathSegment):
def __init__(self, startTime, startPoint, startSpeed, elapsedTime, endPoint, endSpeed, endUnitVelocity, nextLegalRotDirection):
PathSegment.__init__(self, startTime, elapsedTime, endPoint, endSpeed, endUnitVelocity, nextLegalRotDirection)
self.startPoint = startPoint
self.startSpeed = startSpeed
self.lineSegment = LineSegment(startPoint, endPoint)
def draw(self, visualizer, color=DEFAULT_COLOR, filtered=False, width=DEFAULT_WIDTH, **kwargs):
if filtered:
dash = DEFAULT_DASH
else:
dash = None
draw.drawLine(visualizer, self.startPoint, self.endPoint, color=color, arrow=tk.LAST, dash=dash, width=width)
def calcPointDebug(self, point):
timeInterp = self.lineSegment.closestPointParametric(point)
if timeInterp > 1.0:
timeInterp = 1.0
elif timeInterp < 0.0:
timeInterp = 0.0
closestPoint = self.lineSegment.getParametricPoint(timeInterp)
distance = np.linalg.norm(point - closestPoint)
return (closestPoint, distance, self.startTime + timeInterp * self.elapsedTime)
def testIntersection(self, pathIntersectionDetector):
return pathIntersectionDetector.testStraightPathIntersection(self.startTime, self.startPoint, self.endSpeed * self.endUnitVelocity, self.elapsedTime)
def onMotion(self, point, control=False):
if control:
if len(self._points) > 0:
self._previewPoint = self._points[0]
else:
self._previewPoint = point
if len(self._points) > 0:
self._previewLine = LineSegment(self._points[-1], self._previewPoint)
def onKey(self, point, key, ctrl=False):
if key == "Delete":
if len(self._lines) > 0:
self._lines = self._lines[:-1]
if len(self._points) > 0:
self._points = self._points[:-1]
self._previewPoint = point
if len(self._points) > 0:
self._previewLine = LineSegment(self._points[-1], self._previewPoint)
else:
self._previewLine = None
def __init__(self, points, velocity):
"""
A polygon NFZ with a given velocity.
:param points:
:param velocity:
"""
self.points = np.array(points, np.double)
self.velocity = np.array(velocity, np.double)
self._midPoint = self.points.sum(axis=0) / len(self.points)
self._lines = []
for i in range(0, len(points)):
self._lines.append(LineSegment(self.points[i - 1], self.points[i]))
def __init__(self, startTime, startPoint, startSpeed, elapsedTime, endPoint, endSpeed, endUnitVelocity, nextLegalRotDirection):
PathSegment.__init__(self, startTime, elapsedTime, endPoint, endSpeed, endUnitVelocity, nextLegalRotDirection)
self.startPoint = startPoint
self.startSpeed = startSpeed
self.lineSegment = LineSegment(startPoint, endPoint)
def linePointDebug(self, point):
lineSegment = LineSegment(self.lineStartPoint, self.endPoint)
timeInterp = lineSegment.closestPointParametric(point)
closestPoint = lineSegment.getParametricPoint(timeInterp)
distance = np.linalg.norm(point - closestPoint)
return (closestPoint, distance, timeInterp)
class PolyBuilder(SubGUI):
def __init__(self):
self._points = []
self._lines = []
self._previewPoint = None
self._previewLine = None
def onLeftRelease(self, point, control=False):
if control is False:
self._add(point)
else:
self._close()
def onMotion(self, point, control=False):
if control:
if len(self._points) > 0:
self._previewPoint = self._points[0]
else:
self._previewPoint = point
if len(self._points) > 0:
self._previewLine = LineSegment(self._points[-1], self._previewPoint)
def onKey(self, point, key, ctrl=False):
if key == "Delete":
if len(self._lines) > 0:
self._lines = self._lines[:-1]
if len(self._points) > 0:
self._points = self._points[:-1]
self._previewPoint = point
if len(self._points) > 0:
self._previewLine = LineSegment(self._points[-1], self._previewPoint)
else:
self._previewLine = None
def _add(self, point):
if len(self._points) > 0 and np.array_equal(point, self._points[-1]):
return
self._points.append(point)
if len(self._points) > 1:
self._lines.append(LineSegment(self._points[-2], self._points[-1]))
def _close(self):
if len(self._points) < 3:
return None
self._polyBuilt(self._points)
self._points = [] # We gave the points object away so don't delete it
del self._lines[:]
self._previewPoint = None
self._previewLine = None
def _polyBuilt(self, points):
pass
def draw(self, visualizer, color=DEFAULT_COLOR, **kwargs):
SubGUI.draw(self, visualizer, **kwargs)
for point in self._points:
gui.draw.drawPoint(visualizer, point, color=color)
for line in self._lines:
line.draw(visualizer, color=color)
if not self._previewPoint is None:
gui.draw.drawPoint(visualizer, self._previewPoint, color=color)
if not self._previewLine is None:
self._previewLine.draw(visualizer, color=color)
def _add(self, point):
if len(self._points) > 0 and np.array_equal(point, self._points[-1]):
return
self._points.append(point)
if len(self._points) > 1:
self._lines.append(LineSegment(self._points[-2], self._points[-1]))
def __init__(self, p1, p2, velocity):
LineSegment.__init__(self, p1, p2)
self.velocity = np.array(velocity, np.double)