Skip to content

Instantly share code, notes, and snippets.

@Vangos
Last active June 18, 2021 17:55
Show Gist options
  • Save Vangos/c3270d22d902aaeb9a435e37261a2e94 to your computer and use it in GitHub Desktop.
Save Vangos/c3270d22d902aaeb9a435e37261a2e94 to your computer and use it in GitHub Desktop.
Kinect Joint Rotation from Orientation
using System;
using Microsoft.Kinect;
namespace LightBuzz.Vitruvius
{
/// <summary>
/// Provides extension methods for transforming quaternions to rotations.
/// </summary>
public static class OrientationExtensions
{
/// <summary>
/// Rotates the specified quaternion around the X axis.
/// </summary>
/// <param name="quaternion">The orientation quaternion.</param>
/// <returns>The rotation in degrees.</returns>
public static double Pitch(this Vector4 quaternion)
{
double value1 = 2.0 * (quaternion.W * quaternion.X + quaternion.Y * quaternion.Z);
double value2 = 1.0 - 2.0 * (quaternion.X * quaternion.X + quaternion.Y * quaternion.Y);
double roll = Math.Atan2(value1, value2);
return roll * (180.0 / Math.PI);
}
/// <summary>
/// Rotates the specified quaternion around the Y axis.
/// </summary>
/// <param name="quaternion">The orientation quaternion.</param>
/// <returns>The rotation in degrees.</returns>
public static double Yaw(this Vector4 quaternion)
{
double value = +2.0 * (quaternion.W * quaternion.Y - quaternion.Z * quaternion.X);
value = value > 1.0 ? 1.0 : value;
value = value < -1.0 ? -1.0 : value;
double pitch = Math.Asin(value);
return pitch * (180.0 / Math.PI);
}
/// <summary>
/// Rotates the specified quaternion around the Z axis.
/// </summary>
/// <param name="quaternion">The orientation quaternion.</param>
/// <returns>The rotation in degrees.</returns>
public static double Roll(this Vector4 quaternion)
{
double value1 = 2.0 * (quaternion.W * quaternion.Z + quaternion.X * quaternion.Y);
double value2 = 1.0 - 2.0 * (quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z);
double yaw = Math.Atan2(value1, value2);
return yaw * (180.0 / Math.PI);
}
}
}
@aanexplus
Copy link

Hello,
Just wondering if this will work with the new Azure Kinect Orientation?

Thank you

@Vangos
Copy link
Author

Vangos commented Jun 18, 2021

Yes, it should work with any quaternion.

@aanexplus
Copy link

Cool, thanks

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment