[Math] Bearing angle and distance between heading and target point

trigonometry

I'd like to calculate the bearing angle and the distance between heading point and target point with three coordinates for an arduino navigation.

Coordinate system looks something like this.

enter image description here

$(x_1, y_1)$ = heading point, $(x_3, y_3)$ = target point

Angle range should be $-180^\circ$ to $+180^\circ$ so it is possible to continuously adjust bearing angle to zero to drive towards way-point.

Thanks in advance

Best Answer

The three points form a triangle, as in the figure below:

enter image description here

You know the coordinates of the three vertices: $H(x_H,y_H)$, $A(x_A,y_A)$, and $T(x_T,y_T)$. The goal is to compute the angle between $\vec{AT}$ and $\vec{AH}$. I write these as vectors because you want an oriented angle, starting from the segment $\vec{AT}$ and increasing counterclockwise towards $\vec{AH}$.

The derivation below will result in an angle in the range $0 \le \theta < 360^\circ$. You can adjust the range to $-180^\circ \le \theta < 180^\circ$ by simply subtracting $180^\circ$ from the value we'll compute below.

We'll obtain the angle by making use of the dot and cross products of the vectors $\vec{AT}$ and $\vec{AH}$ to find the values of $\cos\theta$ and $\sin\theta$, respectively. The dot product between two vectors in $\mathbb{R}^2$ is a number which can be computed in two ways: $$ \vec{a}\cdot\vec{b} \equiv a_x\,b_x + a_y\,b_y = |\,\vec{a}\,|\,|\,\vec{b}\,|\cos\theta $$

Similarly, the cross product between two vectors in $\mathbb{R}^2$ is also a number which can be computed in two ways: $$ \vec{a}\times\vec{b} \equiv a_x\,b_y - a_y\,b_x = |\,\vec{a}\,|\,|\,\vec{b}\,|\sin\theta $$

Dividing the result from the cross product by the result for the dot product, we can compute $\tan\theta$: $$ \tan\theta \equiv \frac{\sin\theta}{\cos\theta} = \frac{a_x\,b_y - a_y\,b_x}{a_x\,b_x + a_y\,b_y} $$ Note that we can't perform that division when $\cos\theta$ (the denominator) is zero, that is, when $\theta = 90^\circ$ or when $\theta = 270^\circ$. Now for the actual computation.

Let $$ (\, a_x \,,\, a_y \,) = \left(\,x_T-x_A \,,\, y_T-y_A \,\right) $$ $$ (\, b_x \,,\, b_y \,) = \left(\,x_H-x_A \,,\, y_H-y_A \,\right) $$ $$ s = a_x\,b_y - a_y\,b_x $$ $$ c = a_x\,b_x + a_y\,b_y $$ Then, $$ \theta = 0^\circ \quad\mbox{if}\quad s = 0 \quad\mbox{and}\quad c > 0 $$ $$ \theta = 90^\circ \quad\mbox{if}\quad s > 0 \quad\mbox{and}\quad c = 0 $$ $$ \theta = 180^\circ \quad\mbox{if}\quad s = 0 \quad\mbox{and}\quad c < 0 $$ $$ \theta = 270^\circ \quad\mbox{if}\quad s < 0 \quad\mbox{and}\quad c = 0 $$ Now let $$ \theta_0 = \arctan\left|\frac{s}{c}\right| \quad\mbox{if}\quad c \ne 0 $$ Then, $$ \theta = \theta_0 \quad\mbox{if} \quad s > 0 \quad\mbox{and}\quad c > 0 $$ $$ \theta = 180^\circ - \theta_0 \quad\mbox{if} \quad s > 0 \quad\mbox{and}\quad c < 0 $$ $$ \theta = 180^\circ + \theta_0 \quad\mbox{if} \quad s < 0 \quad\mbox{and}\quad c < 0 $$ $$ \theta = 360^\circ - \theta_0 \quad\mbox{if} \quad s < 0 \quad\mbox{and}\quad c > 0 $$

The case when both $c = 0$ and $s = 0$ cannot happen unless the three points $A$, $T$, and $H$ coincide. As I said at the top, this results in $0 \le \theta < 360^\circ$ but you can adjust the range to $-180^\circ \le \theta < 180^\circ$ by subtracting $180^\circ$ from the angle computed above.

Related Question