jointdrive controller added

Assets/8_Scripts/2_General/Body.cs

@@ -9,8 +9,48 @@ public class Body : MonoBehaviour
     public Transform m_Pelvis;
     public int m_DespawnTimer;
 
+	public bool m_JDActive;
+
+	[Header("Body Parts")]
+	public Transform m_Head;
+	public Transform m_Spine;
+	public Transform m_Left_Thigh;
+	public Transform m_Left_Leg;
+	public Transform m_Left_Foot;
+	public Transform m_Right_Thigh;
+	public Transform m_Right_Leg;
+	public Transform m_Right_Foot;
+	public Transform m_Left_Arm;
+	public Transform m_Left_Forearm;
+	public Transform m_Right_Arm;
+	public Transform m_Right_Forearm;
+
+
+	private JointDriveController m_JDController;
+
     private void OnEnable()
     {
+		if(m_JDController == null && m_JDActive) {
+			m_JDController = this.GetComponent<JointDriveController>();
+
+			m_JDController.SetupBodyPart(m_Head);
+			m_JDController.SetupBodyPart(m_Spine);
+
+			m_JDController.SetupBodyPart(m_Left_Thigh);
+			m_JDController.SetupBodyPart(m_Left_Leg);
+			m_JDController.SetupBodyPart(m_Left_Foot);
+
+			m_JDController.SetupBodyPart(m_Right_Thigh);
+			m_JDController.SetupBodyPart(m_Right_Leg);
+			m_JDController.SetupBodyPart(m_Right_Foot);
+
+			m_JDController.SetupBodyPart(m_Left_Arm);
+			m_JDController.SetupBodyPart(m_Left_Forearm);
+
+			m_JDController.SetupBodyPart(m_Right_Arm);
+			m_JDController.SetupBodyPart(m_Right_Forearm);
+		}
+		
         StartCoroutine(DespawnTimer());
     }
 

Assets/8_Scripts/2_General/JointDriveController.cs

