def __init__(self, left0, left1, right0, right1, planeNo, wireNo, trueWireNo): self.leftBound = Line(left0, left1) self.rightBound = Line(right0, right1) self.planeNo = planeNo self.wireNo = wireNo self.trueWireNo = trueWireNo
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 slice_file(image_position_patient_array, image_orientation_patient, output_path=None, input_path=None): print("Status: Loading File.") model = STLModel(input_path) stats = model.stats() print(stats) print("Status: Calculating Slices") v1 = Point3D(image_orientation_patient[0][0], image_orientation_patient[0][1], image_orientation_patient[0][2]) v2 = Point3D(image_orientation_patient[1][0], image_orientation_patient[1][1], image_orientation_patient[1][2]) org = Point3D(0, 0, 0) for i, slice_loc in enumerate(image_position_patient_array): slice_loc = Point3D(slice_loc[0], slice_loc[1], slice_loc[2]) dwg = Drawing(output_path + str(i) + '.svg', profile='tiny') plane = Plane(v1 + slice_loc, v2 + slice_loc, org + slice_loc) x_axis = Line(org + slice_loc, v1 + slice_loc) y_axis = Line(org + slice_loc, v2 + slice_loc) pairs = model.slice_at_plane(plane, x_axis, y_axis) for pair in pairs: dwg.add(dwg.line(pair[0], pair[1], stroke=rgb(0, 0, 0, "%"))) dwg.save() print("Status: Finished Outputting Slices")
def reflect(self, line): from sympy import atan, Line, Point, Dummy, oo g = self l = line o = Point(0, 0) if l == Line(o, slope=0): return g.scale(y=-1) elif l == Line(o, slope=oo): return g.scale(-1) if not hasattr(g, 'reflect') and not all( isinstance(arg, Point) for arg in g.args): raise NotImplementedError a = atan(l.slope) c = l.coefficients d = -c[-1] / c[1] # y-intercept # apply the transform to a single point x, y = Dummy(), Dummy() xf = Point(x, y) xf = xf.translate(y=-d).rotate(-a, o).scale(y=-1).rotate(a, o).translate(y=d) # replace every point using that transform reps = [(p, xf.xreplace({x: p.x, y: p.y})) for p in g.atoms(Point)] return g.xreplace(dict(reps))
def calibrate(self, width, height, top_left, top_right, bottom_left, bottom_right): top_left = self.estimate_average(top_left) top_right = self.estimate_average(top_right) bottom_left = self.estimate_average(bottom_left) bottom_right = self.estimate_average(bottom_right) vertical_left = Line(bottom_left, top_left) vertical_right = Line(bottom_right, top_right) horizontal_top = Line(top_left, top_right) horizontal_bottom = Line(bottom_left, bottom_right) self.abscissa_hinge, = vertical_left.intersection(vertical_right) self.ordinate_hinge, = horizontal_top.intersection(horizontal_bottom) abscissa_low, = vertical_left.intersection(abscissa) abscissa_high, = vertical_right.intersection(abscissa) ordinate_low, = horizontal_bottom.intersection(ordinate) ordinate_high, = horizontal_top.intersection(ordinate) self.to_width = lambda x: width / 4 + width / 2 * ( x - abscissa_low.x) / (abscissa_high.x - abscissa_low.x) self.to_height = lambda y: height / 4 + height / 2 * ( y - ordinate_low.y) / (ordinate_high.y - ordinate_low.y)
def Tangent_lines(circle_C, point_P): # 圆外一点到圆的切线, # 返回从point到切点的line R = float(circle_C.radius.evalf()) circle = [ float(circle_C.center.x.evalf()), float(circle_C.center.y.evalf()) ] point = [float(point_P.x.evalf()), float(point_P.y.evalf())] circle_point_angle = np.arctan2(point[1] - circle[1], point[0] - circle[0]) cos = R / np.sqrt(np.sum((np.array(circle) - np.array(point))**2)) hudu_half = np.arccos(cos) tangent_angle1 = circle_point_angle + hudu_half tangent_point1 = Point(circle[0] + R * np.cos(tangent_angle1), circle[1] + R * np.sin(tangent_angle1)) tangent_angle2 = circle_point_angle - hudu_half tangent_point2 = Point(circle[0] + R * np.cos(tangent_angle2), circle[1] + R * np.sin(tangent_angle2)) return [ Line(Point(point), Point(tangent_point1)), Line(Point(point), Point(tangent_point2)) ]
def locate_puzzle(img, debug=False): # Blurr image to reduce noise in edges thresh = cv.GaussianBlur(img, (3,3), cv.BORDER_REFLECT) param1, param2 = 15, 20 while True: # Canny edge detection edges = cv.Canny(thresh, param1, param2, None, 3) # Dilate and erode edges (from https://stackoverflow.com/questions/48954246/find-sudoku-grid-using-opencv-and-python) edges = cv.dilate(edges, np.ones((3,3),np.uint8), iterations=1) edges = cv.erode(edges, np.ones((5,5),np.uint8), iterations=1) lines = cv.HoughLines(edges, 1, np.pi / 180, 150, None, 0, 0) param1, param2 = param1+5, param2+30 if lines is None: continue if len(lines) <= 35: break # Copy edges to the images that will display the results in BGR cdst = cv.cvtColor(edges, cv.COLOR_GRAY2BGR) # Find four corners of sudoku board poss_corners = [] for i in range(len(lines)): rho = lines[i][0][0] theta = lines[i][0][1] pt1, pt2 = polar_to_points(rho, theta) cv.line(cdst, pt1, pt2, (0,0,255), 3, cv.LINE_AA) p1, p2 = Point(pt1), Point(pt2) l1 = Line(p1, p2) for j in range(len(lines)): if i == j: continue rho = lines[j][0][0] theta = lines[j][0][1] p3, p4 = polar_to_points(rho, theta, return_class=True) l2 = Line(p3, p4) p = l1.intersection(l2) if len(p) == 0: continue p = np.array([int(p[0][1]), int(p[0][0])]) if 0 <= p[0] < len(img) and 0 <= p[1] < len(img[1]): poss_corners.append(p) tl = poss_corners[np.argmin(np.array(list(map(euclidean, poss_corners, [[0,0]]*len(poss_corners)))))] tr = poss_corners[np.argmin(np.array(list(map(euclidean, poss_corners, [[0,len(img[0])]]*len(poss_corners)))))] bl = poss_corners[np.argmin(np.array(list(map(euclidean, poss_corners, [[len(img),0]]*len(poss_corners)))))] br = poss_corners[np.argmin(np.array(list(map(euclidean, poss_corners, [[len(img),len(img[0])]]*len(poss_corners)))))] # Idea for warping using imutils: https://www.pyimagesearch.com/2020/08/10/opencv-sudoku-solver-and-ocr/ img = four_point_transform(img, np.array([ [tl[1],tl[0]], [tr[1],tr[0]], [bl[1],bl[0]], [br[1],br[0]]])) if debug: cv.imshow("Lines", cdst) cv.imshow("Cropped", img) cv.waitKey() return img
def extendExWall(P0, P1, l1, msp): l0 = Line(p0, p1) intersectedPoint = l0.intersection(l1) xE, yE = intersectedPoint xS, yS = p1 msp.add_line(((xS, yS, 0), (xE, yE, 0))) l0 = Line(p1, intersectedPoint) return l0
def __init__(self, p0, p1, center): self.p0 = p0 self.p1 = p1 self.center = center self.radius = self.center.distance(self.p0) line1 = Line(self.center, self.p1) line2 = Line(self.center, self.p0) self.central_angle = line1.angle_between(line2)
def __compute_1st_vanishing_points(self): road_l1 = Line(Point(self.p1), Point(self.p2)) road_l2 = Line(Point(self.p3), Point(self.p4)) road_intersection = intersection(road_l1, road_l2) u0 = float(road_intersection[0][0]) v0 = float(road_intersection[0][1]) return u0, v0
def crossing(p1: Point, p2: Point, p3: Point, p4: Point): line1, seg1 = Line(p1, p2), Segment(p1, p2) line2, seg2 = Line(p3, p4), Segment(p3, p4) intersect = line1.intersection(line2) if intersect: seg1 = Segment(p1, p2) seg2 = Segment(p3, p4) pi = intersect[0] return seg1.contains(pi) and seg2.contains(pi)
def __compute_2nd_vanishing_points(self): # perp_l1 = Line(Point(self.p2), Point(self.p3)) perp_l1 = Line(Point(self.p1), Point(self.p4)) # horizon_l = Line(Point(0, self.v0), (1, self.v0)) horizon_l = Line(Point(self.p2), Point(self.p3)) perp_intersection = intersection(perp_l1, horizon_l) u1 = float(perp_intersection[0][0]) return u1
def projection(self, eye1, eye2): target = self.estimate(eye1, eye2) horizontal, = Line(target, self.abscissa_hinge).intersection(abscissa) vertical, = Line(target, self.ordinate_hinge).intersection(ordinate) x = self.to_width(horizontal.x) y = self.to_height(vertical.y) return float(x), float(y)
def get_polygon(left, right, top, bottom): # convert rays to lines left = Line(left.p1, left.p2) right = Line(right.p1, right.p2) top = Line(top.p1, top.p2) bottom = Line(bottom.p1, bottom.p2) top_left = left.intersection(top)[0] top_right = right.intersection(top)[0] bottom_left = left.intersection(bottom)[0] bottom_right = right.intersection(bottom)[0] return Polygon(top_left, top_right, bottom_right, bottom_left)
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) T = Triangle(P1,A,P2) svg.append(T,"T",i) return T.incircle
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 get_intersection_points2D_with_img(intersection_points: list, plane_range: np.ndarray) -> tuple: x, y = intersection_points p1 = Point(x[0], y[0]) p2 = Point(x[1], y[1]) intersection_line = Line(p1, p2) points1 = Point(plane_range[0, 1], plane_range[0, 0]), Point(plane_range[0, 1], plane_range[1, 0]) points2 = Point(plane_range[0, 1], plane_range[1, 0]), Point(plane_range[1, 1], plane_range[1, 0]) points3 = Point(plane_range[1, 1], plane_range[1, 0]), Point(plane_range[1, 1], plane_range[0, 0]) points4 = Point(plane_range[1, 1], plane_range[0, 0]), Point(plane_range[0, 1], plane_range[0, 0]) line1 = Segment(*points1) line2 = Segment(*points2) line3 = Segment(*points3) line4 = Segment(*points4) result = tuple( filter( lambda x: x != [], intersection_line.intersection(line1) + intersection_line.intersection(line2) + intersection_line.intersection(line3) + intersection_line.intersection(line4))) return (float(result[0].x), float(result[0].y)), (float(result[1].x), float(result[1].y))
def test(f, col=30, n=200): for i in range(0, n): points = [] while True: points = generate_test(col) if points[0] is points[1] or points[0][0] is points[0][ 1] or points[1][0] is points[1][1]: continue l = Line(P(points[0][0], points[0][1]), P(points[1][0], points[1][1])) if len(l.intersection(P(points[2][0], points[2][1]))) != 0: continue break if i % 50 == 0 and i != 0: print('passed {} tests'.format(i)) for j in range(0, 2 * len(points)): point = np.random.randint(0, 25, size=(2)) answer = check(points, point) for k in range(0, len(points) - 1): points.insert(len(points), points[0]) points.remove(points[0]) result = f(points, point) if result is answer: continue print("Test №{} failed".format(i + 1)) print("Expected {}, result {}".format(answer, result)) print("points={}".format(points)) print("point={}".format(point)) draw(points, point) return print("All tests passed")
def find_direction_split(self, template, x, nn, pre_nn): samples = polytope.sample(10000, template, np.array(x)) preprocessed = pre_nn(torch.tensor(samples).float()) preprocessed_np = preprocessed.detach().numpy() samples_ontput = torch.softmax(nn(preprocessed), 1) predicted_label = samples_ontput.detach().numpy()[:, 0] y = np.clip(predicted_label, 1e-7, 1 - 1e-7) inv_sig_y = np.log(y / (1 - y)) # transform to log-odds-ratio space from sklearn.linear_model import LinearRegression lr = LinearRegression() lr.fit(samples, inv_sig_y) template_2d: np.ndarray = np.array([Experiment.e(3, 2), Experiment.e(3, 0) - Experiment.e(3, 1)]) def sigmoid(x): ex = np.exp(x) return ex / (1 + ex) preds = sigmoid(lr.predict(samples)) plot_points_and_prediction(samples @ template_2d.T, preds) plot_points_and_prediction(samples @ template_2d.T, predicted_label) coeff = lr.coef_ intercept = lr.intercept_ a = sympy.symbols('x') b = sympy.symbols('y') classif_line1 = Line(coeff[0].item() * a + coeff[1].item() * b + intercept) new_coeff = -coeff[0].item() / coeff[1].item()
def compute_distances_between_two_points_set(points1, points2, check = None) : index = 0 distances = [] i = 0 li = [] while i < len(points1): p1 = points1[i] if index >= len(points2) - 1: break if check: if i < len(points1) - 1: if check(points2[index], points2[index+1], points1[i], points1[i+1]): logging.warn("crossing~") line = Line(points2[index], points2[index + 1]) pro = line.projection(p1) if is_projection_between_two_points(pro, points2[index], points2[index + 1]): distances.append(p1.distance(pro)) li.append([p1, pro]) index += 1 else: if pro.distance(points2[index + 1]) < pro.distance(points2[index]): i -= 1 index += 1 i += 1 return distances, li
def plotline(ax, rwaypoints, width): pointx, pointy = pointtolist(rwaypoints) ax.plot(pointx, pointy) for i in range(len(rwaypoints) - 1): x = [] y = [] if (rwaypoints[i].x == rwaypoints[i + 1].x and rwaypoints[i].y == rwaypoints[i + 1].y): continue else: line = Line(rwaypoints[i], rwaypoints[i + 1]) diracp = line.direction.unit diracn = diracp.rotate(math.pi / 2) rectpt1 = rwaypoints[i].translate( (diracn.x - diracp.x) * width / 2, (diracn.y - diracp.y) * width / 2) rectpt2 = rwaypoints[i + 1].translate( (diracn.x + diracp.x) * width / 2, (diracn.y + diracp.y) * width / 2) rectpt3 = rwaypoints[i + 1].translate( (diracp.x - diracn.x) * width / 2, (diracp.y - diracn.y) * width / 2) rectpt4 = rwaypoints[i].translate( (-diracp.x - diracn.x) * width / 2, (-diracp.y - diracn.y) * width / 2) x.extend([rectpt1.x, rectpt2.x, rectpt3.x, rectpt4.x]) y.extend([rectpt1.y, rectpt2.y, rectpt3.y, rectpt4.y]) ax.fill(x, y, 'b', alpha=0.2)
def test_project_on_line_parallel(self): """ Simple test, building line and points near it, which can form parallel line. """ line = Line(Point2D(0, 0), Point2D(4, 4)) not_line_points = set() start_point = Point2D(0, -2) points_reference = [] not_line_point = start_point for i in range(0, 5): not_line_points.add(Point2D(not_line_point.x, not_line_point.y)) projected_point = SegmentsInLineFinder.ProjectedPoint(not_line_point, Point2D(not_line_point.x - 1, not_line_point.y + 1), (i - 1) * sqrt(2)) points_reference.append(projected_point) not_line_point = Point2D(not_line_point.x + 1, not_line_point.y + 1) points_test = SegmentsInLineFinder.project_on_line(line, not_line_points) self.assertListEqual(points_test, points_reference)
def get_anchor_pt(p0, p1, t0, t1, r0, r1): jnc_cntr, jnc_radius = get_junction_circle(p0, p1, t0, t1) if -1 == jnc_radius: # Straight line - "infinite" circle anchor_pt = (p0 + p1) / 2. else: aux_start = get_auxiliary_pt(p0, t0, r0, jnc_cntr, jnc_radius, p1) aux_end = get_auxiliary_pt(p1, t1, r1, jnc_cntr, jnc_radius, p0) aux_mid_pt = (aux_start + aux_end) / 2. if aux_mid_pt.distance(jnc_cntr) < 0.0001: # half a circle. Give the preference to p0 assert abs(aux_mid_pt.distance(aux_start) - jnc_radius) < 0.0001 diameter_line = Line(aux_start, aux_end) bisec_radius_vec = diameter_line.perpendicular_line( jnc_cntr).direction.unit anch_pt_1 = jnc_cntr + bisec_radius_vec * jnc_radius anch_pt_2 = jnc_cntr - bisec_radius_vec * jnc_radius test_pt = p0 + t0 closest_cand = get_closest_pt(anch_pt_1, anch_pt_2, test_pt) if closest_cand == anch_pt_2: bisec_radius_vec = -bisec_radius_vec else: bisec_radius_vec = Line(jnc_cntr, aux_mid_pt).direction.unit anchor_pt = jnc_cntr + bisec_radius_vec * jnc_radius DEBUG = 'IN_DEBUG' in globals() if DEBUG and IN_DEBUG: crc = mpt.Circle( [float(aux_start.x), float(aux_start.y)], radius=float(0.1), color="#D4AC0D", fill=True) plt.gca().add_patch(crc) crc = mpt.Circle( [float(aux_end.x), float(aux_end.y)], radius=float(0.1), color="#D4AC0D", fill=True) plt.gca().add_patch(crc) crc = mpt.Circle( [float(anchor_pt.x), float(anchor_pt.y)], radius=float(0.1), color="#85929E", fill=True) plt.gca().add_patch(crc) return anchor_pt
def find_segments_in_points(self, points: List[Point2D]) -> List[Segment]: np_points = np.ndarray((len(points), 2)) segments = [] for idx, point in enumerate(points): np_points[idx, 0] = point.x np_points[idx, 1] = point.y is_density_valid = True is_length_valid = True while len(np_points) > 2 \ and (self.density_threshold is None or is_density_valid) \ and (self.length_threshold is None or is_length_valid): model_robust, inliers_mask = ransac( np_points, LineModelND, min_samples=2, residual_threshold=self.residual_threshold, max_trials=self.max_trials) inliers = np_points[inliers_mask] line_params = model_robust.params origin = Point2D(line_params[0][0], line_params[0][1]) direction = Point2D(line_params[1][0], line_params[1][1]) line = Line(origin, origin + direction) points = [Point2D(point[0], point[1]) for point in inliers] found_segments = SegmentsInLineFinder.find_segments_with_density( line, points, 150) current_segments = [] densities = [] lengths = [] for found_segment in found_segments: current_segments.append(found_segment.segment) densities.append(found_segment.density) lengths.append(found_segment.segment.length) if len(densities) > 0: avg_density = np.mean(densities) avg_length = np.mean(lengths) if self.density_threshold: is_density_valid = avg_density > self.density_threshold if self.length_threshold: is_length_valid = avg_length > self.length_threshold else: is_density_valid = False is_length_valid = False segments += current_segments np_points = np_points[~inliers_mask] return segments
def eval_end_pt_tang(self, first): trgt_pt = self.p0 if first else self.p1 res_norm = Line(self.center, trgt_pt).direction.unit res_tang = res_norm.rotate(np.pi / 2.) halfpl = get_halfplane(self.p0, self.p1, self.center) if -1 == halfpl: res_tang = -res_tang return res_tang
def calculateFaceTilt(faceEyesIrisPoints, regionOfInterestColorObjects): zeroLine = Line(Point(1, 0), Point(0, 0)) leftEye = Point(faceEyesIrisPoints.pop()) rightEye = Point(faceEyesIrisPoints.pop()) eyeMiddlePoint = Point((leftEye + rightEye) / 2) eyeLine = Line(leftEye, rightEye) faceSymmetryLine = eyeLine.perpendicular_line(eyeMiddlePoint) angle = mpmath.degrees(eyeLine.angle_between(zeroLine)) if (int(angle) > 90): return { 'angle': int(angle) - 180, 'tiltline': faceSymmetryLine } else: return { 'angle': int(angle), 'tiltline': faceSymmetryLine }
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 isAdjacent(linei, linej): startLine = Line((0,0),(0,100)); (_, yi) = linei.intersection(startLine)[0]; (_, yj) = linej.intersection(startLine)[0]; if abs(yi-yj) < 100: return True; else: return False;
def get_bisector(p0, p1): #orig_line = Line(p0, p1) #mid_point = (p0+p1)/2. #bisec = orig_line.perpendicular_line(mid_point) #return bisec mid_pt = (float(p0.x + p1.x) / 2., float(p0.y + p1.y) / 2.) bisec_vec = (-float(p1.y - p0.y), float(p1.x - p0.x)) other_pt = (mid_pt[0] + bisec_vec[0], mid_pt[1] + bisec_vec[1]) return Line(mid_pt, other_pt, evaluate=False)
def build_graph(self, obstacles: List[Obstacle]): graph = nx.Graph() polygons = [] visited_lines = [] for obst_coords in obstacles: poly = Polygon(*obst_coords) polygons.append(poly) coors_len = len(obst_coords) i = 0 while i < coors_len: c1 = obst_coords[i] try: c2 = obst_coords[i + 1] except IndexError: c2 = obst_coords[-1] if c1 != c2: visited_lines.append(Line(c1, c2)) graph.add_edge(c1, c2) i += 1 coordinates = [] # brute force the rest for obst_coords in obstacles: coordinates.extend(obst_coords) for coord1 in coordinates: for coord2 in coordinates: if coord1 == coord2: continue l = Line(coord1, coord2) if l in visited_lines: continue for poly in polygons: intersect = l.intersection(poly) if len(intersect) > 1: graph.add_edge()