def create_end_point(obstacles, l):
R = np.random.uniform(low=l/2, high=l)
A = np.random.uniform(low=0, high=np.pi)
while True:
P = Point(R, 0).rotation(A, Point(0, 0))
if point_in_obstacles(P, obstacles)== False:
return P
def main():
points = [Point(300, 100), Point(100, 300), Point(600, 100)]
poly = Polygon(points)
draw.polygon(poly.verticiesAsTuples, outline=foreground)
drawPoint(poly.centroid, size=10, fill='hsb(0,100%,100%)')
img.show()
def get_base_sections():
"""Wing planform sections."""
WING_AIRFOIL = "naca4412"
_sections = [
{
CHORD: 5.6,
LE_LOCATION: Point([0.0, 0.0, 0.0]),
TWIST: 0.0,
AIRFOIL: WING_AIRFOIL,
},
{
CHORD: 3.6,
LE_LOCATION: Point([2.34, 4.6, 0.2]),
TWIST: -2.0,
AIRFOIL: WING_AIRFOIL,
},
{
CHORD: 1.26,
LE_LOCATION: Point([5.5, 14.04, 0.61]),
TWIST: -5.0,
AIRFOIL: WING_AIRFOIL,
},
]
return _sections
def get_sections():
"""Wing planform sections."""
WING_AIRFOIL = "naca4412"
CHORD = WingSectionParameters.CHORD.value
LE_LOCATION = WingSectionParameters.LE_LOCATION.value
TWIST = WingSectionParameters.TWIST.value
AIRFOIL = WingSectionParameters.AIRFOIL.value
_sections = [
{
CHORD: 5.6,
LE_LOCATION: Point([0.0, 0.0, 0.0]),
TWIST: 0.0,
AIRFOIL: WING_AIRFOIL,
},
{
CHORD: 3.6,
LE_LOCATION: Point([2.34, 4.6, 0.2]),
TWIST: -2.0,
AIRFOIL: WING_AIRFOIL,
},
{
CHORD: 1.26,
LE_LOCATION: Point([5.5, 14.04, 0.61]),
TWIST: -5.0,
AIRFOIL: WING_AIRFOIL,
},
]
return _sections
def move(self, dt, gravity=Point(), drag=Point()):
'''
'''
prev = Point(self.position)
A = self.acceleration + gravity + drag
self.velocity += A * dt
self.position += self.velocity
return prev
def drawPoint(point, size=50, fill=foreground):
print(f'Point = {point}')
p1 = point + Point(0, size / 2)
p2 = point - Point(0, size / 2)
p3 = point + Point(size / 2, 0)
p4 = point - Point(size / 2, 0)
draw.line([p1.asTuple(), p2.asTuple()], fill=fill)
draw.line([p3.asTuple(), p4.asTuple()], fill=fill)
def create_layer_points(self, point_distance, layer_distance, layer_size,
layer_depth):
layer_points = []
p = Point(layer_depth * layer_distance,
float(layer_size - 1) / 2 * point_distance)
for j in range(layer_size):
layer_points.append(p)
p = p.translation(0, -point_distance)
return layer_points
def create_arm(angles, lengths):
alpha = np.pi/2
A = Point(0, 0)
lines = []
for i in range(len(angles)):
B = A.translation(lengths[i], 0)
alpha += angles[i]
B = B.rotation(alpha, A)
lines.append(Line(A, B))
A = B
return Polyline(lines)
def get_rooms_tiles(self):
room_area = {}
for x in range(map_width):
for y in range(map_height):
tile = self.map_[x][y]
if tile != -1:
if tile in room_area:
room_area[tile].append(Point(x, y))
else:
room_area[tile] = [Point(x, y)]
return room_area
def acceleration(self):
if self.throttle > 0:
f = self.maxAcceleration * (self.throttle / 100)
r = math.radians(360 - self.heading)
return Point(math.cos(r) * f, math.sin(r) * f)
try:
return self._zeroAcceleration
except AttributeError:
pass
self._zeroAcceleration = Point()
return self._zeroAcceleration
def get_neighbours(self, x, y):
# no check that (x,y) are valid coords
points = []
if x - 1 >= 0:
points.append(Point(x - 1, y))
if x + 1 < map_height:
points.append(Point(x + 1, y))
if y - 1 >= 0:
points.append(Point(x, y - 1))
if y + 1 < map_width:
points.append(Point(x, y + 1))
return points
def place_bookshelf(rng, all_borders, f):
border = rng.choice(all_borders)
x, y = border[0]
angle = border[1]
x_dir, y_dir = FurnitureGenerator.angles[angle]['direction']
bookshelf_types = [(3303, 1901), (171, 98)]
FurnitureGenerator.write(f, x, y, rng.choice(bookshelf_types),
FurnitureGenerator.angles[angle]['angle'] - 90)
new_occupations = []
new_occupations += [Point(x, y)]
new_occupations += [Point(x + y_dir, y + x_dir)]
new_occupations += [Point(x - y_dir, y - x_dir)]
return new_occupations
def place_billiard(rng, center_tiles, f):
x, y = rng.choice(center_tiles)
is_rotated = rng.randrange(2)
FurnitureGenerator.write(f, x, y, (1861, 1092), 90 * is_rotated)
new_occupations = []
new_occupations += [Point(x, y)]
if is_rotated:
new_occupations += [Point(x, y + 1)]
new_occupations += [Point(x, y - 1)]
else:
new_occupations += [Point(x - 1, y)]
new_occupations += [Point(x + 1, y)]
return new_occupations
def serpent(segments, sharpness):
lines = [Line(Point(0., 0.), Point(0., 10.))]
a = np.pi / 2
xprv = 0.
yprv = 10.
sgn = -1
for s in range(segments):
for i in range(sharpness):
a += (np.pi / sharpness) * sgn
x = xprv + 10. * np.cos(a)
y = yprv + 10. * np.sin(a)
lines.append(Line(Point(xprv, yprv), Point(x, y)))
xprv = x
yprv = y
sgn *= -1
return Polyline(lines)
def get_holes(self):
holes = []
for x in range(map_width):
for y in range(map_height):
if self.is_hole(x, y):
holes.append(Point(x, y))
return holes
def __init__(self, xy, gravity, fuel=100, width=11, height=11):
'''
'''
super().__init__(xy, width * 8, height * 8, 3 * gravity)
self.w = width
self.h = height
self.gravity = Point(0, gravity)
self.drawbuf = pygame.Surface(self.rect.size, depth=32)
self.drawbuf.set_colorkey(self.clear)
self.legs = pygame.rect.Rect((0, 0), (2 * width, (2 * height) + 1))
self.hull = pygame.rect.Rect((0, 0), (width, height))
self.plume = pygame.rect.Rect((0, 0), (3 * width, height - 2))
self.hull.center = self.drawbuf.get_rect().center
self.cabin = self.hull.inflate(width / 2, height / 2)
self.service = self.cabin.inflate(-width, 0)
self.service.center = self.cabin.midleft
self.legs.midright = self.hull.midright
self.plume.midright = self.hull.midleft
self.drymass = 10
self.reset(xy, fuel)
def __get_winglet_vector__(length, sweep, cant):
"""Compute winglet tip coordinates in the winglet's LE frame of reference.
Parameters
----------
length : float
sweep : float, degrees
cant : float, degrees
Returns
-------
Point
"""
# Convert degrees to radians
_sweep = np.deg2rad(sweep)
_cant = np.deg2rad(cant)
# Compute unit vector
unit_vector = Point([
np.sin(_sweep) * np.cos(_cant),
np.cos(_sweep) * np.cos(_cant),
np.sin(_cant),
])
# Scale with length
vector = length * unit_vector
return vector
def _create_winglet(self):
"""Create winglet.
Returns
-------
aerosandbox.Wing
"""
# Extract winglet configuration
parameters = self.winglet_parameters
dimensions = self.winglet_dimensions
location_tip = self.__get_winglet_vector__(
length=dimensions["length"],
sweep=parameters[W_ANGLE_SWEEP],
cant=parameters[W_ANGLE_CANT],
)
# Slightly separete from the wing
_epsilon_winglet_wing = Point([0, 0, 0.01])
# Get wing tip section
_section = self.__wingtip_section__
_coordinates = list(_section[LE_LOCATION])
coordinates_weld = _coordinates + _epsilon_winglet_wing
# Match TE of wing tip and winglet root chord
chord_root = dimensions["chord_root"]
chord_tip = dimensions["chord_tip"]
coordinates_weld.x += _section[CHORD] - chord_root
winglet_airfoil = sbx.Airfoil(name=parameters[W_AIRFOIL])
twist_root = parameters[W_ANGLE_TWIST_ROOT]
twist_tip = parameters[W_ANGLE_TWIST_TIP]
winglet = sbx.Wing(
name="Winglet",
xyz_le=list(coordinates_weld),
symmetric=True,
xsecs=[
sbx.WingXSec(
xyz_le=[0, 0, 0],
chord=chord_root,
twist=twist_root,
airfoil=winglet_airfoil,
),
sbx.WingXSec(
xyz_le=list(location_tip),
chord=chord_tip,
twist=twist_tip,
airfoil=winglet_airfoil,
),
],
)
self.winglet = [winglet]
return winglet
def place_entrance(map_, hero_position):
min_distance = 10000
min_point = Point(-1, -1)
for x in range(map_width):
for y in range(map_height):
if "HorizontalWall" in map_[x][y] and \
"VerticalWall" not in map_[x][y - 1] and \
"VerticalWall" not in map_[x + 1][y - 1]:
distance = abs(x - hero_position.x) + abs(y -
hero_position.y)
if distance < min_distance:
min_distance = distance
min_point = Point(x, y)
map_[min_point.x][min_point.y]["HorizontalWall"] = ("Door", 1)
map_[min_point.x][min_point.y - 1]["HorizontalWall"] = ("Transition",
1)
return min_point
def circular(n, circ_dir):
lines = [Line(Point(0., 0.), Point(0., 10.))]
a = np.pi / 2
xprv = 0.
yprv = 10.0
sgn = -1 ### turn right
if circ_dir == 'left':
sgn = 1
for i in range(n):
a += 0.2 * sgn
l = 2 * np.sqrt(i + 10)
x = xprv + l * np.cos(a)
y = yprv + l * np.sin(a)
lines.append(Line(Point(xprv, yprv), Point(x, y)))
xprv = x
yprv = y
return Polyline(lines)
def place_tv_spot(rng, all_borders, f, tiles_tuples):
border = rng.choice(all_borders)
x, y = border[0]
angle = border[1]
x_dir, y_dir = FurnitureGenerator.angles[angle]['direction']
armchair_types = [(178, 104), (1355, 835), (3450, 2013)]
if (x + x_dir * 2, y + y_dir * 2) not in tiles_tuples or (x + x_dir * 3, y + y_dir * 3) not in tiles_tuples:
return []
FurnitureGenerator.write(f, x, y, (191, 110),
FurnitureGenerator.angles[angle]['angle'])
FurnitureGenerator.write(f, x + x_dir, y + y_dir, rng.choice(armchair_types),
FurnitureGenerator.angles[angle]['angle'] + 90)
new_occupations = []
new_occupations += [Point(x, y)]
new_occupations += [Point(x + x_dir, y + y_dir)]
return new_occupations
def get_tiny_corridors(self):
result = []
for x in range(map_width):
for y in range(map_height):
if self.map_[x][y] == -1:
continue
if self.horizontally_narrow(x, y) and self.vertically_narrow(x, y):
result.append(Point(x, y))
return result
def __init__(self,
xy,
width,
height,
maxAcceleration,
heading=90,
initialVelocity=Point()):
super().__init__(xy, width, height, heading, initialVelocity)
self.maxAcceleration = maxAcceleration # scalar
def CreateBoundingBox(self):
self.boundingBox = []
self.boundingBox.append(Point(0, 0))
self.boundingBox.append(Point(0, 0))
self.boundingBox.append(Point(0, 0))
self.boundingBox.append(Point(0, 0))
self.boundingBox[0].y = self.extremePoints[0].y # most north
self.boundingBox[0].x = self.extremePoints[3].x # most west
self.boundingBox[1].y = self.extremePoints[0].y # most north
self.boundingBox[1].x = self.extremePoints[2].x # most east
self.boundingBox[2].y = self.extremePoints[1].y # most south
self.boundingBox[2].x = self.extremePoints[2].x # most east
self.boundingBox[3].y = self.extremePoints[1].y # most south
self.boundingBox[3].x = self.extremePoints[3].x # most west
def testCircleSettingCenterAttribute(self):
p = Point.gaussian()
c = Circle()
c.center = p
self.assertListEqual(c.center.xyz, p.xyz)
c = Circle(p)
c.center = None
self.assertListEqual(c.center.xyz, [0] * 3)
def testCircleSettingCenterAttribute(self):
p = Point.gaussian()
c = Circle()
c.center = p
self.assertListEqual(c.center.xyz, p.xyz)
c = Circle(p)
c.center = None
self.assertListEqual(c.center.xyz, [0] * 3)
def random_path(n, sigma, l):
lines = [Line(Point(0., 0.), Point(0.5, l))]
a = np.pi / 2
xprv = 0.5
yprv = l
for i in range(n):
turn = np.random.normal(0, sigma)
if turn < 0:
turn = max(turn, -sigma)
else:
turn = min(turn, sigma)
a += turn
l = np.random.random() * l + 2
x = xprv + l * np.cos(a)
y = yprv + l * np.sin(a)
lines.append(Line(Point(xprv, yprv), Point(x, y)))
xprv = x
yprv = y
return Polyline(lines)
def addInteractiveSplines(self, lumen, plaque):
self.innerSpline = Spline([lumen[0][self.frame], lumen[1][self.frame]],
'r')
self.outerSpline = Spline(
[plaque[0][self.frame], plaque[1][self.frame]], 'y')
self.innerPoint = [
Point((self.innerSpline.knotPoints[0][idx],
self.innerSpline.knotPoints[1][idx]), 'r')
for idx in range(len(self.innerSpline.knotPoints[0]) - 1)
]
self.outerPoint = [
Point((self.outerSpline.knotPoints[0][idx],
self.outerSpline.knotPoints[1][idx]), 'y')
for idx in range(len(self.outerSpline.knotPoints[0]) - 1)
] # IMPORTANT TO NOT INCLUDE LAST COPIED KNOT POINT DUE TO PERIODICITY
[self.scene.addItem(point) for point in self.innerPoint]
[self.scene.addItem(point) for point in self.outerPoint]
self.scene.addItem(self.innerSpline)
self.scene.addItem(self.outerSpline)
def getPointAt(self, t):
"""
Returns point at the parameter value t
:param t: parameter value, between 0 and 1
:return: Point on curve corresponding to t
"""
# calculate the point using the quadratic bezier equation
x = ((1 - t) ** 2) * self.p0.x + 2 * (1 - t) * t * self.p1.x + t * t * self.p2.x
y = ((1 - t) ** 2) * self.p0.y + 2 * (1 - t) * t * self.p1.y + t * t * self.p2.y
return Point(x, y)
def __init__(self, cx, cy, cz, side_length):
add = side_length / 2
# ---all--- shapes are stored as a list of verteces and a list of edges between two verteces
self.verteces = [
Point(cx + add, cy + add, cz + add),
Point(cx + add, cy + add, cz - add),
Point(cx + add, cy - add, cz + add),
Point(cx + add, cy - add, cz - add),
Point(cx - add, cy + add, cz + add),
Point(cx - add, cy + add, cz - add),
Point(cx - add, cy - add, cz + add),
Point(cx - add, cy - add, cz - add),
]
self.edge = [(0, 1), (0, 4), (0, 2), (1, 3), (1, 5), (2, 6), (2, 3),
(3, 7), (4, 5), (4, 6), (5, 7), (6, 7)]
self.center = Point(cx, cy, cz)
def LoadMap(self):
self.shape = Shape()
self.shapes = []
self.loadState = 0
self.lawn = 'lawn'
self.obstacle = 'obstacle'
self.mower = 'mower'
self.station = 'station'
f = open(
"/home/pi/catkin_ws/src/intelli_mower/src/IntelliMowerAlgorithmRoS/src/map_one",
'r')
for line in f:
if line.strip() == str(self.lawn):
self.loadState = 3
elif line.strip() == str(self.obstacle):
self.loadState = 4
newShape = Shape()
self.shapes.append(newShape)
elif line.strip() == str(self.mower):
self.loadState = 1
elif line.strip() == str(self.station):
self.loadState = 2
else:
if self.loadState == 1:
cords = line.split()
self.mowerP = Point(float(cords[0]), float(cords[1]))
self.mowerP.borderId = int(cords[2])
elif self.loadState == 2:
cords = line.split()
self.stationP = Point(float(cords[0]), float(cords[1]))
self.stationP.borderId = int(cords[2])
# load outer border
elif self.loadState == 3:
cords = line.split()
p = Point(float(cords[0]), float(cords[1]))
p.borderId = int(cords[2])
self.shape.AddBorderPoint(p)
# load inner border
elif self.loadState == 4:
cords = line.split()
p = Point(float(cords[0]), float(cords[1]))
p.borderId = int(cords[2])
self.shapes[-1].AddBorderPoint(p)
f.close()
def testCircleCreationWithCenterAndRadiusArguments(self):
p = Point.gaussian()
c = Circle(p, 2)
self.assertIsInstance(c, Circle)
self.assertListEqual(c.center.xyz, p.xyz)
self.assertEqual(c.radius, 2)
def testCircleCreationWithKeywordsReversed(self):
p = Point.gaussian()
c = Circle(radius=3, center=p)
self.assertIsInstance(c, Circle)
self.assertListEqual(c.center.xyz, p.xyz)
self.assertEqual(c.radius, 3)
def testCircleCreationWithAllKeywords(self):
p = Point.gaussian()
c = Circle(center=p, radius=2)
self.assertIsInstance(c, Circle)
self.assertListEqual(c.center.xyz, p.xyz)
self.assertEqual(c.radius, 2)
def testCircleCreationWithOnlyCenterKeyword(self):
p = Point.gaussian()
c = Circle(center=p)
self.assertIsInstance(c, Circle)
self.assertListEqual(c.center.xyz, p.xyz)
self.assertEqual(c.radius, 1)
