1 files changed, 85 insertions, 1 deletions
diff --git a/ActiveRagdoll/Assets/TABG/Scripts/Action/Balance.cs b/ActiveRagdoll/Assets/TABG/Scripts/Action/Balance.cs
index f8257e7..a003c4e 100644
--- a/ActiveRagdoll/Assets/TABG/Scripts/Action/Balance.cs
+++ b/ActiveRagdoll/Assets/TABG/Scripts/Action/Balance.cs
@@ -1,3 +1,4 @@
+using Rigging.Data;
 using Rigging.Debugging;
 using System.Collections;
 using System.Collections.Generic;
@@ -43,10 +44,93 @@ namespace Rigging.Action
             hip = player.body.hip.GetComponent<Rigidbody>();
         }
 
+        private void BalanceLegs()
+        {
+            using GLScope s = new GLScope(false);
+
+            // 计算双脚的中心位置
+            Vector3 vector = footLeft.transform.position + footLeft.transform.forward * 0.5f;
+            Vector3 vector2 = footRight.transform.position + footRight.transform.forward * 0.5f;
+
+            GLHandle.DrawSphere(vector, 0.1f);
+            GLHandle.DrawSphere(vector2, 0.1f);
+
+            Vector3 vector3 = (vector + vector2) / 2f;
+
+            GLHandle.DrawSphere(vector3, 0.1f);
+            GLHandle.DrawSphere(vector3 + new Vector3(0, 0.3f, 0), 0.1f);
+
+            // 迫使双脚向质心移动
+            if (!(vector3.y + 0.3f > hip.worldCenterOfMass.y))
+            {
+                vector3.y = 0f;//同样只考虑XOZ平面，忽略高度
+                Vector3 vector4 = centerOfMass - vector3; // 质心和双脚中心的偏移
+
+                GLHandle.DrawSphere(vector3, 0.1f);
+                GLHandle.DrawSphere(centerOfMass, 0.1f);
+                GLHandle.DrawSphere(hip.worldCenterOfMass, 0.1f);
+
+                GLHandle.DrawLine(vector, vector + vector4);
+                GLHandle.DrawLine(vector2, vector2 + vector4);
+
+                footLeft.AddForceAtPosition(vector4 * muscleMultiplier * crouchMultiplier * balanceForce, vector, ForceMode.Acceleration);
+                footRight.AddForceAtPosition(vector4 * muscleMultiplier * crouchMultiplier * balanceForce, vector2, ForceMode.Acceleration);
+            }
+        }
+
+        private void CenterLegs()
+        {
+            using GLScope s = new GLScope(true);
+
+            Vector3 vector = footLeft.transform.position + footLeft.transform.forward * 0.5f;
+            Vector3 vector2 = footRight.transform.position + footRight.transform.forward * 0.5f;
+
+            GLHandle.DrawSphere(vector, 0.1f);
+            GLHandle.DrawSphere(vector2, 0.1f);
+
+            // 迫使双脚往质心移动
+
+            // 左脚
+            Vector3 vector3 = vector;
+            if (vector.y + 0.3f < hip.worldCenterOfMass.y)
+            {
+                vector3.y = 0f;
+                footLeft.AddForceAtPosition((centerOfMass - vector3) * muscleMultiplier * footCenterForces, vector, ForceMode.Acceleration);
+                GLHandle.DrawLine(vector, vector + (centerOfMass - vector3));
+            }
+
+            // 右脚
+            Vector3 vector4 = vector2;
+            if (vector4.y + 0.3f < hip.worldCenterOfMass.y)
+            {
+                vector4.y = 0f;
+                footRight.AddForceAtPosition((centerOfMass - vector4) * muscleMultiplier * footCenterForces, vector2, ForceMode.Acceleration);
+                GLHandle.DrawLine(vector2, vector2 + (centerOfMass - vector4));
+            }
+        }
+
         protected override void OnFixedUpdate()
         {
+            //muscleMultiplier = str.strength;
 
-        }   
+            // 计算角色所有rigidbody合起来的质心在XOZ平面的投影位置（即忽略高度）
+            centerOfMass = Vector3.zero;
+            int num = 0;
+            Rigidbody[] array = allRigs;
+            foreach (Rigidbody rigidbody in array)
+            {
+                if ((bool)rigidbody)
+                {
+                    centerOfMass += rigidbody.worldCenterOfMass;
+                    num++;
+                }
+            }
+            centerOfMass /= (float)Mathf.Max(num, 1);
+            centerOfMass.y = 0f;
+
+            BalanceLegs();
+            CenterLegs();
+        }
 
         protected override void OnUpdate()
         {