If you plan to use vectors, you could actually just use some properties of vectors to find the target angle. Let U be the vector in the direction the wheels are currently facing, and V be the vector in the direction you want the wheels to face. Assuming U and V are unit vectors (that is, they have a length of one), the angle between then (let's call it Q) is found by the equation Q = acos((Ux*Vx)+(Uy*Vy)). Because acos returns a value between 0 and pi, we can subtract Q from pi if Q is greater than pi/2 (because, if Q is greater than pi/2, it is quicker to turn the wheels to face in the opposite direction and rotate them backwards). After this set, Q is between 0 and pi/2, but we don't know which direction to rotate in. Assume a positive value of Q corresponds to a counterclockwise rotation, we can then give Q the same sign as the expression (Ux * Vy - Uy * Vx), which is always positive if the rotation required to move U to V is counterclockwise, and negative if the rotation is clockwise. This will give you the shortest counterclockwise rotation, with negative magnitude benefits.
In psuedo-C:
Code:
//To start, we assume magnitude is positive
bool neg_mag_benefits = false;
double getCorrectionAngle(double currentAngle, double destinationAngle) {
double currentX = cos(currentAngle);
double currentY = sin (currentAngle);
double destinationX = cos(destinationAngle);
double destinationY = sin(destinationAngle);
double correctionAngle = acos(currentX * destinationX + currentY * destinationY);
//correctionAngle is in the range 0 to PI.
double crossProd = currentX * destinationY - currentY * destinationX;
if(correctionAngle > PI/2) {
neg_mag_benefits = true; //We will be turning the wheels in a negative direction
correctionAngle = PI - correctionAngle; //In range 0 to PI/2
}
correctionAngle *= abs(crossProd)/crossProd;
return correctionAngle;
}