# Align to velocity vector using Quaternions

## Recommended Posts

nardove    100
Hi, I am studying how to use quaternions to orientate an object to its velocity, I did lots of research on the web about the subject, but they all say the same thing and I cant work out how to implement the concepts, so I am asking for a bit of guidance. Having a velocity vector and location vector how can I align my 3D object to face in the same direction as the velocity? So far I have the following data: Vec3D new_dir = new Vec3D(velocity); new_dir.normalize(); Vec3D new_up = new Vec3D(0.0, 1.0, 0.0); new_up.normalize(); // do I need to normalize this vector? Vec3D crossP = new_dir.cross(new_up); crossP.normalize(); float dotP = new_dir.dot(new_up); float angle = new_dir.angleBetween(new_up, true); From that how can I create a Quaternion? and with that Quaternion How can I rotate the object? I am using Processing as programming language which is based on Java Many thanks rS

##### Share on other sites
jyk    2094
Quote:
 Vec3D new_up = new Vec3D(0.0, 1.0, 0.0); new_up.normalize(); // do I need to normalize this vector?
The question of whether the vector needs to be normalized should be easy enough to answer for yourself. What is the length of new_up after it's constructed? What does the 'normalize' operation do? Mathematically, would the 'normalize' operation change the vector in any way in this case? From that, you should be able to determine whether the normalization is needed.
Quote:
 Having a velocity vector and location vector how can I align my 3D object to face in the same direction as the velocity?
For simply aligning an object with a direction vector (the velocity vector in this case), the position of the object doesn't matter.

There are quite a few ways to align an object with a direction vector. The first thing you have to figure out is what direction the object 'points' in local space, so that you know what it is exactly that you need to align. For most objects, the local-space direction vector will be one of the cardinal axes (often z or x).

The next thing to realize is that there's an infinite number of orientations that will satisfy the specified constraints. Imagine an airplane flying straight up. The plane can roll around its local forward axis while still being aligned with its 'direction' vector. So one way or another, you need to narrow it down to a single orientation.

The 'fixed up' method you seem to be trying to use is one way to do that. It'll work fine so long as the direction vector isn't parallel or nearly parallel with the up vector. The full method would look something like this:
vector3 side = cross(up, forward);side.normalize();up = cross(forward, side);matrix m = matrix_from_basis_vectors(side, up, forward);quaternion q = quaternion_from_matrix(m);
The above pseudocode performs the conversion by way of a matrix simply because 'quaternion from matrix' functions are fairly common, while 'quaternion from basis vectors' functions aren't so common (even though they're basically equivalent).

Another approach to aligning an object with a direction vector is to apply a relative rotation that rotates the object into alignment over the shortest arc. This can be done using an axis-angle computation, but there's also a nice quaternion-specific algorithm that does it a bit more elegantly.

##### Share on other sites
nardove    100
Cristal clear, I will try it out

Cheers
rS

##### Share on other sites
nardove    100
Hi jyk, so you said using my method "It'll work fine so long as the direction vector isn't parallel or nearly parallel with the up vector"

1. is there a way to avoid that? I can see that my object flip somethimes back and forward, is that the reason? if so can it be prevented?

2. will your pseudo code rotating using a quaternion fix the problem in 1?

Cheers
rS

##### Share on other sites
jyk    2094
Quote:
 Original post by nardoveHi jyk, so you said using my method "It'll work fine so long as the direction vector isn't parallel or nearly parallel with the up vector"1. is there a way to avoid that? I can see that my object flip somethimes back and forward, is that the reason? if so can it be prevented?2. will your pseudo code rotating using a quaternion fix the problem in 1?
The example I gave is susceptible to the problem I mentioned. The problem is here:
// If 'up' and 'forward' are nearly parallel, the magnitude (length) of side will be small.vector3 side = cross(up, forward);// Normalizing a vector with small magnitude can have unpredictable results. Mathematically, you// should be able to normalize any vector with non-zero length, but due to the limits of floating-point// precision, normalization may fail even for vectors with very small magnitude (that is, you'll get// undefined results, e.g. NaN's).side.normalize();// If normalizing the 'side' vector fails, everything from here on out will be invalid.up = cross(forward, side);matrix m = matrix_from_basis_vectors(side, up, forward);quaternion q = quaternion_from_matrix(m);
From a conceptual standpoint, it makes sense that the algorithm doesn't work when the direction and up vectors are aligned. The purpose of the up vector is to provide additional information about how to orient the object, and if it's parallel to the direction vector, it provides no such information.

