static inline void QuaternionToAngleAxis(const float * quaternion,
float* angle_axis) {
const float q1 = quaternion[1];
const float q2 = quaternion[2];
const float q3 = quaternion[3];
const float sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3;
// For quaternions representing non-zero rotation, the conversion
// is numerically stable.
if (sin_squared_theta > 0.0f) {
const float sin_theta = sqrt(sin_squared_theta);
const float cos_theta = quaternion[0];
const float two_theta =
2.0 * ((cos_theta < 0.0)
? atan2(-sin_theta, -cos_theta)
: atan2(sin_theta, cos_theta));
const float k = two_theta / sin_theta;
angle_axis[0] = q1 * k;
angle_axis[1] = q2 * k;
angle_axis[2] = q3 * k;
} else {
// For zero rotation, sqrt() will produce NaN in the derivative since
// the argument is zero. By approximating with a Taylor series,
// and truncating at one term, the value and first derivatives will be
// computed correctly when Jets are used.
angle_axis[0] = q1 * 2.0f;
angle_axis[1] = q2 * 2.0f;
angle_axis[2] = q3 * 2.0f;
}
}
四元数转欧拉角代码
最新推荐文章于 2024-02-27 15:04:07 发布