# What's wrong with this turn to face algorithm?

I implement a torpedo object that chases a rotating planet. Specifically, it will turn toward the planet each update. Initially my implement was:

void move() {
vector3<float> to_target = target - get_position();
to_target.normalize();
position += (to_target * speed);
}


which works perfectly for torpedo that is a solid sphere. Now my torpedo is actually a model, which has a forward vector, so using this method looks odd because it doesn't actually turn toward but jump toward. So I revised it a bit to get,

double get_rotation_angle(vector3<float> u, vector3<float> v) const {
u.normalize();
v.normalize();
double cosine_theta = u.dot(v);
// domain of arccosine is [-1, 1]
if (cosine_theta > 1) {
cosine_theta = 1;
}
if (cosine_theta < -1) {
cosine_theta = -1;
}
return math3d::to_degree(acos(cosine_theta));
}

vector3<float> get_rotation_axis(vector3<float> u, vector3<float> v) const {
u.normalize();
v.normalize();
// fix linear case
if (u == v || u == -v) {
v[0] += 0.05;
v[1] += 0.0;
v[2] += 0.05;
v.normalize();
}
vector3<float> axis = u.cross(v);
return axis.normal();
}

void turn_to_face() {
vector3<float> to_target = (target - position);
vector3<float> axis = get_rotation_axis(get_forward(), to_target);
double angle = get_rotation_angle(get_forward(), to_target);
double distance = math3d::distance(position, target);
gl_matrix_mode(GL_MODELVIEW);
gl_push_matrix(); {
gl_translate_f(position.get_x(), position.get_y(), position.get_z());
gl_rotate_f(angle, axis.get_x(), axis.get_y(), axis.get_z());
gl_get_float_v(GL_MODELVIEW_MATRIX, OM);
} gl_pop_matrix();
move();
}

void move() {
vector3<float> to_target = target - get_position();
to_target.normalize();
position += (get_forward() * speed);
}


The logic is simple, I find the rotation axis by cross product, the angle to rotate by dot product, then turn toward the target position each update. Unfortunately, it looks extremely odds since the rotation happens too fast that it always turns back and forth. The forward vector for torpedo is from the ModelView matrix, the third column A:

        MODELVIEW MATRIX
--------------------------------------------------
R           U           A           T
--------------------------------------------------
1           0           0           0
0           1           0           0
0           0           1           0
0           0           0           1
--------------------------------------------------


Any suggestion or idea would be greatly appreciated.

EDIT
In my current implementation, I don't have forward vector, the only vector I have is position. OM is my orientation matrix. In addition, I have my camera call to glu_look_at before any object update, that's the reason why I have to store all the transformation of object in OM and doing this allow me to check for collision easier. In short,
vector3<float> get_forward() : return the third column.
vector3<float> get_position() : return the forth column.
vector3<float> get_up_vector() : return the 2nd column.

And in my drawing routine,

void draw() {
gl_push_matrix(); {
// camera is set here ...
// ...
gl_mult_matrix_f(OM);
// draw
} gl_pop_matrix();
}

• +1 for debugging and logging the modelview matrix, that's an important skill! – Maik Semder Nov 4 '12 at 10:37

At the moment it looks like you are setting the rotation of your torpedo (in the modelview matrix) to the wrong angle. The angle you calculated is between the torpedo's facing vector (forward) and to_target. This is useful in determining how far to turn but for displaying the torpedo with the correct orientation you want the rotation from the identity matrix to forward.

## Maintain a Matrix

Instead of re-creating a matrix every time you call turn_to_face it might be easier to maintain a matrix which you just translate and rotate each time.

matrix44<float> position_and_rotation; // Or whatever class you have.

...

void turn_to_face()
{
...

position_and_rotation.rotate(axis, angle);
glMultMatrixf(position_and_rotation.data()); // Where data() returns a raw
// pointer to the underlying
// data of the matrix.

...
}

void move()
{
position_and_rotation.translate(get_forward() * speed);
}


## Rotate Over Time

What I've written above is closer to correct but it's still wrong. It won't turn your torpedo toward the planet, it will turn it directly to the planet in a single call to turn_to_face. To turn it gradually you'll need to define a rotational_speed.

position_and_rotation.rotate(axis, min(rotational_speed, angle));


That'll mean you never turn more than rotational_speed during a call to turn_to_face and it also stops you from continually over-shooting the correct orientation.

## Minor Stuff

You'll need to make sure you are always actually moving in the direction you are facing. At the moment I can't see anywhere that forward is updated.

In get_rotation_angle you are normalizing both of the vectors which means that the range of the dot product is already [-1,1] (you don't need to manually clamp to that range).

to_target in the move function and distance in the turn_to_face function are both unused so get rid of 'em :)

• Thanks a lot for your ideas. I guess my implementation is wrong because I don't have any way to make it turn smoothly toward target. I will try to implement rotational speed. The code was a bit sloppy since I'm in debugging mode, I will definitely clean it up a bit when I got my idea down correctly. – Chan Nov 4 '12 at 19:41

As Gary already mentioned, you need to take the local forward vector of your torpedo into the calculation (not necessarily the 3rd row of the identity matrix, depends on how your model was created). If the local forward vector of the torpedo is different, change it there.

4 things changed in your code:

1. renamed to_target to forward and introduced forward_torpedo_local
2. dont use gl_load_identity() here, it messes up your camera settings
3. moved code from move() into turn_to_face(), since it is just 1 line of code anyway
4. actually move the torpedo before rendering, otherwise rot and pos are out of sync

Here is the code:

void turn_to_face() {
vector3<float> forward_torpedo_local(0.0, 0.0, 1.0);
vector3<float> forward = (target - position);
forward.normalize();
position += (forward * speed);
vector3<float> axis = get_rotation_axis(forward_torpedo_local, forward);
double angle = get_rotation_angle(forward_torpedo_local, forward);
gl_matrix_mode(GL_MODELVIEW);
gl_push_matrix(); {
gl_translate_f(position.get_x(), position.get_y(), position.get_z());
gl_rotate_f(angle, axis.get_x(), axis.get_y(), axis.get_z());
} gl_pop_matrix();
}


You should normalize the vectors before calling get_rotation_axis and get_rotation_angle, instead of inside those functions, it's less normalizing, but that's just a minor optimization, once it is working.

• Thanks for showing the actual implementation. The reason I call gl_load_identity because I want the position of my object will be respect to the world not to the camera because I have 6 different cameras, and OM is whatever orientation of just the object alone. At the moment, I really don't how to add and use forward vector for my object because I use the third column of OM as my forward vector. I could introduce a forward vector for this object, but it will be inconsistent with other moving objects. – Chan Nov 4 '12 at 19:46
• @Chan "I could introduce a forward vector for this object" There is already a local forward vector, each object has it by design. Open your model with 3D max/blender or whatever tool you used to create it (or just render it without any rotation) and check which axis it is facing. That is your local forward vector and you have to use it to calculate facing. – Maik Semder Nov 5 '12 at 11:38