- Home /
Rotating clockwise or counter clockwise using Quaternions and PID
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);
}
Your answer
Follow this Question
Related Questions
Clock script with custom time not working 0 Answers
Unity.Mathematics equivalent to Quaternion.FromToRotation? 3 Answers
few question about quaternion slerp 3 Answers
Comparing lerping quaternion 1 Answer
Distribute terrain in zones 3 Answers