class AABB:
""" Axis aligned bounding box """
def __init__(self):
self.reinit()
def reinit(self):
inf = 1e500
self.xmin = inf
self.xmax = -inf
self.ymin = inf
self.ymax = -inf
def addPoint(self, x, y):
if x < self.xmin:
self.xmin = x
if x > self.xmax:
self.xmax = x
if y < self.ymin:
self.ymin = y
if y > self.ymax:
self.ymax = y
def width(self):
return self.xmax - self.xmin
def height(self):
return self.ymax - self.ymin
def x(self):
return self.xmin
def y(self):
return self.ymin
def cx(self):
return (self.xmin + self.xmax) / 2.0
def cy(self):
return (self.ymin + self.ymax) / 2.0
def applyTransform(self, transform):
matrix = Transform().createMatrix(transform)
(x1, y1) = matrix.applyOnPoint(self.xmin, self.ymin)
(x2, y2) = matrix.applyOnPoint(self.xmax, self.ymax)
self.reinit()
self.addPoint(x1, y1)
self.addPoint(x2, y2)
def addBezier(self, (lastX, lastY), params):
# to acces values with simplepath format
(Cx1, Cy1, Cx2, Cy2, Cx, Cy) = range(-6, 0)
x1, y1 = params[Cx1], params[Cy1]
x2, y2 = params[Cx2], params[Cy2]
x, y = params[Cx], params[Cy]
bezVer = Bezier(
((lastX, lastY), (x1, y1), (x2, y2), (x, y))).splitCurve()
[self.addPoint(x, y) for x, y in bezVer]
def __init__(self, radius, position, intersect_link=None, parent=None):
pygame.sprite.Sprite.__init__(self)
if Circle.init:
Circle.bezier = Bezier()
Circle.intersection = Intersection()
Circle.init = False
self.transparency = True
# The Shape that this Shape is a child to
self.parent = parent
# Position of this graphic
self.position = position
# Circles defined by radius
self.radius = radius
# CPGroup, sprite group containing all control points for quick access
self.CPGroup = pygame.sprite.Group()
# CPDict, dict containing keyed control points for easy access to modify
self.CPDict = {}
sp = CPSprite(self.position + vec2d(self.radius, self.radius), self, label="move")
self.CPGroup.add(sp)
self.CPDict["move"] = sp
# Add CP for middle of shape to move it
sp = CPSprite(self.position + vec2d(self.radius * 2, self.radius), self, label="radius")
self.CPGroup.add(sp)
self.CPDict["radius"] = sp
# Set ilink property to this shape's intersection_link neighbour
self.ilink = intersect_link
# If this shape has an intersect_link, need to update intersection
self.update_intersect()
self.calc_rect()
self.update()
# open the guide file and extract the guide path
with open("at.svg") as f:
bytestring = f.read()
# interpret the svg as an XML document and remove the "guide" group
tree = lxml.etree.fromstring(bytestring)
viewBox = tree.attrib['viewBox']
dx, dy, w, h = map(float, viewBox.split())
print dx, dy
g = tree.xpath("//*[@id='guide']")[0]
guide = g[0].attrib['d']
points = Bezier.parse_svg(guide)
curve = Bezier(points)
tree.remove(g)
bytestring = lxml.etree.tostring(tree)
print_width_mm = 72
resolution_dpi = 180
width_pixels = int(72/25.4 * 180)
# set up the printer context
printer = win32print.GetDefaultPrinter()
print printer
phandle = win32print.OpenPrinter(printer)
dc = win32ui.CreateDC()
dc.CreatePrinterDC()
dc.StartDoc("foo")
parser.add_argument('--draw', default='quadratic', help='')
parser.add_argument('--batch', default=1, type=int, help='')
# parser.add_argument('--batches', nargs='*', default=[16], type=int, help='')
parser.add_argument('--passes', default=1, type=int, help='')
args = parser.parse_args()
use_cuda = not args.disable_cuda and torch.cuda.is_available()
# Make all tensors cuda tensors
# torch.set_default_tensor_type(torch.cuda.FloatTensor if use_cuda else torch.FloatTensor)
device = torch.device("cuda" if use_cuda else "cpu")
print('Using device "{}"'.format(device))
net = Bezier(res=args.res,
steps=args.steps,
method=args.method,
device=device,
debug=args.debug)
if args.draw == 'quadratic':
control_points_l = [[[0.1, 0.1], [0.9, 0.9], [0.5, 0.9]]]
elif args.draw == 'cubic':
control_points_l = [[[1.0, 0.0], [0.21, 0.12], [0.72, 0.83], [0.0, 1.0]]]
elif args.draw == 'composite':
control_points_l = [
[[0.1, 0.1], [0.9, 0.9], [0.5, 0.9]],
[[0.5, 0.9], [0.1, 0.9], [0.3, 0.3]],
[[0.3, 0.3], [0.9, 0.9], [0.9, 0.1]],
]
elif args.draw == 'char':
from xdrive import XDriveKinematics, EKSDRIVE, XDriveRenderer
from bezier import Bezier
from math import sqrt, pi
import numpy as np
import matplotlib.pyplot as plt
import cv2
path = Bezier([
np.array([0.0, 0.0]),
np.array([-1.26, 0.96]),
np.array([1.61, 4.21]),
np.array([-1.8, 1.98])
], 50)
path_reversed = Bezier([
np.array([-1.8, 1.98]),
np.array([1.61, 4.21]),
np.array([-1.26, 0.96]),
np.array([0.0, 0.0])
], 50)
# path = Bezier([np.array([0.0, 0.0]), np.array([4.0, 0.0])])
# path_reversed = Bezier([np.array([4.0, 0.0]), np.array([0.0, 0.0])])
canvas_size = np.array([800, 800])
m_to_px = 130
dt = 0.01 # 10ms
drive = EKSDRIVE(1.05, 0.35, False)
drive_renderer = XDriveRenderer(drive, m_to_px, np.array([30, 30]),
canvas_size, dt)
,
simulate=simulate_plot)
# capture all the specified control points
# they are relative to the plotter's starting point
specified_control_points = [plotter.initial_position]
for point in sys.argv[1].split(' '):
specified_control_points.append([
plotter.initial_position[0] + float(point.split(',')[0]),
plotter.initial_position[1] + float(point.split(',')[1])
])
if '--control' in sys.argv:
for point in specified_control_points:
if point == plotter.initial_position:
continue
plotter.move_to(point)
# back to start
plotter.move_to(plotter.initial_position)
b = Bezier(specified_control_points)
path = b.generate_path()
# plot curve points
for p in path:
plotter.move_to(p)
# wraps things up with the sim
plotter.finish()
def __init__(self, parent):
# Inizializza Antenato
QGLWidget.__init__(self, parent)
self._zoom_factor = 1.0
self._pan_x = 0
self._pan_y = 0
self.scene = Scene()
self._drawHidden = False
customer = Entity("Customer")
self.scene.add_entity(customer)
order = Entity("Order")
order.set_drawable(True)
self.scene.add_entity(order)
age = Attribute("Age")
age.setPrimaryKey(True)
order.add_attribute(age)
anchor = Anchor()
anchor.anchor_to_rectangle(order)
bezier = Bezier()
#self.scene.add_entity(bezier)
link1 = Link(anchor, age)
order.addLink(link1)
s0 = AutomataState("S0")
s1 = AutomataState("S1")
arc = AutomataArc(s0, s1)
arc2 = AutomataArc(s0, s0)
arc3 = AutomataArc(s1, s1)
self.scene.add_entity(arc)
self.scene.add_entity(arc2)
self.scene.add_entity(arc3)
self.scene.add_entity(s0)
self.scene.add_entity(s1)
self._mid_down_x = None
self._mid_down_y = None
self._mid_up_x = None
self._mid_up_y = None
self._left_down_x = None
self._left_down_y = None
self._left_up_x = None
self._left_up_y = None
self._last_mid_down_x = None
self._last_mid_down_y = None
self._tab_down = False
self._selected_object = None
# generate Map and obstacles
mapsize = 50 #Map size 50*50
freeGrid_num, obstacle, occupy_grid = mapGenerator(start, obstacle_type,
mapsize)
# Vehicle dtnamics
start_to_goal_distance = sqrt(
(start[0] - goal[0])**2 +
(start[1] - goal[1])**2) #distance from start point to goal point
# the vehicle state in the forward 1 second
theta, theta_forward, displacement_rear, displacement_rear_forward, steering_step = vehicle_model(
wheelbase, search_length=2.5, speed=5, dt=1)
# Bezier spline calculation
bezier = Bezier(start, goal, start_heading, goal_heading, start_steering,
mapsize, car_length, car_width, wheelbase, 3 * mapsize,
"Nofound")
bezier_spline = bezier.calculation()
#bezier.plot()
###################################################################################################################################################
# the main process
p = Planner(start, goal, start_heading, goal_heading, start_steering, mapsize,
freeGrid_num, tolerance, car_length, car_width, wheelbase,
obstacle_type)
path, path_tree = p.calculation()
# the trajectory points
x, y = np.vstack(path).T # 沿着竖直方向将矩阵堆叠起来
plt.figure(figsize=(10, 10))
# plot the obstacles
