-
Notifications
You must be signed in to change notification settings - Fork 164
/
from_Q.mo
40 lines (36 loc) · 1.56 KB
/
from_Q.mo
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
within Modelica.Mechanics.MultiBody.Frames;
function from_Q
"Return orientation object R from quaternion orientation object Q"
extends Modelica.Icons.Function;
input Quaternions.Orientation Q
"Quaternions orientation object to rotate frame 1 into frame 2";
input SI.AngularVelocity w[3]
"Angular velocity from frame 2 with respect to frame 1, resolved in frame 2";
output Orientation R "Orientation object to rotate frame 1 into frame 2";
algorithm
/*
T := (2*Q[4]*Q[4] - 1)*identity(3) + 2*(outerProduct([Q[1:3],[Q[1:3]) - Q[4]*
skew(Q[1:3]));
*/
R := Orientation([2*(Q[1]*Q[1] + Q[4]*Q[4]) - 1, 2*(Q[1]*Q[2] + Q[3]*Q[4]),
2*(Q[1]*Q[3] - Q[2]*Q[4]); 2*(Q[2]*Q[1] - Q[3]*Q[4]), 2*(Q[2]*Q[2] + Q[4]
*Q[4]) - 1, 2*(Q[2]*Q[3] + Q[1]*Q[4]); 2*(Q[3]*Q[1] + Q[2]*Q[4]), 2*(Q[3]
*Q[2] - Q[1]*Q[4]), 2*(Q[3]*Q[3] + Q[4]*Q[4]) - 1],w= w);
annotation(Inline=true, Documentation(info="<html>
<h4>Syntax</h4>
<blockquote><pre>
R = Frames.<strong>from_Q</strong>(Q, w);
</pre></blockquote>
<h4>Description</h4>
<p>
This function returns an <a href=\"modelica://Modelica.Mechanics.MultiBody.Frames.Orientation\">orientation object</a> R
computed from a <a href=\"modelica://Modelica.Mechanics.MultiBody.Frames.Quaternions.Orientation\">quaternion object</a> Q
and an angular velocity vector w.
</p>
<h4>See also</h4>
<p>
<a href=\"modelica://Modelica.Mechanics.MultiBody.Frames.to_Q\">to_Q</a>.
<a href=\"modelica://Modelica.Mechanics.MultiBody.Frames.TransformationMatrices.from_Q\">TransformationMatrices.from_Q</a>.
</p>
</html>"));
end from_Q;