After reading through several posts and searching for info on quaternions, I thought I'd toss this in hoping it might help people who have asked questions specific to rotation and gimbal lock. I'll include the code below and a brief explanation and will answer any questions you have regarding it's use. I didn't find this subject covered very well for C-Script, so here goes.
Code:
function ql_qnormalize(&a) // &a must be 4-D vector of the form w,x,y,z quaternion
{
var f =0.0;var g =0.0;
f = sqrt(a[0]*a[0] + a[1]*a[1] + a[2]*a[2] + a[3]*a[3]);
a[0] /= f;
a[1] /= f;
a[2] /= f;
a[3] /= f;
}
function ql_vnormalize(&vec) // &vec is a 3-D vector of the form x,y,z
{
var f = 0.0;
f = sqrt( vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2]);
vec[0] /= f;
vec[1] /= f;
vec[2] /= f;
}
function ql_mul(&a,&b,&c) // [0]=w [1]=x [2]=y [3]=z
{
c[0] = (a[0] * b[0]) - (a[1] * b[1]) - (a[2] * b[2]) - (a[3] * b[3]);
c[1] = (a[0] * b[1]) + (a[1] * b[0]) + (a[2] * b[3]) - (a[3] * b[2]);
c[2] = (a[0] * b[2]) - (a[1] * b[3]) + (a[2] * b[0]) - (a[3] * b[1]);
c[3] = (a[0] * b[3]) + (a[1] * b[2]) - (a[2] * b[1]) + (a[3] * b[0]);
}
function ql_add(&a,&b)
{
a[0] = a[0] + b[0];
a[1] = a[1] + b[1];
a[2] = a[2] + b[2];
a[3] = a[3] + b[3];
}
function ql_sub(&a,&b) // returns in a-b = a
{
a[0] = a[0] - b[0];
a[1] = a[1] - b[1];
a[2] = a[2] - b[2];
a[3] = a[3] - b[3];
}
function ql_conjugate(&a,&b) // conjugate is the same as the unit inverse
{
b[0] = a[0];
b[1] = -a[1];
b[2] = -a[2];
b[3] = -a[3];
}
function ql_scale(&q,b)
{
q[0] = q[0] * b;
q[1] = q[1] * b;
q[2] = q[2] * b;
q[3] = q[3] * b;
}
function ql_AAvecToQ(&vec,theta,&q) // axis/angle to q. a=vector b=angle c=quaternion
{
var s;
s = sin(theta / 2);
ql_vnormalize(vec);
q[0] = cos(theta / 2); // w
q[1] = s * vec[0]; // x
q[2] = s * vec[1]; // y
q[3] = s * vec[2]; // z
ql_qnormalize(q);
}
function ql_qToVec(&q,&vec,&theta) // vec[0]=x vec[1]=y vec[2]=z theta=angle
{
var si;
theta = acos(q[0]);
si = 1.0 / sin(theta);
vec[0] = q[1] * si; // x = q.x * si
vec[1] = q[2] * si; // y = q.y * si
vec[2] = q[3] * si; // z = q.z * si
}
function ql_eulerToQ(&q,&angles) // z=phi y=theta x=psi (yaw, pitch, roll)
{
var cx;var cy;var cz;
var sx;var sy;var sz;
cz = 0.5 * cos(angles[0]);
cy = 0.5 * cos(angles[1]);
cx = 0.5 * cos(angles[2]);
sz = 0.5 * sin(angles[0]);
sy = 0.5 * sin(angles[1]);
sx = 0.5 * sin(angles[2]);
q[0] = (cz * cy * cx) + (sz * sy * sx);
q[1] = (cz * cy * sx) - (sz * sy * cx);
q[2] = (cz * sy * cx) + (sz * cy * sx);
q[3] = (sz * cy * cx) - (cz * sy * sx);
}
function ql_qToMat(&q,&m) // q[0]=w q[1]=x q[2]=y q[3]=z
{
var w;var x;var y;var z;
w = q[0]; x = q[1]; y = q[2]; z = q[3];
m[0] = 1 - (2 * y * y) - (2 * z * z);
m[1] = (2 * x * y) - (2 * w * z);
m[2] = (2 * x * z) + (2 * w * y);
m[3] = (2 * x * y) + (2 * w * z);
m[4] = 1 - (2 * x * x) - (2 * z * z);
m[5] = (2 * y * z) - (2 * w * x);
m[6] = (2 * x * z) - (2 * w * y);
m[7] = (2 * y * z) + (2 * w * x);
m[8] = 1 - (2 * x * x) - (2 * y * y);
}
function ql_MatToVec(&m,&vec) // vec[0]=x vec[1]=y vec[2]=z
{
var tx;var ty;var tz;
var vx;var vy;var vz;
vx = vec[0];vy = vec[1];vz = vec[2];
tx = (vx * m[0]) + (vy * m[1]) + (vz * m[2]);
ty = (vx * m[3]) + (vy * m[4]) + (vz * m[5]);
tz = (vx * m[6]) + (vy * m[7]) + (vz * m[8]);
vec[0] = tx;
vec[1] = ty;
vec[2] = tz;
}
function ql_QMV(&q,&m,&vec) // this is the composite function of the 2 functions above.
{ // it performs a quaternion to rotation matrix
var iw;var ix;var iy;var iz; // then
var tx;var ty;var tz; // it performs a 1x3 * 3x3 matrix multiply which is
var vx;var vy;var vz; // &vec * &m
iw = q[0]; ix = q[1]; iy = q[2]; iz = q[3]; //
vx = vec[0];vy = vec[1];vz = vec[2]; // &vec contains the rotated vector.
m[0] = 1 - (2 * iy * iy) - (2 * iz * iz); //
m[1] = (2 * ix * iy) - (2 * iw * iz); // ...translate and scale this vector and that's it
m[2] = (2 * ix * iz) + (2 * iw * iy); //
m[3] = (2 * ix * iy) + (2 * iw * iz); //
m[4] = 1 - (2 * ix * ix) - (2 * iz * iz); //
m[5] = (2 * iy * iz) - (2 * iw * ix); //
m[6] = (2 * ix * iz) - (2 * iw * iy); //
m[7] = (2 * iy * iz) + (2 * iw * ix); //
m[8] = 1 - (2 * ix * ix) - (2 * iy * iy); //
tx = (vx * m[0]) + (vy * m[1]) + (vz * m[2]); //
ty = (vx * m[3]) + (vy * m[4]) + (vz * m[5]); //
tz = (vx * m[6]) + (vy * m[7]) + (vz * m[8]); //
vec[0] = tx;
vec[1] = ty;
vec[2] = tz;
}
First off, you need to create a vector. This vector represents the quaternion and points (x,y,z) in space can be rotated around this quaternion vector. The benefit of quaternion rotation is that it doesn't have to line up with any of the 3 major axis (the +X/-X, +Y/-Y, +Z/-Z), in fact that's a big part of the power of using quaternions. In normal Euler angle rotations, you are always rotating about a major axis. Gimbal lock is a problem of Euler rotations, since quaternions have no such problem they are great for rotating things in the world. Remember though, these functions are not replacing the the built-in model handling rotations, those are part of the engine and how Conitec handles it internally. What these functions are for is primarily rotating positions about an arbitrary axis, the quaternion. I'll try to give a short example.
Create a vector, assume it's origin is at position 0,0,0.
Code:
qvec[0] = cos(my.skill80) * cos(my.skill81);
qvec[1] = sin(my.skill80) * cos(my.skill81);
qvec[2] = sin(my.skill81);
In this example I'm using skill80 as the PAN (or azimuth) angle and skill81 as the TILT (or elevation) angle. These calculations are standard spherical rotations, however I should note that the standard calculation uses the elevation(tilt) angle starting from the vertical (or +Z axis), this calculation is generally modified (which has been done) so the elevation angle starts from the horizontal plane (the X/Y plane).
qvec[] is just an array var used to hold the result.
We call ql_AAvecToQ sending in this qvec[] array, a rotation value (I'll explain in a moment) and another array var to hold the quaternion we create.
i.e. qlAAvecToQ(qvec, 0, quat1);
The definition of quat1 is simply a 4 element array var.
var quat1[4];
The 0 (zero) as a parameter to the function represents the angle of rotation around the quaternion. This is where quaternion explanations sometimes get people confused. Think of a vector that points in the +Z direction, so it's pointing straight up. Now think of the pan angle, which would be the angle of rotation around the +Z axis. What would happen if took the vector that represents the +Z axis and added a 4th element to that vector array to include the pan angle. Well, a vector that is parallel to the +Z axis would be 0,0,1 (normalized) and say we have a pan angle of 30 degrees. Let's put the pan angle as the first element of the array and move the direction vector into the other positions.
In essence you'd have a 4 element vector that looked like this:
my_vector[4] = 30,0,0,1; // pan angle, (X, Y, Z) vector components
A quaternion is a 4-element vector. It's components are labeled W,X,Y,Z. Where W represents the rotation around the vector given by the X,Y,Z. Our my_vector[4] example could be turned into a real quaternion by sending the vector portion of it to the appropriate function and sending the pan angle as well in the proper parameter position.
var my_newvec[3];
my_newvec[0] = my_vector[1];
my_newvec[1] = my_vector[2];
my_newvec[2] = my_vector[3];
.
ql_AAvecToQ(my_newvec, my_vector[0], quat1);
.
The elements of a quaternion are not simply the angle followed by the simple vector components. The way a quaternion is created is beyond the scope of this post, but there are numerous web sites with tons of info on the subject. That's why the example my_vector[4] can't be used as-is for calculations.
.
Once ql_AAvecToQ() has been called, we have a new quaternion stored in the quat1[4] variable. You may ask, "What now?" A good question. Let me answer it.
.
A quaternion by itself has no use. It's a vector pointing in space and has an angle of rotation value.
.
Remember above how we can think of rotating around the +Z axis using the PAN angle? Well, what if we could rotate the +Z axis in space? That's what the quaternion represents. A vector pointing to any arbitrary point in space, and we can rotate points around that axis. That is the ultimate power of the quaternion, arbitrary rotation around the quaternion's axis.
.
How to do that?
.
You multiply the quaternion and a position. The new position will be the original position rotated around the quaternion axis by the angular amount stored in the quaternion. Cool eh?
.
Let's create a point in space by simply creating a 3-element array (a vector).
.
var my_point[3] = 5.2, 6.1, 1.2;
.
So we have our point, which is actually a vector, hanging there in space. We want to rotate our point around the quaternion axis. There are 2 ways I've given in the functions to do this. First, you can call 2 separate functions or call just 1 function. I merged the 2 functions into one called ql_QMV(). QMV stands for Quaternion-(times)-Matrix-(times)-Vector. QMV takes as input; a quaternion, a 9-element array which is used for the matrix, and a 3-element vector --- this is the vector(point) to be rotated and is used as output to hold the result. Remember the result will be stored in the input vector.
.
var my_matrix[9]; // used by the quaternion functions
.
ql_QMV(qvec, my_matrix, my_point);
.
my_point will be rotated around the qvec axis by the angle stored in qvec. That's the short and simple of performing a rotation using a quaternion. In my next post I'll try to elaborate a little on how better to use this in practice.
.
Quantum Mechanic
Better living at the subatomic level.
