Rotations and translations in real time

Hi guys, I know there are many topics about this theme, but I’m getting mad with this :woozy_face:

I have the UTM coordinates and orientation of my state, at the same time, I have coordinates of the objects that surround me, referred to my reference system and it arrives in a message of other coordinates referred to the same reference system (mine).

I want to find which of the objects that surround me are closer to those coordinates that reach me.

For this, what I have been trying to do is put everything in the reference system of the UTM coordinates and then calculate the distance between each object and the message coordinates, for this, I have applied these formulas:

  posXtransformed = xOrigin*cos(angle) - yOrigin*sin(angle)) + xUTM;
  posYtransformed = xOrigin*sin(angle) + yOrigin*cos(angle)) + yUTM;

Once calculated, I calculate the distances with dist (), I run it at a low frequency and it works so or so well, but I need it in real time. Here the problems come, I do not know very well why it is due, but doing tests such as changing the sign of the angle of rotation, it works the same (this I think makes no sense, and that is why I wanted to ask to see if the part of the rotation I am doing right or wrong); and during execution (in real time) sometimes it finds the position well, but then when I rotate my reference system, the object it detects also changes …

Any kind of help is welcome, and sorry for the English, I was having a hard time thinking about how to write the problem and I used the google translator.

1 Like

Did you resolve this issue?

Your phrase “those coordinates that reach me” was unclear.

If you are at position X surrounded by objects A B C and D, are you trying to find the closest object to X, A? Or are you trying to search on A for the closest object to it, B?

A small optimization: In general, if you are trying to optimize an ordered lists of close objects, and not actual distances, do not use dist() – as that is cartesian distance and involves a square root.


Instead, just use sq(x1-x2) + sq(y1-y2). This gives the squared distance, which is still fine for sorting and identifying the closest object. (I’m assuming that you have a lot of objects). Depending on your precision you could also do this as integers. If you have a proximity distance threshhold, you can square the proximity and compare it to your squared distance list as a cutoff.

1 Like

Mmm maybe :thinking:, not sure yet, I found that I was dragging some rotations and the visualization wasn’t correct, but the calculation seems yes.

I get a point P and want to find the closest object to it ( P ) (dist P to A, P to B…), P is referring to my X position, but just have the update coordinates of P when the message reach to my system (and want to conserve that coordinates even when X moves)

:blush: Thanks, will try it, my amount of objects is variable, sometimes I have less and sometimes more.

By the way I have to correct some issues before I can try it :sweat_smile:.