def get_point_on_plain(self, m_pos, plain):
'''Преобразует курсор m_pos в точку на плоскости'''
n = vector.Vector(plain[0])
pos = vector.Vector(plain[1])
p1 = self.get_pos(m_pos)
p0 = self.eye.matrix[:3, 3]
d = n.dot(p1 - p0)
if d == 0:
return vector.Vector(0, 0, 0)
r = n.dot(pos - p0) / d
return vector.Vector(p0 + r * (p1 - p0))
def rotate_by_normal(self, plane, base_axis, freedom, global_normal):
'''Сориентировать звено по проекции плоскости'''
# 1. преобразуем global_normal нормаль в локальную
if self.parent is not None and isinstance(self.parent, node.Node):
normal = self.parent.world_to_frame(
vector.Vector(0, 0, 0) + global_normal) -\
self.parent.world_to_frame(vector.Vector(0, 0, 0))
else:
normal = global_normal
# 2. найдём cross нормалей
horizont = normal.cross(plane[0].cross(plane[1]))
# найдём угол горизонта
if horizont.mag > 0:
angle = self.get_proj_angle(plane[0], plane[1], horizont)
self.set_proj_angle(freedom, angle, plane[0], plane[1], base_axis)
def calk_ik_pos(self, glob_target, end=vector.Vector(0, 0, 0)):
'''Рассчёт инверсной кинематики'''
# print "calk_ik_pos(calk_ik_pos:{})".format(glob_target)
# 1. переведём в локальную систему координат
target = self.world_to_frame(glob_target)
# вращение по z
if self.freedom_z_angle is not None:
self.calk_ik_on_plane(
(vector.Vector(1, 0, 0), vector.Vector(0, 1, 0)), self.axis,
self.freedom_z_angle, target, end)
# вращение по y
if self.freedom_y_angle is not None:
self.calk_ik_on_plane(
(vector.Vector(0, 0, 1), vector.Vector(1, 0, 0)),
self.axis.cross(self.up), self.freedom_y_angle, target, end)
# вращение по x
if self.freedom_x_angle is not None:
self.calk_ik_on_plane(
(vector.Vector(0, 1, 0), vector.Vector(0, 0, 1)), self.up,
self.freedom_x_angle, target, end)
# рассчитаем инематику для родителя
if self.parent is not None and isinstance(self.parent, node.Node):
self.parent.calk_ik_pos(
glob_target,
self.parent.world_to_frame(self.frame_to_world(end)))
return self.frame_to_world(end)
def get_pos(self, pos):
'''Преобразует курсор pos в точку в плоскости камеры'''
x = pos.x()
y = pos.y()
viewport = glGetIntegerv(GL_VIEWPORT)
projection = glGetDoublev(GL_PROJECTION_MATRIX)
modelview = np.identity(4)
y = viewport[3] - y
z = 0
return vector.Vector(
gluUnProject(x, y, z, modelview, projection, viewport))
def move_end(self, pos):
# 1. calk p_0 angle - vertical angle
self.p_0.ang_y = self.get_proj_angle(
self.o_z,
self.o_x,
self.world_to_frame(pos))
# 2. calk tiangle
cur_pos = self.p_0.world_to_frame(pos)
len_a = self.p_2.pos.mag
len_b = self.end.pos.mag
len_c = cur_pos.mag
# 2.1 p_1 angle
tmp = (2 * len_a * len_c)
if tmp != 0:
tmp2 = (len_a * len_a + len_c * len_c - len_b * len_b)
tmp3 = tmp2 / tmp
try:
angle = math.acos(tmp3)
except ValueError:
angle = 0.0
else:
angle = math.pi
dir_angle = cur_pos.diff_angle(vector.Vector(0, 0, 1))
# detect sign
dir_angle = dir_angle if cur_pos.dot(
vector.Vector(0., -1.0, 0.0)) > 0.0 else -dir_angle
self.p_1.ang_x = dir_angle - angle
# 2.2 p_2 angle
try:
angle = math.acos(
(len_b * len_b + len_a * len_a - len_c * len_c) /
(2 * len_a * len_b))
except ValueError:
angle = math.pi
self.p_2.ang_x = math.pi - angle
def __init__(self,
color=(1.0, 1.0, 1.0, 1.0),
show_center=False,
offset=vector.Vector(0.0, 0.0, 0.0),
center_length=10.0,
**kwargs):
'''Base class for all shapes (box, cilinder, sphere).
colol - shape color
show_center - show center of shape
offset - offset of shape
'''
node.Node.__init__(self, **kwargs)
self.color = color
self.list_id = None
self.visible = True
self.first_make = True
self.show_center = show_center
self.offset = offset
self.center_length = center_length
def calk_ik_pos_2(self, targets):
'''Рассчёт инверсной кинематики'''
# 1. переведём в локальную систему координат
cur_targets = []
glob_targets = []
for glob_target, end, weight in targets:
glob_targets.append(glob_target)
cur_targets.append((self.world_to_frame(glob_target), end, weight))
for glob_target, end, weight in self.targets:
glob_targets.append(glob_target)
cur_targets.append((self.world_to_frame(glob_target), end, weight))
# вращение по z
if self.freedom_z_angle is not None:
self.calk_ik_on_plane_2(
(vector.Vector(1, 0, 0), vector.Vector(0, 1, 0)), self.axis,
self.freedom_z_angle, cur_targets)
# вращение по y
if self.freedom_y_angle is not None:
self.calk_ik_on_plane_2(
(vector.Vector(0, 0, 1), vector.Vector(1, 0, 0)),
self.axis.cross(self.up), self.freedom_y_angle, cur_targets)
# вращение по x
if self.freedom_x_angle is not None:
self.calk_ik_on_plane_2(
(vector.Vector(0, 1, 0), vector.Vector(0, 0, 1)), self.up,
self.freedom_x_angle, cur_targets)
# рассчитаем кинематику для родителя
if self.parent is not None and isinstance(self.parent, node.Node):
data = [(g, self.parent.world_to_frame(self.frame_to_world(t[1])),
t[2]) for g, t in zip(glob_targets, cur_targets)]
self.parent.calk_ik_pos_2(data)
return self.frame_to_world(end)
def rotate_by_normal_y(self, global_normal=vector.Vector(0, 1, 0)):
self.rotate_by_normal((vector.Vector(0, 0, 1), vector.Vector(1, 0, 0)),
self.axis.cross(self.up), self.freedom_y_angle,
global_normal)
def __init__(self, **kwargs):
node.Node.__init__(self, **kwargs)
self.koleno = node.Node(parent=self)
self.eye = node.Node(
parent=self.koleno, pos=vector.Vector(0, 0, 2000))
def rotate_camera(self, x, y):
self.rotate(x, vector.Vector(0, 1, 0))
self.koleno.rotate(y, vector.Vector(1, 0, 0))
def get_plain(self):
'''возвращает плосоксть камеры: (нормаль, позицию)'''
return (
vector.Vector(self.eye.matrix[:3, 2] * -1.0),
vector.Vector(self.matrix[:3, 3]))
