/// @brief updates the skeleton
///
/// This method is responsible for updating the skeleton based on new information.
/// It is called by an external manager with the correct user which controls this specific skeleton.
/// @param userID the user id of the user which controls us. @note this is the NI user ID!
/// @param skeleton the skeleton object to use.
/// @param centerOffset the offset we should use on the center (when moving the root).
/// This is usually the starting position (so the skeleton will not "jump" when doing the first update
public void UpdateSkeleton(int userID, NIUserAndSkeleton skeleton, Vector3 centerOffset)
{
if (skeleton.Valid == false)
{
return; // no skeleton
}
if (skeleton.GetUserCalibrationState(userID) != NIUserAndSkeleton.CalibrationState.calibrated)
{
return; // we are not calibrated.
}
if (skeleton.Skeleton.IsTracking(userID) == false)
{
return; // we are not tracking
}
// we use the torso as root
SkeletonJointTransformation skelTrans = skeleton.Skeleton.GetSkeletonJoint(userID, SkeletonJoint.Torso);
UpdateRoot(skelTrans.Position, centerOffset);
// update each joint with data from OpenNI
foreach (SkeletonJoint joint in Enum.GetValues(typeof(SkeletonJoint)))
{
if (skeleton.Skeleton.IsJointAvailable(joint))
{
SkeletonJointTransformation skelTransJoint = skeleton.Skeleton.GetSkeletonJoint(userID, joint);
UpdateJoint(joint, skelTransJoint, skelTrans.Position);
}
}
}
public void JointFromJSON(SkeletonJoint j, Hashtable dict)
{
ArrayList positionList = (ArrayList)dict["Position"];
ArrayList orientationList = (ArrayList)dict["Orientation"];
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];
xform.Orientation = sjo;
xform.Position = sjp;
UpdateJoint(j, xform);
}
public void UpdateSkeleton(int userId, OpenNISkeleton skeleton)
{
// make sure we have skeleton data for this user
if (!skeletonCapbility.IsTracking(userId))
{
return;
}
// Use torso as root
SkeletonJointTransformation skelTrans = new SkeletonJointTransformation();
skelTrans = skeletonCapbility.GetSkeletonJoint(userId, SkeletonJoint.Torso);
if (skeleton.absolute)
{
Point3D pos = skelTrans.Position.Position;
skeleton.UpdateRoot(new Vector3(pos.X, pos.Y, pos.Z));
}
else
{
Point3D pos = skelTrans.Position.Position;
Vector3 v3dpos = new Vector3(pos.X, pos.Y, -pos.Z);
Vector3 calPos = userCalibrationPosition[userId];
skeleton.UpdateRoot(calPos - v3dpos);
}
// update each joint with data from OpenNI
foreach (SkeletonJoint joint in Enum.GetValues(typeof(SkeletonJoint)))
{
if (skeletonCapbility.IsJointAvailable(joint))
{
skelTrans = skeletonCapbility.GetSkeletonJoint(userId, joint);
skeleton.UpdateJoint(joint, skelTrans);
}
}
}
/// @brief updates a single joint
///
/// This method updates a single joint. The decision of what to update (orientation, position)
/// depends on m_updateOrientation and m_updateJointPositions. Only joints with high confidence
/// are updated. @note it is possible to update only position or only orientation even though both
/// are expected if the confidence of one is low.
/// @param centerOffset the new central position
/// @param joint the joint we want to update
/// @param skelTrans the new transformation of the joint
protected void UpdateJoint(SkeletonJoint joint, SkeletonJointTransformation skelTrans, SkeletonJointPosition centerOffset)
{
// make sure something is hooked up to this joint
if ((int)joint >= m_jointTransforms.Length || !m_jointTransforms[(int)joint])
{
return;
}
// if we have debug lines to draw we need to collect the data.
if (m_linesDebugger != null)
{
Vector3 pos = CalcJointPosition(joint, ref skelTrans, ref centerOffset) + transform.position;
float posConf = skelTrans.Position.Confidence;
Quaternion rot = CalcRotationForJoint(joint, ref skelTrans, ref centerOffset);
float rotConf = skelTrans.Orientation.Confidence;
m_linesDebugger.UpdateJointInfoForJoint(joint, pos, posConf, rot, rotConf);
}
// modify orientation (if needed and confidence is high enough)
if (m_updateOrientation && skelTrans.Orientation.Confidence >= 0.5)
{
m_jointTransforms[(int)joint].rotation = CalcRotationForJoint(joint, ref skelTrans, ref centerOffset);
}
// modify position (if needed, and confidence is high enough)
if (m_updateJointPositions && skelTrans.Position.Confidence >= 0.5f)
{
m_jointTransforms[(int)joint].localPosition = CalcJointPosition(joint, ref skelTrans, ref centerOffset);
}
}
void skeletonCapbility_CalibrationEnd(object sender, CalibrationEndEventArgs e)
{
if (e.Success)
{
if (AttemptToCalibrate)
{
SendMessage("CalibrationComplete", e, SendMessageOptions.DontRequireReceiver);
this.skeletonCapbility.StartTracking(e.ID);
SkeletonJointTransformation skelTrans = new SkeletonJointTransformation();
skelTrans = skeletonCapbility.GetSkeletonJoint(e.ID, SkeletonJoint.Torso);
Point3D pos = skelTrans.Position.Position;
userCalibrationPosition[e.ID] = new Vector3(pos.X, pos.Y, -pos.Z);
calibratedUsers.Add(e.ID);
}
}
else
{
SendMessage("CalibrationFailed", e, SendMessageOptions.DontRequireReceiver);
if (AttemptToCalibrate)
{
this.poseDetectionCapability.StartPoseDetection(calibPose, e.ID);
}
}
calibratingUsers.Remove(e.ID);
}
public void UpdateJoint(SkeletonJoint joint, SkeletonJointTransformation skelTrans)
{
// save raw data
jointData[(int)joint] = skelTrans;
// make sure something is hooked up to this joint
if (!transforms[(int)joint])
{
return;
}
// modify orientation (if confidence is high enough)
if (UpdateOrientation && skelTrans.Orientation.Confidence > 0.5)
{
// Z coordinate in OpenNI is opposite from Unity
// Convert the OpenNI 3x3 rotation matrix to unity quaternion while reversing the Z axis
Vector3 worldZVec = new Vector3(-skelTrans.Orientation.Z1, -skelTrans.Orientation.Z2, skelTrans.Orientation.Z3);
Vector3 worldYVec = new Vector3(skelTrans.Orientation.Y1, skelTrans.Orientation.Y2, -skelTrans.Orientation.Y3);
Quaternion jointRotation = Quaternion.LookRotation(worldZVec, worldYVec);
Quaternion newRotation = transform.rotation * jointRotation * initialRotations[(int)joint];
transforms[(int)joint].rotation = Quaternion.Slerp(transforms[(int)joint].rotation, newRotation, Time.deltaTime * RotationDamping);
}
// modify position (if needed, and confidence is high enough)
if (UpdateJointPositions)
{
Vector3 v3pos = new Vector3(skelTrans.Position.Position.X, skelTrans.Position.Position.Y, -skelTrans.Position.Position.Z);
transforms[(int)joint].localPosition = Vector3.Scale(v3pos, Scale) - rootPosition;
}
}
// EnuemrateActiveJoints
public SkeletonJointTransformation GetSkeletonJoint(UserID user, SkeletonJoint joint)
{
SkeletonJointTransformation transformation = new SkeletonJointTransformation();
int status = SafeNativeMethods.xnGetSkeletonJoint(this.InternalObject, user, joint, ref transformation);
WrapperUtils.ThrowOnError(status);
return(transformation);
}
/// @brief Utility method to calculate the @b LOCAL position of a joint
///
/// This method receives joint information and calculates the @b LOCAL position rotation of the joint
/// (compare to its parent transform) in Unity coordinate system.
/// @param centerOffset the new central position
/// @param joint the joint we want to calculate the position for
/// @param skelTrans the new transformation of the joint
/// @return the @b LOCAL position rotation of the joint (compare to its parent transform) in
/// Unity coordinate system
protected Vector3 CalcJointPosition(SkeletonJoint joint, ref SkeletonJointTransformation skelTrans, ref SkeletonJointPosition centerOffset)
{
Vector3 v3pos = NIConvertCoordinates.ConvertPos(skelTrans.Position.Position);
Vector3 v3Center = NIConvertCoordinates.ConvertPos(centerOffset.Position);
v3pos -= v3Center;
return(v3pos * m_scale);
}
/// @brief Gets a reference joint position and orientation on the joint.
///
/// This method gets the reference joint information for the joint. The reference
/// is the joint position and transformation at the time when the user was chosen AND
/// started tracking.
/// @param joint The joint we want information on.
/// @param referenceTransform [out] The reference Transform.
/// @return True on success and false on failure (e.g. an illegal joint or the user is not tracking).
public bool GetReferenceSkeletonJointTransform(SkeletonJoint joint, out SkeletonJointTransformation referenceTransform)
{
referenceTransform = NIPlayerCandidateObject.m_InitializedZero;
if (!Valid)
{
return(false);
}
return(m_user.GetReferenceSkeletonJointTransform(joint, out referenceTransform));
}
/// @brief Gets a reference joint position and orientation on the joint.
///
/// This method gets the reference joint information for the joint. The reference
/// is the joint position and transformation at the time when the user was chosen AND
/// started tracking.
/// @param joint The joint we want information on.
/// @param referenceTransform [out] The reference Transform.
/// @return True on success and false on failure (e.g. an illegal joint or the user is not tracking).
public bool GetReferenceSkeletonJointTransform(SkeletonJoint joint, out SkeletonJointTransformation referenceTransform)
{
referenceTransform = m_InitializedZero;
if (m_playerStatus != UserStatus.Tracking || m_openNIUserID <= 0)
{
return(false);
}
referenceTransform = m_referenceSkeletonJointTransform[joint];
return(true);
}
/// initialize from OpenNI data
public void FromRaw(SkeletonJointTransformation k)
{
// m_joint = joint;
Point3D p = k.Position.Position;
m_pos.x = p.X;
m_pos.y = p.Y;
m_pos.z = 1500-p.Z; // the z axis of the (Kinect) sensor is opposite to the one in Unity3D.
// 1500 is a arbitrary offset to make the value around 0.
m_posConf = k.Position.Confidence;
SkeletonJointOrientation o = k.Orientation;
m_orient = new Vector3[3];
m_orient[0].x = o.X1; m_orient[0].y = o.X2; m_orient[0].z = -o.X3; // the z axis of the sensor is opposite to the one in Unity3D
m_orient[1].x = o.Y1; m_orient[1].y = o.Y2; m_orient[1].z = -o.Y3;
m_orient[2].x = o.Z1; m_orient[2].y = o.Z2; m_orient[2].z = -o.Z3;
m_orientConf = o.Confidence;
}
/// @brief initializes the class with a new user.
///
/// initializes the class with a new user (The class is invalid (m_uniqueID<0) until this is done successfully)
/// @param usersGenerator the users generator which holds the users.
/// @param uniqueID the uniqueID of the user we should initialize to.
/// @return true on success and false on failure (making the object invalid, i.e. m_uniqueID<0).
public bool InitInternalSkeleton(NIUserAndSkeleton usersGenerator, int uniqueID)
{
m_uniqueID = -1; // to make sure we are invalid until we are sure we are valid.
if (usersGenerator.Valid == false)
return false; // must have a valid users generator to do anything
m_userID = usersGenerator.GetNIUserId(uniqueID);
if (m_userID < 0)
return false; // means the uniqueID was illegal
if (usersGenerator.GetUserCalibrationState(m_userID) != NIUserAndSkeleton.CalibrationState.calibrated)
return false; // we only initialize calibrated users as we need the skeleton.
int numJoints = 0; // holds the place to position the next joint.
foreach (SkeletonJoint joint in Enum.GetValues(typeof(SkeletonJoint)))
{
if (usersGenerator.Skeleton.IsJointAvailable(joint)==false)
continue; // an irrelevant joint.
// we need to add the joint but we don't want to create a new one unless we need to.
if (numJoints < m_jointsIDs.Count)
{
m_jointsIDs[numJoints] = joint;
}
else
{
m_jointsIDs.Add(joint);
}
// we need to add the joint transformation but we don't want to create a new one unless we need to.
if(numJoints<m_jointsTrans.Count)
{
m_jointsTrans[numJoints]=usersGenerator.Skeleton.GetSkeletonJoint(m_userID, joint);
}
else
{
SkeletonJointTransformation skelTrans = usersGenerator.Skeleton.GetSkeletonJoint(m_userID, joint);
m_jointsTrans.Add(skelTrans);
}
numJoints++;
}
if (numJoints < m_jointsIDs.Count)
{
for (; numJoints < m_jointsIDs.Count; numJoints++)
m_jointsIDs[numJoints] = SkeletonJoint.Invalid;
}
m_uniqueID = uniqueID;
return true;
}
/// @brief Utility method to calculate the rotation of a joint
///
/// This method receives joint information and calculates the rotation of the joint in Unity
/// coordinate system.
/// @param centerOffset the new central position
/// @param joint the joint we want to calculate the rotation for
/// @param skelTrans the new transformation of the joint
/// @return the rotation of the joint in Unity coordinate system
protected Quaternion CalcRotationForJoint(SkeletonJoint joint, ref SkeletonJointTransformation skelTrans, ref SkeletonJointPosition centerOffset)
{
// In order to convert the skeleton's orientation to Unity orientation we will
// use the Quaternion.LookRotation method to create the relevant rotation Quaternion.
// for Quaternion.LookRotation to work it needs a "forward" vector and an "upward" vector.
// These are generally the "Z" and "Y" axes respectively in the sensor's coordinate
// system. The orientation received from the skeleton holds these values in their
// appropriate members.
// Get the forward axis from "z".
Point3D sensorForward = Point3D.ZeroPoint;
sensorForward.X = skelTrans.Orientation.Z1;
sensorForward.Y = skelTrans.Orientation.Z2;
sensorForward.Z = skelTrans.Orientation.Z3;
// convert it to Unity
Vector3 worldForward = NIConvertCoordinates.ConvertPos(sensorForward);
worldForward *= -1.0f; // because the Unity "forward" axis is opposite to the world's "z" axis.
if (worldForward.magnitude == 0)
{
return(Quaternion.identity); // we don't have a good point to work with.
}
// Get the upward axis from "Y".
Point3D sensorUpward = Point3D.ZeroPoint;
sensorUpward.X = skelTrans.Orientation.Y1;
sensorUpward.Y = skelTrans.Orientation.Y2;
sensorUpward.Z = skelTrans.Orientation.Y3;
// convert it to Unity
Vector3 worldUpwards = NIConvertCoordinates.ConvertPos(sensorUpward);
if (worldUpwards.magnitude == 0)
{
return(Quaternion.identity); // we don't have a good point to work with.
}
Quaternion jointRotation = Quaternion.LookRotation(worldForward, worldUpwards);
Quaternion newRotation = transform.rotation * jointRotation * m_jointsInitialRotations[(int)joint];
// we try to limit the speed of the change.
return(Quaternion.Slerp(m_jointTransforms[(int)joint].rotation, newRotation, Time.deltaTime * m_rotationDampening));
}
/// returns the skeleton transformation that was set when the user was initially added to the list
/// of users for a specific joint.
/// @param playerID a running number of the player ID (player 1 is 0, player 2 is 1 etc.)
/// @param joint the joint we want the transform of this.
/// @param[out] skelTransform the skeleton transformation
/// @return will return true if we found the joint for that player and false otherwise.
public bool GetJointInitialTransform(int playerID, SkeletonJoint joint, out SkeletonJointTransformation skelTransform)
{
skelTransform = m_internalTransformation;
if (playerID < 0 || playerID >= m_players.Count)
return false;
InternalSkeleton player=m_players[playerID];
if (player.UniqueID < 0)
return false;
List<SkeletonJoint> jointList = player.JointsIDs;
if (jointList == null)
return false; // no joint!
for (int i = 0; i < jointList.Count; i++)
{
if (jointList[i] == joint)
{
skelTransform = player.JointTransforms[i];
return true;
}
}
return false;
}
/// @brief Gets the current joint transformation for a specific joint
///
/// @param joint The joint we want the transformation to.
/// @param curTransform [out] The current joint transformation
/// @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 GetSkeletonJoint(SkeletonJoint joint, out SkeletonJointTransformation curTransform)
{
curTransform = NIPlayerCandidateObject.m_InitializedZero;
if (!Tracking)
{
return(false);
}
if (m_user.Skeleton == null)
{
return(false);
}
try
{
curTransform = m_user.Skeleton.GetSkeletonJoint(m_user.OpenNIUserID, joint);
}
catch
{
return(false);
}
return(true);
}
public void UpdateSkeleton(int userId, OpenNISkeleton skeleton)
{
// make sure we have skeleton data for this user
if (!skeletonCapbility.IsTracking(userId))
{
return;
}
// Use torso as root
SkeletonJointTransformation skelTrans = new SkeletonJointTransformation();
skelTrans = skeletonCapbility.GetSkeletonJoint(userId, SkeletonJoint.Torso);
skeleton.UpdateRoot(skelTrans.Position.Position);
// update each joint with data from OpenNI
foreach (SkeletonJoint joint in Enum.GetValues(typeof(SkeletonJoint)))
{
if (skeletonCapbility.IsJointAvailable(joint))
{
skelTrans = skeletonCapbility.GetSkeletonJoint(userId, joint);
skeleton.UpdateJoint(joint, skelTrans);
}
}
}
// EnuemrateActiveJoints
public void GetSkeletonJoint(UserID user, SkeletonJoint eJoint, SkeletonJointTransformation joint)
{
UInt32 status = OpenNIImporter.xnGetSkeletonJoint(this.InternalObject, user, eJoint, joint);
WrapperUtils.CheckStatus(status);
}
/// initialize from OpenNI data
public JointData (SkeletonJointTransformation k):this()
{
// m_joint = joint;
FromRaw(k);
}
// Overload to instead use the skeleton joint enum.
public static bool GetJointTransformation(uint userID, SkeletonJoint joint, ref SkeletonJointTransformation pTransformation)
{
return GetJointTransformation(userID, (int)joint, ref pTransformation);
}
// Overload to instead use the skeleton joint enum.
public static bool GetJointTransformation(uint userID, SkeletonJoint joint, ref SkeletonJointTransformation pTransformation)
{
return(GetJointTransformation(userID, (int)joint, ref pTransformation));
}
public static extern bool GetJointTransformation(uint userID, SkeletonJoint joint, ref SkeletonJointTransformation pTransformation);
public static extern bool GetJointTransformation(uint userID, SkeletonJoint joint, ref SkeletonJointTransformation pTransformation);
