592 lines
28 KiB
C#
592 lines
28 KiB
C#
// --------------------------------------------------------------------------------------------------------------------
|
|
// <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
|
|
}
|
|
} |