void ProcessCalibration(UserData.SkeletonData skeleton)
{
float poseMath = callibrationPose.Match(skeleton);
if (!Mathf.Approximately((float)System.Math.Round(poseMath, 3), 1))
{
calibrationStarted = false;
}
}
void StartCalibration(UserData.SkeletonData skeleton)
{
float poseMath = callibrationPose.Match(skeleton);
if (Mathf.Approximately((float)System.Math.Round(poseMath, 3), 1))
{
calibrationStarted = true;
onStart?.Invoke();
}
}
/// <summary>
/// Add a new pose
/// </summary>
/// <param name="skeleton">Source skeleton</param>
/// <param name="name">Name of the new pose</param>
/// <returns>ID of the added pose</returns>
public NuitrackPose AddPose(UserData.SkeletonData skeleton, string name = null)
{
name ??= string.Format("Pose {0}", poseEvents.Count + 1);
NuitrackPose pose = new NuitrackPose(name, skeleton);
poseEvents.Add(pose, new PoseProcessEvent());
Matches.Add(pose, new Dictionary <int, float>());
return(pose);
}
void CheckSkeletonPositions(UserData.SkeletonData skeleton)
{
List <UserData.SkeletonData.Joint> joints = new List <UserData.SkeletonData.Joint>(10);
joints.Add(skeleton.GetJoint(nuitrack.JointType.Head));
joints.Add(skeleton.GetJoint(nuitrack.JointType.Torso));
joints.Add(skeleton.GetJoint(nuitrack.JointType.LeftElbow));
joints.Add(skeleton.GetJoint(nuitrack.JointType.LeftWrist));
joints.Add(skeleton.GetJoint(nuitrack.JointType.RightElbow));
joints.Add(skeleton.GetJoint(nuitrack.JointType.RightWrist));
joints.Add(skeleton.GetJoint(nuitrack.JointType.LeftKnee));
joints.Add(skeleton.GetJoint(nuitrack.JointType.RightKnee));
joints.Add(skeleton.GetJoint(nuitrack.JointType.LeftAnkle));
joints.Add(skeleton.GetJoint(nuitrack.JointType.RightAnkle));
float minZ = float.MaxValue;
float proximityLeft = 0, proximityRight = 0;
foreach (UserData.SkeletonData.Joint joint in joints)
{
float posX = joint.Position.x;
float posZ = joint.Position.z;
float angle = fov / 2;
float sideDistance = posZ / Mathf.Cos(angle * Mathf.Deg2Rad);
float frontWidth = Mathf.Sqrt(-(posZ * posZ) + sideDistance * sideDistance);
float distToSide = posX / frontWidth;
if (proximityLeft < distToSide)
{
proximityLeft = distToSide;
}
if (proximityRight > distToSide)
{
proximityRight = distToSide;
}
if (minZ > posZ)
{
minZ = posZ;
}
}
ChangeAlpha(frontGrid, 1.0f + (warningDistance - minZ) / (warningDistance * sensitivity));
ChangeAlpha(leftGrid, 1.0f - (1.0f - proximityLeft) / sensitivity);
ChangeAlpha(rightGrid, 1.0f - (1.0f + proximityRight) / sensitivity);
}
Quaternion GetHeadAngles(UserData.SkeletonData skeleton)
{
Vector3 deltaWrist = skeleton.GetJoint(nuitrack.JointType.LeftWrist).Position - skeleton.GetJoint(nuitrack.JointType.RightWrist).Position;
float angleY = -Mathf.Rad2Deg * Mathf.Atan2(deltaWrist.z, deltaWrist.x);
float angleX = -Mathf.Rad2Deg * Mathf.Atan2(Input.gyro.gravity.z, -Input.gyro.gravity.y);
Vector3 torso = NuitrackManager.Users.Current.Skeleton.GetJoint(nuitrack.JointType.Torso).Position;
Vector3 neck = NuitrackManager.Users.Current.Skeleton.GetJoint(nuitrack.JointType.Neck).Position;
Vector3 diff = neck - torso;
sensorOrientation = Quaternion.Euler(Mathf.Atan2(diff.z, diff.y) * Mathf.Rad2Deg, 0f, 0f);
//Debug.Log("Gravity vector: " + Input.gyro.gravity.ToString("0.000") + "; AngleX: " + angleX.ToString("0") + "; AngleY: " + angleY.ToString("0"));
return(Quaternion.Euler(angleX, angleY, 0f));
}
void Update()
{
if (cooldown > 0f)
{
cooldown -= Time.unscaledDeltaTime;
}
else
{
if (NuitrackManager.Users.Current != null && NuitrackManager.Users.Current.Skeleton != null)
{
UserData.SkeletonData skeleton = NuitrackManager.Users.Current.Skeleton;
if (!calibrationStarted)
{
StartCalibration(skeleton);
}
else
{
if (timer > calibrationTime)
{
calibrationStarted = false;
timer = 0f;
cooldown = calibrationTime;
onSuccess?.Invoke(GetHeadAngles(skeleton));
}
else
{
ProcessCalibration(skeleton);
if (!calibrationStarted)
{
timer = 0f;
onFail?.Invoke();
}
else
{
onProgress?.Invoke(timer / calibrationTime);
timer += Time.unscaledDeltaTime;
}
}
}
}
}
}
void SaveRuntimePose(UserData.SkeletonData skeleton)
{
string name = "Runtime pose";
NuitrackPose newPose = new NuitrackPose(name, skeleton);
string saveFolder = SaveFolder.Replace(Application.dataPath, "");
string[] separatePath = saveFolder.Split(new char[] { '\\', '/' }, System.StringSplitOptions.RemoveEmptyEntries);
NuitrackPose poseAsset = NuitrackEditorHelper.CreateAsset <NuitrackPose>(name, separatePath);
NuitrackPoseWrapper nuitrackPoseWrapper = new NuitrackPoseWrapper(new SerializedObject(poseAsset));
nuitrackPoseWrapper.CopyFrom(newPose);
Destroy(newPose);
EditorApplication.isPlaying = false;
Selection.activeObject = poseAsset;
}
void Update()
{
if (attachToSkelJoint && NuitrackManager.Users.Current != null && NuitrackManager.Users.Current.Skeleton != null)
{
UserData.SkeletonData skeleton = NuitrackManager.Users.Current.Skeleton;
if (hand % 2 == 0)
{
Vector3 rightHandPos = Vector3.up * CalibrationInfo.FloorHeight + CalibrationInfo.SensorOrientation * skeleton.GetJoint(nuitrack.JointType.RightHand).Position;
pointerObject.position = rightHandPos;
}
else
{
Vector3 leftHandPos = Vector3.up * CalibrationInfo.FloorHeight + CalibrationInfo.SensorOrientation * skeleton.GetJoint(nuitrack.JointType.LeftHand).Position;
pointerObject.position = leftHandPos;
}
}
//Debug.Log ("STICK VALUE: " + x + " " + y);
}
// Update is called once per frame
void LateUpdate()
{
if (NuitrackManager.Users.Current == null && NuitrackManager.Users.Current.Skeleton == null)
{
return;
}
hand = PointerPassing.hand;
UserData.SkeletonData skeleton = NuitrackManager.Users.Current.Skeleton;
if (hand % 2 == 0)
{
Quaternion targetRot = skeleton.GetJoint(nuitrack.JointType.RightHand).Rotation;
transform.rotation = Quaternion.Lerp(transform.rotation, new Quaternion(targetRot.x * -1, targetRot.y * 1, targetRot.z * -1, targetRot.w * 1), speed * Time.deltaTime);
}
else
{
Quaternion targetRot = skeleton.GetJoint(nuitrack.JointType.LeftHand).Rotation;
transform.rotation = Quaternion.Lerp(transform.rotation, new Quaternion(targetRot.x * -1, targetRot.y * 1, targetRot.z * -1, targetRot.w * 1), speed * Time.deltaTime);
}
}
//can be used for sensor (angles, floor distance, maybe?) / user calibration (height, lengths)
void Calibration_onSuccess(Quaternion rotation)
{
//sensor orientation:
UserData.SkeletonData skeleton = NuitrackManager.Users.Current.Skeleton;
Vector3 torso = skeleton.GetJoint(nuitrack.JointType.Torso).Position;
Vector3 neck = skeleton.GetJoint(nuitrack.JointType.Neck).Position;
Vector3 diff = neck - torso;
sensorOrientation = Quaternion.Euler(-Mathf.Atan2(diff.z, diff.y) * Mathf.Rad2Deg, 0f, 0f);
//floor height:
if (trackFloorHeight && NuitrackManager.Floor != null)
{
Plane floorPlane = (Plane)NuitrackManager.Floor;
if (floorPlane.normal.sqrMagnitude > 0.01f) //
{
FloorHeight = floorPlane.GetDistanceToPoint(Vector3.zero);
}
}
}
