def is_line_walkable(self, walk_line_segment): if self.quad_tree is None: return False start_circle = geometry.Circle( walk_line_segment.start, self.WALKER_COMFORT_ZONE ) end_circle = geometry.Circle( walk_line_segment.end, self.WALKER_COMFORT_ZONE ) start_points = geometry.intersect_line_and_circle( start_circle, walk_line_segment.perpendicular( walk_line_segment.start ) ) end_points = geometry.intersect_line_and_circle( end_circle, walk_line_segment.perpendicular( walk_line_segment.end ) ) if not walk_line_segment.are_on_the_same_side( start_points[0], end_points[0] ): end_points[0], end_points[1] = end_points[1], end_points[0] trajectory_bounding_rect_segments = [ geometry.LineSegment( start_points[0], start_points[1] ), geometry.LineSegment( start_points[1], end_points[1] ), geometry.LineSegment( end_points[1], end_points[0] ), geometry.LineSegment( end_points[0], start_points[0] ) ] potentially_interfering_walls = [] for bounding_box in get_line_segment_bounding_boxes( walk_line_segment ): potentially_interfering_walls += self.quad_tree.get( bounding_box.expand( self.WALKER_COMFORT_ZONE + 1 ) ) for wall_line_segment in potentially_interfering_walls: if ( does_line_segment_interfere_with_circle( wall_line_segment, start_circle ) or does_line_segment_interfere_with_circle( wall_line_segment, end_circle ) ): return False if does_line_segment_interfere_with_rect( wall_line_segment, trajectory_bounding_rect_segments ): return False return True
def __init__(self,img,width, height) : self.sprite = pygame.sprite.Sprite() self.sprite.image = pygame.transform.scale(pygame.image.load(img).convert_alpha(),(80,80)) self.sprite.rect = self.sprite.image.get_rect() self.sprite.rect.top = height -10 - self.sprite.image.get_height() self.sprite.rect.left = (width/2) - (self.sprite.image.get_width()/2) self.shoots = [] self.colliderCirc = ( geometry.Circle(geometry.Point(40,30),20), geometry.Circle(geometry.Point(20,60),20), geometry.Circle(geometry.Point(60,60),20) )
def render(self, display): head = geometry.Circle([self.x, self.y], self.width + 2, self.color, 0, True) head.render(display) path = geometry.Path(self.path, self.color, self.width) path.render(display)
def test_pseudo_tangent_contact(): circle = gm.Circle(radius=20, center=gm.Point(0, 0)) # Parallel to x-axis pt1 = gm.Point(-1, 25) pt2 = gm.Point(1, 25) l1 = gm.Line(pt1, pt2) l2 = gm.Line(pt2, pt1) iPt1 = circle.get_contact_point_pseudo_tangent(l1) iPt2 = circle.get_contact_point_pseudo_tangent(l2) print "GT: (0,20), Predict: ", iPt1 print "GT: (0,20), Predict: ", iPt2 # Parallel to y-axis pt1 = gm.Point(-25, 5) pt2 = gm.Point(-25, 50) pt3 = gm.Point(30, 5) pt4 = gm.Point(30, 50) l1 = gm.Line(pt1, pt2) l2 = gm.Line(pt3, pt4) iPt1 = circle.get_contact_point_pseudo_tangent(l1) iPt2 = circle.get_contact_point_pseudo_tangent(l2) print "GT: (-20,0), Predict: ", iPt1 print "GT: (20,0), Predict: ", iPt2 # A diagonal pt1 = gm.Point(50, 0) pt2 = gm.Point(0, 50) l1 = gm.Line(pt1, pt2) iPt1 = circle.get_contact_point_pseudo_tangent(l1) print iPt1
def parallel_outstanding_line( corner, end, other_end ): corner_wall_line = geometry.Line( corner, end ) perpendicular = corner_wall_line.perpendicular( corner ) intersections = geometry.intersect_line_and_circle( geometry.Circle( corner, offset ), perpendicular ) assert( len( intersections ) == 2 ) if not corner_wall_line.are_on_the_same_side( intersections[0], other_end ): return perpendicular.perpendicular( intersections[0] ) else: return perpendicular.perpendicular(intersections[1])
def main(args): """ Generates a file containing the coordinates of a body. Parameters ---------- args: namespace Arguments parsed from the command-line. """ if args.body_type == 'file': body = geometry.Geometry(file_path=args.file_path) elif args.body_type == 'circle': body = geometry.Circle(radius=args.circle[0], center=geometry.Point(args.circle[1], args.circle[2]), n=args.n, ds=args.ds) elif args.body_type == 'line': body = geometry.Line(length=args.line[0], start=geometry.Point(args.line[1], args.line[2]), n=args.n, ds=args.ds) elif args.body_type == 'rectangle': body = geometry.Rectangle(bottom_left=geometry.Point(args.rectangle[0], args.rectangle[1]), top_right=geometry.Point(args.rectangle[2], args.rectangle[3]), nx=args.n, ny=args.n, ds=args.ds) elif args.body_type == 'sphere': body = geometry.Sphere(radius=args.sphere[0], center=geometry.Point(args.sphere[1], args.sphere[2], args.sphere[3]), n=args.n, ds=args.ds) body.scale(ratio=args.scale) body.rotation(center=args.rotation, roll=args.roll, yaw=args.yaw, pitch=args.pitch, mode=args.mode) body.translation(displacement=args.translation) if body.dimensions == 2 and args.body_type == 'file': body.discretization(n=args.n, ds=args.ds) if body.dimensions == 2 and args.extrusion: body = body.extrusion(limits=args.extrusion, n=args.n, ds=args.ds, force=args.force) if args.save: output_path = os.path.join(args.save_directory, args.save_name + '.' + args.extension) body.write(file_path=output_path) if args.show: body.plot()
def __init__(self, img , xposition , yposition): self.sprite = pygame.sprite.Sprite() self.sprite.image = pygame.transform.scale(pygame.image.load(img).convert_alpha(),(30,30)) self.sprite.rect = self.sprite.image.get_rect() self.sprite.rect.top = yposition self.sprite.rect.left = xposition self.colliderCirc = ( geometry.Circle(geometry.Point(15,15),12), ) self.direction = Direction.RIGHT
def __init__ (self , img , x , y): self.sprite = pygame.sprite.Sprite() self.sprite.image = pygame.transform.scale(pygame.image.load(img).convert_alpha(),(4,8)) self.sprite.rect = self.sprite.image.get_rect() self.sprite.rect.top = y - self.sprite.image.get_height()/2 self.sprite.rect.left = x - self.sprite.image.get_width()/2 self.colliderCirc = ( geometry.Circle(geometry.Point(2,4),2), ) self.timemove = 2 self.timemovecurr = 0
def collider (self,circle): for x in self.colliderCirc: circaux = geometry.Circle( geometry.Point( self.sprite.rect.left + x.center.x, self.sprite.rect.top + x.center.y ), x.radius ) if circaux.collider(circle): return True return False
def __init__ (self , img , x , y): self.sprite = pygame.sprite.Sprite() self.sprite.image = pygame.transform.scale (pygame.image.load(img).convert_alpha() , (6,10)) self.sprite.rect = self.sprite.image.get_rect() self.sprite.rect.left = x - self.sprite.rect.width self.sprite.rect.top = y - self.sprite.rect.height self.colliderCirc = ( geometry.Circle(geometry.Point(2,4),2), ) self.stepmove = 0 self.totaltimemove = 2
def main(): a = geometry.Rectangle(Point(0, 0), 10, 20) b = geometry.Circle(Point(1, 1), 30) c = geometry.Square(Point(3, 3), 40) print(a) print(b) print(c) shapes = [a, b, c] for shape in shapes: # Kako sistemski dobiti ime klase kojoj pripada objekat? print("%s P=%g O=%g" % (type(shape).__name__, shape.get_area(), shape.get_circum()))
def test_line_segment_interference_with_cirlce(self): circle = geometry.Circle( geometry.Point( 0, 0 ), 10 ) self.assertTrue( does_line_segment_interfere_with_circle( geometry.LineSegment( geometry.Point( -20, 4 ), geometry.Point( 20, 1 ) ), circle ) ) self.assertTrue( does_line_segment_interfere_with_circle( geometry.LineSegment( geometry.Point( -5, 4 ), geometry.Point( 20, 1 ) ), circle ) ) self.assertTrue( does_line_segment_interfere_with_circle( geometry.LineSegment( geometry.Point( -5, 4 ), geometry.Point( 4, 1 ) ), circle ) ) self.assertFalse( does_line_segment_interfere_with_circle( geometry.LineSegment( geometry.Point( -50, 11 ), geometry.Point( 50, 11 ) ), circle ) )
def makeGeometry(): pitch = 1.27 circle = geom.Circle(pitch / 2, pitch / 2, 0.4) fuelCell = geom.SurfaceNode(circle, False) waterCell = geom.SurfaceNode(circle, True) fuel = geom.Region('fuel') fuel.cell = fuelCell fuel.volume = circle.area() water = geom.Region('water') water.cell = waterCell water.volume = pitch**2 - circle.area() g = geom.Geometry(pitch, pitch) g.addRegion(fuel) g.addRegion(water) return g
def main(): np.set_printoptions(precision=3, suppress=True) np.random.seed(0) circles = [geo.Circle(7, 7, 1.5)] rects = [ geo.Rectangle(-6, -10, -5, -1), geo.Rectangle(-6, 1, -5, 10), geo.Rectangle(0, -10, 1, -1), geo.Rectangle(0, 1, 1, 10), ] q_low = np.array([-10, -10]) q_high = np.array([10, 10]) q0 = np.array([-9, -9]) qf = np.array([9, 9]) tree = RRTreeStar(q0, obstacles=circles + rects) # build the tree for i in range(200): print(i) q = sample_config_space(q_low, q_high) q_idx = tree.add_vertex(q) # now start trying to connect qf # i = 0 # while True: # # try to connect the final node every 100th sample # if i % 100 == 0: # q = qf # else: # q = sample_config_space(q_low, q_high) # q_idx = tree.add_vertex(q) # # if q_idx >= 0: # if very_close(tree.vertices[q_idx].value, qf): # print("qf connected") # break # i += 1 print(f"Number of vertices = {len(tree.vertices)}") # print(f"Number of segments = {len(tree.segments)}") path = tree.path_to_root(q_idx) q_path = np.array([tree.vertices[idx].value for idx in path]) print(f"Length of path = {len(path)}") plt.figure() ax = plt.gca() plt.xlim([-10, 10]) plt.ylim([-10, 10]) def plot_edges(ax, vertex): q = vertex.value for idx in vertex.children_idx: child = tree.vertices[idx] qc = child.value ax.plot([q[0], qc[0]], [q[1], qc[1]], color="k") plot_edges(ax, child) plot_edges(ax, tree.vertices[0]) plt.plot(q0[0], q0[1], "o", color="g") plt.plot(qf[0], qf[1], "o", color="r") plt.plot(q_path[:, 0], q_path[:, 1], color="b") for circle in circles: ax.add_patch(plt.Circle(circle.center, circle.r, color="r", fill=True)) for rect in rects: ax.add_patch( plt.Rectangle( (rect.x_min, rect.y_min), rect.width, rect.height, color="r", fill=True ) ) plt.show()
def main(): """ main function contains only printing methods, and user inputs for choosing action """ shapes = geometry.ShapeList() # object containing all shapes added by the user while True: os.system('clear') print( "LEARN GEOMETRY\n\n" "What do you want to do?\n" "\t(1) Add new shape\n" "\t(2) Show all shapes\n" "\t(3) Show shape with the largest perimeter\n" "\t(4) Show shape with the largest area\n" "\t(5) Show formulas\n" "\t(0) Exit program\n" ) option = input("Select an option: ") if option == "1": os.system('clear') print_list_of_shapes() user_choice = input('Select an option: ') if user_choice == '1': os.system('clear') print('Enter the length of circle radius: ') radius = input_value() circle = geometry.Circle(radius) shapes.add_shape(circle) elif user_choice == '2': os.system('clear') print('Enter length of first side of triangle: ') first_side = input_value() print('Enter length of second side of triangle: ') second_side = input_value() print('Enter length of third side of triangle: ') third_side = input_value() try: triangle = geometry.Triangle(first_side, second_side, third_side) shapes.add_shape(triangle) except ValueError: input("Wrong value. Triangle cant be build with that length of sides {}, {}, {}." .format(first_side, second_side, third_side)) elif user_choice == '3': os.system('clear') print('Enter length of equilateral triangle side:') triangle_side = input_value() equilateral_triangle = geometry.EquilateralTriangle(triangle_side) shapes.add_shape(equilateral_triangle) elif user_choice == '4': os.system('clear') print('Enter length of first side of rectangle: ') first_side_of_rectangle = input_value() print('Enter length of secound side of rectangle: ') second_side_of_rectangle = input_value() rectangle = geometry.Rectangle(first_side_of_rectangle, second_side_of_rectangle) shapes.add_shape(rectangle) elif user_choice == '5': os.system('clear') print('Enter length of side of square: ') square_side = input_value() square = geometry.Square(square_side) shapes.add_shape(square) elif user_choice == '6': os.system('clear') print('Enter length of side of regular pentagon: ') pentagon_side = input_value() pentagon = geometry.RegularPentagon(pentagon_side) shapes.add_shape(pentagon) elif user_choice == '0': main() else: raise ValueError("Wrong input") elif option == "2": os.system('clear') if len(shapes.shapes) == 0: input('First add some shapes!\n\n Enter to back to menu') else: print(shapes.get_shapes_table()) input('\nEnter = main menu') elif option == "3": os.system('clear') if len(shapes.shapes) == 0: input('First add some shapes! \n Enter to back to menu') else: print('Shape with the largest perimeter:\n' + str(shapes.get_largest_shape_by_perimeter()) + '\tperimeter:', round(shapes.get_largest_shape_by_perimeter().get_perimeter(), 1)) input('\nEnter to back to menu') elif option == "4": os.system('clear') if len(shapes.shapes) == 0: input('First add some shapes! \n Enter to back menu') else: print('Shape with the largest area:\n' + str(shapes.get_largest_shape_by_area()) + '\tarea:', round(shapes.get_largest_shape_by_area().get_area(), 1)) input('\nEnter to back to menu') elif option == "5": os.system('clear') print_list_of_shapes() show_formulas = input("Enter number to get a shape formulas: ") if show_formulas == '1': shape = 'Circle' area = geometry.Circle.get_area_formula() perimeter = geometry.Circle.get_perimeter_formula() elif show_formulas == '2': shape = 'Triangle' area = geometry.Triangle.get_area_formula() perimeter = geometry.Triangle.get_perimeter_formula() elif show_formulas == '3': shape = 'Equilateral Triangle' area = geometry.EquilateralTriangle.get_area_formula() perimeter = geometry.Triangle.get_perimeter_formula() elif show_formulas == '4': shape = 'Rectangle' area = geometry.Rectangle.get_area_formula() perimeter = geometry.Rectangle.get_perimeter_formula() elif show_formulas == '5': shape = 'Square' area = geometry.Square.get_area_formula() perimeter = geometry.Square.get_perimeter_formula() elif show_formulas == '6': shape = 'Regular Pentagon' area = geometry.RegularPentagon.get_area_formula() perimeter = geometry.RegularPentagon.get_perimeter_formula() elif show_formulas == '0': main() os.system('clear') print('\n{}\n\nFormulas:\nArea: {}\nPerimeter: {}'.format(shape, area, perimeter)) input('\nEnter to back to menu') elif option == "0": sys.exit()
sys.path.append(os.path.realpath(__file__)) import geometry import tr4ckdb import traffic import triangulate __doc__ = """ various tracking/analysis tools from traffic analysis to host triangulation """ if __name__ == "__main__": # create a dummy database # and try collecting some packets into it # this will print IP layer addresses as (source, destination, timestamp) # but omit ones CLAIMING to be this computer with tr4ckdb.IPDB("IPDB-test") as db: #tr4ckdb.dummy(tr4ckdb.IPDB) as db: traffic.PacketTracker( db).track() #, traffic.filter_out_localhost).track() # test triangulation over a set of circles circles = [ geometry.Circle(geometry.Point(0, 0), 1), geometry.Circle(geometry.Point(0.5, 0.5), 1), geometry.Circle(geometry.Point(0.5, 0), 1) ] #triangulate.Triangulator.triangulate()
def main(): np.set_printoptions(precision=3, suppress=True) np.random.seed(0) model = models.ThreeInputModel(output_idx=[0, 1]) circle1 = geo.Circle(3, 1, 0.5) floor = geo.Floor(0) q_low = np.array([-1, -np.pi, -np.pi]) q_high = np.array([6, np.pi, np.pi]) q0 = np.array([0, np.pi / 4.0, -np.pi / 4.0]) qf = np.array([5, np.pi / 4.0, -np.pi / 4.0]) tree = RRTreeStar(model, q0, obstacles=[circle1, floor]) # build the tree for _ in range(200): q = sample_config_space(q_low, q_high) tree.add_vertex(q) # now start trying to connect qf i = 0 while True: # try to connect the final node every 100th sample if i % 100 == 0: q = qf else: q = sample_config_space(q_low, q_high) q_idx = tree.add_vertex(q) if q_idx >= 0: if very_close(tree.vertices[q_idx].value, qf): print("qf connected") break i += 1 print(f"Number of vertices = {len(tree.vertices)}") # print(f"Number of segments = {len(tree.segments)}") path = tree.path_to_root(q_idx) q_path = np.array([tree.vertices[idx].value for idx in path]) print(f"Length of path = {len(path)}") fig = plt.figure() ax = fig.add_subplot(111, projection="3d") ax.plot(q_path[:, 0], q_path[:, 1], zs=q_path[:, 2], color="b") points = np.array([model.sample_points(q)[2:, :] for q in q_path]) plt.figure() ax = plt.gca() ax.set_aspect("equal") plt.plot(points[:, 0, 0], points[:, 0, 1], "o-", label="p0") plt.plot(points[:, 1, 0], points[:, 1, 1], "o-", label="p1") plt.plot(points[:, 2, 0], points[:, 2, 1], "o-", label="p2") ax = plt.gca() ax.add_patch(plt.Circle(circle1.center, circle1.r, color="k", fill=False)) plt.legend() plt.grid() plt.show()
################################################################################ # test circle class # nicolas de toffoli ################################################################################ import pytest import geometry # global objects circle0 = geometry.Circle(0.0) circle1 = geometry.Circle(4.0) def test_perimeter_0(): assert circle0.perimeter() == 0.0 def test_perimeter_1(): assert round(circle1.perimeter(), 2) == 25.13 def test_area_0(): assert circle0.area() == 0.0 def test_area_1(): assert round(circle1.area(), 2) == 50.27
def create_cirlce_instance(self): return geometry.Circle(100)
def get_toc_ball_ball(obj1, obj2, name1, name2, aFriction=0, aColDamp=0): # Initializations tCol = np.inf nrmlCol = None ptCol = None badResult = [np.inf, None, None, None] # Get the velocities of the ball. pos1, vel1 = obj1.get_position(), obj1.get_velocity() pos2, vel2 = obj2.get_position(), obj2.get_velocity() # print 'Before col det', name1, vel1 # print 'Before col det', name2, vel2 # We will go into the frame of reference of object 1 relVel = vel2 - vel1 # Find the direction of collision colDir = pos2 - pos1 colDist = colDir.mag() - (obj1.get_radius() + obj2.get_radius()) colDir.make_unit_norm() # Get the velocity along the direction of collision speed = -relVel.dot(colDir) # print "Speed is: ", speed if speed <= 0: # If the balls will not collide return badResult circ1 = gm.Circle(obj1.get_radius(), pos1) circ2 = gm.Circle(obj2.get_radius(), pos2) tCol, ptCol, nrmlCol, relDist = circ1.intersect_moving_circle( circ2, relVel) if ptCol is None: return badResult # print '##### NRML ####',tCol,ptCol, nrmlCol vOth1 = vel1.project(nrmlCol) vOth2 = vel2.project(nrmlCol) vCol1 = vel1 - vOth1 vCol2 = vel2 - vOth2 m1 = obj1.get_mass() m2 = obj2.get_mass() if aFriction == 0: # constant motion pass elif aFriction > 0: # correct tCol and new vel before collision # initial speed speed = relDist / tCol acc_r = [0, 0, 0, 0] if vel1.mag() > 0: acc_r[0] = vCol1.mag() / vel1.mag() acc_r[2] = vOth1.mag() / vel1.mag() if vel2.mag() > 0: acc_r[1] = vCol2.mag() / vel2.mag() acc_r[3] = vOth2.mag() / vel2.mag() if vCol1.dot(vCol2) <= 0: # -> <- or -> . acc = -aFriction * (m1 * acc_r[0] + m2 * acc_r[1]) else: # --> -> acc = -aFriction * ((m1 * acc_r[0] - m2 * acc_r[1]) * (vCol1.mag() > vCol2.mag()) + (m2 * acc_r[1] - m1 * acc_r[0]) * (vCol1.mag() <= vCol2.mag())) # project to relative direction if acc != 0: if speed * speed + 2 * relDist * acc >= 0: tCol = (math.sqrt(speed * speed + 2 * relDist * acc) - speed) / acc # if any stopped before collision, then "stop " event should happen first # use acc_r -> already check vel1.mag()=0 if tCol * aFriction * m1 * acc_r[0] > vCol1.mag( ) or tCol * aFriction * m2 * acc_r[1] > vCol2.mag(): return badResult else: return badResult vCol1 = vCol1 - vCol1.get_scaled_vector( 1.0) * tCol * aFriction * m1 * acc_r[0] vOth1 = vOth1 - vOth1.get_scaled_vector( 1.0) * tCol * aFriction * m1 * acc_r[2] vCol2 = vCol2 - vCol2.get_scaled_vector( 1.0) * tCol * aFriction * m2 * acc_r[1] vOth2 = vOth2 - vOth2.get_scaled_vector( 1.0) * tCol * aFriction * m2 * acc_r[3] # pdb.set_trace() vCol1New = (vCol1 * (m1 - m2) + 2 * m2 * vCol2) * (1.0 / (m1 + m2)) vCol2New = (vCol2 * (m2 - m1) + 2 * m1 * vCol1) * (1.0 / (m1 + m2)) vel1New = (vCol1New + vOth1) * (1 - aColDamp) # don't set the future velocity yet... may overwrite earlier collision # vel2New = vCol2New + vOth2 # obj1.set_after_collision_velocity(vel1New) # obj2.set_after_collision_velocity(vel2New) # We dont require normal and point of collision. # pdb.set_trace() # print vel1, vel1New, vel2, vel2New, vel1New.mag(), vel2New.mag() # print 'After col det', name1, vel1New # print 'After col det', name2, vel2New return tCol, nrmlCol, ptCol, vel1New
if not killenemy: shipenemy.nextMove(h, w, 4) #if ship.collider(c): # print('Hubo una colicion ') reloj.tick(100) pantalla.fill(BLACK) pantalla.blit(ship.sprite.image, ship.sprite.rect) pantalla.blit(shipenemy.sprite.image, shipenemy.sprite.rect) #pygame.draw.circle(pantalla, BLUE, (c.center.x ,c.center.y), c.radius, 1) pantalla.blit(text, (10, 10)) # esto es para tratar los disparos de mi nave for x in ship.shoots: x.movup(4) pantalla.blit(x.sprite.image, x.sprite.rect) for circ in x.colliderCirc: aux = geometry.Circle( geometry.Point(circ.center.x + x.sprite.rect.left, circ.center.y + x.sprite.rect.top), circ.radius) if shipenemy.collider(aux): ship.shoots.remove(x) print('le dio a la nave enemiga') killenemy = True if x.sprite.rect.top < 5: ship.shoots.remove(x) pygame.display.update()
def setUp(self): self.point = geometry.Point(2.0, 2.0) self.circle = geometry.Circle(0.0, 0.0, 1.0) self.circle2 = geometry.Circle(3.0, 3.0, 1.0)
def get_toc_ball_ball(obj1, obj2, name1, name2): #Initializations tCol = np.inf nrmlCol = None ptCol = None #Get the velocities of the ball. pos1, vel1 = obj1.get_position(), obj1.get_velocity() pos2, vel2 = obj2.get_position(), obj2.get_velocity() #print 'Before col det', name1, vel1 #print 'Before col det', name2, vel2 #We will go into the frame of reference of object 1 relVel = vel2 - vel1 #Find the direction of collision colDir = pos2 - pos1 colDist = colDir.mag() - (obj1.get_radius() + obj2.get_radius()) colDir.make_unit_norm() #Get the velocity along the direction of collision speed = -relVel.dot(colDir) #print "Speed is: ", speed if speed <= 0: #If the balls will not collide return tCol, nrmlCol, ptCol circ1 = gm.Circle(obj1.get_radius(), pos1) circ2 = gm.Circle(obj2.get_radius(), pos2) tCol, ptCol, nrmlCol = circ1.intersect_moving_circle(circ2, relVel) #print tCol if ptCol is None: return tCol, nrmlCol, ptCol #print '##### NRML ####',tCol,ptCol, nrmlCol vOth1 = vel1.project(nrmlCol) vOth2 = vel2.project(nrmlCol) vCol1 = vel1 - vOth1 vCol2 = vel2 - vOth2 ''' #There is one more situation in which no collision will happen. #pos11 = pos1 - pos1 #pos21 = pos2 - pos1 #circ = gm.Circle(obj1.get_radius(), pos11) #isIntersect = circ.is_intersect_line(gm.Line(pos21, pos21 + relVel)) #circ = gm.Circle(obj1.get_radius(), pos1) #isIntersect = circ.is_intersect_line(gm.Line(pos2, pos2 + relVel)) if not isIntersect: return tCol, nrmlCol, ptCol print "Collision will happen" #Now we know that balls are definitely colliding. t = colDist / speed #Get the velocities along the direction of collision vCol1 = vel1.project(colDir) vCol2 = vel2.project(colDir) vOth1 = vel1 - vCol1 vOth2 = vel2 - vCol2 ''' #Find the new velocities along the direction of collision m1 = obj1.get_mass() m2 = obj2.get_mass() #pdb.set_trace() vCol1New = (vCol1 * (m1 - m2) + 2 * m2 * vCol2) * (1.0 / (m1 + m2)) vCol2New = (vCol2 * (m2 - m1) + 2 * m1 * vCol1) * (1.0 / (m1 + m2)) vel1New = vCol1New + vOth1 vel2New = vCol2New + vOth2 obj1.set_after_collision_velocity(vel1New) #obj2.set_after_collision_velocity(vel2New) #We dont require normal and point of collision. #pdb.set_trace() #print vel1, vel1New, vel2, vel2New, vel1New.mag(), vel2New.mag() #print 'After col det', name1, vel1New #print 'After col det', name2, vel2New return tCol, nrmlCol, ptCol
def make_circle(x, y, radius): circle = geometry.Circle(x, y, radius) return circle
def vertices_for_line_segment_end( end, other_end, offset ): offset_center = geometry.measure_out( end, other_end, -offset ) vertices = geometry.intersect_line_and_circle( geometry.Circle( offset_center, offset ), geometry.Line( end, other_end ).perpendicular( offset_center ) ) assert( len( vertices ) == 2 ) return vertices
def main(): np.set_printoptions(precision=3, suppress=True) np.random.seed(0) circles = [geo.Circle(7, 7, 1.5)] rects = [ geo.Rectangle(-6, -10, -5, -1), geo.Rectangle(-6, 1, -5, 10), geo.Rectangle(0, -10, 1, -1), geo.Rectangle(0, 1, 1, 10), ] q_low = np.array([-10, -10]) q_high = np.array([10, 10]) q0 = np.array([-9, -9]) qf = np.array([9, 9]) tree = RRTree(q0, obstacles=circles + rects) while len(tree.vertices) < 2000: q = sample_config_space(q_low, q_high) q_idx = tree.add_vertex(q) i = 0 while True: # try to connect the final node every 100th sample if i % 100 == 0: q = qf else: q = sample_config_space(q_low, q_high) q_idx = tree.add_vertex(q) if q_idx >= 0: if very_close(tree.vertices[q_idx].value, qf): print("qf connected") break i += 1 print(f"Number of vertices = {len(tree.vertices)}") print(f"Number of segments = {len(tree.segments)}") path = tree.path_to_root(q_idx) q_path = np.array([tree.vertices[idx].value for idx in path]) print(f"Length of path = {len(path)}") plt.figure() ax = plt.gca() plt.xlim([-10, 10]) plt.ylim([-10, 10]) for segment in tree.segments: q1 = segment.v1.value q2 = segment.v2.value plt.plot([q1[0], q2[0]], [q1[1], q2[1]], color="k") # for i, vertex in enumerate(tree.vertices): # q = vertex.value # plt.plot(q[0], q[1], ".", color="b") plt.plot(q0[0], q0[1], "o", color="g") plt.plot(qf[0], qf[1], "o", color="r") plt.plot(q_path[:, 0], q_path[:, 1], color="b") for circle in circles: ax.add_patch(plt.Circle(circle.center, circle.r, color="r", fill=True)) for rect in rects: ax.add_patch( plt.Rectangle((rect.x_min, rect.y_min), rect.width, rect.height, color="r", fill=True)) plt.show()
def get_next_particle(self, line): """ Choose next particle according to the random line hitting distribution as described in the paper. """ C = geom.Circle(0, self.cluster_radius + 2) segment = C.get_intersection_segment(line) #segment object if segment == False: print("line missed cluster: no segment") return False elif segment.A == segment.B: print("line missed cluster: one point segment") return False AB = [segment.A, segment.B] random.shuffle(AB) A = AB[0] B = AB[1] """ c is a "walking" circle from A to B. When it intersects with a cluser particle, the square which contains the center of the circle gets added to the cluster, if it is element of boundary_set. c walks with steps of size stepsize """ c = geom.Circle(A, 0.99) stepsize = c.radius * 0.99 dir = (B - A) / abs(B - A) #abs = 1 while True: c_dist = int(abs(c.center)) """ #working with particle cluster layers #for k in [current_dist, current_dist + 1]: for k in [c_dist - 1, c_dist, c_dist + 1]: for point in self.layers[k]: if c.intersects_with_polygon(self.get_square(point)): pos = c.center pos = round(pos.real, 0) + round(pos.imag, 0) * 1j if pos not in self.boundary_set: for neigh in self.get_neighbours(pos): if neigh in self.boundary_set: return neigh print("line hit diagonal") return False return pos """ for point in self.boundary_set: if c.intersects_with_polygon(self.get_square(point)): return point c.center += stepsize * dir if self.get_distance(c.center, 0) > 2 * C.radius: print("line missed cluster: walked by") return False