Last active
June 18, 2021 17:55
-
-
Save Vangos/c3270d22d902aaeb9a435e37261a2e94 to your computer and use it in GitHub Desktop.
Kinect Joint Rotation from Orientation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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
Yes, it should work with any quaternion.