欧拉角与四元素转换

 1.欧拉角转换四元素

public class EulerAngles {
    /**
     * y轴方向
     */
    public float pitch;
    /**
     * z轴方向
     */
    public float yaw;
    /**
     * x轴方向
     */
    public float roll;

    public EulerAngles(float pitch, float yaw, float roll) {
        this.pitch = pitch;
        this.yaw = yaw;
        this.roll = roll;
    }

    /**
     * 四元素数据转换为欧拉角数据
     *
     * @param w
     * @param x
     * @param y
     * @param z
     */
    public EulerAngles(float w, float x, float y, float z) {
        // roll (x-axis rotation)
        float sinr_cosp = 2 * (w * x + y * z);
        float cosr_cosp = 1 - 2 * (x * x + y * y);
        this.roll = (float) Math.atan2(sinr_cosp, cosr_cosp);

        // pitch (y-axis rotation)
        float sinp = 2 * (w * y - z * x);
        if (Math.abs(sinp) >= 1) {
            // use 90 degrees if out of range
            this.pitch = Math.copySign(1.57075f, sinp);
        } else {
            this.pitch = (float) Math.asin(sinp);
        }

        // yaw (z-axis rotation)
        float siny_cosp = 2 * (w * z + x * y);
        float cosy_cosp = 1 - 2 * (y * y + z * z);
        this.yaw = (float) Math.atan2(siny_cosp, cosy_cosp);
    }

    /**
     * 欧拉角转换为四元素数据
     *
     * @return
     */
    public QuaternionUtils ToQuaternion() {
        //欧拉角转四元数，角度减半是因为四元数旋转计算时需要旋转两次，具体原理请查看四元数原理
        float cy = (float) Math.cos(yaw * 0.5f);
        float sy = (float) Math.sin(yaw * 0.5f);
        float cp = (float) Math.cos(pitch * 0.5f);
        float sp = (float) Math.sin(pitch * 0.5f);
        float cr = (float) Math.cos(roll * 0.5f);
        float sr = (float) Math.sin(roll * 0.5f);
        QuaternionUtils q = new QuaternionUtils();
        q.w = cy * cp * cr + sy * sp * sr;
        q.x = cy * cp * sr - sy * sp * cr;
        q.y = sy * cp * sr + cy * sp * cr;
        q.z = sy * cp * cr - cy * sp * sr;
        return q;
    }
}

2.四元素转换欧拉角

public class QuaternionUtils {
    public float w;
    public float x;
    public float y;
    public float z;

    public QuaternionUtils() {
    }

    public QuaternionUtils(float w, float x, float y, float z) {
        this.w = w;
        this.x = x;
        this.y = y;
        this.z = z;
    }

    /**
     * 向量旋转
     *
     * @param vector
     * @param q
     */
    static void VectorRotation(float[] vector, QuaternionUtils q) {
        QuaternionUtils qv = new QuaternionUtils(0, vector[0], vector[1], vector[2]);
        //四元数旋转公式q0*qv*（q0逆）s
        qv = QuaternionUtils.Multiplication(QuaternionUtils.Multiplication(q, qv), q.Inverse());
        vector[0] = qv.x;
        vector[1] = qv.y;
        vector[2] = qv.z;
    }


    /**
     * 返回欧拉角
     *
     * @return
     */
    public EulerAngles ToEulerAngles() {
        // roll (x-axis rotation)
        return new EulerAngles(this.w, this.x, this.y, this.z);
    }

    /**
     * 四元数相乘
     *
     * @param q0
     * @param q1
     * @return
     */
    static QuaternionUtils Multiplication(QuaternionUtils q0, QuaternionUtils q1) {
        QuaternionUtils ret = new QuaternionUtils();
        ret.w = q0.w * q1.w - q0.x * q1.x - q0.y * q1.y - q0.z * q1.z;
        ret.x = q0.w * q1.x + q0.x * q1.w + q0.y * q1.z - q0.z * q1.y;
        ret.y = q0.w * q1.y + q0.y * q1.w + q0.z * q1.x - q0.x * q1.z;
        ret.z = q0.w * q1.z + q0.z * q1.w + q0.x * q1.y - q0.y * q1.x;
        return ret;
    }

    /**
     * 四元数求逆
     */
    public QuaternionUtils Inverse() {
        QuaternionUtils ret;
        ret = this;
        ret.x *= -1;
        ret.y *= -1;
        ret.z *= -1;
        return ret;
    }
}