392 lines
15 KiB
C#
392 lines
15 KiB
C#
/*
|
|
* Copyright (c) Meta Platforms, Inc. and affiliates.
|
|
* All rights reserved.
|
|
*
|
|
* Licensed under the Oculus SDK License Agreement (the "License");
|
|
* you may not use the Oculus SDK except in compliance with the License,
|
|
* which is provided at the time of installation or download, or which
|
|
* otherwise accompanies this software in either electronic or hard copy form.
|
|
*
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* https://developer.oculus.com/licenses/oculussdk/
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, the Oculus SDK
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
|
|
using Oculus.Interaction.Input;
|
|
using System.Collections.Generic;
|
|
using UnityEngine;
|
|
|
|
namespace Oculus.Interaction.GrabAPI
|
|
{
|
|
/// <summary>
|
|
/// This Finger API uses an advanced calculation for the pinch value of the fingers
|
|
/// to detect if they are grabbing
|
|
/// </summary>
|
|
public class PinchGrabAPI : IFingerAPI
|
|
{
|
|
private bool _isPinchVisibilityGood;
|
|
private float DistanceStart => _isPinchVisibilityGood ? PINCH_HQ_DISTANCE_START : PINCH_DISTANCE_START;
|
|
private float DistanceStopMax => _isPinchVisibilityGood ? PINCH_HQ_DISTANCE_STOP_MAX : PINCH_DISTANCE_STOP_MAX;
|
|
private float DistanceStopOffset => _isPinchVisibilityGood ? PINCH_HQ_DISTANCE_STOP_OFFSET : PINCH_DISTANCE_STOP_OFFSET;
|
|
|
|
private const float PINCH_DISTANCE_START = 0.02f;
|
|
private const float PINCH_DISTANCE_STOP_MAX = 0.1f;
|
|
private const float PINCH_DISTANCE_STOP_OFFSET = 0.04f;
|
|
|
|
private const float PINCH_HQ_DISTANCE_START = 0.016f;
|
|
private const float PINCH_HQ_DISTANCE_STOP_MAX = 0.1f;
|
|
private const float PINCH_HQ_DISTANCE_STOP_OFFSET = 0.016f;
|
|
|
|
private const float THUMB_DISTANCE_START = 0.03f;
|
|
private const float THUMB_DISTANCE_STOP_MAX = 0.05f;
|
|
private const float THUMB_DISTANCE_STOP_OFFSET = 0.04f;
|
|
private const float THUMB_MAX_DOT = 0.5f;
|
|
|
|
private const float PINCH_HQ_VIEW_ANGLE_THRESHOLD = 40f;
|
|
|
|
private readonly HandJointId[] THUMB_JOINTS_SELECT = new[]
|
|
{
|
|
HandJointId.HandThumb3,
|
|
HandJointId.HandThumbTip
|
|
};
|
|
|
|
private readonly HandJointId[] THUMB_JOINTS_MAINTAIN = new[]
|
|
{
|
|
HandJointId.HandThumb2,
|
|
HandJointId.HandThumb3,
|
|
HandJointId.HandThumbTip
|
|
};
|
|
|
|
private readonly HandJointId[] INDEX_JOINTS = new[]
|
|
{
|
|
HandJointId.HandIndex1,
|
|
HandJointId.HandIndex2,
|
|
HandJointId.HandIndex3,
|
|
HandJointId.HandIndexTip,
|
|
};
|
|
|
|
private class FingerPinchData
|
|
{
|
|
private readonly HandJointId _tipId;
|
|
private float _minPinchDistance;
|
|
|
|
public Vector3 TipPosition { get; private set; }
|
|
public bool IsPinchingChanged { get; private set; }
|
|
public float PinchStrength;
|
|
public bool IsPinching;
|
|
|
|
public FingerPinchData(HandFinger fingerId)
|
|
{
|
|
_tipId = HandJointUtils.GetHandFingerTip(fingerId);
|
|
}
|
|
|
|
public void UpdateTipPosition(ShadowHand hand)
|
|
{
|
|
var pose = hand.GetWorldPose(_tipId);
|
|
TipPosition = pose.position;
|
|
}
|
|
|
|
public void UpdateIsPinching(float distance, float start, float stopOffset, float stopMax)
|
|
{
|
|
if (!IsPinching)
|
|
{
|
|
if (distance < start)
|
|
{
|
|
IsPinching = true;
|
|
IsPinchingChanged = true;
|
|
_minPinchDistance = distance;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
_minPinchDistance = Mathf.Min(_minPinchDistance, distance);
|
|
if (distance > stopMax ||
|
|
distance > _minPinchDistance + stopOffset)
|
|
{
|
|
IsPinching = false;
|
|
IsPinchingChanged = true;
|
|
_minPinchDistance = float.MaxValue;
|
|
}
|
|
}
|
|
}
|
|
|
|
public void ClearState()
|
|
{
|
|
IsPinchingChanged = false;
|
|
}
|
|
}
|
|
|
|
private readonly FingerPinchData[] _fingersPinchData =
|
|
{
|
|
new FingerPinchData(HandFinger.Thumb),
|
|
new FingerPinchData(HandFinger.Index),
|
|
new FingerPinchData(HandFinger.Middle),
|
|
new FingerPinchData(HandFinger.Ring),
|
|
new FingerPinchData(HandFinger.Pinky)
|
|
};
|
|
|
|
private IHmd _hmd = null;
|
|
private readonly ShadowHand _shadowHand = new();
|
|
private float _handScale;
|
|
private Pose _rootPose;
|
|
|
|
public PinchGrabAPI(IHmd hmd = null)
|
|
{
|
|
_hmd = hmd;
|
|
}
|
|
|
|
public bool GetFingerIsGrabbing(HandFinger finger)
|
|
{
|
|
return _fingersPinchData[(int)finger].IsPinching;
|
|
}
|
|
|
|
public Vector3 GetWristOffsetLocal()
|
|
{
|
|
float maxStrength = _fingersPinchData[0].PinchStrength;
|
|
Vector3 thumbTip = _fingersPinchData[0].TipPosition;
|
|
Vector3 center = thumbTip;
|
|
for (int i = 1; i < Constants.NUM_FINGERS; ++i)
|
|
{
|
|
float strength = _fingersPinchData[i].PinchStrength;
|
|
if (strength > maxStrength)
|
|
{
|
|
maxStrength = strength;
|
|
Vector3 fingerTip = _fingersPinchData[i].TipPosition;
|
|
center = (thumbTip + fingerTip) * 0.5f;
|
|
}
|
|
}
|
|
return center;
|
|
}
|
|
|
|
public bool GetFingerIsGrabbingChanged(HandFinger finger, bool targetPinchState)
|
|
{
|
|
return _fingersPinchData[(int)finger].IsPinchingChanged &&
|
|
_fingersPinchData[(int)finger].IsPinching == targetPinchState;
|
|
}
|
|
|
|
public float GetFingerGrabScore(HandFinger finger)
|
|
{
|
|
return _fingersPinchData[(int)finger].PinchStrength;
|
|
}
|
|
|
|
public void Update(IHand hand)
|
|
{
|
|
hand.GetRootPose(out var rootPose);
|
|
hand.GetJointPosesLocal(out var localJointPoses);
|
|
Update(localJointPoses, hand.Handedness, rootPose, hand.Scale);
|
|
}
|
|
|
|
internal void Update(IReadOnlyList<Pose> handPoses, Handedness handedness, Pose rootPose, float handScale)
|
|
{
|
|
ClearState();
|
|
_shadowHand.SetRoot(Pose.identity);
|
|
#if ISDK_OPENXR_HAND
|
|
_shadowHand.FromJoints(handPoses, false);
|
|
#else
|
|
_shadowHand.FromJoints(handPoses, handedness == Handedness.Left);
|
|
#endif
|
|
|
|
_rootPose = rootPose;
|
|
_handScale = handScale;
|
|
|
|
_isPinchVisibilityGood = PinchHasGoodVisibility(handedness);
|
|
|
|
UpdateThumb(handedness);
|
|
UpdateFinger(HandFinger.Index);
|
|
UpdateFinger(HandFinger.Middle);
|
|
UpdateFinger(HandFinger.Ring);
|
|
UpdateFinger(HandFinger.Pinky);
|
|
}
|
|
|
|
private void UpdateThumb(Handedness handedness)
|
|
{
|
|
int fingerIndex = (int)HandFinger.Thumb;
|
|
_fingersPinchData[fingerIndex].UpdateTipPosition(_shadowHand);
|
|
|
|
float distance = float.PositiveInfinity;
|
|
if (IsThumbNearIndex(handedness))
|
|
{
|
|
var thumb3Pose = _shadowHand.GetWorldPose(HandJointId.HandThumb3);
|
|
Vector3 thumbStart = thumb3Pose.position;
|
|
Vector3 thumbEnd = _fingersPinchData[fingerIndex].TipPosition;
|
|
distance = GetClosestDistanceToJoints(thumbStart, thumbEnd, INDEX_JOINTS, THUMB_MAX_DOT);
|
|
}
|
|
UpdatePinchData(distance, fingerIndex,
|
|
THUMB_DISTANCE_START, THUMB_DISTANCE_STOP_OFFSET, THUMB_DISTANCE_STOP_MAX);
|
|
}
|
|
|
|
private bool IsThumbNearIndex(Handedness handedness)
|
|
{
|
|
var thumbTipPose = _shadowHand.GetWorldPose(HandJointId.HandThumbTip);
|
|
var indexPose = _shadowHand.GetWorldPose(HandJointId.HandIndex2);
|
|
|
|
Vector3 indexSideDir = indexPose.rotation * (handedness == Handedness.Left ? Constants.LeftThumbSide : Constants.RightThumbSide);
|
|
Plane indexPlane = new Plane(indexSideDir, indexPose.position);
|
|
float distanceToPlane = Mathf.Abs(indexPlane.GetDistanceToPoint(thumbTipPose.position));
|
|
return distanceToPlane > 0 && distanceToPlane < THUMB_DISTANCE_STOP_MAX;
|
|
}
|
|
|
|
private void UpdateFinger(HandFinger finger)
|
|
{
|
|
int fingerIndex = (int)finger;
|
|
_fingersPinchData[fingerIndex].UpdateTipPosition(_shadowHand);
|
|
|
|
float distance = float.PositiveInfinity;
|
|
if (_fingersPinchData[fingerIndex].IsPinching)
|
|
{
|
|
distance = GetClosestDistanceToJoints(_fingersPinchData[fingerIndex].TipPosition, THUMB_JOINTS_MAINTAIN);
|
|
}
|
|
if (IsPointNearThumb(_fingersPinchData[fingerIndex].TipPosition, THUMB_JOINTS_SELECT))
|
|
{
|
|
distance = GetClosestDistanceToJoints(_fingersPinchData[fingerIndex].TipPosition, THUMB_JOINTS_SELECT);
|
|
}
|
|
|
|
UpdatePinchData(distance, fingerIndex,
|
|
DistanceStart, DistanceStopOffset, DistanceStopMax);
|
|
}
|
|
|
|
private void UpdatePinchData(float distance, int fingerIndex,
|
|
float distanceStart, float distanceStopOffset, float distanceStopMax)
|
|
{
|
|
_fingersPinchData[fingerIndex].UpdateIsPinching(distance,
|
|
distanceStart, distanceStopOffset, distanceStopMax);
|
|
float pinchPercent = (distance - distanceStart) /
|
|
(distanceStopMax - distanceStart);
|
|
float pinchStrength = 1f - Mathf.Clamp01(pinchPercent);
|
|
|
|
_fingersPinchData[fingerIndex].PinchStrength = pinchStrength;
|
|
}
|
|
|
|
private void ClearState()
|
|
{
|
|
for (int i = 0; i < Constants.NUM_FINGERS; ++i)
|
|
{
|
|
_fingersPinchData[i].ClearState();
|
|
}
|
|
}
|
|
|
|
private bool IsPointNearThumb(Vector3 position, HandJointId[] thumbJoints)
|
|
{
|
|
var boneStart = _shadowHand.GetWorldPose(thumbJoints[0]);
|
|
var boneEnd = _shadowHand.GetWorldPose(thumbJoints[1]);
|
|
Vector3 p0 = boneStart.position;
|
|
Vector3 p1 = boneEnd.position;
|
|
Vector3 lineVec = p1 - p0;
|
|
Vector3 fromP0 = position - p0;
|
|
Vector3 projectedPos = Vector3.Project(fromP0, lineVec.normalized);
|
|
return Vector3.Dot(projectedPos, lineVec) > 0;
|
|
}
|
|
|
|
private float GetClosestDistanceToJoints(Vector3 edgeStart, Vector3 edgeEnd, HandJointId[] targetJoints, float maximumDotAllowed = 1f)
|
|
{
|
|
float minDistance = float.PositiveInfinity;
|
|
for (int i = 0; i < targetJoints.Length - 1; i++)
|
|
{
|
|
var boneStart = _shadowHand.GetWorldPose(targetJoints[i]);
|
|
var boneEnd = _shadowHand.GetWorldPose(targetJoints[i + 1]);
|
|
if (maximumDotAllowed < 1f
|
|
&& Vector3.Dot((edgeEnd - edgeStart).normalized,
|
|
(boneEnd.position - boneStart.position).normalized) >= maximumDotAllowed)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
float distance = DistanceSegmentToSegment(edgeStart, edgeEnd, boneStart.position, boneEnd.position);
|
|
minDistance = Mathf.Min(minDistance, distance);
|
|
}
|
|
|
|
return minDistance;
|
|
}
|
|
|
|
private float GetClosestDistanceToJoints(Vector3 position, HandJointId[] targetJoints)
|
|
{
|
|
float minDistance = float.PositiveInfinity;
|
|
for (int i = 0; i < targetJoints.Length - 1; i++)
|
|
{
|
|
var boneStart = _shadowHand.GetWorldPose(targetJoints[i]);
|
|
var boneEnd = _shadowHand.GetWorldPose(targetJoints[i + 1]);
|
|
minDistance = Mathf.Min(minDistance,
|
|
DistancePointToSegment(position, boneStart.position, boneEnd.position));
|
|
}
|
|
|
|
return minDistance;
|
|
}
|
|
|
|
private float DistancePointToSegment(Vector3 point, Vector3 a0, Vector3 a1)
|
|
{
|
|
Vector3 lineVec = a1 - a0;
|
|
Vector3 fromP0 = point - a0;
|
|
float normalizedProjection = Vector3.Dot(fromP0, lineVec) / Vector3.Dot(lineVec, lineVec);
|
|
float closestT = Mathf.Clamp01(normalizedProjection);
|
|
Vector3 closestPoint = a0 + closestT * lineVec;
|
|
return (closestPoint - point).magnitude;
|
|
}
|
|
|
|
private float DistanceSegmentToSegment(Vector3 a0, Vector3 a1, Vector3 b0, Vector3 b1)
|
|
{
|
|
Vector3 aDir = (a1 - a0);
|
|
Vector3 bDir = (b1 - b0);
|
|
Vector3 orthogonalDir = Vector3.Cross(aDir, bDir);
|
|
|
|
//In order to find the Segment (c) that is the shortest between (a) and (b):
|
|
//Project the two segments (a) and (b) in the Orthogonal plane
|
|
//This way we can find the (A)(B)(C) Triangle rectangle that has
|
|
// A == a0 (in the projected space)
|
|
// B == a point along vector (b) whose angle is 90 degrees to A (in the projected space)
|
|
// C == the point where (a) and (b) cross each other (in the projected space)
|
|
Vector3 A = Vector3.ProjectOnPlane(a0, orthogonalDir);
|
|
Vector3 b0Projected = Vector3.ProjectOnPlane(b0, orthogonalDir);
|
|
Vector3 aDirProjected = Vector3.ProjectOnPlane(aDir, orthogonalDir);
|
|
Vector3 bDirProjected = Vector3.ProjectOnPlane(bDir, orthogonalDir);
|
|
|
|
Vector3 B = b0Projected + Vector3.Project(A - b0Projected, bDirProjected);
|
|
Vector3 adjacentSide = B - A;
|
|
float angleA = Vector3.Dot(aDirProjected.normalized, adjacentSide.normalized);
|
|
float hypotenuse = adjacentSide.magnitude / angleA;
|
|
//C would be A + aDirProjected * hypotenuse.
|
|
|
|
//c0 is the start point in world space for the segment (c). It has to be inside (a)
|
|
Vector3 c0 = a0 + aDir * Mathf.Clamp01(hypotenuse / aDirProjected.magnitude);
|
|
|
|
//c1 is the end point in world space, for the segment (c). It can be found by
|
|
//projecting b0c0 into (b)
|
|
Vector3 b0c1 = Vector3.Project(c0 - b0, bDir);
|
|
|
|
//c1 has to be inside (b)
|
|
if (Vector3.Dot(b0c1, bDir) < 0)
|
|
{
|
|
b0c1 = Vector3.zero;
|
|
}
|
|
else if (b0c1.sqrMagnitude > bDir.sqrMagnitude)
|
|
{
|
|
b0c1 = bDir;
|
|
}
|
|
Vector3 c1 = b0 + b0c1;
|
|
|
|
return Vector3.Distance(c0, c1);
|
|
}
|
|
|
|
private bool PinchHasGoodVisibility(Handedness handedness)
|
|
{
|
|
if (_hmd == null
|
|
|| !_hmd.TryGetRootPose(out Pose centerEyePose))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
Vector3 handVector = _rootPose.rotation *
|
|
(handedness == Handedness.Left ? Constants.LeftPinkySide : Constants.RightPinkySide);
|
|
Vector3 targetVector = centerEyePose.forward;
|
|
|
|
float angle = Vector3.Angle(handVector, targetVector);
|
|
return angle <= PINCH_HQ_VIEW_ANGLE_THRESHOLD;
|
|
}
|
|
}
|
|
}
|