427 lines
14 KiB
C#
427 lines
14 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 System.Collections.Generic;
|
||
using System.Text;
|
||
using UnityEngine;
|
||
|
||
namespace VIVE.OpenXR.Toolkits.RealisticHandInteraction
|
||
{
|
||
/// <summary>
|
||
/// This class is designed to implement IHandGrabbable, allowing objects to be grabbed.
|
||
/// </summary>
|
||
public class HandGrabInteractable : MonoBehaviour, IHandGrabbable
|
||
{
|
||
#region Log
|
||
|
||
const string LOG_TAG = "VIVE.OpenXR.Toolkits.RealisticHandInteraction.HandGrabInteractable";
|
||
private StringBuilder m_sb = null;
|
||
internal StringBuilder sb
|
||
{
|
||
get
|
||
{
|
||
if (m_sb == null) { m_sb = new StringBuilder(); }
|
||
return m_sb;
|
||
}
|
||
}
|
||
private void DEBUG(string msg) { Debug.Log($"{LOG_TAG}, {msg}"); }
|
||
private void WARNING(string msg) { Debug.LogWarning($"{LOG_TAG}, {msg}"); }
|
||
private void ERROR(string msg) { Debug.LogError($"{LOG_TAG}, {msg}"); }
|
||
int logFrame = 0;
|
||
bool printIntervalLog => logFrame == 0;
|
||
|
||
#endregion
|
||
|
||
#region Interface Implement
|
||
private HandGrabInteractor m_Grabber = null;
|
||
public IGrabber grabber => m_Grabber;
|
||
|
||
public bool isGrabbed => m_Grabber != null;
|
||
|
||
[SerializeField]
|
||
private bool m_IsGrabbable = true;
|
||
public bool isGrabbable { get { return m_IsGrabbable; } set { m_IsGrabbable = value; } }
|
||
|
||
[SerializeField]
|
||
private FingerRequirement m_FingerRequirement;
|
||
public FingerRequirement fingerRequirement => m_FingerRequirement;
|
||
|
||
[SerializeField]
|
||
HandGrabbableEvent m_OnBeginGrabbed = new HandGrabbableEvent();
|
||
public HandGrabbableEvent onBeginGrabbed => m_OnBeginGrabbed;
|
||
|
||
[SerializeField]
|
||
HandGrabbableEvent m_OnEndGrabbed = new HandGrabbableEvent();
|
||
public HandGrabbableEvent onEndGrabbed => m_OnEndGrabbed;
|
||
#endregion
|
||
|
||
#region Public State
|
||
|
||
[SerializeField]
|
||
private Rigidbody m_Rigidbody = null;
|
||
#pragma warning disable
|
||
public Rigidbody rigidbody => m_Rigidbody;
|
||
#pragma warning enable
|
||
|
||
[SerializeField]
|
||
private List<GrabPose> m_GrabPoses = new List<GrabPose>();
|
||
public List<GrabPose> grabPoses => m_GrabPoses;
|
||
|
||
private GrabPose m_BestGrabPose = GrabPose.Identity;
|
||
public GrabPose bestGrabPose => m_BestGrabPose;
|
||
|
||
#endregion
|
||
|
||
[SerializeField]
|
||
private bool m_ShowAllIndicator = false;
|
||
private List<Collider> allColliders = new List<Collider>();
|
||
private HandGrabInteractor closestGrabber = null;
|
||
|
||
[SerializeField]
|
||
private IOneHandContraintMovement m_OneHandContraintMovement;
|
||
public IOneHandContraintMovement oneHandContraintMovement { get { return m_OneHandContraintMovement; } set { m_OneHandContraintMovement = value; } }
|
||
public bool isContraint => m_OneHandContraintMovement != null;
|
||
|
||
#pragma warning disable
|
||
[SerializeField]
|
||
private int m_PreviewIndex = -1;
|
||
#pragma warning enable
|
||
private RaycastHit[] hitResults = new RaycastHit[10];
|
||
|
||
#region MonoBehaviour
|
||
private void Awake()
|
||
{
|
||
allColliders.AddRange(transform.GetComponentsInChildren<Collider>(true));
|
||
}
|
||
|
||
private void OnEnable()
|
||
{
|
||
GrabManager.RegisterGrabbable(this);
|
||
Initialize();
|
||
}
|
||
|
||
private void OnDisable()
|
||
{
|
||
GrabManager.UnregisterGrabbable(this);
|
||
}
|
||
#endregion
|
||
|
||
#region Public Interface
|
||
/// <summary>
|
||
/// Set the grabber for the hand grabbable object.
|
||
/// </summary>
|
||
/// <param name="grabber">The grabber to set.</param>
|
||
public void SetGrabber(IGrabber grabber)
|
||
{
|
||
if (grabber is HandGrabInteractor handGrabber)
|
||
{
|
||
m_Grabber = handGrabber;
|
||
HandPose handPose = HandPoseProvider.GetHandPose(handGrabber.isLeft ? HandPoseType.HAND_LEFT : HandPoseType.HAND_RIGHT);
|
||
handPose.GetPosition(JointType.Wrist, out Vector3 wristPos);
|
||
handPose.GetRotation(JointType.Wrist, out Quaternion wristRot);
|
||
UpdateBestGrabPose(handGrabber.isLeft, new Pose(wristPos, wristRot));
|
||
m_OnBeginGrabbed?.Invoke(this);
|
||
DEBUG($"{transform.name} is grabbed by {handGrabber.name}");
|
||
}
|
||
else
|
||
{
|
||
m_Grabber = null;
|
||
m_BestGrabPose = GrabPose.Identity;
|
||
m_OnEndGrabbed?.Invoke(this);
|
||
DEBUG($"{transform.name} is released.");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Enable/Disable indicators. If enabled, display the closest indicator based on grabber position.
|
||
/// </summary>
|
||
/// <param name="enable">True to show the indicator, false to hide it.</param>
|
||
/// <param name="grabber">The grabber for which to show or hide this indicator.</param>
|
||
public void ShowIndicator(bool enable, HandGrabInteractor grabber)
|
||
{
|
||
if (enable)
|
||
{
|
||
closestGrabber = grabber;
|
||
if (m_ShowAllIndicator)
|
||
{
|
||
ShowAllIndicator(grabber.isLeft);
|
||
}
|
||
else
|
||
{
|
||
HandPose handPose = HandPoseProvider.GetHandPose(grabber.isLeft ? HandPoseType.HAND_LEFT : HandPoseType.HAND_RIGHT);
|
||
handPose.GetPosition(JointType.Wrist, out Vector3 wristPos);
|
||
handPose.GetRotation(JointType.Wrist, out Quaternion wristRot);
|
||
int index = FindBestGrabPose(grabber.isLeft, new Pose(wristPos, wristRot));
|
||
ShowIndicatorByIndex(index);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (closestGrabber == grabber)
|
||
{
|
||
closestGrabber = null;
|
||
ShowIndicatorByIndex(-1);
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Calculate the shortest distance between the grabber and the grabbable and convert it into a score based on grabDistance.
|
||
/// </summary>
|
||
/// <param name="grabberPos">The current pose of grabber.</param>
|
||
/// <param name="grabDistance">The maximum grab distance between the grabber and the grabbable object.</param>
|
||
/// <returns>The score represents the distance between the grabber and the grabbable.</returns>
|
||
public float CalculateDistanceScore(Vector3 grabberPos, float grabDistance = 0.03f)
|
||
{
|
||
if (!isGrabbable || isGrabbed) { return 0; }
|
||
Vector3 closestPoint = GetClosestPoint(grabberPos);
|
||
float distanceSqr = (grabberPos - closestPoint).sqrMagnitude;
|
||
float grabDistSqr = grabDistance * grabDistance;
|
||
return distanceSqr > grabDistSqr ? 0 : 1 - (distanceSqr / grabDistSqr);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Update the position and rotation of the self with the pose of the hand that is grabbing it.
|
||
/// </summary>
|
||
/// <param name="grabberPose">The pose of the hand.</param>
|
||
public void UpdatePositionAndRotation(Pose grabberPose)
|
||
{
|
||
if (m_OneHandContraintMovement == null)
|
||
{
|
||
Quaternion handRotDiff = grabberPose.rotation * Quaternion.Inverse(m_BestGrabPose.grabOffset.sourceRotation);
|
||
transform.position = grabberPose.position + handRotDiff * m_BestGrabPose.grabOffset.posOffset;
|
||
transform.rotation = grabberPose.rotation * m_BestGrabPose.grabOffset.rotOffset;
|
||
}
|
||
|
||
if (m_OneHandContraintMovement != null)
|
||
{
|
||
m_OneHandContraintMovement.UpdatePose(grabberPose);
|
||
UpdateMeshPoseByGrabbable();
|
||
}
|
||
}
|
||
#endregion
|
||
|
||
/// <summary>
|
||
/// Generate all indicators and calculate grab offsets.
|
||
/// </summary>
|
||
private void Initialize()
|
||
{
|
||
if (m_OneHandContraintMovement != null)
|
||
{
|
||
m_OneHandContraintMovement.Initialize(this);
|
||
onBeginGrabbed.AddListener(m_OneHandContraintMovement.OnBeginGrabbed);
|
||
onEndGrabbed.AddListener(m_OneHandContraintMovement.OnEndGrabbed);
|
||
}
|
||
|
||
for (int i = 0; i < m_GrabPoses.Count; i++)
|
||
{
|
||
if (m_GrabPoses[i].indicator.enableIndicator || m_ShowAllIndicator)
|
||
{
|
||
if (m_GrabPoses[i].indicator.NeedGenerateIndicator())
|
||
{
|
||
AutoGenerateIndicator(i);
|
||
}
|
||
else
|
||
{
|
||
GrabPose grabPose = m_GrabPoses[i];
|
||
grabPose.indicator.CalculateGrabOffset(transform.position, transform.rotation);
|
||
m_GrabPoses[i] = grabPose;
|
||
}
|
||
}
|
||
}
|
||
ShowIndicatorByIndex(-1);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Automatically generate an indicator by the index of the grab pose.
|
||
/// </summary>
|
||
/// <param name="index">The index of the grab pose.</param>
|
||
private void AutoGenerateIndicator(int index)
|
||
{
|
||
AutoGenIndicator autoGenIndicator = new GameObject($"Indicator {index}", typeof(AutoGenIndicator)).GetComponent<AutoGenIndicator>();
|
||
GrabPose grabPose = m_GrabPoses[index];
|
||
Pose handPose = CalculateActualHandPose(grabPose.grabOffset);
|
||
Vector3 closestPoint = GetClosestPoint(handPose.position);
|
||
autoGenIndicator.SetPose(closestPoint, closestPoint - transform.position);
|
||
grabPose.indicator.Update(true, true, autoGenIndicator.gameObject);
|
||
grabPose.indicator.CalculateGrabOffset(transform.position, transform.rotation);
|
||
m_GrabPoses[index] = grabPose;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Calculate the point closest to the source position.
|
||
/// </summary>
|
||
/// <param name="sourcePos">The position of source.</param>
|
||
/// <returns>The position which closest to the source position.</returns>
|
||
private Vector3 GetClosestPoint(Vector3 sourcePos)
|
||
{
|
||
Vector3 closestPoint = Vector3.zero;
|
||
float shortDistanceSqr = float.MaxValue;
|
||
for (int i = 0; i < allColliders.Count; i++)
|
||
{
|
||
Collider collider = allColliders[i];
|
||
Vector3 closePoint = collider.ClosestPointOnBounds(sourcePos);
|
||
float distanceSqr = (sourcePos - closePoint).sqrMagnitude;
|
||
if (distanceSqr < 0.001f)
|
||
{
|
||
return closePoint;
|
||
}
|
||
|
||
if (collider.bounds.Contains(closePoint))
|
||
{
|
||
Vector3 direction = closePoint - sourcePos;
|
||
direction.Normalize();
|
||
int hitCount = Physics.RaycastNonAlloc(sourcePos, direction, hitResults, Mathf.Sqrt(distanceSqr));
|
||
|
||
for (int j = 0; j < hitCount; j++)
|
||
{
|
||
RaycastHit hit = hitResults[j];
|
||
if (hit.collider == collider)
|
||
{
|
||
float hitDistanceSqr = (sourcePos - hit.point).sqrMagnitude;
|
||
if (distanceSqr > hitDistanceSqr)
|
||
{
|
||
distanceSqr = hitDistanceSqr;
|
||
closePoint = hit.point;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
if (shortDistanceSqr > distanceSqr)
|
||
{
|
||
shortDistanceSqr = distanceSqr;
|
||
closestPoint = closePoint;
|
||
}
|
||
}
|
||
return closestPoint;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Find the best grab pose for the grabber and updates the bestGrabPoseId.
|
||
/// </summary>
|
||
/// <param name="isLeft">Whether the grabber is the left hand.</param>
|
||
/// <param name="grabberPose">The pose of the grabber.</param>
|
||
/// <returns>True if a best grab pose is found; otherwise, false.</returns>
|
||
private void UpdateBestGrabPose(bool isLeft, Pose grabberPose)
|
||
{
|
||
int index = FindBestGrabPose(isLeft, grabberPose);
|
||
if (index != -1 && index < m_GrabPoses.Count)
|
||
{
|
||
m_BestGrabPose = m_GrabPoses[index];
|
||
}
|
||
else
|
||
{
|
||
m_BestGrabPose.grabOffset = new GrabOffset(grabberPose.position, grabberPose.rotation, transform.position, transform.rotation);
|
||
HandPose handPose = HandPoseProvider.GetHandPose(isLeft ? HandPoseType.MESH_LEFT : HandPoseType.MESH_RIGHT);
|
||
if (handPose is MeshHandPose meshHandPose)
|
||
{
|
||
Quaternion[] grabRotations = new Quaternion[(int)JointType.Count];
|
||
for (int i = 0; i < (int)JointType.Count; i++)
|
||
{
|
||
meshHandPose.GetRotation((JointType)i, out grabRotations[i], local: true);
|
||
}
|
||
m_BestGrabPose.recordedGrabRotations = grabRotations;
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Find the best grab pose for the grabber.
|
||
/// </summary>
|
||
/// <param name="isLeft">Whether the grabber is the left hand.</param>
|
||
/// <param name="grabberPose">The pose of the grabber.</param>
|
||
/// <returns>The index of the best grab pose among the grab poses.</returns>
|
||
private int FindBestGrabPose(bool isLeft, Pose grabberPose)
|
||
{
|
||
int index = -1;
|
||
float maxDot = float.MinValue;
|
||
Vector3 currentDirection = grabberPose.position - transform.position;
|
||
for (int i = 0; i < m_GrabPoses.Count; i++)
|
||
{
|
||
if (m_GrabPoses[i].isLeft == isLeft)
|
||
{
|
||
Pose handPose = CalculateActualHandPose(m_GrabPoses[i].grabOffset);
|
||
Vector3 grabDirection = handPose.position - transform.position;
|
||
float dot = Vector3.Dot(currentDirection.normalized, grabDirection.normalized);
|
||
if (dot > maxDot)
|
||
{
|
||
maxDot = dot;
|
||
index = i;
|
||
}
|
||
}
|
||
}
|
||
return index;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Show the indicator corresponding to the specified index and hides others.
|
||
/// </summary>
|
||
/// <param name="index">The index of the indicator to show.</param>
|
||
private void ShowIndicatorByIndex(int index)
|
||
{
|
||
for (int i = 0; i < m_GrabPoses.Count; i++)
|
||
{
|
||
if (index != i)
|
||
{
|
||
m_GrabPoses[i].indicator.SetActive(false);
|
||
}
|
||
}
|
||
if (index >= 0 && index < m_GrabPoses.Count &&
|
||
m_GrabPoses[index].indicator.enableIndicator)
|
||
{
|
||
m_GrabPoses[index].indicator.UpdatePositionAndRotation(transform.position, transform.rotation);
|
||
m_GrabPoses[index].indicator.SetActive(true);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Show all indicators corresponding to the specified hand side and hides others.
|
||
/// </summary>
|
||
/// <param name="isLeft">Whether the hand side is left.</param>
|
||
private void ShowAllIndicator(bool isLeft)
|
||
{
|
||
for (int i = 0; i < m_GrabPoses.Count; i++)
|
||
{
|
||
m_GrabPoses[i].indicator.SetActive(false);
|
||
}
|
||
foreach (var grabPose in m_GrabPoses)
|
||
{
|
||
if (grabPose.isLeft == isLeft)
|
||
{
|
||
grabPose.indicator.UpdatePositionAndRotation(transform.position, transform.rotation);
|
||
grabPose.indicator.SetActive(true);
|
||
}
|
||
}
|
||
}
|
||
|
||
private void UpdateMeshPoseByGrabbable()
|
||
{
|
||
if (grabber is HandGrabInteractor handGrabber)
|
||
{
|
||
HandPose handPose = HandPoseProvider.GetHandPose(handGrabber.isLeft ? HandPoseType.MESH_LEFT : HandPoseType.MESH_RIGHT);
|
||
if (handPose != null && handPose is MeshHandPose meshHandPose)
|
||
{
|
||
Pose realHandPose = CalculateActualHandPose(m_BestGrabPose.grabOffset);
|
||
meshHandPose.SetJointPose(JointType.Wrist, realHandPose);
|
||
}
|
||
}
|
||
}
|
||
|
||
private Pose CalculateActualHandPose(GrabOffset grabOffset)
|
||
{
|
||
Quaternion handRot = transform.rotation * Quaternion.Inverse(grabOffset.rotOffset);
|
||
Quaternion handRotDiff = handRot * Quaternion.Inverse(grabOffset.sourceRotation);
|
||
Vector3 handPos = transform.position - handRotDiff * grabOffset.posOffset;
|
||
return new Pose(handPos, handRot);
|
||
}
|
||
}
|
||
}
|