Rotations/Quaternionsfunktioen für Octave/MATLAB

rotationen.m

% DISCLAIMER: Nur in Octave getestet. Könnte auch in MATLAB funktionieren, muss aber nicht...

% Achse-Winkeldarstellung zu Rotationsmatrix.
function rv = achsewinkel_to_rotmatrix(achse, winkel)
	x = achse(1);
	y = achse(2);
	z = achse(3);
	c = cos(winkel);
	s = sin(winkel);
	omc = 1-c;
	rv = [ ...
		(x^2)*omc+c,  x*y*omc-(z*s),  x*z*omc+(y*s); ...
		y*x*omc+z*s,  (y^2)*omc+c,    y*z*omc-(x*s); ...
		z*x*omc-(y*s), z*y*omc+(x*s), (z^2)*omc+c];
end

% Grad nach Radianten
function rv = deg(d)
	rv = (d/180) * pi;
end

% Quaternionen Multiplizieren
function rv = quat_mult(x, y)
	r1 = x(1)*y(1) - x(2)*y(2) - x(3)*y(3) - x(4)*y(4);
	r2 = x(1)*y(2) + x(2)*y(1) + x(3)*y(4) - x(4)*y(3);
	r3 = x(1)*y(3) - x(2)*y(4) + x(3)*y(1) + x(4)*y(2);
	r4 = x(1)*y(4) + x(2)*y(3) - x(3)*y(2) + x(4)*y(1);
	
	rv = [r1;r2;r3;r4];
end

% Konjugiertes Quaternion bilden
function rv = quat_conj(x)
	rv = [x(1), -x(2), -x(3), -x(4)];
end

% Quaternion in Rotationsmatrix umrechnen
function rv = quat_to_rotmat(x)
	a = x(1);
	b = x(2);
	c = x(3);
	d = x(4);
	
	rv = [1-2*(c^2+d^2), 2*(b*c-a*d),  2*(b*d+a*c); ...
	      2*(b*c+a*d),   1-2*(d^2+b^2), 2*(c*d-a*b); ...
	      2*(b*d-a*c),   2*(c*d+a*b),  1-2*(b^2+c^2)];
end

% Ein Quaternion invertieren
function rv = quat_inverse(x)
	a = x(1);
	b = x(2);
	c = x(3);
	d = x(4);
	
	A = [a, -b, -c, -d; ...
	     b,  a, -d,  c; ...
	     c,  d,  a, -b; ...
	     d, -c,  b,  a];
	b = [1;0;0;0];
	rv = A \ b;
end