using UnityEngine; namespace Passer.Humanoid { using Humanoid.Tracking; [System.Serializable] public class TorsoAnimator : TorsoSensor { public enum BodyRotation { HeadRotation = 1, HeadAndHandRotation = 2, NoRotation = 3, } public BodyRotation bodyRotation; private float torsoLength; private Quaternion torsoUprightOrientation; #region Start public override void Start(HumanoidControl humanoid, Transform targetTransform) { base.Start(humanoid, targetTransform); if (humanoid.avatarRig == null || hipsTarget.hips.bone.transform == null) return; Vector3 torsoDirection = humanoid.headTarget.neck.bone.transform.position - hipsTarget.hips.bone.transform.position; torsoLength = torsoDirection.magnitude; torsoUprightOrientation = Quaternion.FromToRotation(humanoid.up, torsoDirection); } #endregion #region Update public override void Update() { if (!hipsTarget.humanoid.animatorEnabled || !enabled || hipsTarget.humanoid.targetsRig == null) return; if (hipsTarget.humanoid.avatarRig == null || hipsTarget.hips.bone.transform == null) return; Animator targetAnimator = hipsTarget.humanoid.targetsRig; // Don't use procedural if the animator controller plays an animation clip if (targetAnimator.runtimeAnimatorController != null && hipsTarget.humanoid.animatorParameterForward == null && targetAnimator.GetCurrentAnimatorClipInfoCount(0) > 0) return; status = Tracker.Status.Tracking; StoreTargets(); UpdateHipsPosition(); UpdateHipsRotation(); RestoreTargets(); } #region Target Preserve private Quaternion neckRotation; private Vector3 neckPosition; private Quaternion leftHandRotation; private Vector3 leftHandPosition; private Quaternion rightHandRotation; private Vector3 rightHandPosition; private Quaternion leftFootRotation; private Vector3 leftFootPosition; private Quaternion rightFootRotation; private Vector3 rightFootPosition; private void StoreTargets() { neckRotation = hipsTarget.humanoid.headTarget.neck.target.transform.rotation; neckPosition = hipsTarget.humanoid.headTarget.neck.target.transform.position; leftHandRotation = hipsTarget.humanoid.leftHandTarget.hand.target.transform.rotation; leftHandPosition = hipsTarget.humanoid.leftHandTarget.hand.target.transform.position; rightHandRotation = hipsTarget.humanoid.rightHandTarget.hand.target.transform.rotation; rightHandPosition = hipsTarget.humanoid.rightHandTarget.hand.target.transform.position; leftFootRotation = hipsTarget.humanoid.leftFootTarget.foot.target.transform.rotation; leftFootPosition = hipsTarget.humanoid.leftFootTarget.foot.target.transform.localPosition; rightFootRotation = hipsTarget.humanoid.rightFootTarget.foot.target.transform.rotation; rightFootPosition = hipsTarget.humanoid.rightFootTarget.foot.target.transform.localPosition; } private void RestoreTargets() { hipsTarget.humanoid.headTarget.neck.target.transform.rotation = neckRotation; hipsTarget.humanoid.headTarget.neck.target.transform.position = neckPosition; hipsTarget.humanoid.leftHandTarget.hand.target.transform.rotation = leftHandRotation; hipsTarget.humanoid.leftHandTarget.hand.target.transform.position = leftHandPosition; hipsTarget.humanoid.rightHandTarget.hand.target.transform.rotation = rightHandRotation; hipsTarget.humanoid.rightHandTarget.hand.target.transform.position = rightHandPosition; hipsTarget.humanoid.leftFootTarget.foot.target.transform.rotation = leftFootRotation; hipsTarget.humanoid.leftFootTarget.foot.target.transform.localPosition = leftFootPosition; hipsTarget.humanoid.rightFootTarget.foot.target.transform.rotation = rightFootRotation; hipsTarget.humanoid.rightFootTarget.foot.target.transform.localPosition = rightFootPosition; } #endregion Target Preserve private void UpdateHipsRotation() { HeadTarget headTarget = hipsTarget.humanoid.headTarget; HandTarget leftHandTarget = hipsTarget.humanoid.leftHandTarget; HandTarget rightHandTarget = hipsTarget.humanoid.rightHandTarget; if (hipsTarget.hips.target.confidence.rotation < 0.5F) { Quaternion hipsTargetRotation = hipsTarget.hips.target.transform.rotation; Quaternion headTargetRotation = headTarget.head.target.transform.rotation; Vector3 neckTargetPosition = headTarget.neck.target.transform.position; if (bodyRotation == BodyRotation.NoRotation) { } else if (!(bodyRotation == BodyRotation.HeadRotation) && // still need to add foot based rotation (leftHandTarget.hand.target.confidence.rotation > 0.5F && rightHandTarget.hand.target.confidence.rotation > 0.5F)) { Quaternion newHipsRotation = TorsoMovements.CalculateHipsRotation(hipsTarget.hips.target.transform.position, hipsTargetRotation, leftHandTarget.transform.rotation, rightHandTarget.transform.rotation, hipsTarget.humanoid.leftFootTarget.transform, hipsTarget.humanoid.rightFootTarget.transform, headTargetRotation, neckTargetPosition); hipsTarget.hips.target.transform.rotation = newHipsRotation; } else { // Does not seem to be working reliably when the humanoid.up != Vector3.up Vector3 hipsUpDirection = hipsTarget.humanoid.up; // We need to rotate the head to the up direction to reliably determine the head forward direction Quaternion headBoneRotation = headTarget.head.bone.targetRotation; Quaternion rotationToUp = Quaternion.FromToRotation(headBoneRotation * Vector3.up, hipsTarget.humanoid.up); Vector3 hipsBackDirection = -(rotationToUp * headBoneRotation * Vector3.forward); Quaternion q = Quaternion.LookRotation(hipsUpDirection, hipsBackDirection); Quaternion hipRotation = q * Quaternion.Euler(90, 0, 0); hipsTarget.hips.target.transform.rotation = hipRotation; } } hipsTarget.hips.target.confidence.rotation = 0.2F; } protected Vector3 oldPosition; // may be neck, head of hips ATM private void UpdateHipsPosition() { if (hipsTarget.hips.target.confidence.rotation > 0.25F) return; HumanoidControl humanoid = hipsTarget.humanoid; HeadTarget headTarget = humanoid.headTarget; Vector3 oldHipPosition = hipsTarget.hips.target.transform.position; Vector3 neckPosition = headTarget.neck.target.transform.position; if (headTarget.neck.target.confidence.position == 0) neckPosition = headTarget.head.target.transform.position + headTarget.head.target.transform.rotation * (headTarget.neck2eyes - headTarget.head2eyes); // This is necessary here because the avatar can be changed or scaled. torsoLength = Vector3.Distance(headTarget.neck.bone.transform.position, hipsTarget.hips.bone.transform.position); //Quaternion headRotation = hipsTarget.humanoid.headTarget.head.target.transform.rotation; //Quaternion hipsNeckRotation = Quaternion.Slerp(headRotation, hipsTarget.hips.target.transform.rotation, 0.5F); //Debug.Log(neckRotation.eulerAngles + " " + hipsTarget.hips.target.transform.rotation.eulerAngles); Quaternion hipsNeckRotation = Quaternion.identity; Quaternion hipsHeightRotation = CalculateHipsHeightRotation(humanoid); Vector3 torsoVector = /*Vector3.down*/ -humanoid.up * torsoLength; Vector3 spine = hipsNeckRotation * hipsHeightRotation * torsoVector; Vector3 hipsPosition = neckPosition + spine; //hipsPosition = UprightCheck(humanoid, hipsPosition, neckPosition, hipsTarget.hips.target.transform.InverseTransformDirection(-spine), torsoVector); hipsTarget.hips.target.transform.position = hipsPosition; hipsTarget.hips.target.confidence.position = 0.2F; //Vector3 movementDirection = new Vector3(hipsPosition.x - oldHipPosition.x, 0, hipsPosition.z - oldHipPosition.z).normalized; //float angle = Vector3.Angle(movementDirection, humanoid.hitNormal); //if (humanoid.physics && (humanoid.collided && angle >= 90)) { // hipsTarget.hips.target.transform.position = oldHipPosition; //} // Make sure the neck position has not changed at all headTarget.neck.target.transform.position = neckPosition; oldPosition = neckPosition; } private Vector3 CalculateHipsPosition() { HumanoidControl humanoid = hipsTarget.humanoid; HeadTarget headTarget = humanoid.headTarget; Vector3 oldHipPosition = hipsTarget.hips.target.transform.position; Vector3 neckPosition = headTarget.neck.target.transform.position; // This is necessary here because the avatar can be changed or scaled. torsoLength = Vector3.Distance(headTarget.neck.bone.transform.position, hipsTarget.hips.bone.transform.position); Quaternion hipsNeckRotation = Quaternion.identity; Quaternion hipsHeightRotation = CalculateHipsHeightRotation(humanoid); Vector3 torsoVector = -humanoid.up * torsoLength; Vector3 spine = hipsNeckRotation * hipsHeightRotation * torsoVector; Vector3 hipsPosition = neckPosition + spine; //hipsTarget.hips.target.transform.position = hipsPosition; // Make sure the neck position has not changed at all //headTarget.neck.target.transform.position = neckPosition; //oldPosition = neckPosition; return hipsPosition; } private Quaternion CalculateHipsHeightRotation(HumanoidControl humanoid) { //float heightFactor = // (hipsTarget.hips.bone.transform.position.y - humanoid.transform.position.y - humanoid.leftFootTarget.soleThicknessFoot) / // (humanoid.leftFootTarget.upperLeg.bone.length + humanoid.leftFootTarget.lowerLeg.bone.length); Vector3 localHipsPosition = humanoid.transform.InverseTransformPoint(hipsTarget.hips.bone.transform.position); float hipsHeight = Vector3.Project(localHipsPosition, Vector3.up).magnitude; float heightFactor = (hipsHeight - humanoid.leftFootTarget.soleThicknessFoot) / (humanoid.leftFootTarget.upperLeg.bone.length + humanoid.leftFootTarget.lowerLeg.bone.length); if (heightFactor > 1) heightFactor = 1; Quaternion hipsHeightRotation = Quaternion.AngleAxis(60 * (1 - heightFactor), hipsTarget.hips.target.transform.right); return hipsHeightRotation; } private Vector3 UprightCheck(HumanoidControl humanoid, Vector3 hipsPosition, Vector3 neckPosition, Vector3 spineDirection, Vector3 backVector) { float headPositionY = neckPosition.y - humanoid.transform.position.y; float hipsAngle = Vector3.Angle(humanoid.up, spineDirection); float verticalBodyStretch = headPositionY - humanoid.avatarNeckHeight; if (verticalBodyStretch >= -0.01F && (hipsAngle < 20 || Mathf.Abs(spineDirection.normalized.x) < 0.2F || Mathf.Abs(spineDirection.z) < 0.15F)) { // standing upright //Debug.Log("Upright " + verticalBodyStretch + " " + hipsAngle + " " + spineDirection.normalized); Vector3 uprightSpine = humanoid.transform.rotation * torsoUprightOrientation * backVector; Vector3 targetHipPosition = neckPosition + uprightSpine; Vector3 toTargetHipPosition = targetHipPosition - hipsPosition; if (hipsAngle < 30) hipsPosition = hipsPosition + Vector3.ClampMagnitude(toTargetHipPosition, 0.02F); else hipsPosition = targetHipPosition; } //else // Debug.Log("Bent"); return hipsPosition; } public static Quaternion CalculateChestRotation(Quaternion chestRotation, Quaternion hipRotation, Quaternion headRotation) { Vector3 chestAngles = chestRotation.eulerAngles; Vector3 headAnglesCharacterSpace = (Quaternion.Inverse(hipRotation) * headRotation).eulerAngles; float chestYRotation = UnityAngles.Normalize(headAnglesCharacterSpace.y) * 0.3F; Quaternion newChestRotation = hipRotation * Quaternion.Euler(chestAngles.x, chestYRotation, chestAngles.z); return newChestRotation; } #endregion } }