# Translate a 3D point along a heading

I need to translate a point (P1) in 3D a certain amount, call it stepSize, along a vector described by a heading composed of three angles (basically pitch, roll and yaw). I know that in homogeneous coordinates the following translation matrix would be used

and I can multiply my existing point by Tv to get the new point. However, I need something slightly different. I have P1 and want the new point, P2 to be separated by stepSize so I need to recover Vx, Vy and Vz as given in the matrix above (from Wikipedia). Any general suggestions about how to go about that? I will do this in R once I know what to do.

EDIT: Here is some additional information. I am trying to do 3D Turtle Graphics which are very much analogous to aircraft position and headings. Specifically, I am writing code to implement the 3D L-systems described here. This figure from the book gives the coordinate system used in that field (rather non-standard if you ask me).

So I have my 'turtle' at the origin of this system, I have a given standard stepSize, and I have saved the most recent headings, which are the angles around each of the axes H, L and U. I want to move the turtle forward by stepSize. In the graphics system I am using, I have the current Cartesian coordinates of the turtle, and I need to compute the offset in the x, y and z directions to be added to the current position to get the new position (then repeat ...). A small (?) complication is that the graphics drawing system I am using has the (+)ive x/L axis going to the right, not left as in the book figure. H in the figure corresponds to the (+)-ive y-axis, and U to the (+)-ive z axis. Ugh.

• What is the context of the problem? Computer Graphics? Robotics? Something else? – Paul Aug 27 '14 at 15:28
• Computer graphics, specifically I'm building an L-system to plot Turtle Graphics in 3D. So I have a starting point, a distance to move and a heading/direction to move. – Bryan Hanson Aug 27 '14 at 15:29
• Maybe I'm missing something here, but it seems like you already have your heading vector $\vec{v}$ (you'll have to translate your angles to an actual vector) so $\vec{P}_2 = \vec{P}_1+(\vec{v}/||\vec{v}|| )s$ where $s=$stepSize. – Doug Lipinski Aug 27 '14 at 16:27
• @DougLipinski Well, I have my heading as three angles. So maybe what I am missing is how to turn those 3 angles into a vector so I can do the math you describe, which makes perfect sense to me. So for instance if I had pitch, yaw and roll of of 10, 15 and 5 degrees, how does one make that into the vector I need? Sorry, this part of my learning is a but fuzzy... – Bryan Hanson Aug 27 '14 at 17:19
• You'll need to define your angles more strictly with respect to the coordinate system on which P1 and P2 are defined. Pitch, yaw, and roll are body centered coordinates that are relative to a known position and direction. What is that direction? Are your angles actually pitch/yaw/roll, or something else? Maybe they're Tait-Bryan angles en.wikipedia.org/wiki/… ? It's important to be precise. If you add this information to your question (better there than comments) we can probably help. – Doug Lipinski Aug 27 '14 at 17:42

Disclaimer, I only know the small amount I've just read about turtle graphics.

It seems that the "turtle" in a turtle graphics system is described by a state, $(\vec{P},\vec{H},\vec{L},\vec{U})$, consisting of a point in space, $\vec{P}$, and a set of three unit vectors that denote the orientation in space where $\vec{H}$ is the heading while $\vec{L}$ and $\vec{U}$ specify directions normal to the heading as in your image. You can think of $\vec{L}$ and $\vec{U}$ as standing for left and up for an actual turtle (the animal) located at point $\vec{P}$ with its head pointed in the direction of $\vec{H}$. Motions of the turtle are given by either changing the orientation by specified rotations or by moving in the direction of $\vec{H}$. Moving in a direction other than $\vec{H}$ requires first turning so that $\vec{H}$ points in the desired direction. In terms of a global cartesian coordinate system, this amounts to multiplying the orientation vectors by a rotation matrix for rotations or adding $d\vec{H}$ to the current position $\vec{P}$ to move a distance $d$.

In mathematical terms, a rotation operation corresponds to $$\mathbf{O}_\text{new}=\mathbf{O}_\text{old}\mathbf{R}$$ where $\mathbf{O}$ is the orientation matrix with $\vec{H}$, $\vec{L}$, $\vec{U}$ as its columns $$\mathbf{O} = \left[ \vec{H}~\vec{L}~\vec{U} \right]$$ and $\mathbf{R}$ is the rotation matrix. Rotations are given in terms of angular rotations about $\vec{H}$, $\vec{L}$, or $\vec{U}$ which correspond to roll, pitch, and yaw motions. The rotation matrix can be found using the formula for a general rotation about a given axis.

I the translations correspond only to motions in the direction of $\vec{H}$. Thus a move of a distance $d$ corresponds to the operation $$\vec{P}_\text{new} = \vec{P}_\text{old} + d\vec{H}$$

Note that these operations rely on $\vec{H}$, $\vec{L}$, and $\vec{U}$ being unit vectors so you may have to occasionally re-normalize them due to numerical errors from rotation operations.

• Doug, can I ask where did you read that movements can only be in the direction of $\vec{H}$? In what I have read, the movements are described as 'Forward' which I take to mean forward along in the direction of the current heading. But I am not sure if I have interpreted what I have read correctly. If movement is only allowed along $\vec{H}$, I have been way overthinking this, which wouldn't be the first time! – Bryan Hanson Aug 27 '14 at 20:51
• I believe $\vec{H}$ is defined as the heading (hence the use of the letter H), so "forward" means along $\vec{H}$. Perhaps it's an issue of terminology since you said you have "the most recent headings", but I think you mean the most recent orientation vectors. There is only one heading. – Doug Lipinski Aug 27 '14 at 21:02
• OK, I'll have to re-read a few things. I've been thinking about $\vec{H}$, $\vec{L}$, $\vec{U}$ as an axis system which may have tripped me up. Thanks again for the time you've invested in helping me. – Bryan Hanson Aug 27 '14 at 21:04
• No problem, cf. cg.tuwien.ac.at/courses/Fraktale/PDF/fractals8.pdf which lists movements only along the H axis and rotations about any of the axes. As an aside, (H,L,U) is an axis system, it's the axis system that corresponds to the heading, the left direction, and the up direction of a "turtle" moving in 3-space. Some references also use (H,R,U), swapping right for left as one of the axes. – Doug Lipinski Aug 27 '14 at 21:09