391 lines
11 KiB
C#
391 lines
11 KiB
C#
// "Wave SDK
|
||
// © 2020 HTC Corporation. All Rights Reserved.
|
||
//
|
||
// Unless otherwise required by copyright law and practice,
|
||
// upon the execution of HTC SDK license agreement,
|
||
// HTC grants you access to and use of the Wave SDK(s).
|
||
// You shall fully comply with all of HTC’s SDK license agreement terms and
|
||
// conditions signed by you and all SDK and API requirements,
|
||
// specifications, and documentation provided by HTC to You."
|
||
|
||
using UnityEngine;
|
||
|
||
namespace VIVE.OpenXR.Toolkits.RealisticHandInteraction
|
||
{
|
||
/// <summary>
|
||
/// This class is designed to implement IHandGrabber, allowing objects to grab grabbable objects.
|
||
/// </summary>
|
||
public class HandGrabInteractor : MonoBehaviour, IHandGrabber
|
||
{
|
||
private enum GrabState
|
||
{
|
||
None,
|
||
Hover,
|
||
Grabbing,
|
||
};
|
||
|
||
#region Public States
|
||
private HandGrabInteractable m_Grabbable = null;
|
||
public IGrabbable grabbable => m_Grabbable;
|
||
public bool isGrabbing => m_Grabbable != null;
|
||
|
||
[SerializeField]
|
||
private Handedness m_Handedness = Handedness.Left;
|
||
public Handedness handedness => m_Handedness;
|
||
|
||
private HandGrabState m_HandGrabState = null;
|
||
public HandGrabState handGrabState => m_HandGrabState;
|
||
|
||
[SerializeField]
|
||
private float m_GrabDistance = 0.03f;
|
||
public float grabDistance { get { return m_GrabDistance; } set { m_GrabDistance = value; } }
|
||
|
||
[SerializeField]
|
||
private bool m_EnableCollider = true;
|
||
public bool enableCollider
|
||
{
|
||
get { return m_EnableCollider; }
|
||
set
|
||
{
|
||
m_EnableCollider = value;
|
||
if (colliderManager != null)
|
||
{
|
||
colliderManager.EnableCollider(m_EnableCollider);
|
||
}
|
||
}
|
||
}
|
||
|
||
public bool isLeft => handedness == Handedness.Left;
|
||
#endregion
|
||
|
||
[SerializeField]
|
||
private GrabColliderManager colliderManager;
|
||
|
||
private readonly float MinGrabScore = 0.25f;
|
||
private readonly float MinDistanceScore = 0.25f;
|
||
private HandGrabInteractable currentCaidate = null;
|
||
private GrabPose grabPose = GrabPose.Identity;
|
||
private GrabState m_State = GrabState.None;
|
||
private Pose[] fingerTipPoses => new Pose[]
|
||
{
|
||
m_HandGrabState.GetJointPose(JointType.Thumb_Tip),
|
||
m_HandGrabState.GetJointPose(JointType.Index_Tip),
|
||
m_HandGrabState.GetJointPose(JointType.Middle_Tip),
|
||
m_HandGrabState.GetJointPose(JointType.Ring_Tip),
|
||
m_HandGrabState.GetJointPose(JointType.Pinky_Tip),
|
||
};
|
||
private Pose palmPose => m_HandGrabState.GetJointPose(JointType.Palm);
|
||
private Pose[] frozenPoses = new Pose[(int)JointType.Count];
|
||
private bool isFrozen = false;
|
||
private OnBeginGrab beginGrabHandler;
|
||
private OnEndGrab endGrabHandler;
|
||
|
||
#region MonoBehaviour
|
||
private void Awake()
|
||
{
|
||
m_HandGrabState = new HandGrabState(isLeft);
|
||
}
|
||
|
||
private void Start()
|
||
{
|
||
if (colliderManager != null)
|
||
{
|
||
colliderManager.EnableCollider(m_EnableCollider);
|
||
}
|
||
}
|
||
|
||
private void OnEnable()
|
||
{
|
||
GrabManager.RegisterGrabber(this);
|
||
if (colliderManager != null)
|
||
{
|
||
colliderManager.AddImmovableCollisionListener(FreezeHandPose);
|
||
}
|
||
}
|
||
|
||
private void OnDisable()
|
||
{
|
||
GrabManager.UnregisterGrabber(this);
|
||
if (colliderManager != null)
|
||
{
|
||
colliderManager.RemoveImmovableCollisionListener(FreezeHandPose);
|
||
}
|
||
}
|
||
|
||
private void Update()
|
||
{
|
||
m_HandGrabState.UpdateState();
|
||
if (isFrozen)
|
||
{
|
||
return;
|
||
}
|
||
|
||
if (m_State != GrabState.Grabbing)
|
||
{
|
||
FindCandidate();
|
||
}
|
||
|
||
switch (m_State)
|
||
{
|
||
case GrabState.None:
|
||
NoneUpdate();
|
||
break;
|
||
case GrabState.Hover:
|
||
HoverUpdate();
|
||
break;
|
||
case GrabState.Grabbing:
|
||
GrabbingUpdate();
|
||
break;
|
||
}
|
||
}
|
||
#endregion
|
||
|
||
#region Public Interface
|
||
/// <summary>
|
||
/// Get the current joint pose of the grabber.
|
||
/// </summary>
|
||
/// <param name="jointId">The id of the joint for which to get the pose.</param>
|
||
/// <returns>The current pose of the specified joint.</returns>
|
||
public Pose GetCurrentJointPose(int jointId)
|
||
{
|
||
if (isFrozen)
|
||
{
|
||
return frozenPoses[jointId];
|
||
}
|
||
return m_HandGrabState.GetJointPose(jointId);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Get the rotation of the joint in the grab pose.
|
||
/// </summary>
|
||
/// <param name="jointId">The id of the joint for which to get the rotation.</param>
|
||
/// <returns>The rotation of the joint in the grab pose.</returns>
|
||
public bool GetGrabPoseJointRotation(int jointId, out Quaternion rotation)
|
||
{
|
||
rotation = Quaternion.identity;
|
||
if (m_Grabbable == null) { return false; }
|
||
if (jointId >= 0 && grabPose.recordedGrabRotations.Length > jointId)
|
||
{
|
||
rotation = grabPose.recordedGrabRotations[jointId];
|
||
return true;
|
||
}
|
||
else if (grabPose.handGrabGesture != HandGrabGesture.Identity)
|
||
{
|
||
rotation = m_HandGrabState.GetDefaultJointRotationInGesture(grabPose.handGrabGesture, jointId);
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Check if the specific joint is necessary for grabbing.
|
||
/// </summary>
|
||
/// <param name="joint">JointType of the specified joint.</param>
|
||
/// <returns>Return true if this joint is needed for grabbing, otherwise false.</returns>
|
||
public bool IsRequiredJoint(JointType joint)
|
||
{
|
||
if (m_Grabbable != null)
|
||
{
|
||
GetJointIndex(joint, out int group, out _);
|
||
switch (group)
|
||
{
|
||
case 2: return m_Grabbable.fingerRequirement.thumb == GrabRequirement.Required;
|
||
case 3: return m_Grabbable.fingerRequirement.index == GrabRequirement.Required;
|
||
case 4: return m_Grabbable.fingerRequirement.middle == GrabRequirement.Required;
|
||
case 5: return m_Grabbable.fingerRequirement.ring == GrabRequirement.Required;
|
||
case 6: return m_Grabbable.fingerRequirement.pinky == GrabRequirement.Required;
|
||
}
|
||
}
|
||
return false;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Add a listener for the event triggered when the grabber begins grabbing.
|
||
/// </summary>
|
||
/// <param name="handler">The method to be called when the grabber begins grabbing.</param>
|
||
public void AddBeginGrabListener(OnBeginGrab handler)
|
||
{
|
||
beginGrabHandler += handler;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Remove a listener for the event triggered when the grabber begins grabbing.
|
||
/// </summary>
|
||
/// <param name="handler">The method to be removed from the event listeners.</param>
|
||
public void RemoveBeginGrabListener(OnBeginGrab handler)
|
||
{
|
||
beginGrabHandler -= handler;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Add a listener for the event triggered when the grabber ends grabbing.
|
||
/// </summary>
|
||
/// <param name="handler">The method to be called when the grabber ends grabbing.</param>
|
||
public void AddEndGrabListener(OnEndGrab handler)
|
||
{
|
||
endGrabHandler += handler;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Remove a listener for the event triggered when the grabber ends grabbing.
|
||
/// </summary>
|
||
/// <param name="handler">The method to be removed from the event listeners.</param>
|
||
public void RemoveEndGrabListener(OnEndGrab handler)
|
||
{
|
||
endGrabHandler -= handler;
|
||
}
|
||
#endregion
|
||
|
||
/// <summary>
|
||
/// Find the candidate grabbable object for grabber.
|
||
/// </summary>
|
||
private void FindCandidate()
|
||
{
|
||
float distanceScore = float.MinValue;
|
||
if (GetClosestGrabbable(m_GrabDistance, out HandGrabInteractable grabbable, out float score) && score > distanceScore)
|
||
{
|
||
distanceScore = score;
|
||
currentCaidate = grabbable;
|
||
}
|
||
|
||
if (currentCaidate != null)
|
||
{
|
||
float grabScore = Grab.CalculateHandGrabScore(this, currentCaidate);
|
||
if (distanceScore < MinDistanceScore || grabScore < MinGrabScore)
|
||
{
|
||
currentCaidate = null;
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Get the closest grabbable object for grabber.
|
||
/// </summary>
|
||
/// <param name="grabDistance">The maximum grab distance between the grabber and the grabbable object.</param>
|
||
/// <param name="grabbable">The closest grabbable object.</param>
|
||
/// <param name="maxScore">The maximum score indicating the closeness of the grabbable object.</param>
|
||
/// <returns>True if a grabbable object is found within the grab distance; otherwise, false.</returns>
|
||
private bool GetClosestGrabbable(float grabDistance, out HandGrabInteractable grabbable, out float maxScore)
|
||
{
|
||
grabbable = null;
|
||
maxScore = 0f;
|
||
foreach (HandGrabInteractable interactable in GrabManager.handGrabbables)
|
||
{
|
||
interactable.ShowIndicator(false, this);
|
||
foreach (Pose fingerTipPose in fingerTipPoses)
|
||
{
|
||
float distanceScore = interactable.CalculateDistanceScore(fingerTipPose.position, grabDistance);
|
||
if (distanceScore > maxScore)
|
||
{
|
||
maxScore = distanceScore;
|
||
grabbable = interactable;
|
||
}
|
||
}
|
||
}
|
||
if (grabbable != null)
|
||
{
|
||
grabbable.ShowIndicator(true, this);
|
||
}
|
||
return grabbable != null;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Set the state to GrabState.Hover if a candidate is found.
|
||
/// </summary>
|
||
private void NoneUpdate()
|
||
{
|
||
if (currentCaidate != null)
|
||
{
|
||
m_State = GrabState.Hover;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Update the state and related information when the grabber begins grabbing the grabbable.
|
||
/// </summary>
|
||
private void HoverUpdate()
|
||
{
|
||
if (currentCaidate == null)
|
||
{
|
||
m_State = GrabState.None;
|
||
return;
|
||
}
|
||
|
||
if (Grab.HandBeginGrab(this, currentCaidate))
|
||
{
|
||
m_State = GrabState.Grabbing;
|
||
|
||
m_Grabbable = currentCaidate;
|
||
m_Grabbable.SetGrabber(this);
|
||
m_Grabbable.ShowIndicator(false, this);
|
||
grabPose = m_Grabbable.bestGrabPose;
|
||
if (grabPose == GrabPose.Identity)
|
||
{
|
||
Vector3 posOffset = m_Grabbable.transform.position - palmPose.position;
|
||
Quaternion rotOffset = palmPose.rotation;
|
||
grabPose.grabOffset = new GrabOffset(m_Grabbable.transform.position, m_Grabbable.transform.rotation, posOffset, rotOffset);
|
||
}
|
||
beginGrabHandler?.Invoke(this);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Update the position of grabbable object according to the movement of the grabber.
|
||
/// </summary>
|
||
private void GrabbingUpdate()
|
||
{
|
||
if (Grab.HandDoneGrab(this, m_Grabbable) || !Grab.HandIsGrabbing(this, m_Grabbable))
|
||
{
|
||
m_Grabbable.SetGrabber(null);
|
||
m_Grabbable = null;
|
||
m_State = GrabState.Hover;
|
||
endGrabHandler?.Invoke(this);
|
||
return;
|
||
}
|
||
|
||
Quaternion handRotOffset = palmPose.rotation * Quaternion.Inverse(grabPose.grabOffset.rotation);
|
||
Vector3 currentPos = palmPose.position + handRotOffset * grabPose.grabOffset.position;
|
||
Quaternion currentRot = handRotOffset * grabPose.grabOffset.targetRotation;
|
||
m_Grabbable.transform.SetPositionAndRotation(currentPos, currentRot);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Freezes or unfreezes the hand pose.
|
||
/// </summary>
|
||
/// <param name="enable">True to freeze the hand pose; False to unfreeze.</param>
|
||
private void FreezeHandPose(bool enable)
|
||
{
|
||
isFrozen = enable;
|
||
if (isFrozen)
|
||
{
|
||
for (int i = 0; i < frozenPoses.Length; i++)
|
||
{
|
||
frozenPoses[i] = m_HandGrabState.GetJointPose(i);
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Get the position of a specific joint.
|
||
/// </summary>
|
||
/// <param name="joint">The type of joint to get.</param>
|
||
/// <param name="position">The reference to store the position of the joint.</param>
|
||
/// <returns>True if the joint position is successfully retrieved; otherwise, false.</returns>
|
||
private void GetJointIndex(JointType joint, out int group, out int index)
|
||
{
|
||
int jointId = (int)joint + 1;
|
||
group = 0;
|
||
index = jointId;
|
||
|
||
// palm, wrist, thumb, index, middle, ring, pinky
|
||
int[] fingerGroup = { 1, 1, 4, 5, 5, 5, 5 };
|
||
while (index > fingerGroup[group])
|
||
{
|
||
index -= fingerGroup[group];
|
||
group += 1;
|
||
}
|
||
index -= 1;
|
||
}
|
||
}
|
||
}
|