diff --git a/Runtime/Tools/Physics/HybridPhysics.cs b/Runtime/Tools/Physics/HybridPhysics.cs
index 3c4140e..805b1c6 100644
--- a/Runtime/Tools/Physics/HybridPhysics.cs
+++ b/Runtime/Tools/Physics/HybridPhysics.cs
@@ -320,7 +320,34 @@ namespace Passer {
Vector3 tmp = Vector3.Cross(a, b).normalized;
return tmp;
}
- #endregion
+
+ ///
+ /// Rotates a rigibogy to the desired rotation.
+ /// See: https://digitalopus.ca/site/pd-controllers/
+ ///
+ /// The rigidbody to rotate
+ /// The orientation which is desired
+ /// The speed to reach the desired rotation, duration is approximate 1/frequency
+ /// 1 = critical damped, < 1 under damped, > 1 over damped
+ 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