CODEWITHSUNDEEP
openglvectorgraphics3drobotics
anna.sarp
anna.sarp

Reputation: 307

Compute target roll, pitch and yaw angles given initial and target positions and intial roll, pitch and yaw angles

I have a model that moves in simulation world. The frame of operation is right handed with X - forward, Y - left and Z - Up.

I know the current position, P1 (x1, y1, z1) and current RPY angles, (R1, P1, Y1). I have a goal position, P2 (x2, y2, z2). I would like to move my model from P1 to P2. I am stuck at computing the goal RPY angles. I would like my model to turn around first in the direction from P1 to P2 and move in that direction to reach goal.

How do I calculate the target RPY angles, (R2, P2, Y2), given the above information ?

Upvotes: 0

Views: 1513

Answers (1)

meowgoesthedog
meowgoesthedog

Reputation: 15035

  • Calculate the direction vector D = P2 - P1 = (x2 - x1, y2 - y1, z2 - z1)

  • Pitch P2 = asin(Dz / |D|)

  • Yaw Y2 = atan2(Dy, Dx)

(NB can use P2 = atan2(Dz, sqrt(Dx^2 + Dy^2)) instead for more robustness near the spherical poles)

Upvotes: 2

Related Questions