def genDia():
x = random.randint(llim // 2, rlim // 2)
y = random.randint(llim // 2, rlim // 2)
AB = geo.Vector(x, y)
BC = geo.Vector(x, -y)
AD = BC
DC = AB
minx = llim - AB.x
if llim > minx:
minx = llim
maxx = rlim - AB.x
if rlim < maxx:
maxx = rlim
miny = llim - AB.y
if llim > miny:
miny = llim
maxy = rlim - AB.y
if rlim < maxy:
maxy = rlim
A = geo.Point(random.randint(minx, maxx), random.randint(miny, maxy))
OB = geo.Vector(A.x, A.y) + AB
B = geo.Point(OB.x, OB.y)
OC = OB + BC
OD = geo.Vector(A.x, A.y) + AD
C = geo.Point(OC.x, OC.y)
D = geo.Point(OD.x, OD.y)
buf = ["A : " + str(A), "B : " + str(B), "C : " + str(C), "D : " + str(D)]
tasks.update({"Diamond": buf})
def handle_circle_plane_collision(self, circle, plane):
position_vector = geo.Vector(circle.position.x, circle.position.y)
circle_displacement = position_vector * plane.normal - circle.radius
if circle_displacement <= plane.displacement and circle.velocity * plane.normal < 0:
overlap = plane.displacement - circle_displacement
self.stabilize_circle(circle, -plane.normal, overlap)
p = self.calculate_transferred_momentum(circle.velocity,
geo.Vector(0, 0),
1.0 / circle.mass, 0,
-plane.normal)
circle.velocity += plane.normal * p / circle.mass
def get_tangent_points(circle,point):
c = circle[:2]
# vec = geo.Vector(point, c)
#
dis = distance(point, c)
radius = circle[2]
cos = radius/dis
if cos >1:
cos = 1
angle = math.acos(cos)
#translation to center_based coordinate
trc = geo.constance_multi(cos, geo.Vector(c, point))
# rotate
t1 = rotate([trc[:]], angle)[0]
t2 = rotate([trc[:]], -angle)[0]
#translation back
t1 = [t1[0]+circle[0], t1[1]+ circle[1]]
t2 = [t2[0]+circle[0], t2[1]+ circle[1]]
return t1,t2,angle,-angle
def draw_line(line):
min = self.window_to_point(QtCore.QPoint(0, 0))
max = self.window_to_point(
QtCore.QPoint(self.ui.canvas.width(), self.ui.canvas.height()))
if abs(line.normal.x) > abs(line.normal.y):
minline = geoutil.line.from_point_normal(min, geo.Vector(0, 1))
maxline = geoutil.line.from_point_normal(
max, geo.Vector(0, -1))
else:
minline = geoutil.line.from_point_normal(min, geo.Vector(1, 0))
maxline = geoutil.line.from_point_normal(
max, geo.Vector(-1, 0))
minpoint = geoutil.line.intersect(line, minline)
maxpoint = geoutil.line.intersect(line, maxline)
points = [minpoint, maxpoint]
draw_points = [self.point_to_window(point) for point in points]
painter.drawLine(*draw_points)
def move_to(start, goal,velocity):
# 如果在一步內，則不經計算直接到點（同時避免超過)
if (geo.distance(start,goal) <= velocity):
return goal
# move direction
move = geo.normalize(geo.Vector(start,goal))
# mul by v
move = geo.constance_multi(velocity, move)
return geo.vector_add(start, move)
def O3(line0, line1):
theta0 = math.atan2(line0.normal.y, line0.normal.x)
theta1 = math.atan2(line1.normal.y, line1.normal.x)
theta = (theta0 + theta1) / 2
cos = math.cos(theta)
sin = math.sin(theta)
lines = []
for normal in (geo.Vector(cos, sin), geo.Vector(-sin, cos)):
if abs(line0.offset) > abs(MAX_DISTANCE * (line0.normal * normal)):
continue
if abs(line1.offset) > abs(MAX_DISTANCE * (line1.normal * normal)):
continue
t0 = line0.offset / (line0.normal * normal)
t1 = line1.offset / (line1.normal * normal)
offset = (t0 + t1) / 2
lines.append(geo.Line(normal, offset))
return lines
def create_simulator():
sim = simulation.Simulator()
sim.add_polygon(
simulation.Polygon(geo.Point(-3, 2), 30 * 3.14 / 180, [
geo.Point(1, 1),
geo.Point(-1, 1),
geo.Point(-1, -1),
geo.Point(1, -1)
], 1, 2.0 / 3.0, geo.Vector(4, 0), geo.Vector(0, 0, 2 * 3.14)))
sim.add_polygon(
simulation.Polygon(geo.Point(5, 0), 90 * 3.14 / 180, [
geo.Point(3, 1),
geo.Point(-3, 1),
geo.Point(-3, -1),
geo.Point(3, -1)
], 3, 10, geo.Vector(0, 0), geo.Vector(0, 0, 3.14)))
return sim
def sample_num(line, sample_num):
line_v = geo.Vector(line[0],line[1])
points = []
v_add = geo.constance_multi(1/(sample_num), line_v)
v_start = geo.vector_add(line[0],geo.constance_multi(1/(2*sample_num), line_v))
for i in range(sample_num):
points.append(v_start[:])
v_start = geo.vector_add(v_start, v_add)
return points
def genTrap():
k = random.randint(2, (rlim - llim) // 4)
AB = geo.Vector(random.randint(-((rlim - llim) // k), (rlim - llim) // k),
random.randint(-((rlim - llim) // k), (rlim - llim) // k))
DC = AB * k
minx = llim - AB.x
if llim > minx:
minx = llim
maxx = rlim - AB.x
if rlim < maxx:
maxx = rlim
miny = llim - AB.y
if llim > miny:
miny = llim
maxy = rlim - AB.y
if rlim < maxy:
maxy = rlim
A = geo.Point(random.randint(minx, maxx), random.randint(miny, maxy))
OB = geo.Vector(A.x, A.y) + AB
B = geo.Point(OB.x, OB.y)
minx = llim - DC.x
if llim > minx:
minx = llim
maxx = rlim - DC.x
if rlim < maxx:
maxx = rlim
miny = llim - DC.y
if llim > miny:
miny = llim
maxy = rlim - DC.y
if rlim < maxy:
maxy = rlim
D = geo.Point(random.randint(minx, maxx), random.randint(miny, maxy))
OC = geo.Vector(D.x, D.y) + DC
C = geo.Point(OC.x, OC.y)
buf = ["A : " + str(A), "B : " + str(B), "C : " + str(C), "D : " + str(D)]
tasks.update({"Trapeze": buf})
def genParal():
A = ranPFR(llim + 1, rlim - 1)
B = ranPFR(llim + 1, rlim - 1)
AB = geo.Vector(A, B)
DC = AB
minx = llim - DC.x
if llim > minx:
minx = llim
maxx = rlim - DC.x
if rlim < maxx:
maxx = rlim
miny = llim - DC.y
if llim > miny:
miny = llim
maxy = rlim - DC.y
if rlim < maxy:
maxy = rlim
D = geo.Point(random.randint(minx, maxx), random.randint(miny, maxy))
OC = geo.Vector(D.x, D.y) + DC
C = geo.Point(OC.x, OC.y)
buf = ["A : " + str(A), "B : " + str(B), "C : " + str(C), "D : " + str(D)]
tasks.update({"Paral": buf})
def genRect():
k = random.randint(2, (rlim - llim - 1) // 2)
if random.randint(0, 1) == 1:
k *= -1
x = random.randint(llim // abs(k), rlim // abs(k))
y = random.randint(llim // abs(k), rlim // abs(k))
y2 = x * k
x2 = -y * k
if random.randint(0, 1) == 1:
AD = geo.Vector(x, y)
BC = AD
AB = geo.Vector(x2, y2)
DC = AB
else:
AB = geo.Vector(x, y)
DC = AB
AD = geo.Vector(x2, y2)
BC = AD
minx = llim - AB.x
if llim > minx:
minx = llim
maxx = rlim - AB.x
if rlim < maxx:
maxx = rlim
miny = llim - AB.y
if llim > miny:
miny = llim
maxy = rlim - AB.y
if rlim < maxy:
maxy = rlim
A = geo.Point(random.randint(minx, maxx), random.randint(miny, maxy))
OB = geo.Vector(A.x, A.y) + AB
B = geo.Point(OB.x, OB.y)
OC = OB + BC
OD = geo.Vector(A.x, A.y) + AD
C = geo.Point(OC.x, OC.y)
D = geo.Point(OD.x, OD.y)
if random.randint(0, 1) == 1:
AB = geo.Vector(x, y)
DC = AB
buf = ["A : " + str(A), "B : " + str(B), "C : " + str(C), "D : " + str(D)]
tasks.update({"Rectangle": buf})
def genCom():
x = random.randint(llim + 1, rlim - 1)
y = random.randint(llim + 1, rlim - 1)
x2 = random.randint(llim + 1, rlim - 1)
y2 = random.randint(llim + 1, rlim - 1)
while (x == x2) and (y == y2):
x2 = random.randint(llim + 1, rlim - 1)
y2 = random.randint(llim + 1, rlim - 1)
AB = geo.Vector(x, y)
DC = geo.Vector(x2, y2)
minx = llim - AB.x
if llim > minx:
minx = llim
maxx = rlim - AB.x
if rlim < maxx:
maxx = rlim
miny = llim - AB.y
if llim > miny:
miny = llim
maxy = rlim - AB.y
if rlim < maxy:
maxy = rlim
A = geo.Point(random.randint(minx, maxx), random.randint(miny, maxy))
OB = geo.Vector(A.x, A.y) + AB
B = geo.Point(OB.x, OB.y)
x = random.randint(llim - A.x, rlim - A.x)
y = random.randint(llim - A.y, rlim - A.y)
x2 = random.randint(llim - B.x, rlim - B.x)
y2 = random.randint(llim - B.y, rlim - B.y)
while (x == x2) and (y == y2):
x2 = random.randint(llim - B.x, rlim - B.x)
y2 = random.randint(llim - B.y, rlim - B.y)
AD = geo.Vector(x, y)
BC = geo.Vector(x2, y2)
OC = OB + BC
OD = geo.Vector(A.x, A.y) + AD
C = geo.Point(OC.x, OC.y)
D = geo.Point(OD.x, OD.y)
buf = ["A : " + str(A), "B : " + str(B), "C : " + str(C), "D : " + str(D)]
tasks.update({"Common": buf})
def cut_into_length(line, length = 5):
v_add = geo.constance_multi(length, geo.normalize(geo.Vector(line[0],line[1])))
dis = geo.distance(line[0],line[1])
v_start = line[0]
points = []
for d in range(0, math.ceil(dis+length), length):
if d > dis:
v_start = line[1]
else:
v_start = geo.vector_add(v_start, v_add)
points.append(v_start[:])
return points
def gjk_collision(self, points1, points2):
direction = geo.Vector(1, 0)
points = []
points.append(self.minkowski_difference(points1, points2, direction))
points.append(self.minkowski_difference(points1, points2, -direction))
if points[0].x < 0 or points[1].x > 0:
return (None, None)
while True:
AB = (points[1] - points[0]).normalize()
AO = geo.Point(0, 0) - points[0]
direction = AO - AB * (AO * AB)
points.append(
self.minkowski_difference(points1, points2, direction))
AC = points[2] - points[0]
if AC * direction < AO * direction:
return (None, None)
for i in range(0, 3):
C = points[i]
A = points[(i + 1) % 3]
B = points[(i + 2) % 3]
AB = (B - A).normalize()
AC = C - A
AO = geo.Point(0, 0) - A
direction = AO - AB * (AO * AB)
if AC * direction < 0:
del points[i]
break
else:
(distance,
direction) = self.epa_distance(points1, points2, points)
point = self.get_collision_point(points1, points2, direction)
return (point, direction)
def genSqr():
x = random.randint(llim // 2, rlim // 2)
y = random.randint(llim // 2, rlim // 2)
if random.randint(0, 1) == 1:
y2 = x
x2 = -y
else:
y2 = -x
x2 = y
if random.randint(0, 1) == 1:
AD = geo.Vector(x, y)
BC = AD
AB = geo.Vector(x2, y2)
DC = AB
else:
AB = geo.Vector(x, y)
DC = AB
AD = geo.Vector(x2, y2)
BC = AD
minx = max(llim - AB.x, llim, llim - AD.x)
maxx = min(rlim - AB.x, rlim, rlim - AD.x)
miny = max(llim - AB.y, llim, llim - AD.y)
maxy = min(rlim - AB.y, rlim, rlim - AD.y)
A = geo.Point(random.randint(minx, maxx), random.randint(miny, maxy))
OB = geo.Vector(A.x, A.y) + AB
B = geo.Point(OB.x, OB.y)
OC = OB + BC
OD = geo.Vector(A.x, A.y) + AD
C = geo.Point(OC.x, OC.y)
D = geo.Point(OD.x, OD.y)
buf = ["A : " + str(A), "B : " + str(B), "C : " + str(C), "D : " + str(D)]
tasks.update({"Square": buf})
def update(self,i):
goal_tracking = True
get_tangle = False
angle_list = []
tangent_points_list = []
tpoint = self.last_tpoint
lt = [-75, -75]
rt = [-75, -75]
for T in self.Tlist:
rs = geo.Vector(self.pos,T[:2])
angle = collision_detect_angle(rs,self.vec)
if angle == None:
continue
goal_tracking = False
CPA = getCPA(rs, angle)
if CPA >= T[2]:
continue
get_tangle = True
t1,t2,a1,a2 = get_tangent_points(T,self.pos)
angle_list.extend([a1-angle, a2- angle])
tangent_points_list.extend([t1, t2])
# if goal_tracking:
# tpoint = self.goal
# elif get_tangle:
# rangle,langle,rt,lt = self.dist_right_left(angle_list,tangent_points_list)
# if self.priority == 1 or (self.priority ==0 and abs(rangle) >= abs(langle)):
# tpoint = lt
# self.priority = 1
# else:
# tpoint = rt
# self.priority = 2
# keep_moving = False
# get_goal_tangle = False
if goal_tracking:
for T in self.Tlist:
rs = geo.Vector(self.pos,T[:2])
# 這邊不檢測行徑方向，改檢測 goal 方向是否可通
vec = geo.normalize(geo.Vector(self.pos, self.goal))
angle = collision_detect_angle(rs,vec)
if angle == None:
continue
goal_tracking = False
CPA = getCPA(rs, angle)
if CPA >= T[2]:
continue
# get_tangle = True
t1,t2,a1,a2 = get_tangent_points(T,self.pos)
angle_list.extend([a1-angle, a2- angle])
tangent_points_list.extend([t1, t2])
if get_tangle:
rangle,langle,rt,lt = self.dist_right_left(angle_list,tangent_points_list)
if self.priority == 1 or (self.priority ==0 and abs(rangle) >= abs(langle)):
tpoint = lt
self.priority = 1
else:
tpoint = rt
self.priority = 2
print('gt_pair %d, %d'%(goal_tracking, get_tangle))
# if get_goal_tangle:
# rangle,langle,rt,lt = self.dist_right_left(angle_list,tangent_points_list)
# if priority == 1 or (priority ==0 and abs(rangle) >= abs(langle)):
# tpoint = lt
# priority = 1
# else:
# tpoint = rt
# priority = 2
# self.vec = geo.normalize(geo.Vector(self.pos, tpoint))
# elif keep_moving is False:
# self.vec = geo.normalize(geo.Vector(self.pos, tpoint))
# if goal_tracking:
# priority = 0
self.last_tpoint = tpoint
if goal_tracking != get_tangle:
self.vec = geo.normalize(geo.Vector(self.pos, tpoint))
# self.pos = move_to(self.pos,tpoint,1)
self.pos = geo.constance_multi(1,geo.vector_add(self.pos,self.vec))
#
# self.vec = geo.normalize(geo.Vector(self.pos, tpoint))
#print(pos)
x = [self.pos[0]]
y = [self.pos[1]]
self.ln1.set_data(x,y)
#print(tpoint)
# x = [tpoint[0],]
# y = [tpoint[1],]
# print(i)
# print(lt)
# print(rt)
# print(tpoint)
x = [lt[0],rt[0],tpoint[0]]
y = [lt[1],rt[1],tpoint[1]]
self.ln2.set_data(x,y)
#print vec
x = [self.pos[0], tpoint[0]]
y = [self.pos[1], tpoint[1]]
self.ln3.set_data(x,y)
return self.ln1,self.ln2,self.ln3
def masked(self, x, y): v = geo.Vector(x, y) - geo.Vector(*self.circle.center) return v.length_sqr() > self.circle.radius ** 2
def perpendicular(vector):
return geo.Vector(vector.y, -vector.x)
def set_bounds(self, min_x, min_y, max_x, max_y):
self.planes.append(Plane(geo.Vector(1.0, 0), min_x))
self.planes.append(Plane(geo.Vector(-1.0, 0), -max_x))
self.planes.append(Plane(geo.Vector(0, 1.0), min_y))
self.planes.append(Plane(geo.Vector(0, -1.0), -max_y))
import geo
import itertools
import math
R = 0.75
GRAVITY = geo.Vector(0, 20)
class Circle(object):
def __init__(self, position, radius, density, velocity):
self.position = position
self.radius = radius
self.mass = density * math.pi * radius * radius
self.velocity = velocity
class Plane(object):
def __init__(self, normal, displacement):
self.normal = normal
self.displacement = displacement
class Polygon(object):
def __init__(self, position, rotation, points, mass, moment, velocity,
angular_velocity):
self.position = position
self.rotation = rotation
self.points = points
self.mass = mass
self.moment = moment