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
Exemple #2
0
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
Exemple #3
0
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))
Exemple #4
0
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
Exemple #5
0
    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)
Exemple #6
0
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
Exemple #7
0
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
Exemple #8
0
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()))
Exemple #11
0
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]))
Exemple #13
0
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
Exemple #14
0
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
Exemple #15
0
    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
Exemple #16
0
    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
Exemple #17
0
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]
Exemple #18
0
# 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)):
Exemple #19
0
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
Exemple #20
0
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  # 切点2
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]
Exemple #23
0
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
Exemple #25
0
        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  # 切点2
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)
Exemple #27
0
# 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  # 切点2
    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()))
Exemple #29
0
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)
Exemple #30
0
    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 = ''