def during_collision(cue_point, radius, stick_point, ball_coord):
future_point = cue_point
collision_ball_info = cue_point
min_distance = 1e9
temp_ray = Ray(cue_point, stick_point)
temp_Line = Line(cue_point, stick_point)
temp_circle = Circle(cue_point, radius)
temp_collision_points = intersection(temp_Line, temp_circle)
if temp_ray.contains(temp_collision_points[0]):
white_ray = Ray(cue_point, temp_collision_points[1])
else:
white_ray = Ray(cue_point, temp_collision_points[0])
for coord in ball_coord:
enlarged_ball = Circle(Point(coord[0], coord[1]), coord[2] + radius)
intersect_point = intersection(white_ray, enlarged_ball)
if len(intersect_point) == 2 and cue_point.distance(
intersect_point[0]) >= cue_point.distance(intersect_point[1]):
temp_point = intersect_point[1]
elif len(intersect_point) == 2 or len(intersect_point) == 1:
temp_point = intersect_point[0]
else:
continue
dist = cue_point.distance(temp_point)
if min_distance > dist:
min_distance = dist
future_point = temp_point
collision_ball_info = coord
return future_point, collision_ball_info
def find_intersect_ray_circle(board1_xy, board2_xy, board1_dist, board2_dist):
board1_xy = Point(board1_xy[0], board1_xy[1])
board2_xy = Point(board2_xy[0], board2_xy[1])
circle1 = Circle(board1_xy, board1_dist)
circle2 = Circle(board2_xy, board2_dist)
intersect = circle1.intersect(circle2)
intersect = np.array(intersect.args, dtype=float)
return intersect
def test_parabola_intersection():
l1 = Line(Point(1, -2), Point(-1, -2))
l2 = Line(Point(1, 2), Point(-1, 2))
l3 = Line(Point(1, 0), Point(-1, 0))
p1 = Point(0, 0)
p2 = Point(0, -2)
p3 = Point(120, -12)
parabola1 = Parabola(p1, l1)
# parabola with parabola
assert parabola1.intersection(parabola1) == [parabola1]
assert parabola1.intersection(Parabola(
p1, l2)) == [Point2D(-2, 0), Point2D(2, 0)]
assert parabola1.intersection(Parabola(p2, l3)) == [Point2D(0, -1)]
assert parabola1.intersection(Parabola(Point(16, 0),
l1)) == [Point2D(8, 15)]
assert parabola1.intersection(Parabola(Point(
0, 16), l1)) == [Point2D(-6, 8), Point2D(6, 8)]
assert parabola1.intersection(Parabola(p3, l3)) == []
# parabola with point
assert parabola1.intersection(p1) == []
assert parabola1.intersection(Point2D(0, -1)) == [Point2D(0, -1)]
assert parabola1.intersection(Point2D(4, 3)) == [Point2D(4, 3)]
# parabola with line
assert parabola1.intersection(Line(Point2D(-7, 3), Point(
12, 3))) == [Point2D(-4, 3), Point2D(4, 3)]
assert parabola1.intersection(Line(Point(-4, -1),
Point(4, -1))) == [Point(0, -1)]
assert parabola1.intersection(Line(Point(2, 0),
Point(0, -2))) == [Point2D(2, 0)]
raises(
TypeError,
lambda: parabola1.intersection(Line(Point(0, 0, 0), Point(1, 1, 1))))
# parabola with segment
assert parabola1.intersection(Segment2D(
(-4, -5), (4, 3))) == [Point2D(0, -1), Point2D(4, 3)]
assert parabola1.intersection(Segment2D((0, -5),
(0, 6))) == [Point2D(0, -1)]
assert parabola1.intersection(Segment2D((-12, -65), (14, -68))) == []
# parabola with ray
assert parabola1.intersection(Ray2D(
(-4, -5), (4, 3))) == [Point2D(0, -1), Point2D(4, 3)]
assert parabola1.intersection(Ray2D(
(0, 7), (1, 14))) == [Point2D(14 + 2 * sqrt(57), 105 + 14 * sqrt(57))]
assert parabola1.intersection(Ray2D((0, 7), (0, 14))) == []
# parabola with ellipse/circle
assert parabola1.intersection(Circle(
p1, 2)) == [Point2D(-2, 0), Point2D(2, 0)]
assert parabola1.intersection(Circle(
p2, 1)) == [Point2D(0, -1), Point2D(0, -1)]
assert parabola1.intersection(Ellipse(
p2, 2, 1)) == [Point2D(0, -1), Point2D(0, -1)]
assert parabola1.intersection(Ellipse(Point(0, 19), 5, 7)) == []
assert parabola1.intersection(Ellipse((0, 3), 12, 4)) == \
[Point2D(0, -1), Point2D(0, -1), Point2D(-4*sqrt(17)/3, Rational(59, 9)), Point2D(4*sqrt(17)/3, Rational(59, 9))]
# parabola with unsupported type
raises(TypeError, lambda: parabola1.intersection(2))
def find_intersect_circle_circle(circle_center1, r_1, circle_center2, r_2):
point1 = Point(circle_center1)
point2 = Point(circle_center2)
circle1 = Circle(point1, sympify(str(r_1), rational=True))
circle2 = Circle(point2, sympify(str(r_2), rational=True))
intersect = circle2.intersect(circle1)
if len(intersect) == 0:
return np.nan * np.eye(2)
else:
intersect = np.array([intersect.args[0], intersect.args[1]],
dtype=float)
return intersect
def is_close_to(self, other: 'Position', threshold: float):
x_diff = self.point.x * threshold if self.point.x else threshold
y_diff = self.point.y * threshold if self.point.y else threshold
x_point = self.point.x + x_diff
y_point = self.point.y + y_diff
new_pos = Position(x_point, y_point)
radius = self.calculate_distance_from(new_pos)
area = Circle(self.point, radius)
return area.encloses_point(other.point)
def intersection_of_three_circel(x1, y1, r1, x2, y2, r2, x3, y3, r3):
# Todo handle edge cases if any
c1 = Circle(Point(x1, y1), r1)
c2 = Circle(Point(x2, y2), r2)
c3 = Circle(Point(x3, y3), r3)
ans = intersection(c1, c2, c3)
if len(ans) != 0:
ans = [(p.x, p.y) for p in ans]
return ans
else:
return None
def get_location(list_of_positions_and_distance):
"""
This method receive a list with all postions and the distance to the emisor.
To to get the origin this method create three circles and get the intersection
point between this circles
>>> get_location([(-1, 0, 1), (0, -2, 2), (3, 0, 3)])
(0, 0)
"""
circle_one = Circle(
Point(list_of_positions_and_distance[0][0],
list_of_positions_and_distance[0][1]),
list_of_positions_and_distance[0][2])
circle_two = Circle(
Point(list_of_positions_and_distance[1][0],
list_of_positions_and_distance[1][1]),
list_of_positions_and_distance[1][2])
circle_three = Circle(
Point(list_of_positions_and_distance[2][0],
list_of_positions_and_distance[2][1]),
list_of_positions_and_distance[2][2])
intersection_point_set = (set(circle_one.intersection(circle_two))
& set(circle_one.intersection(circle_three))
& set(circle_two.intersection(circle_three)))
if len(intersection_point_set) != 1:
# None or more than 1 point where found
raise Exception
intersection_point = intersection_point_set.pop()
return intersection_point.x, intersection_point.y
def cli(point, circle, sympy, filename):
"""Comparing the `sympy.geometry` and `matplotlib.path` libraries"""
figure = plt.figure()
x, y = point
plt.scatter([x], [y], c='r', marker='X')
cx, cy, r = circle
theta = np.linspace(0, 2 * np.pi, 500)
plt.scatter([cx], [cy], c='b', marker='x')
plt.plot(r * np.cos(theta) + cx, r * np.sin(theta) + cy, 'b--')
plt.title(f'Comparing the .{sympy} and .contains_point methods')
plt.grid()
plt.xlim((-(r + 1) + cx, +(r + 1) + cx))
plt.ylim((-(r + 1) + cy, +(r + 1) + cy))
plt.gca().set_aspect('equal', adjustable='box')
plt.tight_layout()
c1 = ['red', 'blue'][Path.circle((cx, cy), r).contains_point(point)]
c2 = ['red', 'blue'][getattr(Circle((cx, cy), r), sympy)(Point(x, y))]
p1 = mpatches.Patch(color=c1, label='.contains_point')
p2 = mpatches.Patch(color=c2, label=f'.{sympy}')
plt.legend(handles=[p1, p2], loc='upper left')
if filename is None:
plt.show()
else:
figure.savefig(filename, format="png")
def main():
image_address = str(input('Enter the image name: '))
image_address = "pool_Images/" + image_address
print(image_address)
ball_coord, cue_coord, stick_coord = detection.detect_coordinates2(image_address)
#print(cue_coord, ball_coord, stick_coord)
if len(cue_coord) == 0 or len(stick_coord) == 0:
print("No point detected")
return
cue_point = Point(cue_coord[0], cue_coord[1])
stick_point = Point(stick_coord[0], stick_coord[1])
path_of_white_ball(cue_point, stick_coord, cue_coord[2])
#path_of_white_ball(p1, p2, RADIUS)
future_point, collision_ball_info = during_collision(cue_point, cue_coord[2],stick_point,ball_coord)
if future_point == cue_point:
print("No collision")
return
else:
frame = cv.imread(image_address, 1)
cv.circle(frame, (int(collision_ball_info[0]), int(collision_ball_info[1])), 2*int(collision_ball_info[2]), (0, 255, 255), 2)
cv.circle(frame, (int(future_point.x), int(future_point.y)), int(cue_coord[2]), (255, 255, 255), -1)
temp_point = Point(collision_ball_info[0],collision_ball_info[1])
colored_future_point = intersection(Ray(future_point,temp_point),Circle(temp_point,collision_ball_info[2]*5))
cv.line(frame, (cue_point.x, cue_point.y), (future_point.x, future_point.y), (255, 255, 255), thickness=2)
cv.line(frame, ((int)(collision_ball_info[0]), (int)(collision_ball_info[1])), ((int)(colored_future_point[0].x), (int)(colored_future_point[0].y)), (0, 255, 0), thickness=2)
while 1:
cv.namedWindow('Image',cv.WND_PROP_FULLSCREEN)
cv.imshow('Image', frame)
if cv.waitKey(1) & 0xFF == 27:
break
cv.destroyAllWindows()
def Dubins_msg(UAV, target, R0):
v = UAV.v
phi0 = UAV.phi
UAV_p = Point(UAV.site)
target_p = Point(target[0:2])
c1 = Point(UAV_p.x + R0 * np.sin(phi0), UAV_p.y - R0 * np.cos(phi0))
c2 = Point(UAV_p.x - R0 * np.sin(phi0), UAV_p.y + R0 * np.cos(phi0))
len1 = c1.distance(target_p)
len2 = c2.distance(target_p)
center = c1
if len2 > len1:
center = c2
center = Point(round(center.x.evalf(), 4), round(center.y.evalf(), 4))
circle = Circle(center, R0)
tangent_lines = Tangent_lines(circle, target_p)
tangent_line1 = tangent_lines[0]
tangent_line1 = Line(tangent_line1.p2, tangent_line1.p1)
tangent_point1 = tangent_line1.p1
y = float((target_p.y - tangent_point1.y).evalf())
x = float((target_p.x - tangent_point1.x).evalf())
tangent_angle1 = np.arctan2(y, x)
tangent_line2 = tangent_lines[1]
tangent_line2 = Line(tangent_line2.p2, tangent_line2.p1)
tangent_point2 = tangent_line2.p1
y = float((target_p.y - tangent_point2.y).evalf())
x = float((target_p.x - tangent_point2.x).evalf())
tangent_angle2 = np.arctan2(y, x)
vec1 = [UAV_p.x - center.x, UAV_p.y - center.y]
vec2 = [np.cos(phi0), np.sin(phi0)]
direction = np.sign(vec1[0] * vec2[1] - vec1[1] * vec2[0])
sin1 = float(tangent_point1.distance(UAV_p).evalf()) / (2 * R0)
angle1 = 2 * np.arcsin(sin1)
sin2 = float(tangent_point2.distance(UAV_p).evalf()) / (2 * R0)
angle2 = 2 * np.arcsin(sin2)
tangent_point = []
radian = 0
if abs(modf(abs(direction * angle1 + phi0 - tangent_angle1) / (2 * np.pi))[0] - 0.5) > 0.5 - deviation:
tangent_point = tangent_point1
radian = angle1
elif abs(modf(abs(direction * (2 * np.pi - angle1) + phi0 - tangent_angle1) / (2 * np.pi))[
0] - 0.5) > 0.5 - deviation:
tangent_point = tangent_point1
radian = 2 * np.pi - angle1
elif abs(modf(abs(direction * angle2 + phi0 - tangent_angle2) / (2 * np.pi))[0] - 0.5) > 0.5 - deviation:
tangent_point = tangent_point2
radian = angle2
elif abs(modf(abs(direction * (2 * np.pi - angle2) + phi0 - tangent_angle2) / (2 * np.pi))[
0] - 0.5) > 0.5 - deviation:
tangent_point = tangent_point2
radian = 2 * np.pi - angle2
return direction, radian, (float(tangent_point.x.evalf()), float(tangent_point.y.evalf())), (
float(center.x.evalf()), float(center.y.evalf()))
def predict_path(circle_coordinates,cue_ball_coordinate,cue_stick_coordinate,frame):
global prev_stick_point, prev_cue_point
if len(cue_ball_coordinate) < 3 or len(cue_stick_coordinate) < 2:
print("No point detected")
return frame
cue_point = Point(cue_ball_coordinate[0], cue_ball_coordinate[1])
stick_point = Point(cue_stick_coordinate[0], cue_stick_coordinate[1])
future_point, collision_ball_info = during_collision(cue_point, cue_ball_coordinate[2],stick_point,circle_coordinates)
prev_stick_point = stick_point
prev_cue_point = cue_point
flag = 0
if future_point == cue_point:
print("No collision")
else:
print("Collision")
cv.circle(frame, (int(collision_ball_info[0]), int(collision_ball_info[1])), 2*int(collision_ball_info[2]), (0, 255, 255), 2)
cv.circle(frame, (int(future_point.x), int(future_point.y)), int(cue_ball_coordinate[2]), (255, 255, 255), -1)
temp_point = Point(collision_ball_info[0],collision_ball_info[1])
colored_future_point = intersection(Ray(future_point,temp_point),Circle(temp_point,collision_ball_info[2]*5))
cv.line(frame, (cue_point.x, cue_point.y), (future_point.x, future_point.y), (255, 255, 255), thickness=2)
cv.line(frame, ((int)(collision_ball_info[0]), (int)(collision_ball_info[1])), ((int)(colored_future_point[0].x),
(int)(colored_future_point[0].y)), (0, 255, 0), thickness=2)
cv.circle(frame, (int(cue_stick_coordinate[0]), int(cue_stick_coordinate[1])), 3, (123, 55, 55), 4)
cv.circle(frame, (int(cue_ball_coordinate[0]), int(cue_ball_coordinate[1])),int(cue_ball_coordinate[2])*2, (100, 100, 100), 2)
cv.circle(frame, (int(cue_ball_coordinate[0]), int(cue_ball_coordinate[1])), 1, (255, 100, 100), 2)
return frame
def path_of_white_ball(p1, p2, r):
pathBallW[:] = []
middle_ray = Line(p1, p2)
cue_circle = Circle(p1, r)
normal_line = middle_ray.perpendicular_line(p1)
points = intersection(cue_circle, normal_line)
pathBallW.append(middle_ray.parallel_line(points[0]))
pathBallW.append(middle_ray)
pathBallW.append(middle_ray.parallel_line(points[1]))
def minidiscwith2points(P: [Point], q1: Point, q2: Point):
D = get_circle(q1, q2)
for k in range(len(P)):
if D.encloses(P[k]):
pass
else:
D = Circle(q1, q2, P[k])
return D
def enclosing_circle(P: [Point]):
C_min = Circle(Point(250, 250), 10000)
# Two points
for a in P:
for b in P:
if b == a:
continue
C = get_circle(a, b)
for d in P:
if d == b or d == a:
continue
if not C.encloses_point(d):
break
else:
if C.radius < C_min.radius:
C_min = C
# Three points
for a in P:
for b in P:
if b == a:
continue
for c in P:
if c == a or c == b:
continue
C = Circle(a, b, c)
for d in P:
if d == a or d == b or d == c:
continue
if not C.encloses_point(d):
break
else:
if C.radius < C_min.radius:
C_min = C
return C_min
def _collect_shapes(self, d, materials):
"""
Collect and instantiate the dictionary stored in the shapes attribute
given a dictionary describing the shapes in the layer
"""
shapes = OrderedDict()
sorted_d = OrderedDict(
sorted(d.items(), key=lambda tup: tup[1]['order']))
for name, data in sorted_d.items():
shape = data['type'].lower()
if shape == 'circle':
center = Point(data['center']['x'], data['center']['y'])
radius = data['radius']
material = data['material']
if material not in self.materials:
self.materials[material] = materials[material]
shape_obj = Circle(center, radius)
else:
raise NotImplementedError('Can only handle circles right now')
shapes[name] = (shape_obj, material)
self.shapes = shapes
return shapes
def sphere_impact_points(self):
impact_points = getattr(self, '_impact_points', None)
print round(self.semi_major_axis, 2)
if impact_points:
# TODO should include semi minor too
"""Invalid ellipse on change of semi major axis"""
if round(self.semi_major_axis, 2) != round(self._impact_sma, 2):
impact_points = None
print 'invalid!'
if impact_points is None:
print 'create!'
self._impact_sma = self.semi_major_axis
r = self.equatorial_radius
a = self.semi_major_axis
b = self.semi_minor_axis
c = math.sqrt(a ** 2 - b ** 2) # linear eccentricity
elp = Ellipse(Point(c, 0), a, b)
crc = Circle(Point(0, 0), r)
self._impact_points = sorted(list(set([(float(point.x), float(point.y)) for point in elp.intersection(crc)])))
return self._impact_points
def intersects_ellipse(ob, major, minor, tip, tip_r, tip_r2):
"""
Calculates the intersection of an ellipse and two circles
(representing the tip) with different radii
Args:
major: major axis of the ellipse
minor: minor axis of the ellipse
tip: position of the circles (one for both)
tip_r: first radius
tip_r2: second (bigger) radius
"""
# check if there is any chance of contact
if tip[0] + tip_r2 < - major:
# the tip's maximum x is below the object's minimum x
return [np.zeros((2, 1), dtype=np.float32),
np.zeros((2, 1), dtype=np.float32)]
elif tip[0] - tip_r2 > major:
# the tip's minimum x is above the object's maximum x
return [np.zeros((2, 1), dtype=np.float32),
np.zeros((2, 1), dtype=np.float32)]
elif tip[1] + tip_r2 < - minor:
# the tip's maximum y is below the object's minimum y
return [np.zeros((2, 1), dtype=np.float32),
np.zeros((2, 1), dtype=np.float32)]
elif tip[1] - tip_r2 > minor:
# the tip's minimum y is above the object's maximum y
return [np.zeros((2, 1), dtype=np.float32),
np.zeros((2, 1), dtype=np.float32)]
# try with the bigger probe
tp = Circle(Point(tip[0], tip[1]), tip_r2)
if '1' in ob:
cps = ellip1.intersection(tp)
elif '2' in ob:
cps = ellip2.intersection(tp)
elif '3' in ob:
cps = ellip3.intersection(tp)
count = 0.
avg = np.zeros((2, 1), dtype=np.float32)
for cp in cps:
c = np.zeros((2, 1), dtype=np.float32)
c[0] = cp.x
c[1] = cp.y
count += 1
avg += c
if count != 0:
contact2 = avg / count
else:
contact2 = avg
# only check the smaller one if the bigger radius had a contact
if np.linalg.norm(contact2) > 0:
tp = Circle(Point(tip[0], tip[1]), tip_r)
if '1' in ob:
cps = ellip1.intersection(tp)
elif '2' in ob:
cps = ellip2.intersection(tp)
elif '3' in ob:
cps = ellip3.intersection(tp)
count = 0.
avg = np.zeros((2, 1), dtype=np.float32)
for cp in cps:
c = np.zeros((2, 1), dtype=np.float32)
c[0] = cp.x
c[1] = cp.y
count += 1
avg += c
if count != 0:
contact1 = avg / count
else:
contact1 = avg
else:
contact1 = np.zeros((2, 1), dtype=np.float32)
return [contact1, contact2]
# we use sympy to calculate the contact points. For line-circle intersections,
# we first solve the equations symbolically and then plug in the values when
# needed
# intersection line - circle
x, y, m, c, r = sp.symbols('x y m c r', real=True)
# solving for x
eq_line_circle_x = sp.Eq((m * x + c)**2, (r**2 - x**2))
expr_line_circle_x = sp.solve(eq_line_circle_x, x)
# solving for y
eq_line_circle_y = sp.Eq(((y - c) / m)**2, (r**2 - y**2))
expr_line_circle_y = sp.solve(eq_line_circle_y, y)
# Define the objects as sympy ellipses and circles
# Warning: this can lead to an error with sympy versions < 1.1.
ellip1 = Circle(Point(0., 0.), 0.0525)
ellip2 = Ellipse(Point(0., 0.), 0.0525, 0.065445)
ellip3 = Ellipse(Point(0., 0.), 0.0525, 0.0785)
def get_contact_annotation(source_dir, out_dir, num_jobs=10, debug=True,
criteria=['a=0']):
if not os.path.exists(out_dir):
os.mkdir(out_dir)
mats = [f for f in os.listdir(source_dir)
if os.path.isdir(os.path.join(source_dir, f)) if 'delrin' in f]
for mat in mats:
objects = [f for f in os.listdir(os.path.join(source_dir, mat))
if 'zip' not in f]
if not os.path.exists(os.path.join(out_dir, mat)):
def find_common_center(recordings, receiver_data, mode='avg', more_data=False):
# Create circles:
# Find radius for each receiver
# take min, avg, or max
# Get center from database
if not mode in ['min', 'max', 'avg']:
raise Exception("Unrecognized singularization mode: " + str(mode))
circles = {}
transmitter = None
#Collect transmitter -> strength array mapping
for recording in recordings:
if not transmitter:
transmitter = recording['transmitter']
else:
if transmitter != recording['transmitter']:
raise Exception(
"Multiple transmitters found. Please filter by transmitter."
)
if recording['receiver'] not in circles.keys():
circles[recording['receiver']] = []
circles[recording['receiver']].append(recording['rssi'])
#Perform singularization on strength arrays
#Create Circle instances
for (receiver, radii) in circles.items():
if mode == 'min':
radius = min(radii)
elif mode == 'max':
radius = max(radii)
elif mode == 'avg':
radius = sum(radii) / len(radii)
else:
raise Exception("Unrecognized singularization mode: " + str(mode))
rec = receiver_data[receiver]
if rec:
x = rec['x']
y = rec['y']
else:
raise Exception("Unknown receiver found. Please enter data for: " +
str(transmitter))
circles[receiver] = Circle(Point(x, y), abs(radius))
# Find all circle intersection points, store in set
intersects = set()
#For each permutation of circles, two at a time
#Find all intersection points
for c1, c2 in permutations(circles.values(), 2):
intersects.update(c1.intersection(c2))
# For each circle, prune all points not inside, inclusive (including points on the edge of the circle)
for circle in circles.values():
for intersection in set(intersects):
if float(circle.center.distance(intersection)) > float(
circle.radius):
intersects.discard(intersection)
# If points remaining>=3, create polygon from points, get polygon's area and centroid. That is predicted position, uncertainty
# If points remaining==2, create circle centered around midpoint, with d=distance between the points. Circle's center is predicted position, area is incertainty
# If points remaining==1, that is predicted position. Uncertainty is zero
# If points remaining<1, return None
ret = tuple()
uncertainty_shape = None
if len(intersects) >= 3:
poly = Polygon(*intersects)
ret = (poly.centroid, poly.area)
uncertainty_shape = poly
elif len(intersects) == 2:
seg = Segment(*intersects)
circ = Circle(seg.midpoint, seg.length / 2.0)
ret = (circ.center, circ.area)
uncertainty_shape = seg
elif len(intersects) == 1:
ret = (intersects.pop(), 0)
else:
ret = (None, None)
if more_data:
return {
'receivers': circles,
'intersects': intersects,
'uncertainty_shape': uncertainty_shape,
'pos': ret[0],
'uncertainty': ret[1]
}
else:
return ret
from sympy import Point,Line,Circle,intersection,Triangle,N
from svg import Svg
C = Point(0,8)
D = Point(0,2)
xaxis = Line(Point(0,0),Point(1,0))
CircleD = Circle(D,2)
tangentE = CircleD.tangent_lines(C)[0]
E = intersection(tangentE,CircleD)[0]
A = intersection(tangentE, xaxis)[0]
CircleD = Circle(D,2)
svg = Svg()
svg.append(C,"C")
#svg.append(D)
svg.append(CircleD,"CircleD")
svg.append(tangentE,"tangE")
svg.append(E,"E")
svg.append(A,"A")
def find_circle(circle,A,C,D,i):
AD = Line(A,D)
svg.append(AD,"AD",i)
K = intersection(circle, AD)[0]
svg.append(K,"K",i)
tangentK = Line(A,D).perpendicular_line(K)
svg.append(tangentK,"tangK",i)
P1 = intersection(tangentK, Line(A,C))[0]
svg.append(P1,"P1",i)
P2 = intersection(tangentK, xaxis)[0]
svg.append(P2,"P2",i)
from sympy import Circle, Ellipse, Point
import numpy as np
USE_SYMPY_INTERSECTION = True
VISUALISE = True # Display plot after running
SAVE_PLOT = False # Will take priority over VISUALISE
# circle
cx, cy = (10, 20)
r = 15
# ellipse
ecx, ecy = (20, 20)
a = 20 # h_radius
b = 10 # v_radius
c = Circle(Point(cx, cy), r)
e = Ellipse(Point(ecx, ecy), a, b)
if USE_SYMPY_INTERSECTION:
i = e.intersection(c)
c_eq = c.arbitrary_point()
e_eq = e.arbitrary_point()
if VISUALISE or SAVE_PLOT:
import matplotlib.pyplot as plt
from math import sin, cos, radians as rad
fig = plt.figure()
ax = plt.subplot()
ax.set_aspect(1.0)
# 1. 求两个圆心，判断出采用哪一个圆
# 2. 求切线
# 3. 确定用那一段弧长
# 1.求两个圆心，判断出采用哪一个圆
c1 = Point(UAV_p.x + R0 * np.sin(phi0), UAV_p.y - R0 * np.cos(phi0))
c2 = Point(UAV_p.x - R0 * np.sin(phi0), UAV_p.y + R0 * np.cos(phi0))
len1 = c1.distance(target_p)
len2 = c2.distance(target_p)
center = c1
if len2 > len1:
center = c2
# 2. 求切线
circle = Circle(center, R0)
tangent_lines = circle.tangent_lines(target_p)
tangent_line1 = tangent_lines[0] # 注意这里的切线方向是从target-> 切点
tangent_line1 = Line(tangent_line1.p2, tangent_line1.p1) # 改为从切点->target
tangent_point1 = tangent_line1.p1 # 切点1
y = float((target_p.y - tangent_point1.y).evalf())
x = float((target_p.x - tangent_point1.x).evalf())
tangent_angle1 = np.arctan2(y, x) # arctan2(y,x) 向量(x,y)的角度[-pi,pi]
tangent_line2 = tangent_lines[1]
tangent_line2 = Line(tangent_line2.p2, tangent_line2.p1) # 改为从切点->target
tangent_point2 = tangent_line2.p1 # 切点２
y = float((target_p.y - tangent_point2.y).evalf())
x = float((target_p.x - tangent_point2.x).evalf())
tangent_angle2 = np.arctan2(y, x) # arctan2(y,x) 向量(x,y)的角度[-pi,pi]
def get_circle(a, b):
r = a.distance(b)/2
c = a.midpoint(b)
return Circle(c, r)
def draw_polygons(polygon_list, save_dir):
p = Plot(axes='label_axes=True')
c = Circle(Point(0, 0), 1)
p[0] = c
print(UAV.site)
print(target[0:2])
exec()
elif type == 1:
if minc.distance(target) <= R0:
# 如果小的dubins曲线到达不了，就用大的
center = maxc
elif type == 2:
center = maxc
# if minc.distance(target)<=R0:
# # 如果小的dubins曲线到达不了，就用大的
# center=maxc
# # 2. 求切线
circle = Circle(center, R0)
#tangent_lines = circle.tangent_lines(target)
tangent_lines=Tangent_lines(circle,target)
tangent_line1 = tangent_lines[0] # 注意这里的切线方向是从target-> 切点
tangent_line1 = Line(tangent_line1.p2, tangent_line1.p1) # 改为从切点->target
tangent_point1 = tangent_line1.p1 # 切点1
y = float((target.y - tangent_point1.y).evalf())
x = float((target.x - tangent_point1.x).evalf())
tangent_angle1 = np.arctan2(y, x) # arctan2(y,x) 向量(x,y)的角度[-pi,pi]
tangent_line2 = tangent_lines[1]
tangent_line2 = Line(tangent_line2.p2, tangent_line2.p1) # 改为从切点->target
tangent_point2 = tangent_line2.p1 # 切点２
y = float((target.y - tangent_point2.y).evalf())
UAV = Point(5, 5)
UAV_p = np.array([5, 5])
target = Point(10, 8)
R0 = 3
phi0 = np.pi / 4
phi1 = np.arctan2(8, 10)
center = []
np.arctan2()
if phi0 > phi1:
center = Point(UAV.x - R0 * np.sin(phi0),
UAV.y + R0 * np.cos(phi0)) #逆时针起始圆圆心
else:
center = Point(Point.x + R0 * np.sin(phi0),
Point.y - R0 * np.cos(phi0)) #顺时针起始圆圆心
c1 = Circle(center, R0)
t1 = c1.tangent_lines(target)
for tangent_line in t1:
p1 = np.array([tangent_line.p1.x.evalf(),
tangent_line.p1.y.evalf()]) # 目标点
p2 = np.array([tangent_line.p2.x.evalf(), tangent_line.p2.y.evalf()]) # 切点
tan = (p1 - p2)[1] / (p1 - p2)[0]
angle = phi0 - math.atan(tan) #
lens = math.sqrt(sum((p2 - UAV_p)**2))
theta1 = 2 * math.asin(lens / 2 / R0)
# Krävs Sympy 1.0 for att fungera # 1.1.1 Kraschade from sympy import Point,Line,Circle,intersection,Triangle,N import time p1 = Point(0,8) p0 = Point(0,2) X = Line(Point(0,0),Point(1,0)) c0 = Circle(p0,2) l0 = c0.tangent_lines(p1)[0] p2 = intersection(l0, X)[0] l2 = Line(p2, p0) def findCircle(circle): p3 = intersection(circle, l2)[0] # Kraschar här. l3 = l2.perpendicular_line(p3) p5 = intersection(l3, l0)[0] p6 = intersection(l3, X)[0] t1 = Triangle(p5, p2, p6) return t1.incircle circle = c0 for i in range(5): start = time.time() circle = findCircle(circle) r = circle.radius print() print(r) print(i,time.time()-start, N(r))
def Dubins_msg(UAV, target, R0):
# 单架无人机到达目标点所需时间
# 这里的UAV和target是无人机和目标对象
v = UAV.v # 飞机速度
phi0 = UAV.phi # 转化为弧度，[0,2pi]
UAV_p = Point(UAV.site)
target_p = Point(target[0:2])
# 以上为所有已知信息
# 1. 求两个圆心，判断出采用哪一个圆
# 2. 求切线
# 3. 确定用那一段弧长
# 1.求两个圆心，判断出采用哪一个圆
c1 = Point(UAV_p.x + R0 * np.sin(phi0), UAV_p.y - R0 * np.cos(phi0))
c2 = Point(UAV_p.x - R0 * np.sin(phi0), UAV_p.y + R0 * np.cos(phi0))
len1 = c1.distance(target_p)
len2 = c2.distance(target_p)
center = c1
if len2 > len1:
center = c2
# 2. 求切线
center = Point(round(center.x.evalf(), 4), round(center.y.evalf(), 4))
circle = Circle(center, R0)
# start=time.time()
# tangent_lines = circle.tangent_lines(target_p)
tangent_lines = Tangent_lines(circle, target_p)
# end=time.time()
# print(end-start)
tangent_line1 = tangent_lines[0] # 注意这里的切线方向是从target-> 切点
tangent_line1 = Line(tangent_line1.p2, tangent_line1.p1) # 改为从切点->target
tangent_point1 = tangent_line1.p1 # 切点1
y = float((target_p.y - tangent_point1.y).evalf())
x = float((target_p.x - tangent_point1.x).evalf())
tangent_angle1 = np.arctan2(y, x) # arctan2(y,x) 向量(x,y)的角度[-pi,pi]
tangent_line2 = tangent_lines[1]
tangent_line2 = Line(tangent_line2.p2, tangent_line2.p1) # 改为从切点->target
tangent_point2 = tangent_line2.p1 # 切点２
y = float((target_p.y - tangent_point2.y).evalf())
x = float((target_p.x - tangent_point2.x).evalf())
tangent_angle2 = np.arctan2(y, x) # arctan2(y,x) 向量(x,y)的角度[-pi,pi]
# 3. 确定用哪一段弧长
# a. 确定用顺时针还是逆时针
vec1 = [UAV_p.x - center.x, UAV_p.y - center.y]
vec2 = [np.cos(phi0), np.sin(phi0)]
direction = np.sign(vec1[0] * vec2[1] - vec1[1] * vec2[0]) # 1 表示逆时针 -1 表示顺时针
# b. 判断是哪一个切点，哪一段弧
sin1 = float(tangent_point1.distance(UAV_p).evalf()) / (2 * R0)
angle1 = 2 * np.arcsin(sin1) # 无人机位置与切点之间的弧度[0,pi] 小弧
sin2 = float(tangent_point2.distance(UAV_p).evalf()) / (2 * R0)
angle2 = 2 * np.arcsin(sin2)
tangent_point = []
hudu = 0
# 判断式的意思 角度要在误差范围内相隔2kpi，使用modf(abs 把值控制在0-1之内，误差范围内靠近1，靠近0 都ok 用于0.5之间的距离来判断
if abs(modf(abs(direction * angle1 + phi0 - tangent_angle1) / (2 * np.pi))[0] - 0.5) > 0.5 - deviation:
tangent_point = tangent_point1
hudu = angle1
# modf 返回浮点数的小数部分和整数部分modf(1.23) return [0.23,1]
elif abs(modf(abs(direction * (2 * np.pi - angle1) + phi0 - tangent_angle1) / (2 * np.pi))[
0] - 0.5) > 0.5 - deviation:
tangent_point = tangent_point1
hudu = 2 * np.pi - angle1
elif abs(modf(abs(direction * angle2 + phi0 - tangent_angle2) / (2 * np.pi))[0] - 0.5) > 0.5 - deviation:
tangent_point = tangent_point2
hudu = angle2
elif abs(modf(abs(direction * (2 * np.pi - angle2) + phi0 - tangent_angle2) / (2 * np.pi))[
0] - 0.5) > 0.5 - deviation:
tangent_point = tangent_point2
hudu = 2 * np.pi - angle2
# 返回 旋转方向 弧度 切点坐标 圆心坐标
return direction, hudu, (float(tangent_point.x.evalf()), float(tangent_point.y.evalf())), (
float(center.x.evalf()), float(center.y.evalf()))
from sympy import Point, Circle, Line, var
import matplotlib.pyplot as plt
var('t')
c1 = Circle(Point(0, 0), 2)
c2 = Circle(Point(4, 4), 3)
l1 = Line(c1.center, c2.center)
p1 = l1.arbitrary_point(t).subs({t: -c1.radius / (c2.radius - c1.radius)})
p2 = l1.arbitrary_point(t).subs({t: c1.radius / (c1.radius + c2.radius)})
t1 = c1.tangent_lines(p1)
t2 = c1.tangent_lines(p2)
ta = t1 + t2
fig = plt.gcf()
ax = fig.gca()
ax.set_xlim((-10, 10))
ax.set_ylim((-10, 10))
ax.set_aspect(1)
cp1 = plt.Circle((c1.center.x, c1.center.y), c1.radius, fill = False)
cp2 = plt.Circle((c2.center.x, c2.center.y), c2.radius, fill = False)
tp = [0 for i in range(4)]
for i in range(4):
start = ta[i].arbitrary_point(t).subs({t:-10})
end = ta[i].arbitrary_point(t).subs({t:10})
tp[i] = plt.Line2D([start.x, end.x], [start.y, end.y], lw = 2)
ax.add_artist(cp1)
ax.add_artist(cp2)
def __init__(self):
self.s, self.t, self.x, self.y, self.z = symbols('s,t,x,y,z')
self.stack = []
self.defs = {}
self.mode = 0
self.hist = [('', [])] # Innehåller en lista med (kommandorad, stack)
self.lastx = ''
self.clear = True
self.op0 = {
's': lambda: self.s,
't': lambda: self.t,
'x': lambda: self.x,
'y': lambda: self.y,
'z': lambda: self.z,
'oo': lambda: S('oo'),
'inf': lambda: S('oo'),
'infinity': lambda: S('oo'),
'?': lambda: self.help(),
'help': lambda: self.help(),
'hist': lambda: self.history(),
'history': lambda: self.history(),
'sketch': lambda: self.sketch(),
}
self.op1 = {
'radians':
lambda x: pi / 180 * x,
'sin':
lambda x: sin(x),
'cos':
lambda x: cos(x),
'tan':
lambda x: tan(x),
'sq':
lambda x: x**2,
'sqrt':
lambda x: sqrt(x),
'ln':
lambda x: ln(x),
'exp':
lambda x: exp(x),
'log':
lambda x: log(x),
'simplify':
lambda x: simplify(x),
'polynom':
lambda x: self.polynom(x),
'inv':
lambda x: 1 / x,
'chs':
lambda x: -x,
'center':
lambda x: x.center,
'radius':
lambda x: x.radius,
'expand':
lambda x: x.expand(),
'factor':
lambda x: x.factor(),
'incircle':
lambda x: x.incircle,
'circumcircle':
lambda x: x.circumcircle,
'xdiff':
lambda x: x.diff(self.x),
'ydiff':
lambda x: x.diff(self.y),
'xint':
lambda x: x.integrate(self.x),
'xsolve':
lambda x: solve(x, self.x),
'xapart':
lambda x: apart(x, self.x),
'xtogether':
lambda x: together(x, self.x),
'N':
lambda x: N(x),
'info':
lambda x:
[x.__class__.__name__, [m for m in dir(x) if m[0] != '_']],
}
self.op2 = {
'+': lambda x, y: y + x,
'-': lambda x, y: y - x,
'*': lambda x, y: y * x,
'/': lambda x, y: y / x,
'**': lambda x, y: y**x,
'item': lambda x, y: y[x],
'point': lambda x, y: Point(y, x),
'line': lambda x, y: Line(y, x),
'circle': lambda x, y: Circle(y, x),
'tangent_lines': lambda x, y: y.tangent_lines(x),
'intersection': lambda x, y: intersection(x, y),
'perpendicular_line': lambda x, y: y.perpendicular_line(x),
'diff': lambda x, y: y.diff(x),
'int': lambda x, y: y.integrate(x),
'solve': lambda x, y: solve(y, x),
'apart': lambda x, y: apart(y, x),
'together': lambda x, y: together(y, x),
'xeval': lambda x, y: y.subs(self.x, x),
}
self.op3 = {
'triangle': lambda x, y, z: Triangle(x, y, z),
'limit': lambda x, y, z: limit(
z, y, x), # limit(sin(x)/x,x,0) <=> x sin x / x 0 limit
'eval': lambda x, y, z: z.subs(y, x),
}
self.op4 = {
'sum': lambda x, y, z, t: Sum(t, (z, y, x)).doit(
) # Sum(1/x**2,(x,1,oo)).doit() <=> 1 x x * / x 1 oo sum
}
self.lastx = ''