public List <Vector3> GetConfidentPositions(UJoint[] joints)
{
List <Vector3> positions = new List <Vector3>();
for (int i = 0; i < joints.Length; i++)
{
UJoint j = joints[i];
if (j.Confidence == JointConfidenceLevel.Low || j.Confidence == JointConfidenceLevel.None)
{
continue;
}
positions.Add(j.Position);
}
return(positions);
}
/// <summary>
/// Get all joints with their original position and rotation
/// </summary>
/// <returns></returns>
public UJoint[] ResolveSkeleton()
{
//Debug.Log("Resolving Skeleton");
//get all joint positions
UJoint[] joints = new UJoint[27];
for (int i = 0; i < 27; i++)
{
Joint j = trackedBody.GetJoint(i);
UJoint unityJoint = new UJoint(j);
joints[i] = unityJoint;
}
if (AppManager.recording)
{
MotionManager.Instance.StoreFrame(joints);
}
return(joints);
}
/// <summary>
/// Determine how much alike two poses are to one another. Return the result in percent
/// </summary>
/// <param name="lhs"></param>
/// <param name="rhs"></param>
/// <returns></returns>
public int ComparePoses(UJoint[] lhs, UJoint[] rhs)
{
if (lhs == null || rhs == null)
{
return(-1);
}
//JointId[] jointConstraints = AppManager.jointConstraints;
Vector3 diffSum = Vector3.zero;
Vector3 diffMirrorSum = Vector3.zero;
Vector3 originalPelvisPosition = lhs[(int)Pelvis].Position;
Vector3 cPelvisPos = rhs[(int)Pelvis].Position;
Vector3 mirrorComparePelvisPosition = new Vector3(cPelvisPos.x * -1f, cPelvisPos.y, cPelvisPos.z);
//handle unequal length arrays
if (lhs.Length != rhs.Length)
{
Debug.Log("poses had different amount of joints");
return(-1);
}
int comparedJointsCount = 0;
for (int i = 1; i < lhs.Length; i++)
{
UJoint pI = lhs[i];
//don't compare obstructed or out of range joints
if (pI.Confidence == JointConfidenceLevel.Low || pI.Confidence == JointConfidenceLevel.None)
{
continue;
}
/*
* bool jointConstraint = false;
* foreach(var j in jointConstraints)
* {
* if (i == (int)j)
* {
* jointConstraint = true;
* break;
* }
* }
* if (jointConstraint)
* continue;
*/
//joint positions
Vector3 oPos = lhs[i].Position;
Vector3 cPos = rhs[i].Position;
Vector3 cPosMirror = new Vector3(cPos.x * -1, cPos.y, cPos.z);
//joint positions relative to pelvis
Vector3 relOPos = oPos - originalPelvisPosition;
Vector3 relCPos = cPos - cPelvisPos;
Vector3 relCPosMirror = cPosMirror - mirrorComparePelvisPosition;
Vector3 diff = relCPos - relOPos;
Vector3 diffMirror = relCPosMirror - relOPos;
diffSum += diff.Square();
diffMirrorSum += diffMirror.Square();
comparedJointsCount++;
}
if (comparedJointsCount == 0)
{
Debug.LogWarning("All joints had JointConfidenceLevel.Low or .None");
}
//average positional difference in mm
Vector3 diffAverage = diffSum /= comparedJointsCount;
Vector3 diffMirrorAverage = diffMirrorSum /= comparedJointsCount;
float diffDistance = Mathf.Sqrt(diffAverage.x * diffAverage.x + diffAverage.y * diffAverage.y + diffAverage.z * diffAverage.z);
float diffMirrorDistance = Mathf.Sqrt(diffMirrorAverage.x * diffMirrorAverage.x + diffMirrorAverage.y * diffMirrorAverage.y + diffMirrorAverage.z * diffMirrorAverage.z);
if (diffDistance == float.NaN || diffMirrorDistance == float.NaN)
{
return(0);
}
//Debug.Log("DiffDistance: " + diffDistance + ", DiffDistanceMirrored: " + diffMirrorDistance);
if (diffMirrorDistance < diffDistance)
{
diffDistance = diffMirrorDistance;
}
int percent;
if (diffDistance > ZERO_PERCENT)
{
percent = 0;
}
else if (diffDistance < HUNDRED_PERCENT)
{
percent = 100;
}
else
{
percent = 100 - Mathf.RoundToInt((diffDistance - HUNDRED_PERCENT) / (ZERO_PERCENT * 0.01f));
}
//Debug.Log("Positional difference in mm: " + posDifferenceD);
return(percent);
}
