<!DOCTYPE html>
<html>
<head>
<meta http-equiv="Content-Type" content="text/html; charset=utf-8">
<meta http-equiv="X-UA-Compatible" content="IE=emulateIE7" />
<title>Coverage for navipy/maths/quaternion.py: 77%</title>
<link rel="stylesheet" href="style.css" type="text/css">
<script type="text/javascript" src="jquery.min.js"></script>
<script type="text/javascript" src="jquery.hotkeys.js"></script>
<script type="text/javascript" src="jquery.isonscreen.js"></script>
<script type="text/javascript" src="coverage_html.js"></script>
<script type="text/javascript">
jQuery(document).ready(coverage.pyfile_ready);
</script>
</head>
<body class="pyfile">
<div id="header">
<div class="content">
<h1>Coverage for <b>navipy/maths/quaternion.py</b> :
<span class="pc_cov">77%</span>
</h1>
<img id="keyboard_icon" src="keybd_closed.png" alt="Show keyboard shortcuts" />
<h2 class="stats">
70 statements
<span class="run hide_run shortkey_r button_toggle_run">54 run</span>
<span class="mis shortkey_m button_toggle_mis">16 missing</span>
<span class="exc shortkey_x button_toggle_exc">0 excluded</span>
</h2>
</div>
</div>
<div class="help_panel">
<img id="panel_icon" src="keybd_open.png" alt="Hide keyboard shortcuts" />
<p class="legend">Hot-keys on this page</p>
<div>
<p class="keyhelp">
<span class="key">r</span>
<span class="key">m</span>
<span class="key">x</span>
<span class="key">p</span> toggle line displays
</p>
<p class="keyhelp">
<span class="key">j</span>
<span class="key">k</span> next/prev highlighted chunk
</p>
<p class="keyhelp">
<span class="key">0</span> (zero) top of page
</p>
<p class="keyhelp">
<span class="key">1</span> (one) first highlighted chunk
</p>
</div>
</div>
<div id="source">
<table>
<tr>
<td class="linenos">
<p id="n1" class="pln"><a href="#n1">1</a></p>
<p id="n2" class="pln"><a href="#n2">2</a></p>
<p id="n3" class="pln"><a href="#n3">3</a></p>
<p id="n4" class="pln"><a href="#n4">4</a></p>
<p id="n5" class="stm run hide_run"><a href="#n5">5</a></p>
<p id="n6" class="stm run hide_run"><a href="#n6">6</a></p>
<p id="n7" class="stm run hide_run"><a href="#n7">7</a></p>
<p id="n8" class="pln"><a href="#n8">8</a></p>
<p id="n9" class="pln"><a href="#n9">9</a></p>
<p id="n10" class="stm run hide_run"><a href="#n10">10</a></p>
<p id="n11" class="pln"><a href="#n11">11</a></p>
<p id="n12" class="pln"><a href="#n12">12</a></p>
<p id="n13" class="pln"><a href="#n13">13</a></p>
<p id="n14" class="pln"><a href="#n14">14</a></p>
<p id="n15" class="pln"><a href="#n15">15</a></p>
<p id="n16" class="pln"><a href="#n16">16</a></p>
<p id="n17" class="pln"><a href="#n17">17</a></p>
<p id="n18" class="pln"><a href="#n18">18</a></p>
<p id="n19" class="pln"><a href="#n19">19</a></p>
<p id="n20" class="pln"><a href="#n20">20</a></p>
<p id="n21" class="pln"><a href="#n21">21</a></p>
<p id="n22" class="pln"><a href="#n22">22</a></p>
<p id="n23" class="stm run hide_run"><a href="#n23">23</a></p>
<p id="n24" class="stm run hide_run"><a href="#n24">24</a></p>
<p id="n25" class="stm run hide_run"><a href="#n25">25</a></p>
<p id="n26" class="stm run hide_run"><a href="#n26">26</a></p>
<p id="n27" class="pln"><a href="#n27">27</a></p>
<p id="n28" class="pln"><a href="#n28">28</a></p>
<p id="n29" class="stm run hide_run"><a href="#n29">29</a></p>
<p id="n30" class="pln"><a href="#n30">30</a></p>
<p id="n31" class="pln"><a href="#n31">31</a></p>
<p id="n32" class="pln"><a href="#n32">32</a></p>
<p id="n33" class="pln"><a href="#n33">33</a></p>
<p id="n34" class="pln"><a href="#n34">34</a></p>
<p id="n35" class="pln"><a href="#n35">35</a></p>
<p id="n36" class="pln"><a href="#n36">36</a></p>
<p id="n37" class="pln"><a href="#n37">37</a></p>
<p id="n38" class="pln"><a href="#n38">38</a></p>
<p id="n39" class="pln"><a href="#n39">39</a></p>
<p id="n40" class="pln"><a href="#n40">40</a></p>
<p id="n41" class="pln"><a href="#n41">41</a></p>
<p id="n42" class="pln"><a href="#n42">42</a></p>
<p id="n43" class="pln"><a href="#n43">43</a></p>
<p id="n44" class="pln"><a href="#n44">44</a></p>
<p id="n45" class="stm run hide_run"><a href="#n45">45</a></p>
<p id="n46" class="stm run hide_run"><a href="#n46">46</a></p>
<p id="n47" class="stm run hide_run"><a href="#n47">47</a></p>
<p id="n48" class="stm run hide_run"><a href="#n48">48</a></p>
<p id="n49" class="stm run hide_run"><a href="#n49">49</a></p>
<p id="n50" class="stm run hide_run"><a href="#n50">50</a></p>
<p id="n51" class="stm run hide_run"><a href="#n51">51</a></p>
<p id="n52" class="stm run hide_run"><a href="#n52">52</a></p>
<p id="n53" class="pln"><a href="#n53">53</a></p>
<p id="n54" class="pln"><a href="#n54">54</a></p>
<p id="n55" class="stm run hide_run"><a href="#n55">55</a></p>
<p id="n56" class="pln"><a href="#n56">56</a></p>
<p id="n57" class="pln"><a href="#n57">57</a></p>
<p id="n58" class="pln"><a href="#n58">58</a></p>
<p id="n59" class="pln"><a href="#n59">59</a></p>
<p id="n60" class="pln"><a href="#n60">60</a></p>
<p id="n61" class="pln"><a href="#n61">61</a></p>
<p id="n62" class="pln"><a href="#n62">62</a></p>
<p id="n63" class="stm run hide_run"><a href="#n63">63</a></p>
<p id="n64" class="stm run hide_run"><a href="#n64">64</a></p>
<p id="n65" class="stm run hide_run"><a href="#n65">65</a></p>
<p id="n66" class="stm run hide_run"><a href="#n66">66</a></p>
<p id="n67" class="stm run hide_run"><a href="#n67">67</a></p>
<p id="n68" class="stm run hide_run"><a href="#n68">68</a></p>
<p id="n69" class="pln"><a href="#n69">69</a></p>
<p id="n70" class="pln"><a href="#n70">70</a></p>
<p id="n71" class="stm run hide_run"><a href="#n71">71</a></p>
<p id="n72" class="pln"><a href="#n72">72</a></p>
<p id="n73" class="pln"><a href="#n73">73</a></p>
<p id="n74" class="pln"><a href="#n74">74</a></p>
<p id="n75" class="pln"><a href="#n75">75</a></p>
<p id="n76" class="pln"><a href="#n76">76</a></p>
<p id="n77" class="pln"><a href="#n77">77</a></p>
<p id="n78" class="pln"><a href="#n78">78</a></p>
<p id="n79" class="pln"><a href="#n79">79</a></p>
<p id="n80" class="pln"><a href="#n80">80</a></p>
<p id="n81" class="stm run hide_run"><a href="#n81">81</a></p>
<p id="n82" class="stm run hide_run"><a href="#n82">82</a></p>
<p id="n83" class="stm run hide_run"><a href="#n83">83</a></p>
<p id="n84" class="stm run hide_run"><a href="#n84">84</a></p>
<p id="n85" class="stm run hide_run"><a href="#n85">85</a></p>
<p id="n86" class="stm run hide_run"><a href="#n86">86</a></p>
<p id="n87" class="stm run hide_run"><a href="#n87">87</a></p>
<p id="n88" class="stm run hide_run"><a href="#n88">88</a></p>
<p id="n89" class="pln"><a href="#n89">89</a></p>
<p id="n90" class="stm run hide_run"><a href="#n90">90</a></p>
<p id="n91" class="pln"><a href="#n91">91</a></p>
<p id="n92" class="stm run hide_run"><a href="#n92">92</a></p>
<p id="n93" class="pln"><a href="#n93">93</a></p>
<p id="n94" class="pln"><a href="#n94">94</a></p>
<p id="n95" class="pln"><a href="#n95">95</a></p>
<p id="n96" class="pln"><a href="#n96">96</a></p>
<p id="n97" class="pln"><a href="#n97">97</a></p>
<p id="n98" class="stm run hide_run"><a href="#n98">98</a></p>
<p id="n99" class="pln"><a href="#n99">99</a></p>
<p id="n100" class="pln"><a href="#n100">100</a></p>
<p id="n101" class="stm run hide_run"><a href="#n101">101</a></p>
<p id="n102" class="pln"><a href="#n102">102</a></p>
<p id="n103" class="pln"><a href="#n103">103</a></p>
<p id="n104" class="pln"><a href="#n104">104</a></p>
<p id="n105" class="pln"><a href="#n105">105</a></p>
<p id="n106" class="pln"><a href="#n106">106</a></p>
<p id="n107" class="pln"><a href="#n107">107</a></p>
<p id="n108" class="pln"><a href="#n108">108</a></p>
<p id="n109" class="pln"><a href="#n109">109</a></p>
<p id="n110" class="pln"><a href="#n110">110</a></p>
<p id="n111" class="pln"><a href="#n111">111</a></p>
<p id="n112" class="stm run hide_run"><a href="#n112">112</a></p>
<p id="n113" class="pln"><a href="#n113">113</a></p>
<p id="n114" class="stm run hide_run"><a href="#n114">114</a></p>
<p id="n115" class="pln"><a href="#n115">115</a></p>
<p id="n116" class="stm run hide_run"><a href="#n116">116</a></p>
<p id="n117" class="pln"><a href="#n117">117</a></p>
<p id="n118" class="pln"><a href="#n118">118</a></p>
<p id="n119" class="pln"><a href="#n119">119</a></p>
<p id="n120" class="pln"><a href="#n120">120</a></p>
<p id="n121" class="pln"><a href="#n121">121</a></p>
<p id="n122" class="pln"><a href="#n122">122</a></p>
<p id="n123" class="pln"><a href="#n123">123</a></p>
<p id="n124" class="stm run hide_run"><a href="#n124">124</a></p>
<p id="n125" class="pln"><a href="#n125">125</a></p>
<p id="n126" class="stm run hide_run"><a href="#n126">126</a></p>
<p id="n127" class="pln"><a href="#n127">127</a></p>
<p id="n128" class="pln"><a href="#n128">128</a></p>
<p id="n129" class="pln"><a href="#n129">129</a></p>
<p id="n130" class="pln"><a href="#n130">130</a></p>
<p id="n131" class="pln"><a href="#n131">131</a></p>
<p id="n132" class="pln"><a href="#n132">132</a></p>
<p id="n133" class="stm mis"><a href="#n133">133</a></p>
<p id="n134" class="pln"><a href="#n134">134</a></p>
<p id="n135" class="stm mis"><a href="#n135">135</a></p>
<p id="n136" class="pln"><a href="#n136">136</a></p>
<p id="n137" class="pln"><a href="#n137">137</a></p>
<p id="n138" class="pln"><a href="#n138">138</a></p>
<p id="n139" class="pln"><a href="#n139">139</a></p>
<p id="n140" class="pln"><a href="#n140">140</a></p>
<p id="n141" class="pln"><a href="#n141">141</a></p>
<p id="n142" class="stm mis"><a href="#n142">142</a></p>
<p id="n143" class="pln"><a href="#n143">143</a></p>
<p id="n144" class="stm mis"><a href="#n144">144</a></p>
<p id="n145" class="pln"><a href="#n145">145</a></p>
<p id="n146" class="pln"><a href="#n146">146</a></p>
<p id="n147" class="pln"><a href="#n147">147</a></p>
<p id="n148" class="pln"><a href="#n148">148</a></p>
<p id="n149" class="pln"><a href="#n149">149</a></p>
<p id="n150" class="pln"><a href="#n150">150</a></p>
<p id="n151" class="pln"><a href="#n151">151</a></p>
<p id="n152" class="pln"><a href="#n152">152</a></p>
<p id="n153" class="stm run hide_run"><a href="#n153">153</a></p>
<p id="n154" class="pln"><a href="#n154">154</a></p>
<p id="n155" class="pln"><a href="#n155">155</a></p>
<p id="n156" class="stm run hide_run"><a href="#n156">156</a></p>
<p id="n157" class="stm run hide_run"><a href="#n157">157</a></p>
<p id="n158" class="stm run hide_run"><a href="#n158">158</a></p>
<p id="n159" class="pln"><a href="#n159">159</a></p>
<p id="n160" class="pln"><a href="#n160">160</a></p>
<p id="n161" class="pln"><a href="#n161">161</a></p>
<p id="n162" class="pln"><a href="#n162">162</a></p>
<p id="n163" class="pln"><a href="#n163">163</a></p>
<p id="n164" class="stm run hide_run"><a href="#n164">164</a></p>
<p id="n165" class="pln"><a href="#n165">165</a></p>
<p id="n166" class="pln"><a href="#n166">166</a></p>
<p id="n167" class="stm mis"><a href="#n167">167</a></p>
<p id="n168" class="stm mis"><a href="#n168">168</a></p>
<p id="n169" class="stm mis"><a href="#n169">169</a></p>
<p id="n170" class="pln"><a href="#n170">170</a></p>
<p id="n171" class="pln"><a href="#n171">171</a></p>
<p id="n172" class="stm run hide_run"><a href="#n172">172</a></p>
<p id="n173" class="pln"><a href="#n173">173</a></p>
<p id="n174" class="pln"><a href="#n174">174</a></p>
<p id="n175" class="stm run hide_run"><a href="#n175">175</a></p>
<p id="n176" class="stm run hide_run"><a href="#n176">176</a></p>
<p id="n177" class="stm run hide_run"><a href="#n177">177</a></p>
<p id="n178" class="pln"><a href="#n178">178</a></p>
<p id="n179" class="pln"><a href="#n179">179</a></p>
<p id="n180" class="stm run hide_run"><a href="#n180">180</a></p>
<p id="n181" class="pln"><a href="#n181">181</a></p>
<p id="n182" class="pln"><a href="#n182">182</a></p>
<p id="n183" class="stm mis"><a href="#n183">183</a></p>
<p id="n184" class="pln"><a href="#n184">184</a></p>
<p id="n185" class="pln"><a href="#n185">185</a></p>
<p id="n186" class="stm run hide_run"><a href="#n186">186</a></p>
<p id="n187" class="pln"><a href="#n187">187</a></p>
<p id="n188" class="pln"><a href="#n188">188</a></p>
<p id="n189" class="stm mis"><a href="#n189">189</a></p>
<p id="n190" class="pln"><a href="#n190">190</a></p>
<p id="n191" class="pln"><a href="#n191">191</a></p>
<p id="n192" class="stm run hide_run"><a href="#n192">192</a></p>
<p id="n193" class="pln"><a href="#n193">193</a></p>
<p id="n194" class="pln"><a href="#n194">194</a></p>
<p id="n195" class="stm mis"><a href="#n195">195</a></p>
<p id="n196" class="stm mis"><a href="#n196">196</a></p>
<p id="n197" class="stm mis"><a href="#n197">197</a></p>
<p id="n198" class="stm mis"><a href="#n198">198</a></p>
<p id="n199" class="stm mis"><a href="#n199">199</a></p>
<p id="n200" class="pln"><a href="#n200">200</a></p>
<p id="n201" class="stm mis"><a href="#n201">201</a></p>
<p id="n202" class="stm mis"><a href="#n202">202</a></p>
</td>
<td class="text">
<p id="t1" class="pln"><span class="str">"""</span><span class="strut"> </span></p>
<p id="t2" class="pln"><span class="strut"> </span></p>
<p id="t3" class="pln"><span class="strut"> </span></p>
<p id="t4" class="pln"><span class="str">"""</span><span class="strut"> </span></p>
<p id="t5" class="stm run hide_run"><span class="key">import</span> <span class="nam">numpy</span> <span class="key">as</span> <span class="nam">np</span><span class="strut"> </span></p>
<p id="t6" class="stm run hide_run"><span class="key">from</span> <span class="nam">navipy</span><span class="op">.</span><span class="nam">maths</span> <span class="key">import</span> <span class="nam">constants</span><span class="strut"> </span></p>
<p id="t7" class="stm run hide_run"><span class="key">from</span> <span class="nam">navipy</span><span class="op">.</span><span class="nam">maths</span><span class="op">.</span><span class="nam">tools</span> <span class="key">import</span> <span class="nam">vector_norm</span><span class="strut"> </span></p>
<p id="t8" class="pln"><span class="strut"> </span></p>
<p id="t9" class="pln"><span class="strut"> </span></p>
<p id="t10" class="stm run hide_run"><span class="key">def</span> <span class="nam">qat</span><span class="op">(</span><span class="nam">a</span><span class="op">,</span> <span class="nam">n</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t11" class="pln"> <span class="str">""" axis-angle quaternion function</span><span class="strut"> </span></p>
<p id="t12" class="pln"><span class="str"> Returns a unit quaternion</span><span class="strut"> </span></p>
<p id="t13" class="pln"><span class="strut"> </span></p>
<p id="t14" class="pln"><span class="str"> :param a: angle in degrees</span><span class="strut"> </span></p>
<p id="t15" class="pln"><span class="str"> :param n: unit canonical vector for a specific axis</span><span class="strut"> </span></p>
<p id="t16" class="pln"><span class="str"> :returns: a vector</span><span class="strut"> </span></p>
<p id="t17" class="pln"><span class="str"> :rtype: (np.ndarray)</span><span class="strut"> </span></p>
<p id="t18" class="pln"><span class="str"> ..ref: James Diebel</span><span class="strut"> </span></p>
<p id="t19" class="pln"><span class="str"> "Representing Attitude: Euler Angles, Unit Quaternions, and Rotation</span><span class="strut"> </span></p>
<p id="t20" class="pln"><span class="str"> Vectors."</span><span class="strut"> </span></p>
<p id="t21" class="pln"><span class="str"> (2006): p. 17 (6.12)</span><span class="strut"> </span></p>
<p id="t22" class="pln"><span class="str"> """</span><span class="strut"> </span></p>
<p id="t23" class="stm run hide_run"> <span class="nam">tmp</span> <span class="op">=</span> <span class="nam">np</span><span class="op">.</span><span class="nam">zeros</span><span class="op">(</span><span class="op">(</span><span class="num">4</span><span class="op">)</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t24" class="stm run hide_run"> <span class="nam">tmp</span><span class="op">[</span><span class="num">0</span><span class="op">]</span> <span class="op">=</span> <span class="nam">np</span><span class="op">.</span><span class="nam">cos</span><span class="op">(</span><span class="op">(</span><span class="num">1</span> <span class="op">/</span> <span class="num">2</span><span class="op">)</span> <span class="op">*</span> <span class="nam">a</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t25" class="stm run hide_run"> <span class="nam">tmp</span><span class="op">[</span><span class="num">1</span><span class="op">:</span><span class="num">4</span><span class="op">]</span> <span class="op">=</span> <span class="nam">np</span><span class="op">.</span><span class="nam">dot</span><span class="op">(</span><span class="nam">n</span><span class="op">,</span> <span class="nam">np</span><span class="op">.</span><span class="nam">sin</span><span class="op">(</span><span class="op">(</span><span class="num">1</span> <span class="op">/</span> <span class="num">2</span><span class="op">)</span> <span class="op">*</span> <span class="nam">a</span><span class="op">)</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t26" class="stm run hide_run"> <span class="key">return</span> <span class="nam">tmp</span><span class="strut"> </span></p>
<p id="t27" class="pln"><span class="strut"> </span></p>
<p id="t28" class="pln"><span class="strut"> </span></p>
<p id="t29" class="stm run hide_run"><span class="key">def</span> <span class="nam">from_euler</span><span class="op">(</span><span class="nam">ai</span><span class="op">,</span> <span class="nam">aj</span><span class="op">,</span> <span class="nam">ak</span><span class="op">,</span> <span class="nam">axes</span><span class="op">=</span><span class="str">'xyz'</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t30" class="pln"> <span class="str">"""Return quaternion from Euler angles and axis sequence.</span><span class="strut"> </span></p>
<p id="t31" class="pln"><span class="str"> ai, aj, ak : Euler's roll, pitch and yaw angles</span><span class="strut"> </span></p>
<p id="t32" class="pln"><span class="str"> axes : One of 24 axis sequences as string or encoded tuple</span><span class="strut"> </span></p>
<p id="t33" class="pln"><span class="str"> :param ai: angle in degrees to be rotated about the first axis</span><span class="strut"> </span></p>
<p id="t34" class="pln"><span class="str"> :param aj: angle in degrees to be rotated about the second axis</span><span class="strut"> </span></p>
<p id="t35" class="pln"><span class="str"> :param ak: angle in degrees to be rotated about the third axis</span><span class="strut"> </span></p>
<p id="t36" class="pln"><span class="str"> :param axes: string that encodes the order of the axes and</span><span class="strut"> </span></p>
<p id="t37" class="pln"><span class="str"> whether rotational or stationary axes should be used</span><span class="strut"> </span></p>
<p id="t38" class="pln"><span class="str"> :returns: a vector</span><span class="strut"> </span></p>
<p id="t39" class="pln"><span class="str"> :rtype: (np.ndarray)</span><span class="strut"> </span></p>
<p id="t40" class="pln"><span class="str"> ..ref: James Diebel</span><span class="strut"> </span></p>
<p id="t41" class="pln"><span class="str"> "Representing Attitude: Euler Angles, Unit Quaternions, and Rotation</span><span class="strut"> </span></p>
<p id="t42" class="pln"><span class="str"> Vectors."</span><span class="strut"> </span></p>
<p id="t43" class="pln"><span class="str"> (2006): p. 18 (6.14)</span><span class="strut"> </span></p>
<p id="t44" class="pln"><span class="str"> """</span><span class="strut"> </span></p>
<p id="t45" class="stm run hide_run"> <span class="nam">vects</span> <span class="op">=</span> <span class="op">[</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span><span class="op">,</span> <span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">1</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span><span class="op">,</span> <span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span><span class="op">]</span><span class="strut"> </span></p>
<p id="t46" class="stm run hide_run"> <span class="nam">r</span><span class="op">,</span> <span class="nam">i</span><span class="op">,</span> <span class="nam">j</span><span class="op">,</span> <span class="nam">k</span> <span class="op">=</span> <span class="nam">constants</span><span class="op">.</span><span class="nam">_AXES2TUPLE</span><span class="op">[</span><span class="nam">axes</span><span class="op">]</span><span class="strut"> </span></p>
<p id="t47" class="stm run hide_run"> <span class="nam">q1</span> <span class="op">=</span> <span class="nam">qat</span><span class="op">(</span><span class="nam">ai</span><span class="op">,</span> <span class="nam">vects</span><span class="op">[</span><span class="nam">i</span><span class="op">]</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t48" class="stm run hide_run"> <span class="nam">q2</span> <span class="op">=</span> <span class="nam">qat</span><span class="op">(</span><span class="nam">aj</span><span class="op">,</span> <span class="nam">vects</span><span class="op">[</span><span class="nam">j</span><span class="op">]</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t49" class="stm run hide_run"> <span class="nam">q3</span> <span class="op">=</span> <span class="nam">qat</span><span class="op">(</span><span class="nam">ak</span><span class="op">,</span> <span class="nam">vects</span><span class="op">[</span><span class="nam">k</span><span class="op">]</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t50" class="stm run hide_run"> <span class="nam">quaternion</span> <span class="op">=</span> <span class="nam">multiply</span><span class="op">(</span><span class="nam">np</span><span class="op">.</span><span class="nam">array</span><span class="op">(</span><span class="nam">q1</span><span class="op">)</span><span class="op">,</span> <span class="nam">np</span><span class="op">.</span><span class="nam">array</span><span class="op">(</span><span class="nam">q2</span><span class="op">)</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t51" class="stm run hide_run"> <span class="nam">qijk</span> <span class="op">=</span> <span class="nam">multiply</span><span class="op">(</span><span class="nam">quaternion</span><span class="op">,</span> <span class="nam">np</span><span class="op">.</span><span class="nam">array</span><span class="op">(</span><span class="nam">q3</span><span class="op">)</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t52" class="stm run hide_run"> <span class="key">return</span> <span class="nam">qijk</span><span class="strut"> </span></p>
<p id="t53" class="pln"><span class="strut"> </span></p>
<p id="t54" class="pln"><span class="strut"> </span></p>
<p id="t55" class="stm run hide_run"><span class="key">def</span> <span class="nam">about_axis</span><span class="op">(</span><span class="nam">angle</span><span class="op">,</span> <span class="nam">axis</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t56" class="pln"> <span class="str">"""Return quaternion for rotation about axis.</span><span class="strut"> </span></p>
<p id="t57" class="pln"><span class="strut"> </span></p>
<p id="t58" class="pln"><span class="str"> ..ref: James Diebel</span><span class="strut"> </span></p>
<p id="t59" class="pln"><span class="str"> "Representing Attitude: Euler Angles, Unit Quaternions, and Rotation</span><span class="strut"> </span></p>
<p id="t60" class="pln"><span class="str"> Vectors."</span><span class="strut"> </span></p>
<p id="t61" class="pln"><span class="str"> (2006): p. 17 (6.4), equation 175</span><span class="strut"> </span></p>
<p id="t62" class="pln"><span class="str"> """</span><span class="strut"> </span></p>
<p id="t63" class="stm run hide_run"> <span class="nam">q</span> <span class="op">=</span> <span class="nam">np</span><span class="op">.</span><span class="nam">array</span><span class="op">(</span><span class="op">[</span><span class="num">0.0</span><span class="op">,</span> <span class="nam">axis</span><span class="op">[</span><span class="num">0</span><span class="op">]</span><span class="op">,</span> <span class="nam">axis</span><span class="op">[</span><span class="num">1</span><span class="op">]</span><span class="op">,</span> <span class="nam">axis</span><span class="op">[</span><span class="num">2</span><span class="op">]</span><span class="op">]</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t64" class="stm run hide_run"> <span class="nam">qlen</span> <span class="op">=</span> <span class="nam">vector_norm</span><span class="op">(</span><span class="nam">q</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t65" class="stm run hide_run"> <span class="key">if</span> <span class="nam">qlen</span> <span class="op">></span> <span class="nam">constants</span><span class="op">.</span><span class="nam">_EPS</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t66" class="stm run hide_run"> <span class="nam">q</span> <span class="op">*=</span> <span class="nam">np</span><span class="op">.</span><span class="nam">sin</span><span class="op">(</span><span class="nam">angle</span> <span class="op">/</span> <span class="num">2.0</span><span class="op">)</span> <span class="op">/</span> <span class="nam">qlen</span><span class="strut"> </span></p>
<p id="t67" class="stm run hide_run"> <span class="nam">q</span><span class="op">[</span><span class="num">0</span><span class="op">]</span> <span class="op">=</span> <span class="nam">np</span><span class="op">.</span><span class="nam">cos</span><span class="op">(</span><span class="nam">angle</span> <span class="op">/</span> <span class="num">2.0</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t68" class="stm run hide_run"> <span class="key">return</span> <span class="nam">q</span><span class="strut"> </span></p>
<p id="t69" class="pln"><span class="strut"> </span></p>
<p id="t70" class="pln"><span class="strut"> </span></p>
<p id="t71" class="stm run hide_run"><span class="key">def</span> <span class="nam">matrix</span><span class="op">(</span><span class="nam">quaternion</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t72" class="pln"> <span class="str">"""Return homogeneous rotation matrix from quaternion.</span><span class="strut"> </span></p>
<p id="t73" class="pln"><span class="str"> :param quaternion : vector with at least 3 entrences (unit quaternion)</span><span class="strut"> </span></p>
<p id="t74" class="pln"><span class="str"> :returns: a matrix</span><span class="strut"> </span></p>
<p id="t75" class="pln"><span class="str"> :rtype: (np.ndarray)</span><span class="strut"> </span></p>
<p id="t76" class="pln"><span class="str"> ..ref: James Diebel</span><span class="strut"> </span></p>
<p id="t77" class="pln"><span class="str"> "Representing Attitude: Euler Angles, Unit Quaternions, and Rotation</span><span class="strut"> </span></p>
<p id="t78" class="pln"><span class="str"> Vectors."</span><span class="strut"> </span></p>
<p id="t79" class="pln"><span class="str"> (2006): p. 15 (6.4)</span><span class="strut"> </span></p>
<p id="t80" class="pln"><span class="str"> """</span><span class="strut"> </span></p>
<p id="t81" class="stm run hide_run"> <span class="nam">q</span> <span class="op">=</span> <span class="nam">np</span><span class="op">.</span><span class="nam">array</span><span class="op">(</span><span class="nam">quaternion</span><span class="op">,</span> <span class="nam">dtype</span><span class="op">=</span><span class="nam">np</span><span class="op">.</span><span class="nam">float64</span><span class="op">,</span> <span class="nam">copy</span><span class="op">=</span><span class="key">True</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t82" class="stm run hide_run"> <span class="nam">qnorm</span> <span class="op">=</span> <span class="nam">np</span><span class="op">.</span><span class="nam">sqrt</span><span class="op">(</span><span class="nam">q</span><span class="op">[</span><span class="num">0</span><span class="op">]</span><span class="op">**</span><span class="num">2</span> <span class="op">+</span> <span class="nam">q</span><span class="op">[</span><span class="num">1</span><span class="op">]</span><span class="op">**</span><span class="num">2</span> <span class="op">+</span> <span class="nam">q</span><span class="op">[</span><span class="num">2</span><span class="op">]</span><span class="op">**</span><span class="num">2</span> <span class="op">+</span> <span class="nam">q</span><span class="op">[</span><span class="num">3</span><span class="op">]</span><span class="op">**</span><span class="num">2</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t83" class="stm run hide_run"> <span class="key">if</span> <span class="nam">qnorm</span> <span class="op">!=</span> <span class="num">1</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t84" class="stm run hide_run"> <span class="nam">q</span> <span class="op">=</span> <span class="nam">q</span> <span class="op">/</span> <span class="nam">qnorm</span><span class="strut"> </span></p>
<p id="t85" class="stm run hide_run"> <span class="nam">q0</span> <span class="op">=</span> <span class="nam">q</span><span class="op">[</span><span class="num">0</span><span class="op">]</span><span class="strut"> </span></p>
<p id="t86" class="stm run hide_run"> <span class="nam">q1</span> <span class="op">=</span> <span class="nam">q</span><span class="op">[</span><span class="num">1</span><span class="op">]</span><span class="strut"> </span></p>
<p id="t87" class="stm run hide_run"> <span class="nam">q2</span> <span class="op">=</span> <span class="nam">q</span><span class="op">[</span><span class="num">2</span><span class="op">]</span><span class="strut"> </span></p>
<p id="t88" class="stm run hide_run"> <span class="nam">q3</span> <span class="op">=</span> <span class="nam">q</span><span class="op">[</span><span class="num">3</span><span class="op">]</span><span class="strut"> </span></p>
<p id="t89" class="pln"><span class="strut"> </span></p>
<p id="t90" class="stm run hide_run"> <span class="nam">mat</span> <span class="op">=</span> <span class="nam">np</span><span class="op">.</span><span class="nam">identity</span><span class="op">(</span><span class="num">4</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t91" class="pln"> <span class="com"># According to equation 125</span><span class="strut"> </span></p>
<p id="t92" class="stm run hide_run"> <span class="nam">mat</span><span class="op">[</span><span class="op">:</span><span class="num">3</span><span class="op">,</span> <span class="op">:</span><span class="num">3</span><span class="op">]</span> <span class="op">=</span> <span class="nam">np</span><span class="op">.</span><span class="nam">array</span><span class="op">(</span><span class="op">[</span><span class="strut"> </span></p>
<p id="t93" class="pln"> <span class="op">[</span><span class="nam">q0</span><span class="op">**</span><span class="num">2</span> <span class="op">+</span> <span class="nam">q1</span><span class="op">**</span><span class="num">2</span> <span class="op">-</span> <span class="nam">q2</span><span class="op">**</span><span class="num">2</span> <span class="op">-</span> <span class="nam">q3</span><span class="op">**</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span> <span class="op">*</span> <span class="nam">q1</span> <span class="op">*</span> <span class="nam">q2</span> <span class="op">+</span><span class="strut"> </span></p>
<p id="t94" class="pln"> <span class="num">2</span> <span class="op">*</span> <span class="nam">q0</span> <span class="op">*</span> <span class="nam">q3</span><span class="op">,</span> <span class="num">2</span> <span class="op">*</span> <span class="nam">q1</span> <span class="op">*</span> <span class="nam">q3</span> <span class="op">-</span> <span class="num">2</span> <span class="op">*</span> <span class="nam">q0</span> <span class="op">*</span> <span class="nam">q2</span><span class="op">]</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t95" class="pln"> <span class="op">[</span><span class="num">2</span> <span class="op">*</span> <span class="nam">q1</span> <span class="op">*</span> <span class="nam">q2</span> <span class="op">-</span> <span class="num">2</span> <span class="op">*</span> <span class="nam">q0</span> <span class="op">*</span> <span class="nam">q3</span><span class="op">,</span> <span class="nam">q0</span><span class="op">**</span><span class="num">2</span> <span class="op">-</span> <span class="nam">q1</span><span class="op">**</span><span class="num">2</span> <span class="op">+</span><span class="strut"> </span></p>
<p id="t96" class="pln"> <span class="nam">q2</span><span class="op">**</span><span class="num">2</span> <span class="op">-</span> <span class="nam">q3</span><span class="op">**</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span> <span class="op">*</span> <span class="nam">q2</span> <span class="op">*</span> <span class="nam">q3</span> <span class="op">+</span> <span class="num">2</span> <span class="op">*</span> <span class="nam">q0</span> <span class="op">*</span> <span class="nam">q1</span><span class="op">]</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t97" class="pln"> <span class="op">[</span><span class="num">2</span> <span class="op">*</span> <span class="nam">q1</span> <span class="op">*</span> <span class="nam">q3</span> <span class="op">+</span> <span class="num">2</span> <span class="op">*</span> <span class="nam">q0</span> <span class="op">*</span> <span class="nam">q2</span><span class="op">,</span> <span class="num">2</span> <span class="op">*</span> <span class="nam">q2</span> <span class="op">*</span> <span class="nam">q3</span> <span class="op">-</span> <span class="num">2</span> <span class="op">*</span> <span class="nam">q0</span> <span class="op">*</span> <span class="nam">q1</span><span class="op">,</span> <span class="nam">q0</span><span class="op">**</span><span class="num">2</span> <span class="op">-</span> <span class="nam">q1</span><span class="op">**</span><span class="num">2</span> <span class="op">-</span> <span class="nam">q2</span><span class="op">**</span><span class="num">2</span> <span class="op">+</span> <span class="nam">q3</span><span class="op">**</span><span class="num">2</span><span class="op">]</span><span class="op">]</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t98" class="stm run hide_run"> <span class="key">return</span> <span class="nam">mat</span><span class="strut"> </span></p>
<p id="t99" class="pln"><span class="strut"> </span></p>
<p id="t100" class="pln"><span class="strut"> </span></p>
<p id="t101" class="stm run hide_run"><span class="key">def</span> <span class="nam">from_matrix</span><span class="op">(</span><span class="nam">matrix</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t102" class="pln"> <span class="str">"""Return quaternion from rotation matrix.</span><span class="strut"> </span></p>
<p id="t103" class="pln"><span class="strut"> </span></p>
<p id="t104" class="pln"><span class="str"> :param matrix: a rotation matrix</span><span class="strut"> </span></p>
<p id="t105" class="pln"><span class="str"> :returns: a vector</span><span class="strut"> </span></p>
<p id="t106" class="pln"><span class="str"> :rtype: (np.ndarray)</span><span class="strut"> </span></p>
<p id="t107" class="pln"><span class="str"> ..ref: James Diebel</span><span class="strut"> </span></p>
<p id="t108" class="pln"><span class="str"> "Representing Attitude: Euler Angles, Unit Quaternions, and Rotation</span><span class="strut"> </span></p>
<p id="t109" class="pln"><span class="str"> Vectors."</span><span class="strut"> </span></p>
<p id="t110" class="pln"><span class="str"> (2006): p. 15 (6.5)</span><span class="strut"> </span></p>
<p id="t111" class="pln"><span class="str"> """</span><span class="strut"> </span></p>
<p id="t112" class="stm run hide_run"> <span class="nam">r</span> <span class="op">=</span> <span class="nam">matrix</span><span class="strut"> </span></p>
<p id="t113" class="pln"> <span class="com"># Split cases according to equation 145</span><span class="strut"> </span></p>
<p id="t114" class="stm run hide_run"> <span class="key">if</span> <span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op">>=</span> <span class="op">-</span><span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span> <span class="key">and</span> <span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">>=</span> <span class="op">-</span><span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span><span class="op">)</span> <span class="key">and</span> <span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">>=</span> <span class="op">-</span><span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t115" class="pln"> <span class="com"># equation 141</span><span class="strut"> </span></p>
<p id="t116" class="stm run hide_run"> <span class="key">return</span> <span class="op">(</span><span class="num">1</span> <span class="op">/</span> <span class="num">2</span><span class="op">)</span> <span class="op">*</span> <span class="nam">np</span><span class="op">.</span><span class="nam">array</span><span class="op">(</span><span class="op">[</span><span class="nam">np</span><span class="op">.</span><span class="nam">sqrt</span><span class="op">(</span><span class="num">1</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t117" class="pln"> <span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span><span class="op">)</span> <span class="op">/</span><span class="strut"> </span></p>
<p id="t118" class="pln"> <span class="nam">np</span><span class="op">.</span><span class="nam">sqrt</span><span class="op">(</span><span class="num">1</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t119" class="pln"> <span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span> <span class="op">/</span><span class="strut"> </span></p>
<p id="t120" class="pln"> <span class="nam">np</span><span class="op">.</span><span class="nam">sqrt</span><span class="op">(</span><span class="num">1</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t121" class="pln"> <span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span><span class="op">)</span> <span class="op">/</span><span class="strut"> </span></p>
<p id="t122" class="pln"> <span class="nam">np</span><span class="op">.</span><span class="nam">sqrt</span><span class="op">(</span><span class="num">1</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span><span class="op">]</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t123" class="pln"><span class="strut"> </span></p>
<p id="t124" class="stm run hide_run"> <span class="key">if</span> <span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op"><</span> <span class="op">-</span><span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span> <span class="key">and</span> <span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">></span> <span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span><span class="op">)</span> <span class="key">and</span> <span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">></span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t125" class="pln"> <span class="com"># equation 142</span><span class="strut"> </span></p>
<p id="t126" class="stm run hide_run"> <span class="key">return</span> <span class="op">(</span><span class="num">1</span> <span class="op">/</span> <span class="num">2</span><span class="op">)</span> <span class="op">*</span> <span class="nam">np</span><span class="op">.</span><span class="nam">array</span><span class="op">(</span><span class="op">[</span><span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span><span class="op">)</span> <span class="op">/</span><span class="strut"> </span></p>
<p id="t127" class="pln"> <span class="nam">np</span><span class="op">.</span><span class="nam">sqrt</span><span class="op">(</span><span class="num">1</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t128" class="pln"> <span class="nam">np</span><span class="op">.</span><span class="nam">sqrt</span><span class="op">(</span><span class="num">1</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t129" class="pln"> <span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span><span class="op">)</span> <span class="op">/</span><span class="strut"> </span></p>
<p id="t130" class="pln"> <span class="nam">np</span><span class="op">.</span><span class="nam">sqrt</span><span class="op">(</span><span class="num">1</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t131" class="pln"> <span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span> <span class="op">/</span><span class="strut"> </span></p>
<p id="t132" class="pln"> <span class="nam">np</span><span class="op">.</span><span class="nam">sqrt</span><span class="op">(</span><span class="num">1</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span><span class="op">]</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t133" class="stm mis"> <span class="key">if</span> <span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op">></span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span> <span class="key">and</span> <span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op"><</span> <span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span><span class="op">)</span> <span class="key">and</span> <span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op"><</span> <span class="op">-</span><span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t134" class="pln"> <span class="com"># equation 143</span><span class="strut"> </span></p>
<p id="t135" class="stm mis"> <span class="key">return</span> <span class="op">(</span><span class="num">1</span> <span class="op">/</span> <span class="num">2</span><span class="op">)</span> <span class="op">*</span> <span class="nam">np</span><span class="op">.</span><span class="nam">array</span><span class="op">(</span><span class="op">[</span><span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span> <span class="op">/</span><span class="strut"> </span></p>
<p id="t136" class="pln"> <span class="nam">np</span><span class="op">.</span><span class="nam">sqrt</span><span class="op">(</span><span class="num">1</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t137" class="pln"> <span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span><span class="op">)</span> <span class="op">/</span><span class="strut"> </span></p>
<p id="t138" class="pln"> <span class="nam">np</span><span class="op">.</span><span class="nam">sqrt</span><span class="op">(</span><span class="num">1</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t139" class="pln"> <span class="nam">np</span><span class="op">.</span><span class="nam">sqrt</span><span class="op">(</span><span class="num">1</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t140" class="pln"> <span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span><span class="op">)</span> <span class="op">/</span><span class="strut"> </span></p>
<p id="t141" class="pln"> <span class="nam">np</span><span class="op">.</span><span class="nam">sqrt</span><span class="op">(</span><span class="num">1</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span><span class="op">]</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t142" class="stm mis"> <span class="key">if</span> <span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op"><</span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span> <span class="key">and</span> <span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op"><</span> <span class="op">-</span><span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span><span class="op">)</span> <span class="key">and</span> <span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op"><</span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t143" class="pln"> <span class="com"># equation 144</span><span class="strut"> </span></p>
<p id="t144" class="stm mis"> <span class="key">return</span> <span class="op">(</span><span class="num">1</span> <span class="op">/</span> <span class="num">2</span><span class="op">)</span> <span class="op">*</span> <span class="nam">np</span><span class="op">.</span><span class="nam">array</span><span class="op">(</span><span class="op">[</span><span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span><span class="op">)</span> <span class="op">/</span><span class="strut"> </span></p>
<p id="t145" class="pln"> <span class="nam">np</span><span class="op">.</span><span class="nam">sqrt</span><span class="op">(</span><span class="num">1</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t146" class="pln"> <span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span> <span class="op">/</span><span class="strut"> </span></p>
<p id="t147" class="pln"> <span class="nam">np</span><span class="op">.</span><span class="nam">sqrt</span><span class="op">(</span><span class="num">1</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t148" class="pln"> <span class="op">(</span><span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span><span class="op">)</span> <span class="op">/</span><span class="strut"> </span></p>
<p id="t149" class="pln"> <span class="nam">np</span><span class="op">.</span><span class="nam">sqrt</span><span class="op">(</span><span class="num">1</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t150" class="pln"> <span class="nam">np</span><span class="op">.</span><span class="nam">sqrt</span><span class="op">(</span><span class="num">1</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span> <span class="op">-</span> <span class="nam">r</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">1</span><span class="op">]</span> <span class="op">+</span> <span class="nam">r</span><span class="op">[</span><span class="num">2</span><span class="op">,</span> <span class="num">2</span><span class="op">]</span><span class="op">)</span><span class="op">]</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t151" class="pln"><span class="strut"> </span></p>
<p id="t152" class="pln"><span class="strut"> </span></p>
<p id="t153" class="stm run hide_run"><span class="key">def</span> <span class="nam">multiply</span><span class="op">(</span><span class="nam">quaternion1</span><span class="op">,</span> <span class="nam">quaternion0</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t154" class="pln"> <span class="str">"""Return multiplication of two quaternions.</span><span class="strut"> </span></p>
<p id="t155" class="pln"><span class="str"> """</span><span class="strut"> </span></p>
<p id="t156" class="stm run hide_run"> <span class="nam">w0</span><span class="op">,</span> <span class="nam">x0</span><span class="op">,</span> <span class="nam">y0</span><span class="op">,</span> <span class="nam">z0</span> <span class="op">=</span> <span class="nam">quaternion0</span><span class="strut"> </span></p>
<p id="t157" class="stm run hide_run"> <span class="nam">w1</span><span class="op">,</span> <span class="nam">x1</span><span class="op">,</span> <span class="nam">y1</span><span class="op">,</span> <span class="nam">z1</span> <span class="op">=</span> <span class="nam">quaternion1</span><span class="strut"> </span></p>
<p id="t158" class="stm run hide_run"> <span class="key">return</span> <span class="nam">np</span><span class="op">.</span><span class="nam">array</span><span class="op">(</span><span class="op">[</span><span class="op">-</span><span class="nam">x1</span> <span class="op">*</span> <span class="nam">x0</span> <span class="op">-</span> <span class="nam">y1</span> <span class="op">*</span> <span class="nam">y0</span> <span class="op">-</span> <span class="nam">z1</span> <span class="op">*</span> <span class="nam">z0</span> <span class="op">+</span> <span class="nam">w1</span> <span class="op">*</span> <span class="nam">w0</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t159" class="pln"> <span class="nam">x1</span> <span class="op">*</span> <span class="nam">w0</span> <span class="op">+</span> <span class="nam">y1</span> <span class="op">*</span> <span class="nam">z0</span> <span class="op">-</span> <span class="nam">z1</span> <span class="op">*</span> <span class="nam">y0</span> <span class="op">+</span> <span class="nam">w1</span> <span class="op">*</span> <span class="nam">x0</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t160" class="pln"> <span class="op">-</span><span class="nam">x1</span> <span class="op">*</span> <span class="nam">z0</span> <span class="op">+</span> <span class="nam">y1</span> <span class="op">*</span> <span class="nam">w0</span> <span class="op">+</span> <span class="nam">z1</span> <span class="op">*</span> <span class="nam">x0</span> <span class="op">+</span> <span class="nam">w1</span> <span class="op">*</span> <span class="nam">y0</span><span class="op">,</span><span class="strut"> </span></p>
<p id="t161" class="pln"> <span class="nam">x1</span> <span class="op">*</span> <span class="nam">y0</span> <span class="op">-</span> <span class="nam">y1</span> <span class="op">*</span> <span class="nam">x0</span> <span class="op">+</span> <span class="nam">z1</span> <span class="op">*</span> <span class="nam">w0</span> <span class="op">+</span> <span class="nam">w1</span> <span class="op">*</span> <span class="nam">z0</span><span class="op">]</span><span class="op">,</span> <span class="nam">dtype</span><span class="op">=</span><span class="nam">np</span><span class="op">.</span><span class="nam">float64</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t162" class="pln"><span class="strut"> </span></p>
<p id="t163" class="pln"><span class="strut"> </span></p>
<p id="t164" class="stm run hide_run"><span class="key">def</span> <span class="nam">conjugate</span><span class="op">(</span><span class="nam">quaternion</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t165" class="pln"> <span class="str">"""Return conjugate of quaternion.</span><span class="strut"> </span></p>
<p id="t166" class="pln"><span class="str"> """</span><span class="strut"> </span></p>
<p id="t167" class="stm mis"> <span class="nam">q</span> <span class="op">=</span> <span class="nam">np</span><span class="op">.</span><span class="nam">array</span><span class="op">(</span><span class="nam">quaternion</span><span class="op">,</span> <span class="nam">dtype</span><span class="op">=</span><span class="nam">np</span><span class="op">.</span><span class="nam">float64</span><span class="op">,</span> <span class="nam">copy</span><span class="op">=</span><span class="key">True</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t168" class="stm mis"> <span class="nam">np</span><span class="op">.</span><span class="nam">negative</span><span class="op">(</span><span class="nam">q</span><span class="op">[</span><span class="num">1</span><span class="op">:</span><span class="op">]</span><span class="op">,</span> <span class="nam">q</span><span class="op">[</span><span class="num">1</span><span class="op">:</span><span class="op">]</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t169" class="stm mis"> <span class="key">return</span> <span class="nam">q</span><span class="strut"> </span></p>
<p id="t170" class="pln"><span class="strut"> </span></p>
<p id="t171" class="pln"><span class="strut"> </span></p>
<p id="t172" class="stm run hide_run"><span class="key">def</span> <span class="nam">inverse</span><span class="op">(</span><span class="nam">quaternion</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t173" class="pln"> <span class="str">"""Return inverse of quaternion.</span><span class="strut"> </span></p>
<p id="t174" class="pln"><span class="str"> """</span><span class="strut"> </span></p>
<p id="t175" class="stm run hide_run"> <span class="nam">q</span> <span class="op">=</span> <span class="nam">np</span><span class="op">.</span><span class="nam">array</span><span class="op">(</span><span class="nam">quaternion</span><span class="op">,</span> <span class="nam">dtype</span><span class="op">=</span><span class="nam">np</span><span class="op">.</span><span class="nam">float64</span><span class="op">,</span> <span class="nam">copy</span><span class="op">=</span><span class="key">True</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t176" class="stm run hide_run"> <span class="nam">np</span><span class="op">.</span><span class="nam">negative</span><span class="op">(</span><span class="nam">q</span><span class="op">[</span><span class="num">1</span><span class="op">:</span><span class="op">]</span><span class="op">,</span> <span class="nam">q</span><span class="op">[</span><span class="num">1</span><span class="op">:</span><span class="op">]</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t177" class="stm run hide_run"> <span class="key">return</span> <span class="nam">q</span> <span class="op">/</span> <span class="nam">np</span><span class="op">.</span><span class="nam">dot</span><span class="op">(</span><span class="nam">q</span><span class="op">,</span> <span class="nam">q</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t178" class="pln"><span class="strut"> </span></p>
<p id="t179" class="pln"><span class="strut"> </span></p>
<p id="t180" class="stm run hide_run"><span class="key">def</span> <span class="nam">real</span><span class="op">(</span><span class="nam">quaternion</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t181" class="pln"> <span class="str">"""Return real part of quaternion.</span><span class="strut"> </span></p>
<p id="t182" class="pln"><span class="str"> """</span><span class="strut"> </span></p>
<p id="t183" class="stm mis"> <span class="key">return</span> <span class="nam">float</span><span class="op">(</span><span class="nam">quaternion</span><span class="op">[</span><span class="num">0</span><span class="op">]</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t184" class="pln"><span class="strut"> </span></p>
<p id="t185" class="pln"><span class="strut"> </span></p>
<p id="t186" class="stm run hide_run"><span class="key">def</span> <span class="nam">imag</span><span class="op">(</span><span class="nam">quaternion</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t187" class="pln"> <span class="str">"""Return imaginary part of quaternion.</span><span class="strut"> </span></p>
<p id="t188" class="pln"><span class="str"> """</span><span class="strut"> </span></p>
<p id="t189" class="stm mis"> <span class="key">return</span> <span class="nam">np</span><span class="op">.</span><span class="nam">array</span><span class="op">(</span><span class="nam">quaternion</span><span class="op">[</span><span class="num">1</span><span class="op">:</span><span class="num">4</span><span class="op">]</span><span class="op">,</span> <span class="nam">dtype</span><span class="op">=</span><span class="nam">np</span><span class="op">.</span><span class="nam">float64</span><span class="op">,</span> <span class="nam">copy</span><span class="op">=</span><span class="key">True</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t190" class="pln"><span class="strut"> </span></p>
<p id="t191" class="pln"><span class="strut"> </span></p>
<p id="t192" class="stm run hide_run"><span class="key">def</span> <span class="nam">diff</span><span class="op">(</span><span class="nam">quaternion0</span><span class="op">,</span> <span class="nam">quaternion1</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t193" class="pln"> <span class="str">""" The axis and angle between two quaternions</span><span class="strut"> </span></p>
<p id="t194" class="pln"><span class="str"> """</span><span class="strut"> </span></p>
<p id="t195" class="stm mis"> <span class="nam">q</span> <span class="op">=</span> <span class="nam">multiply</span><span class="op">(</span><span class="nam">quaternion1</span><span class="op">,</span> <span class="nam">conjugate</span><span class="op">(</span><span class="nam">quaternion0</span><span class="op">)</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t196" class="stm mis"> <span class="nam">length</span> <span class="op">=</span> <span class="nam">np</span><span class="op">.</span><span class="nam">sum</span><span class="op">(</span><span class="nam">np</span><span class="op">.</span><span class="nam">sqrt</span><span class="op">(</span><span class="nam">q</span><span class="op">[</span><span class="num">1</span><span class="op">:</span><span class="num">4</span><span class="op">]</span> <span class="op">*</span> <span class="nam">q</span><span class="op">[</span><span class="num">1</span><span class="op">:</span><span class="num">4</span><span class="op">]</span><span class="op">)</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t197" class="stm mis"> <span class="nam">angle</span> <span class="op">=</span> <span class="num">2</span> <span class="op">*</span> <span class="nam">np</span><span class="op">.</span><span class="nam">arctan2</span><span class="op">(</span><span class="nam">length</span><span class="op">,</span> <span class="nam">q</span><span class="op">[</span><span class="num">0</span><span class="op">]</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t198" class="stm mis"> <span class="key">if</span> <span class="nam">np</span><span class="op">.</span><span class="nam">isclose</span><span class="op">(</span><span class="nam">length</span><span class="op">,</span> <span class="num">0</span><span class="op">)</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t199" class="stm mis"> <span class="nam">axis</span> <span class="op">=</span> <span class="nam">np</span><span class="op">.</span><span class="nam">array</span><span class="op">(</span><span class="op">[</span><span class="num">1</span><span class="op">,</span> <span class="num">0</span><span class="op">,</span> <span class="num">0</span><span class="op">]</span><span class="op">)</span><span class="strut"> </span></p>
<p id="t200" class="pln"> <span class="key">else</span><span class="op">:</span><span class="strut"> </span></p>
<p id="t201" class="stm mis"> <span class="nam">axis</span> <span class="op">=</span> <span class="nam">q</span><span class="op">[</span><span class="num">1</span><span class="op">:</span><span class="num">4</span><span class="op">]</span> <span class="op">/</span> <span class="nam">length</span><span class="strut"> </span></p>
<p id="t202" class="stm mis"> <span class="key">return</span> <span class="nam">axis</span><span class="op">,</span> <span class="nam">angle</span><span class="strut"> </span></p>
</td>
</tr>
</table>
</div>
<div id="footer">
<div class="content">
<p>
<a class="nav" href="index.html">« index</a> <a class="nav" href="https://coverage.readthedocs.io">coverage.py v4.5.2</a>,
created at 2019-01-22 15:55
</p>
</div>
</div>
</body>
</html>