mirror of https://github.com/ArduPilot/ardupilot
29 lines
836 B
Mathematica
29 lines
836 B
Mathematica
|
function q = fromAxisAngle(vec)
|
||
|
|
% Create a quaternion from its axis-angle representation. Just the rotation
|
||
|
|
% vector is given as input.
|
||
|
|
% Based on QuaternionT<T>::from_axis_angle(Vector3<T>)
|
||
|
|
|
||
|
|
q = zeros(4,1);
|
||
|
|
angle = single(sqrt(vec(1)^2 + vec(2)^2 + vec(3)^2));
|
||
|
|
|
||
|
|
if angle == 0
|
||
|
|
q(1) = single(1);
|
||
|
|
q(2) = single(0);
|
||
|
|
q(3) = single(0);
|
||
|
|
q(4) = single(0);
|
||
|
|
else
|
||
|
|
axis = vec ./ angle;
|
||
|
|
% The following lines are based on QuaternionT<T>::from_axis_angle(const Vector3<T> &axis, T theta)
|
||
|
|
if angle == 0
|
||
|
|
q(1) = single(1);
|
||
|
|
q(2) = single(0);
|
||
|
|
q(3) = single(0);
|
||
|
|
q(4) = single(0);
|
||
|
|
else
|
||
|
|
st2 = single(sin(0.5*angle));
|
||
|
|
q(1) = single(cos(0.5*angle));
|
||
|
|
q(2) = axis(1) * st2;
|
||
|
|
q(3) = axis(2) * st2;
|
||
|
|
q(4) = axis(3) * st2;
|
||
|
|
end
|
||
|
|
end
|