Add ultimate xr
This commit is contained in:
562
Assets/UltimateXR/Runtime/Scripts/Animation/IK/UxrArmIKSolver.cs
Normal file
562
Assets/UltimateXR/Runtime/Scripts/Animation/IK/UxrArmIKSolver.cs
Normal file
@@ -0,0 +1,562 @@
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
// <copyright file="UxrArmIKSolver.cs" company="VRMADA">
|
||||
// Copyright (c) VRMADA, All rights reserved.
|
||||
// </copyright>
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
using UltimateXR.Avatar;
|
||||
using UltimateXR.Avatar.Rig;
|
||||
using UltimateXR.Core;
|
||||
using UltimateXR.Core.Math;
|
||||
using UltimateXR.Core.Settings;
|
||||
using UltimateXR.Extensions.Unity;
|
||||
using UnityEngine;
|
||||
|
||||
namespace UltimateXR.Animation.IK
|
||||
{
|
||||
/// <summary>
|
||||
/// IK component that implements basic Inverse Kinematics for an arm.
|
||||
/// </summary>
|
||||
public class UxrArmIKSolver : UxrIKSolver
|
||||
{
|
||||
#region Inspector Properties/Serialized Fields
|
||||
|
||||
[Header("General")] [SerializeField] private UxrArmOverExtendMode _overExtendMode = UxrArmOverExtendMode.LimitHandReach;
|
||||
|
||||
[Header("Clavicle")] [SerializeField] [Range(0, 1)] private float _clavicleDeformation = DefaultClavicleDeformation;
|
||||
[SerializeField] private float _clavicleRangeOfMotionAngle = DefaultClavicleRangeOfMotionAngle;
|
||||
[SerializeField] private bool _clavicleAutoComputeBias = true;
|
||||
[SerializeField] private Vector3 _clavicleDeformationAxesBias = Vector3.zero;
|
||||
[SerializeField] private Vector3 _clavicleDeformationAxesScale = new Vector3(1.0f, 0.8f, 1.0f);
|
||||
|
||||
[Header("Arm (shoulder), forearm & hand")] [SerializeField] private float _armRangeOfMotionAngle = DefaultArmRangeOfMotionAngle;
|
||||
[SerializeField] [Range(0, 1)] private float _relaxedElbowAperture = DefaultElbowAperture;
|
||||
[SerializeField] [Range(0, 1)] private float _elbowApertureRotation = DefaultElbowApertureRotation;
|
||||
|
||||
[SerializeField] private bool _smooth = true;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Public Types & Data
|
||||
|
||||
public const float DefaultClavicleDeformation = 0.4f;
|
||||
public const float DefaultClavicleRangeOfMotionAngle = 30.0f;
|
||||
public const float DefaultArmRangeOfMotionAngle = 100.0f;
|
||||
public const float DefaultElbowAperture = 0.5f;
|
||||
public const float DefaultElbowApertureRotation = 0.3f;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the clavicle bone.
|
||||
/// </summary>
|
||||
public Transform Clavicle { get; private set; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the arm bone.
|
||||
/// </summary>
|
||||
public Transform Arm { get; private set; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the forearm bone.
|
||||
/// </summary>
|
||||
public Transform Forearm { get; private set; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the hand bone.
|
||||
/// </summary>
|
||||
public Transform Hand { get; private set; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets whether it is the left or right arm.
|
||||
/// </summary>
|
||||
public UxrHandSide Side => _side;
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets how far [0.0, 1.0] the elbow will from the body when solving the IK. Lower values will bring the elbow
|
||||
/// closer to the body.
|
||||
/// </summary>
|
||||
public float RelaxedElbowAperture
|
||||
{
|
||||
get => _relaxedElbowAperture;
|
||||
set => _relaxedElbowAperture = value;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets what happens when the real hand makes the VR arm to over-extend. This may happen if the user has a
|
||||
/// longer arm than the VR model, if the controller is placed far away or if the avatar is grabbing an object with
|
||||
/// constraints that lock the hand position.
|
||||
/// </summary>
|
||||
public UxrArmOverExtendMode OverExtendMode
|
||||
{
|
||||
get => _overExtendMode;
|
||||
set => _overExtendMode = value;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Public Overrides UxrIKSolver
|
||||
|
||||
/// <inheritdoc />
|
||||
public override bool Initialized => _initialized;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Public Methods
|
||||
|
||||
/// <summary>
|
||||
/// Solves a pass in the Inverse Kinematics.
|
||||
/// </summary>
|
||||
/// <param name="armSolveOptions">Arm solving options</param>
|
||||
/// <param name="armOverExtendMode">What happens when the hand moves farther than the actual arm length</param>
|
||||
public void SolveIKPass(UxrArmSolveOptions armSolveOptions, UxrArmOverExtendMode armOverExtendMode)
|
||||
{
|
||||
if (Hand == null || Forearm == null || Arm == null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
Vector3 localClaviclePos = ToLocalAvatarPos(Clavicle.position);
|
||||
Vector3 localForearmPos = ToLocalAvatarPos(Forearm.position);
|
||||
Vector3 localHandPos = ToLocalAvatarPos(Hand.position);
|
||||
|
||||
if (Clavicle != null)
|
||||
{
|
||||
if (armSolveOptions.HasFlag(UxrArmSolveOptions.ResetClavicle))
|
||||
{
|
||||
Clavicle.transform.localRotation = _clavicleUniversalLocalAxes.InitialLocalRotation;
|
||||
}
|
||||
|
||||
if (armSolveOptions.HasFlag(UxrArmSolveOptions.SolveClavicle))
|
||||
{
|
||||
// Compute the rotation to make the clavicle look at the elbow.
|
||||
// Computations are performed in local avatar space to allow avatars with pitch/roll and improve precision.
|
||||
|
||||
Vector3 avatarClavicleLookAt = (localForearmPos - localClaviclePos).normalized;
|
||||
avatarClavicleLookAt = Vector3.Scale(avatarClavicleLookAt, _clavicleDeformationAxesScale) + _clavicleDeformationAxesBias;
|
||||
|
||||
Quaternion avatarClavicleRotation = ToLocalAvatarRot(Clavicle.rotation);
|
||||
|
||||
Quaternion avatarClavicleRotationLookAt = Quaternion.Slerp(avatarClavicleRotation,
|
||||
Quaternion.LookRotation(avatarClavicleLookAt) * _clavicleUniversalLocalAxes.UniversalToActualAxesRotation,
|
||||
_clavicleDeformation);
|
||||
|
||||
float deformationAngle = Quaternion.Angle(avatarClavicleRotationLookAt, avatarClavicleRotation);
|
||||
|
||||
if (deformationAngle > _clavicleRangeOfMotionAngle)
|
||||
{
|
||||
avatarClavicleRotationLookAt = Quaternion.Slerp(avatarClavicleRotation, avatarClavicleRotationLookAt, _clavicleRangeOfMotionAngle / deformationAngle);
|
||||
}
|
||||
|
||||
// Smooth out:
|
||||
|
||||
float totalDegrees = Quaternion.Angle(_lastClavicleLocalRotation, avatarClavicleRotationLookAt);
|
||||
float degreesRot = ClavicleMaxDegreesPerSecond * Time.deltaTime;
|
||||
|
||||
if (_smooth == false)
|
||||
{
|
||||
_lastClavicleRotationInitialized = false;
|
||||
}
|
||||
|
||||
if (_lastClavicleRotationInitialized == false || totalDegrees < 0.001f)
|
||||
{
|
||||
Clavicle.rotation = ToWorldRot(avatarClavicleRotationLookAt);
|
||||
}
|
||||
else
|
||||
{
|
||||
Clavicle.rotation = Quaternion.Slerp(ToWorldRot(_lastClavicleLocalRotation),
|
||||
ToWorldRot(avatarClavicleRotationLookAt),
|
||||
Mathf.Clamp01(degreesRot / totalDegrees));
|
||||
}
|
||||
}
|
||||
|
||||
Hand.position = ToWorldPos(localHandPos);
|
||||
}
|
||||
|
||||
// Find the plane of intersection between 2 spheres (sphere with "upper arm" radius and sphere with "forearm" radius).
|
||||
// Computations are performed in local avatar space to allow avatars with pitch/roll and improve precision.
|
||||
|
||||
localForearmPos = ToLocalAvatarPos(Forearm.position);
|
||||
Vector3 localArmPos = ToLocalAvatarPos(Arm.position);
|
||||
|
||||
float a = 2.0f * (localHandPos.x - localArmPos.x);
|
||||
float b = 2.0f * (localHandPos.y - localArmPos.y);
|
||||
float c = 2.0f * (localHandPos.z - localArmPos.z);
|
||||
float d = localArmPos.x * localArmPos.x - localHandPos.x * localHandPos.x + localArmPos.y * localArmPos.y - localHandPos.y * localHandPos.y +
|
||||
localArmPos.z * localArmPos.z - localHandPos.z * localHandPos.z - _upperArmLocalLength * _upperArmLocalLength + _forearmLocalLength * _forearmLocalLength;
|
||||
|
||||
// Find the center of the circle intersecting the 2 spheres. Check if the intersection exists (hand may be stretched over the limits)
|
||||
float t = (localArmPos.x * a + localArmPos.y * b + localArmPos.z * c + d) / (a * (localArmPos.x - localHandPos.x) + b * (localArmPos.y - localHandPos.y) + c * (localArmPos.z - localHandPos.z));
|
||||
|
||||
Vector3 localArmToCenter = (localHandPos - localArmPos) * t;
|
||||
Vector3 localCenter = localForearmPos;
|
||||
float safeDistance = 0.001f;
|
||||
float maxHandDistance = _upperArmLocalLength + _forearmLocalLength - safeDistance;
|
||||
float circleRadius = 0.0f;
|
||||
|
||||
if (localArmToCenter.magnitude + _forearmLocalLength > maxHandDistance)
|
||||
{
|
||||
// Too far from shoulder and arm is over-extending. Solve depending on selected mode, but some are applied at the end of this method.
|
||||
localArmToCenter = localArmToCenter.normalized * (_upperArmLocalLength - safeDistance * 0.5f);
|
||||
localCenter = localArmPos + localArmToCenter;
|
||||
|
||||
if (armOverExtendMode == UxrArmOverExtendMode.LimitHandReach)
|
||||
{
|
||||
// Clamp hand distance
|
||||
Hand.position = ToWorldPos(localArmPos + localArmToCenter.normalized * maxHandDistance);
|
||||
}
|
||||
|
||||
float angleRadians = Mathf.Acos((localCenter - localArmPos).magnitude / _upperArmLocalLength);
|
||||
circleRadius = Mathf.Sin(angleRadians) * _upperArmLocalLength;
|
||||
}
|
||||
else if (localArmToCenter.magnitude < 0.04f)
|
||||
{
|
||||
// Too close to shoulder: keep current elbow position.
|
||||
localArmToCenter = localForearmPos - localArmPos;
|
||||
localCenter = localForearmPos;
|
||||
}
|
||||
else
|
||||
{
|
||||
localCenter = localArmPos + localArmToCenter;
|
||||
|
||||
// Find the circle radius
|
||||
float angleRadians = Mathf.Acos((localCenter - localArmPos).magnitude / _upperArmLocalLength);
|
||||
circleRadius = Mathf.Sin(angleRadians) * _upperArmLocalLength;
|
||||
}
|
||||
|
||||
Vector3 finalLocalHandPosition = ToLocalAvatarPos(Hand.position);
|
||||
Quaternion finalHandRotation = Hand.rotation;
|
||||
|
||||
// Compute the point inside this circle using the elbowAperture parameter.
|
||||
// Possible range is from bottom to exterior (far left or far right for left arm and right arm respectively).
|
||||
Vector3 planeNormal = -new Vector3(a, b, c);
|
||||
|
||||
Transform otherArm = _side == UxrHandSide.Left ? Avatar.AvatarRig.RightArm.UpperArm : Avatar.AvatarRig.LeftArm.UpperArm;
|
||||
Vector3 otherLocalArmPos = otherArm != null ? ToLocalAvatarPos(otherArm.position) : Vector3.zero;
|
||||
|
||||
if (otherArm == null)
|
||||
{
|
||||
otherLocalArmPos = ToLocalAvatarPos(Arm.position);
|
||||
otherLocalArmPos.x = -otherLocalArmPos.x;
|
||||
}
|
||||
|
||||
Quaternion rotToShoulder = Quaternion.LookRotation(Vector3.Cross((localArmPos - otherLocalArmPos) * (_side == UxrHandSide.Left ? -1.0f : 1.0f), Vector3.up).normalized, Vector3.up);
|
||||
Vector3 armToHand = (finalLocalHandPosition - localArmPos).normalized;
|
||||
Quaternion rotArmForward = rotToShoulder * Quaternion.LookRotation(Quaternion.Inverse(rotToShoulder) * localArmToCenter, Quaternion.Inverse(rotToShoulder) * armToHand);
|
||||
|
||||
Vector3 vectorFromCenterSide = Vector3.Cross(_side == UxrHandSide.Left ? rotArmForward * Vector3.up : rotArmForward * -Vector3.up, planeNormal);
|
||||
|
||||
if (otherArm != null)
|
||||
{
|
||||
bool isBack = Vector3.Cross(localArmPos - otherLocalArmPos, localCenter - localArmPos).y * (_side == UxrHandSide.Left ? -1.0f : 1.0f) > 0.0f;
|
||||
|
||||
/*
|
||||
* Do stuff with isBack
|
||||
*/
|
||||
}
|
||||
|
||||
// Compute elbow aperture value [0.0, 1.0] depending on the relaxedElbowAperture parameter and the current wrist torsion
|
||||
float wristDegrees = _side == UxrHandSide.Left ? -Avatar.AvatarRigInfo.GetArmInfo(UxrHandSide.Left).WristTorsionInfo.WristTorsionAngle : Avatar.AvatarRigInfo.GetArmInfo(UxrHandSide.Right).WristTorsionInfo.WristTorsionAngle;
|
||||
float elbowApertureBiasDueToWrist = wristDegrees / WristTorsionDegreesFactor * _elbowApertureRotation;
|
||||
float elbowAperture = Mathf.Clamp01(_relaxedElbowAperture + elbowApertureBiasDueToWrist);
|
||||
|
||||
_elbowAperture = _elbowAperture < 0.0f ? elbowAperture : Mathf.SmoothDampAngle(_elbowAperture, elbowAperture, ref _elbowApertureVelocity, ElbowApertureRotationSmoothTime);
|
||||
|
||||
// Now compute the elbow position using it
|
||||
Vector3 vectorFromCenterBottom = _side == UxrHandSide.Left ? Vector3.Cross(vectorFromCenterSide, planeNormal) : Vector3.Cross(planeNormal, vectorFromCenterSide);
|
||||
|
||||
Vector3 elbowPosition = localCenter + Vector3.Lerp(vectorFromCenterBottom, vectorFromCenterSide, _elbowAperture).normalized * circleRadius;
|
||||
|
||||
// Compute the desired rotation
|
||||
Vector3 armForward = (elbowPosition - localArmPos).normalized;
|
||||
|
||||
// Check range of motion of the arm
|
||||
if (Arm.parent != null)
|
||||
{
|
||||
Vector3 armNeutralForward = ToLocalAvatarDir(Arm.parent.TransformDirection(_armNeutralForwardInParent));
|
||||
|
||||
if (Vector3.Angle(armForward, armNeutralForward) > _armRangeOfMotionAngle)
|
||||
{
|
||||
armForward = Vector3.RotateTowards(armNeutralForward, armForward, _armRangeOfMotionAngle * Mathf.Deg2Rad, 0.0f);
|
||||
elbowPosition = localArmPos + armForward * _upperArmLocalLength;
|
||||
}
|
||||
}
|
||||
|
||||
// Compute the position and rotation of the rest
|
||||
Vector3 forearmForward = (ToLocalAvatarPos(Hand.position) - elbowPosition).normalized;
|
||||
float elbowAngle = Vector3.Angle(armForward, forearmForward);
|
||||
Vector3 elbowAxis = elbowAngle > ElbowMinAngleThreshold ? Vector3.Cross(forearmForward, armForward).normalized : Vector3.up;
|
||||
|
||||
elbowAxis = _side == UxrHandSide.Left ? -elbowAxis : elbowAxis;
|
||||
|
||||
Quaternion armRotationTarget = Quaternion.LookRotation(armForward, elbowAxis);
|
||||
Quaternion forearmRotationTarget = Quaternion.LookRotation(forearmForward, elbowAxis);
|
||||
|
||||
// Transform from top hierarchy to bottom to avoid jitter. Since we consider Z forward and Y the elbow rotation axis, we also
|
||||
// need to transform from this "universal" space to the actual axes the model uses.
|
||||
Arm.rotation = ToWorldRot(armRotationTarget * _armUniversalLocalAxes.UniversalToActualAxesRotation);
|
||||
|
||||
if (Vector3.Distance(finalLocalHandPosition, localArmPos) > maxHandDistance)
|
||||
{
|
||||
// Arm over extended: solve if the current mode is one of the remaining 2 to handle:
|
||||
if (armOverExtendMode == UxrArmOverExtendMode.ExtendUpperArm)
|
||||
{
|
||||
// Move the elbow away to reach the hand. This will stretch the arm.
|
||||
elbowPosition = finalLocalHandPosition - (finalLocalHandPosition - elbowPosition).normalized * _forearmLocalLength;
|
||||
}
|
||||
else if (armOverExtendMode == UxrArmOverExtendMode.ExtendArm)
|
||||
{
|
||||
// Stretch both the arm and forearm
|
||||
Vector3 elbowPosition2 = finalLocalHandPosition - (finalLocalHandPosition - elbowPosition).normalized * _forearmLocalLength;
|
||||
elbowPosition = (elbowPosition + elbowPosition2) * 0.5f;
|
||||
}
|
||||
}
|
||||
|
||||
Forearm.SetPositionAndRotation(ToWorldPos(elbowPosition), ToWorldRot(forearmRotationTarget * _forearmUniversalLocalAxes.UniversalToActualAxesRotation));
|
||||
Hand.SetPositionAndRotation(ToWorldPos(finalLocalHandPosition), finalHandRotation);
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Unity
|
||||
|
||||
/// <summary>
|
||||
/// Computes internal IK parameters.
|
||||
/// </summary>
|
||||
protected override void Awake()
|
||||
{
|
||||
base.Awake();
|
||||
|
||||
ComputeParameters();
|
||||
_initialized = true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Subscribe to events.
|
||||
/// </summary>
|
||||
protected override void OnEnable()
|
||||
{
|
||||
base.OnEnable();
|
||||
UxrManager.AvatarsUpdated += UxrManager_AvatarsUpdated;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Unsubscribes from events.
|
||||
/// </summary>
|
||||
protected override void OnDisable()
|
||||
{
|
||||
base.OnDisable();
|
||||
UxrManager.AvatarsUpdated -= UxrManager_AvatarsUpdated;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Event Handling Methods
|
||||
|
||||
/// <summary>
|
||||
/// Stores the clavicle orientation to smooth it out the next frame.
|
||||
/// </summary>
|
||||
private void UxrManager_AvatarsUpdated()
|
||||
{
|
||||
if (Clavicle != null)
|
||||
{
|
||||
_lastClavicleLocalRotation = Quaternion.Inverse(Avatar.transform.rotation) * Clavicle.rotation;
|
||||
_lastClavicleRotationInitialized = true;
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Protected Overrides UxrIKSolver
|
||||
|
||||
/// <summary>
|
||||
/// Solves the IK for the current frame.
|
||||
/// </summary>
|
||||
protected override void InternalSolveIK()
|
||||
{
|
||||
if (Avatar == null || Avatar.AvatarController == null || !Avatar.AvatarController.Initialized)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (Clavicle != null)
|
||||
{
|
||||
// If we have a clavicle, perform another pass this time taking it into account.
|
||||
// The first pass won't clamp the hand distance because thanks to the clavicle rotation there is a little more reach.
|
||||
SolveIKPass(UxrArmSolveOptions.ResetClavicle, UxrArmOverExtendMode.ExtendForearm);
|
||||
SolveIKPass(UxrArmSolveOptions.ResetClavicle | UxrArmSolveOptions.SolveClavicle, _overExtendMode);
|
||||
}
|
||||
else
|
||||
{
|
||||
SolveIKPass(UxrArmSolveOptions.None, _overExtendMode);
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Private Methods
|
||||
|
||||
/// <summary>
|
||||
/// Transforms a point from world space to local avatar space.
|
||||
/// </summary>
|
||||
/// <param name="pos">World space position</param>
|
||||
/// <returns>Avatar space position</returns>
|
||||
private Vector3 ToLocalAvatarPos(Vector3 pos)
|
||||
{
|
||||
return Avatar.transform.InverseTransformPoint(pos);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Transforms a point from local avatar space to world space.
|
||||
/// </summary>
|
||||
/// <param name="pos">Avatar space position</param>
|
||||
/// <returns>World space position</returns>
|
||||
private Vector3 ToWorldPos(Vector3 pos)
|
||||
{
|
||||
return Avatar.transform.TransformPoint(pos);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Transforms a direction from world space to local avatar space.
|
||||
/// </summary>
|
||||
/// <param name="dir">World space direction</param>
|
||||
/// <returns>Avatar space direction</returns>
|
||||
private Vector3 ToLocalAvatarDir(Vector3 dir)
|
||||
{
|
||||
return Avatar.transform.InverseTransformDirection(dir);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Transforms a rotation from world space to local avatar space.
|
||||
/// </summary>
|
||||
/// <param name="rot">World space rotation</param>
|
||||
/// <returns>Avatar space rotation</returns>
|
||||
private Quaternion ToLocalAvatarRot(Quaternion rot)
|
||||
{
|
||||
return Quaternion.Inverse(Avatar.transform.rotation) * rot;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Transforms a rotation from local avatar space to world space.
|
||||
/// </summary>
|
||||
/// <param name="rot">Avatar space rotation</param>
|
||||
/// <returns>World space rotation</returns>
|
||||
private Quaternion ToWorldRot(Quaternion rot)
|
||||
{
|
||||
return Avatar.transform.rotation * rot;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Computes the internal parameters for the IK.
|
||||
/// </summary>
|
||||
private void ComputeParameters()
|
||||
{
|
||||
if (Avatar == null)
|
||||
{
|
||||
if (UxrGlobalSettings.Instance.LogLevelAnimation >= UxrLogLevel.Errors)
|
||||
{
|
||||
Debug.LogError($"{UxrConstants.AnimationModule} {nameof(UxrArmIKSolver)} can't find {nameof(UxrAvatar)} component upwards in the hierarchy. Component is located in {this.GetPathUnderScene()}");
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
_side = transform.HasParent(Avatar.AvatarRig.LeftArm.Clavicle ?? Avatar.AvatarRig.LeftArm.UpperArm) ? UxrHandSide.Left : UxrHandSide.Right;
|
||||
|
||||
// Set up references
|
||||
if (Clavicle == null)
|
||||
{
|
||||
Clavicle = _side == UxrHandSide.Left ? Avatar.AvatarRig.LeftArm.Clavicle : Avatar.AvatarRig.RightArm.Clavicle;
|
||||
}
|
||||
|
||||
if (Arm == null)
|
||||
{
|
||||
Arm = _side == UxrHandSide.Left ? Avatar.AvatarRig.LeftArm.UpperArm : Avatar.AvatarRig.RightArm.UpperArm;
|
||||
}
|
||||
|
||||
if (Forearm == null)
|
||||
{
|
||||
Forearm = _side == UxrHandSide.Left ? Avatar.AvatarRig.LeftArm.Forearm : Avatar.AvatarRig.RightArm.Forearm;
|
||||
}
|
||||
|
||||
if (Hand == null)
|
||||
{
|
||||
Hand = Avatar.GetHandBone(_side);
|
||||
}
|
||||
|
||||
UxrAvatarArm arm = Avatar.GetArm(_side);
|
||||
|
||||
if (arm != null && arm.UpperArm && arm.Forearm && arm.Hand.Wrist)
|
||||
{
|
||||
// Compute lengths in local avatar coordinates in case avatar has scaling.
|
||||
// We use special treatment for the local hand in case that the component is being added at runtime and the hand is driven by a multiplayer NetworkTransform component that already moved it.
|
||||
|
||||
Vector3 localUpperArm = ToLocalAvatarPos(arm.UpperArm.position);
|
||||
Vector3 localForearm = ToLocalAvatarPos(arm.Forearm.position);
|
||||
Vector3 localHand = ToLocalAvatarPos(arm.Hand.Wrist.transform.parent.TransformPoint(Avatar.AvatarRigInfo.GetArmInfo(_side).HandUniversalLocalAxes.InitialLocalPosition));
|
||||
|
||||
_upperArmLocalLength = Vector3.Distance(localUpperArm, localForearm);
|
||||
_forearmLocalLength = Vector3.Distance(localForearm, localHand);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (UxrGlobalSettings.Instance.LogLevelAnimation >= UxrLogLevel.Errors)
|
||||
{
|
||||
Debug.LogError($"{UxrConstants.AnimationModule} {nameof(UxrArmIKSolver)} can't find one or more of the following bones: upper arm, forearm, wrist. Component is located in {this.GetPathUnderScene()}");
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
_clavicleUniversalLocalAxes = Avatar.AvatarRigInfo.GetArmInfo(_side).ClavicleUniversalLocalAxes;
|
||||
_armUniversalLocalAxes = Avatar.AvatarRigInfo.GetArmInfo(_side).ArmUniversalLocalAxes;
|
||||
_forearmUniversalLocalAxes = Avatar.AvatarRigInfo.GetArmInfo(_side).ForearmUniversalLocalAxes;
|
||||
|
||||
// Compute arm range of motion neutral direction
|
||||
_armNeutralForwardInParent = Vector3.forward;
|
||||
_armNeutralForwardInParent = Quaternion.AngleAxis(30.0f * (_side == UxrHandSide.Left ? -1.0f : 1.0f), Vector3.up) * _armNeutralForwardInParent;
|
||||
_armNeutralForwardInParent = Quaternion.AngleAxis(30.0f, Vector3.right) * _armNeutralForwardInParent;
|
||||
|
||||
if (Arm.parent != null)
|
||||
{
|
||||
_armNeutralForwardInParent = Arm.parent.InverseTransformDirection(Avatar.transform.TransformDirection(_armNeutralForwardInParent));
|
||||
}
|
||||
|
||||
if (Clavicle && Avatar)
|
||||
{
|
||||
// If we have a clavicle, set it up too
|
||||
if (_clavicleAutoComputeBias)
|
||||
{
|
||||
Vector3 clavicleLookAt = (Forearm.position - Clavicle.position).normalized;
|
||||
Avatar.transform.InverseTransformDirection(clavicleLookAt);
|
||||
|
||||
_clavicleDeformationAxesBias = new Vector3(0.0f, -clavicleLookAt.y + 0.25f, -clavicleLookAt.z);
|
||||
}
|
||||
}
|
||||
|
||||
_elbowAperture = -1.0f;
|
||||
_elbowApertureVelocity = 0.0f;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Private Types & Data
|
||||
|
||||
private const float ClavicleMaxDegreesPerSecond = 360.0f;
|
||||
private const float WristTorsionDegreesFactor = 150.0f;
|
||||
private const float ElbowApertureRotationSmoothTime = 0.1f;
|
||||
private const float ElbowMinAngleThreshold = 3.0f;
|
||||
|
||||
private UxrHandSide _side;
|
||||
private bool _initialized;
|
||||
private UxrUniversalLocalAxes _clavicleUniversalLocalAxes;
|
||||
private UxrUniversalLocalAxes _armUniversalLocalAxes;
|
||||
private UxrUniversalLocalAxes _forearmUniversalLocalAxes;
|
||||
private float _upperArmLocalLength;
|
||||
private float _forearmLocalLength;
|
||||
private float _elbowAperture = -1.0f;
|
||||
private float _elbowApertureVelocity;
|
||||
private Vector3 _armNeutralForwardInParent = Vector3.zero;
|
||||
private Quaternion _lastClavicleLocalRotation = Quaternion.identity;
|
||||
private bool _lastClavicleRotationInitialized;
|
||||
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 61a25f8a356cfe1489a26b996a2d81de
|
||||
timeCreated: 1510133937
|
||||
licenseType: Pro
|
||||
MonoImporter:
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,34 @@
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
// <copyright file="UxrArmOverExtendMode.cs" company="VRMADA">
|
||||
// Copyright (c) VRMADA, All rights reserved.
|
||||
// </copyright>
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
namespace UltimateXR.Animation.IK
|
||||
{
|
||||
/// <summary>
|
||||
/// Enumerates the different solutions that can be used when an avatar with visible arms moves a hand farther than the
|
||||
/// actual length of the arm.
|
||||
/// </summary>
|
||||
public enum UxrArmOverExtendMode
|
||||
{
|
||||
/// <summary>
|
||||
/// Hand reach will be limited to what the actual arm length permits.
|
||||
/// </summary>
|
||||
LimitHandReach,
|
||||
|
||||
/// <summary>
|
||||
/// Stretch the forearm.
|
||||
/// </summary>
|
||||
ExtendForearm,
|
||||
|
||||
/// <summary>
|
||||
/// Stretch the upper arm.
|
||||
/// </summary>
|
||||
ExtendUpperArm,
|
||||
|
||||
/// <summary>
|
||||
/// Stretch both the upper arm and forearm.
|
||||
/// </summary>
|
||||
ExtendArm
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,3 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 3d2ff8c20c3e488b83833ab43415e40a
|
||||
timeCreated: 1643116515
|
||||
@@ -0,0 +1,33 @@
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
// <copyright file="UxrArmSolveOptions.cs" company="VRMADA">
|
||||
// Copyright (c) VRMADA, All rights reserved.
|
||||
// </copyright>
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
using System;
|
||||
|
||||
namespace UltimateXR.Animation.IK
|
||||
{
|
||||
/// <summary>
|
||||
/// Different clavicle options supported by <see cref="UxrArmIKSolver.SolveIKPass" /> when clavicle data is present in
|
||||
/// the rig.
|
||||
/// </summary>
|
||||
[Flags]
|
||||
public enum UxrArmSolveOptions
|
||||
{
|
||||
/// <summary>
|
||||
/// No options.
|
||||
/// </summary>
|
||||
None = 0,
|
||||
|
||||
/// <summary>
|
||||
/// Reset the clavicle position.
|
||||
/// </summary>
|
||||
ResetClavicle = 1,
|
||||
|
||||
/// <summary>
|
||||
/// Solve the clavicle position. Can be used together with <see cref="ResetClavicle" /> so that the clavicle is solved
|
||||
/// without using the current position data.
|
||||
/// </summary>
|
||||
SolveClavicle = 1 << 1
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,3 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 9663654228ad49fcb30c5657e7c2ec76
|
||||
timeCreated: 1645360837
|
||||
@@ -0,0 +1,59 @@
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
// <copyright file="UxrBodyIK.IndependentBoneInfo.cs" company="VRMADA">
|
||||
// Copyright (c) VRMADA, All rights reserved.
|
||||
// </copyright>
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
using UnityEngine;
|
||||
|
||||
namespace UltimateXR.Animation.IK
|
||||
{
|
||||
public sealed partial class UxrBodyIK
|
||||
{
|
||||
#region Private Types & Data
|
||||
|
||||
/// <summary>
|
||||
/// Independent bones are bones that are driven externally and not using IK, such as the hands (wrist bones), which are
|
||||
/// driven by the tracked input controllers. They need to be kept track of when parent bones are modified by Inverse
|
||||
/// Kinematics to make sure that they are kept in the same place afterwards. Otherwise due to parenting the position
|
||||
/// they should have would change.
|
||||
/// </summary>
|
||||
private class IndependentBoneInfo
|
||||
{
|
||||
#region Public Types & Data
|
||||
|
||||
/// <summary>
|
||||
/// Gets the bone transform.
|
||||
/// </summary>
|
||||
public Transform Transform { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the correct current position.
|
||||
/// </summary>
|
||||
public Vector3 Position { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the correct current position.
|
||||
/// </summary>
|
||||
public Quaternion Rotation { get; set; }
|
||||
|
||||
#endregion
|
||||
|
||||
#region Constructors & Finalizer
|
||||
|
||||
/// <summary>
|
||||
/// Constructor.
|
||||
/// </summary>
|
||||
/// <param name="transform">Bone transform</param>
|
||||
public IndependentBoneInfo(Transform transform)
|
||||
{
|
||||
Transform = transform;
|
||||
Position = transform.position;
|
||||
Rotation = transform.rotation;
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,3 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 67c356fd5432455fa75bee27571d5a2a
|
||||
timeCreated: 1643734039
|
||||
592
Assets/UltimateXR/Runtime/Scripts/Animation/IK/UxrBodyIK.cs
Normal file
592
Assets/UltimateXR/Runtime/Scripts/Animation/IK/UxrBodyIK.cs
Normal file
@@ -0,0 +1,592 @@
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
// <copyright file="UxrBodyIK.cs" company="VRMADA">
|
||||
// Copyright (c) VRMADA, All rights reserved.
|
||||
// </copyright>
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
using System.Collections.Generic;
|
||||
using UltimateXR.Avatar;
|
||||
using UltimateXR.Avatar.Rig;
|
||||
using UltimateXR.Core;
|
||||
using UltimateXR.Core.Math;
|
||||
using UltimateXR.Core.Settings;
|
||||
using UltimateXR.Extensions.Unity;
|
||||
using UnityEngine;
|
||||
|
||||
namespace UltimateXR.Animation.IK
|
||||
{
|
||||
/// <summary>
|
||||
/// Class that provides functionality to compute Inverse Kinematics for a humanoid body.
|
||||
/// </summary>
|
||||
public sealed partial class UxrBodyIK
|
||||
{
|
||||
#region Public Types & Data
|
||||
|
||||
/// <summary>
|
||||
/// Gets whether the object was initialized.
|
||||
/// </summary>
|
||||
public bool Initialized { get; private set; }
|
||||
|
||||
#endregion
|
||||
|
||||
#region Public Methods
|
||||
|
||||
/// <summary>
|
||||
/// Initializes the IK object.
|
||||
/// </summary>
|
||||
/// <param name="avatar">Avatar that the IK will be computed for</param>
|
||||
/// <param name="settings">IK settings to use</param>
|
||||
/// <param name="usesExternalArmIK">Whether the avatar uses an arm IK</param>
|
||||
/// <param name="usesExternalLegIK">Whether the avatar uses leg IK</param>
|
||||
public void Initialize(UxrAvatar avatar, UxrBodyIKSettings settings, bool usesExternalArmIK, bool usesExternalLegIK)
|
||||
{
|
||||
Initialized = true;
|
||||
_avatar = avatar;
|
||||
_avatarTransform = avatar.transform;
|
||||
_settings = settings;
|
||||
|
||||
// Get body root
|
||||
|
||||
if (avatar.AvatarRig.Head.Head == null)
|
||||
{
|
||||
if (UxrGlobalSettings.Instance.LogLevelAvatar >= UxrLogLevel.Errors)
|
||||
{
|
||||
Debug.LogError($"{UxrConstants.AvatarModule} Avatar {avatar.name} has no head setup in the {nameof(UxrAvatar)}'s Rig field");
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
_independentBones = new List<IndependentBoneInfo>();
|
||||
|
||||
List<Transform> bodyTransforms = new List<Transform>();
|
||||
|
||||
if (avatar.AvatarRig.UpperChest)
|
||||
{
|
||||
bodyTransforms.Add(avatar.AvatarRig.UpperChest);
|
||||
_upperChestUniversalLocalAxes = new UxrUniversalLocalAxes(avatar.AvatarRig.UpperChest, _avatarTransform);
|
||||
}
|
||||
|
||||
if (avatar.AvatarRig.Chest)
|
||||
{
|
||||
bodyTransforms.Add(avatar.AvatarRig.Chest);
|
||||
_chestUniversalLocalAxes = new UxrUniversalLocalAxes(avatar.AvatarRig.Chest, _avatarTransform);
|
||||
}
|
||||
|
||||
if (avatar.AvatarRig.Spine)
|
||||
{
|
||||
bodyTransforms.Add(avatar.AvatarRig.Spine);
|
||||
_spineUniversalLocalAxes = new UxrUniversalLocalAxes(avatar.AvatarRig.Spine, _avatarTransform);
|
||||
}
|
||||
|
||||
if (avatar.AvatarRig.Hips)
|
||||
{
|
||||
bodyTransforms.Add(avatar.AvatarRig.Hips);
|
||||
}
|
||||
|
||||
if (avatar.AvatarRig.Head.Neck && avatar.AvatarRig.Head.Neck != null && avatar.AvatarRig.Head.Neck != _avatarTransform && !bodyTransforms.Contains(avatar.AvatarRig.Head.Neck.parent))
|
||||
{
|
||||
bodyTransforms.Add(avatar.AvatarRig.Head.Neck.parent);
|
||||
}
|
||||
|
||||
if (avatar.AvatarRig.UpperChest && avatar.AvatarRig.UpperChest.parent != null && avatar.AvatarRig.UpperChest.parent != _avatarTransform && !bodyTransforms.Contains(avatar.AvatarRig.UpperChest.parent))
|
||||
{
|
||||
bodyTransforms.Add(avatar.AvatarRig.UpperChest.parent);
|
||||
}
|
||||
|
||||
if (avatar.AvatarRig.Chest && avatar.AvatarRig.Chest.parent != null && avatar.AvatarRig.Chest.parent != _avatarTransform && !bodyTransforms.Contains(avatar.AvatarRig.Chest.parent))
|
||||
{
|
||||
bodyTransforms.Add(avatar.AvatarRig.Chest.parent);
|
||||
}
|
||||
|
||||
if (avatar.AvatarRig.Spine && avatar.AvatarRig.Spine.parent != null && avatar.AvatarRig.Spine.parent != _avatarTransform && !bodyTransforms.Contains(avatar.AvatarRig.Spine.parent))
|
||||
{
|
||||
bodyTransforms.Add(avatar.AvatarRig.Spine.parent);
|
||||
}
|
||||
|
||||
if (avatar.AvatarRig.Hips && avatar.AvatarRig.Hips.parent != null && avatar.AvatarRig.Hips.parent != _avatarTransform && !bodyTransforms.Contains(avatar.AvatarRig.Hips.parent))
|
||||
{
|
||||
bodyTransforms.Add(avatar.AvatarRig.Hips.parent);
|
||||
}
|
||||
|
||||
_avatarBodyRoot = TransformExt.GetCommonRootTransformFromSet(bodyTransforms.ToArray());
|
||||
|
||||
if (_avatarBodyRoot == null)
|
||||
{
|
||||
if (UxrGlobalSettings.Instance.LogLevelAvatar >= UxrLogLevel.Warnings)
|
||||
{
|
||||
Debug.LogWarning($"{UxrConstants.AvatarModule} No common avatar body root found. If there is an avatar body it will not follow the head position.");
|
||||
}
|
||||
|
||||
_avatarBodyRoot = new GameObject("Dummy Root").transform;
|
||||
_avatarBodyRoot.SetParent(_avatarTransform);
|
||||
_avatarBodyRoot.SetPositionAndRotation(_avatarTransform.position, _avatarTransform.rotation);
|
||||
}
|
||||
|
||||
// Neck
|
||||
|
||||
_avatarNeck = avatar.AvatarRig.Head.Neck;
|
||||
|
||||
if (_avatarNeck == null)
|
||||
{
|
||||
_avatarNeck = new GameObject("Dummy Neck").transform;
|
||||
_avatarNeck.SetParent(_avatarBodyRoot);
|
||||
_avatarNeck.SetPositionAndRotation(_avatarTransform.position + _avatarTransform.up * _settings.NeckBaseHeight + _avatarTransform.forward * _settings.NeckForwardOffset, _avatarTransform.rotation);
|
||||
|
||||
if (avatar.AvatarRig.Head.Head != null)
|
||||
{
|
||||
avatar.AvatarRig.Head.Head.SetParent(_avatarNeck);
|
||||
}
|
||||
}
|
||||
|
||||
_neckUniversalLocalAxes = new UxrUniversalLocalAxes(_avatarNeck, _avatarTransform);
|
||||
|
||||
// Head
|
||||
|
||||
_headUniversalLocalAxes = new UxrUniversalLocalAxes(avatar.AvatarRig.Head.Head, _avatarTransform);
|
||||
_avatarHead = avatar.AvatarRig.Head.Head;
|
||||
|
||||
// Eyes
|
||||
|
||||
_avatarEyes = new GameObject("Dummy Eyes").transform;
|
||||
_avatarEyes.SetParent(_avatarHead);
|
||||
_avatarEyes.SetPositionAndRotation(_avatarTransform.position + _avatarTransform.up * _settings.EyesBaseHeight + _avatarTransform.forward * _settings.EyesForwardOffset, _avatarTransform.rotation);
|
||||
|
||||
// Avatar Forward
|
||||
|
||||
_avatarForward = new GameObject("Dummy Forward").transform;
|
||||
_avatarForward.SetParent(_avatarTransform);
|
||||
_avatarForward.SetPositionAndRotation(_avatarHead.position - avatar.transform.up * _avatarHead.position.y, _avatarTransform.rotation);
|
||||
|
||||
_avatarBodyRoot.SetParent(_avatarForward);
|
||||
|
||||
// Initialize
|
||||
|
||||
_neckPosRelativeToEyes = Quaternion.Inverse(_avatarEyes.rotation) * (_avatarNeck.position - _avatarEyes.position);
|
||||
_neckRotRelativeToEyes = Quaternion.Inverse(_avatarEyes.rotation) * _avatarNeck.rotation;
|
||||
_avatarForwardPosRelativeToNeck = _avatarForward.position - _avatarNeck.transform.position;
|
||||
_avatarForwardTarget = _avatarForward.forward;
|
||||
_straightSpineForward = _avatarForward.forward;
|
||||
|
||||
if (usesExternalArmIK)
|
||||
{
|
||||
if (avatar.LeftHandBone)
|
||||
{
|
||||
_independentBones.Add(new IndependentBoneInfo(avatar.LeftHandBone));
|
||||
}
|
||||
|
||||
if (avatar.RightHandBone)
|
||||
{
|
||||
_independentBones.Add(new IndependentBoneInfo(avatar.RightHandBone));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Computes the Pre-pass in the IK solving.
|
||||
/// </summary>
|
||||
public void PreSolveAvatarIK()
|
||||
{
|
||||
if (_avatarHead == null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// Push transforms. These transforms hang from the body and need to be kept in place because
|
||||
// due to parenting their absolute position/orientation will be altered
|
||||
|
||||
foreach (IndependentBoneInfo boneInfo in _independentBones)
|
||||
{
|
||||
boneInfo.Rotation = boneInfo.Transform.rotation;
|
||||
boneInfo.Position = boneInfo.Transform.position;
|
||||
}
|
||||
|
||||
// Reset upper body
|
||||
|
||||
_avatarNeck.localPosition = _neckUniversalLocalAxes.InitialLocalPosition;
|
||||
_avatarHead.localPosition = _headUniversalLocalAxes.InitialLocalPosition;
|
||||
_avatarNeck.localRotation = _neckUniversalLocalAxes.InitialLocalRotation;
|
||||
_avatarHead.localRotation = _headUniversalLocalAxes.InitialLocalRotation;
|
||||
|
||||
if (_avatar.AvatarRig.UpperChest)
|
||||
{
|
||||
_avatar.AvatarRig.UpperChest.localRotation = _upperChestUniversalLocalAxes.InitialLocalRotation;
|
||||
}
|
||||
|
||||
if (_avatar.AvatarRig.Chest)
|
||||
{
|
||||
_avatar.AvatarRig.Chest.localRotation = _chestUniversalLocalAxes.InitialLocalRotation;
|
||||
}
|
||||
|
||||
if (_avatar.AvatarRig.Spine)
|
||||
{
|
||||
_avatar.AvatarRig.Spine.localRotation = _spineUniversalLocalAxes.InitialLocalRotation;
|
||||
}
|
||||
|
||||
// Compute neck position/rotation to make the avatar eyes match the camera
|
||||
|
||||
Transform cameraTransform = _avatar.CameraComponent.transform;
|
||||
Vector3 localAvatarPivotPos = _avatar.transform.InverseTransformPoint(_avatarForward.position);
|
||||
Vector3 neckPosition = GetWorldPosFromOffset(cameraTransform, cameraTransform, _neckPosRelativeToEyes);
|
||||
Quaternion neckRotation = cameraTransform.rotation * _neckRotRelativeToEyes;
|
||||
|
||||
_avatarNeck.SetPositionAndRotation(neckPosition, neckRotation);
|
||||
|
||||
// Update avatar pivot
|
||||
|
||||
_avatarForward.position = GetWorldPosFromOffset(_avatarForward, _avatarNeck, _avatarForwardPosRelativeToNeck);
|
||||
bool smoothForwardRotation = true;
|
||||
|
||||
if (Vector3.Angle(cameraTransform.forward, _avatar.transform.up) > CameraUpsideDownAngleThreshold && Vector3.Angle(cameraTransform.forward, -_avatar.transform.up) > CameraUpsideDownAngleThreshold)
|
||||
{
|
||||
// _straightSpineForward contains the forward direction where the avatar looks (vector.y is 0).
|
||||
// This is different from _avatarForward.forward because avatarForward allows the head to rotate
|
||||
// some degrees without rotating the whole body along with it.
|
||||
_straightSpineForward = Vector3.ProjectOnPlane(cameraTransform.forward, _avatar.transform.up);
|
||||
|
||||
if (Vector3.Angle(cameraTransform.forward, _avatarForward.forward) > 90.0f)
|
||||
{
|
||||
// Bad orientation, fix instantly.
|
||||
smoothForwardRotation = false;
|
||||
}
|
||||
}
|
||||
|
||||
float bodyRotationAngle = Vector3.Angle(_straightSpineForward, _avatarForward.forward);
|
||||
if (bodyRotationAngle > _settings.HeadFreeRangeTorsion)
|
||||
{
|
||||
float radians = (bodyRotationAngle - _settings.HeadFreeRangeTorsion) * Mathf.Deg2Rad;
|
||||
_avatarForwardTarget = Vector3.RotateTowards(_avatarForwardTarget, _straightSpineForward, radians, 0.0f);
|
||||
}
|
||||
|
||||
// Update avatar forward direction
|
||||
|
||||
float rotationSpeedMultiplier = Vector3.Angle(_avatarForward.forward, _avatarForwardTarget) / 30.0f;
|
||||
float maxForwardDegreesDelta = AvatarRotationDegreesPerSecond * rotationSpeedMultiplier * _settings.BodyPivotRotationSpeed * Time.deltaTime;
|
||||
|
||||
_avatarForward.rotation = Quaternion.RotateTowards(Quaternion.LookRotation(_avatarForward.forward, _avatar.transform.up),
|
||||
Quaternion.LookRotation(_avatarForwardTarget, _avatar.transform.up),
|
||||
smoothForwardRotation ? maxForwardDegreesDelta : 180.0f);
|
||||
|
||||
// Since the avatar pivot is parent of all body nodes, move the neck back to its position
|
||||
_avatarNeck.SetPositionAndRotation(neckPosition, neckRotation);
|
||||
|
||||
if (_settings.LockBodyPivot)
|
||||
{
|
||||
_avatarForward.position = _avatar.transform.TransformPoint(localAvatarPivotPos);
|
||||
}
|
||||
|
||||
// We've computed the avatar head orientation using only the neck. Now redistribute it using the _neckHeadBalance parameter so that
|
||||
// we can get a smoother head rotation by using both the neck and the head.
|
||||
|
||||
if (_settings.NeckHeadBalance > 0.0f)
|
||||
{
|
||||
Quaternion neckUniversalRotation = Quaternion.Inverse(_avatarForward.rotation) * _neckUniversalLocalAxes.UniversalRotation * Quaternion.Inverse(_neckUniversalLocalAxes.InitialUniversalLocalReferenceRotation);
|
||||
|
||||
Vector3 headPosition = _avatarHead.position;
|
||||
Quaternion headRotation = _avatarHead.rotation;
|
||||
|
||||
// Compute partial neck rotation
|
||||
neckRotation = _avatarForward.rotation * (Quaternion.Slerp(Quaternion.identity, neckUniversalRotation, 1.0f - _settings.NeckHeadBalance) *
|
||||
_neckUniversalLocalAxes.InitialUniversalLocalReferenceRotation *
|
||||
_neckUniversalLocalAxes.UniversalToActualAxesRotation);
|
||||
|
||||
_avatarNeck.rotation = neckRotation;
|
||||
_avatarHead.rotation = headRotation;
|
||||
|
||||
_avatarForward.position -= _avatarHead.position - headPosition;
|
||||
|
||||
neckPosition = _avatarNeck.position;
|
||||
}
|
||||
|
||||
// Check influence of head rotation on chest/spine transforms
|
||||
|
||||
if (!_settings.LockBodyPivot)
|
||||
{
|
||||
// Compute head rotation without the rotation around the Y axis:
|
||||
Quaternion headPropagateRotation = Quaternion.Inverse(Quaternion.LookRotation(_straightSpineForward, _avatar.transform.up)) * _headUniversalLocalAxes.UniversalRotation;
|
||||
|
||||
// Remove the rotation that the head can do without propagation to chest/spine:
|
||||
headPropagateRotation = Quaternion.RotateTowards(headPropagateRotation, Quaternion.identity, _settings.HeadFreeRangeBend);
|
||||
|
||||
// Compute influence on chest/spine elements:
|
||||
|
||||
float totalWeight = 0.0f;
|
||||
|
||||
if (_avatar.AvatarRig.UpperChest)
|
||||
{
|
||||
totalWeight += _settings.UpperChestBend;
|
||||
}
|
||||
|
||||
if (_avatar.AvatarRig.Chest)
|
||||
{
|
||||
totalWeight += _settings.ChestBend;
|
||||
}
|
||||
|
||||
if (_avatar.AvatarRig.Spine)
|
||||
{
|
||||
totalWeight += _settings.SpineBend;
|
||||
}
|
||||
|
||||
// Propagate head rotation:
|
||||
|
||||
if (totalWeight > 0.0f)
|
||||
{
|
||||
if (totalWeight < 1.0f)
|
||||
{
|
||||
totalWeight = 1.0f;
|
||||
}
|
||||
|
||||
if (_avatar.AvatarRig.UpperChest)
|
||||
{
|
||||
_avatar.AvatarRig.UpperChest.rotation = _avatarForward.rotation *
|
||||
(Quaternion.Slerp(Quaternion.identity, headPropagateRotation, _settings.UpperChestBend / totalWeight) *
|
||||
_upperChestUniversalLocalAxes.InitialUniversalLocalReferenceRotation * _upperChestUniversalLocalAxes.UniversalToActualAxesRotation);
|
||||
}
|
||||
|
||||
if (_avatar.AvatarRig.Chest)
|
||||
{
|
||||
_avatar.AvatarRig.Chest.rotation = _avatarForward.rotation *
|
||||
(Quaternion.Slerp(Quaternion.identity, headPropagateRotation, _settings.ChestBend / totalWeight) *
|
||||
_chestUniversalLocalAxes.InitialUniversalLocalReferenceRotation * _chestUniversalLocalAxes.UniversalToActualAxesRotation);
|
||||
}
|
||||
|
||||
if (_avatar.AvatarRig.Spine)
|
||||
{
|
||||
_avatar.AvatarRig.Spine.rotation = _avatarForward.rotation *
|
||||
(Quaternion.Slerp(Quaternion.identity, headPropagateRotation, _settings.SpineBend / totalWeight) *
|
||||
_spineUniversalLocalAxes.InitialUniversalLocalReferenceRotation * _spineUniversalLocalAxes.UniversalToActualAxesRotation);
|
||||
}
|
||||
}
|
||||
|
||||
// Make whole avatar move back so that head remains with the same position/orientation
|
||||
|
||||
_avatarNeck.rotation = neckRotation;
|
||||
_avatarForward.position -= _avatarNeck.position - neckPosition;
|
||||
_avatarNeck.position = neckPosition;
|
||||
}
|
||||
|
||||
// If the avatar moves, straighten the forward direction
|
||||
|
||||
float avatarMovedDistance = Vector3.Distance(localAvatarPivotPos, _avatarForward.position);
|
||||
|
||||
if (avatarMovedDistance / Time.deltaTime > AvatarStraighteningMinSpeed)
|
||||
{
|
||||
float degreesToStraighten = avatarMovedDistance * DegreesStraightenedPerMeterMoved;
|
||||
_avatarForwardTarget = Vector3.RotateTowards(_avatarForwardTarget, _straightSpineForward, degreesToStraighten * Mathf.Deg2Rad, 0.0f);
|
||||
}
|
||||
|
||||
// Pop independent transforms (usually hands and other transforms with their own sensors)
|
||||
|
||||
foreach (IndependentBoneInfo boneInfo in _independentBones)
|
||||
{
|
||||
boneInfo.Transform.SetPositionAndRotation(boneInfo.Position, boneInfo.Rotation);
|
||||
}
|
||||
|
||||
if (_settings.LockBodyPivot)
|
||||
{
|
||||
_avatarForward.position = _avatar.transform.TransformPoint(localAvatarPivotPos);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Computes the post-pass in the IK solving.
|
||||
/// </summary>
|
||||
public void PostSolveAvatarIK()
|
||||
{
|
||||
if (_avatarHead == null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// Push transforms. These transforms hang from the body and need to be kept in place because
|
||||
// due to parenting their absolute position/orientation will be altered
|
||||
|
||||
foreach (IndependentBoneInfo boneInfo in _independentBones)
|
||||
{
|
||||
boneInfo.Rotation = boneInfo.Transform.rotation;
|
||||
boneInfo.Position = boneInfo.Transform.position;
|
||||
}
|
||||
|
||||
Vector3 neckPosition = _avatarNeck.position;
|
||||
Quaternion neckRotation = _avatarNeck.rotation;
|
||||
|
||||
// Compute torso rotation
|
||||
|
||||
float torsoRotation = 0.0f; // No degrees, just an abstract quantity we use later to multiply by a factor and get degrees
|
||||
float totalWeight = 0.0f;
|
||||
|
||||
if (_avatar.AvatarRig.UpperChest)
|
||||
{
|
||||
torsoRotation = GetArmTorsoRotationWeight(_avatar.AvatarRig.UpperChest, _upperChestUniversalLocalAxes, _avatar.AvatarRig.LeftArm, UxrHandSide.Left) +
|
||||
GetArmTorsoRotationWeight(_avatar.AvatarRig.UpperChest, _upperChestUniversalLocalAxes, _avatar.AvatarRig.RightArm, UxrHandSide.Right);
|
||||
|
||||
totalWeight += _settings.UpperChestTorsion;
|
||||
}
|
||||
|
||||
if (_avatar.AvatarRig.Chest)
|
||||
{
|
||||
torsoRotation = GetArmTorsoRotationWeight(_avatar.AvatarRig.Chest, _chestUniversalLocalAxes, _avatar.AvatarRig.LeftArm, UxrHandSide.Left) +
|
||||
GetArmTorsoRotationWeight(_avatar.AvatarRig.Chest, _chestUniversalLocalAxes, _avatar.AvatarRig.RightArm, UxrHandSide.Right);
|
||||
|
||||
totalWeight += _settings.ChestTorsion;
|
||||
}
|
||||
|
||||
if (_avatar.AvatarRig.Spine)
|
||||
{
|
||||
torsoRotation = GetArmTorsoRotationWeight(_avatar.AvatarRig.Spine, _spineUniversalLocalAxes, _avatar.AvatarRig.LeftArm, UxrHandSide.Left) +
|
||||
GetArmTorsoRotationWeight(_avatar.AvatarRig.Spine, _spineUniversalLocalAxes, _avatar.AvatarRig.RightArm, UxrHandSide.Right);
|
||||
|
||||
totalWeight += _settings.SpineTorsion;
|
||||
}
|
||||
|
||||
torsoRotation *= 0.5f; // Because range is [-2, +2] since both hands have [-1, 1] range.
|
||||
|
||||
// Rotate upper body elements
|
||||
|
||||
float maxRotationDegrees = 70.0f;
|
||||
|
||||
if (totalWeight > 0.0f)
|
||||
{
|
||||
if (totalWeight < 1.0f)
|
||||
{
|
||||
totalWeight = 1.0f;
|
||||
}
|
||||
|
||||
float upperChestTorsionAngle = maxRotationDegrees * torsoRotation * (_settings.UpperChestTorsion / totalWeight);
|
||||
float chestTorsionAngle = maxRotationDegrees * torsoRotation * (_settings.ChestTorsion / totalWeight);
|
||||
float spineTorsionAngle = maxRotationDegrees * torsoRotation * (_settings.SpineTorsion / totalWeight);
|
||||
|
||||
_upperChestTorsionAngle = Mathf.SmoothDampAngle(_upperChestTorsionAngle, upperChestTorsionAngle, ref _upperChestTorsionSpeed, BodyTorsionSmoothTime);
|
||||
_chestTorsionAngle = Mathf.SmoothDampAngle(_chestTorsionAngle, chestTorsionAngle, ref _chestTorsionSpeed, BodyTorsionSmoothTime);
|
||||
_spineTorsionAngle = Mathf.SmoothDampAngle(_spineTorsionAngle, spineTorsionAngle, ref _spineTorsionSpeed, BodyTorsionSmoothTime);
|
||||
|
||||
if (_avatar.AvatarRig.UpperChest)
|
||||
{
|
||||
_avatar.AvatarRig.UpperChest.localRotation *= Quaternion.AngleAxis(_spineTorsionAngle, _upperChestUniversalLocalAxes.LocalUp);
|
||||
}
|
||||
|
||||
if (_avatar.AvatarRig.Chest)
|
||||
{
|
||||
_avatar.AvatarRig.Chest.localRotation *= Quaternion.AngleAxis(_chestTorsionAngle, _chestUniversalLocalAxes.LocalUp);
|
||||
}
|
||||
|
||||
if (_avatar.AvatarRig.Spine)
|
||||
{
|
||||
_avatar.AvatarRig.Spine.localRotation *= Quaternion.AngleAxis(_spineTorsionAngle, _spineUniversalLocalAxes.LocalUp);
|
||||
}
|
||||
}
|
||||
|
||||
// Pop independent transforms (usually hands and other transforms with their own sensors)
|
||||
|
||||
foreach (IndependentBoneInfo boneInfo in _independentBones)
|
||||
{
|
||||
boneInfo.Transform.SetPositionAndRotation(boneInfo.Position, boneInfo.Rotation);
|
||||
}
|
||||
|
||||
_avatarNeck.SetPositionAndRotation(neckPosition, neckRotation);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Notifies whenever the avatar was moved in order to update the internal forward looking vector.
|
||||
/// </summary>
|
||||
/// <param name="e">Move event parameters</param>
|
||||
public void NotifyAvatarMoved(UxrAvatarMoveEventArgs e)
|
||||
{
|
||||
if (!Initialized)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
float angle = Vector3.SignedAngle(e.OldForward, e.NewForward, _avatar.transform.up);
|
||||
_avatarForwardTarget = Quaternion.AngleAxis(angle, _avatar.transform.up) * _avatarForwardTarget;
|
||||
_straightSpineForward = Quaternion.AngleAxis(angle, _avatar.transform.up) * _straightSpineForward;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Private Methods
|
||||
|
||||
/// <summary>
|
||||
/// Computes an world position based on an offset from an object.
|
||||
/// </summary>
|
||||
/// <param name="axes">The axes <paramref name="offset" /> refer to</param>
|
||||
/// <param name="transform">The object origin</param>
|
||||
/// <param name="offset">The offset components</param>
|
||||
/// <returns>Offset vector</returns>
|
||||
private Vector3 GetWorldPosFromOffset(Transform axes, Transform transform, Vector3 offset)
|
||||
{
|
||||
return transform.position + axes.right * offset.x + axes.up * offset.y + axes.forward * offset.z;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Computes how much of an influence an arm has on the upper body to make it rotate left or right.
|
||||
/// </summary>
|
||||
/// <param name="upperBodyTransform">Any upper body node (spine, chest...)</param>
|
||||
/// <param name="upperBodyUniversalLocalAxes">
|
||||
/// The upper body nodes's universal local axes.
|
||||
/// These will be the upper body transform local axes that map to the avatar right, up and
|
||||
/// forward vectors.
|
||||
/// </param>
|
||||
/// <param name="arm">The arm</param>
|
||||
/// <param name="handSide">Is it a left arm or right arm?</param>
|
||||
/// <returns>
|
||||
/// Value [-1.0, 1.0] that tells how much the upper body should rotate to the left or
|
||||
/// right due to the arm position.
|
||||
/// </returns>
|
||||
private float GetArmTorsoRotationWeight(Transform upperBodyTransform, UxrUniversalLocalAxes upperBodyUniversalLocalAxes, UxrAvatarArm arm, UxrHandSide handSide)
|
||||
{
|
||||
if (arm.UpperArm && arm.Forearm)
|
||||
{
|
||||
Vector3 shoulderToElbowVector = arm.Forearm.position - arm.UpperArm.position;
|
||||
Vector3 projectedShoulderToElbowVector = Vector3.ProjectOnPlane(shoulderToElbowVector, upperBodyUniversalLocalAxes.WorldUp);
|
||||
|
||||
float straightFactor = projectedShoulderToElbowVector.magnitude / shoulderToElbowVector.magnitude;
|
||||
float armAngle = Vector3.SignedAngle(upperBodyUniversalLocalAxes.WorldRight * (handSide == UxrHandSide.Left ? -1.0f : 1.0f), projectedShoulderToElbowVector, upperBodyUniversalLocalAxes.WorldUp);
|
||||
|
||||
return armAngle / 180.0f * straightFactor;
|
||||
}
|
||||
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Private Types & Data
|
||||
|
||||
private const float CameraUpsideDownAngleThreshold = 15.0f; // To avoid gimbal errors we will not update the avatar rotation if the avatar is looking up or down with this angle respect to the vertical axis.
|
||||
private const float AvatarRotationDegreesPerSecond = 1080.0f; // Degrees per second we will move the avatar to compensate for the head torsion
|
||||
private const float DegreesStraightenedPerMeterMoved = 120.0f; // Amount of degrees to straighten the avatar direction with respect to the head direction when the avatar moves.
|
||||
private const float AvatarStraighteningMinSpeed = 0.3f; // To straighten the avatar direction it will need to be moving at least this amount of units per second
|
||||
private const float BodyTorsionSmoothTime = 0.1f; // Body torsion smoothening time
|
||||
|
||||
private UxrAvatar _avatar;
|
||||
private Transform _avatarTransform;
|
||||
private UxrBodyIKSettings _settings;
|
||||
private List<IndependentBoneInfo> _independentBones;
|
||||
|
||||
private Transform _avatarBodyRoot;
|
||||
private Transform _avatarForward;
|
||||
private Transform _avatarNeck;
|
||||
private Transform _avatarHead;
|
||||
private Transform _avatarEyes;
|
||||
|
||||
private Vector3 _neckPosRelativeToEyes;
|
||||
private Quaternion _neckRotRelativeToEyes;
|
||||
private Vector3 _avatarForwardPosRelativeToNeck;
|
||||
private Vector3 _avatarForwardTarget;
|
||||
private Vector3 _straightSpineForward;
|
||||
|
||||
private UxrUniversalLocalAxes _spineUniversalLocalAxes;
|
||||
private UxrUniversalLocalAxes _chestUniversalLocalAxes;
|
||||
private UxrUniversalLocalAxes _upperChestUniversalLocalAxes;
|
||||
private UxrUniversalLocalAxes _neckUniversalLocalAxes;
|
||||
private UxrUniversalLocalAxes _headUniversalLocalAxes;
|
||||
private float _spineTorsionAngle;
|
||||
private float _chestTorsionAngle;
|
||||
private float _upperChestTorsionAngle;
|
||||
private float _spineTorsionSpeed;
|
||||
private float _chestTorsionSpeed;
|
||||
private float _upperChestTorsionSpeed;
|
||||
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 618804a1df753f84194da553f01a2025
|
||||
timeCreated: 1519294533
|
||||
licenseType: Pro
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,124 @@
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
// <copyright file="UxrBodyIKSettings.cs" company="VRMADA">
|
||||
// Copyright (c) VRMADA, All rights reserved.
|
||||
// </copyright>
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
using System;
|
||||
using UnityEngine;
|
||||
|
||||
namespace UltimateXR.Animation.IK
|
||||
{
|
||||
/// <summary>
|
||||
/// Stores parameters that drive Inverse Kinematics for full-body avatars.
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// For now only half-body Inverse Kinematics is supported. Full-body will be implemented at some point.
|
||||
/// </remarks>
|
||||
[Serializable]
|
||||
public class UxrBodyIKSettings
|
||||
{
|
||||
#region Inspector Properties/Serialized Fields
|
||||
|
||||
[SerializeField] private bool _lockBodyPivot;
|
||||
[SerializeField] private float _bodyPivotRotationSpeed = 0.2f;
|
||||
[SerializeField] private float _headFreeRangeBend = 20.0f;
|
||||
[SerializeField] private float _headFreeRangeTorsion = 30.0f;
|
||||
[SerializeField] private float _neckHeadBalance = 0.5f;
|
||||
[SerializeField] private float _spineBend = 0.05f;
|
||||
[SerializeField] private float _spineTorsion = 0.4f;
|
||||
[SerializeField] private float _chestBend = 0.3f;
|
||||
[SerializeField] private float _chestTorsion = 0.8f;
|
||||
[SerializeField] private float _upperChestBend = 0.4f;
|
||||
[SerializeField] private float _upperChestTorsion = 0.2f;
|
||||
[SerializeField] private float _neckBaseHeight = 1.6f;
|
||||
[SerializeField] private float _neckForwardOffset;
|
||||
[SerializeField] private float _eyesBaseHeight = 1.75f;
|
||||
[SerializeField] private float _eyesForwardOffset = 0.1f;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Public Types & Data
|
||||
|
||||
/// <summary>
|
||||
/// Gets whether the avatar pivot will be kept in place so that it will only rotate around.
|
||||
/// </summary>
|
||||
public bool LockBodyPivot => _lockBodyPivot;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the speed the body will turn around with. This is used to smooth out rotation.
|
||||
/// </summary>
|
||||
public float BodyPivotRotationSpeed => _bodyPivotRotationSpeed;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the amount of degrees the head can bend before requiring rotation of other bones down the spine.
|
||||
/// </summary>
|
||||
public float HeadFreeRangeBend => _headFreeRangeBend;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the amount of degrees the head can turn before requiring rotation of other bones down the spine.
|
||||
/// </summary>
|
||||
public float HeadFreeRangeTorsion => _headFreeRangeTorsion;
|
||||
|
||||
/// <summary>
|
||||
/// Gets a value in [0.0, 1.0] range that tells how rotation will be distributed between the head and the neck. 0.0
|
||||
/// will fully use the neck and 1.0 will fully use the head. Values in between will distribute it among the two.
|
||||
/// </summary>
|
||||
public float NeckHeadBalance => _neckHeadBalance;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the amount the spine will bend when the head bends.
|
||||
/// </summary>
|
||||
public float SpineBend => _spineBend;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the amount the spine will turn when the head turns.
|
||||
/// </summary>
|
||||
public float SpineTorsion => _spineTorsion;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the amount the chest will bend when the head bends.
|
||||
/// </summary>
|
||||
public float ChestBend => _chestBend;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the amount the chest will turn when the head turns.
|
||||
/// </summary>
|
||||
public float ChestTorsion => _chestTorsion;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the amount the upper chest will bend when the head bends.
|
||||
/// </summary>
|
||||
public float UpperChestBend => _upperChestBend;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the amount the upper chest will turn when the head turns.
|
||||
/// </summary>
|
||||
public float UpperChestTorsion => _upperChestTorsion;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the height of the base of the neck starting from the avatar root Y. This is used to create a dummy neck when
|
||||
/// the avatar is lacking a neck bone.
|
||||
/// </summary>
|
||||
public float NeckBaseHeight => _neckBaseHeight;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the forward offset of the neck starting from the avatar root Z. This is used to create a dummy neck when the
|
||||
/// avatar is lacking a neck bone.
|
||||
/// </summary>
|
||||
public float NeckForwardOffset => _neckForwardOffset;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the height of the eyes starting from the avatar root Y. This is used to know where to place the avatar head
|
||||
/// knowing the camera will be positioned on the eyes.
|
||||
/// </summary>
|
||||
public float EyesBaseHeight => _eyesBaseHeight;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the forward offset of the eyes starting from the avatar root Z. This is used to know where to place the avatar
|
||||
/// head knowing the camera will be positioned on the eyes.
|
||||
/// </summary>
|
||||
public float EyesForwardOffset => _eyesForwardOffset;
|
||||
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,3 @@
|
||||
fileFormatVersion: 2
|
||||
guid: a8d0b000596d4a8c9294cb3450d0fdfe
|
||||
timeCreated: 1642860814
|
||||
@@ -0,0 +1,23 @@
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
// <copyright file="UxrCcdConstraintType.cs" company="VRMADA">
|
||||
// Copyright (c) VRMADA, All rights reserved.
|
||||
// </copyright>
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
namespace UltimateXR.Animation.IK
|
||||
{
|
||||
/// <summary>
|
||||
/// Enumerates the different constraints of a CCD link.
|
||||
/// </summary>
|
||||
public enum UxrCcdConstraintType
|
||||
{
|
||||
/// <summary>
|
||||
/// Constrained to one axis.
|
||||
/// </summary>
|
||||
SingleAxis,
|
||||
|
||||
/// <summary>
|
||||
/// Constrained to two axes.
|
||||
/// </summary>
|
||||
TwoAxes
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,3 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 46ce404cf07f4b73a03b51e6675b8766
|
||||
timeCreated: 1643125910
|
||||
@@ -0,0 +1,35 @@
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
// <copyright file="UxrCcdIKSolver.IterationResult.cs" company="VRMADA">
|
||||
// Copyright (c) VRMADA, All rights reserved.
|
||||
// </copyright>
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
namespace UltimateXR.Animation.IK
|
||||
{
|
||||
public partial class UxrCcdIKSolver
|
||||
{
|
||||
#region Private Types & Data
|
||||
|
||||
/// <summary>
|
||||
/// Result of the CCD algorithm iteration
|
||||
/// </summary>
|
||||
private enum IterationResult
|
||||
{
|
||||
/// <summary>
|
||||
/// The effector has reached the goal.
|
||||
/// </summary>
|
||||
GoalReached,
|
||||
|
||||
/// <summary>
|
||||
/// The effector is still trying to reach the goal.
|
||||
/// </summary>
|
||||
ReachingGoal,
|
||||
|
||||
/// <summary>
|
||||
/// There was an error and no links were rotated in order to reach the goal.
|
||||
/// </summary>
|
||||
Error,
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,3 @@
|
||||
fileFormatVersion: 2
|
||||
guid: ffa1891b73bb4713833bc72dc78e06c6
|
||||
timeCreated: 1643742218
|
||||
421
Assets/UltimateXR/Runtime/Scripts/Animation/IK/UxrCcdIKSolver.cs
Normal file
421
Assets/UltimateXR/Runtime/Scripts/Animation/IK/UxrCcdIKSolver.cs
Normal file
@@ -0,0 +1,421 @@
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
// <copyright file="UxrCcdIKSolver.cs" company="VRMADA">
|
||||
// Copyright (c) VRMADA, All rights reserved.
|
||||
// </copyright>
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using UltimateXR.Extensions.Unity;
|
||||
using UltimateXR.Extensions.Unity.Math;
|
||||
using UnityEngine;
|
||||
|
||||
namespace UltimateXR.Animation.IK
|
||||
{
|
||||
/// <summary>
|
||||
/// Component that we use to solve IK chains using CCD (Cyclic Coordinate Descent). A chain is defined
|
||||
/// by a set of links, an effector and a goal.
|
||||
/// The links are bones that will try to make the effector reach the same exact point, or the closest to, the goal.
|
||||
/// Usually the effector is on the tip of the last bone.
|
||||
/// Each link can have different rotation constraints to simulate different behaviours and systems.
|
||||
/// </summary>
|
||||
public partial class UxrCcdIKSolver : UxrIKSolver
|
||||
{
|
||||
#region Inspector Properties/Serialized Fields
|
||||
|
||||
[SerializeField] private int _maxIterations = 10;
|
||||
[SerializeField] private float _minDistanceToGoal = 0.001f;
|
||||
[SerializeField] private List<UxrCcdLink> _links = new List<UxrCcdLink>();
|
||||
[SerializeField] private Transform _endEffector;
|
||||
[SerializeField] private Transform _goal;
|
||||
[SerializeField] private bool _constrainGoalToEffector;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Public Types & Data
|
||||
|
||||
/// <summary>
|
||||
/// Gets the list of links in the CCD.
|
||||
/// </summary>
|
||||
public IReadOnlyList<UxrCcdLink> Links => _links.AsReadOnly();
|
||||
|
||||
/// <summary>
|
||||
/// Gets the end effector, which is the point that is part of the chain that will try to match the goal position.
|
||||
/// </summary>
|
||||
public Transform EndEffector => _endEffector;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the goal, which is the goal that the chain will try to match with the <see cref="EndEffector" />.
|
||||
/// </summary>
|
||||
public Transform Goal => _goal;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Public Overrides UxrIKSolver
|
||||
|
||||
/// <inheritdoc />
|
||||
public override bool Initialized => _initialized;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Public Methods
|
||||
|
||||
/// <summary>
|
||||
/// Initializes the internal data for the IK chain. This will only need to be called once during Awake(), but inside
|
||||
/// the Unity editor we can call it also for drawing some gizmos that need it.
|
||||
/// </summary>
|
||||
public void ComputeLinkData()
|
||||
{
|
||||
if (_links != null && _endEffector != null)
|
||||
{
|
||||
for (int i = 0; i < _links.Count; ++i)
|
||||
{
|
||||
if (_links[i].Bone != null && !(i < _links.Count - 1 && _links[i + 1].Bone == null))
|
||||
{
|
||||
_links[i].MtxToLocalParent = Matrix4x4.identity;
|
||||
|
||||
if (_links[i].Bone.parent != null)
|
||||
{
|
||||
_links[i].MtxToLocalParent = _links[i].Bone.parent.worldToLocalMatrix;
|
||||
}
|
||||
|
||||
_links[i].Initialized = true;
|
||||
_links[i].InitialLocalRotation = _links[i].Bone.localRotation;
|
||||
_links[i].LocalSpaceAxis1ZeroAngleVector = _links[i].RotationAxis1.GetPerpendicularVector();
|
||||
_links[i].LocalSpaceAxis2ZeroAngleVector = _links[i].RotationAxis2.GetPerpendicularVector();
|
||||
_links[i].ParentSpaceAxis1 = _links[i].MtxToLocalParent.MultiplyVector(_links[i].Bone.TransformDirection(_links[i].RotationAxis1));
|
||||
_links[i].ParentSpaceAxis2 = _links[i].MtxToLocalParent.MultiplyVector(_links[i].Bone.TransformDirection(_links[i].RotationAxis2));
|
||||
_links[i].ParentSpaceAxis1ZeroAngleVector = _links[i].MtxToLocalParent.MultiplyVector(_links[i].Bone.TransformDirection(_links[i].LocalSpaceAxis1ZeroAngleVector));
|
||||
_links[i].ParentSpaceAxis2ZeroAngleVector = _links[i].MtxToLocalParent.MultiplyVector(_links[i].Bone.TransformDirection(_links[i].LocalSpaceAxis2ZeroAngleVector));
|
||||
_links[i].LinkLength = i == _links.Count - 1 ? Vector3.Distance(_links[i].Bone.position, _endEffector.position) : Vector3.Distance(_links[i].Bone.position, _links[i + 1].Bone.position);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Restores the initial link rotations.
|
||||
/// </summary>
|
||||
public void RestoreInitialRotations()
|
||||
{
|
||||
if (_links != null)
|
||||
{
|
||||
foreach (UxrCcdLink link in _links)
|
||||
{
|
||||
if (link.Bone != null && link.Initialized)
|
||||
{
|
||||
link.Bone.transform.localRotation = link.InitialLocalRotation;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Sets the weight of the given link.
|
||||
/// </summary>
|
||||
/// <param name="link">Link index</param>
|
||||
/// <param name="weight">Link weight [0.0f, 1.0f]</param>
|
||||
public void SetLinkWeight(int link, float weight)
|
||||
{
|
||||
if (link >= 0 && link < _links.Count)
|
||||
{
|
||||
_links[link].Weight = weight;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Sets the default values for the given link.
|
||||
/// </summary>
|
||||
/// <param name="link">Link index</param>
|
||||
public void SetLinkDefaultValues(int link)
|
||||
{
|
||||
if (link >= 0 && link < _links.Count)
|
||||
{
|
||||
_links[link] = new UxrCcdLink();
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Unity
|
||||
|
||||
/// <summary>
|
||||
/// Initializes the link data.
|
||||
/// </summary>
|
||||
protected override void Awake()
|
||||
{
|
||||
base.Awake();
|
||||
ComputeLinkData();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Checks if the goal needs to be parented so that the IK computation doesn't affect the goal itself.
|
||||
/// </summary>
|
||||
protected override void Start()
|
||||
{
|
||||
base.Start();
|
||||
|
||||
if (_goal.HasParent(_endEffector) || _links.Any(l => _goal.HasParent(l.Bone)))
|
||||
{
|
||||
_goal.SetParent(transform);
|
||||
}
|
||||
|
||||
_initialized = true;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Protected Overrides UxrIKSolver
|
||||
|
||||
/// <summary>
|
||||
/// IK solver implementation. Will try to make the end effector in the link chain to match the goal.
|
||||
/// </summary>
|
||||
protected override void InternalSolveIK()
|
||||
{
|
||||
Vector3 goalPosition = _goal.position;
|
||||
Vector3 goalForward = _goal.forward;
|
||||
|
||||
for (int i = 0; i < _maxIterations; ++i)
|
||||
{
|
||||
IterationResult result = ComputeSingleIterationCcd(_links, _endEffector, goalPosition, goalForward, _minDistanceToGoal);
|
||||
|
||||
if (result != IterationResult.ReachingGoal)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (_constrainGoalToEffector && Vector3.Distance(goalPosition, _endEffector.position) > _minDistanceToGoal)
|
||||
{
|
||||
_goal.position = _endEffector.position;
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Private Methods
|
||||
|
||||
/// <summary>
|
||||
/// Fixes an angle so that it is always in the -180, 180 degrees range.
|
||||
/// </summary>
|
||||
/// <param name="angle">Angle in degrees</param>
|
||||
/// <returns>Angle in the -180, 180 degrees range</returns>
|
||||
private static float FixAngle(float angle)
|
||||
{
|
||||
angle = angle % 360.0f;
|
||||
|
||||
if (angle > 180.0f)
|
||||
{
|
||||
angle -= 360.0f;
|
||||
}
|
||||
else if (angle < -180.0f)
|
||||
{
|
||||
angle += 360.0f;
|
||||
}
|
||||
|
||||
return angle;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Computes a single iteration of the CCD algorithm on our link chain.
|
||||
/// </summary>
|
||||
/// <param name="links">List of links (bones) of the chain</param>
|
||||
/// <param name="endEffector">The point on the chain that will try to reach the goal</param>
|
||||
/// <param name="goalPosition">The goal that the end effector will try to reach</param>
|
||||
/// <param name="goalForward">The goal forward vector that the end effector will try to reach if alignment is enabled</param>
|
||||
/// <param name="minDistanceToGoal">Minimum distance to the goal that is considered success</param>
|
||||
/// <returns>Result of the iteration</returns>
|
||||
private static IterationResult ComputeSingleIterationCcd(List<UxrCcdLink> links, Transform endEffector, Vector3 goalPosition, Vector3 goalForward, float minDistanceToGoal)
|
||||
{
|
||||
if (Vector3.Distance(goalPosition, endEffector.position) <= minDistanceToGoal)
|
||||
{
|
||||
return IterationResult.GoalReached;
|
||||
}
|
||||
|
||||
// Iterate from tip to base
|
||||
|
||||
bool linksRotated = false;
|
||||
|
||||
for (var i = links.Count - 1; i >= 0; i--)
|
||||
{
|
||||
UxrCcdLink link = links[i];
|
||||
|
||||
if (Vector3.Distance(goalPosition, endEffector.position) <= minDistanceToGoal)
|
||||
{
|
||||
return IterationResult.GoalReached;
|
||||
}
|
||||
|
||||
// Compute the matrix that transforms from world space to the parent bone's local space
|
||||
|
||||
link.MtxToLocalParent = Matrix4x4.identity;
|
||||
|
||||
if (link.Bone.parent != null)
|
||||
{
|
||||
link.MtxToLocalParent = link.Bone.parent.worldToLocalMatrix;
|
||||
}
|
||||
|
||||
// Compute the vector that rotates around axis1 corresponding to 0 degrees. It will be computed in local space of the parent link.
|
||||
|
||||
Vector3 parentSpaceAngle1Vector = link.MtxToLocalParent.MultiplyVector(link.Bone.TransformDirection(link.LocalSpaceAxis1ZeroAngleVector));
|
||||
|
||||
if (link.Constraint == UxrCcdConstraintType.TwoAxes)
|
||||
{
|
||||
// When dealing with 2 axis constraint mode we need to recompute the rotation axis in parent space
|
||||
link.ParentSpaceAxis1 = link.MtxToLocalParent.MultiplyVector(link.Bone.TransformDirection(link.RotationAxis1));
|
||||
}
|
||||
|
||||
// Using the computations above, calculate the angle1 value. This is the value of rotation in degrees corresponding to the first constraint axis
|
||||
|
||||
link.Angle1 = Vector3.SignedAngle(Vector3.ProjectOnPlane(link.ParentSpaceAxis1ZeroAngleVector, link.ParentSpaceAxis1),
|
||||
Vector3.ProjectOnPlane(parentSpaceAngle1Vector, link.ParentSpaceAxis1),
|
||||
link.ParentSpaceAxis1);
|
||||
|
||||
// Now let's rotate around axis1 if needed. We will compute the current vector from this node to the effector and also the current vector from this node
|
||||
// to the target. Our goal is to make the first vector match the second vector but we may only rotate around axis1. So what we do is project the goal vector
|
||||
// onto the plane with axis1 as its normal and this will be the result of our "valid" rotation due to the constraint.
|
||||
|
||||
Vector3 currentDirection = endEffector.position - link.Bone.position;
|
||||
Vector3 desiredDirection = goalPosition - link.Bone.position;
|
||||
|
||||
if (link.AlignToGoal)
|
||||
{
|
||||
currentDirection = endEffector.forward;
|
||||
desiredDirection = goalForward;
|
||||
}
|
||||
|
||||
Vector3 worldAxis1 = link.Bone.TransformDirection(link.RotationAxis1);
|
||||
Vector3 closestVectorAxis1Rotation = Vector3.ProjectOnPlane(desiredDirection, worldAxis1);
|
||||
|
||||
float newAxis1AngleIncrement = link.Weight * Vector3.SignedAngle(Vector3.ProjectOnPlane(currentDirection, worldAxis1), closestVectorAxis1Rotation, worldAxis1);
|
||||
float totalAngleAxis1 = FixAngle(link.Angle1 + newAxis1AngleIncrement);
|
||||
|
||||
// Now that we have computed our increment, let's see if we need to clamp it between the limits
|
||||
|
||||
if (link.Axis1HasLimits)
|
||||
{
|
||||
if (totalAngleAxis1 > link.Axis1AngleMax)
|
||||
{
|
||||
newAxis1AngleIncrement -= totalAngleAxis1 - link.Axis1AngleMax;
|
||||
}
|
||||
else if (totalAngleAxis1 < link.Axis1AngleMin)
|
||||
{
|
||||
newAxis1AngleIncrement += link.Axis1AngleMin - totalAngleAxis1;
|
||||
}
|
||||
|
||||
totalAngleAxis1 = FixAngle(link.Angle1 + newAxis1AngleIncrement);
|
||||
}
|
||||
|
||||
// Do we need to rotate?
|
||||
|
||||
if (Mathf.Approximately(newAxis1AngleIncrement, 0.0f) == false)
|
||||
{
|
||||
link.Angle1 = totalAngleAxis1;
|
||||
link.Bone.localRotation = link.InitialLocalRotation * Quaternion.AngleAxis(link.Angle1, link.RotationAxis1);
|
||||
|
||||
if (link.Constraint == UxrCcdConstraintType.TwoAxes)
|
||||
{
|
||||
link.Bone.localRotation = link.Bone.localRotation * Quaternion.AngleAxis(link.Angle2, link.RotationAxis2);
|
||||
}
|
||||
|
||||
linksRotated = true;
|
||||
}
|
||||
|
||||
if (link.Constraint == UxrCcdConstraintType.TwoAxes)
|
||||
{
|
||||
// Axis 2. Axis 2 works exactly like axis 1 but we operate on another plane
|
||||
|
||||
Vector3 parentSpaceAngle2Vector = link.MtxToLocalParent.MultiplyVector(link.Bone.TransformDirection(link.LocalSpaceAxis2ZeroAngleVector));
|
||||
|
||||
link.ParentSpaceAxis2 = link.MtxToLocalParent.MultiplyVector(link.Bone.TransformDirection(link.RotationAxis2));
|
||||
link.Angle2 = Vector3.SignedAngle(Vector3.ProjectOnPlane(link.ParentSpaceAxis2ZeroAngleVector, link.ParentSpaceAxis2),
|
||||
Vector3.ProjectOnPlane(parentSpaceAngle2Vector, link.ParentSpaceAxis2),
|
||||
link.ParentSpaceAxis2);
|
||||
|
||||
currentDirection = endEffector.position - link.Bone.position;
|
||||
desiredDirection = goalPosition - link.Bone.position;
|
||||
|
||||
if (link.AlignToGoal)
|
||||
{
|
||||
currentDirection = endEffector.forward;
|
||||
desiredDirection = goalForward;
|
||||
}
|
||||
|
||||
Vector3 worldAxis2 = link.Bone.TransformDirection(link.RotationAxis2);
|
||||
Vector3 closestVectorAxis2Rotation = Vector3.ProjectOnPlane(desiredDirection, worldAxis2);
|
||||
|
||||
float newAxis2AngleIncrement = link.Weight * Vector3.SignedAngle(Vector3.ProjectOnPlane(currentDirection, worldAxis2), closestVectorAxis2Rotation, worldAxis2);
|
||||
float totalAngleAxis2 = FixAngle(link.Angle2 + newAxis2AngleIncrement);
|
||||
|
||||
if (link.Axis2HasLimits)
|
||||
{
|
||||
if (totalAngleAxis2 > link.Axis2AngleMax)
|
||||
{
|
||||
newAxis2AngleIncrement -= totalAngleAxis2 - link.Axis2AngleMax;
|
||||
}
|
||||
else if (totalAngleAxis2 < link.Axis2AngleMin)
|
||||
{
|
||||
newAxis2AngleIncrement += link.Axis2AngleMin - totalAngleAxis2;
|
||||
}
|
||||
|
||||
totalAngleAxis2 = FixAngle(link.Angle2 + newAxis2AngleIncrement);
|
||||
}
|
||||
|
||||
if (Mathf.Approximately(newAxis2AngleIncrement, 0.0f) == false)
|
||||
{
|
||||
// Rotation order is first angle2 then angle1 because previously we have rotated in this order already
|
||||
link.Angle2 = totalAngleAxis2;
|
||||
link.Bone.localRotation = link.InitialLocalRotation * Quaternion.AngleAxis(link.Angle1, link.RotationAxis1) * Quaternion.AngleAxis(link.Angle2, link.RotationAxis2);
|
||||
|
||||
linksRotated = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return linksRotated ? Vector3.Distance(goalPosition, endEffector.position) <= minDistanceToGoal ? IterationResult.GoalReached : IterationResult.ReachingGoal : IterationResult.Error;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the transform that should be used to restore the goal position every time an IK link
|
||||
/// is reoriented.
|
||||
/// We use this in cases where we manipulate an object that the goal is part of, and the IK chain
|
||||
/// is in a hierarchy above the object/goal. This is needed because when computing the different
|
||||
/// IK steps, the goal and the object may be repositioned as a consequence, being below in the chain.
|
||||
/// As a double measure, what we try to reposition is the topmost parent that is below the IK chain,
|
||||
/// since the goal may be a dummy at the end of the chain and repositioning the goal alone would
|
||||
/// not be enough.
|
||||
/// </summary>
|
||||
/// <param name="links">List of links (bones) of the chain</param>
|
||||
/// <param name="goal">The goal that the end effector will try to reach</param>
|
||||
/// <returns>Transform that should be stored</returns>
|
||||
private static Transform GetGoalSafeRestoreTransform(List<UxrCcdLink> links, Transform goal)
|
||||
{
|
||||
Transform current = goal;
|
||||
Transform previous = goal;
|
||||
|
||||
while (current != null)
|
||||
{
|
||||
for (int i = links.Count - 1; i >= 0; --i)
|
||||
{
|
||||
if (current == links[i].Bone && current != previous)
|
||||
{
|
||||
// Found a bone. previous here is the child that we should move/rotate in order to
|
||||
// preserve the original goal position/orientation.
|
||||
return previous;
|
||||
}
|
||||
}
|
||||
|
||||
previous = current;
|
||||
current = current.parent;
|
||||
}
|
||||
|
||||
return goal;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Private Types & Data
|
||||
|
||||
private bool _initialized;
|
||||
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
fileFormatVersion: 2
|
||||
guid: fba00edd9cbdb6b43915c325836d87c8
|
||||
timeCreated: 1510133937
|
||||
licenseType: Pro
|
||||
MonoImporter:
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
185
Assets/UltimateXR/Runtime/Scripts/Animation/IK/UxrCcdLink.cs
Normal file
185
Assets/UltimateXR/Runtime/Scripts/Animation/IK/UxrCcdLink.cs
Normal file
@@ -0,0 +1,185 @@
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
// <copyright file="UxrCcdLink.cs" company="VRMADA">
|
||||
// Copyright (c) VRMADA, All rights reserved.
|
||||
// </copyright>
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
using System;
|
||||
using UnityEngine;
|
||||
|
||||
namespace UltimateXR.Animation.IK
|
||||
{
|
||||
/// <summary>
|
||||
/// Defines a link -bone- in an IK chain solved using CCD.
|
||||
/// </summary>
|
||||
/// <seealso cref="UxrCcdIKSolver" />
|
||||
[Serializable]
|
||||
public class UxrCcdLink
|
||||
{
|
||||
#region Inspector Properties/Serialized Fields
|
||||
|
||||
// Setup in the editor
|
||||
[SerializeField] private Transform _bone;
|
||||
[SerializeField] private float _weight;
|
||||
[SerializeField] private UxrCcdConstraintType _constraint;
|
||||
[SerializeField] private Vector3 _rotationAxis1;
|
||||
[SerializeField] private Vector3 _rotationAxis2;
|
||||
[SerializeField] private bool _axis1HasLimits;
|
||||
[SerializeField] private float _axis1AngleMin;
|
||||
[SerializeField] private float _axis1AngleMax;
|
||||
[SerializeField] private bool _axis2HasLimits;
|
||||
[SerializeField] private float _axis2AngleMin;
|
||||
[SerializeField] private float _axis2AngleMax;
|
||||
[SerializeField] private bool _alignToGoal;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Public Types & Data
|
||||
|
||||
/// <summary>
|
||||
/// Gets the link transform.
|
||||
/// </summary>
|
||||
public Transform Bone => _bone;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the link constraint type.
|
||||
/// </summary>
|
||||
public UxrCcdConstraintType Constraint => _constraint;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the first rotation axis.
|
||||
/// </summary>
|
||||
public Vector3 RotationAxis1 => _rotationAxis1;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the second rotation axis when there are two constraints.
|
||||
/// </summary>
|
||||
public Vector3 RotationAxis2 => _rotationAxis2;
|
||||
|
||||
/// <summary>
|
||||
/// Gets whether the first axis has rotational limits.
|
||||
/// </summary>
|
||||
public bool Axis1HasLimits => _axis1HasLimits;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the lower angle limits of the first axis.
|
||||
/// </summary>
|
||||
public float Axis1AngleMin => _axis1AngleMin;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the higher angle limits of the first axis.
|
||||
/// </summary>
|
||||
public float Axis1AngleMax => _axis1AngleMax;
|
||||
|
||||
/// <summary>
|
||||
/// Gets whether the second axis has rotational limits when there are two constraints.
|
||||
/// </summary>
|
||||
public bool Axis2HasLimits => _axis2HasLimits;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the lower angle limits of the second axis when there are two constraints.
|
||||
/// </summary>
|
||||
public float Axis2AngleMin => _axis2AngleMin;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the higher angle limits of the second axis when there are two constraints.
|
||||
/// </summary>
|
||||
public float Axis2AngleMax => _axis2AngleMax;
|
||||
|
||||
/// <summary>
|
||||
/// Gets whether the effector should not only try to position itself on the goal but also use the same orientation.
|
||||
/// </summary>
|
||||
public bool AlignToGoal => _alignToGoal;
|
||||
|
||||
/// <summary>
|
||||
/// The weight among all the CCD links in the chain.
|
||||
/// </summary>
|
||||
public float Weight
|
||||
{
|
||||
get => _weight;
|
||||
set => _weight = value;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets whether the data has been initialized.
|
||||
/// </summary>
|
||||
public bool Initialized { get; internal set; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the local rotation at the beginning.
|
||||
/// </summary>
|
||||
public Quaternion InitialLocalRotation { get; internal set; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets a reference perpendicular to axis1 that is considered as the reference of having 0 degrees around axis1.
|
||||
/// </summary>
|
||||
public Vector3 LocalSpaceAxis1ZeroAngleVector { get; internal set; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets a reference perpendicular to axis2 that is considered as the reference of having 0 degrees around axis2.
|
||||
/// </summary>
|
||||
public Vector3 LocalSpaceAxis2ZeroAngleVector { get; internal set; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the length of the link.
|
||||
/// </summary>
|
||||
public float LinkLength { get; internal set; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets <see cref="RotationAxis1" /> in local space of the parent object.
|
||||
/// </summary>
|
||||
public Vector3 ParentSpaceAxis1 { get; internal set; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets <see cref="RotationAxis2" /> in local space of the parent object.
|
||||
/// </summary>
|
||||
public Vector3 ParentSpaceAxis2 { get; internal set; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets <see cref="LocalSpaceAxis1ZeroAngleVector" /> in local space of the parent object.
|
||||
/// </summary>
|
||||
public Vector3 ParentSpaceAxis1ZeroAngleVector { get; internal set; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets <see cref="LocalSpaceAxis2ZeroAngleVector" /> in local space of the parent object.
|
||||
/// </summary>
|
||||
public Vector3 ParentSpaceAxis2ZeroAngleVector { get; internal set; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the transformation matrix that gets from world-space to local space in the parent transform.
|
||||
/// </summary>
|
||||
public Matrix4x4 MtxToLocalParent { get; internal set; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets rotation degrees around <see cref="RotationAxis1" />.
|
||||
/// </summary>
|
||||
public float Angle1 { get; internal set; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets rotation degrees around <see cref="RotationAxis2" />.
|
||||
/// </summary>
|
||||
public float Angle2 { get; internal set; }
|
||||
|
||||
#endregion
|
||||
|
||||
#region Constructors & Finalizer
|
||||
|
||||
/// <summary>
|
||||
/// Default constructor.
|
||||
/// </summary>
|
||||
public UxrCcdLink()
|
||||
{
|
||||
_weight = 1.0f;
|
||||
_constraint = UxrCcdConstraintType.SingleAxis;
|
||||
_rotationAxis1 = Vector3.right;
|
||||
_rotationAxis2 = Vector3.up;
|
||||
_axis1HasLimits = true;
|
||||
_axis1AngleMin = -45.0f;
|
||||
_axis1AngleMax = 45.0f;
|
||||
_axis2HasLimits = false;
|
||||
_axis2AngleMin = -45.0f;
|
||||
_axis2AngleMax = 45.0f;
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,3 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 34bdb75b6a3c4a7cb567e7f471b643a1
|
||||
timeCreated: 1643125714
|
||||
131
Assets/UltimateXR/Runtime/Scripts/Animation/IK/UxrIKSolver.cs
Normal file
131
Assets/UltimateXR/Runtime/Scripts/Animation/IK/UxrIKSolver.cs
Normal file
@@ -0,0 +1,131 @@
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
// <copyright file="UxrIKSolver.cs" company="VRMADA">
|
||||
// Copyright (c) VRMADA, All rights reserved.
|
||||
// </copyright>
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
using System;
|
||||
using UltimateXR.Core;
|
||||
using UltimateXR.Core.Components.Composite;
|
||||
using UnityEngine;
|
||||
|
||||
namespace UltimateXR.Animation.IK
|
||||
{
|
||||
/// <summary>
|
||||
/// Base IK Solver class. IK solvers should inherit from it and override the <see cref="InternalSolveIK" /> method.
|
||||
/// Not all solvers need to be part of an avatar, but the <see cref="UxrAvatarComponent{T}" /> inheritance is used to
|
||||
/// be able to enumerate all the solvers that are part of an avatar.
|
||||
/// </summary>
|
||||
public abstract class UxrIKSolver : UxrAvatarComponent<UxrIKSolver>
|
||||
{
|
||||
#region Inspector Properties/Serialized Fields
|
||||
|
||||
[SerializeField] private bool _enabled = true;
|
||||
[SerializeField] private bool _manualUpdate;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Public Types & Data
|
||||
|
||||
/// <summary>
|
||||
/// Called right before the IK is about to be solved during the current frame
|
||||
/// </summary>
|
||||
public event Action Solving;
|
||||
|
||||
/// <summary>
|
||||
/// Called right after the IK was solved during the current frame
|
||||
/// </summary>
|
||||
public event Action Solved;
|
||||
|
||||
/// <summary>
|
||||
/// Gets whether the component is initialized.
|
||||
/// </summary>
|
||||
public abstract bool Initialized { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets if the solver needs to be updated automatically.
|
||||
/// </summary>
|
||||
public bool NeedsAutoUpdate => gameObject.activeInHierarchy && enabled && SolverEnabled && !ManualUpdate;
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the IK solver enabled state?
|
||||
/// </summary>
|
||||
public bool SolverEnabled
|
||||
{
|
||||
get => _enabled;
|
||||
set => _enabled = value;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets if the IK solver will update itself. Otherwise the user will be responsible of calling <see cref="SolveIK" />.
|
||||
/// </summary>
|
||||
public bool ManualUpdate
|
||||
{
|
||||
get => _manualUpdate;
|
||||
set => _manualUpdate = value;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Public Methods
|
||||
|
||||
/// <summary>
|
||||
/// Solves the IK. Calls <see cref="InternalSolveIK" />,which is implemented in child classes, but calls the
|
||||
/// appropriate <see cref="Solving" /> and <see cref="Solved" /> events.
|
||||
/// </summary>
|
||||
public void SolveIK()
|
||||
{
|
||||
Solving?.Invoke();
|
||||
InternalSolveIK();
|
||||
Solved?.Invoke();
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Unity
|
||||
|
||||
/// <summary>
|
||||
/// Subscribes to events
|
||||
/// </summary>
|
||||
protected override void OnEnable()
|
||||
{
|
||||
base.OnEnable();
|
||||
UxrManager.StageUpdating += UxrManager_StageUpdating;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Unsubscribes from events
|
||||
/// </summary>
|
||||
protected override void OnDisable()
|
||||
{
|
||||
base.OnDisable();
|
||||
UxrManager.StageUpdating -= UxrManager_StageUpdating;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Event Handling Methods
|
||||
|
||||
/// <summary>
|
||||
/// Will solve the IK chain in case it is not part of an avatar. If it is part of a VR avatar, the VR avatar will take
|
||||
/// care of calling the SolveIK method so that it is processed in the correct order, after the hands are updated.
|
||||
/// </summary>
|
||||
private void UxrManager_StageUpdating(UxrUpdateStage stage)
|
||||
{
|
||||
if (stage == UxrUpdateStage.PostProcess && Avatar == null && NeedsAutoUpdate)
|
||||
{
|
||||
SolveIK();
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Protected Methods
|
||||
|
||||
/// <summary>
|
||||
/// To be implemented in child classes to execute the actual IK solving algorithm for the current frame
|
||||
/// </summary>
|
||||
protected abstract void InternalSolveIK();
|
||||
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 0c24e6ba752cd9d47b3c68e991363e16
|
||||
timeCreated: 1519111756
|
||||
licenseType: Pro
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,88 @@
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
// <copyright file="UxrWristTorsionIKSolver.cs" company="VRMADA">
|
||||
// Copyright (c) VRMADA, All rights reserved.
|
||||
// </copyright>
|
||||
// --------------------------------------------------------------------------------------------------------------------
|
||||
using UltimateXR.Core;
|
||||
using UltimateXR.Core.Math;
|
||||
using UltimateXR.Extensions.Unity;
|
||||
using UltimateXR.Extensions.Unity.Math;
|
||||
using UnityEngine;
|
||||
|
||||
namespace UltimateXR.Animation.IK
|
||||
{
|
||||
/// <summary>
|
||||
/// IK solver that distributes a wrist torsion among different bones in a forearm in order to smooth it out.
|
||||
/// </summary>
|
||||
public class UxrWristTorsionIKSolver : UxrIKSolver
|
||||
{
|
||||
#region Inspector Properties/Serialized Fields
|
||||
|
||||
[SerializeField] [Range(0.0f, 1.0f)] private float _amount = 1.0f;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Public Types & Data
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the amount of torsion to apply on this Transform from the source. 0 = no torsion, 1 = full torsion,
|
||||
/// etc.
|
||||
/// </summary>
|
||||
public float Amount
|
||||
{
|
||||
get => _amount;
|
||||
set => _amount = value;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Public Overrides UxrIKSolver
|
||||
|
||||
/// <inheritdoc />
|
||||
public override bool Initialized => _initialized;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Unity
|
||||
|
||||
/// <summary>
|
||||
/// Initializes the component.
|
||||
/// </summary>
|
||||
protected override void Awake()
|
||||
{
|
||||
base.Awake();
|
||||
|
||||
_handSide = transform.HasParent(Avatar.AvatarRig.LeftArm.UpperArm) ? UxrHandSide.Left : UxrHandSide.Right;
|
||||
_startLocalRotation = transform.localRotation;
|
||||
|
||||
UxrUniversalLocalAxes handUniversalLocalAxes = Avatar.AvatarRigInfo.GetArmInfo(_handSide).HandUniversalLocalAxes;
|
||||
_torsionLocalAxis = transform.InverseTransformDirection(handUniversalLocalAxes.WorldForward).GetClosestAxis();
|
||||
|
||||
_initialized = true;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Protected Overrides UxrIKSolver
|
||||
|
||||
/// <summary>
|
||||
/// Solves the Inverse Kinematics.
|
||||
/// </summary>
|
||||
protected override void InternalSolveIK()
|
||||
{
|
||||
float angle = Avatar.AvatarRigInfo.GetArmInfo(_handSide).WristTorsionInfo.WristTorsionAngle;
|
||||
transform.localRotation = _startLocalRotation * Quaternion.AngleAxis(angle * Amount, _torsionLocalAxis);
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Private Types & Data
|
||||
|
||||
private bool _initialized;
|
||||
private UxrHandSide _handSide;
|
||||
private Quaternion _startLocalRotation;
|
||||
private Vector3 _torsionLocalAxis;
|
||||
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
fileFormatVersion: 2
|
||||
guid: a41528363b03e1c4a8b730dee5b434ab
|
||||
timeCreated: 1510043257
|
||||
licenseType: Pro
|
||||
MonoImporter:
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
Reference in New Issue
Block a user