Gravity vector and forward vector to local heading (yaw), pitch and roll

Question

I've looked around for hours and I simply don't get it. I am the first to admit I am severely lacking in mathematics. So please, keep it simple.

I am trying to get a vectors rotation (angle) around another vector so i can get useful Yaw, Pitch and Roll from said vectors. This on a "vehicle" that has a forward vector (and of course accompanying right and up vectors) which is "flying" over a planet (sphere) with gravity going towards it's center. This gravity vector is also known.

Here's an example of what i am looking at:

I am using Unity to EXPERIMENT with. The goal is to implement this in another project. Thus, all the powers of Unity doesn't help me find a solution for my other project. I am constricted to Vector3.Cross(), Vector3.Dot, Math.Cos/Sin/ACos/ASin/Tan/ATan/ATan2 etc.

Here's the code i have. I did remove all the calculations i tried beforehand as they all failed and i'd rather have a clean answer.

    // Time that debug lines and rays will be displayed for.
    var duration = Time.deltaTime;
    // Draws a line through the south/north poles of the sphwere.
    Debug.DrawLine(Vector3.down * 100, Vector3.up * 100, Color.cyan, duration, false);

    // Gravity that affects the vehicle
    var gravity = Vector3.Normalize(planet.transform.position - transform.position) * 9.81F;
    Debug.DrawRay(transform.position, gravity, new Color(0.4F, 0.0F, 0.0F), duration, false);

    var gravityEast = Vector3.Cross(gravity.normalized, Vector3.down).normalized;
    var gravityNorth = Vector3.Cross(gravityEast.normalized, -gravity.normalized).normalized;

    Debug.DrawRay(transform.position, gravityEast, Color.yellow, duration, false);
    Debug.DrawRay(transform.position, gravityNorth, Color.white, duration, false);
    Debug.DrawRay(transform.position, transform.forward * 0.5F, Color.blue, duration, false);

    text.text = "gravity: " + gravity + "\n";
    text.text += "gravityEast: " + gravityEast + "\n";
    text.text += "gravityNorth: " + gravityNorth + "\n\n";

    text.text += "forward: " + transform.forward + "\n\n";

I am looking to get Yaw in 360 degrees (-180 to 180 works too and even radians, that's not a problem), roll in -180 to 180 degrees and pitch in -90 to 90 degrees. This all in respect to the gravity vector and where zero Yaw is pointing to the north along the sphere. 90 Yaw is east, 180 is south and 270 is west.

As i said, i've been looking at hundreds of different examples (both on SE and elsewhere) and i still don't understand how to do this. All the things i have tried have either not worked at all or they have worked only in certain circumstances such as the "vehicle" being on one side of the sphere but not the others. As far as i can understand, i need to transform the calculations to local vector space so that the formulas stay relative to my ever changing gravity vector.

And because i am mathematically illiterate at best, a bunch of mathematical formulas won't really help me. I've looked at them all and it just doesn't click. Instead, if this can be shown in pseudocode or even straight up C# that would be optimal.

And just to be perfectly clear here. I know i can use "transform.eulerAngles" on my cube to get global and local angles right away. Again, i need to port the solution to another project that doesn't come with those simple properties/fields.

Thank you!

Answer 1

I will try to break down the step-by-step questions I'd take to solve this problem, with the hope it helps you to understand how to go about solving vector-related problems in the future.

Note: I'll be using radians, and even if your final on-screen output is in degrees I recommend you do calculations in radians as opposed to degrees, since they behave more nicely in a number of ways (for example, you don't have to use additional scaling factors when recalculating angular velocity or acceleration). There's a reason why just about any math library uses radians. Also, sin and cos are defined assuming angles increase going counterclockwise, so you may have to adjust for that. If you prefer clockwise angles and degrees, that's just a matter of multiplying by -1 and 180/pi respectively where appropriate.

Q) What is pitch?
A) pitch is the angle between a direction and the horizontal component of that direction (which we'll call heading). of course, "horizontal" depends on your definition of "up", so we use the gravity vector.

float pitch(Vector3 direction, Vector3 gravity) { 
    Vector3 heading = projectOnPlane(direction, gravity);
    return angleBetween(direction, heading); 
}

Q) What is Yaw?
A) Yaw is the angle between the heading and north. So, it looks very similar to the pitch equation:

float pitch(Vector3 direction, Vector3 gravity, Vector3 north) { 
    Vector3 heading = projectOnPlane(direction, gravity);
    return angleBetween(direction, north); 
}

Q) Okay, but how do I get the heading -- the "horizontal" component (i.e. perpendicular to gravity) of a direction?
A) formula: See this formula from Wikipedia:Vector Projection, although you may find easier places to read on vector projection. To project on to a plane, you subtract the vector projection on the normal vector of the plane from the original vector. Or, phrased in english, to get the horizontal component of a vector, you subtract the vertical component from the original vector. Translated verbatim into code:

float projectOnPlane(Vector3 a, Vector3 b) { 
    Vector3 bProjection = (Vector3.Dot(a, b) / b.Length) * (b / b.Length);
    return a - bProjection;
}

Q) How do I get the angle between two vectors?
A) Most places tell give you some Equation that looks like . However, this will return the same value for either a clockwise or counterclockwise rotation, and we want the signed angle. So, we want the Atan2-based formula:

 float angleBetween(Vector3 a, Vector3 b) { 
    return Math.Atan2(Vector3.Cross(a, b).Length, Vector3.Dot(a, b));
 }

