/// this method receives a new point (HandPointContext) and updates the structures.
/// @param newPoint the new point
/// @param confidence the confidence of the point.
public void UpdatePoint(Point3D newPoint, float confidence)
{
if (m_lastFrame < 0)
{
// this is an initialization
if (confidence < 0.5f)
{
return; // bad point, nothing to do.
}
m_lastFrame = Time.frameCount;
m_latestGoodPositionInFrame = NIConvertCoordinates.ConvertPos(newPoint);
m_oldFrameLastGoodPosition = m_latestGoodPositionInFrame;
return;
}
if (Time.frameCount != m_lastFrame)
{
// this is a new frame.
m_oldFrameLastGoodPosition = m_latestGoodPositionInFrame;
m_lastFrame = Time.frameCount;
}
if (confidence >= 0.5f)
{
m_latestGoodPositionInFrame = NIConvertCoordinates.ConvertPos(newPoint);
}
}
/// mono-behavior update (called once per frame)
public void Update()
{
if (m_playerManager == null || m_playerManager.Valid == false)
{
return; // we can do nothing.
}
NISelectedPlayer player = m_playerManager.GetPlayer(m_playerNumber);
if (player == null || player.Valid == false || player.Tracking == false)
{
RotateToCalibrationPose(); // we don't have anything to work with.
return;
}
Vector3 skelPos = Vector3.zero;
SkeletonJointTransformation skelTrans;
if (player.GetReferenceSkeletonJointTransform(SkeletonJoint.Torso, out skelTrans))
{
if (skelTrans.Position.Confidence < 0.5f)
{
player.RecalcReferenceJoints(); // we NEED the torso to be good.
}
if (skelTrans.Position.Confidence >= 0.5f)
{
skelPos = NIConvertCoordinates.ConvertPos(skelTrans.Position.Position);
}
}
UpdateSkeleton(player, skelPos);
}
/// @brief Gets the starting position if @ref m_startingPoseReferenceType is TrackedJointReference
///
/// @return The reference position of the tracked joint (zero if there is a problem).
protected Vector3 GetStartingPosFromTrackedJoint()
{
// The position we count from is the initial calibration pose.
NISelectedPlayer player = m_playerManager.GetPlayer(m_playerNum);
if (player == null || player.Valid == false)
{
return(Vector3.zero); // no position.
}
SkeletonJointTransformation skelTrans;
if (player.GetReferenceSkeletonJointTransform(m_jointToUse, out skelTrans) == false)
{
return(Vector3.zero); // we don't have the transform
}
Point3D pos = skelTrans.Position.Position;
Vector3 res = NIConvertCoordinates.ConvertPos(pos);
if (skelTrans.Position.Confidence < 0.5f || float.IsNaN(res.magnitude) || res.z >= 0)
{
// this means the value is irrelevant.
player.RecalcReferenceJoints();
SkeletonJointPosition skelJointPos;
if (player.GetSkeletonJointPosition(m_jointToUse, out skelJointPos) == false)
{
return(Vector3.zero);
}
res = NIConvertCoordinates.ConvertPos(skelJointPos.Position);
}
return(res);
}
/// returns the current position with confidence
///
/// @param confidence the confidence of the point
/// @return the position
public Vector3 GetPosWithConfidence(out float confidence)
{
Point3D where = GetSkelPoint(out confidence);
Vector3 res = NIConvertCoordinates.ConvertPos(where);
return(res);
}
/// mono-behavior update (called once per frame)
public void Update()
{
if (m_mapper.Valid == false)
{
return; // we can do nothing.
}
if (m_skeletonsControllers == null)
{
return; // we don't have any skeletons controllers to update
}
for (int i = 0; i < m_skeletonsControllers.Length; i++)
{
int userID = m_mapper.GetPlayerOpenNIID(i);
if (userID < 0)
{
m_skeletonsControllers[i].RotateToCalibrationPose(); // we don't have anything to work with.
continue;
}
Vector3 skelPos = Vector3.zero;
SkeletonJointTransformation skelTrans;
if (m_mapper.GetJointInitialTransform(i, SkeletonJoint.Torso, out skelTrans))
{
if (skelTrans.Position.Confidence >= 0.5f)
{
skelPos = NIConvertCoordinates.ConvertPos(skelTrans.Position.Position);
}
}
m_skeletonsControllers[i].UpdateSkeleton(userID, m_mapper.UsersGenerator, skelPos);
}
}
/// @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);
}
// protected methods
/// this method tries to fill a new point on each of the relevant joints.
/// It returns true if it succeed and false otherwise
/// @note it will fail if even one of the points has a low confidence!
/// @return true on success, false on failure.
protected bool FillPoints()
{
// first we find a reference to the skeleton capability
NISkeletonTracker hand = m_pointTracker as NISkeletonTracker;
if (hand == null)
{
return(false); // no hand to track
}
NISelectedPlayer player = hand.GetTrackedPlayer();
if (player == null || player.Valid == false || player.Tracking == false)
{
return(false); // no player to work with...
}
// We need to figure out if we have a good confidence on all joints
SkeletonJointPosition rightHand;
SkeletonJointPosition leftHand;
SkeletonJointPosition rightElbow;
SkeletonJointPosition leftElbow;
if (player.GetSkeletonJointPosition(SkeletonJoint.RightHand, out rightHand) == false || rightHand.Confidence <= 0.5f)
{
return(false);
}
if (player.GetSkeletonJointPosition(SkeletonJoint.LeftHand, out leftHand) == false || leftHand.Confidence <= 0.5f)
{
return(false);
}
if (player.GetSkeletonJointPosition(SkeletonJoint.RightElbow, out rightElbow) == false || rightElbow.Confidence <= 0.5f)
{
return(false);
}
if (player.GetSkeletonJointPosition(SkeletonJoint.LeftElbow, out leftElbow) == false || leftElbow.Confidence <= 0.5f)
{
return(false);
}
Vector3 pos = NIConvertCoordinates.ConvertPos(rightHand.Position);
m_pointsRightHand.AddPoint(ref pos);
pos = NIConvertCoordinates.ConvertPos(leftHand.Position);
m_pointsLeftHand.AddPoint(ref pos);
pos = NIConvertCoordinates.ConvertPos(rightElbow.Position);
m_pointsRightElbow.AddPoint(ref pos);
pos = NIConvertCoordinates.ConvertPos(leftElbow.Position);
m_pointsLeftElbow.AddPoint(ref pos);
return(true);
}
/// @brief updates the root position
///
/// This method updates the root position and if m_updateRootPosition is true, also move the entire transform
/// @note we do not update if we do not have a high enough confidence!
/// @param skelRoot the new central position
/// @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
protected void UpdateRoot(SkeletonJointPosition skelRoot, Vector3 centerOffset)
{
if (skelRoot.Confidence < 0.5f)
{
return; // we are not confident enough!
}
m_rootPosition = NIConvertCoordinates.ConvertPos(skelRoot.Position);
m_rootPosition -= centerOffset;
m_rootPosition *= m_scale;
m_rootPosition += m_originalRootPosition;
if (m_updateRootPosition)
{
transform.position = transform.rotation * m_rootPosition;
}
}
/// This gets the center of mass for a specific user
/// @param niUserID the NI id of a user
/// @return position of the center of mass of the user (undefined if none was found).
public Vector3 GetUserCenterOfMass(int niUserID)
{
try
{
Point3D com = m_userGenerator.GetCoM(niUserID);
if (com.Z <= 0)
{
return(Vector3.forward); // this means a positive 'z' which is illegal!
}
return(NIConvertCoordinates.ConvertPos(com));
}
catch
{
return(Vector3.forward);
}
}
/// returns the current position with confidence
///
/// @param confidence the confidence of the point
/// @return the position
public Vector3 GetPosWithConfidence(out float confidence)
{
confidence = 0.0f;
NISelectedPlayer player = m_playerManager.GetPlayer(m_playerNum);
if (player == null || player.Valid == false || player.Tracking == false)
{
return(Vector3.zero); // no position.
}
SkeletonJointPosition skelJointPos;
if (player.GetSkeletonJointPosition(m_jointToUse, out skelJointPos) == false)
{
return(Vector3.zero);
}
confidence = skelJointPos.Confidence;
return(NIConvertCoordinates.ConvertPos(skelJointPos.Position));
}
/// @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));
}
// protected methods
/// this method tries to fill a new point on each of the relevant joints.
/// It returns true if it succeed and false otherwise
/// @note it will fail if even one of the points has a low confidence!
/// @return true on success, false on failure.
protected bool FillPoints()
{
// first we find a reference to the skeleton capability
NISkeletonTracker hand = m_pointTracker as NISkeletonTracker;
if (hand == null)
{
return(false); // no hand to track
}
int userID;
SkeletonCapability capability = hand.GetSkeletonCapability(out userID);
if (capability == null)
{
return(false); // no skeleton capability
}
// now that we have a legal capability lets make sure all relevant joints are supported
if (capability.IsJointActive(SkeletonJoint.RightHand) == false)
{
return(false); // joint not supported!
}
if (capability.IsJointActive(SkeletonJoint.LeftHand) == false)
{
return(false); // joint not supported!
}
if (capability.IsJointActive(SkeletonJoint.RightElbow) == false)
{
return(false); // joint not supported!
}
if (capability.IsJointActive(SkeletonJoint.LeftElbow) == false)
{
return(false); // joint not supported!
}
// since now we know all relevant joints are supported we need to figure out if we have
// a good confidence for
SkeletonJointPosition rightHand = capability.GetSkeletonJointPosition(userID, SkeletonJoint.RightHand);
if (rightHand.Confidence <= 0.5f)
{
return(false); // we have low confidence so lets ignore this set
}
SkeletonJointPosition leftHand = capability.GetSkeletonJointPosition(userID, SkeletonJoint.LeftHand);
if (leftHand.Confidence <= 0.5f)
{
return(false); // we have low confidence so lets ignore this set
}
SkeletonJointPosition rightElbow = capability.GetSkeletonJointPosition(userID, SkeletonJoint.RightElbow);
if (rightElbow.Confidence <= 0.5f)
{
return(false); // we have low confidence so lets ignore this set
}
SkeletonJointPosition leftElbow = capability.GetSkeletonJointPosition(userID, SkeletonJoint.LeftElbow);
if (leftElbow.Confidence <= 0.5f)
{
return(false); // we have low confidence so lets ignore this set
}
Vector3 pos = NIConvertCoordinates.ConvertPos(rightHand.Position);
m_pointsRightHand.AddPoint(ref pos);
pos = NIConvertCoordinates.ConvertPos(leftHand.Position);
m_pointsLeftHand.AddPoint(ref pos);
pos = NIConvertCoordinates.ConvertPos(rightElbow.Position);
m_pointsRightElbow.AddPoint(ref pos);
pos = NIConvertCoordinates.ConvertPos(leftElbow.Position);
m_pointsLeftElbow.AddPoint(ref pos);
return(true);
}
/// This is the mono-behavior Update.<br>
/// This method checks if the floor normal is already calculated. If not, it checks if
/// we have a calibrated user. It then goes over the various calibrated users and attempts
/// to find one which has legal head and torso positions. Once it finds one it creates the normal
/// from them.<br>
/// @note The assumption is that immediately after calibration, the calibrated user stands erect
/// and that the floor's is leveled. In addition, we assume that even if the joint's confidence is
/// low, as long as the value is legal (i.e. non zero and is a number) then it is a legal position
/// (as was defined in the calibration).
public void Update()
{
if (NIConvertCoordinates.NormalUpdated)
{
return; // nothing to do here, already got a normal.
}
if (m_manager.m_useSkeleton == false || m_manager.UserSkeletonValid == false)
{
return; // we don't have a valid user generator to work with
}
// get the list of users
IList <int> userList = m_manager.UserGenrator.Users;
if (userList == null || userList.Count <= 0)
{
return; // no users
}
// for each user we try to find if it is calibrated and has valid data
foreach (int uniqueID in userList)
{
if (uniqueID < 0)
{
continue; // an invalid user
}
int userID = m_manager.UserGenrator.GetNIUserId(uniqueID);
if (userID < 0)
{
continue; // an invalid user
}
if (m_manager.UserGenrator.GetUserCalibrationState(userID) != NIUserAndSkeleton.CalibrationState.calibrated)
{
continue; // we only initialize based on calibrated users as we need the skeleton.
}
if (m_manager.UserGenrator.Skeleton == null)
{
continue; // no skeleton
}
if (m_manager.UserGenrator.Skeleton.IsJointAvailable(SkeletonJoint.Head) == false ||
m_manager.UserGenrator.Skeleton.IsJointAvailable(SkeletonJoint.Torso) == false)
{
return; // we don't have the relevant joints.
}
SkeletonJointTransformation skelTransform;
skelTransform = m_manager.UserGenrator.Skeleton.GetSkeletonJoint(userID, SkeletonJoint.Head);
Vector3 headPos = NIConvertCoordinates.ConvertPos(skelTransform.Position.Position);
float mag = headPos.sqrMagnitude;
if (float.IsNaN(mag) || float.IsInfinity(mag))
{
continue; // not a good number
}
if (headPos.z >= 0)
{
continue; // this is not a good value
}
skelTransform = m_manager.UserGenrator.Skeleton.GetSkeletonJoint(userID, SkeletonJoint.Torso);
Vector3 torsoPos = NIConvertCoordinates.ConvertPos(skelTransform.Position.Position);
mag = torsoPos.sqrMagnitude;
if (float.IsNaN(mag) || float.IsInfinity(mag))
{
continue; // not a good number
}
if (torsoPos.z >= 0)
{
continue; // this is not a good value
}
// if we are here we have two good values.
NIConvertCoordinates.UpdateFloorNormal(headPos - torsoPos, false);
m_manager.Log("Updated floor normal", NIEventLogger.Categories.Initialization, NIEventLogger.Sources.Skeleton);
}
}