If this case is likely to come up in your simulation, I'd recommend instead using relative rotations to align your object incrementally. The basic algorithm looks like this:
vector3 axis = cross(current_forward, desired_forward);float length = axis.length();if (length > epsilon) {    axis /= length;    float angle = atan2(length, dot(current_forward, desired_forward));    quaternion q = quaternion_from_axis_angle(axis, angle);    orientation = q * orientation; // 'orientation' is a quaternion    orientation.normalize();}
That's off the top of my head, but I think I got it right.

There's also a more elegant version of this algorithm that is specific to quaternions (the above is a general form of the algorithm that will work equally well with quaternions and matrices). I would try the above first and make sure it works, but if you want I can post the quaternion-specific version as well.

##### Share on other sites
nardove    100
Hi, I havent update my code to your prev pseudocode using quaternions, from my prev code I change crossP to new_side, so this is how I perform my orientation

-- INI CODE

Vec3D new_dir = new Vec3D(velocity);
new_dir.normalize();
Vec3D new_up = new Vec3D(0.0, 1.0, 0.0);
new_up.normalize(); // do I need to normalize this vector?
Vec3D new_side = new_dir.cross(new_up);
new_side.normalize();

float dotP = new_dir.dot(new_up);
float angle = new_dir.angleBetween(new_up, true);

pushMatrix();
// update location
translate(location.x, location.y, location.z);
// orientation to velocity
rotate(-angle, new_side.x, new_side.y, new_side.z);
/* draw happens here */
popMatrix();

-- END CODE

You said things will go wrong when new_side.normalize(); fails, and from there I have to check

float length = new_side.length();
if (length > EPSILON) {
new_side /= length;
....

from there you lost me, will it be possible provide a pseudocode using my logic to rotate instead of quaternions?

Thanks a lot this post has been great source of information and understanding
rS

##### Share on other sites
jyk    2094
Quote:
 from there you lost me, will it be possible provide a pseudocode using my logic to rotate instead of quaternions?
Unfortunately I don't think I can provide a working version of your method, because I don't quite understand what it's doing (or trying to do). Among other things, there's some context missing; it's not really clear from your example (to me at least) how the transforms are being handled, and what the 'push' and 'pop' calls are supposed to do.

Perhaps you could describe in a bit more detail what type of behavior you're after. Are you trying to get the object to rotate incrementally each update so as to align with the velocity vector? Or are you trying to build a transform 'from scratch' that will align the object with the velocity vector?

##### Share on other sites
nardove    100
I understand, I ask too much, ok so push and pop Matrix

"The pushMatrix() function saves the current coordinate system to the stack and popMatrix() restores the prior coordinate system. pushMatrix() and popMatrix() are used in conjuction with the other transformation methods and may be embedded to control the scope of the transformations."

very similar to opengl gl.glPushMatrix() and gl.glPopMatrix()

Using this code I calculate my angle and axis of rotation

Vec3D new_dir = new Vec3D(velocity);
new_dir.normalize();
Vec3D new_up = new Vec3D(0.0, 1.0, 0.0);
new_up.normalize(); // do I need to normalize this vector?
Vec3D new_side = new_dir.cross(new_up);
new_side.normalize();

float dotP = new_dir.dot(new_up);
float angle = new_dir.angleBetween(new_up, true);

So angle = angle of rotation and new_side = axis

This code I do the matrix transformations

pushMatrix();
// update location
translate(location.x, location.y, location.z);
// orientation to velocity
rotate(-angle, new_side.x, new_side.y, new_side.z);
/* draw happens here */
popMatrix();

Here is a link to my work in progress, so you can see the problem, and you can also look at the code, all the prev code is in the Boids Class in the display() method

http://nardove.com/p5/swimming_jellie/swimming_jellie_test_2/

If you click and drag the jelly will follow the mouse position and if you are on top an drag to the sides you can see the jump in the rotation, if you let it wander it becomes more clear the sudden jump in the orientation

Cheers
rS

##### Share on other sites
jyk    2094
I understand what the matrix push/pop functions do, but what's not immediately clear from your example is the state of the transform matrix going into the push/pop block. Is it identity? Are these transforms being combined with an existing transform? Are the transforms cumulative?

I'm sure I could figure it out if I examined your code carefully enough, but it would be easier if the example were a little more clear.

Actually, just answering this question from my previous post:
Quote:
 Perhaps you could describe in a bit more detail what type of behavior you're after. Are you trying to get the object to rotate incrementally each update so as to align with the velocity vector? Or are you trying to build a transform 'from scratch' that will align the object with the velocity vector?
Would probably help.

##### Share on other sites
nardove    100
There is no other matrix transformations, the only transformations are inside the push and pop, all I am doing is taking the velocity vector and create a new direction vector from it, them I declare a hardocoded up vector, and with those 2 I take the cross product to get my axis of rotation, them I calculate the angle of rotation between the new direction vector and the hardcoded up (0, 1, 0)

and plug those 2 in the rotate method like this rotate(angle, croosp.x, crossp.y, crossp.z);

I can see that some times there is a flip of the rotation making the object to face the opposite direction and also if the object goes up it will do a 360 rotation int the y axis I belive.

This concept of 3D orientation is new to me, and from that it is dificult for me to explain in more detail.

I have a wander steering behavior going on, and I want the objects to face the same direction as their velocity, at the moment it is working but like I said there are moments were they go nuts.

Sorry if I cant explain it in more detail you see I am a rooky in the 3D programming stuff.

All your help its very much appreciated!
rS

##### Share on other sites
jyk    2094
The jellyfish looks good! Is that your doing? If so, what mechanism are you using to animate the tentacles?

The little 'spins' that it does are actually kind of neat, but I'm guessing they're not intentional :) Anyway, for this type of motion I would definitely recommend accumulated relative rotations. To keep things simple, I'll skip the quaternion-specific version of the algorithm and just describe the straightforward version. In pseudocode, it looks something like this (typed directly into the post, and not tested in any way):
// Somewhere during initialization:quaternion q = whatever_you_want_the_initial_orientation_to_be();// For each update:matrix m = matrix_from_quaternion(q);vector3 forward = m.get_third_basis_vector(); // Ask if you're not sure what this meansvector3 desired_forward = normalize(velocity);vector3 axis = cross(forward, desired_forward);float length = axis.length();if (length > epsilon) {    axis /= length;    float angle = atan2(length, dot(forward, desired_forward));    quaternion rotation = quaternion_from_axis_angle(axis, angle);    q = rotation * q;    q.normalize();}
I think that's about the best advice I can offer at this point. If you have questions about the above pseudocode though, feel free to ask.

##### Share on other sites
nardove    100
Thanks for the comments, the tentacles are a set of springs, most of the code I use a very good set of Java libraries from here http://toxiclibs.org/docs/core/ it does help me a lot to get all the crazy 3D formulas in to simple methods

So I try to translate your pseudocode to this

first initialize the variables

Quaternion quat = new Quaternion(1.0, 0.0, 0.0, 0.0);
Quaternion rotationQuat;
Matrix4x4 mat;

for the update

// INI CODE

mat = quat.getMatrix();

Vec3D forward = new Vec3D((float)mat.matrix[0][2],
(float)mat.matrix[1][2],
(float)mat.matrix[2][2]);

Vec3D desired_forward = forward.normalize();
Vec3D axis = forward.cross(desired_forward);
float leng = axis.magnitude();

if (leng > EPSILON) {
axis.scaleSelf(1.0 / leng);
float angle = atan2(leng, forward.dot(desired_forward));
rotationQuat = new Quaternion();
rotationQuat.createFromAxisAngle(axis, angle);
quat = quat.multiply(rotationQuat);
quat.normalize();
}

// perform transformation
pushMatrix();
// update location
translate(location.x, location.y, location.z);
// orientation to velocity
rotate(rotationQuat.w, rotationQuat.x, rotationQuat.y, rotationQuat.z);

agent.display();

popMatrix();

// END CODE

But I get rotationQuat = NULL

Any ideas?

Cheers
rS

##### Share on other sites
jyk    2094
Couple of things. First, you can use [ source ] tags (no spaces) when posting to get one of those nice little code boxes. Ok, on to the code. This:
Vec3D forward = new Vec3D((float)mat.matrix[0][2],(float)mat.matrix[1][2],(float)mat.matrix[2][2]);
Is probably right, but just be sure that your matrices are actually column-basis matrices (as opposed to having the basis vectors in the rows). Next, take a look at this line:
Vec3D desired_forward = forward.normalize();
And see if you can spot any difference between it and the corresponding line in the pseudocode I posted earlier.

Lastly, this:
rotate(rotationQuat.w, rotationQuat.x, rotationQuat.y, rotationQuat.z);
Looks a little suspicious. I don't know what your rotate() function does, but keep in mind that although a quaternion encodes an axis-angle rotation, under normal circumstances its elements cannot directly be interpreted as an axis-angle pair. (Typically, what you would do here is convert the quaternion to a matrix, and then upload the matrix to the graphics API.)

##### Share on other sites
nardove    100
Hello, [ source ] hey lets give it a try ;) I am new to the forum and didn't find those tags, OK so I change the line

