I need some help with programming the calculation of displacement. Given the distance the object has moved in the XY plane, and the yaw (heading) of the object, I'd like to calculate the displacement of the object. For example, the object moved 5m North and 2m East.
The data I have is the distance it travels in the XY plane (X distance and Y distance) and the heading of the device which is determined by the direction of X. I know how to calculate it on paper but I am having trouble to program it.
I've attached two examples that shows how I obtained the calculations. The black dot is the displacement of the object. In the 1st example, the X-distance travelled is 3 meters while the Y-distance is 4 meters and the heading of the X-axis is 70°. After calculating, I managed to get that it has travelled 2.733m South and 4.187m East. In example 2, it travel, the X-distance is -5m and the Y-distance is -12m, the heading is 160°. I calculated that the object travels 8.80m North and 9.566m East.
Assume you have your XY pane rotated by some angle Zp and have a 2D movement vector within that pane, let's say its direction rotated by Zv – what is total rotation of the vector, relative to the global orientation? Zp + Zv...
So all you need to do is applying the rotation Zp to the movement vector within XY-pane, i. e. apply a rotation matrix to the vector. This matrix for a rotation by z looks like:
cos(z) -sin(z)
sin(z) cos(z)
With our data above:
north = cos(Zp) * x - sin(Zp) * y
east = sin(Zp) * x + cos(Zp) * y
which gives, with your first sample data applied:
north = cos(70°) * 3 - sin(70°) * 4 ~ -2.7327
east = sin(70°) * 3 + cos(70°) * 4 ~ 4.1872
north = cos(160°) * -5 - sin(160°) * -12 ~ 8.8027
east = sin(160°) * -5 + cos(160°) * -12 ~ 9.5662
Corresponding pretty much to the values you calculated (note: negative movement to north is positive movement towards south...).