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
