class Leg:
def __init__(self, b_1_settings=None, cmd_queue=None, group_index=1):
self.angle_0 = 0.0
self.angle_1 = 0.0
self.angle_2 = 0.0
self.angle_3 = 0.0
self.cmd_queue = cmd_queue
self.group_index = group_index
#1
if b_1_settings is not None:
self.b_1 = MyFrame(**b_1_settings)
else:
self.b_1 = MyFrame()
self.b_1_box_1 = box(frame=self.b_1,
size=(63, 2, 40),
pos=(-31.5, 12.69, 0),
axis=(1, 0, 0))
self.b_1_box_2 = box(frame=self.b_1,
size=(63, 2, 40),
pos=(-31.5, -39.02, 0),
axis=(1, 0, 0))
self.b_1_box_3 = box(frame=self.b_1,
size=(2, 53.71, 40),
pos=(-1, -13.17, 0),
axis=(1, 0, 0))
#2
self.b_2 = MyFrame(frame=self.b_1,
pos=(-53, 0, 0))
self.b_2_celinder = cylinder(frame=self.b_2,
pos=(0, 16.1),
axis=(0,-1),
radius=8,
length=63)
self.b_2_box_1 = box(frame=self.b_2,
size=(61.5, 43.61, 26.5),
pos=(9.25, -13.21, -1.75),
axis=(1, 0, 0))
self.b_2_box_2 = box(frame=self.b_2,
size=(28.81, 60, 43.6),
pos=(-35.9, -4.99, -0.3),
axis=(1, 0, 0))
#3
self.b_3 = MyFrame(frame=self.b_2,
pos=(-35, -14.99, 0))
self.b_3_celinder = cylinder(frame=self.b_3,
pos=(0, 0, -29.6),
axis=(0, 0, 1),
radius=8,
length=59.1,
color=(1, 0, 0))
self.b_3_box_1 = box(frame=self.b_3,
size=(42, 20, 2),
pos=vector(-11, 0, 25.85),
color=(1, 0, 0))
self.b_3_box_2 = box(frame=self.b_3,
size=(42, 20, 2),
pos=vector(-11, 0, -25.85),
color=(1, 0, 0))
self.b_3_box_3 = box(frame=self.b_3,
size=(2, 20, 53.7),
pos=vector(-31, 0, 0),
color=(1, 0, 0))
self.b_3_box_4 = box(frame=self.b_3,
size=(68, 25, 25),
pos=vector(-(32 + 34), 0, 0),
color=(1, 0, 0))
#4.
self.b_4 = MyFrame(frame=self.b_3,
pos=(-(32 + 34 + 14), 0, 0))
self.b_4_celinder = cylinder(frame=self.b_4,
pos=(0, 0, -29.6),
axis=(0, 0, 1),
radius=8,
length=59.1,
color=(0, 0, 1))
self.b_4_box_1 = box(frame=self.b_4,
size=(39.5, 20, 2),
pos=(-9.75, 0, 25.5),
color=(0, 0, 1))
self.b_4_box_2 = box(frame=self.b_4,
size=(39.5, 20, 2),
pos=(-9.75, 0, -25.5),
color=(0, 0, 1))
self.b_4_box_3 = box(frame=self.b_4,
size=(2, 20, 53.7),
pos=(-28.5, 0, 0),
color=(0, 0, 1))
self.b_4_box_4 = box(frame=self.b_4,
size=(55.5, 10, 10),
pos=(-57.25, 0, 0),
color=(0, 0, 1))
def has_obj(self, obj):
return obj is self.b_4_box_4
if obj in self.b_1.objects:
return True
if obj in self.b_2.objects:
return True
if obj in self.b_3.objects:
return True
if obj in self.b_4.objects:
return True
return False
def set_angle_0(self, in_angle):
border = pi / 2.0
if in_angle >= border:
in_angle = border
elif in_angle < -border:
in_angle = border
sub_angle = in_angle - self.angle_0
if sub_angle != 0.0:
self.angle_0 = in_angle
self.b_2.rotate(angle=sub_angle, axis=(0,1,0))
if self.cmd_queue is not None:
self.cmd_queue.put((self.group_index, 2, (pi - (in_angle + border)) / pi))
def set_angle_1(self, in_angle):
sub_angle = in_angle - self.angle_1
if sub_angle != 0.0:
self.angle_1 = in_angle
self.b_3.rotate(angle=sub_angle, axis=(0,0,1))
if self.cmd_queue is not None:
offset = 140 / 180 * pi
self.cmd_queue.put((self.group_index, 1, (offset - in_angle) / pi))
def set_angle_2(self, in_angle):
sub_angle = in_angle - self.angle_2
if sub_angle != 0.0:
self.angle_2 = in_angle
self.b_4.rotate(angle=sub_angle, axis=(0,0,1))
if self.cmd_queue is not None:
offset = 122 / 180 * pi
self.cmd_queue.put((self.group_index, 0, (offset - in_angle) / pi))
def calc_angles(self, end_pos):
a = mag(self.b_4.pos)
b = 85.0
c = mag(end_pos)
c = c if c <= a + b else a + b
alfa_1 = math.acos((a * a + c * c - b * b) / (2 * a * c))
alfa_2 = pi - (math.acos((b * b + a * a - c * c) / (2 * b * a)))
betta = diff_angle((end_pos[0], end_pos[1]), (-1,0))
if dot((end_pos[0], end_pos[1]), (0, -1)) >= 0:
betta = betta - alfa_1
else:
betta = -betta - alfa_1
return (betta, alfa_2)
def set_pos(self, end_pos):
#1. найдём угол поворота плоскости b_2 относиельно b_1 и знак угла
v1 = self.b_1.world_to_frame(end_pos) - self.b_2.pos
v1.y = 0.0
b_2_angle = diff_angle(v1, (-1, 0, 0))
b_2_angle = b_2_angle if dot(v1, (0, 0, 1)) >= 0. else -b_2_angle
#2. перевод end_pos в координаты b_3
b_3_g_pos = self.b_2.world_to_frame(end_pos) - self.b_3.pos
angles = self.calc_angles(b_3_g_pos)
self.set_angle_0(b_2_angle)
self.set_angle_1(angles[0])
self.set_angle_2(angles[1])
def get_pos(self):
return self.b_4.frame_to_world((-85.0, 0, 0))
class Pauk:
def __init__(self, cmd_queue=None):
self.f_1 = MyFrame()
self.f_2 = MyFrame(frame=self.f_1, pos=(0, -20, 0))
self.f_2.rotate(axis=(0, 1, 0), angle=-pi / 4.0)
self.base_box = box(frame=self.f_2,
size=(50, 40, 50),
color=(1, 0, 0))
self.legs = []
self.legs.append(Leg({"pos": (25, 20 - 13.7, 0), "axis": (-1, 0, 0), "frame": self.f_2}, cmd_queue, 0))
self.legs.append(Leg({"pos": (-25, 20 - 13.7, 0), "axis": (1, 0, 0), "frame": self.f_2}, cmd_queue, 1))
self.legs.append(Leg({"pos": (0, 20 - 13.7, 25.), "axis": (0, 0, -1), "frame": self.f_2}, cmd_queue, 2))
self.legs.append(Leg({"pos": (0, 20 - 13.7, -25.), "axis": (0, 0, 1), "frame": self.f_2}, cmd_queue, 3))
self.time = 0.0
self.base_height = 120
self.set_pos((0, self.base_height, 0))
self.leg_len = 160
for id, point in enumerate(self.get_init_legs_points()):
self.legs[id].set_pos(point)
self.can_move = True
self.first_step = True
self.move_direction = vector(0, 0, 0)
self.move_time = 0.0
self.move_angle = 0.0
self.angle = 0.0
def set_angle(self, angle):
da = angle - self.angle
if da != 0.0:
self.f_1.rotate(angle=da, axis=self.f_1.up)
self.angle = angle
def get_angle(self):
return self.angle
def get_init_legs_points(self):
res = []
for leg in self.legs:
pos = self.f_1.world_to_frame(leg.b_2.frame_to_world((0, 0, 0)))
pos.y = 0
pos = norm(pos) * self.leg_len + vector(0, -1, 0) * self.base_height
print pos
res.append(self.f_1.frame_to_world(pos))
return res
def get_data_by_time(self, animation, time):
if len(animation) == 0:
return vector(0, 0, 0)
elif len(animation) == 1:
return vector(animation[0][1])
else:
if time < animation[0][0]:
return animation[0][1]
elif time >= animation[-1][0]:
return animation[-1][1]
for i in range(1, len(animation)):
if animation[i][0] >= time:
return vector(animation[i-1][1]) + (vector(animation[i][1]) - vector(animation[i-1][1])) *\
(time - animation[i-1][0]) / (animation[i][0] - animation[i - 1][0])
def set_next_move(self, direction, angle, time):
if not self.can_move:
self.move_direction = self.f_1.frame_to_world(direction) - self.f_1.pos
self.move_angle = angle
self.move_time = time
self.can_move = True
def get_leg_by_obj(self, obj):
for leg in self.legs:
if leg.has_obj(obj):
return leg
return None
def is_body(self, obj):
return obj is self.base_box
def get_pos(self):
return self.f_1.pos
def set_pos(self, pos):
self.f_1.pos = pos
#сделать шиг в заданном направление за заданное время.
def update(self, rate=30.0):
#разрешено движение.
if self.can_move:
#рассчёт анимации ног.
if self.first_step:
max_up = 20
leg_move_time = self.move_time / 4.0
dt = 0.0
#инимация перемещения
if self.move_angle == 0:
#определим последовательность ног
sequence = [0, 1, 2, 3]
dot_x = dot(self.f_1.axis, self.move_direction)
dot_z = dot(vector(-self.f_1.axis.z, self.f_1.axis.y, self.f_1.axis.x), self.move_direction)
if dot_x >= 0.0 and dot_z >= 0.0:
sequence = [0, 1, 2, 3]
elif dot_x < 0.0 and dot_z >= 0.0:
sequence = [2, 3, 0, 1]
elif dot_x < 0.0 and dot_z < 0.0:
sequence = [1, 0, 3, 2]
else:
sequence = [3, 2, 1, 0]
#ходьба 1
if 1:
#создание набора анимации
for id in sequence:
leg = self.legs[id]
leg.anim = [(0.0 + dt, vector(leg.get_pos())),
(leg_move_time / 2.0 + dt, (leg.get_pos() + self.move_direction / 2) + self.f_1.up * max_up),
(leg_move_time + dt, (leg.get_pos() + self.move_direction))]
dt = dt + leg_move_time
self.body_pos_anim = [(0.0, vector(self.get_pos())),
(self.move_time, self.get_pos() + self.move_direction)]
self.body_angle_anim = [(0.0, vector(self.angle, 0.0, 0.0))]
self.first_step = False
self.time = 0.0
self.max_time = self.move_time
#ходьба 2
else:
dt = self.move_time / 2
leg = self.legs[sequence[0]]
leg.anim = [(0.0, vector(leg.get_pos())),
(dt / 2, (leg.get_pos() + self.move_direction / 2 + self.f_1.up * max_up)),
(dt, (leg.get_pos() + self.move_direction))]
leg = self.legs[sequence[1]]
leg.anim = [(0.0, vector(leg.get_pos())),
(dt / 2, (leg.get_pos() + self.move_direction / 2 + self.f_1.up * max_up)),
(dt, (leg.get_pos() + self.move_direction))]
leg = self.legs[sequence[2]]
leg.anim = [(dt, vector(leg.get_pos())),
(dt + dt / 2.0, (leg.get_pos() + self.move_direction / 2 + self.f_1.up * max_up)),
(dt + dt, (leg.get_pos() + self.move_direction))]
leg = self.legs[sequence[3]]
leg.anim = [(dt, vector(leg.get_pos())),
(dt + dt / 2.0, (leg.get_pos() + self.move_direction / 2 + self.f_1.up * max_up)),
(dt + dt, (leg.get_pos() + self.move_direction))]
dt = self.move_time / 4
dv = self.move_direction / 4
dv_left = rotate(dv, angle=pi / 2, axis=self.f_1.up)
dv_rigth = rotate(dv, angle=-pi / 2, axis=self.f_1.up)
self.body_pos_anim = [(dt * 0, self.get_pos()),
(dt * 1, self.get_pos() + dv * 1 + dv_left + self.f_1.up * max_up),
(dt * 2, self.get_pos() + dv * 2),
(dt * 3, self.get_pos() + dv * 3 + dv_rigth + self.f_1.up * max_up),
(dt * 4, self.get_pos() + dv * 4)]
self.body_angle_anim = [(0.0, vector(self.angle, 0.0, 0.0))]
self.first_step = False
self.time = 0.0
self.max_time = self.move_time
else:
#определим последовательность ног
for id in [0, 1, 2, 3]:
leg = self.legs[id]
new_pos_1 = self.get_pos() + rotate(vector=(leg.get_pos() - self.get_pos()),
angle=self.move_angle / 2,
axis=self.f_1.up)
new_pos_2 = self.get_pos() + rotate(vector=(leg.get_pos() - self.get_pos()),
angle=self.move_angle,
axis=self.f_1.up)
leg.anim = [(0.0 + dt, vector(leg.get_pos())),
(leg_move_time / 2.0 + dt, new_pos_1 + self.f_1.up * max_up),
(leg_move_time + dt, new_pos_2)]
dt = dt + leg_move_time
self.body_pos_anim = [(0.0, vector(self.get_pos()))]
self.body_angle_anim = [(0.0, vector(self.angle, 0.0, 0.0)),
(self.move_time, vector(self.angle + self.move_angle, 0.0, 0.0))]
self.first_step = False
self.time = 0.0
self.max_time = self.move_time
self.time = self.time + 1.0 / rate
#анимация тела
self.set_pos(self.get_data_by_time(self.body_pos_anim, self.time))
self.set_angle(self.get_data_by_time(self.body_angle_anim, self.time)[0])
#анимация ног
for leg in self.legs:
leg.set_pos(self.get_data_by_time(leg.anim, self.time))
#проверка конца анимации.
if self.time >= self.max_time:
self.can_move = False
self.first_step = True