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.
$(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:
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.
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.