0

This is to solve the following problem : Let $v$, $v'$, $u$ and $u'$ be unit vectors different from each other and built so that there exist a single quaternion that rotates $v$ towards $v'$ that also rotates $u$ towards $u'$. I need to find that quaternion.

To solve this I get I have to solve an equation similar to $q = q'$, $q$ being a quaternion that rotates $v$ towards $v'$ and $q'$ being a quaternion that rotates $u$ towards $u'$. However I know there are infinite solutions for a quaternion to rotate one vector towards another and the only formulas I know give direct solutions that won't work for this problem (such as the half-way quaternion solution).

I think I need the necessary and sufficient conditions for a quaternion to rotate one vector towards another in order to solve this but couldn't find other solutions that direct solutions.

1 Answers1

0

An unit quaternion $\mathbf{q}$ that rotates $v$ to $v^\prime$ (given $\lVert v \rVert = \lVert v^\prime \rVert$) can be defined as $$\mathbf{q} = \cos\frac{\theta}{2} + a \sin\frac{\theta}{2}$$ where $$\begin{aligned} a &= \frac{v \times v^\prime}{\left\lVert v \times v^\prime \right\rVert} \\ \cos \theta &= \frac{v \cdot v^\prime}{\left\lVert v \right\rVert \left\lVert v^\prime \right\rVert} \\ \sin \theta &= \frac{\left\lVert v \times v^\prime \right\rVert}{\left\lVert v \right\rVert \left\lVert v^\prime \right\rVert} \\ \end{aligned}$$ To additionally rotate $u$ to $u^\prime$, you need a rotation $\mathbf{p}$ around axis $v^\prime$ by suitable angle $\varphi$.

Let $u_q = \mathbf{q} u \mathbf{q}^{-1}$ (where $\mathbf{q}^{-1} = \sin\frac{\theta}{2} - a \cos\frac{\theta}{2}$), i.e. $u$ rotated by $\mathbf{q}$. Then, $$\begin{aligned} b &= \frac{v^\prime}{\left\lVert v^\prime \right\rVert} \\ \cos\varphi &= \frac{u_q \cdot u^\prime}{\left\lVert u_q \right\rVert \left\lVert u^\prime \right\rVert} \\ \mathbf{p} &= \cos\left(\frac{\varphi}{2}\right) + b \sin\left(\frac{\varphi}{2}\right) \\ \end{aligned}$$

and the combined rotation needed is $\mathbf{p}\mathbf{q}$.


Another, more general option is to construct the two orientations' basis vectors (rotation matrices), combine them to get the rotation matrix needed, then recover the rotation quaternion from the rotation matrix.

If you have two linearly independent vectors $\vec{a}$ and $\vec{b}$ (meaning there is no $\lambda \in \mathbb{R}$ so that $\vec{a} = \lambda \vec{b}$), you can construct the basis vectors $\hat{e}_1$, $\hat{e}_2$, and $\hat{e}_3$ very easily. The first basis vector is just one of the vectors scaled to unit length: $$\hat{e}_1 = \frac{\vec{a}}{\left\lVert\vec{a}\right\rVert}$$ The second basis vector is the perpendicular part of the second vector with respect to the first basis vector. We can obtain this by one step of the Gram–Schmidt process, and normalizing the result to unit length: $$\hat{e}_2 = \frac{ \vec{b} - \vec{e}_1 \left( \vec{e}_1 \cdot \vec{b} \right) }{ \left\lVert \vec{b} - \vec{e}_1 \left( \vec{e}_1 \cdot \vec{b} \right) \right\rVert }$$ The third basis vector is the cross product of the two: $$\hat{e}_3 = \hat{e}_1 \times \hat{e}_2$$ and the rotation matrix describing that is $$\mathbf{R} = \left[ \begin{matrix} \hat{e}_1 & \hat{e}_2 & \hat{e}_3 \end{matrix} \right] = \left[ \begin{matrix} x_1 & x_2 & x_3 \\ y_1 & y_2 & y_3 \\ z_1 & z_2 & z_3 \\ \end{matrix} \right ]$$ Because this matrix is orthonormal, its inverse is its transpose, $\mathbf{R}^{-1} = \mathbf{R}^T$.

If $\mathbf{R}_1$ describes the current orientation, and $\mathbf{R}_2$ the desired orientation, then $$\mathbf{R}_{1\to 2} = \mathbf{R}_2 \mathbf{R}_1^{-1} = \mathbf{R}_2 \mathbf{R}_1^T$$ is the rotation needed.


