initial upload

2025-09-21 22:42:26 +02:00
commit d03bcd4ba5
6231 changed files with 351582 additions and 0 deletions
--- a/Unity-Master/Assets/Plugins/RootMotion/FinalIK/IK
+++ b/Unity-Master/Assets/Plugins/RootMotion/FinalIK/IK
@ -0,0 +1,359 @@
+using UnityEngine;
+using System.Collections;
+using System;
+
+	namespace RootMotion.FinalIK {
+
+	/// <summary>
+	/// Forward and Backward Reaching Inverse Kinematics solver.
+	/// 
+	/// This class is based on the "FABRIK: A fast, iterative solver for the inverse kinematics problem." paper by Aristidou, A., Lasenby, J.
+	/// </summary>
+	[System.Serializable]
+	public class IKSolverFABRIK : IKSolverHeuristic {
+		
+		#region Main Interface
+
+		/// <summary>
+		/// Solving stage 1 of the %FABRIK algorithm.
+		/// </summary>
+		public void SolveForward(Vector3 position) {
+			if (!initiated) {
+				if (!Warning.logged) LogWarning("Trying to solve uninitiated FABRIK chain.");
+				return;
+			}
+			
+			OnPreSolve();
+			
+			ForwardReach(position);
+		}
+		
+		/// <summary>
+		/// Solving stage 2 of the %FABRIK algorithm.
+		/// </summary>
+		public void SolveBackward(Vector3 position) {
+			if (!initiated) {
+				if (!Warning.logged) LogWarning("Trying to solve uninitiated FABRIK chain.");
+				return;
+			}
+			
+			BackwardReach(position);
+			
+			OnPostSolve();
+		}
+
+		public override Vector3 GetIKPosition() {
+			if (target != null) return target.position;
+			return IKPosition;
+		}
+
+		/// <summary>
+		/// Called before each iteration of the solver.
+		/// </summary>
+		public IterationDelegate OnPreIteration;
+
+		#endregion Main Interface
+
+		private bool[] limitedBones = new bool[0];
+		private Vector3[] solverLocalPositions = new Vector3[0];
+
+		protected override void OnInitiate() {
+			if (firstInitiation || !Application.isPlaying) IKPosition = bones[bones.Length - 1].transform.position;
+
+			for (int i = 0; i < bones.Length; i++) {
+				bones[i].solverPosition = bones[i].transform.position;
+				bones[i].solverRotation = bones[i].transform.rotation;
+			}
+			
+			limitedBones = new bool[bones.Length];
+			solverLocalPositions = new Vector3[bones.Length];
+			
+			InitiateBones();
+
+			for (int i = 0; i < bones.Length; i++) {
+				solverLocalPositions[i] = Quaternion.Inverse(GetParentSolverRotation(i)) * (bones[i].transform.position - GetParentSolverPosition(i));
+			}
+		}
+		
+		protected override void OnUpdate() {
+			if (IKPositionWeight <= 0) return;
+			IKPositionWeight = Mathf.Clamp(IKPositionWeight, 0f, 1f);
+			
+			OnPreSolve();
+
+			if (target != null) IKPosition = target.position;
+			if (XY) IKPosition.z = bones[0].transform.position.z;
+
+			Vector3 singularityOffset = maxIterations > 1? GetSingularityOffset(): Vector3.zero;
+
+			// Iterating the solver
+			for (int i = 0; i < maxIterations; i++) {
+				// Optimizations
+				if (singularityOffset == Vector3.zero && i >= 1 && tolerance > 0 && positionOffset < tolerance * tolerance) break;
+				lastLocalDirection = localDirection;
+
+				if (OnPreIteration != null) OnPreIteration(i);
+				
+				Solve(IKPosition + (i == 0? singularityOffset: Vector3.zero));
+			}
+			
+			OnPostSolve();
+		}
+		
+		/*
+		 * If true, the solver will work with 0 length bones
+		 * */
+		protected override bool boneLengthCanBeZero { get { return false; }} // Returning false here also ensures that the bone lengths will be calculated
+
+		/*
+		 * Interpolates the joint position to match the bone's length
+		*/
+		private Vector3 SolveJoint(Vector3 pos1, Vector3 pos2, float length) {
+			if (XY) pos1.z = pos2.z;
+			
+			return pos2 + (pos1 - pos2).normalized * length;
+		}
+
+		/*
+		 * Check if bones have moved from last solved positions
+		 * */
+		private void OnPreSolve() {
+			chainLength = 0;
+
+			for (int i = 0; i < bones.Length; i++) {
+				bones[i].solverPosition = bones[i].transform.position;
+				bones[i].solverRotation = bones[i].transform.rotation;
+
+				if (i < bones.Length - 1) {
+					bones[i].length = (bones[i].transform.position - bones[i + 1].transform.position).magnitude;
+					bones[i].axis = Quaternion.Inverse(bones[i].transform.rotation) * (bones[i + 1].transform.position - bones[i].transform.position);
+
+					chainLength += bones[i].length;
+				}
+
+				if (useRotationLimits) solverLocalPositions[i] = Quaternion.Inverse(GetParentSolverRotation(i)) * (bones[i].transform.position - GetParentSolverPosition(i));
+			}
+		}
+		
+		/*
+		 * After solving the chain
+		 * */
+		private void OnPostSolve() {
+			// Rotating bones to match the solver positions
+			if (!useRotationLimits) MapToSolverPositions();
+			else MapToSolverPositionsLimited();
+
+			lastLocalDirection = localDirection;
+		}
+		
+		private void Solve(Vector3 targetPosition) {
+			// Forward reaching
+			ForwardReach(targetPosition);
+
+			// Backward reaching
+			BackwardReach(bones[0].transform.position);
+		}
+		
+		/*
+		 * Stage 1 of FABRIK algorithm
+		 * */
+		private void ForwardReach(Vector3 position) {
+			// Lerp last bone's solverPosition to position
+			bones[bones.Length - 1].solverPosition = Vector3.Lerp(bones[bones.Length - 1].solverPosition, position, IKPositionWeight);
+
+			for (int i = 0; i < limitedBones.Length; i++) limitedBones[i] = false;
+			
+			for (int i = bones.Length - 2; i > -1; i--) {
+				// Finding joint positions
+				bones[i].solverPosition = SolveJoint(bones[i].solverPosition, bones[i + 1].solverPosition, bones[i].length);
+				
+				// Limiting bone rotation forward
+				LimitForward(i, i + 1);
+			}
+			
+			// Limiting the first bone's rotation
+			LimitForward(0, 0);
+		}
+
+		private void SolverMove(int index, Vector3 offset) {
+			for (int i = index; i < bones.Length; i++) {
+				bones[i].solverPosition += offset;
+			}
+		}
+
+		private void SolverRotate(int index, Quaternion rotation, bool recursive) {
+			for (int i = index; i < bones.Length; i++) {
+				bones[i].solverRotation = rotation * bones[i].solverRotation;
+
+				if (!recursive) return;
+			}
+		}
+
+		private void SolverRotateChildren(int index, Quaternion rotation) {
+			for (int i = index + 1; i < bones.Length; i++) {
+				bones[i].solverRotation = rotation * bones[i].solverRotation;
+			}
+		}
+
+		private void SolverMoveChildrenAroundPoint(int index, Quaternion rotation) {
+			for (int i = index + 1; i < bones.Length; i++) {
+				Vector3 dir = bones[i].solverPosition - bones[index].solverPosition;
+				bones[i].solverPosition = bones[index].solverPosition + rotation * dir;
+			}
+		}
+
+		private Quaternion GetParentSolverRotation(int index) {
+			if (index > 0) return bones[index - 1].solverRotation;
+			if (bones[0].transform.parent == null) return Quaternion.identity;
+			return bones[0].transform.parent.rotation;
+		}
+
+		private Vector3 GetParentSolverPosition(int index) {
+			if (index > 0) return bones[index - 1].solverPosition;
+			if (bones[0].transform.parent == null) return Vector3.zero;
+			return bones[0].transform.parent.position;
+		}
+
+		private Quaternion GetLimitedRotation(int index, Quaternion q, out bool changed) {
+			changed = false;
+			
+			Quaternion parentRotation = GetParentSolverRotation(index);
+			Quaternion localRotation = Quaternion.Inverse(parentRotation) * q;
+			
+			Quaternion limitedLocalRotation = bones[index].rotationLimit.GetLimitedLocalRotation(localRotation, out changed);
+			
+			if (!changed) return q;
+			
+			return parentRotation * limitedLocalRotation;
+		}
+
+		/*
+		 * Applying rotation limit to a bone in stage 1 in a more stable way
+		 * */
+		private void LimitForward(int rotateBone, int limitBone) {
+			if (!useRotationLimits) return;
+			if (bones[limitBone].rotationLimit == null) return;
+
+			// Storing last bone's position before applying the limit
+			Vector3 lastBoneBeforeLimit = bones[bones.Length - 1].solverPosition;
+
+			// Moving and rotating this bone and all its children to their solver positions
+			for (int i = rotateBone; i < bones.Length - 1; i++) {
+				if (limitedBones[i]) break;
+
+				Quaternion fromTo = Quaternion.FromToRotation(bones[i].solverRotation * bones[i].axis, bones[i + 1].solverPosition - bones[i].solverPosition);
+				SolverRotate(i, fromTo, false);
+			}
+
+			// Limit the bone's rotation
+			bool changed = false;
+			Quaternion afterLimit = GetLimitedRotation(limitBone, bones[limitBone].solverRotation, out changed);
+
+			if (changed) {
+				// Rotating and positioning the hierarchy so that the last bone's position is maintained
+				if (limitBone < bones.Length - 1) {
+					Quaternion change = QuaTools.FromToRotation(bones[limitBone].solverRotation, afterLimit);
+					bones[limitBone].solverRotation = afterLimit;
+					SolverRotateChildren(limitBone, change);
+					SolverMoveChildrenAroundPoint(limitBone, change);
+
+					// Rotating to compensate for the limit
+					Quaternion fromTo = Quaternion.FromToRotation(bones[bones.Length - 1].solverPosition - bones[rotateBone].solverPosition, lastBoneBeforeLimit - bones[rotateBone].solverPosition);
+
+					SolverRotate(rotateBone, fromTo, true);
+					SolverMoveChildrenAroundPoint(rotateBone, fromTo);
+
+					// Moving the bone so that last bone maintains its initial position
+					SolverMove(rotateBone, lastBoneBeforeLimit - bones[bones.Length - 1].solverPosition);
+				} else {
+					// last bone
+					bones[limitBone].solverRotation = afterLimit;
+				}
+			}
+
+			limitedBones[limitBone] = true;
+		}
+		
+		/*
+		 * Stage 2 of FABRIK algorithm
+		 * */
+		private void BackwardReach(Vector3 position) {
+			if (useRotationLimits) BackwardReachLimited(position);
+			else BackwardReachUnlimited(position);
+		}
+		
+		/*
+		 * Stage 2 of FABRIK algorithm without rotation limits
+		 * */
+		private void BackwardReachUnlimited(Vector3 position) {
+			// Move first bone to position
+			bones[0].solverPosition = position;
+			
+			// Finding joint positions
+			for (int i = 1; i < bones.Length; i++) {
+				bones[i].solverPosition = SolveJoint(bones[i].solverPosition, bones[i - 1].solverPosition, bones[i - 1].length);
+			}
+		}
+		
+		/*
+		 * Stage 2 of FABRIK algorithm with limited rotations
+		 * */
+		private void BackwardReachLimited(Vector3 position) {
+			// Move first bone to position
+			bones[0].solverPosition = position;
+
+			// Applying rotation limits bone by bone
+			for (int i = 0; i < bones.Length - 1; i++) {
+				// Rotating bone to look at the solved joint position
+				Vector3 nextPosition = SolveJoint(bones[i + 1].solverPosition, bones[i].solverPosition, bones[i].length);
+
+				Quaternion swing = Quaternion.FromToRotation(bones[i].solverRotation * bones[i].axis, nextPosition - bones[i].solverPosition);
+				Quaternion targetRotation = swing * bones[i].solverRotation;
+
+				// Rotation Constraints
+				if (bones[i].rotationLimit != null) {
+					bool changed = false;
+					targetRotation = GetLimitedRotation(i, targetRotation, out changed);
+				}
+
+				Quaternion fromTo = QuaTools.FromToRotation(bones[i].solverRotation, targetRotation);
+				bones[i].solverRotation = targetRotation;
+				SolverRotateChildren(i, fromTo);
+
+				// Positioning the next bone to its default local position
+				bones[i + 1].solverPosition = bones[i].solverPosition + bones[i].solverRotation * solverLocalPositions[i + 1];
+			}
+
+			// Reconstruct solver rotations to protect from invalid Quaternions
+			for (int i = 0; i < bones.Length; i++) {
+				bones[i].solverRotation = Quaternion.LookRotation(bones[i].solverRotation * Vector3.forward, bones[i].solverRotation * Vector3.up);
+			}
+		}
+
+		/*
+		 * Rotate bones to match the solver positions when not using Rotation Limits
+		 * */
+		private void MapToSolverPositions() {
+			bones[0].transform.position = bones[0].solverPosition;
+			
+			for (int i = 0; i < bones.Length - 1; i++) {
+				if (XY) {
+					bones[i].Swing2D(bones[i + 1].solverPosition);
+				} else {
+					bones[i].Swing(bones[i + 1].solverPosition);
+				}
+			}
+		}
+
+		/*
+		 * Rotate bones to match the solver positions when using Rotation Limits
+		 * */
+		private void MapToSolverPositionsLimited() {
+            bones[0].transform.position = bones[0].solverPosition;
+
+			for (int i = 0; i < bones.Length; i++) {
+				if (i < bones.Length - 1) bones[i].transform.rotation = bones[i].solverRotation;
+			}
+		}
+	}
+}