How do I calculate the yaw, pitch, and roll of a p

2020-02-16 04:59发布

问题:

Given a point in 3D space, what are the three angles (e.g. Euler angles) needed to transform a line to point to that object?

Imagine I have a line (or a box) in 3D and I want to transform its heading, pitch, and bank to point to the 3D point from the origin, what values would I use for those angles?

I can't figure out the math to calculate the angles to point to a location such as (1,2,3).

回答1:

Note: Instead of "yaw, pitch, roll", I'm going to use the conventions "heading, pitch, bank" as defined by 3D Math Primer for Graphics and Game Development by Fletcher Dunn.

Firstly, notice that in a 2D coordinate system, you only need a single angle + magnitude to "point" to any point in 2D.

Similarly, in a 3D coordinate system, you only need two angles + magnitude to "point" to any point in 3D. The last angle ("bank" or "roll") does not affect the location of a point in 3D. Instead it "spins" the arrow that would point to it. If the object is 360 degrees symmetrical, you won't see spin affecting the object at all. If the object is not symmetrical (e.g. an airplane) it will affect the object (e.g. tilting one wing towards the ground and the other towards the sky).

So the original question actually becomes, "how do I find the heading angle, pitch angle, and magnitude to "point" to any point in 3D space?"

You can easily figure this out using trigonometry functions. Imagine we have the point (1,2,3) and we're trying to calculate the heading, pitch, magnitude.

For the following example, let's use this diagram, where the left axis is X, up is Y, and right is Z. The point (1,2,3), then is represented by the blue sphere.

1. Find the magnitude

First, let's find the easiest value, the magnitude. Luckily for us, the magnitude (length) between any two points is easy to find no matter how many dimensions we are in, simply by using the Pythagorean theorem. Since we are in 3D and we're calculating the distance from the origin to our point, our distance formula becomes:

magnitude = sqrt(x*x + y*y + z*z)

Plugging in our actual values:

magnitude = sqrt(1*1 + 2*2 + 3*3)
          = 3.7416573868

So our magnitude (or length) is ~3.741.

2. Find the heading

Next, to find the heading, notice that we just care about rotation about the XZ plane, and we don't care about the Y-axis at all. If we were to "flatten" the 3D space into 2D, it becomes trivial to find the heading.

We can draw a triangle that forms a 90 degree angle with the X-axis (red triangle) and then calculate that angle. Recall from trigonometry tan(angle) = opposite / adjacent, and solving for angle, we get angle = arctan(opposite / adjacent).

In this case "adjacent" is a known quantity (redAdjacent = x = 1), and "opposite" is known too (redOpposite = z = 3). Instead of using arctan to solve the equation though, we want to use atan2 since it'll handle all the different cases of x and y for us.

So we have:

heading = atan2(redOpposite, redAdjacent)

Plugging in our actual values:

heading = atan2(3, 1)
        = 1.249045772398

so our heading is 1.249 rad, or ~72°.

3. Find the pitch

Finally we need to find the pitch. Similarly to what we did with the heading, we can flatten the the 3D space into 2D along the plane that contains these three points: (A) the origin (0,0,0), (B) our point (1,2,3), and (C) our point as it would project onto the XZ plane (1,0,3) (e.g. by setting 0 for the Y-value).

If we draw a triangle between all 3 of these points, you will notice that they form a right-triangle again (green triangle). We can simply calculate the angle using arctan2 again.

We already calculated the green hypotenuse in step 1 (i.e. the magnitude of our vector):

greenHypotenuse = sqrt(x*x + y*y + z*z)
                = 3.7416573868

We also know the opposite of the green triangle is the same as the y-value:

greenOpposite = y
              = 2

Using the pythagorean theorem, we can find the length of the adjacent angle:

greenOpposite^2 + greenAdjacent^2 = greenHypotenuse^2
y*y + greenAdjacent^2 = x*x + y*y + z*z
greenAdjacent^2 = x*x + z*z
greenAdjacent = sqrt(x*x + z*z)

Notice that another way to calculate the adjacent length of the green triangle is to notice that redHypotenuse == greenAdjacent, and we could find redHypotenuse using:

redHypotenuse^2 = redAdjacent^2 + redOpposite^2
                = x*x + z*z
redHypotenuse = sqrt(x*x + z*z)

Plugging in actual values, we get:

greenAdjacent = sqrt(1*1 + 3*3)
              = 3.1622776602

So now that we know the adjacent and opposite lengths of the green triangle, we can use arctan2 again:

pitch = atan2(greenOpposite, greenAdjacent)
      = atan2(2, 3.1622776602)
      = 0.563942641356

So our pitch is 0.5634 radians, or about 32°.

Conclusion

If you were to draw a line from the origin, with length 3.741, heading 1.249 rad, and pitch 0.564 rad, it would extend from (0,0,0) to (1,2,3).