则(设旋转前坐标:(x0, y0, z0), 旋转后坐标:(x3, y3, z3)):
- # 四元数转换为欧拉角
- def quat2eular(w, x, y, z):
- # 笛卡尔坐标系
- roll = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
- pitch = math.asin(2 * (w * y - x * z))
- yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
- # Direct3D, 笛卡尔坐标的X轴变为Z轴, Y轴变为X轴, Z轴变为Y轴
- # roll = math.atan2(2 * (w * z + x * y), 1 - 2 * (z * z + x * x))
- # pitch = math.asin(2 * (w * x - y * z))
- # yaw = math.atan2(2 * (w * y + z * x), 1 - 2 * (x * x + y * y))
- # 转换为角度
- return math.degrees(yaw), math.degrees(pitch), math.degrees(roll)
- # 欧拉角转换为四元数, 旋转顺序为ZYX(偏航角yaw, 俯仰角pitch, 横滚角roll)
- def eular2quat(yaw, pitch, roll):
- # 注意这里必须先转换为弧度, 因为这里的三角计算均使用的是弧度.
- yaw = math.radians(yaw)
- pitch = math.radians(pitch)
- roll = math.radians(roll)
- cy, sy = math.cos(yaw * 0.5), math.sin(yaw * 0.5)
- cp, sp = math.cos(pitch * 0.5), math.sin(pitch * 0.5)
- cr, sr = math.cos(roll * 0.5), math.sin(roll * 0.5)
- # 笛卡尔坐标系
- w = cr * cp * cy + sr * sp * sy
- x = sr * cp * cy - cr * sp * sy
- y = cr * sp * cy + sr * cp * sy
- z = cr * cp * sy - sr * sp * cy
- # Direct3D, 笛卡尔坐标的X轴变为Z轴, Y轴变为X轴, Z轴变为Y轴
- # w = cr * cp * cy + sr * sp * sy
- # x = cr * sp * cy + sr * cp * sy
- # y = cr * cp * sy - sr * sp * cy
- # z = sr * cp * cy - cr * sp * sy
- return w, x, y, z
- if __name__ == "__main__":
- y, p, r = (10, 20, 30)
- q0, q1, q2, q3 = eular2quat(y, p, r)
- print(q0, q1, q2, q3)
- y, p, r = quat2eular(q0, q1, q2, q3)
- q0, q1, q2, q3 = eular2quat(y, p, r)
- print(q0, q1, q2, q3)
- print(quat2eular(q0, q1, q2, q3))