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