# Reaching a point B in Cartesian coordinate via Euler angles knows its initial point A Euler angles and Cartesian coordinates

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

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