Skip to content

Commit

Permalink
Add shoulder pitch adjustment
Browse files Browse the repository at this point in the history
  • Loading branch information
nicoco007 committed Jan 30, 2024
1 parent cf20d71 commit 3ad83fb
Show file tree
Hide file tree
Showing 4 changed files with 247 additions and 26 deletions.
27 changes: 27 additions & 0 deletions Source/CustomAvatar/IKSolverVR_Arm.cs
Original file line number Diff line number Diff line change
@@ -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 <https://www.gnu.org/licenses/>.

extern alias BeatSaberFinalIK;

using BeatSaberFinalIK::RootMotion.FinalIK;

namespace CustomAvatar
{
internal class IKSolverVR_Arm : IKSolverVR.Arm
{
public float shoulderPitchOffset = -30f;
}
}
92 changes: 92 additions & 0 deletions Source/CustomAvatar/Patches/IKSolverVR.cs
Original file line number Diff line number Diff line change
@@ -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 <https://www.gnu.org/licenses/>.

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<CodeInstruction> Transpiler(IEnumerable<CodeInstruction> 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();
}
}
}
74 changes: 74 additions & 0 deletions Source/CustomAvatar/Utilities/CodeInstructionExtensions.cs
Original file line number Diff line number Diff line change
@@ -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 <https://www.gnu.org/licenses/>.

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<T>(this CodeInstruction codeInstruction, OpCode opcode, T operand)
{
return codeInstruction.opcode == opcode && EqualityComparer<T>.Default.Equals((T)codeInstruction.operand, operand);
}
}
}
80 changes: 54 additions & 26 deletions Source/CustomAvatar/Utilities/IKHelper.cs
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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
}
}

0 comments on commit 3ad83fb

Please sign in to comment.