231 lines
9.2 KiB
C#
231 lines
9.2 KiB
C#
#if AR_FOUNDATION_5 || AR_FOUNDATION_6
|
|
using Unity.Collections;
|
|
using Unity.XR.PXR;
|
|
using UnityEngine;
|
|
using UnityEngine.XR.ARSubsystems;
|
|
#if PICO_OPENXR_SDK
|
|
using Unity.XR.OpenXR.Features.PICOSupport;
|
|
#endif
|
|
public class PXR_HumanBodySubsystem : XRHumanBodySubsystem
|
|
{
|
|
internal const string k_SubsystemId = "PXR_HumanBodySubsystem";
|
|
|
|
class HumanBodyProvider : Provider
|
|
{
|
|
bool isBodyTrackingSupported = false;
|
|
bool init = false;
|
|
|
|
private BodyTrackingGetDataInfo bdi = new BodyTrackingGetDataInfo();
|
|
private BodyTrackingData bd = new BodyTrackingData();
|
|
BodyTrackingStatus bs = new BodyTrackingStatus();
|
|
bool istracking = false;
|
|
public override void Start()
|
|
{
|
|
PLog.i(k_SubsystemId, "Start");
|
|
init = true;
|
|
}
|
|
|
|
public override void Stop()
|
|
{
|
|
PLog.i(k_SubsystemId, "Stop");
|
|
#if PICO_OPENXR_SDK
|
|
int ret = BodyTrackingFeature.StopBodyTracking();
|
|
#else
|
|
int ret = PXR_MotionTracking.StopBodyTracking();
|
|
#endif
|
|
}
|
|
|
|
public override void Destroy()
|
|
{
|
|
PLog.i(k_SubsystemId, "Destroy");
|
|
}
|
|
|
|
public override void GetSkeleton(TrackableId trackableId, Allocator allocator, ref NativeArray<XRHumanBodyJoint> skeleton)
|
|
{
|
|
PLog.d(k_SubsystemId, $"GetSkeleton isBodyTrackingSupported={isBodyTrackingSupported}");
|
|
#if UNITY_ANDROID
|
|
if (isBodyTrackingSupported)
|
|
{
|
|
// Get the position and orientation data of each body node.
|
|
#if PICO_OPENXR_SDK
|
|
BodyTrackingFeature.GetBodyTrackingState(ref istracking, ref bs);
|
|
#else
|
|
PXR_MotionTracking.GetBodyTrackingState(ref istracking, ref bs);
|
|
#endif
|
|
// Debug.Log($"GetBodyTrackingState stateCode = {bs.stateCode} message = {bs.message} ");
|
|
// If not calibrated, invoked system motion tracker app for calibration.
|
|
|
|
|
|
// If not calibrated, invoked system motion tracker app for calibration.
|
|
if (bs.stateCode!=BodyTrackingStatusCode.BT_VALID)
|
|
{
|
|
return;
|
|
}
|
|
|
|
int ret = -1;
|
|
|
|
#if PICO_OPENXR_SDK
|
|
ret = BodyTrackingFeature.GetBodyTrackingData(ref bdi, ref bd);
|
|
#else
|
|
ret = PXR_MotionTracking.GetBodyTrackingData(ref bdi, ref bd);
|
|
#endif
|
|
|
|
// if the return is successful
|
|
if (ret == 0)
|
|
{
|
|
skeleton = new NativeArray<XRHumanBodyJoint>((int)BodyTrackerRole.ROLE_NUM, allocator);
|
|
for (int i = 0; i < (int)BodyTrackerRole.ROLE_NUM; i++)
|
|
{
|
|
BodyTrackerTransPose localPose = bd.roleDatas[i].localPose;
|
|
Vector3 pos = new Vector3((float)bd.roleDatas[i].localPose.PosX, (float)bd.roleDatas[i].localPose.PosY,
|
|
(float)bd.roleDatas[i].localPose.PosZ);
|
|
Quaternion qu = new Quaternion((float)bd.roleDatas[i].localPose.RotQx, (float)bd.roleDatas[i].localPose.RotQy,
|
|
(float)bd.roleDatas[i].localPose.RotQz, (float)bd.roleDatas[i].localPose.RotQw);
|
|
|
|
if (i == 0)
|
|
{
|
|
qu *= Quaternion.Euler(new Vector3(0, 180, 0));
|
|
}
|
|
else if (i == 1)
|
|
{
|
|
qu *= Quaternion.Euler(new Vector3(0, 0, -95));
|
|
}
|
|
else if (i == 2)
|
|
{
|
|
qu *= Quaternion.Euler(new Vector3(0, 0, 95));
|
|
}
|
|
else if (i == 4)
|
|
{
|
|
qu *= Quaternion.Euler(new Vector3(0, 0, -90));
|
|
}
|
|
else if (i == 3 || i == 5 || i == 12)
|
|
{
|
|
qu *= Quaternion.Euler(new Vector3(0, 0, 90));
|
|
}
|
|
else if (i == 7)
|
|
{
|
|
qu *= Quaternion.Euler(new Vector3(180, -90, 0));
|
|
}
|
|
else if (i == 6 || i == 9 || i == 15)
|
|
{
|
|
qu *= Quaternion.Euler(new Vector3(0, 90, 90));
|
|
}
|
|
else if (i == 8 || i == 10 || i == 11)
|
|
{
|
|
qu *= Quaternion.Euler(new Vector3(0, 90, 0));
|
|
}
|
|
else if (i == 13 || i == 14 || i == 16 || i == 17 || i == 18 || i == 19 || i == 20)
|
|
{
|
|
qu *= Quaternion.Euler(new Vector3(0, 0, 180));
|
|
}
|
|
else if (i == 21)
|
|
{
|
|
qu *= Quaternion.Euler(new Vector3(180, 0, 180));
|
|
}
|
|
|
|
Pose pose = new Pose(pos, qu);
|
|
XRHumanBodyJoint mXRHumanBodyJoint = new XRHumanBodyJoint(i, 0, Vector3.one, pose, Vector3.one, pose, true);
|
|
skeleton[i] = mXRHumanBodyJoint;
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
|
|
public override TrackableChanges<XRHumanBody> GetChanges(XRHumanBody defaultHumanBody, Allocator allocator)
|
|
{
|
|
PLog.d(k_SubsystemId, $"GetChanges init={init}, bodyTracking={PXR_ProjectSetting.GetProjectConfig().bodyTracking} ");
|
|
if (init)
|
|
{
|
|
if (PXR_ProjectSetting.GetProjectConfig().bodyTracking)
|
|
{
|
|
PXR_Plugin.MotionTracking.UPxr_WantBodyTrackingService();
|
|
|
|
// Query whether the current device supports human body tracking.
|
|
#if PICO_OPENXR_SDK
|
|
isBodyTrackingSupported = BodyTrackingFeature.IsBodyTrackingSupported();
|
|
#else
|
|
PXR_MotionTracking.GetBodyTrackingSupported(ref isBodyTrackingSupported);
|
|
#endif
|
|
if (isBodyTrackingSupported)
|
|
{
|
|
BodyTrackingBoneLength bones = new BodyTrackingBoneLength();
|
|
|
|
#if PICO_OPENXR_SDK
|
|
// Start BodyTracking
|
|
BodyTrackingFeature.StartBodyTracking(BodyJointSet.BODY_JOINT_SET_BODY_FULL_START, bones);
|
|
// Has Pico motion tracker completed calibration (0: not completed; 1: completed)?
|
|
BodyTrackingFeature.GetBodyTrackingState(ref istracking, ref bs);
|
|
#else
|
|
// Start BodyTracking
|
|
PXR_MotionTracking.StartBodyTracking(BodyJointSet.BODY_JOINT_SET_BODY_FULL_START, bones);
|
|
// Has Pico motion tracker completed calibration (0: not completed; 1: completed)?
|
|
PXR_MotionTracking.GetBodyTrackingState(ref istracking, ref bs);
|
|
#endif
|
|
// Debug.Log($"GetBodyTrackingState stateCode = {bs.stateCode} message = {bs.message} ");
|
|
// If not calibrated, invoked system motion tracker app for calibration.
|
|
|
|
if (bs.stateCode!=BodyTrackingStatusCode.BT_VALID)
|
|
{
|
|
if (bs.message==BodyTrackingMessage.BT_MESSAGE_TRACKER_NOT_CALIBRATED||bs.message==BodyTrackingMessage.BT_MESSAGE_UNKNOWN)
|
|
{
|
|
#if PICO_OPENXR_SDK
|
|
BodyTrackingFeature.StartMotionTrackerCalibApp();
|
|
#else
|
|
PXR_MotionTracking.StartMotionTrackerCalibApp();
|
|
#endif
|
|
}
|
|
}
|
|
// If not calibrated, invoked system motion tracker app for calibration.
|
|
|
|
}
|
|
}
|
|
init = false;
|
|
return new TrackableChanges<XRHumanBody>(1, 0, 0, allocator);
|
|
}
|
|
else
|
|
{
|
|
return new TrackableChanges<XRHumanBody>(0, 1, 0, allocator);
|
|
}
|
|
}
|
|
}
|
|
|
|
[RuntimeInitializeOnLoadMethod(RuntimeInitializeLoadType.SubsystemRegistration)]
|
|
static void RegisterDescriptor()
|
|
{
|
|
PLog.i(k_SubsystemId, "RegisterDescriptor");
|
|
#if AR_FOUNDATION_5
|
|
var mXRHumanBodySubsystemCinfo = new XRHumanBodySubsystemCinfo
|
|
#endif
|
|
|
|
#if AR_FOUNDATION_6
|
|
var mXRHumanBodySubsystemCinfo = new XRHumanBodySubsystemDescriptor.Cinfo
|
|
#endif
|
|
{
|
|
id = k_SubsystemId,
|
|
providerType = typeof(HumanBodyProvider),
|
|
subsystemTypeOverride = typeof(PXR_HumanBodySubsystem),
|
|
supportsHumanBody2D = false,
|
|
supportsHumanBody3D = true,
|
|
supportsHumanBody3DScaleEstimation = true,
|
|
};
|
|
|
|
#if AR_FOUNDATION_6
|
|
XRHumanBodySubsystemDescriptor.Register(mXRHumanBodySubsystemCinfo);
|
|
#endif
|
|
|
|
#if AR_FOUNDATION_5
|
|
if (!Register(mXRHumanBodySubsystemCinfo))
|
|
{
|
|
PLog.e(k_SubsystemId, $"Failed to register the {k_SubsystemId} subsystem.");
|
|
}
|
|
else
|
|
{
|
|
PLog.i(k_SubsystemId, $"success to register the {k_SubsystemId} subsystem.");
|
|
}
|
|
#endif
|
|
|
|
}
|
|
|
|
}
|
|
#endif |