I'm trying to program an autopilot system for a spaceship in a videogame. I am currently stuck with trying to figure out how to convert the target 'destination' coords to yaw, pitch, and roll angles for the ship to move in so it can line up to face the target directly.
I already have:
- forward and up vectors (normalized) of the ship
- ship coords
- target coords
My idea to solve this is to first convert the target coord from the "global" coord system to a "ship" coord system by subtracting the target vector by the ships coord and then use rotational matrices to get it in line with the ship - the angles of the matrices would somehow be gotten from the forward/up vectors of the ship and comparing them to the axis' of the global coord system. Then I would use spherical coord conversion to find the pitch/yaw and then I wouldn't need a roll (which would be nice).
Would this concept work? If so, how would I go about finding angles from the 2 ship vectors? Is there a more efficient method?
The language in-game is very simple (no trig) so Taylor series and such are needed and I have a strict character/line limit, so the most compact solution (least trig/least steps) would be the best. thanks!
Let $S$ be the vector representing the ship coordinates, and $T$ be the vector representing the target coordinates. Further, let $F$ be the unit vector in the forward direction of the ship and $U$ be the unit vector in the "up" direction of the ship. First thing, as you mentioned is to compute the unit vector of the position of the target with respect to the ship, this is given by
$F' = \dfrac{ T - S }{\| T - S \|} $
This vector will be the "new" forward direction of the ship after rotation. Next we want to select the "new" UP direction of the ship, and this can be any vector perpendicular to $F'$. Suppose we want to select this vector as close as possible to the original UP direction of the ship. This can found by the orthogonal projection of $U$ onto the plane whose normal vector is $F'$. Hence,
$U' = \dfrac{ (I_3 - F' F'^T ) U } { \| (I_3 - F' F'^T) U \| } $
Now we have $F, U$ of the original orientation of the ship, and $F', U'$ of the final orientation (all unit vectors). They are related by
$ F' = R F $ and $ U' = R U $
To compute the rotation matrix $R$, compute the cross product of $F$ and $U$ and of $F'$ and $U'$, namely, let
$V = F \times U $ and $V' = F' \times U' $
Then, we now have a complete set of three mutually orthogonal vectors, related as follows:
$[F', U', V'] = R [ F, U , V] $
Hence, we can compute the rotation matrix $R$ as follows:
$R = [F', U', V'] [F, U, V]^{-1} = [F', U', V'] [F, U, V]^T $
The last equality results from the fact that the matrix $[F, U, V]$ is orthogonal, that is, its inverse is its transpose.
The rotation matrix $R$ given above is relative to the "global" coordinate system.
If we want to compute the above rotation relative to the ship coordinate system, then define
$R_1 = [F , U, V] $
as the coordinate system attached to the ship, and using this change of coordinates, it follows that the rotation matrix $R'$ relative to this basis is
$R' = R_1^T R R_1 = [F, U, V]^T [F', U', V'] $
Now $R'$ can be decomposed into Roll (about $F$), Pitch (about $V$), Yaw (about $U$).
Then $R' = R_V(\theta_V) R_U(\theta_U) R_F (\theta_F) $
Define
$c_1 = \cos \theta_F , s_1 = \sin \theta_F $
$c_2 = \cos \theta_U , s_2 = \sin \theta_U $
$c_3 = \cos \theta_V, s_3 = \sin \theta_V $
Then
$R_F(\theta_F) = \begin{bmatrix} 1 && 0 && 0 \\ 0 && c_1 && -s_1 \\ 0 && s_1 && c_1 \end{bmatrix}$
$R_U(\theta_U) = \begin{bmatrix} c_2 && 0 && s_2 \\ 0 && 1 && 0 \\ - s_2 && 0 && c_2 \end{bmatrix} $
$R_V(\theta_V) = \begin{bmatrix} c_3 && - s_3 && 0 \\ s_3 && c_3 && 0 \\ 0 && 0 && 1 \end{bmatrix} $
Hence,
$R' = R_V R_U R_F =\begin{bmatrix} c_2 c_3 && c_3 s_1 s_2 - s_3 c_1 && c_3 c_1 s_2 + s_1 s_3 \\ c_2 s_3 && s_1 s_2 s_3 + c_1 c_3 && c_1 s_2 s_3 - s_1 c_3 \\ -s_2 && s_1 c_2 && c_1 c_2 \end{bmatrix}$
Looking at the first column and the last row of the above matrix, we deduce that
$ \theta_V = \text{ATAN2}(R'_{11}, R'_{21} ) $
$ \theta_F = \text{ATAN2}(R'_{33}, R'_{32} ) $
$ \theta_U = \sin^{-1} (-R'_{31} ) $