float theta = convertToDegrees(tan(agent.getDirection().x / agent.getDesiredVector().y));
That looks pretty bad, now that I look at it closely. First of all, you are dividing coordinates of different vectors, which is probably not what you want. Then you are taking the tangent of that ratio, when you probably want the arctangent. Then you convert the return value of the tangent as if it were an angle, which it isn't; this reinforces my feeling that you meant to use arctangent.
If you just want to check whether you should be turning left or right, then next thing you are going to complain about is that your agent oscillates once the desired attitude is reached.
Here's some code that does what you probably mean to do:
#include <iostream>
#include <complex>
#include <cmath>
typedef std::complex<double> Complex;
Complex capped_rotation(Complex rotation, Complex max_rotation) {
return (std::imag(rotation) > std::imag(max_rotation)) ? max_rotation
: (std::imag(rotation) < -std::imag(max_rotation)) ? std::conj(max_rotation)
: rotation;
}
int main() {
static const double degree = std::atan(1.0) / 45.0;
Complex current = std::polar(1.0, 140.0 * degree);
Complex desired = std::polar(1.0, -5.0 * degree);
Complex max_rotation = std::polar(1.0, 20.0 * degree);
std::cout << "We are starting at " << current << " and want to get to " << desired << ".\n\n";
for (int i = 0; i < 30; ++i) {
std::cout << "After " << i << " steps, we are at " << current << ".\n";
current *= capped_rotation(desired / current, max_rotation);
}
}
Don't be intimidated by the complex numbers. Complex numbers of modulus 1 are precisely the normalized direction vectors, which can also represent rotations.