Vec3D desired_forward = velocity.normalize();

It is way better this way, sorry for the old ones

Now when I print the rotationQuat instead of Null I get axis: [0.0,0.0,0.0], w: 1.0 constantly any ideas?

And I think you are right after I get the rotationQuat I will need to gt a rotation matrix, bu tI need to fix the logic first

Cheers
rS

##### Share on other sites
nardove    100
It looks like there is something wrong with the createFromAxisAngle method I use to build the rotationQuat, I will check it out and let you know, if it doesnt work I will have to do it manually

Cheers
rS

##### Share on other sites
jyk    2094
1. Does createFromAxisAngle() expect the angle in radians or degrees?

2. Does atan2() return the angle in radians or degrees?

3. What is the value of EPSILON?

4. What do you see if you also print out the values of 'axis' and 'angle'?

[Edit: If there's something wrong with createFromAxisAngle(), that could explain it. If you have access to the code for that function, you might post it.]

##### Share on other sites
nardove    100
Hi jyk,

EPSILON = 1.0E-4

...

length: 0.8714546
angle: 1.05816
axis: {x:1.6277998E-8, y:0.9906332, z:0.13654976}
rotationQuat: {axis: [0.0,0.0,0.0], w: 1.0}

length: 0.86895746
angle: 1.0530915
axis: {x:1.6521371E-8, y:0.99034965, z:0.13859132}
rotationQuat: {axis: [0.0,0.0,0.0], w: 1.0}

...

The library is a .jar file not sure how/if is possible to look inside

I am still testing and throwing arrows like crazy

Cheers
rS

##### Share on other sites
jyk    2094
Quote:
 length: 0.8714546angle: 1.05816axis: {x:1.6277998E-8, y:0.9906332, z:0.13654976}rotationQuat: {axis: [0.0,0.0,0.0], w: 1.0}length: 0.86895746angle: 1.0530915axis: {x:1.6521371E-8, y:0.99034965, z:0.13859132}rotationQuat: {axis: [0.0,0.0,0.0], w: 1.0}
With those axis-angle values, you shouldn't be getting an identity quaternion. What math library are you using? Can you post a link to the documentation?

(Also, as a side note, it's a bit inaccurate to label the the x, y, and z elements of a quaternion as the 'axis'. When a quaternion is used to represent a rotation, these elements are related to the axis of rotation, but they are not equivalent to the axis of rotation.)

##### Share on other sites
nardove    100
Documentation can be found here http://toxiclibs.org/docs/core/

Whate are the [ tags ] for a url?

rS

##### Share on other sites
jyk    2094
Quote:
 Original post by nardoveDocumentation can be found here http://toxiclibs.org/docs/core/Whate are the [ tags ] for a url?rS
For URLs, use the HTML 'a' element along with the 'href' attribute to create clickable links.

It looks to me like createFromAxisAngle() is a static member function that returns a new quaternion, but it doesn't look like you're doing anything with the return value. I imagine this is why your quaternion is coming out as identity.

##### Share on other sites
nardove    100
We got some motion! I change the line to

rotationQuat = rotationQuat.createFromAxisAngle(axis, angle);

Then

Matrix4x4 rotationMatrix = rotationQuat.getMatrix();

and as you can see here http://nardove.com/p5/misc/steering_3g

go nuts, I need to check the applyMatrix() values, the problem may be there

applyMatrix reference http://processing.org/reference/applyMatrix_.html

mat = quat.getMatrix();    //println(mat.matrix[0][0]);    Vec3D forward = new Vec3D((float)mat.matrix[0][2],                              (float)mat.matrix[1][2],                              (float)mat.matrix[2][2]);        Vec3D desired_forward = velocity.normalize();    Vec3D axis = forward.cross(desired_forward);    float leng = axis.magnitude();            if (leng > EPSILON) {      axis.scaleSelf(1 / leng);      float angle = atan2(leng, forward.dot(desired_forward));      rotationQuat = rotationQuat.createFromAxisAngle(axis, angle);            quat = quat.multiply(rotationQuat);      quat.normalize();            if (verbose) {        println("length:       " + leng);        println("angle:        " + angle);        println("axis:         " + axis);        println("rotationQuat: " + rotationQuat);        println("- - - - - - - - - - - - - - - - - - - - - - - -");      }    }            Matrix4x4 rotationMatrix = rotationQuat.getMatrix();    println(rotationMatrix);        //applyQuat2Matrix(rotationQuat);        pushMatrix();    // update location    translate(location.x, location.y, location.z);    // orientation to velocity    //rotate(rotationQuat.w, rotationQuat.x, rotationQuat.y, rotationQuat.z);    //rotate(res[0], res[1], res[2], res[3]);    applyMatrix((float)rotationMatrix.matrix[0][0], (float)rotationMatrix.matrix[0][1], (float)rotationMatrix.matrix[0][2], 0.0,                (float)rotationMatrix.matrix[1][0], (float)rotationMatrix.matrix[1][1], (float)rotationMatrix.matrix[1][2], 0.0,                (float)rotationMatrix.matrix[2][0], (float)rotationMatrix.matrix[2][1], (float)rotationMatrix.matrix[2][2], 0.0,                                        0.0,                         0.0,                         0.0, 1.0);         agent.display();        popMatrix();

... almost there :)

rS

## Create an account or sign in to comment

You need to be a member in order to leave a comment

## Create an account

Sign up for a new account in our community. It's easy!

Register a new account