@@ -0,0 +1,152 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+ [System.Serializable]
+    public class BodyPart
+    {
+        [Header("Body Part Info")] [Space(10)] public ConfigurableJoint joint;
+        public Rigidbody rb;
+        [HideInInspector] public Vector3 startingPos;
+        [HideInInspector] public Quaternion startingRot;
+        [HideInInspector] public JointDriveController thisJdController;
+
+        [Header("Current Joint Settings")]
+        [Space(10)]
+        public Vector3 currentEularJointRotation;
+
+        [HideInInspector] public float currentStrength;
+        public float currentXNormalizedRot;
+        public float currentYNormalizedRot;
+        public float currentZNormalizedRot;
+
+        [Header("Other Debug Info")]
+        [Space(10)]
+        public Vector3 currentJointForce;
+
+        public float currentJointForceSqrMag;
+        public Vector3 currentJointTorque;
+        public float currentJointTorqueSqrMag;
+        public AnimationCurve jointForceCurve = new AnimationCurve();
+        public AnimationCurve jointTorqueCurve = new AnimationCurve();
+
+        /// <summary>
+        /// Reset body part to initial configuration.
+        /// </summary>
+        public void Reset(BodyPart bp)
+        {
+            bp.rb.transform.position = bp.startingPos;
+            bp.rb.transform.rotation = bp.startingRot;
+            bp.rb.velocity = Vector3.zero;
+            bp.rb.angularVelocity = Vector3.zero;
+        }
+
+        /// <summary>
+        /// Apply torque according to defined goal `x, y, z` angle and force `strength`.
+        /// </summary>
+        public void SetJointTargetRotation(float x, float y, float z)
+        {
+            x = (x + 1f) * 0.5f;
+            y = (y + 1f) * 0.5f;
+            z = (z + 1f) * 0.5f;
+
+            var xRot = Mathf.Lerp(joint.lowAngularXLimit.limit, joint.highAngularXLimit.limit, x);
+            var yRot = Mathf.Lerp(-joint.angularYLimit.limit, joint.angularYLimit.limit, y);
+            var zRot = Mathf.Lerp(-joint.angularZLimit.limit, joint.angularZLimit.limit, z);
+
+            currentXNormalizedRot =
+                Mathf.InverseLerp(joint.lowAngularXLimit.limit, joint.highAngularXLimit.limit, xRot);
+            currentYNormalizedRot = Mathf.InverseLerp(-joint.angularYLimit.limit, joint.angularYLimit.limit, yRot);
+            currentZNormalizedRot = Mathf.InverseLerp(-joint.angularZLimit.limit, joint.angularZLimit.limit, zRot);
+
+            joint.targetRotation = Quaternion.Euler(xRot, yRot, zRot);
+            currentEularJointRotation = new Vector3(xRot, yRot, zRot);
+        }
+
+        public void SetJointStrength(float strength)
+        {
+            var rawVal = (strength + 1f) * 0.5f * thisJdController.maxJointForceLimit;
+            var jd = new JointDrive
+            {
+                positionSpring = thisJdController.maxJointSpring,
+                positionDamper = thisJdController.jointDampen,
+                maximumForce = rawVal
+            };
+            joint.slerpDrive = jd;
+            currentStrength = jd.maximumForce;
+        }
+    }
+
+    public class JointDriveController : MonoBehaviour
+    {
+        [Header("Joint Drive Settings")]
+        [Space(10)]
+        public float maxJointSpring;
+
+        public float jointDampen;
+        public float maxJointForceLimit;
+
+        [HideInInspector] public Dictionary<Transform, BodyPart> bodyPartsDict = new Dictionary<Transform, BodyPart>();
+
+        [HideInInspector] public List<BodyPart> bodyPartsList = new List<BodyPart>();
+        const float k_MaxAngularVelocity = 50.0f;
+
+        /// <summary>
+        /// Create BodyPart object and add it to dictionary.
+        /// </summary>
+        public void SetupBodyPart(Transform t)
+        {
+            var bp = new BodyPart
+            {
+                rb = t.GetComponent<Rigidbody>(),
+                joint = t.GetComponent<ConfigurableJoint>(),
+                startingPos = t.position,
+                startingRot = t.rotation
+            };
+            bp.rb.maxAngularVelocity = k_MaxAngularVelocity;
+			
+            if (bp.joint)
+            {
+                var jd = new JointDrive
+                {
+                    positionSpring = maxJointSpring,
+                    positionDamper = jointDampen,
+                    maximumForce = maxJointForceLimit
+                };
+                bp.joint.slerpDrive = jd;
+            }
+
+            bp.thisJdController = this;
+            bodyPartsDict.Add(t, bp);
+            bodyPartsList.Add(bp);
+        }
+
+        public void GetCurrentJointForces()
+        {
+            foreach (var bodyPart in bodyPartsDict.Values)
+            {
+                if (bodyPart.joint)
+                {
+                    bodyPart.currentJointForce = bodyPart.joint.currentForce;
+                    bodyPart.currentJointForceSqrMag = bodyPart.joint.currentForce.magnitude;
+                    bodyPart.currentJointTorque = bodyPart.joint.currentTorque;
+                    bodyPart.currentJointTorqueSqrMag = bodyPart.joint.currentTorque.magnitude;
+                    if (Application.isEditor)
+                    {
+                        if (bodyPart.jointForceCurve.length > 1000)
+                        {
+                            bodyPart.jointForceCurve = new AnimationCurve();
+                        }
+
+                        if (bodyPart.jointTorqueCurve.length > 1000)
+                        {
+                            bodyPart.jointTorqueCurve = new AnimationCurve();
+                        }
+
+                        bodyPart.jointForceCurve.AddKey(Time.time, bodyPart.currentJointForceSqrMag);
+                        bodyPart.jointTorqueCurve.AddKey(Time.time, bodyPart.currentJointTorqueSqrMag);
+                    }
+                }
+            }
+        }
+    }

Assets/8_Scripts/2_General/JointDriveController.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: e109baaf62426244eb24aec54decdf71
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: