3d - pyopengl中用于旋转的pyquaternion
问题描述
我正在尝试使用 pyopgengl 创建一个简单的人类外骨骼模拟，并使用 pyquaternion 来处理旋转。线（l1 和 l2）连接形成接头。一条垂直的法线 (l3) 由关节处的有趣线创建。l2 应该能够围绕 l1 和 l3 旋转。
在代码中，箭头键围绕原点旋转相机*。按住 c 或 v 并使用键盘 + 或 - 来增加或减少引导线围绕其基线或 perp 线的旋转。按 Enter 在中间线或最远线之间循环。
有 5 行（2 行是 perp 行）。围绕其基础和 perp 轴旋转中线可以正常工作，但不适用于最远的线。尝试围绕其底部（中间）和 perp 旋转最远的线并没有得到预期的结果。
我怀疑我使用四元数存在一些问题。
*参见 main 以获得更准确的键绑定
import pygame
from pygame.locals import *
from OpenGL.GL import *
from OpenGL.GLU import *
import math as m
import numpy as np
from pyquaternion import Quaternion
class Point:
def __init__(self, name, x, y, z):
self.x = x
self.y = y
self.z = z
self.color = (0, 1, 0)
self.name = name
self.p = [self.x, self.y, self.z]
def __str__(self):
return '{} - x:{} y:{} z:{}'.format(self.name, self.x, self.y, self.z)
def __add__(self, other):
# print('other:', type(other))
x = self.x + other.x
y = self.y + other.y
z = self.z + other.z
p = Point('',x,y,z)
return p
def coor(self):
return (self.x, self.y, self.z)
def arr(self):
return self.p
def setpos(self, arr): # arr is a 3 long array
self.x = arr[0]
self.y = arr[1]
self.z = arr[2]
def draw(self):
glBegin(GL_POINTS)
glColor3fv(self.color)
glVertex3fv((self.x, self.y, self.z))
glEnd()
def rotate_z(self, a):
x = self.x
y = self.y
self.x = x * m.cos(a) - y * m.sin(a)
self.y = x * m.sin(a) + y * m.cos(a)
def rotate_y(self, a):
x = self.x
z = self.z
self.z = z * m.cos(a) - x * m.sin(a)
self.x = z * m.sin(a) + x * m.cos(a)
def rotate_x(self, a):
y = self.y
z = self.z
self.y = y * m.cos(a) - z * m.sin(a)
self.z = y * m.sin(a) + z * m.cos(a)
def translate(self, x, y, z):
self.x += x
self.y += y
self.z += z
# print(self)
def getMag(self):
return m.sqrt(self.x ** 2 + self.y ** 2 + self.z ** 2)
class Line:
def __init__(self, name, p1, p2): # p1 and p2 are Point instances
self.ogdir = p2
# tail
self.startp = p1
# tip
self.dir = p2
# direction vector
self.radVec = self.startp + self.dir
# perp vector
self.perp = 0
self.base = 0
self.a1 = 0
self.a2 = 0
self.q = 0
self.color = 1, 0, 0
self.name = name
self.update()
def update(self):
self.radVec = self.startp + self.dir
#self.endp = Point('', self.radVec[0], self.radVec[1], self.radVec[2])
def getRadVec(self):
self.update()
return self.radVec
def __str__(self):
return self.name
def draw(self):
self.update()
glBegin(GL_LINES)
glColor3fv(self.color)
glVertex3fv(self.startp.coor())
glVertex3fv(self.radVec.coor())
glEnd()
def rotate_x(self, a):
self.startp.rotate_x(a)
self.dir.rotate_x(a)
self.update()
print(self.radVec)
def rotate_y(self, a):
self.startp.rotate_y(a)
self.dir.rotate_y(a)
self.update()
def rotate_z(self, a):
self.startp.rotate_z(a)
self.dir.rotate_z(a)
self.update()
def translate(self, x, y, z):
self.startp.translate(x, y, z)
self.dir.translate(x, y, z)
self.update()
def uv(self): # makes self turn into a unit vector;
a = self.dir.getMag()
self.dir.x /= a
self.dir.y /= a
self.dir.z /= a
def setAxis(self, p, b):
self.perp = p
self.base = b
# needs fix
def rot(self, ax, deg): # axis is p=0 b=1
if ax == 0:
self.a1 += deg
if ax == 1:
self.a2 += deg
self.q = Quaternion(axis=self.base.dir.arr(), degrees=m.pi * self.a2) * Quaternion(axis=self.perp.dir.arr(), degrees=m.pi * self.a1)
self.dir.setpos(self.q.rotate(self.dir.arr()))
self.update()
def rotPerp(self, deg):
self.a1 += deg
self.q = Quaternion(axis=self.perp.dir.arr(), degrees=m.pi * self.a1)
self.dir.setpos(self.q.rotate(self.dir.arr()))
self.update()
def rotBase(self, deg):
self.a2 += deg
self.q = Quaternion(axis=self.base.dir.arr(), degrees=m.pi * self.a2)
self.dir.setpos(self.q.rotate(self.dir.arr()))
self.update()
def axis():
glBegin(GL_LINES)
glColor3fv((1, 0, 0))
glVertex3fv((1, 0, 0))
glVertex3fv((0, 0, 0))
glColor3fv((0, 1, 0))
glVertex3fv((0, 1, 0))
glVertex3fv((0, 0, 0))
glColor3fv((0, 0, 1))
glVertex3fv((0, 0, 1))
glVertex3fv((0, 0, 0))
glEnd()
def recPris(lv, sv, w, h, p, c): # lv and sv are Lines, w is the width of the square base, h is the height, p is the anchor point(lv.startp=0 or lb.radvec=1), c is a rgb array
# create unit vectors. Lines lv and ls must be perpendicular
l = np.array(lv.dir.arr()) / np.linalg.norm(lv.dir.arr())
s = np.array(sv.dir.arr()) / np.linalg.norm(sv.dir.arr())
# create 3rd perp vector
nv = np.cross(l, s)
n = nv / np.linalg.norm(nv)
if p == 0:
la = lv.startp.arr()
lb = np.add(lv.startp.arr(), np.negative(l * h))
elif p == 1:
la = np.add(lv.startp.arr(), l * h)
lb = lv.getRadVec().arr()
else:
print("incorrect p parameter")
points = [
[tuple(np.add(la, w / 2 * np.add(n, -s)))],
[tuple(np.add(la, w / 2 * np.add(-n, -s)))],
[tuple(np.add(la, w / 2 * np.add(-n, s)))],
[tuple(np.add(la, w / 2 * np.add(n, s)))],
[tuple(np.add(lb, w / 2 * np.add(n, -s)))],
[tuple(np.add(lb, w / 2 * np.add(-n, -s)))],
[tuple(np.add(lb, w / 2 * np.add(-n, s)))],
[tuple(np.add(lb, w / 2 * np.add(n, s)))],
]
edges = [
[0, 1],
[0, 3],
[0, 4],
[2, 1],
[2, 3],
[2, 6],
[5, 1],
[5, 4],
[5, 6],
[7, 3],
[7, 4],
[7, 6]
]
surfaces = [
[0, 1, 2, 3],
[1, 5, 6, 2],
[2, 6, 7, 3],
[3, 7, 4, 0],
[0, 4, 5, 1],
[4, 5, 6, 7]
]
# draw faces
glBegin(GL_QUADS)
for surface in surfaces:
glColor3fv(c)
for vertex in surface:
glVertex3fv(points[vertex])
glEnd()
# draw edges
glBegin(GL_LINES)
c[1] = c[1] / 2 + .3
for edge in edges:
for vertex in edge:
glColor3fv((c))
glVertex3fv(points[vertex])
glEnd()
def main():
pygame.init()
display = (1000, 800)
pygame.display.set_mode(display, DOUBLEBUF | OPENGL)
gluPerspective(90, (display[0] / display[1]), 0.1, 50.0)
glTranslatef(0.0, 0.0, -10)
glRotate(10, 1, 0, 0)
glRotate(-45, 0, 1, 0)
p1 = Point('p1', 0, 1, 0)
p2 = Point('p2', 2, 0, 0)
l1 = Line('l1', p1, p2)
l1.color = 0, 1, 1
p3 = Point('p3', 2, 1, 1)
l2 = Line('l2', l1.radVec, p3)
l2.color = 1, 0, 0
n = tuple(np.cross(p2.coor(), p3.coor()))
p4 = Point('p4', n[0], n[1], n[2])
l3 = Line('l3', l1.radVec, p4)
l3.color = 0, 0, 1
l3.uv()
p6 = Point('p6', 2, 0, 0)
l4 = Line('l4', l2.radVec, p6)
l4.color = 0, 1, 0
n1 = tuple(np.cross(l2.dir.coor(), l4.dir.coor()))
p7 = Point('p7', n1[0], n1[1], n[2])
l5 = Line('l5', l2.radVec, p7)
l5.uv()
objects = [l2, l4]
c1 = 0
c = 0
cur = objects[c]
print(type(objects[c]), objects[c], c)
lines = [l1, l2, l3, l4, l5]
while True:
# update point data
l2.startp = l1.radVec
l3.startp = l1.radVec
l4.startp = l2.radVec
l5.startp = l2.radVec
l3.dir.setpos(np.cross(l1.dir.coor(), l2.dir.coor()))
l5.dir.setpos(np.cross(l2.dir.coor(), l4.dir.coor()))
l3.uv()
l5.uv()
l2.setAxis(l3, l1)
l4.setAxis(l5, l2)
#q = Quaternion(axis=cur.perp.dir.arr(), degrees=cur.a1) * Quaternion(axis=cur.base.dir.arr(), degrees=cur.a2)
#cur.dir.setpos(q.rotate(cur.dir.arr()))
# key commmands
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
quit()
# zoom
if event.type == pygame.MOUSEBUTTONDOWN:
if event.button == 4:
glTranslate(0, 0, 1.0)
if event.button == 5:
glTranslate(0, 0, -1.0)
# navigation
keys = pygame.key.get_pressed()
if keys[pygame.K_LEFT]:
glRotate(5, 0, 1, 0)
if keys[pygame.K_RIGHT]:
glRotate(-5, 0, 1, 0)
if keys[pygame.K_UP]:
glRotate(5, 1, 0, 0)
if keys[pygame.K_DOWN]:
glRotate(-5, 1, 0, 0)
if keys[pygame.K_KP_ENTER]: # changes line object
c = (c + 1) % len(objects)
cur = objects[c]
print(cur, c)
# move object
if keys[pygame.K_x]:
# print(l2.startp)
if keys[pygame.K_KP_PLUS]:
cur.translate(.05, 0, 0)
if keys[pygame.K_KP_MINUS]:
cur.translate(-.05, 0, 0)
if keys[pygame.K_KP4]:
cur.rotate_x(m.pi / 12)
if keys[pygame.K_KP6]:
cur.rotate_x(-m.pi / 12)
if keys[pygame.K_y]:
if keys[pygame.K_KP_PLUS]:
cur.translate(0, .05, 0)
if keys[pygame.K_KP_MINUS]:
cur.translate(0, -.05, 0)
if keys[pygame.K_KP4]:
cur.rotate_y(m.pi / 12)
if keys[pygame.K_KP6]:
cur.rotate_y(-m.pi / 12)
if keys[pygame.K_z]:
if keys[pygame.K_KP_PLUS]:
cur.translate(0, 0, .05)
if keys[pygame.K_KP_MINUS]:
cur.translate(0, 0, -.05)
if keys[pygame.K_KP4]:
cur.rotate_z(m.pi / 12)
if keys[pygame.K_KP6]:
cur.rotate_z(-m.pi / 12)
# rotate cur about perp
if keys[pygame.K_v]:
if keys[pygame.K_KP_PLUS]:
cur.rot(0,1)
if keys[pygame.K_KP_MINUS]:
cur.rot(0,-1)
# rotate cur about base
if keys[pygame.K_c]:
if keys[pygame.K_KP_PLUS]:
cur.rot(1,1)
if keys[pygame.K_KP_MINUS]:
cur.rot(1,-1)
# glRotatef(1,1,0,0)
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
# Cube()
axis()
l1.draw()
l2.draw()
l3.draw()
l4.draw()
l5.draw()
#recPris(l4,l5,.5,l4.dir.getMag()/2,1,[0,0,1])
pygame.display.flip()
pygame.time.wait(10)
main()
解决方案
推荐阅读
- php - 当我上传课程文件时，如何增加 Moodle 中的文件上传大小？
- debugging - 调试 yacc YYDEBUG y.debug 在哪里
- javascript - {{#each}} 未执行。Handlebars.compile(templateSource) 无法正常工作
- c - after generate the rpc files and try to compile it i get an conflict error
- ios - 过滤CoreData结果 - 列表（UI）不根据过滤结果更新
- google-apps-script - MailApp.send 电子邮件 - 未发送消息/邮件因外部电子邮件 ID 被退回/没有添加在谷歌表格、谷歌表单中工作
- flutter - 我如何在颤振应用程序中解决这个问题
- c++ - 在 C++ 中，派生类（参数比基类多）可以使用基类函数和重载运算符吗？
- django-rest-framework - Django Rest Framework - 更新视图中的 ForeignKey Field 条目
- python - 我正在开发一个有多个页面的 GUI（我使用不同的类来显示不同的页面）。我面临的问题是导航