In order to rotate body $B_2$ properly, I need to determine the local coordinate system (vectors of $x$, $y$ and $z$-axis) based on body $\frac{B_1}{B_2}$ and align the $z$-axis of this coordinate system with $B_2$'s orientation ($z$-axis arrow in blue). The $x$-axis should be orthogonal to the $z$-axis of $\frac{B1}{B2}$.
The initial global quaternions (or the rotation matrices) for $\frac{B1}{B2}$ are known (global coordinate system). I also have all start and end points, and thus the initial direction vector is known ($z$-axis).
Calculation of $X$ and $Y$ via the cross product of the $z$-axis of $B_1$ and $B_2$ is not reliable, especially not if the $z$-axis vectors are initially aligned. So I wonder how to calculate $X$ and $Y$ from $Z$ and the initial quaternion (or the rotation matrix).
Best Answer
I'm assuming that you want to rotate $B_2$ so that its $x$-axis is normal to the $z$-axis of $B_1$. One way to do this is to work in the coordinate frame of $B_2$ by rotating both bodies by $B_2^T$ $$ A_1=B_2^TB_1\\ A_2=B_2^TB_2 $$ so that $A_2$ is the identity matrix $I$. Now rotate $A_2$ about its $z$-axis so that its $y$-axis aligns with the $x$ and $y$ components of the $z$-axis of $B_1$ $$ A_2'=\begin{bmatrix} c & s & 0 \\ -s & c & 0 \\ 0 & 0 & 1 \\ \end{bmatrix}A_2 $$ where $c={v(2)\over|v|}$, $s={v(1) \over|v|}$ and $v=[A_1(1,3)\,\,\, A_1(2,3)]$. Now the $x$-axis of $B'_2=B_2A'_2$ is normal to the $z$ axis of $B_1$.
If the $z$-axes of $B_1$ and $B_2$ are almost parallel, $|v|$ will be small and you may not want to update $B'_2$ if $|v|$ is less than some multiple of your machine precision.