private static void UpdateDeviceJoints(IVRModuleDeviceStateRW state, Vector3[] rawJoints, bool isLeft)
{
var hmdPose = VRModule.GetDeviceState(VRModule.HMD_DEVICE_INDEX).pose;
GestureInterface.SetCameraTransform(hmdPose.pos, hmdPose.rot);
var roomSpaceWrist2index = rawJoints[5] - rawJoints[0];
var roomSpaceWrist2middle = rawJoints[9] - rawJoints[0];
var roomSpaceWrist2pinky = rawJoints[17] - rawJoints[0];
var roomSpaceWristUp = isLeft ? Vector3.Cross(roomSpaceWrist2pinky, roomSpaceWrist2index) : Vector3.Cross(roomSpaceWrist2index, roomSpaceWrist2pinky);
var roomSpaceWristPose = new RigidPose(rawJoints[0], Quaternion.LookRotation(roomSpaceWrist2middle, roomSpaceWristUp));
state.pose = roomSpaceWristPose;
state.handJoints[HandJointName.Wrist] = new JointPose(roomSpaceWristPose);
state.handJoints[HandJointName.Palm] = new JointPose(new RigidPose((rawJoints[0] + rawJoints[9]) * 0.5f, roomSpaceWristPose.rot));
var camSpaceFingerRight = roomSpaceWristPose.rot * Vector3.right;
Quaternion roomSpaceRot;
roomSpaceRot = CalculateJointRot(rawJoints, 1, 2, roomSpaceWristPose.rot * (isLeft ? new Vector3(0.1f, -5.67f, -0.1f) : new Vector3(0.1f, 5.67f, 0.1f)));
state.handJoints[HandJointName.ThumbMetacarpal] = new JointPose(new RigidPose(rawJoints[1], roomSpaceRot));
roomSpaceRot = CalculateJointRot(rawJoints, 2, 3, roomSpaceWristPose.rot * (isLeft ? new Vector3(0.1f, -5.67f, -0.1f) : new Vector3(0.1f, 5.67f, 0.1f)));
state.handJoints[HandJointName.ThumbProximal] = new JointPose(new RigidPose(rawJoints[2], roomSpaceRot));
roomSpaceRot = CalculateJointRot(rawJoints, 3, 4, roomSpaceWristPose.rot * (isLeft ? new Vector3(1.72f, -5.67f, -3.55f) : new Vector3(1.72f, 5.67f, 3.55f)));
state.handJoints[HandJointName.ThumbDistal] = new JointPose(new RigidPose(rawJoints[3], roomSpaceRot));
state.handJoints[HandJointName.ThumbTip] = new JointPose(new RigidPose(rawJoints[4], roomSpaceRot));
roomSpaceRot = CalculateJointRot(rawJoints, 5, 6, camSpaceFingerRight);
state.handJoints[HandJointName.IndexProximal] = new JointPose(new RigidPose(rawJoints[5], roomSpaceRot));
roomSpaceRot = CalculateJointRot(rawJoints, 6, 7, camSpaceFingerRight);
state.handJoints[HandJointName.IndexIntermediate] = new JointPose(new RigidPose(rawJoints[6], roomSpaceRot));
roomSpaceRot = CalculateJointRot(rawJoints, 7, 8, camSpaceFingerRight);
state.handJoints[HandJointName.IndexDistal] = new JointPose(new RigidPose(rawJoints[7], roomSpaceRot));
state.handJoints[HandJointName.IndexTip] = new JointPose(new RigidPose(rawJoints[8], roomSpaceRot));
roomSpaceRot = CalculateJointRot(rawJoints, 9, 10, camSpaceFingerRight);
state.handJoints[HandJointName.MiddleProximal] = new JointPose(new RigidPose(rawJoints[9], roomSpaceRot));
roomSpaceRot = CalculateJointRot(rawJoints, 10, 11, camSpaceFingerRight);
state.handJoints[HandJointName.MiddleIntermediate] = new JointPose(new RigidPose(rawJoints[10], roomSpaceRot));
roomSpaceRot = CalculateJointRot(rawJoints, 11, 12, camSpaceFingerRight);
state.handJoints[HandJointName.MiddleDistal] = new JointPose(new RigidPose(rawJoints[11], roomSpaceRot));
state.handJoints[HandJointName.MiddleTip] = new JointPose(new RigidPose(rawJoints[12], roomSpaceRot));
roomSpaceRot = CalculateJointRot(rawJoints, 13, 14, camSpaceFingerRight);
state.handJoints[HandJointName.RingProximal] = new JointPose(new RigidPose(rawJoints[13], roomSpaceRot));
roomSpaceRot = CalculateJointRot(rawJoints, 14, 15, camSpaceFingerRight);
state.handJoints[HandJointName.RingIntermediate] = new JointPose(new RigidPose(rawJoints[14], roomSpaceRot));
roomSpaceRot = CalculateJointRot(rawJoints, 15, 16, camSpaceFingerRight);
state.handJoints[HandJointName.RingDistal] = new JointPose(new RigidPose(rawJoints[15], roomSpaceRot));
state.handJoints[HandJointName.RingTip] = new JointPose(new RigidPose(rawJoints[16], roomSpaceRot));
roomSpaceRot = CalculateJointRot(rawJoints, 17, 18, camSpaceFingerRight);
state.handJoints[HandJointName.PinkyProximal] = new JointPose(new RigidPose(rawJoints[17], roomSpaceRot));
roomSpaceRot = CalculateJointRot(rawJoints, 18, 19, camSpaceFingerRight);
state.handJoints[HandJointName.PinkyIntermediate] = new JointPose(new RigidPose(rawJoints[18], roomSpaceRot));
roomSpaceRot = CalculateJointRot(rawJoints, 19, 20, camSpaceFingerRight);
state.handJoints[HandJointName.PinkyDistal] = new JointPose(new RigidPose(rawJoints[19], roomSpaceRot));
state.handJoints[HandJointName.PinkyTip] = new JointPose(new RigidPose(rawJoints[20], roomSpaceRot));
}
private IEnumerator StartDetectionCoroutine()
{
while (true)
{
var error = GestureInterface.StartGestureDetection(option);
lock (this)
{
switch (error)
{
case GestureFailure.None:
retryCount = RETRY_COUNT;
lastResultFrame = -1;
isStarted = true;
Debug.Log(LOG_PREFIX + "Detection started");
break;
case GestureFailure.Camera:
--retryCount;
if (retryCount >= 0)
{
Debug.LogWarning(LOG_PREFIX + "StartGestureDetection fail. Front camera function not found. retrying(" + (retryCount + 1) + ")...");
}
else
{
Debug.LogWarning(LOG_PREFIX + "StartGestureDetection fail. Front camera function not found.");
}
break;
default:
retryCount = 0;
Debug.LogWarning(LOG_PREFIX + "StartGestureDetection fail. error:" + error);
break;
}
if (retryCount <= 0 || isStarted)
{
yield break;
}
}
yield return(retryInterval);
}
}
protected override void OnDeactivated()
{
GestureInterface.StopGestureDetection();
startDetectionTask.Cancel();
}
protected override void OnActivated()
{
retryCount = RETRY_COUNT;
GestureInterface.UseExternalTransform(true);
}
protected override void OnUpdateDeviceConnectionAndPoses()
{
lock (this)
{
if (!isStarted && retryCount > 0 && startDetectionTask.IsDone)
{
// try start engine detection
LiteCoroutine.StartCoroutine(ref startDetectionCoroutine, startDetectionTask.RestartTask(StartDetectionCoroutine()));
}
if (!isStarted)
{
return;
}
}
var hmdPose = VRModule.GetDeviceState(VRModule.HMD_DEVICE_INDEX).pose;
GestureInterface.SetCameraTransform(hmdPose.pos, hmdPose.rot);
// fetch raw data from engine
IntPtr resultPtr;
int resultFrame;
var resultSize = GestureInterface.GetGestureResult(out resultPtr, out resultFrame);
if (resultFrame < 0)
{
Debug.Log(LOG_PREFIX + "Detection stopped");
isStarted = false;
return;
}
else if (resultFrame <= lastResultFrame)
{
// skip frame
return;
}
lastResultFrame = resultFrame;
leftResult.isConnected = false;
rightResult.isConnected = false;
for (int i = 0, imax = resultSize; i < imax; ++i)
{
var result = (GestureResult)Marshal.PtrToStructure(resultPtr, typeof(GestureResult));
if (result.isLeft)
{
leftResult = new HandResultData()
{
isConnected = true,
rawData = result,
};
}
else
{
rightResult = new HandResultData()
{
isConnected = true,
rawData = result,
};
}
#if NET_4_6
resultPtr = IntPtr.Add(resultPtr, sizeofGestureResult);
#else
resultPtr = new IntPtr(resultPtr.ToInt64() + sizeofGestureResult);
#endif
}
UpdateDeviceConnectionAndPoses(ref leftResult, ref leftDeviceIndex, true);
UpdateDeviceConnectionAndPoses(ref rightResult, ref rightDeviceIndex, false);
}
protected override void OnUpdateDeviceConnectionAndPoses()
{
// try start engine detection
if (!isStarted)
{
var now = Time.unscaledTime;
if (now >= nextRetryTime && retryCount >= 0)
{
--retryCount;
nextRetryTime = now + RETRY_INTERVAL;
var error = GestureInterface.StartGestureDetection(option);
switch (error)
{
case GestureFailure.None:
retryCount = RETRY_COUNT;
lastResultFrame = -1;
isStarted = true;
Debug.Log("[ViveHandTrackingSubmodule] StartGestureDetection");
break;
case GestureFailure.Camera:
--retryCount;
nextRetryTime = now + RETRY_INTERVAL;
if (retryCount >= 0)
{
Debug.LogWarning("[ViveHandTrackingSubmodule] StartGestureDetection fail. Front camera function not found. retrying...");
}
else
{
Debug.LogWarning("[ViveHandTrackingSubmodule] StartGestureDetection fail. Front camera function not found.");
}
break;
default:
retryCount = 0;
Debug.LogWarning("[ViveHandTrackingSubmodule] StartGestureDetection fail. error:" + error);
break;
}
}
}
if (!isStarted)
{
return;
}
// fetch raw data from engine
IntPtr resultPtr;
int resultFrame;
var resultSize = GestureInterface.GetGestureResult(out resultPtr, out resultFrame);
if (resultFrame < 0)
{
Debug.Log("[ViveHandTrackingSubmodule] Detection stopped!");
isStarted = false;
return;
}
else if (resultFrame < lastResultFrame)
{
// skip frame
return;
}
lastResultFrame = resultFrame;
leftResult.isConnected = false;
rightResult.isConnected = false;
for (int i = 0, imax = resultSize; i < imax; ++i)
{
var result = (GestureResult)Marshal.PtrToStructure(resultPtr, typeof(GestureResult));
if (result.isLeft)
{
leftResult = new HandResultData()
{
isConnected = true,
joints = result.points,
gesture = result.gesture,
confidence = result.confidence,
pinchLevel = result.pinchLevel,
};
}
else
{
rightResult = new HandResultData()
{
isConnected = true,
joints = result.points,
gesture = result.gesture,
confidence = result.confidence,
pinchLevel = result.pinchLevel,
};
}
#if NET_4_6
resultPtr = IntPtr.Add(resultPtr, sizeofGestureResult);
#else
resultPtr = new IntPtr(resultPtr.ToInt64() + sizeofGestureResult);
#endif
}
UpdateDeviceConnectionAndPoses(ref leftResult, ref leftDeviceIndex, true);
UpdateDeviceConnectionAndPoses(ref rightResult, ref rightDeviceIndex, false);
}
protected override void OnDeactivated()
{
GestureInterface.StopGestureDetection();
}
