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;
}
}