private static bool ExtractHandJointData(WVR_HandJointData_t handJointData, ref WVR_Pose_t[] jointsPose)
{
if (handJointData.jointCount == 0)
{
Debug.Log("ExtractHandJointData() WVR_GetHandTrackingData WVR_HandJointData_t jointCount SHOULD NOT be 0!!");
return(false);
}
if (jointsPose.Length != handJointData.jointCount)
{
Debug.Log("ExtractHandJointData() The WVR_GetHandJointCount count " + jointsPose.Length
+ " differs from WVR_GetHandTrackingData WVR_HandJointData_t jointCount " + handJointData.jointCount);
jointsPose = new WVR_Pose_t[handJointData.jointCount];
}
WVR_Pose_t wvr_pose_type = default(WVR_Pose_t);
int offset = 0;
for (int i = 0; i < jointsPose.Length; i++)
{
if (IntPtr.Size == 4)
{
jointsPose[i] = (WVR_Pose_t)Marshal.PtrToStructure(new IntPtr(handJointData.joints.ToInt32() + offset), typeof(WVR_Pose_t));
}
else
{
jointsPose[i] = (WVR_Pose_t)Marshal.PtrToStructure(new IntPtr(handJointData.joints.ToInt64() + offset), typeof(WVR_Pose_t));
}
offset += Marshal.SizeOf(wvr_pose_type);
}
return(true);
}
private void DumpPoseState(WVR_PoseState_t tPoseState)
{
// if(tPoseState == null || !tPoseState.IsValidPose ) return;
bool tIsValidPose = tPoseState.IsValidPose;
WVR_Matrix4f_t tPoseMatrix = tPoseState.PoseMatrix;
WVR_Vector3f_t tVelocity = tPoseState.Velocity;
WVR_Vector3f_t tAngV = tPoseState.AngularVelocity;
bool tIs6DoFPose = tPoseState.Is6DoFPose;
long tStamp_ns = tPoseState.PoseTimestamp_ns;
WVR_Vector3f_t tAcceleration = tPoseState.Acceleration;
WVR_Vector3f_t tAngAcc = tPoseState.AngularAcceleration;
float tPredictedMilliSec = tPoseState.PredictedMilliSec;
WVR_PoseOriginModel tOriginModel = tPoseState.OriginModel;
WVR_Pose_t tRawPose = tPoseState.RawPose;
WVR_Vector3f_t tPosition = tRawPose.position;
WVR_Quatf_t tRotation = tRawPose.rotation;
Log.d(LOG_TAG, "PoseState:: IsValidPose=" + tIsValidPose +
",Stamp_ns=" + tStamp_ns +
",RawPose.Postion(x,y,z)=" + tPosition.v0 + "," + tPosition.v1 + "," + tPosition.v2 +
",RawPose.Rotation(w,x,y,z)=" + tRotation.w + "," + tRotation.x + "," + tRotation.y + "," + tRotation.z +
",Velocity(x,y,z)=" + tVelocity.v0 + "," + tVelocity.v1 + "," + tVelocity.v2 +
",AngularVelocity(x,y,z)=" + tAngV.v0 + "," + tAngV.v1 + "," + tAngV.v2 +
",Acc(x,y,z)=" + tAcceleration.v0 + "," + tAcceleration.v1 + "," + tAcceleration.v2 +
",AngAcc(x,y,z)=" + tAngAcc.v0 + "," + tAngAcc.v1 + "," + tAngAcc.v2 +
",OriginModel=" + tOriginModel +
",PredictedMilliSec=" + tPredictedMilliSec +
",PoseMatrix(4X1)=" + tPoseMatrix.m0 + "," + tPoseMatrix.m1 + "," + tPoseMatrix.m2 + "," + tPoseMatrix.m3 +
",PoseMatrix(4X2)=" + tPoseMatrix.m4 + "," + tPoseMatrix.m5 + "," + tPoseMatrix.m6 + "," + tPoseMatrix.m7 +
",PoseMatrix(4X3)=" + tPoseMatrix.m8 + "," + tPoseMatrix.m9 + "," + tPoseMatrix.m10 + "," + tPoseMatrix.m11 +
",PoseMatrix(4X4)=" + tPoseMatrix.m12 + "," + tPoseMatrix.m13 + "," + tPoseMatrix.m14 + "," + tPoseMatrix.m15 +
".<end>");
}
private static void InitializeHandJointData(ref WVR_HandJointData_t handJointData, ref WVR_Pose_t[] jointsPose, uint count)
{
handJointData.isValidPose = false;
handJointData.confidence = 0;
handJointData.jointCount = count;
WVR_Pose_t wvr_pose_type = default(WVR_Pose_t);
handJointData.joints = Marshal.AllocHGlobal(Marshal.SizeOf(wvr_pose_type) * (int)count);
jointsPose = new WVR_Pose_t[count];
long offset = 0;
if (IntPtr.Size == 4)
{
offset = handJointData.joints.ToInt32();
}
else
{
offset = handJointData.joints.ToInt64();
}
for (int i = 0; i < jointsPose.Length; i++)
{
IntPtr wvr_pose_ptr = new IntPtr(offset);
Marshal.StructureToPtr(jointsPose[i], wvr_pose_ptr, false);
offset += Marshal.SizeOf(wvr_pose_type);
}
}
