Create a gist now

Instantly share code, notes, and snippets.

What would you like to do?
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);
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment