欧拉角与四元数互转，及四元数slerp球面线性插值算法
Posted 程序媛一枚~
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了欧拉角与四元数互转，及四元数slerp球面线性插值算法相关的知识，希望对你有一定的参考价值。
欧拉角与四元数互转，及四元数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.4以上是关于欧拉角与四元数互转，及四元数slerp球面线性插值算法的主要内容，如果未能解决你的问题，请参考以下文章