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