VPython—旋转坐标系

使用arrow( )创建三个坐标轴代表一个坐标系，其中X0-Y0-Z0为参考坐标系（固定不动），X-Y-Z为运动坐标系，这两个坐标系原点重合，运动坐标系可以绕参考坐标系或其自身旋转。在屏幕上输出一个转换矩阵，该矩阵描述了动坐标系相对于参考坐标系的姿态，矩阵第一列表示动坐标系的X轴在参考坐标系中的方向，第二列表示动坐标系的Y轴在参考坐标系中的方向，第二列表示动坐标系的Z轴在参考坐标系中的方向。显而易见，当两个坐标系姿态一致时，转换矩阵为3阶单位矩阵。

程序中按键盘的上下方向坐标系绕Y轴旋转，按左右键坐标系绕Z轴旋转，按W,S键绕X轴旋转。由于本例子使用的是VPython5，对于键盘事件只支持轮询方式检测，即在while循环中不停地检测是否有按键按下。在最新的VPython6中支持了键盘事件响应，可以自定义事件响应的回掉函数。

对于绕Z轴旋转这一动作来说，可以使绕参考坐标系的Z轴，也可以是绕动坐标系自身的Z轴。绕自身坐标轴旋转的角度称为欧拉角，而飞行器和航海中常用的RPY角（Roll-Pitch-Yaw:横滚、俯仰、偏航）是绕固定坐标系旋转。

对于旋转三个坐标轴来说可以将其放在一个自定义的旋转函数中，每次旋转只需调用该函数；也可以将其添加进同一个frame，旋转frame时frame中的所有对象都会跟着一起旋转。注意：下面的程序中旋转frame时是绕着参考坐标系的坐标轴旋转的，调用自定义的旋转函数旋转坐标系时是按照动坐标系的坐标轴旋转的。

# -*- coding: utf-8 -*-
from visual import *

scene.title = 'Coordinates' # 设置窗口标题
scene.width = 600           # 设置窗口宽度和高度
scene.height = 600
scene.forward = (-1,-1,-1)  # 设置camera视线方向

# Create a frame object,group objects together to make a composite object
f = frame()
 
# 定义参考坐标系
X0 = arrow(pos=(0,0,0), axis=(1,0,0), color=color.cyan, shaftwidth=0.05)
Y0 = arrow(pos=(0,0,0), axis=(0,1,0), color=color.cyan, shaftwidth=0.05)
Z0 = arrow(pos=(0,0,0), axis=(0,0,1), color=color.yellow,shaftwidth=0.05)


# 定义运动坐标系的X,Y,Z轴
X = arrow(frame = f, pos=(0,0,0), axis=(1,0,0), color=(1,0,0), shaftwidth=0.1)
Y = arrow(frame = f, pos=(0,0,0), axis=(0,1,0), color=(0,1,0), shaftwidth=0.1)
Z = arrow(frame = f, pos=(0,0,0), axis=(0,0,1), color=(0,0,1), shaftwidth=0.1)

tf_str = 'Transformation matrix:\n%-8.2f %-8.2f %-8.2f\n%-8.2f %-8.2f %-8.2f\n%-8.2f %-8.2f %-8.2f\n'%(1,0,0,0,1,0,0,0,1)

# With the label object you can display 2D text in a box, and the label always faces forward
info = label(pos=(-0.5,0.5,0), text = tf_str, box=False, opacity=0, height=16)


delta_angle = radians(2)  # 2°

def rotate_axis(angle, axis, origin):
    X.rotate(angle=angle, axis=axis, origin=origin)
    Y.rotate(angle=angle, axis=axis, origin=origin)
    Z.rotate(angle=angle, axis=axis, origin=origin)
    
while True:
    rate(100)
    
    if scene.kb.keys: # event waiting to be processed?
        s = scene.kb.getkey() # get keyboard info
        
        # 按左右箭头绕Z轴旋转
        if s == 'left': 
            #f.rotate(angle=delta_angle, axis=Z.axis, origin=(0,0,0))
            rotate_axis(angle = delta_angle, axis = Z.axis, origin = (0,0,0))
        elif s == 'right': 
            #f.rotate(angle=-delta_angle, axis=Z.axis, origin=(0,0,0))
            rotate_axis(angle = -delta_angle, axis = Z.axis, origin = (0,0,0))

        # 按上下箭头绕Y轴旋转    
        if s == 'up': 
            #f.rotate(angle=delta_angle, axis=Y.axis, origin=(0,0,0))
            rotate_axis(angle = delta_angle, axis = Y.axis, origin = (0,0,0))           
        elif s == 'down': 
            #f.rotate(angle=-delta_angle, axis=Y.axis, origin=(0,0,0))
            rotate_axis(angle = -delta_angle, axis = Y.axis, origin = (0,0,0))
            
        # 按w,s键绕X轴旋转    
        if s == 'w': 
            #f.rotate(angle=delta_angle, axis=X.axis, origin=(0,0,0))
            rotate_axis(angle = delta_angle, axis = X.axis, origin = (0,0,0))            
        elif s == 's': 
            #f.rotate(angle=-delta_angle, axis=X.axis, origin=(0,0,0))
            rotate_axis(angle = -delta_angle, axis = X.axis, origin = (0,0,0))

        info.text = 'Transformation matrix:\n%-8.2f %-8.2f %-8.2f\n%-8.2f %-8.2f %-8.2f\n%-8.2f %-8.2f %-8.2f\n'\
                        % (X.axis[0],Y.axis[0],Z.axis[0],\
                           X.axis[1],Y.axis[1],Z.axis[1],\
                           X.axis[2],Y.axis[2],Z.axis[2])