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]))