欧拉角与四元数互转，及四元数slerp球面线性插值算法

1. 欧拉角与四元数是什么？

roll：翻滚角，pitch：俯仰角，heading：航向角

roll、pitch、heading，这3个角又称为欧拉角，欧拉角是弧度。弧度与度°可以通过公式转换；

四元数：w，x，y，z，有 xx+yy+zz+ww = 1，四元数在计算机图形学中是姿态和姿态内插中常用的一种表达。
四元数更能表达光滑移动的相机，球面线性插值具有连续性，在旋转之间做内插和形成刚性变换链也都比较容易。

欧拉角与四元数可以互转，四元数插值完在转回欧拉角，对于航向角突变的情况会更准确；

• Math.toDegrees(eulerAngles.roll); // 弧度转角度
• Math.toRadians(roll); // 角度转弧度
• roll 范围 [-180°~180°]
• pitch 范围 [-180°~180°]
• heading 范围 [0°~360°]

2. 源码

2.1 欧拉角类

package test;

/****************************
 * Class Name： EulerAngles
 * Description： <欧拉角类>
 * @Author: seminar
 * @create: 2021/05/21
 * @since: 1.0.0
 ***************************/
public class EulerAngles {
    /**
     * Math.toRadians(roll) 角度转弧度
     * Math.toDegrees(roll) 弧度转角度
     * <p>
     * 翻滚角（roll） 弧度
     */
    public double roll;

    /**
     * 俯仰角(pitch) 弧度
     */
    public double pitch;

    /**
     * yaw 即heading(航向角) 弧度
     */
    public double yaw;

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

    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) {
            this.pitch = Math.copySign(1.57075f, sinp); // use 90 degrees if out of range
        } 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);
    }

    public Quaternion 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);
        Quaternion q = new Quaternion();
        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.2 四元数类

package test;

import lombok.extern.slf4j.Slf4j;

/****************************
 * Class Name： Quaternion
 * Description： <四元数类>
 * @Author: seminar
 * @create: 2021/05/21
 * @since: 1.0.0
 ***************************/
@Slf4j
public class Quaternion {
    public float w;
    public float x;
    public float y;
    public float z;

    public Quaternion() {
    }

    public Quaternion(Quaternion b) {
        this.w = b.w;
        this.x = b.x;
        this.y = b.y;
        this.z = b.z;
    }

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

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


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

    //四元数相乘
    static Quaternion Multiplication(Quaternion q0, Quaternion q1) {
        Quaternion ret = new Quaternion();
        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 Quaternion Inverse() {
        Quaternion ret;
        ret = this;
        ret.x *= -1;
        ret.y *= -1;
        ret.z *= -1;
        return ret;
    }
}

2.3 欧拉角与四元数互转及球面线性插值算法

球面线性插值也称四元数内插，更加光滑；

package test;

import test.EulerAngles;
import test.Quaternion;
import lombok.extern.slf4j.Slf4j;

import static java.lang.Math.abs;

/*************************************
 *Class Name: EulerAngle2QuatUtil
 *Description: <四元数与欧拉角互转>
 *@author: seminar
 *@create: 2021/5/24
 *@since 1.0.0
 *************************************/
@Slf4j
public class EulerAngle2QuatUtil {

    /**
     * 归一化
     *
     * @param x
     * @param y
     * @param z
     * @param w
     * @return
     */
    public Quaternion normalizeQuaternion(float w, float x, float y, float z) {
        double lengthD = 1.0f / (w * w + x * x + y * y + z * z);
        w *= lengthD;
        x *= lengthD;
        y *= lengthD;
        z *= lengthD;
        return new Quaternion(w, x, y, z);
    }

    /**
     * Slerp球面线性插值（Spherical Linear Interpolation）
     *
     * @param a 原始数据a
     * @param b 原始数据b
     * @param t 要插值的比例（中间插一个值1/2）
     * @return
     */
    public Quaternion makeInterpolated(Quaternion a, Quaternion b, double t) {
        Quaternion out = new Quaternion();
        double cosHalfTheta = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
        if (cosHalfTheta < 0.0F) {
            b = new Quaternion(b);
            cosHalfTheta = -cosHalfTheta;
            b.x = -b.x;
            b.y = -b.y;
            b.z = -b.z;
            b.w = -b.w;
        }

        double halfTheta = (double) Math.acos((double) cosHalfTheta);
        double sinHalfTheta = (double) Math.sqrt((double) (1.0F - cosHalfTheta * cosHalfTheta));
        double ratioA;
        double ratioB;
        if ((double) abs(sinHalfTheta) > 0.001D) {
            double oneOverSinHalfTheta = 1.0F / sinHalfTheta;
            ratioA = (double) Math.sin((double) ((1.0F - t) * halfTheta)) * oneOverSinHalfTheta;
            ratioB = (double) Math.sin((double) (t * halfTheta)) * oneOverSinHalfTheta;
        } else {
            ratioA = 1.0F - t;
            ratioB = t;
        }

        out.x = (float) (ratioA * a.x + ratioB * b.x);
        out.y = (float) (ratioA * a.y + ratioB * b.y);
        out.z = (float) (ratioA * a.z + ratioB * b.z);
        out.w = (float) (ratioA * a.w + ratioB * b.w);

        out = normalizeQuaternion(out.w, out.x, out.y, out.z);
        return out;
    }

