[vlc-devel] [PATCH 18/18] viewpoint: use quaternion instead of euler angles
Alexandre Janniaux
ajanni at videolabs.io
Wed Mar 31 09:25:50 UTC 2021
It enables viewpoint producers to provide an orientation without
singularities at north and south poles.
Refs #18089, #18760
---
include/vlc_viewpoint.h | 18 +--
src/misc/viewpoint.c | 240 ++++++++++++++++++++++++++++++++--------
2 files changed, 205 insertions(+), 53 deletions(-)
diff --git a/include/vlc_viewpoint.h b/include/vlc_viewpoint.h
index b3fbe812c7..b3336b5ae0 100644
--- a/include/vlc_viewpoint.h
+++ b/include/vlc_viewpoint.h
@@ -37,24 +37,28 @@
/**
* Viewpoints
*/
+
struct vlc_viewpoint_t {
- float yaw; /* yaw in degrees */
- float pitch; /* pitch in degrees */
- float roll; /* roll in degrees */
+ /**
+ * orientation quaternion with the following properties:
+ * 1/ use ijk = -1 for the operations
+ * 2/ memory layout is [x, y, z, w] (like GLSL)
+ * 3/ system is right-handed
+ */
+ float quat[4];
float fov; /* field of view in degrees */
};
static inline void vlc_viewpoint_init( vlc_viewpoint_t *p_vp )
{
- p_vp->yaw = p_vp->pitch = p_vp->roll = 0.0f;
+ p_vp->quat[3] = 1;
+ p_vp->quat[0] = p_vp->quat[1] = p_vp->quat[2] = 0;
p_vp->fov = FIELD_OF_VIEW_DEGREES_DEFAULT;
}
static inline void vlc_viewpoint_clip( vlc_viewpoint_t *p_vp )
{
- p_vp->yaw = fmodf( p_vp->yaw, 360.f );
- p_vp->pitch = fmodf( p_vp->pitch, 360.f );
- p_vp->roll = fmodf( p_vp->roll, 360.f );
+ // TODO: normalize quaternion
p_vp->fov = VLC_CLIP( p_vp->fov, FIELD_OF_VIEW_DEGREES_MIN,
FIELD_OF_VIEW_DEGREES_MAX );
}
diff --git a/src/misc/viewpoint.c b/src/misc/viewpoint.c
index 72f68bb40f..ea03c6ced1 100644
--- a/src/misc/viewpoint.c
+++ b/src/misc/viewpoint.c
@@ -25,63 +25,211 @@
#endif
#include <vlc_viewpoint.h>
+#include <assert.h>
-void vlc_viewpoint_to_4x4( const vlc_viewpoint_t *vp, float *m )
+/* Quaternion to/from Euler conversion. */
+
+static void QuaternionToEuler(float *yaw, float *pitch, float *roll, const float *q)
{
- float yaw = vp->yaw * (float)M_PI / 180.f;
- float pitch = vp->pitch * (float)M_PI / 180.f;
- float roll = vp->roll * (float)M_PI / 180.f;
-
- float s, c;
-
- s = sinf(pitch);
- c = cosf(pitch);
- const float x_rot[4][4] = {
- { 1.f, 0.f, 0.f, 0.f },
- { 0.f, c, -s, 0.f },
- { 0.f, s, c, 0.f },
- { 0.f, 0.f, 0.f, 1.f } };
-
- s = sinf(yaw);
- c = cosf(yaw);
- const float y_rot[4][4] = {
- { c, 0.f, s, 0.f },
- { 0.f, 1.f, 0.f, 0.f },
- { -s, 0.f, c, 0.f },
- { 0.f, 0.f, 0.f, 1.f } };
-
- s = sinf(roll);
- c = cosf(roll);
- const float z_rot[4][4] = {
- { c, s, 0.f, 0.f },
- { -s, c, 0.f, 0.f },
- { 0.f, 0.f, 1.f, 0.f },
- { 0.f, 0.f, 0.f, 1.f } };
-
- /**
- * Column-major matrix multiplication mathematically equal to
- * z_rot * x_rot * y_rot
+ /* The matrix built from the angles is made from the multiplication of the
+ * following matrices:
+ * ⎡ cos(yaw) 0 -sin(yaw) ⎤
+ * m_yaw (y_rot) = ⎢ 0 1 0 ⎥
+ * ⎣ sin(yaw) 0 cos(yaw) ⎦
+ *
+ * ⎡ 1 0 0 ⎤
+ * m_pitch (x_rot) = ⎢ 0 cos(pitch) sin(pitch) ⎥
+ * ⎣ 0 -sin(pitch) cos(pitch) ⎦
+ *
+ * ⎡ cos(roll) sin(roll) 0 ⎤
+ * m_roll (z_rot) = ⎢ -sin(roll) cos(roll) 0 ⎥
+ * ⎣ 0 0 1 ⎦
+ *
+ * Which, multiplied in the correct order will bring, with the symbols
+ * rewritten: sin = s , cos = c, yaw = y, pitch = p, roll = r
+ *
+ * ⎡s(p)⋅s(r)⋅s(y) + c(r)⋅c(y) s(r)⋅c(p) s(p)⋅s(r)⋅c(y) - s(y)⋅c(r)⎤
+ * V = ⎢s(p)⋅s(y)⋅c(r) - s(r)⋅c(y) c(p)⋅c(r) s(p)⋅c(r)⋅c(y) + s(r)⋅s(y)⎥
+ * ⎣ s(y)⋅c(p) -s(p) c(p)⋅c(y) ⎦
+ *
+ * We can first extract pitch = atan2( -V_32, sqrt(V_31^2 + V_33^2) )
+ *
+ * By taking the case |pitch| = 90 degree, it simplify c(y) and s(y) and:
+ * roll = atan2( V_11, -V_21 )
+ * yaw = atan2( V_11, -V_13 )
+ *
+ * Otherwise, |pitch| != 90 degree and we can get:
+ * roll = atan2( V_12, V_22 )
+ * yaw = atan2( V_31, V_33 )
+ *
+ * The equivalent matrix obtained by converting the equivalent quaternion
+ * Q = ((x, y, z), w) into a tranform matrix (\ref vlc_viewpoint_to_4x4)
+ * is equal to:
+ *
+ * ⎡ xx + ww -yy - zz 2*(xy - zw) 2*(xz + yw) ⎤
+ * U = ⎢ 2*(xy + zw) 1 - 2*(xx + zz) 2*(yz - xw) ⎥
+ * ⎣ 2*(xz - yw) 2*(yz + xw) 1 - 2*(xx + yy) ⎦
+ *
+ * By identifying the coefficient in the matrix V computed above and the
+ * matrix U obtained from converting the quaternion to 3x3 matrix, we get
+ * the following results.
*/
- memset(m, 0, 16 * sizeof(float));
- for (int i=0; i<4; ++i)
- for (int j=0; j<4; ++j)
- for (int k=0; k<4; ++k)
- for (int l=0; l<4; ++l)
- m[4*i+l] += y_rot[i][j] * x_rot[j][k] * z_rot[k][l];
+
+ /* Rename variables and precompute square values to improve readability, as
+ * they are used multiple times. */
+ float qw = q[3];
+ float qx = q[0];
+ float qy = q[1];
+ float qz = q[2];
+
+ float sqx = qx * qx;
+ float sqy = qy * qy;
+ float sqz = qz * qz;
+ float sqw = qw * qw;
+
+ /* The test value is extracted from V_23 = -sin(pitch), which is also
+ * computed as V_23 = 2 * (qw*qx + qy*qz). When abs(V_23 / 2) > = 0.4999,
+ * the cos(pitch) will be 0 and we need a fallback method to get the other
+ * angles in the singularity. If the quaternion is scaled, the scale is
+ * squared because we multiply components by pair, so there's no need for
+ * square root here when computing unit. */
+ float test = - (qw*qx + qy*qz);
+ float unit = sqx + sqy + sqz + sqw;
+
+ /* Diagonal values. */
+ float V_11 = 1 - 2 * (sqy + sqz);
+ float V_22 = 1 - 2 * (sqx + sqz);
+ float V_33 = 1 - 2 * (sqx + sqy);
+
+ /* Values with a minus sign. */
+ float V_13 = 2 * (qx * qz - qw * qy);
+ float V_21 = 2 * (qx * qy - qw * qz);
+
+ /* Values with only plus sign. */
+ float V_23 = 2 * (qw * qx + qy * qz);
+
+ /**
+ * Original code for this part was from:
+ * http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
+ **/
+ if (test > 0.499f * unit)
+ {
+ /* singularity at north pole */
+ *yaw = -asin(V_11);
+ *pitch = - (float)M_PI_2;
+ *roll = 0;
+ }
+ else if (test < -0.499f * unit)
+ {
+ /* singularity at south pole */
+ *yaw = asin(V_11);
+ *pitch = (float)M_PI_2;
+ *roll = 0;
+ }
+ else
+ {
+ *yaw = atan2( V_13, V_33 );
+ *pitch = atan2( -V_23, sqrt( V_21*V_21 + V_22*V_22 ) );
+ *roll = -atan2( V_21, V_22 );
+ }
+}
+
+static void EulerToQuaternion(float *q, float yaw, float pitch, float roll)
+{
+ const float c_yaw = cos(yaw / 2.f);
+ const float s_yaw = sin(yaw / 2.f);
+ const float c_pitch = cos(pitch / 2.f);
+ const float s_pitch = sin(pitch / 2.f);
+ const float c_roll = cos(roll / 2.f);
+ const float s_roll = sin(roll / 2.f);
+
+ /* The values below are the result of converting each Euler rotation
+ * into it's equivalent quaternion, and then multiplying the quaternions
+ * together. It also means that any additional Euler rotation order
+ * conventions can easly be added with the same method. */
+
+ q[3] = c_yaw * c_pitch * c_roll - s_yaw * s_pitch * s_roll;
+ q[0] = s_yaw * c_pitch * s_roll - c_yaw * s_pitch * c_roll;
+ q[1] = -s_yaw * c_pitch * c_roll - c_yaw * s_pitch * s_roll;
+ q[2] = c_yaw * c_pitch * s_roll + s_yaw * s_pitch * c_roll;
+}
+
+void vlc_viewpoint_to_4x4( const vlc_viewpoint_t *vp, float *m )
+{
+ const float *q = vp->quat;
+
+ /* Scaling factor is squared when multiplying the components
+ * pair-wise, so we don't need to use square roots here. */
+ const float square_norm = q[0]*q[0] + q[1]*q[1]
+ + q[2]*q[2] + q[3]*q[3];
+
+ const float xx = q[0] * q[0] / square_norm;
+ const float yy = q[1] * q[1] / square_norm;
+ const float zz = q[2] * q[2] / square_norm;
+ const float ww = q[3] * q[3] / square_norm;
+
+ const float xy = q[0] * q[1] / square_norm;
+ const float zw = q[2] * q[3] / square_norm;
+ const float yz = q[1] * q[2] / square_norm;
+ const float xw = q[0] * q[3] / square_norm;
+
+ const float xz = q[0] * q[2] / square_norm;
+ const float yw = q[1] * q[3] / square_norm;
+
+ /* The quaternion is the opposite rotation of the view.
+ * We need to inverse the matrix at the same time. */
+
+ /* ⎡ xx + ww -yy - zz 2*(xy - zw) 2*(xz + yw) ⎤
+ * U = ⎢ 2*(xy + zw) 1 - 2*(xx + zz) 2*(yz - xw) ⎥
+ * ⎣ 2*(xz - yw) 2*(yz + xw) 1 - 2*(xx + yy) ⎦
+ *
+ * Written column by column, as a usual transformation matrix in the
+ * projective space,
+ *
+ * M = [ R ][ T ]
+ * [ 0 ][ 1 ]
+ *
+ * with R being the rotation matrix from the quaternion and T being a
+ * translation vector, { 0 } here.
+ **/
+
+ m[0] = xx + ww - yy - zz;
+ m[1] = 2 * (xy + zw);
+ m[2] = 2 * (xz - yw);
+ m[3] = 0;
+
+ m[4] = 2 * (xy - zw);
+ m[5] = 1 - 2 * (xx + zz);
+ m[6] = 2 * (yz + xw);
+ m[7] = 0;
+
+ m[8] = 2 * (xz + yw);
+ m[9] = 2 * (yz - xw);
+ m[10] = 1 - 2 * (xx + yy);
+ m[11] = 0;
+
+ m[12] = m[13] = m[14] = 0;
+ m[15] = 1;
}
void vlc_viewpoint_from_euler(vlc_viewpoint_t *vp,
float yaw, float pitch, float roll)
{
- vp->yaw = -yaw;
- vp->pitch = -pitch;
- vp->roll = -roll;
+ /* convert angles from degrees into radians */
+ yaw = -yaw * (float)M_PI / 180.f;
+ pitch = -pitch * (float)M_PI / 180.f;
+ roll = -roll * (float)M_PI / 180.f;
+
+ EulerToQuaternion(vp->quat, yaw, pitch, roll);
}
void vlc_viewpoint_to_euler(const vlc_viewpoint_t *vp,
float *yaw, float *pitch, float *roll)
{
- *yaw = -vp->yaw;
- *pitch = -vp->pitch;
- *roll = -vp->roll;
+ QuaternionToEuler(yaw, pitch, roll, vp->quat);
+
+ /* convert angles from radian into degrees */
+ *yaw = -180.f / (float)M_PI * (*yaw);
+ *pitch = -180.f / (float)M_PI * (*pitch);
+ *roll = -180.f / (float)M_PI * (*roll);
}
--
2.31.0
More information about the vlc-devel
mailing list