29 lines
869 B
Mathematica
29 lines
869 B
Mathematica
|
%% Helper Functions
|
||
|
|
|
||
|
|
function R = generateRotationMatrix(axis, theta)
|
||
|
|
% rotation_matrix returns the 3x3 rotation matrix associated with
|
||
|
|
% counterclockwise rotation about the given axis by theta radians.
|
||
|
|
|
||
|
|
% Normalize the axis
|
||
|
|
axis = axis / sqrt(dot(axis, axis));
|
||
|
|
|
||
|
|
% Compute the rotation matrix components
|
||
|
|
a = cos(theta / 2.0);
|
||
|
|
b = -axis(1) * sin(theta / 2.0);
|
||
|
|
c = -axis(2) * sin(theta / 2.0);
|
||
|
|
d = -axis(3) * sin(theta / 2.0);
|
||
|
|
|
||
|
|
% Precompute terms for readability
|
||
|
|
aa = a * a; bb = b * b; cc = c * c; dd = d * d;
|
||
|
|
bc = b * c; ad = a * d; ac = a * c; ab = a * b;
|
||
|
|
bd = b * d; cd = c * d;
|
||
|
|
|
||
|
|
% Construct the rotation matrix
|
||
|
|
R = [aa + bb - cc - dd, 2 * (bc + ad), 2 * (bd - ac);
|
||
|
|
2 * (bc - ad), aa + cc - bb - dd, 2 * (cd + ab);
|
||
|
|
2 * (bd + ac), 2 * (cd - ab), aa + dd - bb - cc];
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
|