My custom humanoid animation, root motion calculations

HumanoidAnimation.cs

using AutoMapper.QueryableExtensions.Impl;
using Godot;
using GodotEx;

namespace NotNot.GodotNet.animation;

/// <summary>
/// convert any humanoid animation to root motion, and be able to save to disk. 
/// </summary>
[Tool]
public partial class HumanoidAnimation : Resource
{
	public record class LoadOptions
	{
		public required string? animName;
		/// <summary>
		/// how to configure looping animations
		/// </summary>
		public required LoopSettingOption loopSetting;

		public required bool compressAnim;
	}

	public enum LoopSettingOption
	{
		/// <summary>
		/// if existing anim has a loop specified, it will be used.   otherwise will set to linear if 'loop' is in the name (but not 'loop_false')
		/// </summary>
		KeepExistingWithNameHint,
		ForceLinear,
		ForcePingpong,
		ForceNone,
		LinearIfNone,
		PingpongIfNone,
	}


	/// <summary>
	/// humanoid animation we compute details for
	/// </summary>
	[Export]
	public Animation anim;


	public static HumanoidAnimation LoadSingleFromFile(string resPath, LoadOptions options)
	{
		var singleLib = ResourceLoader.Load<AnimationLibrary>(resPath);
		var animNames = singleLib.GetAnimationList();
		__.Assert(animNames.Count == 1);
		var mmName = animNames[0];
		var rawAnim = singleLib.GetAnimation(mmName);

		var rmAnim = new HumanoidAnimation();
		rmAnim.PopulateFrom(rawAnim, options);

		return rmAnim;
	}

	public static HumanoidAnimation LoadFromExisting(Animation animToClone, LoadOptions options)
	{
		var toReturn = new HumanoidAnimation()
		{
			
		};

		toReturn.PopulateFrom(animToClone, options);
		return toReturn;
	}

	public static bool IsAnimHumanoid(Animation animToCheck)
	{
		var boneNames = HumanoidBones.GetAllBones();


		var trackCount = animToCheck.GetTrackCount();

		for (var i = 0; i < trackCount; i++)
		{
			var trackPath = animToCheck.TrackGetPath(i).ToString();
			var animBone = trackPath._GetAfter(":", true);

			


			var foundBone = boneNames.Contains(animBone);


			//foreach (var bone in boneNames)
			//{
			//	if (trackPath.Contains(bone.ToString()))
			//	{
			//		boneNames.Remove(bone);
			//		foundBone = true;
			//		break;
			//	}
			//}
			__.Assert(foundBone);
			if (foundBone is false)
			{
				return false;
			}
		}

		return true;

	}
	public void PopulateFrom(Animation animToClone, LoadOptions options)
	{

		if (IsAnimHumanoid(animToClone) is false)
		{
			throw __.Throw($"{animToClone.ResourceName} not humanoid");
		}

		anim = (Animation)animToClone.Duplicate(true);

		

		if (options.animName is not null)
		{
			anim.ResourceName = options.animName;
		}

		ResourceName = anim.ResourceName;

		//set loop mode
		{
			//set default by animation name if needed
			if (anim.LoopMode == Animation.LoopModeEnum.None &&(
				    anim.ResourceName._Contains("loop", StringComparison.InvariantCultureIgnoreCase) || animToClone.ResourceName._Contains("loop", StringComparison.InvariantCultureIgnoreCase)
					 ))
			{
				if ((anim.ResourceName._Contains("loop_false", StringComparison.InvariantCultureIgnoreCase) is false) 
				&& (animToClone.ResourceName._Contains("loop_false", StringComparison.InvariantCultureIgnoreCase) is false) )//don't set if 'loop_false' is in name
				{
					anim.LoopMode = Animation.LoopModeEnum.Linear;
				}
			}

			switch (options.loopSetting)
			{
				case LoopSettingOption.ForceLinear:
					anim.LoopMode = Animation.LoopModeEnum.Linear;
					break;
				case LoopSettingOption.ForceNone:
					anim.LoopMode = Animation.LoopModeEnum.None;
					break;
				case LoopSettingOption.ForcePingpong:
					anim.LoopMode = Animation.LoopModeEnum.Pingpong;
					break;
				case LoopSettingOption.KeepExistingWithNameHint:
					break;
				case LoopSettingOption.LinearIfNone:
					if (anim.LoopMode == Animation.LoopModeEnum.None)
					{
						anim.LoopMode = Animation.LoopModeEnum.Linear;
					}
					break;
				case LoopSettingOption.PingpongIfNone:
					if (anim.LoopMode == Animation.LoopModeEnum.None)
					{
						anim.LoopMode = Animation.LoopModeEnum.Pingpong;
					}
					break;
				default:
					throw __.Throw("need to implement enum choice");
			}

		}

		_TryAddRootAnimation();

		if (options.compressAnim is true)
		{
			anim.Compress();
		}
	}

