[Method] GetRotationAngles - Converting a direction vector to pitch and yaw angles

Background
Howdy! This method is designed to help you compute pitch (elevation) and yaw (azimuth) to a target direction using a specific block as a reference.

The geometry of this problem is as follows:


Input(s)

• targetVector: Direction vector to the target
• reference: Block to use as a reference to compute pitch and yaw

Output(s)

• yaw: Yaw angle measured clockwise from the reference block's forward vector. Positive means the targetVector is to the right of the forward vector. [radians]
• pitch: Pitch angle measured up from the forward-right plane. Positive means the targetVector is above the forward-up plane. [radians]

Code

Pastebin link

Raw Text

Code:

/*
/// Whip's Get Rotation Angles Method v12 - 2/16/18 ///
Dependencies: VectorAngleBetween()
* Fix to solve for zero cases when a vertical target vector is input
* Fixed straight up case
* Fixed sign on straight up case
* Converted math to local space
*/
void GetRotationAngles(Vector3D targetVector, IMyTerminalBlock reference, out double yaw, out double pitch)
{
   var localTargetVector = Vector3D.TransformNormal(targetVector, MatrixD.Transpose(reference.WorldMatrix));
   var flattenedTargetVector = new Vector3D(localTargetVector.X, 0, localTargetVector.Z);
  
	yaw = VectorAngleBetween(Vector3D.Forward, flattenedTargetVector) * Math.Sign(localTargetVector.X); //right is positive
   if (Math.Abs(yaw) < 1E-6 && localTargetVector.Z > 0) //check for straight back case
		yaw = Math.PI;
  
   if (Vector3D.IsZero(flattenedTargetVector)) //check for straight up case
		pitch = MathHelper.PiOver2 * Math.Sign(localTargetVector.Y);
   else
		pitch = VectorAngleBetween(localTargetVector, flattenedTargetVector) * Math.Sign(localTargetVector.Y); //up is positive
}

double VectorAngleBetween(Vector3D a, Vector3D b) //returns radians
{
   if (Vector3D.IsZero(a) || Vector3D.IsZero(b))
	   return 0;
   else
	   return Math.Acos(MathHelper.Clamp(a.Dot(b) / Math.Sqrt(a.LengthSquared() * b.LengthSquared()), -1, 1));
}

Example

Code:

void Main()
{
	var referenceBlock = SomeFunctionToAssignThisBlock(); //I like to use remote controls for the reference
	var targetPosition = new Vector3D(69, 69, 69);
	var referencePosition = referenceBlock.GetPosition();
	var directionToTarget = targetPosition - referencePosition; //this is how we get our target vector
	double pitch = 0;
	double yaw = 0;

	GetRotationAngles(directionToTarget, referenceBlock, out yaw, out pitch);
}

 

Local up vector is +Y
Local right vector is +X ?
Local forward vector is -Z ?!?

 

Lol yes, it hurts me.