public JointReference(UserData.SkeletonData.Joint joint, float tolerance = 0.9f)
{
Quaternion rotation = NuitrackManager.DepthSensor.IsMirror() ? MirrorSensorPlane(joint.Rotation) : joint.RotationMirrored;
orientation = rotation;
this.tolerance = tolerance;
}
/// <summary>
/// Absolute match of the joint orientation to the sample
/// </summary>
/// <param name="joint">Source Joint</param>
/// <returns>
/// 0 - joint orientation does not match the sample,
/// 1 - joint orientation match to the sample with specified tolerance
/// </returns>
public float Match(UserData.SkeletonData.Joint joint)
{
if (isActive && NuitrackManager.DepthSensor != null)
{
Quaternion rotation = NuitrackManager.DepthSensor.IsMirror() ? MirrorSensorPlane(joint.Rotation) : joint.RotationMirrored;
float dot = Vector3.Dot(orientation * joint.NuitrackType.GetNormalDirection(), rotation * joint.NuitrackType.GetNormalDirection());
return(Mathf.Clamp01(dot / tolerance));
}
else
{
return(0);
}
}
void Update()
{
UserData user = ControllerUser;
if (user == null || user.Skeleton == null)
{
return;
}
if (!initialized)
{
Init();
}
foreach (KeyValuePair <nuitrack.JointType, RectTransform> jointsInfo in joints)
{
nuitrack.JointType jointType = jointsInfo.Key;
RectTransform rectTransform = jointsInfo.Value;
UserData.SkeletonData.Joint j = user.Skeleton.GetJoint(jointType);
if (j.Confidence > JointConfidence)
{
rectTransform.gameObject.SetActive(true);
rectTransform.anchoredPosition = j.AnchoredPoint(parentRect.rect, rectTransform);
}
else
{
rectTransform.gameObject.SetActive(false);
}
if (jointType.GetParent() != nuitrack.JointType.None)
{
RectTransform endJoint = joints[jointType.GetParent()];
if (rectTransform.gameObject.activeSelf && endJoint.gameObject.activeSelf)
{
connections[jointType].gameObject.SetActive(true);
connections[jointType].anchoredPosition = rectTransform.anchoredPosition;
connections[jointType].transform.right = endJoint.position - rectTransform.position;
float distance = Vector3.Distance(endJoint.anchoredPosition, rectTransform.anchoredPosition);
connections[jointType].transform.localScale = new Vector3(distance, 1f, 1f);
}
else
{
connections[jointType].gameObject.SetActive(false);
}
}
}
}
/// <summary>
/// Relative match of the joint orientation to the sample
/// </summary>
/// <param name="joint">Source Joint</param>
/// <param name="skeletonInverseRotation"></param>
/// <param name="poseInverseRotation"></param>
/// <returns>
/// 0 - joint orientation does not match the sample,
/// 1 - joint orientation match to the sample with specified tolerance
/// </returns>
public float Match(UserData.SkeletonData.Joint joint, Quaternion skeletonInverseRotation, Quaternion poseInverseRotation)
{
if (isActive)
{
Quaternion rotation = NuitrackManager.DepthSensor.IsMirror() ? MirrorSensorPlane(joint.Rotation) : joint.RotationMirrored;
Quaternion relativeSkeletonJointRotation = poseInverseRotation * orientation;
Quaternion relativeReferenceJointRotation = skeletonInverseRotation * rotation;
float dot = Vector3.Dot(relativeSkeletonJointRotation * joint.NuitrackType.GetNormalDirection(), relativeReferenceJointRotation * joint.NuitrackType.GetNormalDirection());
return(Mathf.Clamp01(dot / tolerance));
}
else
{
return(0);
}
}
/// <summary>
/// Getting skeleton data from thr sensor and updating transforms of the model bones
/// </summary>
void Process(UserData user)
{
if (!alignmentBoneLength)
{
jointsRigged[rootJoint].bone.position = GetJointLocalPos(GetJoint(rootJoint).Position);
}
foreach (var riggedJoint in jointsRigged)
{
//Get joint from the Nuitrack
//nuitrack.Joint joint = skeleton.GetJoint(riggedJoint.Key);
UserData.SkeletonData.Joint jointTransform = user.Skeleton.GetJoint(riggedJoint.Key);
if (jointTransform.Confidence > JointConfidence)
{
//Get modelJoint
ModelJoint modelJoint = riggedJoint.Value;
//Bone rotation
Quaternion jointRotation = IsTransformSpace ? jointTransform.RotationMirrored : jointTransform.Rotation;
modelJoint.bone.rotation = SpaceTransform.rotation * (jointRotation * modelJoint.baseRotOffset);
if (alignmentBoneLength)
{
Vector3 newPos = GetJointLocalPos(jointTransform.Position);
modelJoint.bone.position = newPos;
//Bone scale
if (modelJoint.parentBone != null && modelJoint.jointType.GetParent() != rootJoint)
{
//Take the Transform of a parent bone
Transform parentBone = modelJoint.parentBone;
//calculate how many times the distance between the child bone and its parent bone has changed compared to the base distance (which was recorded at the start)
float scaleDif = modelJoint.baseDistanceToParent / Vector3.Distance(newPos, parentBone.position);
//change the size of the bone to the resulting value (On default bone size (1,1,1))
parentBone.localScale = Vector3.one / scaleDif;
//compensation for size due to hierarchy
parentBone.localScale *= parentBone.localScale.x / parentBone.lossyScale.x;
}
}
}
}
}
void Update()
{
UserData user = ControllerUser;
if (user == null || user.Skeleton == null)
{
return;
}
if (headTransform != null)
{
#if UNITY_IOS
headTransform.position = headDirectionTransform.rotation * neckHMDOffset + (rotate180 ? q180 : q0) * (Vector3.up * CalibrationInfo.FloorHeight + CalibrationInfo.SensorOrientation * GetJoint(JointType.Neck).Position) + basePivotOffset;
#else
headTransform.position = (rotate180 ? q180 : q0) * (Vector3.up * CalibrationInfo.FloorHeight + CalibrationInfo.SensorOrientation * GetJoint(JointType.Head).Position) + basePivotOffset;
#endif
basePivot = (rotate180 ? q180 : q0) * (Vector3.up * CalibrationInfo.FloorHeight + CalibrationInfo.SensorOrientation * GetJoint(JointType.Waist).Position) + basePivotOffset;
}
if (!skeletonRoot.activeSelf)
{
skeletonRoot.SetActive(true);
}
for (int i = 0; i < jointsInfo.Length; i++)
{
UserData.SkeletonData.Joint j = user.Skeleton.GetJoint(jointsInfo[i]);
if (j.Confidence > JointConfidence)
{
if (!joints[jointsInfo[i]].activeSelf)
{
joints[jointsInfo[i]].SetActive(true);
}
joints[jointsInfo[i]].transform.position = (rotate180 ? q180 : q0) * (Vector3.up * CalibrationInfo.FloorHeight + CalibrationInfo.SensorOrientation * j.Position) + basePivotOffset;
joints[jointsInfo[i]].transform.rotation = (rotate180 ? q180 : q0) * CalibrationInfo.SensorOrientation * j.RotationMirrored;
leftHandPos = (rotate180 ? q180 : q0) * (Vector3.up * CalibrationInfo.FloorHeight + CalibrationInfo.SensorOrientation * GetJoint(JointType.LeftHand).Position) + basePivotOffset;
rightHandPos = (rotate180 ? q180 : q0) * (Vector3.up * CalibrationInfo.FloorHeight + CalibrationInfo.SensorOrientation * GetJoint(JointType.RightHand).Position) + basePivotOffset;
}
else
{
if (joints[jointsInfo[i]].activeSelf)
{
joints[jointsInfo[i]].SetActive(false);
}
}
}
for (int i = 0; i < jointsInfo.Length; i++)
{
JointType jointType = jointsInfo[i];
JointType parentType = jointsInfo[i].GetParent();
if (parentType != JointType.None)
{
if (joints[jointType].activeSelf && joints[parentType].activeSelf)
{
if (!connections[i].activeSelf)
{
connections[i].SetActive(true);
}
Vector3 diff = joints[parentType].transform.position - joints[jointType].transform.position;
connections[i].transform.position = joints[jointType].transform.position;
connections[i].transform.rotation = Quaternion.LookRotation(diff);
connections[i].transform.localScale = new Vector3(1f, 1f, diff.magnitude);
}
else
{
if (connections[i].activeSelf)
{
connections[i].SetActive(false);
}
}
}
}
}
void DrawFramePreview()
{
float pointScale = 0.025f;
float lineScale = 0.01f;
ShowPreview = EditorGUILayout.BeginFoldoutHeaderGroup(ShowPreview, "Frame viewer");
if (ShowPreview)
{
if (!EditorApplication.isPlaying)
{
NuitrackSDKGUI.DrawMessage("RGB and depth frames will be displayed run time.", LogType.Log);
}
else
{
if (rgbCache == null)
{
rgbCache = new TextureCache();
}
if (depthCache == null)
{
depthCache = new TextureCache();
}
List <Vector2> pointCoord = new List <Vector2>();
rgbTexture = NuitrackManager.ColorFrame.ToRenderTexture(rgbCache);
depthTexture = NuitrackManager.DepthFrame.ToRenderTexture(textureCache: depthCache);
Rect rgbRect = NuitrackSDKGUI.DrawFrame(rgbTexture, "RGB frame");
Texture pointTexture = EditorGUIUtility.IconContent("sv_icon_dot0_pix16_gizmo").image;
float lineSize = rgbRect.size.magnitude * lineScale;
foreach (UserData user in NuitrackManager.Users)
{
if (user.Skeleton != null)
{
Color userColor = FrameUtils.SegmentToTexture.GetColorByID(user.ID);
foreach (nuitrack.JointType jointType in System.Enum.GetValues(typeof(nuitrack.JointType)))
{
nuitrack.JointType parentJointType = jointType.GetParent();
UserData.SkeletonData.Joint joint = user.Skeleton.GetJoint(jointType);
if (joint.Confidence > 0.1f)
{
Vector2 startPoint = new Vector2(rgbRect.x + rgbRect.width * joint.Proj.x, rgbRect.y + rgbRect.height * (1 - joint.Proj.y));
pointCoord.Add(startPoint);
if (jointType.GetParent() != nuitrack.JointType.None)
{
UserData.SkeletonData.Joint parentJoint = user.Skeleton.GetJoint(parentJointType);
if (parentJoint.Confidence > 0.1f)
{
Vector2 endPoint = new Vector2(rgbRect.x + rgbRect.width * parentJoint.Proj.x, rgbRect.y + rgbRect.height * (1 - parentJoint.Proj.y));
Handles.DrawBezier(startPoint, endPoint, startPoint, endPoint, userColor, null, lineSize);
}
}
}
}
float pointSize = rgbRect.size.magnitude * pointScale;
foreach (Vector3 point in pointCoord)
{
Rect rect = new Rect(point.x - pointSize / 2, point.y - pointSize / 2, pointSize, pointSize);
GUI.DrawTexture(rect, pointTexture, ScaleMode.ScaleToFit);
}
}
}
NuitrackSDKGUI.DrawFrame(depthTexture, "Depth frame");
Repaint();
}
}
EditorGUILayout.EndFoldoutHeaderGroup();
}
void ProcessSkeletons()
{
if (NuitrackManager.Users.Count == 0)
{
HideAllSkeletons();
return;
}
//Debug.Log("NumUsers: " + skeletonData.NumUsers.ToString());
int[] skelIds = new int[skeletonsRoots.Keys.Count];
skeletonsRoots.Keys.CopyTo(skelIds, 0);
for (int i = 0; i < skelIds.Length; i++)
{
UserData user = NuitrackManager.Users.GetUser(skelIds[i]);
if (user == null || user.Skeleton == null)
{
skeletonsRoots[skelIds[i]].SetActive(false);
}
}
foreach (UserData user in NuitrackManager.Users)
{
if (user.Skeleton == null)
{
continue;
}
if (!skeletonsRoots.ContainsKey(user.ID)) // if don't have gameObjects for skeleton ID, create skeleton gameobjects (root, joints and connections)
{
GameObject skelRoot = new GameObject();
skelRoot.name = "Root_" + user.ID.ToString();
skeletonsRoots.Add(user.ID, skelRoot);
Dictionary <nuitrack.JointType, GameObject> skelJoints = new Dictionary <nuitrack.JointType, GameObject>();
for (int i = 0; i < jointsInfo.Length; i++)
{
GameObject joint = (GameObject)Instantiate(jointPrefab, Vector3.zero, Quaternion.identity);
skelJoints.Add(jointsInfo[i], joint);
joint.transform.parent = skelRoot.transform;
joint.SetActive(false);
}
joints.Add(user.ID, skelJoints);
GameObject[] skelConnections = new GameObject[connectionsInfo.GetLength(0)];
for (int i = 0; i < skelConnections.Length; i++)
{
GameObject conn = (GameObject)Instantiate(connectionPrefab, Vector3.zero, Quaternion.identity);
skelConnections[i] = conn;
conn.transform.parent = skelRoot.transform;
conn.SetActive(false);
}
connections.Add(user.ID, skelConnections);
}
if (!skeletonsRoots[user.ID].activeSelf)
{
skeletonsRoots[user.ID].SetActive(true);
}
for (int i = 0; i < jointsInfo.Length; i++)
{
UserData.SkeletonData.Joint j = user.Skeleton.GetJoint(jointsInfo[i]);
if (j.Confidence > 0.01f)
{
if (!joints[user.ID][jointsInfo[i]].activeSelf)
{
joints[user.ID][jointsInfo[i]].SetActive(true);
}
joints[user.ID][jointsInfo[i]].transform.position = j.Position;
//skel.Joints[i].Orient.Matrix:
// 0, 1, 2,
// 3, 4, 5,
// 6, 7, 8
// -------
// right(X), up(Y), forward(Z)
joints[user.ID][jointsInfo[i]].transform.rotation = j.Rotation;
}
else
{
if (joints[user.ID][jointsInfo[i]].activeSelf)
{
joints[user.ID][jointsInfo[i]].SetActive(false);
}
}
}
for (int i = 0; i < connectionsInfo.GetLength(0); i++)
{
if (joints[user.ID][connectionsInfo[i, 0]].activeSelf && joints[user.ID][connectionsInfo[i, 1]].activeSelf)
{
if (!connections[user.ID][i].activeSelf)
{
connections[user.ID][i].SetActive(true);
}
Vector3 diff = joints[user.ID][connectionsInfo[i, 1]].transform.position - joints[user.ID][connectionsInfo[i, 0]].transform.position;
connections[user.ID][i].transform.position = joints[user.ID][connectionsInfo[i, 0]].transform.position;
connections[user.ID][i].transform.rotation = Quaternion.LookRotation(diff);
connections[user.ID][i].transform.localScale = new Vector3(1f, 1f, diff.magnitude);
}
else
{
if (connections[user.ID][i].activeSelf)
{
connections[user.ID][i].SetActive(false);
}
}
}
}
}
