CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/Tools/simulink/arducopter/functions/fromAxisAngle.m
Views: 1799
1
function q = fromAxisAngle(vec)
2
% Create a quaternion from its axis-angle representation. Just the rotation
3
% vector is given as input.
4
% Based on QuaternionT<T>::from_axis_angle(Vector3<T>)
5
6
q = zeros(4,1);
7
angle = single(sqrt(vec(1)^2 + vec(2)^2 + vec(3)^2));
8
9
if angle == 0
10
q(1) = single(1);
11
q(2) = single(0);
12
q(3) = single(0);
13
q(4) = single(0);
14
else
15
axis = vec ./ angle;
16
% The following lines are based on QuaternionT<T>::from_axis_angle(const Vector3<T> &axis, T theta)
17
if angle == 0
18
q(1) = single(1);
19
q(2) = single(0);
20
q(3) = single(0);
21
q(4) = single(0);
22
else
23
st2 = single(sin(0.5*angle));
24
q(1) = single(cos(0.5*angle));
25
q(2) = axis(1) * st2;
26
q(3) = axis(2) * st2;
27
q(4) = axis(3) * st2;
28
end
29
end
30