def partition_trajectory(trajectory_line_segs, partition_cost_func,
no_partition_cost_func):
if len(trajectory_line_segs) < 1:
raise ValueError
low = 0
length = 1
partition_points = [0]
last_pt = trajectory_line_segs[len(trajectory_line_segs) - 1].end
trajectory_line_segs.append(LineSegment(last_pt, last_pt))
for high in range(2, len(trajectory_line_segs)):
partition_line = LineSegment(trajectory_line_segs[low].start,
trajectory_line_segs[high].start)
# , trajectory_line_segs[high].as_dict(), partition_cost_func(trajectory_line_segs, low, high), no_partition_cost_func(trajectory_line_segs, low, high))
print("mm", low, high, trajectory_line_segs[low].as_dict(),
trajectory_line_segs[high].as_dict(),
partition_cost_func(trajectory_line_segs, low, high),
no_partition_cost_func(trajectory_line_segs, low, high))
if trajectory_line_segs[high - 2].unit_vector\
.almost_equals(trajectory_line_segs[high - 1].unit_vector):
continue
elif trajectory_line_segs[high].start.almost_equals(trajectory_line_segs[low].start) or \
partition_cost_func(trajectory_line_segs, low, high) > \
no_partition_cost_func(trajectory_line_segs, low, high):
print("ll", low, high, trajectory_line_segs[low].as_dict(),
trajectory_line_segs[high].as_dict())
partition_points.append(high - 1)
low = high - 1
partition_points.append(len(trajectory_line_segs) - 1)
print("PARTITION_POINTS", [p for p in partition_points])
return partition_points
def test_line_segment_generation():
LineSegment()
with pytest.raises(ValueError):
LineSegment(Point(0, 0, 0), Point(0, 0, 0))
LineSegment(Point(1, 1, 1), Point(2, 2, 2))
def test_ray_intersection():
"""Check for intersection of a ray with line segments."""
ray = Ray(Point(0, 0), Point(1, 1))
line_segment_1 = LineSegment(Point(5, 5), Point(4, 6))
assert ray.intersects(line_segment_1)
assert not ray.properly_intersects(line_segment_1)
line_segment_2 = LineSegment(Point(1, 2), Point(2, 1))
assert ray.intersects(line_segment_2)
assert ray.properly_intersects(line_segment_2)
line_segment_3 = LineSegment(Point(2, 1), Point(4, 2))
assert not ray.intersects(line_segment_3)
assert not ray.properly_intersects(line_segment_3)
def call_partition_trajectory(trajectory_point_list):
if len(trajectory_point_list) < 2:
raise ValueError
def encoding_cost_func(trajectory_line_segs, low, high, partition_line):
return encoding_cost(trajectory_line_segs,
low,
high,
partition_line=partition_line,
angular_dist_func=angular_distance,
perpendicular_dist_func=perpendicular_distance)
def partition_cost_func(trajectory_line_segs, low, high):
return partition_cost(trajectory_line_segs,
low,
high,
model_cost_func=model_cost,
encoding_cost_func=encoding_cost_func)
#convert points in the trajectory_point_list into LineSegment(point[i],point[i+1])
trajectory_line_segs = map(
lambda i: LineSegment(trajectory_point_list[i], trajectory_point_list[
i + 1]), range(0,
len(trajectory_point_list) - 1))
return partition_trajectory(trajectory_line_segs=trajectory_line_segs,
partition_cost_func=partition_cost_func,
no_partition_cost_func=no_partition_cost)
def update_scooper_position(self):
scooper_s = LineSegment(Point([0, 0]), Point([self.scooper_length, 0]))
scooper_a = scooper_s.transform(
get_transform(self.scooper_angle, [self.arm_length, 0]))
self.scooper_segment = scooper_a.transform(
get_transform(self.arm_angle, [0, self.center_height]))
self.scooper_p_end = self.scooper_segment.p2
def test_contains_point():
ls1 = LineSegment(Point(1, 1, 1), Point(2, 2, 2))
assert ls1.contains_point(Point(1.2, 1.2, 1.2))
assert not ls1.contains_point(Point(0.5, 0.5, 0.5))
assert ls1.contains_point(Point(1, 1, 1))
assert ls1.contains_point(Point(2, 2, 2))
assert not ls1.contains_point(Point(0, 1, 0))
def test_algorithms(self, path, polygon, s, t, pictures):
"""Run all algorithms on the given polygon and check that the results are equal."""
assert isinstance(polygon, Polygon)
signal.signal(signal.SIGALRM, timeout)
squared_timeout = ceil(0.001 * polygon.len**2)
try:
signal.alarm(squared_timeout)
trapezoid_path = list(trapezoid_shortest_path(polygon, s, t))
signal.alarm(0)
except TimeoutError:
raise TimeoutError(
'Trapezoid calculation took longer than {0}s!'.format(
squared_timeout))
try:
signal.alarm(squared_timeout)
lee_preparata_path = list(
lee_preparata_shortest_path(polygon, s, t))
signal.alarm(0)
except TimeoutError:
raise TimeoutError(
'Lee-Preparata calculation took to longer than {0}s!'.format(
squared_timeout))
if trapezoid_path != lee_preparata_path:
if pictures:
save_polygon(path, polygon, trapezoid_path, lee_preparata_path,
s, t)
assert trapezoid_path == lee_preparata_path
for path_ix in range(len(trapezoid_path) - 1):
for edge_ix in range(len(polygon)):
if polygon.edge(edge_ix).properly_intersects(
LineSegment(trapezoid_path[path_ix],
trapezoid_path[path_ix + 1])):
if pictures:
save_polygon(path, polygon, trapezoid_path,
lee_preparata_path, s, t)
assert not polygon.edge(edge_ix).properly_intersects(
LineSegment(trapezoid_path[path_ix],
trapezoid_path[path_ix + 1]))
def partition_cost(trajectory_line_segs, low, high, model_cost_func, encoding_cost_func):
if low >= high:
raise IndexError
partition_line = LineSegment(trajectory_line_segs[low].start,
trajectory_line_segs[high].start)
model_cost = model_cost_func(partition_line)
encoding_cost = encoding_cost_func(trajectory_line_segs, low, high, partition_line)
return model_cost + encoding_cost
def encoding_cost(self, start, end):
self.check_indice_args(start, end)
approximation_line = LineSegment(self.points[start], self.points[end])
total_perp = 0.0
total_angular = 0.0
for i in xrange(start, end):
line_seg = LineSegment(self.points[i], self.points[i + 1])
total_perp += perpendicular_distance(approximation_line, line_seg)
total_angular += angular_distance(approximation_line, line_seg)
if total_perp < 1.0:
total_perp = 1.0
if total_angular < 1.0:
total_angular = 1.0
return math.log(total_perp, 2) + math.log(total_angular, 2)
def checkLoS(self, points):
"""Determine line of sight between all points in the list by constructing a line segment for the two points
in question and comparing it for intersection with line segments in the BSP tree
:param points: a list of Point objects
:return: a list of lists, n by n, an entry at [i][j] tells wether point i and point j have line of sight with each other
"""
LoS = []
for point in points:
LoS.append(['X'] * len(points))
for FromIndex, FromPoint in enumerate(points):
for ToIndex, ToPoint in enumerate(points):
# if LoS is not determined
if (FromIndex != ToIndex) and (LoS[FromIndex][ToIndex] == 'X'):
# Assume there is LoS
LoS[FromIndex][ToIndex] = 'T'
LoS[ToIndex][FromIndex] = 'T'
SightSegment = LineSegment(
points[FromIndex], points[ToIndex])
# Point to root node
stack = [self.tree]
IsIntersection = False
# NumOfIntersections = 0
NumOfTraversals = 0
while len(stack) != 0 and IsIntersection == False:
TreePointer = stack.pop()
NumOfTraversals += 1
compareLoS = TreePointer.data[0].compare(SightSegment)
if compareLoS == 'P':
if SightSegment.split(TreePointer.data[0]) is not None:
LoS[FromIndex][ToIndex] = 'F'
LoS[ToIndex][FromIndex] = 'F'
else:
if TreePointer.left is not None:
stack.append(TreePointer.left)
if TreePointer.right is not None:
stack.append(TreePointer.right)
elif compareLoS == 'F':
if TreePointer.left is not None:
stack.append(TreePointer.left)
elif compareLoS == 'B':
if TreePointer.right is not None:
stack.append(TreePointer.right)
distance = points[FromIndex].getDistance(points[ToIndex])
if IsIntersection:
print(('Distance: %0.1f' % distance) +
', # of traversals(F): ' + str(NumOfTraversals))
else:
print(('Distance: %0.1f' % distance) +
', # of traversals(T): ' + str(NumOfTraversals))
return LoS
def readLinesFromFile(self, filename):
"""Not in use currently"""
with open(filename, 'r') as f:
for line in f.readlines():
if line[0] != '#':
data = [x for x in line.split('\t')]
points = [int(x) for x in data[0].split(',')]
self.tree.data.append(LineSegment(Point(points[0], points[1]), Point(
points[2], points[3]), int(data[1]), data[2][0:len(data[2]) - 1]))
def line_intersects_barriers(self, point1, point2):
"""
Given two points, it checks if the line created by those points intersect any map barrier
"""
line = LineSegment(point1, point2)
for barrier in self.barriers:
if barrier.intersects(line):
return True
return False
def get_key_back_egde(p, angle):
# back top of the key
p0 = point_at_distance(p, KEYCAP_BOX['y'] / 2, angle - 90)
# above back top of the key
p1 = point_at_distance(p0, -KEY_INTERSECTION_CLEARANCE, angle)
# back bottom of the key
p2 = point_at_distance(p1,
(KEY_INTERSECTION_CLEARANCE * 10) + KEYCAP_BOX['z'],
angle)
return LineSegment(p1, p2)
def get_key_front_egde(p, angle):
# front top of the key
p0 = point_at_distance(p, KEYCAP_BOX['y'] / 2, angle + 90)
# above front top of the key
p1 = point_at_distance(p0, -KEY_INTERSECTION_CLEARANCE, angle)
# below front bottom of the key
p2 = point_at_distance(p1,
(KEY_INTERSECTION_CLEARANCE * 3) + KEYCAP_BOX['z'],
angle)
return LineSegment(p1, p2)
def test_quad_tree_load_test(self):
quad_tree = QuadTree(BoundingBox(0, 100, 0, 100))
points_bounding_box = BoundingBox(-10, 110, -10, 110)
for i in range(200):
point1 = QuadTreeTests.random_point(points_bounding_box)
point2 = QuadTreeTests.random_point(points_bounding_box)
while point1 == point2:
point2 = QuadTreeTests.random_point(points_bounding_box)
quad_tree.add(LineSegment(point1, point2))
self.assertTrue(
type(quad_tree.get(BoundingBox(10, 90, 10, 90))) is list)
def update_ground_line(self):
self.circle = Circle(
self.center_p, self.scooper_p_end.get_distance_with(self.center_p))
self.left_p_old = self.left_p_new
self.right_p_old = self.right_p_new
self.left_p_new = self.circle.get_intersection_with(self.left_segment0)
self.right_p_new = self.circle.get_intersection_with(
self.right_segment0)
if self.left_p_new and self.right_p_new:
self.left_segment0 = LineSegment(self.left_inf_p, self.left_p_new)
self.right_segment0 = LineSegment(self.right_inf_p,
self.right_p_new)
self.left_segment1 = LineSegment(self.left_p_new, self.left_p_old)
self.right_segment1 = LineSegment(self.right_p_new,
self.right_p_old)
self.middle_arc_old = self.middle_arc_new
v1 = Vector(self.center_p, self.left_p_new)
v2 = Vector(self.center_p, self.right_p_new)
self.middle_arc_new = Arc(self.circle.p, self.circle.r,
np.rad2deg(v1.angle),
np.rad2deg(v2.angle))
else:
self.plot_in_ground_flag = True
self.left_segment1 = LineSegment(
self.left_segment0.p2,
Point([-self.circle.r, self.center_height]))
self.right_segment1 = LineSegment(
self.right_segment0.p2,
Point([self.circle.r, self.center_height]))
self.middle_arc_old = self.middle_arc_new
self.middle_arc_new = Arc(self.circle.p, self.circle.r, 180, 360)
def get_nearest_point_in_line_segs_to_point(point):
min_dist_line_seg = None
min_square_dist = None
for other_traj in all_trajectories:
if other_traj.id != traj.id:
for other_pt in get_all_points_of_trajectory(other_traj):
temp_square_dist = math.pow(other_pt.x - point.x, 2) + \
math.pow(other_pt.y - point.y, 2)
if min_square_dist == None or temp_square_dist < min_square_dist:
min_square_dist = temp_square_dist
min_dist_line_seg = LineSegment(point, other_pt)
return min_dist_line_seg
class ROBOT:
outline = Box(Vector(0.6 * M, 0.5 * M))
_armor_length = 0.14 * M
armor_lines = [
LineSegment(*y_mirrors(Vector(outline.dims.x / 2, _armor_length / 2))),
LineSegment(*x_mirrors(Vector(_armor_length / 2, outline.dims.y / 2))),
LineSegment(*x_mirrors(Vector(_armor_length / 2, -outline.dims.y /
2))),
LineSegment(*y_mirrors(Vector(-outline.dims.x / 2, _armor_length / 2)))
]
armor_damages = [20, 40, 40, 60]
_drive_radius = 0.3 * M
_factor = 1 / (math.sqrt(2) * _drive_radius)
drive_config = MotionConfig(3 * MS, 6 * MS2, 0.1 * MS2)
rotation_config = MotionConfig(_factor * drive_config.top_speed,
_factor * drive_config.top_accel,
_factor * drive_config.friction_decel)
gimbal_yaw_config = MotionConfig(300 * DS, 1000 * DS2, 10 * DS2)
zero_tolerance = 0.001
rebound_coeff = 0.4
shot_cooldown = 0.1 * S
def test_ray_intersection_implications(first_level, second_level, limit):
"""Check for intersection of a ray with line segments and its implications."""
for _ in range(first_level):
ray = Ray(
Point(random.uniform(-limit, limit), random.uniform(-limit,
limit)),
Point(random.uniform(-limit, limit), random.uniform(-limit,
limit)))
for _ in range(second_level):
line_segment = LineSegment(
Point(random.uniform(-limit, limit),
random.uniform(-limit, limit)),
Point(random.uniform(-limit, limit),
random.uniform(-limit, limit)))
assert (not ray.properly_intersects(line_segment)) or (
ray.intersects(line_segment))
def get_ls_list(pts):
# emptyness check
if not pts:
raise ValueError("pts doesn't have any values")
# size check
if len(pts) < 2:
raise ValueError("pts didn't have at least two points")
ls_list = []
last_pt = None
for pt in pts:
if last_pt is not None:
ls = LineSegment(last_pt, pt)
ls_list.append(ls)
last_pt = pt
return ls_list
def partition_trajectory(trajectory_line_segs, partition_cost_func,
no_partition_cost_func):
if len(trajectory_line_segs) < 1:
raise ValueError
low = 0
partition_points = [0]
last_pt = trajectory_line_segs[len(trajectory_line_segs) - 1].end
trajectory_line_segs.append(LineSegment(last_pt, last_pt))
for high in range(2, len(trajectory_line_segs)):
if trajectory_line_segs[high - 2].unit_vector\
.almost_equals(trajectory_line_segs[high - 1].unit_vector):
continue
elif trajectory_line_segs[high].start.almost_equals(trajectory_line_segs[low].start) or \
partition_cost_func(trajectory_line_segs, low, high) > \
no_partition_cost_func(trajectory_line_segs, low, high):
partition_points.append(high - 1)
low = high - 1
partition_points.append(len(trajectory_line_segs) - 1)
return partition_points
def test_algorithms(self, path, polygon, s, t, pictures, ignore_delaunay):
"""Run all algorithms on the given polygon and check that the results are equal."""
assert isinstance(polygon, Polygon)
signal.signal(signal.SIGALRM, timeout)
s_point = polygon.point_inside_at(s)
t_point = polygon.point_inside_at(t)
cubed_timeout = ceil(0.001 * polygon.len**3)
squared_timeout = ceil(0.001 * polygon.len**2)
delaunay_path = None
if not ignore_delaunay:
signal.alarm(cubed_timeout)
try:
delaunay_path = list(
delaunay_shortest_path(polygon, s_point, t_point))
signal.alarm(0)
except TimeoutError:
raise TimeoutError(
'Delaunay calculation took longer than {0}s!'.format(
cubed_timeout))
signal.alarm(squared_timeout)
try:
trapezoid_path = list(
trapezoid_shortest_path(polygon, s_point, t_point))
signal.alarm(0)
except TimeoutError:
raise TimeoutError(
'Trapezoid calculation took longer than {0}s!'.format(
squared_timeout))
signal.alarm(squared_timeout)
try:
makestep_path = list(
makestep_shortest_path(polygon, s_point, t_point))
signal.alarm(0)
except TimeoutError:
raise TimeoutError(
'Makestep calculation took to longer than {0}s!'.format(
squared_timeout))
if (not delaunay_path == trapezoid_path == makestep_path
and delaunay_path
is not None) or (not trapezoid_path == makestep_path):
if pictures:
save_polygon(path, polygon, trapezoid_path, delaunay_path,
makestep_path, s, t)
assert delaunay_path == trapezoid_path
assert delaunay_path == makestep_path
assert makestep_path == trapezoid_path
for path_ix in range(len(trapezoid_path) - 1):
for edge_ix in range(len(polygon)):
if polygon.edge(edge_ix).properly_intersects(
LineSegment(trapezoid_path[path_ix],
trapezoid_path[path_ix + 1])):
if pictures:
save_polygon(path, polygon, trapezoid_path,
delaunay_path, makestep_path, s, t)
assert not polygon.edge(edge_ix).properly_intersects(
LineSegment(trapezoid_path[path_ix],
trapezoid_path[path_ix + 1]))
def test_length():
ls1 = LineSegment(Point(1, 0, 0), Point(2, 0, 0))
ls2 = LineSegment(Point(3, 0, 0), Point(0, 4, 0))
assert ls1.length() == 1
assert ls2.length() == 5
def step(self):
old_center = self.center.copy()
self.center += self.speed
return LineSegment(old_center, self.center)
def get_line_segment_from_points(point_a, point_b):
return LineSegment(point_a, point_b)
def test_algorithms(path, polygon, s, t, pictures):
"""Run all algorithms on the given polygon and check that the results are equal."""
timeout = 60
name = str(abs(hash(repr(polygon))))
file_name = os.path.join(path, name)
try:
signal.alarm(timeout)
delaunay_path = list(
delaunay_shortest_path(polygon, Point(*s.tuple()),
Point(*t.tuple())))
except TimeoutError:
print('\nDelaunay calculation took longer than {0}s!'.format(timeout))
return
except NotInGeneralPositionException:
print('\nPolygon not in general position!')
return
except BaseException:
save_polygon(file_name, polygon)
with open(file_name + '.exception', 'w') as f:
print('Delaunay-Exception', file=f)
print('{0} -> {1}\n'.format(s, t), file=f)
traceback.print_exc(file=f)
traceback.print_exc()
return
finally:
signal.alarm(0)
try:
signal.alarm(timeout)
lee_preparata_path = list(
lee_preparata_shortest_path(polygon, Point(*s.tuple()),
Point(*t.tuple())))
except TimeoutError:
print('Lee-Preparata calculation took to longer than {0}s!'.format(
timeout))
return
except BaseException:
save_polygon(file_name, polygon)
with open(file_name + '.exception', 'w') as f:
print('Lee-Preparata-Exception', file=f)
print('{0} -> {1}\n'.format(s, t), file=f)
traceback.print_exc(file=f)
traceback.print_exc()
return
finally:
signal.alarm(0)
error = False
if delaunay_path != lee_preparata_path:
if pictures:
save_polygon(file_name, polygon, delaunay_path, lee_preparata_path,
s.index, t.index)
print('\nFailed: {path}, {s} -> {t}'.format(path=name,
s=s.index,
t=t.index))
error = True
if not error:
for path_ix in range(len(delaunay_path) - 1):
for edge_ix in range(len(polygon)):
if polygon.edge(edge_ix).properly_intersects(
LineSegment(delaunay_path[path_ix],
delaunay_path[path_ix + 1])):
if pictures:
save_polygon(file_name, polygon, delaunay_path,
lee_preparata_path, s, t)
print('\nFailed: {path}, {s} -> {t}'.format(path=name,
s=s.index,
t=t.index))
error = True
break
if error:
break
print('.', end='', flush=True)
def test_quad_tree_smoke_test(self):
quad_tree = QuadTree(BoundingBox(0, 0, 100, 100))
quad_tree.add(LineSegment(Point(10, 10), Point(20, 20)))
self.assertEqual(quad_tree.get(BoundingBox(10, 90, 10, 90)),
[LineSegment(Point(10, 10), Point(20, 20))])
def __init__(self):
config_file_path = os.path.join(os.path.dirname(__file__),
'config/param_moon.json')
with open(config_file_path) as config_file:
param = json.load(config_file)
param = dict([(str(k), v) for k, v in param.items()])
self.state = 0
self.arm_angle = 359
self.arm_length = param["arm_length"]
self.arm_speed = 0
self.scooper_angle = 45
self.scooper_length = param["tool_length"]
self.tool_depth = 0
self.tool_angle = 0
self.center_height = 2.5
self.center_height_step = 0.02
self.excavator_force = ExcavationForce(param)
self.scooper_segment = None # 1
self.origin_p = Point([0, 0])
self.left_inf_p = Point([-10, 0])
self.right_inf_p = Point([10, 0])
self.left_segment0 = LineSegment(self.left_inf_p, self.origin_p)
self.left_segment1 = LineSegment(self.left_inf_p, self.origin_p)
self.middle_arc_old = None
self.middle_arc_new = None
self.right_segment1 = LineSegment(self.origin_p, self.right_inf_p)
self.right_segment0 = LineSegment(self.origin_p, self.right_inf_p)
self.circle = None
self.left_segment1_temp = None
self.middle_arc_temp_old = None
self.middle_arc_temp_new = None
self.right_segment1_temp = None
self.left_p_new = self.origin_p
self.left_p_old = self.origin_p
self.right_p_new = self.origin_p
self.right_p_old = self.origin_p
self.center_p = Point([0, self.center_height])
self.scooper_p_end = None
self.fig = plt.figure()
self.ax = plt.gca()
self.ax.set_aspect('equal')
plt.show(block=False)
plt.draw()
self.plot_middle_arc_temp_old = None
self.plot_middle_arc_temp_new = None
self.plot_left_segment1_temp = None
self.plot_right_segment1_temp = None
self.plot_circle = None
self.plot_left_segment0 = None
self.plot_right_segment0 = None
self.plot_scooper_segment = None
self.plot_update_flag = False
self.plot_in_ground_flag = False
def line_segment(x1, y1, x2, y2):
"""Create a line segment from four points."""
return LineSegment(Point(x1, y1), Point(x2, y2))
def update_scooper_intersection(self):
if self.state == 0:
self.tool_depth = 0
self.tool_angle = 0
if not self.plot_update_flag:
self.left_segment1_temp = self.left_segment1
self.middle_arc_temp_old = self.middle_arc_old
self.middle_arc_temp_new = None
self.right_segment1_temp = self.right_segment1
else:
self.left_segment1_temp = None
self.middle_arc_temp_old = None
self.middle_arc_temp_new = self.middle_arc_new
self.right_segment1_temp = None
elif self.state == 1: # only intersect with left_segment1
p = self.left_segment1.get_intersection_with(self.scooper_segment)
self.tool_depth = p.get_distance_with(self.scooper_p_end)
self.tool_angle = abs(self.scooper_segment.angle)
self.left_segment1_temp = LineSegment(p, self.left_p_old)
self.middle_arc_temp_old = self.middle_arc_old
v = Vector(self.middle_arc_new.p, p)
self.middle_arc_temp_new = Arc(self.middle_arc_new.p,
self.middle_arc_new.r,
self.middle_arc_new.deg_angle1,
np.rad2deg(v.angle))
self.right_segment1_temp = self.right_segment1
elif self.state == 2: # only intersect with middle_arc_old
p = self.middle_arc_old.get_intersection_with(self.scooper_segment)
self.tool_depth = p.get_distance_with(self.scooper_p_end)
self.tool_angle = 90 - self.scooper_angle
self.left_segment1_temp = None
v1 = Vector(self.middle_arc_old.p, p)
self.middle_arc_temp_old = Arc(self.middle_arc_old.p,
self.middle_arc_old.r,
np.rad2deg(v1.angle),
self.middle_arc_old.deg_angle2)
v2 = Vector(self.middle_arc_new.p, self.scooper_p_end)
self.middle_arc_temp_new = Arc(self.middle_arc_new.p,
self.middle_arc_new.r,
self.middle_arc_new.deg_angle1,
np.rad2deg(v2.angle))
self.right_segment1_temp = self.right_segment1
elif self.state == 3: # intersect with right_segment1
if self.middle_arc_old is not None and self.middle_arc_old.has_intersection_with(
self.scooper_segment):
p1 = self.middle_arc_old.get_intersection_with(
self.scooper_segment)
p2 = self.right_segment1.get_intersection_with(
self.scooper_segment)
self.tool_depth = p1.get_distance_with(p2)
self.tool_angle = 90 - self.scooper_angle
self.left_segment1_temp = None
v1 = Vector(self.middle_arc_old.p, p1)
self.middle_arc_temp_old = Arc(self.middle_arc_old.p,
self.middle_arc_old.r,
np.rad2deg(v1.angle),
self.middle_arc_old.deg_angle2)
self.middle_arc_temp_new = self.middle_arc_new
self.right_segment1_temp = LineSegment(self.right_p_old, p2)
else:
p = self.right_segment1.get_intersection_with(
self.scooper_segment)
self.tool_depth = p.get_distance_with(self.scooper_p_end)
self.tool_angle = abs(self.scooper_segment.angle)
self.left_segment1_temp = None
self.middle_arc_temp_old = None
v2 = Vector(self.center_p, self.scooper_p_end)
self.middle_arc_temp_new = Arc(self.middle_arc_new.p,
self.middle_arc_new.r,
self.middle_arc_new.deg_angle1,
np.rad2deg(v2.angle))
self.right_segment1_temp = LineSegment(p, self.right_p_new)
else:
self.tool_depth = 0