Problems getting relative pitch and yaw from vessel heading

[stibbons]

I'm currently trying to add relative pitch and heading of the various orbital vectors to KSPSerialIO, so I can render them on an external navball display.

Some research dug up a useful discussion on calculating pitch and heading, and using that as I reference I have the following functions to calculate relative pitch and heading of a Vector from the current vessel.

        private double[] getOffsetFromHeading(Vessel ActiveVessel, Vector3d targetVector)
        {
            Vector3d yawComponent = Vector3d.Exclude(ActiveVessel.GetTransform().forward, targetVector);
            Vector3d yawCross = Vector3d.Cross(yawComponent, ActiveVessel.GetTransform().right);
            double yaw = SignedVectorAngle(yawComponent, ActiveVessel.GetTransform().up, yawCross);

            Vector3d pitchComponent = Vector3d.Exclude(ActiveVessel.GetTransform().right, targetVector);
            Vector3d pitchCross = Vector3d.Cross(pitchComponent, ActiveVessel.GetTransform().forward);
            double pitch = SignedVectorAngle(pitchComponent, ActiveVessel.GetTransform().up, pitchCross);

            if (Math.Abs(yaw) > 90) {
                yaw = -yaw;
                // This condition makes sure progradePitch doesn't wrap from -x to 360-x
                if (pitch > 0) {
                    pitch = pitch - 180;
                } else {
                    pitch = pitch + 180;
                }
            }
            return new double[] {pitch, yaw};
        }

        private double SignedVectorAngle(Vector3d referenceVector, Vector3d otherVector, Vector3d normal)
        {
            Vector3d perpVector;
            double angle;
            //Use the geometry object normal and one of the input vectors to calculate the perpendicular vector
            perpVector = Vector3d.Cross(normal, referenceVector);
            //Now calculate the dot product between the perpendicular vector (perpVector) and the other input vector
            angle = Vector3d.Angle(referenceVector, otherVector);
            angle *= Math.Sign(Vector3d.Dot(perpVector, otherVector));

            return angle;
        }

And I'm now trying to use it to calculate the position of orbital prograde by doing this

Vector3 progradeVector = ActiveVessel.GetObtVelocity().normalized;
double[] progradeHeading = getOffsetFromHeading(ActiveVessel, progradeVector);
Debug.Log(String.Format("Prograde pitch: {0:F2} heading: {1:F2}", progradeHeading[0], progradeHeading[1]));

This code seems to work properly when the heading is around 0 (or 180). But I'm seeing very unexpected behaviour with the reported pitch varying drastically as the heading gets close to +/- 90 degrees. I tested by lining up with prograde and then pitching down slightly so the prograde marker was above my heading. Then started yawing to the left. Here's a log of a full rotation. It shows the heading (yaw) increasing fairly linearly, and I would expect the pitch to remain close to constant. Instead it gets very high, peaking at -90 degrees yaw, dropping lower than the original pitch at 180, and then climbing to another peak at 90 degrees yaw.

The only rational explanation I've come up with that might explain it is that I need to apply the pitch number first, then the roll, and I'll end up with the vector in the right place. So I tried doing that on my display, and it still doesn't look right. This is from a different run of the same test, but with similar results.

Any other suggestions for what I might be doing wrong here? Do I need to transform the prograde vector? I assumed it was already in the right reference frame.

[Crzyrndm]

When you say relative pitch and heading from prograde, I assume you're trying to calculate yaw and AoA which Pilot Assistant does here. (I don't see any obvious errors, but maybe another example will shed some light) (ProjectOnPlane)

EDIT

What on earth is that limit section doing. Vector3d.Angle will always be inside +/- 180 degrees, and since it's relative, shouldn't be exactly what you want?

Edited January 22, 2016 by Crzyrndm

[stibbons]

I should apologise now - I've never done any 3D programming before, and am struggling to get my head around it.

10 hours ago, Crzyrndm said:

When you say relative pitch and heading from prograde, I assume you're trying to calculate yaw and AoA which Pilot Assistant does here. (I don't see any obvious errors, but maybe another example will shed some light) (ProjectOnPlane)

