/// <summary>
/// converts an openni matrix rotation to a unity quaternion
/// flipping z axis.
/// </summary>
/// <param name="m">
/// A <see cref="SkeletonJointOrientation"/>
/// </param>
/// <returns>
/// A <see cref="Quaternion"/>
/// </returns>
public static Quaternion openNIMatrixToQuat(SkeletonJointOrientation m)
{
SkeletonJointOrientation n = new SkeletonJointOrientation();
n.X1 = m.X1;
n.X2 = m.X2;
n.X3 = -m.X3;
n.Y1 = m.Y1;
n.Y2 = m.Y2;
n.Y3 = -m.Y3;
n.Z1 = -m.Z1;
n.Z2 = -m.Z2;
n.Z3 = m.Z3;
return matrixToQuat(n);
}
public static Quaternion SkeletonJointOrientationToQuaternion(SkeletonJointOrientation m)
{
float tr = m.X1 + m.Y2 + m.Z3;
float S = 0f;
float qw = 0f;
float qx = 0f;
float qy = 0f;
float qz = 0f;
if(tr > 0) {
S = Mathf.Sqrt(tr + 1.0f) * 2f;
qw = 0.25f * S;
qx = (m.Y3 - m.Z2) / S;
qy = (m.Z1 - m.X3) / S;
qz = (m.X2 - m.Y1) / S;
} else if((m.X1 > m.Y2) && (m.X1 > m.Z3)) {
S = Mathf.Sqrt(1.0f + m.X1 - m.Y2 - m.Z3) * 2f;
qw = (m.Y3 - m.Z2) / S;
qx = 0.25f * S;
qy = (m.Y1 + m.X2) / S;
qz = (m.Z1 + m.X3) / S;
} else if(m.Y2 > m.Z3) {
S = Mathf.Sqrt(1.0f + m.Y2 - m.X1 - m.Z3) * 2f;
qw = (m.Z1 - m.X3) / S;
qx = (m.Y1 + m.X2) / S;
qy = 0.25f * S;
qz = (m.Z2 + m.Y3) / S;
} else {
S = Mathf.Sqrt(1.0f + m.Z3 - m.X1 - m.Y2) * 2f;
qw = (m.X2 - m.Y1) / S;
qx = (m.Z1 + m.X3) / S;
qy = (m.Z2 + m.Y3) / S;
qz = 0.25f * S;
}
return new Quaternion(qx, qy, qz, qw);
}
/// <summary>
/// convert a matrix rotation to a quaternion
/// based on: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
/// </summary>
/// <param name="m">
/// A <see cref="SkeletonJointOrientation"/>
/// </param>
/// <returns>
/// A <see cref="Quaternion"/>
/// </returns>
public static Quaternion matrixToQuat(SkeletonJointOrientation m)
{
float tr = m.X1 + m.Y2 + m.Z3;
float qw = 0f;
float qx = 0f;
float qy = 0f;
float qz = 0f;
if (tr > 0)
{
float S = Mathf.Sqrt(tr+1.0f) * 2f; // S=4*qw
qw = 0.25f * S;
qx = (m.Y3 - m.Z2) / S; //(m21 - m12) / S;
qy = (m.Z1 - m.X3) / S; //(m02 - m20) / S;
qz = (m.X2 - m.Y1) / S; //(m10 - m01) / S;
} else if ((m.X1 > m.Y2) && (m.X1 > m.Z3))//((m00 > m11)&(m00 > m22)) {
{
float S = Mathf.Sqrt(1.0f + m.X1 - m.Y2 - m.Z3) * 2f; //sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*qx
qw = (m.Y3 - m.Z2) / S;//(m21 - m12) / S;
qx = 0.25f * S;
qy = (m.Y1 + m.X2) / S;//(m01 + m10) / S;
qz = (m.Z1 + m.X3) / S;//(m02 + m20) / S;
} else if (m.Y2 > m.Z3)//(m11 > m22)
{
float S = Mathf.Sqrt(1.0f + m.Y2 - m.X1 - m.Z3) * 2f;//sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*qy
qw = (m.Z1 - m.X3) / S; //(m02 - m20) / S;
qx = (m.Y1 + m.X2) / S; //(m01 + m10) / S;
qy = 0.25f * S;
qz = (m.Z2 + m.Y3) / S; //(m12 + m21) / S;
} else
{
float S = Mathf.Sqrt(1.0f + m.Z3 - m.X1 - m.Y2) * 2f;//sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*qz
qw = (m.X2 - m.Y1) / S;//(m10 - m01) / S;
qx = (m.Z1 + m.X3) / S;//(m02 + m20) / S;
qy = (m.Z2 + m.Y3) / S;//(m12 + m21) / S;
qz = 0.25f * S;
}
return new Quaternion(qx, qy, qz, qw);
}
public void DrawOrientation(ref WriteableBitmap image, int id, UserGenerator userGenerator, SkeletonJoint joint, Point3D corner)
{
SkeletonJointOrientation orientation = new SkeletonJointOrientation();
SkeletonJointPosition position = new SkeletonJointPosition();
position = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, joint);
orientation = userGenerator.SkeletonCapability.GetSkeletonJointOrientation(id, joint);
if (position.Confidence != 1 && orientation.Confidence != 1)
{
return;
}
SkeletonJointPosition v1 = new SkeletonJointPosition();
SkeletonJointPosition v2 = new SkeletonJointPosition();
v1.Confidence = v2.Confidence = 1;
v1.Position = position.Position;
v2.Position = new Point3D(v1.Position.X + 100 * orientation.X1,
v1.Position.Y + 100 * orientation.Y1,
v1.Position.Z + 100 * orientation.Z1);
DrawTheLine(ref image, ref v1, ref v2);
v1.Position = position.Position;
v2.Position = new Point3D(v1.Position.X + 100 * orientation.X2,
v1.Position.Y + 100 * orientation.Y2,
v1.Position.Z + 100 * orientation.Z2);
DrawTheLine(ref image, ref v1, ref v2);
v1.Position = position.Position;
v2.Position = new Point3D(v1.Position.X + 100 * orientation.X3,
v1.Position.Y + 100 * orientation.Y3,
v1.Position.Z + 100 * orientation.Z3);
DrawTheLine(ref image, ref v1, ref v2);
}
public SkeletonJointOrientation GetSkeletonJointOrientation(UserID user, SkeletonJoint joint)
{
SkeletonJointOrientation orientation = new SkeletonJointOrientation();
int status = SafeNativeMethods.xnGetSkeletonJointOrientation(this.InternalObject, user, joint, ref orientation);
WrapperUtils.ThrowOnError(status);
return orientation;
}
/// @brief Gets the current joint orientation for a specific joint
///
/// @param joint The joint we want the orientation to.
/// @param curRotation [out] The current joint rotation
/// @return True on success and false on failure (e.g. the user is not tracking).
/// @note An exception might occur if there is an error (e.g. an illegal joint is used).
public bool GetSkeletonJointOrientation(SkeletonJoint joint, out SkeletonJointOrientation curRotation)
{
curRotation = NIPlayerCandidateObject.m_InitializedZero.Orientation;
if (!Tracking)
return false;
if (m_user.Skeleton == null)
return false;
try
{
curRotation = m_user.Skeleton.GetSkeletonJointOrientation(m_user.OpenNIUserID, joint);
}
catch
{
return false;
}
return true;
}
public void JointFromJSON(SkeletonJoint j, Hashtable dict)
{
ArrayList positionList = (ArrayList)dict["Position"];
Hashtable oriHash = (Hashtable) dict["Orientation"];
ArrayList orientationList = (ArrayList) oriHash["Data"];
SkeletonJointOrientation sjo = new SkeletonJointOrientation();
sjo.X1 = 1.0f;
SkeletonJointPosition sjp = new SkeletonJointPosition();
SkeletonJointTransformation xform = new SkeletonJointTransformation();
// object -> double ->float is okay, but object->float isn't
// (the object is a Double)
sjp.Position = new Point3D((float)(double)positionList[0],
(float)(double)positionList[1],
(float)(double)positionList[2]);
sjo.X1 = (float)(double)orientationList[0];
sjo.X2 = (float)(double)orientationList[1];
sjo.X3 = (float)(double)orientationList[2];
sjo.Y1 = (float)(double)orientationList[3];
sjo.Y2 = (float)(double)orientationList[4];
sjo.Y3 = (float)(double)orientationList[5];
sjo.Z1 = (float)(double)orientationList[6];
sjo.Z2 = (float)(double)orientationList[7];
sjo.Z3 = (float)(double)orientationList[8];
sjo.Confidence = (float)(double)oriHash["Confidence"];
xform.Orientation = sjo;
xform.Position = sjp;
UpdateJoint(j, xform);
}
public static extern XnStatus xnGetSkeletonJointOrientation(XnNodeHandle hInstance, XnUserID user, SkeletonJoint joint, ref SkeletonJointOrientation orientation);
