四元数与旋转向量的转化

float r1, r2, r3;     	//旋转向量
float qx, qy, qz, qw; 	//四元数
float theta；	        //角度

//四元数转旋转向量
float theta = 2 * acosf(qw);
r1 = qx / sinf(theta * 0.5f) * theta;
r2 = qy / sinf(theta * 0.5f) * theta;
r3 = qz / sinf(theta * 0.5f) * theta;

//旋转向量转四元数
theta = sqrt(r1 * r1 + r2 * r2 + r3 * r3);
qw    = cosf(theta * 0.5f);
qx    = r1 * sinf(theta * 0.5f) / theta;
qy    = r2 * sinf(theta * 0.5f) / theta;
qz    = r3 * sinf(theta * 0.5f) / theta;