    /**
     * 欧拉角（弧度）转四元数
     *
     * @param pitch
     * @param yaw
     * @param roll
     * @return
     */
    public Quaternion toQuaternion(double pitch, double yaw, double roll) {
        EulerAngles eu = new EulerAngles((float) Math.toRadians(pitch), (float) Math.toRadians(yaw), (float) Math.toRadians(roll)); // 角度转弧度
        return eu.toQuaternion();
    }

    /**
     * 四元数转欧拉角（弧度）
     *
     * @param quaternion
     * @return
     */
    public EulerAngles toEulerAngles(Quaternion quaternion) {
        return quaternion.toEulerAngles();
    }

    /**
     * 姿态角——即欧拉角转四元数，对俩个四元数进行球面插值，四元数转回欧拉角并返回
     *
     * @param pitch    位置一俯仰角 -180~180
     * @param yaw      位置一航向角 0~360
     * @param roll     位置一翻滚角 -180~180
     * @param pitch1   位置二俯仰角 -180~180
     * @param yaw1     位置二俯仰角 0~360°
     * @param roll1    位置二翻滚角 -180~180
     * @param t        位置一时间
     * @param t1       位置二时间
     * @param t_insert 要计算姿态角的位置对应时间
     * @return
     */
    public EulerAngles slerpInsert(float pitch, float yaw, float roll, float pitch1, float yaw1, float roll1, long t, long t1, long t_insert) {
        // 位置1 欧拉角转四元数
        // 位置2 欧拉角转四元数
        Quaternion p = toQuaternion(pitch, yaw, roll);
        Quaternion q = toQuaternion(pitch1, yaw1, roll1);

        // 计算插入的scale
        float scale = (float) ((t_insert - t) / ((t1 - t) * 1.0));

        // Slerp球面线性插值
        Quaternion r = makeInterpolated(q, p, scale);

        // 四元数转欧拉角
        EulerAngles eulerAngles = r.toEulerAngles();
        return eulerAngles;
    }

    public static void main(String[] args) {
//        示例，中间1615609866585L的插值不太对
//                         Roll    Pitch    Heading
// 1615609866544L         -0.9    -0.405   358.809
// 1615609866585L         -0.942   -0.362  314.489
// 1615609866625L         -0.956   -0.331  0.178

//        正确结果
//                         Roll    Pitch    Heading
// 1615609866544L         -0.9,    -0.405,  358.809
// 1615609866585L         -0.929， -0.368， 359.502
// 1615609866625L         -0.956,  -0.331,  0.178

        // 调用EulerAngle2QuatUtil实现姿态角插值的获取
        float roll = -0.9f, pitch = -0.405f, yaw = 358.809f;
        EulerAngle2QuatUtil eq = new EulerAngle2QuatUtil();
        Quaternion p = eq.toQuaternion(pitch, yaw, roll);
        log.info("p: {} {} {} {}", p.w, p.x, p.y, p.z);

        float roll1 = -0.956f, pitch1 = -0.331f, yaw1 = 0.178f;
        Quaternion q = eq.toQuaternion(pitch1, yaw1, roll1);
        log.info("q: {} {} {} {}", q.w, q.x, q.y, q.z);

        long t = 1615609866544L;
        long t1 = 1615609866625L;
        long t_insert = 1615609866585L;
        float scale = (float) ((t_insert - t) / ((t1 - t) * 1.0));

        // Slerp球面线性插值
        Quaternion r = eq.makeInterpolated(q, p, scale);
        EulerAngles eulerAngles = r.toEulerAngles();
        float roll2 = (float) Math.toDegrees(eulerAngles.roll); // 弧度转回角度
        float pitch2 = (float) Math.toDegrees(eulerAngles.pitch); // 弧度转回角度
        float heading2 = (float) (Math.toDegrees(eulerAngles.yaw) > 0 ? Math.toDegrees(eulerAngles.yaw) : Math.toDegrees(eulerAngles.yaw) + 360); // 弧度转回角度(航向角0~360°)

        log.info("{} {} {}", Double.parseDouble(String.format("%.3f", roll2)), Double.parseDouble(String.format("%.3f", pitch2)), Double.parseDouble(String.format("%.3f", heading2)));

        testSlerpInsert(pitch, yaw, roll, pitch1, yaw1, roll1, t, t1, t_insert);

//  0.000     -8.523      0.000
//  0.000     -0.432     93.112
        testSlerpInsert(-8.523f, 0.00f, 0.00f, -0.432f, 93.112f, 0.00f, t, t1, t_insert);
//        0.000      1.054     66.847
//        1.237     -1.956     62.336
        testSlerpInsert(1.054f, 66.847f, 0.00f, -1.956f, 62.336f, 1.237f, t, t1, t_insert);

//        0.411      5.393    338.058
//        0.402      5.395    338.063
        testSlerpInsert(5.393f, 338.058f, 0.411f, 5.395f, 338.063f, 0.402f, t, t1, t_insert);
    }