	private void _TryAddRootAnimation()
	{
		//add root bone, if none, and compute root motion


		var rootBonePath = HumanoidBones.Nodepath_Root;
		bool alreadyHadRootBone = anim.FindTrack(rootBonePath, Animation.TrackType.Position3D) != -1;
		//_GD.Print($"CALCULATING ROOT MOTION: already had root? {alreadyHadRootBone} for {adjustedAnim.ResourceName} ");
		if (alreadyHadRootBone is true)
		{
			return;
		}

		var trackIdxRootRot = anim.AddTrack(Animation.TrackType.Rotation3D);
		var trackIdxRootPos = anim.AddTrack(Animation.TrackType.Position3D);
		anim.TrackSetPath(trackIdxRootPos, rootBonePath);
		anim.TrackSetPath(trackIdxRootRot, rootBonePath);

		var hipBonePath = HumanoidBones.Nodepath_Hips;
		var trackIdxHipPos = anim.FindTrack(hipBonePath, Animation.TrackType.Position3D);
		var trackIdxHipRot = anim.FindTrack(hipBonePath, Animation.TrackType.Rotation3D);
		__.Assert(trackIdxHipPos > -1);


		//compute root motion for anims that didn't have a root bone
		//all key values are absolute, not relative to previous key
		{
			//IMPORTANT ALGORITHM NOTE:  each track has different keys (count and location) so need to work on each independently.

			//take hip movement and put directly onto root
			{
				for (var keyIdx = 0; keyIdx < anim.TrackGetKeyCount(trackIdxHipPos); keyIdx++)
				{
					//these positions are in absolute terms compared to the parent bone.  not relative to former key.
					var time = anim.TrackGetKeyTime(trackIdxHipPos, keyIdx);

					//read current hip pos
					var hipPos = anim.TrackGetKeyValue(trackIdxHipPos, keyIdx).AsVector3();


					//take movement on the XZ plane from the hip and put on root
					var rootPos = new Vector3(hipPos.X, 0, hipPos.Z); //negative Z due to godot .Forward difference
					var newHipPos = hipPos - rootPos;



					//update anim tracks with new data
					{

						//insert root pos
						//rootPosFinal += rootPosDelta;
						anim.PositionTrackInsertKey(trackIdxRootPos, time, rootPos);

						//update hip pos
						anim.TrackSetKeyValue(trackIdxHipPos, keyIdx, newHipPos);
					}
				}
			}

			////take hip rotation along Y axis and put directly onto root
			//NOTE:  rotations are absolute!
			{
				// controls when rotation is transfers to root from hips.  when hip rotation diverges too much from root Y rotation, rotation will be xfered to hips.
				// lower value means root will take more rotation from hips
				// higher value means root will tend to have no rotation, trying to let hips do rotations.  
				// value of 0 means all rotation is directed to root, but we still interpolate to smooth out the final root rotation.
				var rootRotationSensitivity = 5.0f;  //radians

				//radians.  y0 is current, y1 is next proposed, y1L was last frame proposed.
				float y0Current = 0, y1This = 0, y1Last = 0;
				var lastTime = 0d;
				float deltaYThis = 0, deltaYLast = 0;

				float weightMultipler = 1;
				float weight = 0.2f;

				// Loop through all keyframes of the hip rotation track
				var hipKeyCount = anim.TrackGetKeyCount(trackIdxHipRot);
				for (var keyIdx = 0; keyIdx < hipKeyCount; keyIdx++)
				{
					// Get the time for the current keyframe
					var time = anim.TrackGetKeyTime(trackIdxHipRot, keyIdx);
					var deltaTime = (float)(time - lastTime);
					if (time == 0)
					{
						//need to estimate based on duration and frame length
						deltaTime = anim.Length / hipKeyCount;
					}
					lastTime = time;

					float deltaSensitivity = (float)(rootRotationSensitivity * deltaTime);

					// Read current hip rotation in quaternion form
					var hipRotOriginal = anim.TrackGetKeyValue(trackIdxHipRot, keyIdx).AsQuaternion();
					var hipRotOriginalEuler = hipRotOriginal.GetEuler(); // Convert quaternion to Euler angles



					var y1Final = 0f;
					y1Last = y1This;

					y1This = hipRotOriginalEuler.Y;


					//debug -PI to PI theta transitions
					{
						//////put in 0 to 2pi terms
						//y1This = ((y1This + _2PI) % _2PI);
						//y1Last = ((y1Last + _2PI) % _2PI);
						//y0Current = ((y0Current + _2PI) % _2PI);

						//var isFlipSigned = false;
						//if (y0Current < _PI && y1This > _PI)
						//{
						//	isFlipSigned = true;
						//}
						//else if (y1This < _PI && y0Current > _PI)
						//{
						//	isFlipSigned = true;
						//}

						//if (isFlipSigned)
						//{
						//	//_GD.Print($"FLIPED! [{y0Current},{y1This}]  t={time}");
						//}
					}

					// Check if the total rotation exceeds maximum sensitivity
					deltaYLast = deltaYThis;
					deltaYThis = y1This - y0Current;
					var isDeltaThisBigger = (deltaYLast._Abs() < deltaYThis._Abs());
					var isDeltaAboveSensitivity = deltaYThis._Abs() > deltaSensitivity;
					if (isDeltaAboveSensitivity || isDeltaThisBigger)
					{
						//_GD.Print($"going to add root rotation this frame...");

						if (isDeltaThisBigger && isDeltaAboveSensitivity)
						{
							
							weight += 0.1f;
							weight = weight._Min(1);
							_GD.Print($"above sensitivty threshhold and our hip rotation is bigger than last frame, so increase our weight to 'catch up' root rotation w={weight}");
						}
						else
						{
							//_GD.Print($"decrease our weight to 'slow down' the rate of catch up");
							weight -= 0.1f;
							weight = weight._Max(0.2f);
						}
						
						//the final root rotation (of y axis) that we will apply this frame, interpolated by weight
						y1Final = GMath.LerpAngle(y0Current, y1This, weight);
					}
					else
					{
						//_GD.Print($"the rotation is both below our sensitivy threshhold and our rotation this frame is smaller than last");
						//no root motion this frame
						weight -= 0.1f;
						weight = weight._Max(0.2f);

						//no change to root pos
						y1Final = y0Current;
					}

					//save our next as current;
					y0Current = y1Final;



					//_GD.Print($"[{y0Current},{y1This}]  t={time}, dt={deltaTime}, dY={deltaYThis}, dS={deltaSensitivity}, w={weight} wv={deltaTime * GMath.Exp(weightMultipler)}");

					//apply back to our variables
					var finalRootRotationEuler = new Vector3(0, y1Final, 0);

					// Create a quaternion from the potential root rotation
					var finalRootRot = Quaternion.FromEuler(finalRootRotationEuler);

					// Calculate the new hip rotation by subtracting our rootRotation
					var hipRotNew = hipRotOriginal._Difference(finalRootRot);
					////this also works, the resulting quaternion is diff but result is same:
					//var hipRotNew = Quaternion.FromEuler(hipRotOriginalEuler - finalRootRotationEuler);

					// Insert the new root rotation into the root rotation track
					anim.RotationTrackInsertKey(trackIdxRootRot, time, finalRootRot);

					// Update the hip rotation with the newly calculated rotation
					anim.TrackSetKeyValue(trackIdxHipRot, keyIdx, hipRotNew);
				}
			}

		}


	}
}