BITFALL/Assets/BITKit/Unity/Scripts/Physics/PhysicsBasedAnimation.cs

using System;
using System.Collections;
using System.Collections.Generic;
using BITKit.Core.Tuple;
using UnityEngine;

namespace BITKit.Physics
{
	[Serializable]
	public class JointConfigure
	{
		public Transform animate;
		public ConfigurableJoint joint;
		public Quaternion InitialRotation { get; set; }
		public Rigidbody Rigidbody { get; set; }
		public ConfigurableJointMotion AngularXMotion { get; set; }
		public ConfigurableJointMotion AngularYMotion { get; set; }
		public ConfigurableJointMotion AngularZMotion { get; set; }
		public ConfigurableJointMotion XMotion { get; set; }
		public ConfigurableJointMotion YMotion { get; set; }
		public ConfigurableJointMotion ZMotion { get; set; }
	}
	public class PhysicsBasedAnimation : MonoBehaviour
	{
		[Header(Constant.Header.Settings)]
		[Range(0, 1)] public float Blend;

		[SerializeField] public bool allowFixedJoints;
		
		[Header(Constant.Header.Components)]
		[SerializeField] private JointConfigure[] fixedJointConfigures;
		[SerializeField] private JointConfigure[] jointConfigures;
		
		[Header(Constant.Header.Settings)]
		[SerializeField] private float positionSpring;
		[SerializeField] private float positionDamper;
		[SerializeField] private float torqueForce;
		[SerializeField] private float maximumForce;
		
		[Header(Constant.Header.Optional)]
		[SerializeField] private Optional<UnityTuple<Rigidbody, Transform>>  rootSync;
		[SerializeField] private Optional<ConfigurableJointMotion> overrideMotion;
		[SerializeField] private Optional<ConfigurableJointMotion> overrideAngularMotion;
		
		private readonly ValidHandle allowFixedJoint = new();
		private readonly ValidHandle allowPhysicsAnimation = new();

		private void Start()
		{
			foreach (var x in jointConfigures)
			{
				x.InitialRotation = x.animate.localRotation;
				if (x.animate.localRotation != x.joint.transform.localRotation)
				{
					BIT4Log.Log<PhysicsBasedAnimation>($"{x.animate.name}初始旋转与关节旋转不一致，将会导致动画不正确");
				}

				x.Rigidbody = x.joint.GetComponent<Rigidbody>();
				x.AngularXMotion = x.joint.angularXMotion;
				x.AngularYMotion = x.joint.angularYMotion;
				x.AngularZMotion = x.joint.angularZMotion;
				x.XMotion = x.joint.xMotion;
				x.YMotion = x.joint.yMotion;
				x.ZMotion = x.joint.zMotion;
			}

			allowFixedJoint.AddListener(OnAllowFixedJoint);
			allowPhysicsAnimation.AddListener(OnAllowPhysicsAnimation);
		}

		private void OnAllowPhysicsAnimation(bool allow)
		{
			foreach (var x in jointConfigures)
			{
				x.joint.angularXMotion = !allow ? x.AngularXMotion : ConfigurableJointMotion.Free;
				x.joint.angularYMotion = !allow ? x.AngularYMotion : ConfigurableJointMotion.Free;
				x.joint.angularZMotion = !allow ? x.AngularZMotion : ConfigurableJointMotion.Free;
				x.joint.xMotion = !allow ? x.XMotion : ConfigurableJointMotion.Locked;
				x.joint.yMotion = !allow ? x.YMotion : ConfigurableJointMotion.Locked;
				x.joint.zMotion = !allow ? x.ZMotion : ConfigurableJointMotion.Locked;
			}
		}

		private void FixedUpdate()
		{
			allowFixedJoint.SetElements(this,allowFixedJoints);
			allowPhysicsAnimation.SetElements(this,Blend is not 0);
			
			//var spring = Mathf.Lerp(Blend,0,angularSprint);
			var drive = new JointDrive
			{
				positionDamper = positionDamper,
				positionSpring =Mathf.Lerp(0,positionSpring,Blend),
				maximumForce = maximumForce,
			};
			
			var blendTorqueForce = Mathf.Lerp(0,this.torqueForce,Blend);
			foreach (var jointConfigure in jointConfigures)
			{
				jointConfigure.joint.angularXDrive = drive;
				jointConfigure.joint.angularYZDrive = drive;
				jointConfigure.joint.xDrive = drive;
				jointConfigure.joint.yDrive = drive;
				jointConfigure.joint.zDrive = drive;
				
				jointConfigure.joint.targetRotation =
					Quaternion.Lerp(
						jointConfigure.joint.transform.localRotation,
						Quaternion.Inverse(jointConfigure.animate.localRotation) *
						jointConfigure.InitialRotation,Blend
						)
					;
				jointConfigure.joint.targetPosition =
					Vector3.Lerp(
						jointConfigure.joint.transform.localPosition,
						jointConfigure.animate.localPosition,
						Blend
					);
				
				if (overrideAngularMotion.Allow)
				{
					jointConfigure.joint.angularXMotion = overrideAngularMotion.Value;
					jointConfigure.joint.angularYMotion = overrideAngularMotion.Value;
					jointConfigure.joint.angularZMotion = overrideAngularMotion.Value;
				}
				
				if (overrideMotion.Allow)
				{
					jointConfigure.joint.xMotion = overrideMotion.Value;
					jointConfigure.joint.yMotion = overrideMotion.Value;
					jointConfigure.joint.zMotion = overrideMotion.Value;
				}

				jointConfigure.Rigidbody.AddTorque(
					MathV.CalculateTorque(jointConfigure.joint.transform,jointConfigure.animate.rotation) * blendTorqueForce
					,ForceMode.VelocityChange
				);
				
				// jointConfigure.Rigidbody.AddForce(
				// 	jointConfigure.animate.localPosition - jointConfigure.Rigidbody.position,ForceMode.VelocityChange
				// 	);
				jointConfigure.Rigidbody.useGravity =Blend is 0;

				//jointConfigure.Rigidbody.MoveRotation(jointConfigure.animate.rotation);
			}

			if (rootSync.Allow)
			{
				var root = rootSync.Value;
				root.Item1.transform.localPosition = root.Item2.localPosition;
				root.Item1.MoveRotation(root.Item2.rotation);
			}

			
		}

		private void OnAllowFixedJoint(bool allow)
		{
			foreach (var x in fixedJointConfigures)
			{
				x.joint.angularXMotion = allow ? ConfigurableJointMotion.Limited :  ConfigurableJointMotion.Free;
				x.joint.angularYMotion = allow ? ConfigurableJointMotion.Limited : ConfigurableJointMotion.Free;
				x.joint.angularZMotion = allow ? ConfigurableJointMotion.Limited :  ConfigurableJointMotion.Free;
				x.joint.xMotion = allow ? ConfigurableJointMotion.Limited : ConfigurableJointMotion.Free;
				x.joint.yMotion = allow ? ConfigurableJointMotion.Limited : ConfigurableJointMotion.Free;
				x.joint.zMotion = allow ? ConfigurableJointMotion.Limited :  ConfigurableJointMotion.Free;
			}
		}
	}

}