Thinking it over some more last night, I'm suspecting I've been calculating this the wrong way, and yaw and AoA aren't actually want I want. To draw the prograde marker I'm taking a point on the surface of the navball representing the up vector, and doing a 3D transform to rotate it to the prograde vector. My current understanding is that I need the Euler angles for that, not yaw and AoA. Does that make sense?

10 hours ago, Crzyrndm said:

What on earth is that limit section doing. Vector3d.Angle will always be inside +/- 180 degrees, and since it's relative, shouldn't be exactly what you want?

I didn't find anything about the limits of Angle (although, looking again, the Vector3.Angle docs are pretty clear about it always returning the smaller angle) so was just being super guarded.

[Crzyrndm]

No worries, I was in the same position about twelve months ago when I started on PA. Just have to keep working at it...

So to define what you have and what you want:

• you have a navball reference direction of up / pitch 90 (relative to your navball I'm assuming? ie. it rotates with everything else)
• you have a KSP vector that corresponds to your navball reference (eg. surface normal)
• you want to create a vector that matches a KSP vector on the navball (navball space)

If ^^ is correct, things are about to get a little curly (in a nice way hopefully...). Transforming one direction to another by a rotation is normally done using Quaternions, and Quat's aren't nice things to try and imagine (particularly if you're already having fun with vectors...). This should be a fairly simple application though

// First, calculate the rotation required to go from up to prograde in game space
// From: vector from the center of the body through the vessel (up)
// To: vessel surface prograde
Quaternion progradeRot = Quaternion.FromToRotation(vessel.transform.position - vessel.mainbody.transform.position, vessel.srf_velocity);

// Second, use that rotation to translate your *navball* up reference around to the prograde direction in navball space
// Quaterion * Vector3 gives a new vector3 which is the original rotated by the rotation the Quaterion defines
Vector3 navballPrograde = progradeRot * navballUp;

// You can also get a Euler representation of the Quat (just be aware it's only accurate to about 1 decimal place, eg. 180.5 degrees)
// from memory it's x: pitch, y: yaw, z:roll, but you will want to check that since KSP is often wierd on that front
Vector3 EulerRot = progradeRot.eulerAngles;

EDIT

If you just want the vector in navball space and don't actually care about the angles, there's a better way than the above. A rotation which can be used for any KSP vector you wish to show in navball space

// Calculate the rotation which transforms KSP up to navball up
// From: the planet surface normal (ie. planet up / KSP up)
// To: navball up / navball reference vector
Quaternion navballTransformRot = Quaternion.FromToRotation(planetUp, navballUp);

// now we can multiply any KSP vector by that rotation to get the vector in navball space
Vector3 navballPrograde = navballTransformRot * vessel.srf_prograde;
Vector3 navballRight = navballTransformRot * vessel.transform.right;
// etc.

 

Edited January 23, 2016 by Crzyrndm

[stibbons]

On 1/23/2016 at 7:17 PM, Crzyrndm said:

• you have a navball reference direction of up / pitch 90 (relative to your navball I'm assuming? ie. it rotates with everything else)
• you have a KSP vector that corresponds to your navball reference (eg. surface normal)
• you want to create a vector that matches a KSP vector on the navball (navball space)

 

It's actually slightly easier than this, because I'm already transforming the navball up to match vessel up (see updateHeadingPitchRollField()). That should mean I can ignore the world reference and just calculate the rotation relative to the vessel position. So it seems to me that for surface prograde progradeRot would be more like

Quaternion progradeRot = Quaternion.FromToRotation(vessel.transform.position, vessel.srf_velocity);

And I definitely do need the euler rotation, but it looks like I can easily reuse existing plugin code to convert that Quat.

Thanks very much for the advice and code, @Crzyrndm. Knowing how to calculate rotations like that has really helped illuminate how transforms and quaternions work. I think I've got a handle on how it needs to be done now, but it'll be a little while longer before I've got time to properly test it.