    private static void testSlerpInsert(float pitch, float yaw, float roll, float pitch1, float yaw1, float roll1, long t, long t1, long t_insert) {
        log.info("==================testSlerpInsert start===============");
        EulerAngle2QuatUtil eq = new EulerAngle2QuatUtil();
        EulerAngles eulerAngles = eq.slerpInsert(pitch, yaw, roll, pitch1, yaw1, roll1, t, t1, t_insert);
        float roll2 = (float) Math.toDegrees(eulerAngles.roll); // 弧度转回角度
        float pitch2 = (float) Math.toDegrees(eulerAngles.pitch); // 弧度转回角度
        float heading2 = (float) (Math.toDegrees(eulerAngles.yaw) > 0 ? Math.toDegrees(eulerAngles.yaw) : Math.toDegrees(eulerAngles.yaw) + 360); // 弧度转回角度(航向角0~360°)

        log.info("slerpInsert {} {} {}", Double.parseDouble(String.format("%.3f", roll2)), Double.parseDouble(String.format("%.3f", pitch2)), Double.parseDouble(String.format("%.3f", heading2)));
        log.info("==================testSlerpInsert end=================");
    }

    private static Quaternion getQuaternion(float roll, float pitch, float yaw) {
        EulerAngle2QuatUtil eq = new EulerAngle2QuatUtil();
        EulerAngles eu = new EulerAngles((float) Math.toRadians(pitch), (float) Math.toRadians(yaw), (float) Math.toRadians(roll));
        Quaternion quaternion = eu.toQuaternion();
        EulerAngles eulerAngles = quaternion.toEulerAngles();
        float roll2 = (float) Math.toDegrees(eulerAngles.roll); // 弧度转回角度
        float pitch2 = (float) Math.toDegrees(eulerAngles.pitch); // 弧度转回角度
        float heading2 = (float) (Math.toDegrees(eulerAngles.yaw) > 0 ? Math.toDegrees(eulerAngles.yaw) : Math.toDegrees(eulerAngles.yaw) + 360); // 弧度转回角度(航向角0~360°)
        log.info("toDegree: {} {} {}", Double.parseDouble(String.format("%.3f", roll2)), Double.parseDouble(String.format("%.3f", pitch2)), Double.parseDouble(String.format("%.3f", heading2)));
        return quaternion;
    }
}

参考

• https://blog.csdn.net/xiaoma_bk/article/details/79082629?utm_medium=distribute.pc_aggpage_search_result.none-task-blog-2_{aggregatepage}first_rank_v2~rank_aggregation-6-79082629.pc_agg_rank_aggregation&utm_term=%E5%9B%9B%E5%85%83%E6%95%B0%E6%AC%A7%E6%8B%89%E8%A7%92%E8%BD%AC%E6%8D%A2%E5%85%AC%E5%BC%8F&spm=1000.2123.3001.4430
• 在线转换工具
• 四元数插值
• 四元数插值2
• 四元数与欧拉角互转

最后

以上就是阔达小兔子为你收集整理的欧拉角与四元数互转，及四元数slerp球面线性插值算法的全部内容，希望文章能够帮你解决欧拉角与四元数互转，及四元数slerp球面线性插值算法所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。