127 lines
3.8 KiB
C#
127 lines
3.8 KiB
C#
using System.Text;
|
|
using UnityEngine;
|
|
|
|
namespace VIVE.OpenXR.Toolkits.RealisticHandInteraction
|
|
{
|
|
public class OneGrabRotateConstraint : IOneHandContraintMovement
|
|
{
|
|
#region Log
|
|
|
|
const string LOG_TAG = "VIVE.OpenXR.Toolkits.RealisticHandInteraction.OneGrabRotateConstraint";
|
|
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
|
|
|
|
private enum RotationAxis
|
|
{
|
|
XAxis = 0,
|
|
YAxis = 1,
|
|
ZAxis = 2,
|
|
}
|
|
|
|
[SerializeField]
|
|
private Transform m_Constraint;
|
|
[SerializeField]
|
|
private Transform m_Pivot;
|
|
[SerializeField]
|
|
private RotationAxis m_RotationAxis = RotationAxis.XAxis;
|
|
[SerializeField]
|
|
private ConstraintInfo m_ClockwiseAngle = ConstraintInfo.Identity;
|
|
public float clockwiseAngle => m_ClockwiseAngle.value;
|
|
[SerializeField]
|
|
private ConstraintInfo m_CounterclockwiseAngle = ConstraintInfo.Identity;
|
|
public float counterclockwiseAngle => m_CounterclockwiseAngle.value;
|
|
private float m_TotalDegrees = 0.0f;
|
|
public float totalDegrees => m_TotalDegrees;
|
|
private Pose previousHandPose = Pose.identity;
|
|
|
|
public override void Initialize(IGrabbable grabbable)
|
|
{
|
|
if (grabbable is HandGrabInteractable handGrabbable)
|
|
{
|
|
if (m_Constraint == null)
|
|
{
|
|
m_Constraint = handGrabbable.transform;
|
|
WARNING("Since no constraint object is set, self will be used as the constraint object.");
|
|
}
|
|
if (m_Pivot == null)
|
|
{
|
|
m_Pivot = handGrabbable.transform;
|
|
WARNING("Since no pivot is set, self will be used as the pivot.");
|
|
}
|
|
}
|
|
}
|
|
|
|
public override void OnBeginGrabbed(IGrabbable grabbable)
|
|
{
|
|
if (grabbable.grabber is HandGrabInteractor 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);
|
|
previousHandPose = new Pose(wristPos, wristRot);
|
|
}
|
|
}
|
|
|
|
public override void UpdatePose(Pose handPose)
|
|
{
|
|
if (previousHandPose == Pose.identity)
|
|
{
|
|
previousHandPose = handPose;
|
|
return;
|
|
}
|
|
|
|
Vector3 axis = Vector3.zero;
|
|
switch (m_RotationAxis)
|
|
{
|
|
case RotationAxis.XAxis: axis = Vector3.right; break;
|
|
case RotationAxis.YAxis: axis = Vector3.up; break;
|
|
case RotationAxis.ZAxis: axis = Vector3.forward; break;
|
|
}
|
|
Vector3 worldAxis = m_Pivot.TransformDirection(axis);
|
|
|
|
Vector3 previousOffset = previousHandPose.position - m_Pivot.position;
|
|
Vector3 previousVector = Vector3.ProjectOnPlane(previousOffset, worldAxis);
|
|
|
|
Vector3 targetOffset = handPose.position - m_Pivot.position;
|
|
Vector3 targetVector = Vector3.ProjectOnPlane(targetOffset, worldAxis);
|
|
|
|
float angleDelta = Vector3.Angle(previousVector, targetVector);
|
|
angleDelta *= Vector3.Dot(Vector3.Cross(previousVector, targetVector), worldAxis) > 0.0f ? 1.0f : -1.0f;
|
|
|
|
float previousAngle = m_TotalDegrees;
|
|
m_TotalDegrees += angleDelta;
|
|
if (m_CounterclockwiseAngle.enableConstraint)
|
|
{
|
|
m_TotalDegrees = Mathf.Max(m_TotalDegrees, -m_CounterclockwiseAngle.value);
|
|
}
|
|
if (m_ClockwiseAngle.enableConstraint)
|
|
{
|
|
m_TotalDegrees = Mathf.Min(m_TotalDegrees, m_ClockwiseAngle.value);
|
|
}
|
|
angleDelta = m_TotalDegrees - previousAngle;
|
|
m_Constraint.RotateAround(m_Pivot.position, worldAxis, angleDelta);
|
|
|
|
previousHandPose = handPose;
|
|
}
|
|
|
|
public override void OnEndGrabbed(IGrabbable grabbable)
|
|
{
|
|
previousHandPose = Pose.identity;
|
|
}
|
|
}
|
|
}
|