I have a point A:-

Known it's Cartesian coordinates (X,Y,Z) and its Euler angle Aka body rotation (R,P,Y) respectively Roll (rotation around X axis) , Pitch (rotaion around Y axis) and Yaw (rotation around Z axis).

Now I have a point B:-

Knows its Cartesian coordinates only (X',Y',Z') .

What I need to find is : the "Euler angles / Rotation" (R',P',Y') if the body changed its rotation to , it will reach point B .
I also have the ability to calculate quaternions if needed to

enter image description here

  • 1
    $\begingroup$ the point is constrained to a sphere, I guess? $\endgroup$ – marco trevi Sep 29 '14 at 13:55
  • 1
    $\begingroup$ @marcotrevi what do you mean ? $\endgroup$ – RoboMan Sep 29 '14 at 14:06
  • $\begingroup$ that the point can move only on the surface of a sphere...$X^2+Y^2+Z^2$ is constant...? $\endgroup$ – marco trevi Sep 29 '14 at 14:10
  • 1
    $\begingroup$ @marcotrevi yes , actually it's a quad copter $\endgroup$ – RoboMan Sep 29 '14 at 14:22
  • $\begingroup$ Maybe this wikipedia entry could help you - en.wikipedia.org/wiki/Rotation_matrix $\endgroup$ – marco trevi Sep 29 '14 at 14:29

My interpretation of the problem statement is as follows (please correct me if I've misunderstood):

  • Given: an initial point $A = (x, y, z)$, an initial orientation $(R, P, Y)$, and a destination point $B = (x', y', z')$
  • Find: an orientation $(R', P', Y')$ such that an object at point $A$ with that orientation would be facing directly towards $B$

First, note that the vector from $A$ to $B$ can be expressed as $$\vec{AB} = B-A = (x'-x, \ y'-y, \ z'-z)$$

There are actually infinitely many different orientations that "point along" this vector $\vec{AB}$. To see why, imagine an aircraft doing an aileron roll while flying from point $A$ to point $B$. The aircraft's direction of motion is always pointed towards the destination point $B$, but its orientation is constantly changing throughout the maneuver.

So, we only need to find one possible orientation meeting the above criteria.

We can think of an orientation as a 3D rotation applied to the default coordinate axes. There are many distinct ways to represent rotations, as outlined in http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions. (Conveniently, this article also defines conversions to and from the various formalisms.)

Let's work with the rotation matrix representation. First, let's say we want the unit vector $\hat{x} = (1,0,0)$ to map onto the unit vector $$\hat{u} = \frac{\vec{AB}}{|\vec{AB}|},$$

which means that $\hat{u}$ gives us the first column of the rotation matrix.

Since the transformed axes ($\hat{u}$, $\hat{v}$, and $\hat{w}$) must be mutually orthogonal, we know that the second column $\hat{v}$ of the rotation matrix must satisfy $\hat{u}\cdot\hat{v}=0.$ That is, $$v_1 u_1 + v_2 u_2 + v_3 u_3 = 0.$$ And, since $\hat{v}$ must be a unit vector, $$|\hat{v}|^2 = v_1^2+v_2^2+v_3^2 = 1.$$

With two equations governing three unknowns, we have one degree of freedom remaining. (As with the aileron-roll example, we've fixed the direction in which one transformed axis will point, but the coordinate system is still free to rotate around the transformed axis.)

Let's arbitrarily pick a value of $\hat{v}$ with zero $z$ component and a positive $y$ component; that is, let's set $v_3=0$, solve the (quadratic) system of equations, and pick the solution that yields $v_2 > 0$. (In rare cases where $\hat{u}$ lies exactly along the $y$ axis, forcing $v_2 = 0$, we can choose a slightly different, but still essentially arbitrary, constraint.)

This gives us two columns of the rotation matrix. We can solve for the final column, $\hat{w}$, by again using the unit-vector and orthogonality properties. The quadratic equations should have two solutions, but we'll pick the one unique solution that satisfies the same "right-hand rule" that our initial coordinate system satisfied (typically $\hat{u}\times\hat{v}=\hat{w}$).

Now, with a full rotation matrix, we can translate this into a roll, pitch, and yaw using the conversions listed in the "Rotation formalisms" article cited above.

Note: in order to get a unique answer, we had to introduce an arbitrary constraint along the way, when solving for $\hat{v}$. Other constraints might be equally valid, depending on the use case. For instance, one approach would be to specify that the roll $R$ should remain constant ($R = R'$).


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.