October3d55/M/UltraleapTracking/Source/UltraleapTrackingCore/Private/UltraleapTrackingData.cpp

472 lines
13 KiB
C++
Raw Normal View History

2025-07-21 10:22:56 +08:00
/******************************************************************************
* Copyright (C) Ultraleap, Inc. 2011-2021. *
* *
* Use subject to the terms of the Apache License 2.0 available at *
* http://www.apache.org/licenses/LICENSE-2.0, or another agreement *
* between Ultraleap and you, your company or other organization. *
******************************************************************************/
#include "UltraleapTrackingData.h"
#include "LeapC.h"
#include "LeapUtility.h"
#define MAX_DIGITS 5 // almost all humans have 5?
#define MAX_DIGIT_BONES 4 // some bones don't have all bones, see Leap documentation
FLeapHandData FLeapFrameData::HandForId(int32 HandId)
{
for (auto& Hand : Hands)
{
// if found return hand
if (Hand.Id == HandId)
{
return Hand;
}
}
// not found? return an empty hand
FLeapHandData EmptyHand;
return EmptyHand;
}
void FLeapFrameData::SetFromLeapFrame(
struct _LEAP_TRACKING_EVENT* frame, const FVector& LeapMountTranslationOffset, const FQuat& LeapMountRotationOffset)
{
if (frame == nullptr)
{
return;
}
// Copy basics
NumberOfHandsVisible = frame->nHands;
FrameRate = frame->framerate;
TimeStamp = frame->info.timestamp;
// Copy hand data
if (Hands.Num() != NumberOfHandsVisible) // always clear the hand data if number of hands changed
{
Hands.Empty();
}
LeftHandVisible = false;
RightHandVisible = false;
for (int i = 0; i < NumberOfHandsVisible; i++)
{
// Expand as necessary to fit
if (Hands.Num() <= i)
{
FLeapHandData HandData;
Hands.Add(HandData);
}
const LEAP_HAND& LeapHand = frame->pHands[i];
Hands[i].SetFromLeapHand((_LEAP_HAND*) &LeapHand, LeapMountTranslationOffset, LeapMountRotationOffset);
if (Hands[i].HandType == EHandType::LEAP_HAND_LEFT)
{
LeftHandVisible = true;
}
else if (Hands[i].HandType == EHandType::LEAP_HAND_RIGHT)
{
RightHandVisible = true;
}
}
FrameId = frame->tracking_frame_id;
}
void FLeapFrameData::SetInterpolationPartialFromLeapFrame(
struct _LEAP_TRACKING_EVENT* frame, const FVector& LeapMountTranslationOffset, const FQuat& LeapMountRotationOffset)
{
if (frame == nullptr)
{
return;
}
if (NumberOfHandsVisible != frame->nHands)
{
return;
}
for (int i = 0; i < NumberOfHandsVisible; i++)
{
const LEAP_HAND& LeapHand = frame->pHands[i];
Hands[i].SetArmPartialsFromLeapHand(
(_LEAP_HAND*) &LeapHand, LeapMountTranslationOffset, LeapMountRotationOffset);
}
TimeStamp = frame->info.timestamp;
}
void FLeapFrameData::ScaleFrame(float InScale)
{
for (auto& Hand : Hands)
{
Hand.ScaleHand(InScale);
}
}
void FLeapFrameData::RotateFrame(const FRotator& InRotation)
{
for (auto& Hand : Hands)
{
Hand.RotateHand(InRotation);
}
}
void FLeapFrameData::TranslateFrame(const FVector& InTranslation)
{
for (auto& Hand : Hands)
{
Hand.TranslateHand(InTranslation);
}
}
void FLeapHandData::InitFromEmpty(const EHandType HandTypeIn, const int HandID)
{
static int FingerID = 0;
Confidence = 1.0;
GrabAngle = 100;
GrabStrength = 0.5;
PinchStrength = 0.5;
Id = HandID;
for (int i = 0; i < MAX_DIGITS; i++)
{
if (Digits.Num() <= i) // will only pay the cost of filling once
{
FLeapDigitData DigitData;
DigitData.Bones.AddZeroed(4);
DigitData.FingerId = ++FingerID;
Digits.Add(DigitData);
}
}
PinchDistance = 50;
HandType = HandTypeIn;
VisibleTime = 1;
Flags = 0;
}
void FLeapHandData::UpdateFromDigits()
{
// The hand merger only sets the bone arrays
// Set the high level digits and digit members here
for (auto& Digit : Digits)
{
Digit.Metacarpal = Digit.Bones[0];
Digit.Proximal = Digit.Bones[1];
Digit.Intermediate = Digit.Bones[2];
Digit.Distal = Digit.Bones[3];
// this could be a merged state
Digit.IsExtended = false;
}
Thumb = Digits[0];
Index = Digits[1];
Middle = Digits[2];
Ring = Digits[3];
Pinky = Digits[4];
}
void FLeapHandData::SetFromLeapHand(
struct _LEAP_HAND* hand, const FVector& LeapMountTranslationOffset, const FQuat& LeapMountRotationOffset)
{
if (!hand)
{
return;
}
Arm.SetFromLeapBone((_LEAP_BONE*) &hand->arm, LeapMountTranslationOffset, LeapMountRotationOffset);
Confidence = hand->confidence;
GrabAngle = hand->grab_angle;
GrabStrength = hand->grab_strength;
Id = hand->id;
for (int i = 0; i < MAX_DIGITS; i++)
{
if (Digits.Num() <= i) // will only pay the cost of filling once
{
FLeapDigitData DigitData;
Digits.Add(DigitData);
}
Digits[i].SetFromLeapDigit((_LEAP_DIGIT*) &hand->digits[i], LeapMountTranslationOffset, LeapMountRotationOffset);
}
Flags = hand->flags;
Index.SetFromLeapDigit((_LEAP_DIGIT*) &hand->index, LeapMountTranslationOffset, LeapMountRotationOffset);
Middle.SetFromLeapDigit((_LEAP_DIGIT*) &hand->middle, LeapMountTranslationOffset, LeapMountRotationOffset);
Pinky.SetFromLeapDigit((_LEAP_DIGIT*) &hand->pinky, LeapMountTranslationOffset, LeapMountRotationOffset);
Ring.SetFromLeapDigit((_LEAP_DIGIT*) &hand->ring, LeapMountTranslationOffset, LeapMountRotationOffset);
Thumb.SetFromLeapDigit((_LEAP_DIGIT*) &hand->thumb, LeapMountTranslationOffset, LeapMountRotationOffset);
PinchDistance = FLeapUtility::ScaleLeapFloatToUE(hand->pinch_distance);
PinchStrength = hand->pinch_strength;
HandType = (EHandType) hand->type;
Palm.SetFromLeapPalm((_LEAP_PALM*) &hand->palm, LeapMountTranslationOffset, LeapMountRotationOffset);
VisibleTime = ((double) hand->visible_time / 1000000.0); // convert to seconds
}
void FLeapHandData::SetArmPartialsFromLeapHand(struct _LEAP_HAND* hand, const FVector& LeapMountTranslationOffset, const FQuat& LeapMountRotationOffset)
{
// Arm Partial
Arm.NextJoint = FLeapUtility::ConvertAndScaleLeapVectorToFVectorWithHMDOffsets(hand->arm.next_joint,LeapMountTranslationOffset, LeapMountRotationOffset);
Arm.PrevJoint = FLeapUtility::ConvertAndScaleLeapVectorToFVectorWithHMDOffsets(
hand->arm.prev_joint, LeapMountTranslationOffset, LeapMountRotationOffset);
// Palm Partial
Palm.Position = FLeapUtility::ConvertAndScaleLeapVectorToFVectorWithHMDOffsets(
hand->palm.position, LeapMountTranslationOffset, LeapMountRotationOffset);
// Debug - Set Orientation
// Palm.Direction = ConvertLeapVectorToFVector(hand->palm.direction);
// Palm.Normal = ConvertLeapVectorToFVector(hand->palm.normal);
// Palm.Orientation = FRotationMatrix::MakeFromXZ(Palm.Direction, -Palm.Normal).Rotator();
}
void FLeapHandData::ScaleHand(float InScale)
{
Arm.ScaleBone(InScale);
Index.ScaleDigit(InScale);
Middle.ScaleDigit(InScale);
Pinky.ScaleDigit(InScale);
Ring.ScaleDigit(InScale);
Thumb.ScaleDigit(InScale);
Palm.ScalePalm(InScale);
}
void FLeapHandData::RotateHand(const FRotator& InRotation)
{
Arm.RotateBone(InRotation);
Index.RotateDigit(InRotation);
Middle.RotateDigit(InRotation);
Pinky.RotateDigit(InRotation);
Ring.RotateDigit(InRotation);
Thumb.RotateDigit(InRotation);
Palm.RotatePalm(InRotation);
for (auto& Digit : Digits)
{
Digit.RotateDigit(InRotation);
}
}
void FLeapHandData::TranslateHand(const FVector& InTranslation)
{
Arm.TranslateBone(InTranslation);
Index.TranslateDigit(InTranslation);
Middle.TranslateDigit(InTranslation);
Pinky.TranslateDigit(InTranslation);
Ring.TranslateDigit(InTranslation);
Thumb.TranslateDigit(InTranslation);
Palm.TranslatePalm(InTranslation);
for(auto& Digit : Digits)
{
Digit.TranslateDigit(InTranslation);
}
}
void FLeapBoneData::SetFromLeapBone(
struct _LEAP_BONE* bone, const FVector& LeapMountTranslationOffset, const FQuat& LeapMountRotationOffset)
{
if (!bone)
{
return;
}
NextJoint = FLeapUtility::ConvertAndScaleLeapVectorToFVectorWithHMDOffsets(
bone->next_joint, LeapMountTranslationOffset, LeapMountRotationOffset);
PrevJoint = FLeapUtility::ConvertAndScaleLeapVectorToFVectorWithHMDOffsets(
bone->prev_joint, LeapMountTranslationOffset, LeapMountRotationOffset);
Rotation = FLeapUtility::ConvertToFQuatWithHMDOffsets(bone->rotation,LeapMountRotationOffset).Rotator();
Width = FLeapUtility::ScaleLeapFloatToUE(bone->width);
}
void FLeapBoneData::ScaleBone(float InScale)
{
NextJoint *= InScale;
PrevJoint *= InScale;
}
void FLeapBoneData::RotateBone(const FRotator& InRotation)
{
NextJoint = InRotation.RotateVector(NextJoint);
PrevJoint = InRotation.RotateVector(PrevJoint);
Rotation = FLeapUtility::CombineRotators(Rotation, InRotation);
}
void FLeapBoneData::TranslateBone(const FVector& InTranslation)
{
NextJoint += InTranslation;
PrevJoint += InTranslation;
}
void FLeapDigitData::SetFromLeapDigit(
struct _LEAP_DIGIT* digit, const FVector& LeapMountTranslationOffset, const FQuat& LeapMountRotationOffset)
{
// set bone data
for (int i = 0; i < MAX_DIGIT_BONES; i++)
{
if (Bones.Num() <= i) // will only pay the cost of filling once
{
FLeapBoneData BoneData;
Bones.Add(BoneData);
}
Bones[i].SetFromLeapBone((_LEAP_BONE*) &digit->bones[i], LeapMountTranslationOffset, LeapMountRotationOffset);
}
Distal.SetFromLeapBone((_LEAP_BONE*) &digit->distal, LeapMountTranslationOffset, LeapMountRotationOffset);
Intermediate.SetFromLeapBone((_LEAP_BONE*) &digit->intermediate, LeapMountTranslationOffset, LeapMountRotationOffset);
Metacarpal.SetFromLeapBone((_LEAP_BONE*) &digit->metacarpal, LeapMountTranslationOffset, LeapMountRotationOffset);
Proximal.SetFromLeapBone((_LEAP_BONE*) &digit->proximal, LeapMountTranslationOffset, LeapMountRotationOffset);
FingerId = digit->finger_id;
IsExtended = digit->is_extended == 1;
}
void FLeapDigitData::ScaleDigit(float InScale)
{
Distal.ScaleBone(InScale);
Intermediate.ScaleBone(InScale);
Metacarpal.ScaleBone(InScale);
Proximal.ScaleBone(InScale);
for (auto& Bone : Bones)
{
Bone.ScaleBone(InScale);
}
}
void FLeapDigitData::RotateDigit(const FRotator& InRotation)
{
Distal.RotateBone(InRotation);
Intermediate.RotateBone(InRotation);
Metacarpal.RotateBone(InRotation);
Proximal.RotateBone(InRotation);
for (auto& Bone : Bones)
{
Bone.RotateBone(InRotation);
}
}
void FLeapDigitData::TranslateDigit(const FVector& InTranslation)
{
Distal.TranslateBone(InTranslation);
Intermediate.TranslateBone(InTranslation);
Metacarpal.TranslateBone(InTranslation);
Proximal.TranslateBone(InTranslation);
for (auto& Bone : Bones)
{
Bone.TranslateBone(InTranslation);
}
}
void FLeapPalmData::SetFromLeapPalm(
struct _LEAP_PALM* palm, const FVector& LeapMountTranslationOffset, const FQuat& LeapMountRotationOffset)
{
Direction = FLeapUtility::ConvertLeapVectorToFVector(palm->direction);
Normal = FLeapUtility::ConvertLeapVectorToFVector(palm->normal);
Orientation = FLeapUtility::ConvertLeapQuatToFQuat(palm->orientation).Rotator();
Position = FLeapUtility::ConvertAndScaleLeapVectorToFVectorWithHMDOffsets(
palm->position, LeapMountTranslationOffset, LeapMountRotationOffset);
StabilizedPosition = FLeapUtility::ConvertAndScaleLeapVectorToFVectorWithHMDOffsets(
palm->stabilized_position, LeapMountTranslationOffset, LeapMountRotationOffset);
Velocity = FLeapUtility::ConvertAndScaleLeapVectorToFVectorWithHMDOffsets(
palm->velocity, LeapMountTranslationOffset, LeapMountRotationOffset);
Width = FLeapUtility::ScaleLeapFloatToUE(palm->width);
}
void FLeapPalmData::ScalePalm(float InScale)
{
Position *= InScale;
StabilizedPosition *= InScale;
Velocity *= InScale;
}
void FLeapPalmData::RotatePalm(const FRotator& InRotation)
{
Position = InRotation.RotateVector(Position);
StabilizedPosition = InRotation.RotateVector(StabilizedPosition);
Velocity = InRotation.RotateVector(Velocity);
Direction = InRotation.RotateVector(Direction);
Normal = InRotation.RotateVector(Normal);
Orientation = FLeapUtility::CombineRotators(Orientation, InRotation);
}
void FLeapPalmData::TranslatePalm(const FVector& InTranslation)
{
Position += InTranslation;
StabilizedPosition += InTranslation;
// Velocity += InTranslation;
}
FLeapOptions::FLeapOptions()
{
// Good Vive settings used as defaults
Mode = LEAP_MODE_DESKTOP;
TrackingFidelity = LEAP_NORMAL;
LeapServiceLogLevel = LEAP_LOG_INFO; // most verbose by default
bUseTimeWarp = true;
bUseInterpolation = true;
bTransformOriginToHMD = true;
TimewarpOffset = 5500;
TimewarpFactor = 1.f;
HandInterpFactor = 0.f;
FingerInterpFactor = 0.f;
// in mm
// HMDPositionOffset = FVector(90.0, 0, 0); // Vive default, for oculus use 80,0,0
// HMDRotationOffset = FRotator(0, 0, 0); // If imperfectly mounted it might need to sag
bUseFrameBasedGestureDetection = false;
StartGrabThreshold = .8f;
EndGrabThreshold = .5f;
StartPinchThreshold = .8f;
EndPinchThreshold = .5f;
GrabTimeout = 100000;
PinchTimeout = 100000;
bUseOpenXRAsSource = false;
HMDPositionOffset = FVector(80.f, 0, 0);
HMDRotationOffset = FRotator(0, 0, 0);
// bEnableImageStreaming = false; //default image streaming to off
}
FLeapStats::FLeapStats() : FrameExtrapolationInMS(0)
{
}
void FLeapDevice::SetFromLeapDevice(struct _LEAP_DEVICE_INFO* LeapInfo)
{
Status = LeapInfo->status;
Caps = LeapInfo->caps;
PID = FString(LeapDevicePIDToString(LeapInfo->pid));
Baseline = LeapInfo->baseline;
Serial = LeapInfo->serial;
HorizontalFOV = LeapInfo->h_fov;
VerticalFOV = LeapInfo->v_fov;
Range = LeapInfo->range;
}