1 files changed, 130 insertions, 0 deletions
diff --git a/_ActiveRagdoll/Actions/Balance.cs b/_ActiveRagdoll/Actions/Balance.cs
new file mode 100644
index 0000000..7776c88
--- /dev/null
+++ b/_ActiveRagdoll/Actions/Balance.cs
@@ -0,0 +1,130 @@
+using UnityEngine;
+
+//Player Balance 动作-控制双脚的位置，让双脚和双脚的中点尽可能和重心在水平的投影重合
+public class Balance : MonoBehaviour
+{
+    #region rigs
+    private Rigidbody handLeft;
+	private Rigidbody handRight;
+	private Rigidbody footLeft; // kneeLeft
+	private Rigidbody footRight; // kneeRight
+	private Rigidbody hip;
+    #endregion 
+
+    private Vector3 centerOfMass; // 5.0759, 0, -7.2038
+
+    private Rigidbody[] allRigs;//所有14个parts
+
+	public float[] balanceForce; // 50 30 30 30
+
+	public float[] footCenterForces; // 50 20 20 20
+
+	private AnimationHandler animationHandler; // 当前动作编号
+
+	private PlayerDeath death;
+
+	private Strength str;
+
+	private float muscleMultiplier; // 1
+
+	private void Start()
+	{
+		death = GetComponent<PlayerDeath>();
+		str = GetComponent<Strength>();
+		allRigs = GetComponentsInChildren<Rigidbody>();
+		HandLeft componentInChildren = GetComponentInChildren<HandLeft>();
+		if ((bool)componentInChildren)
+		{
+			handLeft = componentInChildren.GetComponent<Rigidbody>();
+		}
+		HandRight componentInChildren2 = GetComponentInChildren<HandRight>();
+		if ((bool)componentInChildren2)
+		{
+			handRight = componentInChildren2.GetComponent<Rigidbody>();
+		}
+		footLeft = GetComponentInChildren<KneeLeft>().GetComponent<Rigidbody>();
+		footRight = GetComponentInChildren<KneeRight>().GetComponent<Rigidbody>();
+		hip = GetComponentInChildren<Hip>().GetComponent<Rigidbody>();
+		animationHandler = GetComponent<AnimationHandler>();
+	}
+
+	private void FixedUpdate()
+	{
+		if (!death.dead)
+		{
+			muscleMultiplier = str.strength;
+			centerOfMass = Vector3.zero;
+			Rigidbody[] array = allRigs;
+			foreach (Rigidbody rigidbody in array)
+			{
+				centerOfMass += rigidbody.worldCenterOfMass;
+			}
+			centerOfMass /= (float)allRigs.Length;
+			centerOfMass.y = 0f;
+			BalanceLegs();
+			CenterLegs();
+		}
+	}
+
+	// 让双脚尽量靠近重心的水平投影
+	private void CenterLegs()
+	{
+		Vector3 vector = footLeft.transform.position + footLeft.transform.forward * 0.5f;
+		Vector3 vector2 = footRight.transform.position + footRight.transform.forward * 0.5f;
+		Vector3 vector3 = vector;
+		if (vector.y + 0.3f < hip.worldCenterOfMass.y)
+		{
+			vector3.y = 0f;
+			footLeft.AddForceAtPosition((centerOfMass - vector3) * muscleMultiplier * footCenterForces[animationHandler.animationState], vector, ForceMode.Acceleration);
+		}
+		Vector3 vector4 = vector2;
+		if (vector4.y + 0.3f < hip.worldCenterOfMass.y)
+		{
+			vector4.y = 0f;
+			footRight.AddForceAtPosition((centerOfMass - vector4) * muscleMultiplier * footCenterForces[animationHandler.animationState], vector2, ForceMode.Acceleration);
+		}
+	}
+
+	// 让双脚的中点尽量和重心的水平投影重合
+	private void BalanceLegs()
+	{
+		Vector3 vector = footLeft.transform.position + footLeft.transform.forward * 0.5f;
+		Vector3 vector2 = footRight.transform.position + footRight.transform.forward * 0.5f;
+		Vector3 vector3 = (vector + vector2) / 2f;
+
+		Vector3 pos = vector3;
+		pos.y += 0.3f;
+		Vector3 vector03 = vector3;
+
+        if (!(vector3.y + 0.3f > hip.worldCenterOfMass.y))
+		{
+			vector3.y = 0f;
+			Vector3 vector4 = centerOfMass - vector3; // centerOfMass是xoz平面，不含y
+
+			REPL.footCenter.GetComponent<DebugRigidBody>().customDraw = () =>
+			{
+				return;
+
+				Draw.Sphere(vector, 0.05f);
+				Draw.Sphere(vector2, 0.05f);
+
+				Draw.color = Color.red;
+				Draw.Sphere(pos, 0.05f);
+				Draw.Sphere(vector03, 0.05f);
+				Draw.color = Color.white;
+
+				Draw.Sphere(hip.worldCenterOfMass, 0.2f);
+
+				Draw.Sphere(centerOfMass, 0.05f);
+
+                Draw.color = Color.green;
+                Draw.Line3D(vector, vector + vector4);
+                Draw.Line3D(vector2, vector2 + vector4);
+                Draw.color = Color.white;
+			};
+
+            footLeft.AddForceAtPosition(vector4 * muscleMultiplier * balanceForce[animationHandler.animationState], vector, ForceMode.Acceleration);
+            footRight.AddForceAtPosition(vector4 * muscleMultiplier * balanceForce[animationHandler.animationState], vector2, ForceMode.Acceleration);
+        };
+	}
+}