Yaw, Pitch and Roll with D
By Danny Arends in D programmingPosted at: 04 Sep 2012, 21:31, last edited: 04 Sep 2012, 21:31
To continue the previous post, and expand a bit on the advantages of compile time function execution (CTFE). I'll show some things I like about the D 2.0 language. Lets start by creating all possible 360 yaw, pitch and roll matrices that are used to rotate objects in e.g. 3D projections or simulations.
Please note this is not suitable for real applications due to the grimbal lock condition (see wiki). (this was pointed out on reddit by willvarfar In real application use quaternions !!
We'll create a new structure, that will hold a float or double matrix. In D we can add an opBinary function to a structure to allow to write expression that are closer to the original mathematical notation. This will allow us to write things like:
auto X = A * B + C;
where A, B and C are variables holding matrix structures. More on operator overloading can be found here. In D 2.0 we can define a structure like this as:
struct mat(T = float){
T[][] data;
pure mat!T opBinary(string op)(mat!T rhs){
static if (op == '*' || op == '+' || op == '-'){
return mat!T(mmOp!(T, op)(data, rhs.data));
}else static assert(0, 'Operator '~op~' not implemented');
}
pure bool opEquals(ref const mat!T rhs){
equals_t tmp = true;
foreach(i; 0..data.length){
foreach(j; 0..data[i].length){
if(!tmp) return tmp;
tmp = tmp && (data[i][j] == rhs.data[i][j]);
}
}
return tmp;
}
}
The opEquals function compares 2 matrix structures for equality. This can be done more efficient (e.g. check if array sizes match beforehand). the opBinary calls the mmOp functions which applies the operator across the array.
pure T[][] mmOp(T = float, string op)(in T[][] A, in T[][] B)
in{
assert(isFloatingPoint!T, 'T must be a FloatingPoint type');
assert(A.length > 0,'A has no length');
assert(B.length > 0,'B has no length');
}
body{
T[][] d;
T tmp;
foreach(i; 0 .. A.length){
T[] row;
foreach(j; 0 .. B[i].length){
tmp = 0.0;
foreach(k; 0 .. A[i].length){
tmp += mixin('A[i][k] '~op~' B[k][j]');
}
row ~= tmp;
}
d ~= row;
}
return d;
}
It uses a mixin, so with only one function we can do: A[i][k] * B[k][j], A[i][k] + B[k][j] or A[i][k] - B[k][j] on either a float or double matrix. This is why I like D. We created a very 'readable' 'generic' function. read more on mixins here.
There exists many ways of doing this, faster, safer, shorter, less memory consuming. But lets get back on track. We now have a way to define matrices, so lets generate our roll, pitch and yaw matrices. it uses the CTFE sine and cosine functions from the previous blog post.
The gen_rotationmatrices function will reserve room for 360 * 3 matrices, and then fill them with the appropriate matrices.
pure mat!(T)[3][] gen_rotationmatrices(T = float)(){
mat!(T)[3][] result = new mat!(T)[3][](360);
T si,ci; //shorter names
foreach(i; 0 .. 360){
si = sin(i);
ci = cos(i);
result[i] = [
mat!(T)([[ 1.0, 0.0, 0.0], [ 0.0, ci, -si], [ 0.0, si, ci]]),
mat!(T)([[ ci, 0.0, si], [ 0.0, 1.0, 0.0], [ -si, 0.0, ci]]),
mat!(T)([[ ci, -si, 0.0], [ si, ci, 0.0], [ 0.0, 0.0, 1.0]])];
}
return result;
}
And because D makes executing a function at compile time a breeze. We run this function at compile time just by assigning the output to an immutable or enum.
immutable rmatrix = gen_rotationmatrices!double();
I love helper functions so I defined 4 of them they will return yaw, pitch and roll matrices. And finally the last helper function uses the opBinary * to construct a rotation matrix by multiplying the yaw pitch and roll matrices after look up:
enum{YAW = 0, PITCH = 1, ROLL = 2};
pure auto yaw(int deg){
deg = degreeloop(deg);
return cast(matrix)rmatrix[deg][YAW];
}
pure auto pitch(int deg){
deg = degreeloop(deg);
return cast(matrix)rmatrix[deg][PITCH];
}
pure auto roll(int deg){
deg = degreeloop(deg);
return cast(matrix)rmatrix[deg][ROLL];
}
pure auto rotationmatrix(int y,int p,int r,){
return yaw(y) * pitch(p) * roll(r);
}
Well thats about it for now I recon.
Find the whole code at: https://gist.github.com/3560459
Last modified: 04 Sep 2012, 21:31 | Edit