To recover quaternion $$\mathbf{q} = w + x\mathbf{i} + y\mathbf{j} + z\mathbf{k}$$ from a pure 3×3 rotation matrix $$\mathbf{R} = \left[ \begin{matrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \\ \end{matrix} \right]$$ in a robust, numerically stable manner, I recommend the following procedure:

  • If $r_{11} + r_{22} + r_{33} \ge 0$: $$\left\lbrace ~ \begin{aligned} w &= \sqrt{1 + r_{11} + r_{22} + r_{33}} / 2\\ x &= \pm \sqrt{1 + r_{11} - r_{22} - r_{33}} / 2, \text{ same sign as } r_{32} - r_{23} \\ y &= \pm \sqrt{1 - r_{11} + r_{22} - r_{33}} / 2, \text{ same sign as } r_{13} - r_{31} \\ z &= \pm \sqrt{1 - r_{11} - r_{22} + r_{33}} / 2, \text{ same sign as } r_{21} - r_{12} \\ \end{aligned} \right.$$

  • Otherwise, if $r_{11} \ge r_{22}$ and $r_{11} \ge r_{33}$: $$\left\lbrace ~ \begin{aligned} s &= 2 \sqrt{ 1 + r_{11} - r_{22} - r_{33}} \\ w &= ( r_{32} - r_{23} ) / s \\ x &= s / 4 \\ y &= ( r_{21} + r_{12} ) / s \\ z &= ( r_{13} + r_{31} ) / s \\ \end{aligned} \right.$$

  • Otherwise, if $r_{22} \ge r_{11}$ and $r_{22} \ge r_{33}$: $$\left\lbrace ~ \begin{aligned} s &= 2 \sqrt{ 1 - r_{11} + r_{22} - r_{33}} \\ w &= ( r_{13} - r_{31} ) / s \\ x &= ( r_{21} + r_{12} ) / s \\ y &= s / 4 \\ z &= ( r_{32} + r_{23} ) / s \\ \end{aligned} \right.$$

  • Otherwise: $$\left\lbrace ~ \begin{aligned} s &= 2 \sqrt{ 1 - r_{11} - r_{22} + r_{33}} \\ w &= ( r_{21} - r_{12} ) / s \\ x &= ( r_{13} + r_{31} ) / s \\ y &= ( r_{32} + r_{23} ) / s \\ z &= s / 4 \\ \end{aligned} \right.$$

This is based on the quaternion-derived rotation matrix $\mathbf{R}$, $$\mathbf{R} = \left[ \begin{matrix} 1 - 2 (y^2 + z^2) & 2 (x y - w z) & 2 (x z + w y) \\ 2 (x y + w z) & 1 - 2 (x^2 + z^2) & 2 (y z - w x) \\ 2 (x z - w y) & 2 (y z + w x) & 1 - 2 (x^2 + y^2) \\ \end{matrix} \right] = \left[ \begin{matrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \\ \end{matrix} \right]$$ having the following properties: $$\begin{aligned} r_{11} &= w^2 + x^2 - y^2 - z^2 \\ r_{22} &= w^2 - x^2 + y^2 - z^2 \\ r_{33} &= w^2 - x^2 - y^2 + z^2 \\ r_{32} - r_{23} &= 4 w x \\ r_{13} - r_{31} &= 4 w y \\ r_{21} - r_{12} &= 4 w z \\ r_{21} + r_{12} &= 4 x y \\ r_{13} + r_{31} &= 4 x z \\ r_{32} + r_{23} &= 4 y z \\ r_{11} + r_{22} + r_{33} &= 3 w^2 - x^2 - y^2 - z^2 \\ 1 + r_{11} + r_{22} + r_{33} &= 4 w^2 ~ \text{ if } ~ w = \sqrt{1 - x^2 - y^2 - z^2} \\ 1 + r_{11} - r_{22} - r_{33} &= 4 x^2 ~ \text{ if } ~ w = \sqrt{1 - x^2 - y^2 - z^2} \\ 1 - r_{11} + r_{22} - r_{33} &= 4 y^2 ~ \text{ if } ~ w = \sqrt{1 - x^2 - y^2 - z^2} \\ 1 - r_{11} - r_{22} + r_{33} &= 4 y^2 ~ \text{ if } ~ w = \sqrt{1 - x^2 - y^2 - z^2} \\ \end{aligned}$$ When $r_{11} + r_{12} + r_{13} \ge 0$, we can recover the quaternion from the diagonal entries relying on $w^2 + x^2 + y^2 + z^2 = 1$. However, when the sum is negative $w$ is very small, and we can get better numerical stability by starting with $x^2$, $y^2$, or $z^2$ (whichever is greatest), and using the non-diagonal elements of $\mathbf{R}$ to obtain the other three components.

Guest
  • 141
  • Thank you for your answer. I tested the first method and it is working, there is just a slight ambiguity with in which direction the quaternions rotate : the cosine function, contrary to the sine function, don't allow to directly know if the rotation angle is + or - since cos(θ) = cos(-θ). It impacts the sine since sin(θ) = -sin(-θ) and the resultant quaternion's vector part coordinates signs can be swapped from what you initially get. I guess this is easily fixable, but I remember rotation matrices don't suffer from such issues so I will probably use your second method. – Elemyrh Jun 08 '20 at 20:26
  • @Elemyrh: My experiments with the direct quaternion approach indicates it doesn't seem very stable numerically – but that could be just my naïve implementation of the above (in Python3). Also, the trigonometric functions are surprisingly "slow" in real life. The direct basis/rotation matrix method ensures orthonormality and is quite robust numerically. As it only uses multiplication, division, addition, subtraction, and square root (no trigonometric functions), it is surprisingly efficient. It may look complicated or slow, but really isn't. – Guest Jun 09 '20 at 09:14