From 3ad83fbebe1f1f85c971a8e89bded77cc088d2bd Mon Sep 17 00:00:00 2001 From: Nicolas Gnyra Date: Tue, 30 Jan 2024 14:35:58 -0500 Subject: [PATCH] Add shoulder pitch adjustment --- Source/CustomAvatar/IKSolverVR_Arm.cs | 27 ++++++ Source/CustomAvatar/Patches/IKSolverVR.cs | 92 +++++++++++++++++++ .../Utilities/CodeInstructionExtensions.cs | 74 +++++++++++++++ Source/CustomAvatar/Utilities/IKHelper.cs | 80 ++++++++++------ 4 files changed, 247 insertions(+), 26 deletions(-) create mode 100644 Source/CustomAvatar/IKSolverVR_Arm.cs create mode 100644 Source/CustomAvatar/Patches/IKSolverVR.cs create mode 100644 Source/CustomAvatar/Utilities/CodeInstructionExtensions.cs diff --git a/Source/CustomAvatar/IKSolverVR_Arm.cs b/Source/CustomAvatar/IKSolverVR_Arm.cs new file mode 100644 index 00000000..83e566c6 --- /dev/null +++ b/Source/CustomAvatar/IKSolverVR_Arm.cs @@ -0,0 +1,27 @@ +// Beat Saber Custom Avatars - Custom player models for body presence in Beat Saber. +// Copyright © 2018-2024 Nicolas Gnyra and Beat Saber Custom Avatars Contributors +// +// This library is free software: you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation, either +// version 3 of the License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . + +extern alias BeatSaberFinalIK; + +using BeatSaberFinalIK::RootMotion.FinalIK; + +namespace CustomAvatar +{ + internal class IKSolverVR_Arm : IKSolverVR.Arm + { + public float shoulderPitchOffset = -30f; + } +} diff --git a/Source/CustomAvatar/Patches/IKSolverVR.cs b/Source/CustomAvatar/Patches/IKSolverVR.cs new file mode 100644 index 00000000..b5292b0a --- /dev/null +++ b/Source/CustomAvatar/Patches/IKSolverVR.cs @@ -0,0 +1,92 @@ +// Beat Saber Custom Avatars - Custom player models for body presence in Beat Saber. +// Copyright © 2018-2024 Nicolas Gnyra and Beat Saber Custom Avatars Contributors +// +// This library is free software: you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation, either +// version 3 of the License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . + +extern alias BeatSaberFinalIK; + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Reflection.Emit; +using BeatSaberFinalIK::RootMotion.FinalIK; +using CustomAvatar.Utilities; +using HarmonyLib; + +namespace CustomAvatar.Patches +{ + [HarmonyPatch(typeof(IKSolverVR), MethodType.Constructor)] + internal static class IKSolverVR_Constructor + { + public static void Postfix(IKSolverVR __instance) + { + __instance.leftArm = new IKSolverVR_Arm(); + __instance.rightArm = new IKSolverVR_Arm(); + } + } + + [HarmonyPatch(typeof(IKSolverVR.Arm), nameof(IKSolverVR.Arm.Solve))] + internal static class IKSolverVR_Arm_PitchAngleOffset + { + private static readonly FieldInfo kPitchOffsetAngleField = AccessTools.DeclaredField(typeof(IKSolverVR_Arm), nameof(IKSolverVR_Arm.shoulderPitchOffset)); + + public static IEnumerable Transpiler(IEnumerable instructions) + { + return new CodeMatcher(instructions) + /* Quaternion.AngleAxis(isLeft ? pitchOffsetAngle : -pitchOffsetAngle, chestForward) */ + .MatchForward(false, + new CodeMatch(i => i.Equals(OpCodes.Ldc_R4, 30f)), + new CodeMatch(i => i.Branches(out Label? _)), + new CodeMatch(i => i.Equals(OpCodes.Ldc_R4, -30f))) + .SetAndAdvance(OpCodes.Ldarg_0, null) + .InsertAndAdvance( + new CodeInstruction(OpCodes.Ldfld, kPitchOffsetAngleField), + new CodeInstruction(OpCodes.Neg)) + .Advance(1) + .SetAndAdvance(OpCodes.Ldarg_0, null) + .InsertAndAdvance( + new CodeInstruction(OpCodes.Ldfld, kPitchOffsetAngleField)) + + /* pitch -= pitchOffsetAngle */ + .MatchForward(false, + new CodeMatch(i => i.LoadsLocal(11)), + new CodeMatch(i => i.opcode == OpCodes.Ldc_R4 && (float)i.operand == -30f), + new CodeMatch(OpCodes.Sub), + new CodeMatch(i => i.SetsLocal(11))) + .ThrowIfInvalid("`pitch -= pitchOffsetAngle` not found") + .Advance(1) + .SetAndAdvance(OpCodes.Ldarg_0, null) + .InsertAndAdvance( + new CodeInstruction(OpCodes.Ldfld, kPitchOffsetAngleField)) + + /* DamperValue(pitch, -45f - pitchOffsetAngle, 45f - pitchOffsetAngle) */ + .MatchForward(false, + new CodeMatch(i => i.LoadsLocal(11)), + new CodeMatch(i => i.Equals(OpCodes.Ldc_R4, -15f)), + new CodeMatch(i => i.Equals(OpCodes.Ldc_R4, 75f))) + .Advance(1) + .SetOperandAndAdvance(-45f) + .InsertAndAdvance( + new CodeInstruction(OpCodes.Ldarg_0, null), + new CodeInstruction(OpCodes.Ldfld, kPitchOffsetAngleField), + new CodeInstruction(OpCodes.Sub)) + .SetOperandAndAdvance(45f) + .InsertAndAdvance( + new CodeInstruction(OpCodes.Ldarg_0, null), + new CodeInstruction(OpCodes.Ldfld, kPitchOffsetAngleField), + new CodeInstruction(OpCodes.Sub)) + .InstructionEnumeration(); + } + } +} diff --git a/Source/CustomAvatar/Utilities/CodeInstructionExtensions.cs b/Source/CustomAvatar/Utilities/CodeInstructionExtensions.cs new file mode 100644 index 00000000..6dc9cf1d --- /dev/null +++ b/Source/CustomAvatar/Utilities/CodeInstructionExtensions.cs @@ -0,0 +1,74 @@ +// Beat Saber Custom Avatars - Custom player models for body presence in Beat Saber. +// Copyright © 2018-2024 Nicolas Gnyra and Beat Saber Custom Avatars Contributors +// +// This library is free software: you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation, either +// version 3 of the License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . + +using System; +using System.Collections.Generic; +using System.Linq; +using System.Reflection.Emit; +using HarmonyLib; + +namespace CustomAvatar.Utilities +{ + internal static class CodeInstructionExtensions + { + private static readonly OpCode[] kLoadLocalCodes = + { + OpCodes.Ldloc_S, + OpCodes.Ldloc, + }; + + private static readonly OpCode[] kSetLocalCodes = + { + OpCodes.Stloc, + OpCodes.Stloc_S, + }; + + internal static bool LoadsLocal(this CodeInstruction instruction, int index) + { + if (!kLoadLocalCodes.Contains(instruction.opcode)) + { + return false; + } + + return instruction.operand switch + { + LocalBuilder localBuilder => localBuilder.LocalIndex == index, + int localIndex => index == localIndex, + _ => throw new InvalidCastException(), + }; + } + + internal static bool SetsLocal(this CodeInstruction instruction, int index) + { + if (!kSetLocalCodes.Contains(instruction.opcode)) + { + return false; + } + + return instruction.operand switch + { + LocalBuilder localBuilder => localBuilder.LocalIndex == index, + int localIndex => index == localIndex, + _ => throw new InvalidCastException(), + }; + } + + internal static bool Equals(this CodeInstruction codeInstruction, OpCode opcode, T operand) + { + return codeInstruction.opcode == opcode && EqualityComparer.Default.Equals((T)codeInstruction.operand, operand); + } + } +} diff --git a/Source/CustomAvatar/Utilities/IKHelper.cs b/Source/CustomAvatar/Utilities/IKHelper.cs index 7e1b3ab4..ad7f5e6b 100644 --- a/Source/CustomAvatar/Utilities/IKHelper.cs +++ b/Source/CustomAvatar/Utilities/IKHelper.cs @@ -109,32 +109,47 @@ private void CopyManagerFieldsToVRIK(VRIKManager vrikManager, VRIK vrik) vrik.solver.spine.moveBodyBackWhenCrouching = vrikManager.solver_spine_moveBodyBackWhenCrouching; vrik.solver.spine.maintainPelvisPosition = vrikManager.solver_spine_maintainPelvisPosition; vrik.solver.spine.maxRootAngle = vrikManager.solver_spine_maxRootAngle; - vrik.solver.leftArm.target = vrikManager.solver_leftArm_target; - vrik.solver.leftArm.bendGoal = vrikManager.solver_leftArm_bendGoal; - vrik.solver.leftArm.positionWeight = vrikManager.solver_leftArm_positionWeight; - vrik.solver.leftArm.rotationWeight = vrikManager.solver_leftArm_rotationWeight; - vrik.solver.leftArm.shoulderRotationMode = vrikManager.solver_leftArm_shoulderRotationMode; - vrik.solver.leftArm.shoulderRotationWeight = vrikManager.solver_leftArm_shoulderRotationWeight; - vrik.solver.leftArm.shoulderTwistWeight = vrikManager.solver_leftArm_shoulderTwistWeight; - vrik.solver.leftArm.bendGoalWeight = vrikManager.solver_leftArm_bendGoalWeight; - vrik.solver.leftArm.swivelOffset = vrikManager.solver_leftArm_swivelOffset; - vrik.solver.leftArm.wristToPalmAxis = vrikManager.solver_leftArm_wristToPalmAxis; - vrik.solver.leftArm.palmToThumbAxis = vrikManager.solver_leftArm_palmToThumbAxis; - vrik.solver.leftArm.armLengthMlp = vrikManager.solver_leftArm_armLengthMlp; - vrik.solver.leftArm.stretchCurve = vrikManager.solver_leftArm_stretchCurve; - vrik.solver.rightArm.target = vrikManager.solver_rightArm_target; - vrik.solver.rightArm.bendGoal = vrikManager.solver_rightArm_bendGoal; - vrik.solver.rightArm.positionWeight = vrikManager.solver_rightArm_positionWeight; - vrik.solver.rightArm.rotationWeight = vrikManager.solver_rightArm_rotationWeight; - vrik.solver.rightArm.shoulderRotationMode = vrikManager.solver_rightArm_shoulderRotationMode; - vrik.solver.rightArm.shoulderRotationWeight = vrikManager.solver_rightArm_shoulderRotationWeight; - vrik.solver.rightArm.shoulderTwistWeight = vrikManager.solver_rightArm_shoulderTwistWeight; - vrik.solver.rightArm.bendGoalWeight = vrikManager.solver_rightArm_bendGoalWeight; - vrik.solver.rightArm.swivelOffset = vrikManager.solver_rightArm_swivelOffset; - vrik.solver.rightArm.wristToPalmAxis = vrikManager.solver_rightArm_wristToPalmAxis; - vrik.solver.rightArm.palmToThumbAxis = vrikManager.solver_rightArm_palmToThumbAxis; - vrik.solver.rightArm.armLengthMlp = vrikManager.solver_rightArm_armLengthMlp; - vrik.solver.rightArm.stretchCurve = vrikManager.solver_rightArm_stretchCurve; + +#if UNITY_EDITOR + IKSolverVR.Arm leftArm = vrik.solver.leftArm; +#else + var leftArm = (IKSolverVR_Arm)vrik.solver.leftArm; + leftArm.shoulderPitchOffset = CalculatePitchOffset(true, vrik.references.root, vrik.references.leftShoulder, vrik.references.leftUpperArm); +#endif + leftArm.target = vrikManager.solver_leftArm_target; + leftArm.bendGoal = vrikManager.solver_leftArm_bendGoal; + leftArm.positionWeight = vrikManager.solver_leftArm_positionWeight; + leftArm.rotationWeight = vrikManager.solver_leftArm_rotationWeight; + leftArm.shoulderRotationMode = vrikManager.solver_leftArm_shoulderRotationMode; + leftArm.shoulderRotationWeight = vrikManager.solver_leftArm_shoulderRotationWeight; + leftArm.shoulderTwistWeight = vrikManager.solver_leftArm_shoulderTwistWeight; + leftArm.bendGoalWeight = vrikManager.solver_leftArm_bendGoalWeight; + leftArm.swivelOffset = vrikManager.solver_leftArm_swivelOffset; + leftArm.wristToPalmAxis = vrikManager.solver_leftArm_wristToPalmAxis; + leftArm.palmToThumbAxis = vrikManager.solver_leftArm_palmToThumbAxis; + leftArm.armLengthMlp = vrikManager.solver_leftArm_armLengthMlp; + leftArm.stretchCurve = vrikManager.solver_leftArm_stretchCurve; + +#if UNITY_EDITOR + IKSolverVR.Arm rightArm = vrik.solver.rightArm; +#else + var rightArm = (IKSolverVR_Arm)vrik.solver.rightArm; + rightArm.shoulderPitchOffset = CalculatePitchOffset(false, vrik.references.root, vrik.references.rightShoulder, vrik.references.rightUpperArm); +#endif + rightArm.target = vrikManager.solver_rightArm_target; + rightArm.bendGoal = vrikManager.solver_rightArm_bendGoal; + rightArm.positionWeight = vrikManager.solver_rightArm_positionWeight; + rightArm.rotationWeight = vrikManager.solver_rightArm_rotationWeight; + rightArm.shoulderRotationMode = vrikManager.solver_rightArm_shoulderRotationMode; + rightArm.shoulderRotationWeight = vrikManager.solver_rightArm_shoulderRotationWeight; + rightArm.shoulderTwistWeight = vrikManager.solver_rightArm_shoulderTwistWeight; + rightArm.bendGoalWeight = vrikManager.solver_rightArm_bendGoalWeight; + rightArm.swivelOffset = vrikManager.solver_rightArm_swivelOffset; + rightArm.wristToPalmAxis = vrikManager.solver_rightArm_wristToPalmAxis; + rightArm.palmToThumbAxis = vrikManager.solver_rightArm_palmToThumbAxis; + rightArm.armLengthMlp = vrikManager.solver_rightArm_armLengthMlp; + rightArm.stretchCurve = vrikManager.solver_rightArm_stretchCurve; + vrik.solver.leftLeg.target = vrikManager.solver_leftLeg_target; vrik.solver.leftLeg.bendGoal = vrikManager.solver_leftLeg_bendGoal; vrik.solver.leftLeg.positionWeight = vrikManager.solver_leftLeg_positionWeight; @@ -172,5 +187,18 @@ private void CopyManagerFieldsToVRIK(VRIKManager vrikManager, VRIK vrik) vrik.solver.locomotion.onLeftFootstep = vrikManager.solver_locomotion_onLeftFootstep; vrik.solver.locomotion.onRightFootstep = vrikManager.solver_locomotion_onRightFootstep; } + +#if !UNITY_EDITOR + private static float CalculatePitchOffset(bool isLeft, Transform root, Transform shoulder, Transform upperArm) + { + // Reading IKSolverVR.Arm's Shoulder Pitch section, it looks like: + // - chestForward is essentially just the root forward + // - pitch is always (90 degrees to either side of chestUp) * chestRotation, so effectively left and right of the chest. + Vector3 forward = root.forward; + Vector3 right = root.right; + + return Vector3.SignedAngle(isLeft ? -right : right, upperArm.position - shoulder.position, isLeft ? forward : -forward) - 45f; + } +#endif } }