*debug

5 files changed, 249 insertions, 13 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()
         {
diff --git a/ActiveRagdoll/Assets/TABG/Scripts/Debug/GLHandle.cs b/ActiveRagdoll/Assets/TABG/Scripts/Debug/GLHandle.cs
index f2a6679..7f2eeae 100644
--- a/ActiveRagdoll/Assets/TABG/Scripts/Debug/GLHandle.cs
+++ b/ActiveRagdoll/Assets/TABG/Scripts/Debug/GLHandle.cs
@@ -1,18 +1,141 @@
-using System.Collections;
-using System.Collections.Generic;
+using System;
 using UnityEngine;
 
-public class GLHandle : MonoBehaviour
+namespace Rigging.Debugging
 {
-    // Start is called before the first frame update
-    void Start()
+
+    public class GLScope : IDisposable
     {
-        
+        public GLScope(bool enabled)
+        {
+            GLHandle.EnterScope(enabled);
+        }
+
+        public void Dispose()
+        {
+            GLHandle.EndScope();
+        }
     }
 
-    // Update is called once per frame
-    void Update()
+    [DefaultExecutionOrder(-100)]
+    public class GLHandle : MonoBehaviour
     {
-        
+
+        private static GLHandle instance;
+        private static event System.Action onGL;
+        private static event System.Action onGLFixedUpdate;
+
+        private static Material lineMaterial;
+
+        private static bool bScopeEnabled;
+
+        static GLHandle()
+        {
+            bScopeEnabled = true;
+        }
+
+        static void CreateLineMaterial()
+        {
+            if (!lineMaterial)
+            {
+                // Unity has a built-in shader that is useful for drawing
+                // simple colored things.
+                Shader shader = Shader.Find("Hidden/Internal-Colored");
+                lineMaterial = new Material(shader);
+                lineMaterial.hideFlags = HideFlags.HideAndDontSave;
+                // Turn on alpha blending
+                lineMaterial.SetInt("_SrcBlend", (int)UnityEngine.Rendering.BlendMode.SrcAlpha);
+                lineMaterial.SetInt("_DstBlend", (int)UnityEngine.Rendering.BlendMode.OneMinusSrcAlpha);
+                // Turn backface culling off
+                lineMaterial.SetInt("_Cull", (int)UnityEngine.Rendering.CullMode.Off);
+                // Turn off depth writes
+                lineMaterial.SetInt("_ZWrite", 0);
+                lineMaterial.SetInt("_ZTest", 0);
+            }
+        }
+
+        private void FixedUpdate()
+        {
+            onGLFixedUpdate = null;
+        }
+
+        public void OnRenderObject()
+        {
+            CreateLineMaterial();
+
+            // Apply the line material
+            lineMaterial.SetPass(0);
+
+            onGL?.Invoke();
+            onGL = null;
+
+            onGLFixedUpdate?.Invoke();
+        }
+
+        static void Check()
+        {
+            if(instance == null)
+            {
+                GameObject gl = new GameObject();
+                gl.name = "GLHandle";
+                instance = gl.AddComponent<GLHandle>();
+            }
+        }
+
+        public static void EnterScope(bool enabled = true)
+        {
+            bScopeEnabled = enabled;
+        }
+
+        public static void EndScope()
+        {
+            bScopeEnabled = true;
+        }
+
+        public static void DrawSphere(Vector3 pos, float radius, int verticalSegments = 1, int radialSegments = 2)
+        {
+            Check();
+
+            if (!bScopeEnabled) return;
+
+            if(Time.inFixedTimeStep)
+            {
+                onGLFixedUpdate += () =>
+                {
+                    Draw.Sphere(pos, radius, verticalSegments, radialSegments);
+                };
+            }
+            else
+            {
+                onGL += () =>
+                {
+                    Draw.Sphere(pos, radius, verticalSegments, radialSegments);
+                };
+            }
+        }
+
+        public static void DrawLine(Vector3 p1, Vector3 p2)
+        {
+            Check();
+
+            if (!bScopeEnabled) return;
+
+            if (Time.inFixedTimeStep)
+            {
+                onGLFixedUpdate += () =>
+                {
+                    Draw.Line3D(p1, p2);
+                };
+            }
+            else
+            {
+                onGL += () =>
+                {
+                    Draw.Line3D(p1, p2);
+                };
+            }
+        }
+
     }
-}
+
+}
\ No newline at end of file
diff --git a/ActiveRagdoll/Assets/TABG/Scripts/Utils/Utils.cs b/ActiveRagdoll/Assets/TABG/Scripts/Utils/Utils.cs
new file mode 100644
index 0000000..746f80e
--- /dev/null
+++ b/ActiveRagdoll/Assets/TABG/Scripts/Utils/Utils.cs
@@ -0,0 +1,18 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace Rigging
+{
+
+    public static class Utils
+    {
+        public static bool IsInsidePhysics()
+        {
+            return Time.inFixedTimeStep;
+        }
+
+
+    }
+
+}
diff --git a/ActiveRagdoll/Assets/TABG/Scripts/Utils/Utils.cs.meta b/ActiveRagdoll/Assets/TABG/Scripts/Utils/Utils.cs.meta
new file mode 100644
index 0000000..770d207
--- /dev/null
+++ b/ActiveRagdoll/Assets/TABG/Scripts/Utils/Utils.cs.meta
@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: cbdf43156a3daa549be2eea4d250de4b
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
diff --git a/ActiveRagdoll/Assets/TABG/Test.unity b/ActiveRagdoll/Assets/TABG/Test.unity
index a350660..81f2617 100644
--- a/ActiveRagdoll/Assets/TABG/Test.unity
+++ b/ActiveRagdoll/Assets/TABG/Test.unity
@@ -670,8 +670,8 @@ Transform:
   m_PrefabInstance: {fileID: 0}
   m_PrefabAsset: {fileID: 0}
   m_GameObject: {fileID: 1848931106}
-  m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
-  m_LocalPosition: {x: 0, y: 1, z: -10}
+  m_LocalRotation: {x: 0.14744055, y: 0.41793463, z: -0.068966135, w: 0.89377606}
+  m_LocalPosition: {x: -3.688255, y: 2.6339426, z: -2.137497}
   m_LocalScale: {x: 1, y: 1, z: 1}
   m_ConstrainProportionsScale: 0
   m_Children: []