Added additional torque method

Runtime/Tools/Physics/HybridPhysics.cs

@@ -320,7 +320,34 @@ namespace Passer {
             Vector3 tmp = Vector3.Cross(a, b).normalized;
             return tmp;
         }
-        #endregion
+
+        /// <summary>
+        /// Rotates a rigibogy to the desired rotation.
+        /// See: https://digitalopus.ca/site/pd-controllers/
+        /// </summary>
+        /// <param name="rb">The rigidbody to rotate</param>
+        /// <param name="desiredRotation">The orientation which is desired</param>
+        /// <param name="frequency">The speed to reach the desired rotation, duration is approximate 1/frequency</param>
+        /// <param name="damping">1 = critical damped, < 1 under damped, > 1 over damped </param>
+        public static void ApplyTorqueBackwardsPD(Rigidbody rb, Quaternion desiredRotation, float frequency = 1, float damping = 1) {
+            float kp = (6f * frequency) * (6f * frequency) * 0.25f;
+            float kd = 4.5f * frequency * damping;
+
+            Vector3 x;
+            float xMag;
+            Quaternion q = desiredRotation * Quaternion.Inverse(rb.transform.rotation);
+            q.ToAngleAxis(out xMag, out x);
+            x.Normalize();
+            x *= Mathf.Deg2Rad;
+            Vector3 pidv = kp * x * xMag - kd * rb.angularVelocity;
+            Quaternion rotInertia2World = rb.inertiaTensorRotation * rb.transform.rotation;
+            pidv = Quaternion.Inverse(rotInertia2World) * pidv;
+            pidv.Scale(rb.inertiaTensor);
+            pidv = rotInertia2World * pidv;
+            rb.AddTorque(pidv);
+        }
+
+        #endregion Torque
 
         #region Utilities