Given the code below, the object would begin facing to the right, and if something changed **yawState** to either back or forward orientations you'd see it orient itself to face that direction. Now, this is where I'm having problems, if I transition from right to forward it works, but right to left makes it wind all the way around to arrive at the wanted adjacent orientation rather than just taking the shortest path.
I believe Quaternion.slerp has the functionality where it takes the shortest path to a rotation, but I do not want to use slerp to orient the object as it doesn't play nice with physics (thus the PID).
Can anyone see the error of my ways or highlight something simple I'm missing?
Thank you!
var desiredRot = Quaternion.identity; var yawState = Direction.Right; void rotate() { switch (yawState.currentDirection) { case YawState.Direction.Forward: desiredRot = Quaternion.AngleAxis(0, Vector3.up); break; case YawState.Direction.Back: desiredRot = Quaternion.AngleAxis(180, Vector3.up); break; case YawState.Direction.Left: desiredRot = Quaternion.AngleAxis(90, Vector3.up); break; case YawState.Direction.Right: desiredRot = Quaternion.AngleAxis(-90, Vector3.up); break; } } void RotateWithPID(float frequency, float damping, Quaternion desiredRotation) { var rotation = gameObject.transform.rotation; var kp = (6f * frequency) * (6f * frequency) * 0.25f; var kd = 4.5f * frequency * damping; var q = desiredRotation * Quaternion.Inverse(rotation); q.ToAngleAxis (out var xMag, out var x); x.Normalize(); x *= Mathf.Deg2Rad; var pidVelocity = kp * x * xMag - kd * rigidbody.angularVelocity; var rotInertia2World = rigidbody.inertiaTensorRotation * rotation; pidVelocity = Quaternion.Inverse(rotInertia2World) * pidVelocity; pidVelocity.Scale(rigidbody.inertiaTensor); pidVelocity = rotInertia2World * pidVelocity; rigidbody.AddTorque(pidVelocity); } void Update() { rotate() RotateWithPID(0.5, 1, desiredRot); }
I believe Quaternion.slerp has the functionality where it takes the shortest path to a rotation, but I do not want to use slerp to orient the object as it doesn't play nice with physics (thus the PID).
Can anyone see the error of my ways or highlight something simple I'm missing?
Thank you!
var desiredRot = Quaternion.identity; var yawState = Direction.Right; void rotate() { switch (yawState.currentDirection) { case YawState.Direction.Forward: desiredRot = Quaternion.AngleAxis(0, Vector3.up); break; case YawState.Direction.Back: desiredRot = Quaternion.AngleAxis(180, Vector3.up); break; case YawState.Direction.Left: desiredRot = Quaternion.AngleAxis(90, Vector3.up); break; case YawState.Direction.Right: desiredRot = Quaternion.AngleAxis(-90, Vector3.up); break; } } void RotateWithPID(float frequency, float damping, Quaternion desiredRotation) { var rotation = gameObject.transform.rotation; var kp = (6f * frequency) * (6f * frequency) * 0.25f; var kd = 4.5f * frequency * damping; var q = desiredRotation * Quaternion.Inverse(rotation); q.ToAngleAxis (out var xMag, out var x); x.Normalize(); x *= Mathf.Deg2Rad; var pidVelocity = kp * x * xMag - kd * rigidbody.angularVelocity; var rotInertia2World = rigidbody.inertiaTensorRotation * rotation; pidVelocity = Quaternion.Inverse(rotInertia2World) * pidVelocity; pidVelocity.Scale(rigidbody.inertiaTensor); pidVelocity = rotInertia2World * pidVelocity; rigidbody.AddTorque(pidVelocity); } void Update() { rotate() RotateWithPID(0.5, 1, desiredRot); }