Quaternion conversion to "exponential map" (or "rotation vector" or "Euler vector" or "exponential twist"). This code looks compact, efficient, precise for me. Please can you improve it or criticize? Thanks.
std::vector<double> quaternionToExpMap(const Quaternion& q) {
double sinHalfTheta = std::sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
double scale = (q.w < 0.0) ? -2.0 : 2.0;
if (sinHalfTheta != 0.0) {
scale *= std::atan2(sinHalfTheta, std::abs(q.w)) / sinHalfTheta;
}
return {q.x * scale, q.y * scale, q.z * scale};
}