using UnityEngine; namespace Passer.Humanoid { [System.Serializable] public class ArmMovements : Movements { #if UNITY_EDITOR private static readonly bool debug = false; #endif #region Update public static void Update(HandTarget handTarget) { if (handTarget == null || handTarget.hand.bone.transform == null) return; var handTargetBone = handTarget.hand.target; var forearmTargetBone = handTarget.forearm.target; var upperArmTargetBone = handTarget.upperArm.target; if (handTargetBone.confidence.position <= 0.21F) { if (forearmTargetBone.confidence.position > 0.9F && forearmTargetBone.confidence.rotation >= 0.9F) { handTargetBone.transform.position = forearmTargetBone.transform.TransformPoint(handTarget.outward * handTarget.forearm.bone.length); } else if (forearmTargetBone.confidence.rotation >= 0.9F && handTargetBone.confidence.rotation >= 0.9F) { handTargetBone.transform.position = CalculateHandPositionFromForearmRotation(handTarget, forearmTargetBone.transform.rotation); } else if (handTargetBone.confidence.rotation >= 0.9F) { handTargetBone.transform.position = CalculateHandPosition(handTarget); } else if (forearmTargetBone.confidence.rotation >= 0.9F) { handTargetBone.transform.position = CalculateHandPositionFromForearm(handTarget, forearmTargetBone.transform.rotation); } } if (handTargetBone.confidence.rotation <= 0.21F) { if (forearmTargetBone.confidence.rotation >= 0.9F) handTargetBone.transform.rotation = forearmTargetBone.transform.rotation; } if (!Application.isPlaying) { handTarget.armMovements.FullInverseKinematics(handTarget, handTarget.rotationSpeedLimitation); } else if (handTarget.humanoid.targetsRig.runtimeAnimatorController != null && handTarget.hand.target.confidence.position == 0) { handTarget.armMovements.FullForwardKinematics(handTarget); } else { if ((handTargetBone.confidence.position >= forearmTargetBone.confidence.rotation && handTargetBone.confidence.position >= upperArmTargetBone.confidence.rotation) || (forearmTargetBone.confidence.position >= 0.9F && forearmTargetBone.confidence.rotation >= 0.9F) ) handTarget.armMovements.FullInverseKinematics(handTarget, handTarget.rotationSpeedLimitation); else if ((handTarget.forearm.target.confidence.position > handTarget.upperArm.target.confidence.rotation) || (forearmTargetBone.confidence.rotation > upperArmTargetBone.confidence.rotation)) { handTarget.armMovements.ForearmForwardKinematics(handTarget); } else { handTarget.armMovements.FullForwardKinematics(handTarget); } } } protected static Vector3 CalculateHandPosition(HandTarget handTarget) { #if UNITY_EDITOR if (debug) Debug.Log(handTarget.side + " CalculateHandPosition"); #endif Quaternion hipsYRotation = Quaternion.AngleAxis(handTarget.humanoid.hipsTarget.transform.eulerAngles.y, handTarget.humanoid.up); Vector3 pivotPoint = handTarget.humanoid.hipsTarget.hips.bone.transform.position + hipsYRotation * (handTarget.isLeft ? new Vector3(-0.25F, 0.15F, -0.05F) : new Vector3(0.25F, 0.15F, -0.05F)); Quaternion forearmRotation; if (handTarget.forearm.target.confidence.rotation >= handTarget.hand.target.confidence.rotation) forearmRotation = handTarget.forearm.target.transform.rotation * (handTarget.isLeft ? Quaternion.Euler(0, -90, 0) : Quaternion.Euler(0, 90, 0)); else forearmRotation = handTarget.hand.target.transform.rotation * (handTarget.isLeft ? Quaternion.Euler(0, -90, 0) : Quaternion.Euler(0, 90, 0)); Vector3 localForearmDirection = handTarget.humanoid.hipsTarget.transform.InverseTransformDirection(forearmRotation * Vector3.forward); if (localForearmDirection.x < 0 || localForearmDirection.y > 0) { pivotPoint += hipsYRotation * Vector3.forward * Mathf.Lerp(0, 0.15F, -localForearmDirection.x * 3 + localForearmDirection.y); } if (localForearmDirection.y > 0) { pivotPoint += hipsYRotation * Vector3.up * Mathf.Lerp(0, 0.2F, localForearmDirection.y); } if (localForearmDirection.z < 0.2F) { localForearmDirection = new Vector3(localForearmDirection.x, localForearmDirection.y, 0.2F); forearmRotation = Quaternion.LookRotation(handTarget.humanoid.hipsTarget.transform.TransformDirection(localForearmDirection), forearmRotation * Vector3.up); } Vector3 handPosition = pivotPoint + forearmRotation * Vector3.forward * handTarget.forearm.bone.length; return handPosition; } protected static Vector3 CalculateHandPositionFromForearm(HandTarget handTarget, Quaternion forearmRotation) { #if UNITY_EDITOR if (debug) Debug.Log(handTarget.side + " CalculateHandPositionFromForearm "); #endif HipsTarget hipsTarget = handTarget.humanoid.hipsTarget; Quaternion hipsYRotation = Quaternion.AngleAxis(hipsTarget.transform.eulerAngles.y, handTarget.humanoid.up); Vector3 pivotPoint = hipsTarget.hips.bone.transform.position + hipsYRotation * (handTarget.isLeft ? new Vector3(-0.25F, 0.15F, -0.05F) : new Vector3(0.25F, 0.15F, -0.05F)); forearmRotation = handTarget.forearm.target.transform.rotation * Quaternion.AngleAxis(handTarget.isLeft ? -90 : 90, Vector3.up); Vector3 forearmDirection = forearmRotation * Vector3.forward; Vector3 localForearmDirection = hipsTarget.transform.InverseTransformDirection(forearmDirection); if (localForearmDirection.x < 0 || localForearmDirection.y > 0) pivotPoint += hipsYRotation * Vector3.forward * Mathf.Lerp(0, 0.15F, -localForearmDirection.x * 3 + localForearmDirection.y); if (localForearmDirection.y > 0) pivotPoint += hipsYRotation * Vector3.up * Mathf.Lerp(0, 0.2F, localForearmDirection.y); if (localForearmDirection.z < 0.2F) { localForearmDirection = new Vector3(localForearmDirection.x, localForearmDirection.y, 0.2F); forearmDirection = hipsTarget.transform.TransformDirection(localForearmDirection); forearmRotation = Quaternion.LookRotation(hipsTarget.transform.TransformDirection(forearmDirection), forearmRotation * Vector3.up); } Vector3 handPosition = pivotPoint + forearmRotation * Vector3.forward * handTarget.forearm.bone.length; return handPosition; } protected static Vector3 CalculateHandPositionFromForearmRotation(HandTarget handTarget, Quaternion forearmRotation) { #if UNITY_EDITOR if (debug) Debug.Log(handTarget.side + " CalculateHandPositionFromForearmRotation "); #endif Vector3 pivotPoint = handTarget.forearm.bone.transform.position; Vector3 handPosition = pivotPoint + forearmRotation * Vector3.forward * handTarget.forearm.bone.length; return handPosition; } public void FullInverseKinematics(HandTarget handTarget, bool speedLimitation = true) { #if UNITY_EDITOR if (debug) Debug.Log(handTarget.side + " FullInverseKinematics "); #endif Vector3 handPosition = NaturalHandPosition(handTarget); Quaternion handOrientation = NaturalHandRotation(handTarget);//handTarget.hand.bone.targetRotation; if (handTarget.shoulder.target.transform != null) { Quaternion shoulderRotation = NaturalShoulderOrientation(handTarget, ref lastLocalShoulderRotation); handTarget.shoulder.SetBoneRotation(shoulderRotation); } CalculateStretchlessTarget(handTarget); handPosition = NaturalHandPosition(handTarget); if (handTarget.upperArm.target.transform != null) { Quaternion upperArmRotation = handTarget.armMovements.NaturalUpperArmOrientation(handTarget, handPosition, handOrientation, speedLimitation); handTarget.upperArm.SetBoneRotation(upperArmRotation); if (handTarget.forearm.target.transform != null) { Vector3 armUp = upperArmRotation * Vector3.up; Quaternion forearmRotationC = Cerebellum_CalculateArmBoneOrientation(handTarget.forearm.bone.transform.position, armUp, handPosition, handTarget.isLeft); Quaternion forearmRotation = NaturalForearmOrientation(handTarget, upperArmRotation, handPosition, speedLimitation); handTarget.forearm.SetBoneRotation(forearmRotation); if (!Application.isPlaying || !handTarget.humanoid.physics || (handTarget.handRigidbody != null && handTarget.handRigidbody.isKinematic)) { if (handTarget.forearm.bone.transform != null) { handOrientation = handTarget.armMovements.CalculateHandOrientation(handTarget, forearmRotation); handTarget.hand.SetBoneRotation(handOrientation); handTarget.hand.bone.transform.rotation = HandRotationLimitations(handTarget, handTarget.hand.bone.transform.rotation); if (!Application.isPlaying || handTarget.stretchlessTarget != null) { // We need to set the hand position because it is detached PlaceHandOnForearm(handTarget, forearmRotation); } else { handTarget.hand.bone.transform.position = handPosition; } } else { handTarget.hand.bone.transform.rotation = handTarget.hand.target.transform.rotation * handTarget.hand.target.toBoneRotation; handTarget.hand.bone.transform.position = handTarget.hand.target.transform.position; } } } } } // Forearm Forward Kinematics, rest Inverse Kinematics private void ForearmForwardKinematics(HandTarget handTarget) { #if UNITY_EDITOR if (debug) Debug.Log(handTarget.side + " ForearmForwardKinematics "); #endif Vector3 upperArmUp = CalculateUpperArmUp(handTarget.forearm.target.transform.rotation); Quaternion upperArmRotation = CalculateUpperArmRotation(handTarget.upperArm.bone.transform.position, upperArmUp, handTarget.forearm.target.transform.position, handTarget.isLeft); handTarget.upperArm.bone.transform.rotation = upperArmRotation * handTarget.upperArm.target.toBoneRotation; handTarget.forearm.bone.transform.rotation = handTarget.forearm.target.transform.rotation * handTarget.forearm.target.toBoneRotation; CalculateStretchlessTarget(handTarget); if (!handTarget.humanoid.physics || handTarget.handRigidbody.isKinematic) { //Quaternion handOrientation = NaturalHandOrientation(forearmRotation); // Not working good enough yet if (handTarget.hand.target.confidence.rotation > 0.1F) { Quaternion handOrientation = handTarget.hand.target.transform.rotation; handTarget.hand.bone.transform.rotation = handOrientation * handTarget.hand.target.toBoneRotation; } else handTarget.hand.bone.transform.rotation = handTarget.forearm.target.transform.rotation * handTarget.hand.target.toBoneRotation; // We need to set the hand position because it is detached handTarget.hand.bone.transform.position = handTarget.forearm.bone.transform.position + handTarget.forearm.target.transform.rotation * handTarget.outward * handTarget.forearm.bone.length; } } // Forward Kinematics private void FullForwardKinematics(HandTarget handTarget) { #if UNITY_EDITOR if (debug) Debug.Log(handTarget.side + " FullForwardKinematics "); #endif if (handTarget.humanoid.physics && (handTarget.handRigidbody == null || !handTarget.handRigidbody.isKinematic)) { // Hand is colliding, so we need to use Inverse Kinematics FullInverseKinematics(handTarget); } // Still needs rotation speed limits handTarget.shoulder.SetBoneRotation(handTarget.shoulder.target.transform.rotation); // Still needs rotation speed limits handTarget.upperArm.SetBoneRotation(handTarget.upperArm.target.transform.rotation); //Quaternion forearmRotation = NaturalForearmOrientation(upperArmRotation); // Not working yet handTarget.forearm.SetBoneRotation(handTarget.forearm.target.transform.rotation); CalculateStretchlessTarget(handTarget); if (!handTarget.humanoid.physics || handTarget.handRigidbody.isKinematic) { //Quaternion handOrientation = NaturalHandOrientation(forearmRotation); // Not working good enough yet Quaternion handOrientation = handTarget.hand.target.transform.rotation; handTarget.hand.bone.transform.rotation = handOrientation * handTarget.hand.target.toBoneRotation; // We need to set the hand position because it is detached PlaceHandOnForearm(handTarget, handTarget.forearm.target.transform.rotation); } } private static void CalculateStretchlessTarget(HandTarget handTarget) { if (handTarget.stretchlessTarget == null) return; if (handTarget.upperArm.bone.transform != null && handTarget.forearm.bone.transform != null) { float armLength = handTarget.upperArm.bone.length + handTarget.forearm.bone.length; //handTarget.transform.position = handTarget.hand.targetTransform.position; // Don't do this, because it moves the custom targets... Vector3 armDirection = handTarget.hand.target.transform.position - handTarget.upperArm.bone.transform.position; float distance = armDirection.magnitude; if (distance > armLength + 0.0F) { handTarget.stretchlessTarget.position = handTarget.upperArm.bone.transform.position + armDirection.normalized * armLength; //handTarget.stretchlessTarget.position = handTarget.hand.target.transform.position; return; } } handTarget.stretchlessTarget.localPosition = Vector3.zero; } #endregion #region Shoulder private Quaternion lastLocalShoulderRotation = Quaternion.identity; private static Quaternion NaturalShoulderOrientation(HandTarget handTarget, ref Quaternion lastLocalShoulderRotation) { if (handTarget.shoulder.target.confidence.rotation > 0.5F) return handTarget.shoulder.target.transform.rotation; HipsTarget hipsTarget = handTarget.humanoid.hipsTarget; Quaternion torsoRotation; if (hipsTarget.chest.bone.transform != null) torsoRotation = hipsTarget.chest.bone.targetRotation; else torsoRotation = Quaternion.LookRotation(hipsTarget.hips.bone.targetRotation * Vector3.forward, handTarget.humanoid.up); Vector3 upperArmBasePosition = handTarget.shoulder.bone.transform.position + torsoRotation * handTarget.shoulder.target.baseRotation * handTarget.outward * handTarget.shoulder.bone.length; float upperArm2HandDistance = Vector3.Distance(upperArmBasePosition, handTarget.hand.target.transform.position); float armLength = handTarget.upperArm.bone.length + handTarget.forearm.bone.length; float distanceToTarget = upperArm2HandDistance - armLength - 0.03F; Quaternion shoulderRotation = handTarget.shoulder.bone.targetRotation; if (distanceToTarget > 0) { // we need to use the shoulder now to get closer to the target Vector3 targetDirection = handTarget.hand.bone.transform.position - handTarget.shoulder.bone.transform.position; Quaternion toTargetRotation = Quaternion.LookRotation(targetDirection) * Quaternion.AngleAxis(handTarget.isLeft ? 90 : -90, Vector3.up); shoulderRotation = Quaternion.Slerp(torsoRotation * handTarget.shoulder.bone.baseRotation, toTargetRotation, distanceToTarget * 4); } else if (distanceToTarget < 0) { shoulderRotation = Quaternion.Slerp(shoulderRotation, torsoRotation * handTarget.shoulder.bone.baseRotation, -distanceToTarget * 10); } if (handTarget.shoulder.bone.jointLimitations) shoulderRotation = LimitAngle(handTarget.shoulder, ref lastLocalShoulderRotation, shoulderRotation); return shoulderRotation; } #endregion #region UpperArm public Quaternion NaturalUpperArmOrientation(HandTarget handTarget, Vector3 handPosition, Quaternion handOrientation, bool speedLimitation) { //if (handTarget.upperArm.bone.transform == null) // return handTarget.upperArm.target.transform.rotation; Quaternion oldUpperArmRotation = handTarget.upperArm.bone.targetRotation; Quaternion upperArmRotation = CalculateUpperArmRotation(handTarget, handPosition, handOrientation); if (speedLimitation) upperArmRotation = LimitRotationSpeed(oldUpperArmRotation, upperArmRotation); return upperArmRotation; } private static Quaternion CalculateUpperArmRotation(Vector3 upperArmPosition, Vector3 upperArmUp, Vector3 forearmPosition, bool isLeft) { Vector3 upperArmForward = forearmPosition - upperArmPosition; Quaternion upperArmRotation = Quaternion.LookRotation(upperArmForward, upperArmUp); if (isLeft) upperArmRotation *= Quaternion.Euler(0, 90, 0); else upperArmRotation *= Quaternion.Euler(0, -90, 0); return upperArmRotation; } private Quaternion CalculateUpperArmRotation(HandTarget handTarget, Vector3 handPosition, Quaternion handRotation) { Vector3 upperArmUp = GetElbowAxis(handTarget, handPosition, handRotation); return CalculateUpperArmRotation2(handTarget, upperArmUp, handPosition); } public static Quaternion UpperArmRotationIK(Vector3 upperArmPosition, Vector3 handPosition, Vector3 elbowAxis, float upperArmLength, float forearmLength, bool isLeft) { Vector3 upperArmDirection = handPosition - upperArmPosition; float upperArm2HandDistance = upperArmDirection.magnitude; float upperArmAngle = CosineRule(upperArm2HandDistance, upperArmLength, forearmLength); if (isLeft) upperArmAngle = -upperArmAngle; Quaternion upperArmRotation = Quaternion.LookRotation(upperArmDirection, elbowAxis); upperArmRotation = Quaternion.AngleAxis(upperArmAngle, upperArmRotation * Vector3.up) * upperArmRotation; upperArmRotation *= Quaternion.Euler(0, isLeft ? 90 : -90, 0); return upperArmRotation; } // get upper arm up from target positions private Vector3 GetElbowAxis(HandTarget handTarget, Vector3 handPosition, Quaternion handRotation) { // Something is not right here when using Neuron only Vector3 upperArmUp; // if (handTarget.forearm.target.confidence.rotation < 0.5F) { upperArmUp = CalculateElbowAxis(handTarget, handPosition, handRotation); // } // else { // upperArmUp = handTarget.forearm.target.transform.up; //Debug.DrawRay(handTarget.forearm.target.transform.position, upperArmUp, Color.magenta); // } return upperArmUp; } public Vector3 CalculateElbowAxis(HandTarget handTarget, Vector3 handPosition, Quaternion handRotation) { HipsTarget hipsTarget = handTarget.humanoid.hipsTarget; Quaternion hipsOrientation = Quaternion.LookRotation(hipsTarget.hips.target.transform.forward, handTarget.humanoid.up); Vector3 elbowAxis = Quaternion.Inverse(hipsOrientation) * handRotation * Vector3.up; if (handTarget.isLeft) elbowAxis = (elbowAxis + (Vector3.left + Vector3.up).normalized) / 2; else elbowAxis = (elbowAxis + (Vector3.right + Vector3.up).normalized) / 2; float elbowAxisY = elbowAxis.y; float elbowAxisZ = elbowAxis.z; Vector3 dHandUpper = Quaternion.Inverse(hipsOrientation) * (handPosition - handTarget.upperArm.bone.transform.position); if (!handTarget.isLeft) { if (dHandUpper.x < 0) elbowAxisZ -= dHandUpper.x * 10; } else { if (dHandUpper.x > 0) elbowAxisZ += dHandUpper.x * 10; } if (dHandUpper.y > 0) elbowAxisY += dHandUpper.y * 10; elbowAxisY = Mathf.Clamp(elbowAxisY, 0.01F, 1); elbowAxisZ = Mathf.Clamp(elbowAxisZ, -0.1F, 0.1F); elbowAxis = hipsOrientation * new Vector3(elbowAxis.x, elbowAxisY, elbowAxisZ); return elbowAxis; } public static Vector3 CalculateUpperArmUp(Quaternion forearmRotation) { return forearmRotation * Vector3.up; } private Quaternion lastLocalUpperArmRotation = Quaternion.identity; public Quaternion CalculateUpperArmRotation2(HandTarget handTarget, Vector3 upperArmUp, Vector3 handPosition) { float upperArm2HandDistance = Vector3.Distance(handTarget.upperArm.bone.transform.position, handPosition); float upperArmAngle = CosineRule(upperArm2HandDistance, handTarget.upperArm.bone.length, handTarget.forearm.bone.length); if (handTarget.isLeft) upperArmAngle = -upperArmAngle; Vector3 upperArmForward = handPosition - handTarget.upperArm.bone.transform.position; Quaternion upperArmRotation = Quaternion.LookRotation(upperArmForward, upperArmUp); upperArmRotation = Quaternion.AngleAxis(upperArmAngle, upperArmRotation * Vector3.up) * upperArmRotation; if (handTarget.isLeft) upperArmRotation *= Quaternion.Euler(0, 90, 0); else upperArmRotation *= Quaternion.Euler(0, -90, 0); if (handTarget.upperArm.bone.jointLimitations) upperArmRotation = LimitAngle(handTarget.upperArm, ref lastLocalUpperArmRotation, upperArmRotation); return upperArmRotation; } #endregion #region Forearm private static Quaternion Cerebellum_CalculateArmBoneOrientation(Vector3 bonePosition, Vector3 boneUp, Vector3 nextBonePosition, bool isLeft) { Vector3 boneForward = nextBonePosition - bonePosition; Quaternion boneOrientation = Quaternion.LookRotation(boneForward, boneUp); boneOrientation = boneOrientation * Quaternion.AngleAxis(isLeft ? 90.0F : -90.0F, Vector3.up); return boneOrientation; } public static Quaternion NaturalForearmOrientation(HandTarget handTarget, Quaternion upperArmRotation, Vector3 handPosition, bool speedLimitation) { Quaternion oldForearmRotation = handTarget.forearm.bone.transform.rotation * handTarget.forearm.bone.toTargetRotation; float forearmAngle = CalculateForearmAngle(handTarget, handTarget.upperArm.bone.transform.position, handPosition, handTarget.upperArm.bone.length, handTarget.forearm.bone.length); Quaternion localForearmOrientation = Quaternion.AngleAxis(forearmAngle, Vector3.up); Quaternion forearmOrientation = upperArmRotation * localForearmOrientation; if (speedLimitation) forearmOrientation = LimitRotationSpeed(oldForearmRotation, forearmOrientation); Vector3 forearmUp = (handTarget.hand.bone.targetRotation * Vector3.up + handTarget.upperArm.bone.targetRotation * Vector3.up) / 2; forearmOrientation = Quaternion.LookRotation(forearmOrientation * handTarget.outward, forearmUp); forearmOrientation *= Quaternion.AngleAxis(handTarget.isLeft ? 90 : -90, Vector3.up); return forearmOrientation; } public static Quaternion ForearmRotationIK(Vector3 forearmPosition, Vector3 handPosition, Quaternion handRotation, Quaternion upperArmRotation, bool isLeft) { Vector3 elbowAxis = (handRotation * Vector3.up/* + upperArmRotation * Vector3.up) / 2;*/); return ForearmRotationIK(forearmPosition, handPosition, elbowAxis, isLeft); } public static Quaternion ForearmRotationIK(Vector3 forearmPosition, Vector3 handPosition, Vector3 elbowAxis, bool isLeft) { Quaternion forearmRotation = ArmBoneRotationIK(forearmPosition, handPosition, elbowAxis, isLeft); //Vector3 forearmDirection = handPosition - forearmPosition; //Quaternion forearmRotation = Quaternion.LookRotation(forearmDirection, elbowAxis); //if (isLeft) // forearmRotation *= Quaternion.Euler(0, 90, 0); //else // forearmRotation *= Quaternion.Euler(0, -90, 0); return forearmRotation; } public static float CalculateForearmAngle(HandTarget handTarget, Vector3 upperArmPosition, Vector3 handPosition, float upperArmLength, float forearmLength) { float upperArmLength2 = upperArmLength * upperArmLength; float forearmLength2 = forearmLength * forearmLength; float upperArm2HandDistance = Vector3.Distance(upperArmPosition, handPosition); float upperArm2HandDistance2 = upperArm2HandDistance * upperArm2HandDistance; float forearmAngle = Mathf.Acos((upperArmLength2 + forearmLength2 - upperArm2HandDistance2) / (2 * upperArmLength * forearmLength)) * Mathf.Rad2Deg; if (float.IsNaN(forearmAngle)) forearmAngle = 180; if (handTarget.isLeft) forearmAngle = 180 - forearmAngle; else forearmAngle = forearmAngle - 180; if (handTarget.forearm.bone.jointLimitations) { if (handTarget.isLeft) forearmAngle = Mathf.Clamp(forearmAngle, 0, handTarget.forearm.bone.maxAngle); else forearmAngle = Mathf.Clamp(forearmAngle, -handTarget.forearm.bone.maxAngle, 0); } return forearmAngle; } //public float CalculateForearmAngle(HandTarget handTarget, Quaternion upperArmOrientation) { // Quaternion localForearmOrienation = Quaternion.Inverse(upperArmOrientation) * handTarget.forearm.target.transform.rotation; // Vector3 forearmAngles = localForearmOrienation.eulerAngles; // //if (handTarget.jointLimitations) // // forearmAngles = LimitAngles(handTarget.forearm, forearmAngles); // return forearmAngles.y; //} #endregion #region Hand private Quaternion lastLocalHandRotation = Quaternion.identity; private Quaternion CalculateHandOrientation(HandTarget handTarget, Quaternion forearmOrientation) { Quaternion handOrientation; if (handTarget.hand.target.confidence.position > 0.5F && handTarget.hand.target.confidence.rotation < 0.5F) handOrientation = CalculateHandOrientation(handTarget); else handOrientation = handTarget.hand.target.transform.rotation; if (handTarget.hand.bone.jointLimitations) handOrientation = LimitAngle(handTarget.hand, ref lastLocalHandRotation, handOrientation); handOrientation = HandRotationLimitations(handTarget, handOrientation); return handOrientation; } private Quaternion CalculateHandOrientation(HandTarget handTarget) { Vector3 forwarmForward = handTarget.hand.target.transform.position - handTarget.forearm.target.transform.position; Vector3 forearmUp = handTarget.forearm.bone.targetRotation * Vector3.up; Quaternion handOrientation = Quaternion.LookRotation(forwarmForward, forearmUp); handOrientation *= Quaternion.AngleAxis(handTarget.isLeft ? 90 : -90, Vector3.up); return handOrientation; } private static Quaternion NaturalHandRotation(HandTarget handTarget) { Quaternion handRotation = handTarget.hand.bone.targetRotation; handRotation = HandRotationLimitations(handTarget, handRotation); return handRotation; } private static Vector3 NaturalHandPosition(HandTarget handTarget) { Vector3 handPosition = GetHandPosition(handTarget); handPosition = HandLimitations(handTarget, handPosition); return handPosition; } private static Vector3 GetHandPosition(HandTarget handTarget) { if (Application.isPlaying && (handTarget.humanoid.physics && (handTarget.handRigidbody == null || !handTarget.handRigidbody.isKinematic) || handTarget.stretchlessTarget == null)) return handTarget.hand.bone.transform.position; else if (handTarget.stretchlessTarget != null) return handTarget.stretchlessTarget.position; else if (handTarget.hand.target.transform != null) return handTarget.hand.target.transform.position; else return Vector3.zero; } private static void PlaceHandOnForearm(HandTarget handTarget, Quaternion forearmRotation) { handTarget.hand.bone.transform.position = handTarget.forearm.bone.transform.position + forearmRotation * handTarget.outward * handTarget.forearm.bone.length; } #endregion #region Limitations private static Vector3 HandLimitations(HandTarget handTarget, Vector3 position) { handTarget.hand.bone.transform.position = position; if (handTarget.grabbedObject != null && handTarget.grabSocket.attachedHandle != null) { MechanicalJoint mechanicalJoint = handTarget.grabbedObject.GetComponent(); if (mechanicalJoint != null && mechanicalJoint.enabled) { //Vector3 correctionVector = kinematicLimitations.GetCorrectionVector(); //Vector3 socketPosition = handTarget.grabSocket.worldPosition; //Debug.DrawRay(handTarget.grabbedObject.transform.position, correctionVector, Color.magenta); //handTarget.grabSocket.attachedHandle.SetPosition(socketPosition + correctionVector); return position; //handTarget.hand.target.transform.position + correctionVector; } } return position; } private static Quaternion HandRotationLimitations(HandTarget handTarget, Quaternion rotation) { //handTarget.hand.bone.transform.rotation = rotation; if (handTarget.grabbedObject != null && handTarget.grabSocket.attachedHandle != null) { //KinematicLimitations kinematicJoint = handTarget.grabbedObject.GetComponent(); //if (kinematicJoint != null && kinematicJoint.enabled) { // Handle handle = handTarget.grabbedObject.GetComponent(); // if (handle != null) { // Quaternion socketRotation = handTarget.grabSocket.worldRotation; // Quaternion correction = kinematicJoint.GetCorrectionAxisRotation(kinematicJoint.limitAngleAxis); // handle.SetRotation(socketRotation);// * correction); // //handTarget.grabbedObject.transform.rotation = handTarget.hand.target.transform.rotation * correction; // return handTarget.hand.target.transform.rotation * correction; // } //} //KinematicLimitations kinematicLimitations = handTarget.grabbedObject.GetComponent(); //if (kinematicLimitations != null && kinematicLimitations.enabled) { // Quaternion correctionRotation = kinematicLimitations.GetCorrectionRotation(); // correctionRotation *= handTarget.hand.target.toBoneRotation; // return rotation * correctionRotation; //} } return rotation; } #endregion private static Quaternion IK(Vector3 bone1position, float bone1length, Vector3 bone1up, float bone2length, Vector3 targetPosition, bool isLeft) { Vector3 bone1forward = targetPosition - bone1position; float distance2target = bone1forward.magnitude; float bone1angle = CosineRule(distance2target, bone1length, bone2length); if (isLeft) bone1angle = -bone1angle; Quaternion bone1rotation = Quaternion.LookRotation(bone1forward, bone1up); bone1rotation = Quaternion.AngleAxis(bone1angle, bone1up) * bone1rotation; if (isLeft) bone1rotation *= Quaternion.Euler(0, 90, 0); else bone1rotation *= Quaternion.Euler(0, -90, 0); return bone1rotation; } public static Quaternion CalculateBoneRotation(Vector3 bonePosition, Vector3 parentBonePosition) { Vector3 direction = bonePosition - parentBonePosition; if (direction.magnitude > 0) { return Quaternion.LookRotation(direction); } else return Quaternion.identity; } public static Quaternion CalculateBoneRotation(Vector3 bonePosition, Vector3 parentBonePosition, Vector3 upDirection) { Vector3 direction = bonePosition - parentBonePosition; if (direction.magnitude > 0) { return Quaternion.LookRotation(direction, upDirection); } else return Quaternion.identity; } public static Quaternion ArmBoneRotationIK(Vector3 bonePosition, Vector3 targetPosition, Vector3 upAxis, bool isLeft) { Vector3 boneDirection = targetPosition - bonePosition; Quaternion boneRotation = Quaternion.LookRotation(boneDirection, upAxis); boneRotation *= Quaternion.Euler(0, isLeft ? 90 : -90, 0); return boneRotation; } public static float CosineRule(float a, float b, float c) { float a2 = a * a; float b2 = b * b; float c2 = c * c; double angle = System.Math.Acos((a2 + b2 - c2) / (2 * a * b)) * Mathf.Rad2Deg; if (double.IsNaN(angle)) angle = 0; return (float)angle; } } }