Tada! we're done!

Answer 2

I found the solution myself after a (bad) night's sleep on it.

Working Code

void Update () {
    // NOTE: I am really bad at this (vectors, cross products, dot products etc), hence my comments will be to the best of my understanding.

    // Time that debug lines and rays will be displayed for.
    var duration = Time.deltaTime;
    // Draws a line through the south/north poles of the sphwere.
    Debug.DrawLine(Vector3.down * 100, Vector3.up * 100, Color.gray, duration, false);

    // Gravity that affects the vehicle
    var gravity = Vector3.Normalize(planet.transform.position - transform.position) * 9.81F;
    // Cross products of gravity to describe planes.
    var gravityEast = Vector3.Cross(gravity.normalized, Vector3.down).normalized;
    var gravityNorth = Vector3.Cross(gravityEast.normalized, -gravity.normalized).normalized;

    // Draw vehicle forward, right and up vectors. Makes it easier to see how it's currently rotated.
    Debug.DrawRay(transform.position, transform.forward * 0.5F, Color.cyan, duration, false);
    Debug.DrawRay(transform.position, transform.right * 0.5F, Color.cyan, duration, false);
    Debug.DrawRay(transform.position, transform.up * 0.5F, Color.cyan, duration, false);

    // Draw gravity vector and it's plane vectors.
    Debug.DrawRay(transform.position, gravity.normalized, new Color(0.0F, 0.0F, 0.5F), duration, false);
    Debug.DrawRay(transform.position, gravityEast, new Color(0.5F, 0.5F, 0.0F), duration, false);
    Debug.DrawRay(transform.position, gravityNorth, new Color(0.5F, 0.0F, 0.0F), duration, false);

    // Debug text to verify the absolute values of the gravity and it's plane vectors.
    text.text = "gravity: " + gravity + "\n";
    text.text += "gravityEast: " + gravityEast + "\n";
    text.text += "gravityNorth: " + gravityNorth + "\n\n";

    // Define "aligned" (plane) vectors that rotate with the vehicle.
    //  For example, gravityAlignedRight will stay pointing along the height plane over the
    //  surface of the sphere and rotate with the vehicle always pointing to the right.
    //  This is used to determine angular difference between vehicles right vector and gravity's aligned right vector.
    //  The reason we need that is because Vector3.Angle() would measure the whole angle between a fixed vector such as
    //  gravityEast and the vehicle right vector otherwise.
    // gravityAlignedForward is used for yaw and pitch.
    // gravityAlignedDown is used for pitch and is needed to get the correct dot product for pitch.
    var gravityAlignedRight = Vector3.Cross(gravity.normalized, -transform.forward).normalized;
    var gravityAlignedForward = Vector3.Cross(gravity.normalized, gravityAlignedRight).normalized;
    var gravityAlignedDown = Vector3.Cross(gravityAlignedRight, transform.forward).normalized;

    // Draw aligned gravity vectors for ease of debugging.
    Debug.DrawRay(transform.position, gravityAlignedForward * 0.25F, Color.green, duration, false);
    Debug.DrawRay(transform.position, gravityAlignedRight * 0.25F, Color.magenta, duration, false);
    Debug.DrawRay(transform.position, gravityAlignedDown * 0.25F, Color.red, duration, false);

    // And draw the absolute values which helped me set them up.
    // That is, i had to experiment my way forward to each of these...
    text.text += "gravityAlignedRight: " + gravityAlignedRight + "\n";
    text.text += "gravityAlignedForward: " + gravityAlignedForward + "\n";
    text.text += "gravityAlignedDown: " + gravityAlignedDown + "\n\n";

    // Get dot products for yaw, pitch and roll so we can set the appropriate sign on the final YPR readings.
    var yawDot = Vector3.Dot(gravityAlignedForward, gravityEast);
    var pitchDot = -Vector3.Dot(gravityAlignedDown, gravityAlignedForward);
    var rollDot = -Vector3.Dot(transform.up, gravityAlignedRight);

    // Measure angle between the aligned gravity vectors and the vehicles forward and right vectors.
    // yaw is special in that it uses the aligned forward gravity vector. Don't ask me why, i wouldn't know... ;)
    var yaw = Vector3.Angle(gravityAlignedForward, gravityNorth) * Mathf.Sign(yawDot);
    var pitch = Vector3.Angle(transform.forward, gravityAlignedForward) * Mathf.Sign(pitchDot);
    var roll = Vector3.Angle(transform.right, gravityAlignedRight) * Mathf.Sign(rollDot);

    // Debug text showing the results of all this... It works, finally.
    text.text += "yawDot: " + yawDot + "\n";
    text.text += "pitchDot: " + pitchDot + "\n";
    text.text += "rollDot: " + rollDot + "\n\n";

    text.text += "yaw: " + yaw + "\n";
    text.text += "pitch: " + pitch + "\n";
    text.text += "roll: " + roll + "\n";
}

---

Debug shots

Some shots where everything just works... I am happy now because i can actually port this to the other project! (I hope)

Ok, so i made a mistake when taking the screenshot, pitch from 22.5 to 135 in unity

And with all that said, thanks to everyone that read and tried to help me out. I never want to have to figure this out again...

That being said, i still would very much appreciate input on what i have done and if it can be simplified in any way as well as perhaps some eureka hints you could send my way to actually make me understand what i have done here.

Cheers!

EDIT:

IT WORKS! ;)
Thanks again!