Broken Ragdoll

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.Animations.Rigging;

	public class Ragdoll : MonoBehaviour
	{
		Animator animator;
    [SerializeField]
    float ragdollMecanimBlendTime = 0.5f;  //How long do we blend when transitioning from ragdolled to animated
	public bool blendToAnim;
	float _ragdollingEndTime;   //A helper variable to store the time when we transitioned from ragdolled to blendToAnim state

	readonly List<Limb> limbs = new List<Limb>();
	readonly List<TransformComponent> _transforms = new List<TransformComponent>();

	Transform _hipsTransform;
	//Rigidbody _hipsTransformRigid;

	Vector3 _storedHipsPosition;
	Vector3 _storedHipsPositionPrivAnim;
	Vector3 _storedHipsPositionPrivBlend;

	private void Start()
    {
        animator = GetComponent<Animator>();
		_hipsTransform = animator.GetBoneTransform(HumanBodyBones.Hips);
		//_hipsTransformRigid = _hipsTransform.GetComponent<Rigidbody>();
		
		//Get all the rigid bodies that belong to the ragdoll
		Rigidbody[] rigidBodies = GetComponentsInChildren<Rigidbody>();

		foreach (Rigidbody rigidbody in rigidBodies)
		{
			if (rigidbody.transform == transform)
				continue;

			Limb thisLimb = new Limb(rigidbody);
			limbs.Add(thisLimb);
		}

		// disable ragdoll by default
		setRagdoll(false);
	}

	struct Limb
	{
		public readonly Rigidbody RigidBody;
		public readonly CharacterJoint Joint;
		public readonly Vector3 ConnectedAnchorDefault;

		public Limb(Rigidbody rigid)
		{
			RigidBody = rigid;
			Joint = rigid.GetComponent<CharacterJoint>();
			if (Joint != null)
				ConnectedAnchorDefault = Joint.connectedAnchor;
			else
				ConnectedAnchorDefault = Vector3.zero;
		}
	}

	private void setRagdoll(bool state)
    {
		// SET HERE CHARACTER RELATED STUFF \\ -- CharacterEnable(!state);

		foreach (var limb in limbs)
		{
			Collider partColider = limb.RigidBody.GetComponent<Collider>();

			// fix for RagdollHelper (bone collider - BoneHelper.cs)
			if (partColider == null)
			{
				const string colliderNodeSufix = "_ColliderRotator"; //Make empty child for root(hips) and rename it to same was root self + _ColliderRotator
				string childName = limb.RigidBody.name + colliderNodeSufix;
				Transform transform = limb.RigidBody.transform.Find(childName);
				partColider = transform.GetComponent<Collider>();
			}

			partColider.isTrigger = !state;

			if (state)
			{
				limb.RigidBody.isKinematic = false;
				StartCoroutine(FixTransformAndEnableJoint(limb));
			}
			else
				limb.RigidBody.isKinematic = true;
		}
	}

	private void GetUp()
	{
		//Transition from ragdolled to animated through the blendToAnim state
		_ragdollingEndTime = Time.time;     //store the state change time
											//_anim.SetFloat(_animatorForward, 0f);
											//_anim.SetFloat(_animatorTurn, 0f);
		animator.enabled = true;               //enable animation
		blendToAnim = true;
		_storedHipsPositionPrivAnim = Vector3.zero;
		_storedHipsPositionPrivBlend = Vector3.zero;

		_storedHipsPosition = _hipsTransform.position;

		// get distanse to floor
		Vector3 shiftPos = _hipsTransform.position - transform.position;
		//shiftPos.y = GetDistanceToFloor(shiftPos.y);

		// shift and rotate character node without children
		//MoveNodeWithoutChildren(shiftPos);

		//Store the ragdolled position for blending
		foreach (TransformComponent trComp in _transforms)
		{
			trComp.StoredRotation = trComp.Transform.localRotation;
			trComp.PrivRotation = trComp.Transform.localRotation;

			trComp.StoredPosition = trComp.Transform.localPosition;
			trComp.PrivPosition = trComp.Transform.localPosition;
		}

		//Initiate the get up animation
		string getUpAnim = "GetUp.StandUp_Back"; //"GetUp.StandUp_Belly"
		animator.Play(getUpAnim, 0, 0);    // you have to set time to 0, or if your animation will interrupt, next time animation starts from previous position
		setRagdoll(false);    // disable gravity on ragdollParts.
	}

	static IEnumerator FixTransformAndEnableJoint(Limb joint)
	{
		if (joint.Joint == null || !joint.Joint.autoConfigureConnectedAnchor)
			yield break;

		SoftJointLimit highTwistLimit = new SoftJointLimit();
		SoftJointLimit lowTwistLimit = new SoftJointLimit();
		SoftJointLimit swing1Limit = new SoftJointLimit();
		SoftJointLimit swing2Limit = new SoftJointLimit();

		SoftJointLimit curHighTwistLimit = highTwistLimit = joint.Joint.highTwistLimit;
		SoftJointLimit curLowTwistLimit = lowTwistLimit = joint.Joint.lowTwistLimit;
		SoftJointLimit curSwing1Limit = swing1Limit = joint.Joint.swing1Limit;
		SoftJointLimit curSwing2Limit = swing2Limit = joint.Joint.swing2Limit;

		float aTime = 0.3f;
		Vector3 startConPosition = joint.Joint.connectedBody.transform.InverseTransformVector(joint.Joint.transform.position - joint.Joint.connectedBody.transform.position);

		joint.Joint.autoConfigureConnectedAnchor = false;
		for (float t = 0.0f; t < 1.0f; t += Time.deltaTime / aTime)
		{
			Vector3 newConPosition = Vector3.Lerp(startConPosition, joint.ConnectedAnchorDefault, t);
			joint.Joint.connectedAnchor = newConPosition;

			curHighTwistLimit.limit = Mathf.Lerp(177, highTwistLimit.limit, t);
			curLowTwistLimit.limit = Mathf.Lerp(-177, lowTwistLimit.limit, t);
			curSwing1Limit.limit = Mathf.Lerp(177, swing1Limit.limit, t);
			curSwing2Limit.limit = Mathf.Lerp(177, swing2Limit.limit, t);

			joint.Joint.highTwistLimit = curHighTwistLimit;
			joint.Joint.lowTwistLimit = curLowTwistLimit;
			joint.Joint.swing1Limit = curSwing1Limit;
			joint.Joint.swing2Limit = curSwing2Limit;


			yield return null;
		}
		joint.Joint.connectedAnchor = joint.ConnectedAnchorDefault;
		yield return new WaitForFixedUpdate();
		joint.Joint.autoConfigureConnectedAnchor = true;


		joint.Joint.highTwistLimit = highTwistLimit;
		joint.Joint.lowTwistLimit = lowTwistLimit;
		joint.Joint.swing1Limit = swing1Limit;
		joint.Joint.swing2Limit = swing2Limit;
	}


	private void Update()
    {
        if(Input.GetKeyDown(KeyCode.DownArrow))
        {
			setRagdoll(true);
			animator.enabled = false;
        }

		if (Input.GetKeyDown(KeyCode.UpArrow))
		{
			setRagdoll(false);
			GetUp();
		}
	}

    private void LateUpdate()
    {
		if (!blendToAnim)
			return;

		float ragdollBlendAmount = 1f - Mathf.InverseLerp(_ragdollingEndTime, _ragdollingEndTime + ragdollMecanimBlendTime, Time.time);

		// In LateUpdate(), Mecanim has already updated the body pose according to the animations.
		// To enable smooth transitioning from a ragdoll to animation, we lerp the position of the hips
		// and slerp all the rotations towards the ones stored when ending the ragdolling

		if (_storedHipsPositionPrivBlend != _hipsTransform.position)
		{
			_storedHipsPositionPrivAnim = _hipsTransform.position;
		}
		_storedHipsPositionPrivBlend = Vector3.Lerp(_storedHipsPositionPrivAnim, _storedHipsPosition, ragdollBlendAmount);
		_hipsTransform.position = _storedHipsPositionPrivBlend;

		foreach (TransformComponent trComp in _transforms)
		{
			//rotation is interpolated for all body parts
			if (trComp.PrivRotation != trComp.Transform.localRotation)
			{
				trComp.PrivRotation = Quaternion.Slerp(trComp.Transform.localRotation, trComp.StoredRotation, ragdollBlendAmount);
				trComp.Transform.localRotation = trComp.PrivRotation;
			}

			//position is interpolated for all body parts
			if (trComp.PrivPosition != trComp.Transform.localPosition)
			{
				trComp.PrivPosition = Vector3.Slerp(trComp.Transform.localPosition, trComp.StoredPosition, ragdollBlendAmount);
				trComp.Transform.localPosition = trComp.PrivPosition;
			}
		}

		//if the ragdoll blend amount has decreased to zero, move to animated state
		if (Mathf.Abs(ragdollBlendAmount) < Mathf.Epsilon)
		{
			blendToAnim = false;
		}
	}

	//Declare a class that will hold useful information for each body part
	sealed class TransformComponent
	{
		public readonly Transform Transform;
		public Quaternion PrivRotation;
		public Quaternion StoredRotation;

		public Vector3 PrivPosition;
		public Vector3 StoredPosition;

		public TransformComponent(Transform t)
		{
			Transform = t;
		}
	}
}