【Python、Unity3d】欧拉角计算得到物体坐标系正上、正前方向向量（transform.up和transform.forward）

问题

• 写在最前面：问题的本质是物体姿态四元数右乘一个方向向量，相关Unity的解决方案有很多，问题是在Python里如何计算。

已知：已从Unity中获取到物体每一时刻的欧拉角

transform.rotation.eulerAngles.x
transform.rotation.eulerAngles.y
transform.rotation.eulerAngles.z

问题：需要由欧拉角得到物体每一时刻在自己坐标系的正上、正前方向向量

• 例如这棵树的初始姿态，欧拉角对应(0， 0， 0)
  方向向量-正上，对应(0， 1， 0)
  方向向量-正前，对应(0， 0， 1)
  z轴为前、y轴为上
  
• 现经过一系列旋转到达末姿态，想要知道此时树干方向的向量
  方向向量-相对于物体坐标系的正上、正前
  

一、原理

参考 【转】Unity四元数和向量相乘作用及其运算规则

参考 四元数和向量相乘作用及其运算规则

二、Unity中的实现

在Unity里是个相当简单的问题，下面几行结果都是一样的
本质是物体姿态的四元数右乘一个方向向量（up或forward）

//物体坐标系的正上：

Debug.Log(transform.up);
Debug.Log(transform.rotation*Vector3.up);
Debug.Log(Quaternion.Euler(transform.rotation.eulerAngles) * Vector3.up)；

//物体坐标系的正前：

Debug.Log(transform.forward);
Debug.Log(transform.rotation*Vector3.forward);

三、Python中的实现

脱离了Unity，原始数据只有欧拉角，开始处理数据
先欧拉角转四元数，再一系列右乘，最后的结果与Unity的transform.up值相同

import pandas as pd
import os
import numpy as np
import math
from math import cos, sin, pi, atan2, asin


def euler2quaternion(r, p, y):
	# euler to quaternion
    sinp = math.sin(math.radians(p / 2))
    siny = math.sin(math.radians(y / 2))
    sinr = math.sin(math.radians(r / 2))

    cosp = math.cos(math.radians(p / 2))
    cosy = math.cos(math.radians(y / 2))
    cosr = math.cos(math.radians(r / 2))

    w = cosr * cosp * cosy + sinr * sinp * siny
    x = sinr * cosp * cosy - cosr * sinp * siny
    y = cosr * sinp * cosy + sinr * cosp * siny
    z = cosr * cosp * siny - sinr * sinp * cosy

    return w, x, y, z


def quaternion_multiply(w1, x1, y1, z1, w2, x2, y2, z2):
    # two quaternion multiply
    q1q2 = [w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2,
            w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2,
            w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2,
            w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2]
    # print(q1q2)
    return q1q2


def quaternion2vector(w1, x1, y1, z1):
    # 四元数右乘方向向量
    # w2, x2, y2, z2 = 0, 0, 1, 0对应Vector3.up=(0, 1, 0)
    # 如果是Vector3.forward=(0, 0, 1)，则w2, x2, y2, z2 = 0, 0, 0, 1
    w2, x2, y2, z2 = 0, 0, 1, 0
    
    # Q乘V
    qv = quaternion_multiply(w1, x1, y1, z1, w2, x2, y2, z2)
	
	# Q的模
    nq = np.sqrt(w1 ** 2 + x1 ** 2 + y1 ** 2 + z1 ** 2)
	
	# Q的逆矩阵
    q_ni = [w1 / nq, -x1 / nq, -y1 / nq, -z1 / nq]
	
	# QV乘Q逆
    qvq = quaternion_multiply(qv[0], qv[1], qv[2], qv[3], q_ni[0], q_ni[1], q_ni[2], q_ni[3])
	
	# 保留四位小数
    vector = [round(qvq[1], 4), round(qvq[2], 4), round(qvq[3], 4)]

    return vector

qw, qx, qy, qz = euler2quaternion(0, 90, 0)
transform_up = quaternion2vector(qw, qx, qy, qz)

print(transform_up)