<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
<html><head><meta http-equiv="Content-Type" content="text/html;charset=iso-8859-1">
<title>CQuaternion.h Source File</title>
<link href="doxygen.css" rel="stylesheet" type="text/css">
<link href="tabs.css" rel="stylesheet" type="text/css">
</head><body>
<div align="left"><a href="http://www.mrpt.org/">Main MRPT website</a> > <b>C++ reference</b> </div>
<div align="right">
<a href="index.html"><img border="0" src="mrpt_logo.png" alt="MRPT logo"></a>
</div>
<!-- Generated by Doxygen 1.7.5 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
<div id="navrow1" class="tabs">
<ul class="tablist">
<li><a href="index.html"><span>Main Page</span></a></li>
<li><a href="pages.html"><span>Related Pages</span></a></li>
<li><a href="modules.html"><span>Modules</span></a></li>
<li><a href="namespaces.html"><span>Namespaces</span></a></li>
<li><a href="annotated.html"><span>Classes</span></a></li>
<li class="current"><a href="files.html"><span>Files</span></a></li>
<li>
<div id="MSearchBox" class="MSearchBoxInactive">
<div class="left">
<form id="FSearchBox" action="search.php" method="get">
<img id="MSearchSelect" src="search/mag.png" alt=""/>
<input type="text" id="MSearchField" name="query" value="Search" size="20" accesskey="S"
onfocus="searchBox.OnSearchFieldFocus(true)"
onblur="searchBox.OnSearchFieldFocus(false)"/>
</form>
</div><div class="right"></div>
</div>
</li>
</ul>
</div>
<div id="navrow2" class="tabs2">
<ul class="tablist">
<li><a href="files.html"><span>File List</span></a></li>
<li><a href="globals.html"><span>File Members</span></a></li>
</ul>
</div>
<div class="header">
<div class="headertitle">
<div class="title">CQuaternion.h</div> </div>
</div>
<div class="contents">
<a href="_c_quaternion_8h.html">Go to the documentation of this file.</a><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="comment">/* +---------------------------------------------------------------------------+</span>
<a name="l00002"></a>00002 <span class="comment"> | The Mobile Robot Programming Toolkit (MRPT) C++ library |</span>
<a name="l00003"></a>00003 <span class="comment"> | |</span>
<a name="l00004"></a>00004 <span class="comment"> | http://www.mrpt.org/ |</span>
<a name="l00005"></a>00005 <span class="comment"> | |</span>
<a name="l00006"></a>00006 <span class="comment"> | Copyright (C) 2005-2011 University of Malaga |</span>
<a name="l00007"></a>00007 <span class="comment"> | |</span>
<a name="l00008"></a>00008 <span class="comment"> | This software was written by the Machine Perception and Intelligent |</span>
<a name="l00009"></a>00009 <span class="comment"> | Robotics Lab, University of Malaga (Spain). |</span>
<a name="l00010"></a>00010 <span class="comment"> | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |</span>
<a name="l00011"></a>00011 <span class="comment"> | |</span>
<a name="l00012"></a>00012 <span class="comment"> | This file is part of the MRPT project. |</span>
<a name="l00013"></a>00013 <span class="comment"> | |</span>
<a name="l00014"></a>00014 <span class="comment"> | MRPT is free software: you can redistribute it and/or modify |</span>
<a name="l00015"></a>00015 <span class="comment"> | it under the terms of the GNU General Public License as published by |</span>
<a name="l00016"></a>00016 <span class="comment"> | the Free Software Foundation, either version 3 of the License, or |</span>
<a name="l00017"></a>00017 <span class="comment"> | (at your option) any later version. |</span>
<a name="l00018"></a>00018 <span class="comment"> | |</span>
<a name="l00019"></a>00019 <span class="comment"> | MRPT is distributed in the hope that it will be useful, |</span>
<a name="l00020"></a>00020 <span class="comment"> | but WITHOUT ANY WARRANTY; without even the implied warranty of |</span>
<a name="l00021"></a>00021 <span class="comment"> | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |</span>
<a name="l00022"></a>00022 <span class="comment"> | GNU General Public License for more details. |</span>
<a name="l00023"></a>00023 <span class="comment"> | |</span>
<a name="l00024"></a>00024 <span class="comment"> | You should have received a copy of the GNU General Public License |</span>
<a name="l00025"></a>00025 <span class="comment"> | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |</span>
<a name="l00026"></a>00026 <span class="comment"> | |</span>
<a name="l00027"></a>00027 <span class="comment"> +---------------------------------------------------------------------------+ */</span>
<a name="l00028"></a>00028
<a name="l00029"></a>00029 <span class="preprocessor">#ifndef CQuaternion_H</span>
<a name="l00030"></a>00030 <span class="preprocessor"></span><span class="preprocessor">#define CQuaternion_H</span>
<a name="l00031"></a>00031 <span class="preprocessor"></span>
<a name="l00032"></a>00032 <span class="preprocessor">#include <<a class="code" href="_c_matrix_template_numeric_8h.html">mrpt/math/CMatrixTemplateNumeric.h</a>></span>
<a name="l00033"></a>00033 <span class="preprocessor">#include <<a class="code" href="_c_array_8h.html">mrpt/math/CArray.h</a>></span>
<a name="l00034"></a>00034
<a name="l00035"></a>00035 <span class="keyword">namespace </span>mrpt
<a name="l00036"></a>00036 {
<a name="l00037"></a>00037 <span class="keyword">namespace </span>math
<a name="l00038"></a>00038 {
<a name="l00039"></a>00039 <span class="comment">// For use with a constructor of CQuaternion</span>
<a name="l00040"></a><a class="code" href="namespacemrpt_1_1math.html#a8ab289d85828809b390d477f824a908a">00040</a> <span class="keyword">enum</span> <a class="code" href="namespacemrpt_1_1math.html#a8ab289d85828809b390d477f824a908a">TConstructorFlags_Quaternions</a>
<a name="l00041"></a>00041 {
<a name="l00042"></a><a class="code" href="namespacemrpt_1_1math.html#a8ab289d85828809b390d477f824a908aa845e9a46f0b0e9601057449d65849fb5">00042</a> <a class="code" href="namespacemrpt_1_1math.html#a8ab289d85828809b390d477f824a908aa845e9a46f0b0e9601057449d65849fb5">UNINITIALIZED_QUATERNION</a> = 0
<a name="l00043"></a>00043 };
<a name="l00044"></a>00044 <span class="comment"></span>
<a name="l00045"></a>00045 <span class="comment"> /** A quaternion, which can represent a 3D rotation as pair \f$ (r,\mathbf{u}) \f$, with a real part "r" and a 3D vector \f$ \mathbf{u} = (x,y,z) \f$, or alternatively, q = r + ix + jy + kz.</span>
<a name="l00046"></a>00046 <span class="comment"> *</span>
<a name="l00047"></a>00047 <span class="comment"> * The elements of the quaternion can be accessed by either:</span>
<a name="l00048"></a>00048 <span class="comment"> * - r(), x(), y(), z(), or</span>
<a name="l00049"></a>00049 <span class="comment"> * - the operator [] with indices running from 0 (=r) to 3 (=z).</span>
<a name="l00050"></a>00050 <span class="comment"> *</span>
<a name="l00051"></a>00051 <span class="comment"> * Users will usually employ the typedef "CQuaternionDouble" instead of this template.</span>
<a name="l00052"></a>00052 <span class="comment"> *</span>
<a name="l00053"></a>00053 <span class="comment"> * For more information about quaternions, see:</span>
<a name="l00054"></a>00054 <span class="comment"> * - http://people.csail.mit.edu/bkph/articles/Quaternions.pdf</span>
<a name="l00055"></a>00055 <span class="comment"> * - http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation</span>
<a name="l00056"></a>00056 <span class="comment"> *</span>
<a name="l00057"></a>00057 <span class="comment"> * \ingroup mrpt_base_grp</span>
<a name="l00058"></a>00058 <span class="comment"> * \sa mrpt::poses::CPose3D</span>
<a name="l00059"></a>00059 <span class="comment"> */</span>
<a name="l00060"></a>00060 <span class="keyword">template</span> <<span class="keyword">class</span> T>
<a name="l00061"></a>00061 <span class="keyword">class </span>CQuaternion : <span class="keyword">public</span> CArrayNumeric<T,4>
<a name="l00062"></a>00062 {
<a name="l00063"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a15d61e09fd7d5a035e455b84069356d8">00063</a> <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1math_1_1_c_array_numeric.html">CArrayNumeric<T,4></a> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a15d61e09fd7d5a035e455b84069356d8">Base</a>;
<a name="l00064"></a>00064 <span class="keyword">public</span>:
<a name="l00065"></a>00065 <span class="comment">/* @{ Constructors</span>
<a name="l00066"></a>00066 <span class="comment"> */</span>
<a name="l00067"></a>00067 <span class="comment"></span>
<a name="l00068"></a>00068 <span class="comment"> /** Can be used with UNINITIALIZED_QUATERNION as argument, does not initialize the 4 elements of the quaternion (use this constructor when speed is critical). */</span>
<a name="l00069"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#abce802fa76e3481f6c99aad27b0cb184">00069</a> <span class="keyword">inline</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#abce802fa76e3481f6c99aad27b0cb184" title="Can be used with UNINITIALIZED_QUATERNION as argument, does not initialize the 4 elements of the quat...">CQuaternion</a>(<a class="code" href="namespacemrpt_1_1math.html#a8ab289d85828809b390d477f824a908a">TConstructorFlags_Quaternions</a> constructor_dummy_param) { }
<a name="l00070"></a>00070 <span class="comment"></span>
<a name="l00071"></a>00071 <span class="comment"> /** Default constructor: construct a (1, (0,0,0) ) quaternion representing no rotation. */</span>
<a name="l00072"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a7e483f7e8c686bd98db13f5f1a65094c">00072</a> <span class="keyword">inline</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a7e483f7e8c686bd98db13f5f1a65094c" title="Default constructor: construct a (1, (0,0,0) ) quaternion representing no rotation.">CQuaternion</a>()
<a name="l00073"></a>00073 {
<a name="l00074"></a>00074 (*this)[0] = 1;
<a name="l00075"></a>00075 (*this)[1] = 0;
<a name="l00076"></a>00076 (*this)[2] = 0;
<a name="l00077"></a>00077 (*this)[3] = 0;
<a name="l00078"></a>00078 }
<a name="l00079"></a>00079 <span class="comment"></span>
<a name="l00080"></a>00080 <span class="comment"> /** Construct a quaternion from its parameters 'r', 'x', 'y', 'z', with q = r + ix + jy + kz. */</span>
<a name="l00081"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#adcaaed4fa688755a4c67cdbb9625af5f">00081</a> <span class="keyword">inline</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a7e483f7e8c686bd98db13f5f1a65094c" title="Default constructor: construct a (1, (0,0,0) ) quaternion representing no rotation.">CQuaternion</a>(<span class="keyword">const</span> T <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>,<span class="keyword">const</span> T <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>,<span class="keyword">const</span> T <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>,<span class="keyword">const</span> T <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>)
<a name="l00082"></a>00082 {
<a name="l00083"></a>00083 (*this)[0] = <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>;
<a name="l00084"></a>00084 (*this)[1] = <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>;
<a name="l00085"></a>00085 (*this)[2] = <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>;
<a name="l00086"></a>00086 (*this)[3] = <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>;
<a name="l00087"></a>00087 <a class="code" href="mrpt__macros_8h.html#ade1805af05ddae6a6ddb62c8b33ceaf1">ASSERTDEBMSG_</a>(<a class="code" href="group__metric__grp.html#gaf15608f8914516f8d949a8c053d55021">std::abs</a>(<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a409be4f6b23e66c2bee70da319d05525" title="Return the squared norm of the quaternion.">normSqr</a>()-1.0)<1e-5, <a class="code" href="namespacemrpt.html#a3a27af794b658df5491e2b7678f8ccb8" title="A std::string version of C sprintf.">mrpt::format</a>(<span class="stringliteral">"Initialization data for quaternion is not normalized: %f %f %f %f -> sqrNorm=%f"</span>,r,x,y,z,<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a409be4f6b23e66c2bee70da319d05525" title="Return the squared norm of the quaternion.">normSqr</a>()) );
<a name="l00088"></a>00088 }
<a name="l00089"></a>00089
<a name="l00090"></a>00090 <span class="comment">/* @}</span>
<a name="l00091"></a>00091 <span class="comment"> */</span>
<a name="l00092"></a>00092
<a name="l00093"></a>00093
<a name="l00094"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc">00094</a> <span class="keyword">inline</span> T <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()<span class="keyword">const </span>{<span class="keywordflow">return</span> (*<span class="keyword">this</span>)[0];} <span class="comment">//!< Return r coordinate of the quaternion</span>
<a name="l00095"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a">00095</a> <span class="comment"></span> <span class="keyword">inline</span> T <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()<span class="keyword">const </span>{<span class="keywordflow">return</span> (*<span class="keyword">this</span>)[1];} <span class="comment">//!< Return x coordinate of the quaternion</span>
<a name="l00096"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9">00096</a> <span class="comment"></span> <span class="keyword">inline</span> T <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()<span class="keyword">const </span>{<span class="keywordflow">return</span> (*<span class="keyword">this</span>)[2];} <span class="comment">//!< Return y coordinate of the quaternion</span>
<a name="l00097"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1">00097</a> <span class="comment"></span> <span class="keyword">inline</span> T <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()<span class="keyword">const </span>{<span class="keywordflow">return</span> (*<span class="keyword">this</span>)[3];} <span class="comment">//!< Return z coordinate of the quaternion</span>
<a name="l00098"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a96f1e3d33afabaa3c301c26e43cf2f72">00098</a> <span class="comment"></span> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a96f1e3d33afabaa3c301c26e43cf2f72" title="Set r coordinate of the quaternion.">r</a>(<span class="keyword">const</span> T <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>) {(*this)[0]=<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a96f1e3d33afabaa3c301c26e43cf2f72" title="Set r coordinate of the quaternion.">r</a>;} <span class="comment">//!< Set r coordinate of the quaternion</span>
<a name="l00099"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3b12f8ed65efa2d485fe4314da4c6054">00099</a> <span class="comment"></span> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3b12f8ed65efa2d485fe4314da4c6054" title="Set x coordinate of the quaternion.">x</a>(<span class="keyword">const</span> T <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>) {(*this)[1]=<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3b12f8ed65efa2d485fe4314da4c6054" title="Set x coordinate of the quaternion.">x</a>;} <span class="comment">//!< Set x coordinate of the quaternion</span>
<a name="l00100"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a7647915dd59f4c7e9ce973b676bd5b98">00100</a> <span class="comment"></span> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a7647915dd59f4c7e9ce973b676bd5b98" title="Set y coordinate of the quaternion.">y</a>(<span class="keyword">const</span> T <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>) {(*this)[2]=<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a7647915dd59f4c7e9ce973b676bd5b98" title="Set y coordinate of the quaternion.">y</a>;} <span class="comment">//!< Set y coordinate of the quaternion</span>
<a name="l00101"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae41e503dc4db2ded94ffa3ad325293db">00101</a> <span class="comment"></span> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae41e503dc4db2ded94ffa3ad325293db" title="Set z coordinate of the quaternion.">z</a>(<span class="keyword">const</span> T <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>) {(*this)[3]=<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae41e503dc4db2ded94ffa3ad325293db" title="Set z coordinate of the quaternion.">z</a>;} <span class="comment">//!< Set z coordinate of the quaternion</span>
<a name="l00102"></a>00102 <span class="comment"></span><span class="comment"></span>
<a name="l00103"></a>00103 <span class="comment"> /** Set this quaternion to the rotation described by a 3D Rodrigues rotation vector.</span>
<a name="l00104"></a>00104 <span class="comment"> */</span>
<a name="l00105"></a>00105 <span class="keyword">template</span> <<span class="keyword">class</span> ARRAYLIKE>
<a name="l00106"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#aa90eda234f74977c51b4cea049280780">00106</a> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#aa90eda234f74977c51b4cea049280780" title="Set this quaternion to the rotation described by a 3D Rodrigues rotation vector.">fromRodriguesVector</a>(<span class="keyword">const</span> ARRAYLIKE &in)
<a name="l00107"></a>00107 {
<a name="l00108"></a>00108 <span class="keywordflow">if</span> (in.size()!=3) <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"Wrong Dimension in input vector for quaternion Constructor"</span>);
<a name="l00109"></a>00109
<a name="l00110"></a>00110 <span class="keyword">const</span> T <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a> = in[0];
<a name="l00111"></a>00111 <span class="keyword">const</span> T <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a> = in[1];
<a name="l00112"></a>00112 <span class="keyword">const</span> T <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a> = in[2];
<a name="l00113"></a>00113 <span class="keywordflow">if</span> ((x==0)&&(y==0)&&(z==0))
<a name="l00114"></a>00114 {
<a name="l00115"></a>00115 (*this)[0] = 1;
<a name="l00116"></a>00116 (*this)[1] = 0;
<a name="l00117"></a>00117 (*this)[2] = 0;
<a name="l00118"></a>00118 (*this)[3] = 0;
<a name="l00119"></a>00119 }
<a name="l00120"></a>00120 <span class="keywordflow">else</span>
<a name="l00121"></a>00121 {
<a name="l00122"></a>00122 <span class="keyword">const</span> T angle = sqrt(x*x+y*y+z*z);
<a name="l00123"></a>00123 <span class="keyword">const</span> T s = (sin(angle/2))/angle;
<a name="l00124"></a>00124 <span class="keyword">const</span> T c = cos(angle/2);
<a name="l00125"></a>00125 (*this)[0] = c;
<a name="l00126"></a>00126 (*this)[1] = x * s;
<a name="l00127"></a>00127 (*this)[2] = y * s;
<a name="l00128"></a>00128 (*this)[3] = z * s;
<a name="l00129"></a>00129 }
<a name="l00130"></a>00130 }
<a name="l00131"></a>00131 <span class="comment"></span>
<a name="l00132"></a>00132 <span class="comment"> /** Calculate the "cross" product (or "composed rotation") of two quaternion: this = q1 x q2</span>
<a name="l00133"></a>00133 <span class="comment"> * After the operation, "this" will represent the composed rotations of q1 and q2 (q2 applied "after" q1).</span>
<a name="l00134"></a>00134 <span class="comment"> */</span>
<a name="l00135"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a5734caf259b563fc2bb24b862ea780bd">00135</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a5734caf259b563fc2bb24b862ea780bd" title="Calculate the "cross" product (or "composed rotation") of two quaternion: this = q1 x q2 After the op...">crossProduct</a>(<span class="keyword">const</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html" title="A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...">CQuaternion</a> &q1, <span class="keyword">const</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html" title="A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...">CQuaternion</a> &q2)
<a name="l00136"></a>00136 {
<a name="l00137"></a>00137 this-><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>( q1.r()*q2.r() - q1.x()*q2.x() - q1.y()*q2.y() - q1.z()*q2.z() );
<a name="l00138"></a>00138 this-><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>( q1.r()*q2.x() + q2.r()*q1.x() + q1.y()*q2.z() - q2.y()*q1.z() );
<a name="l00139"></a>00139 this-><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>( q1.r()*q2.y() + q2.r()*q1.y() + q1.z()*q2.x() - q2.z()*q1.x() );
<a name="l00140"></a>00140 this-><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>( q1.r()*q2.z() + q2.r()*q1.z() + q1.x()*q2.y() - q2.x()*q1.y() );
<a name="l00141"></a>00141 this-><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a554edb930bf463d2dc566552823b6ff6" title="Normalize this quaternion, so its norm becomes the unitity.">normalize</a>();
<a name="l00142"></a>00142 }
<a name="l00143"></a>00143 <span class="comment"></span>
<a name="l00144"></a>00144 <span class="comment"> /** Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by this quaternion</span>
<a name="l00145"></a>00145 <span class="comment"> */</span>
<a name="l00146"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a6d4c113c163aea8aa1cf69ac22d378bb">00146</a> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a6d4c113c163aea8aa1cf69ac22d378bb" title="Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by this quaternion.">rotatePoint</a>(<span class="keyword">const</span> <span class="keywordtype">double</span> lx,<span class="keyword">const</span> <span class="keywordtype">double</span> ly,<span class="keyword">const</span> <span class="keywordtype">double</span> lz, <span class="keywordtype">double</span> &gx,<span class="keywordtype">double</span> &gy,<span class="keywordtype">double</span> &gz )<span class="keyword"> const</span>
<a name="l00147"></a>00147 <span class="keyword"> </span>{
<a name="l00148"></a>00148 <span class="keyword">const</span> <span class="keywordtype">double</span> t2 = <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>(); <span class="keyword">const</span> <span class="keywordtype">double</span> t3 = <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>(); <span class="keyword">const</span> <span class="keywordtype">double</span> t4 = <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>(); <span class="keyword">const</span> <span class="keywordtype">double</span> t5 =-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>(); <span class="keyword">const</span> <span class="keywordtype">double</span> t6 = <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>();
<a name="l00149"></a>00149 <span class="keyword">const</span> <span class="keywordtype">double</span> t7 = <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>(); <span class="keyword">const</span> <span class="keywordtype">double</span> t8 =-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>(); <span class="keyword">const</span> <span class="keywordtype">double</span> t9 = <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>(); <span class="keyword">const</span> <span class="keywordtype">double</span> t10=-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>();
<a name="l00150"></a>00150 gx = 2*((t8+ t10)*lx+(t6 - t4)*ly+(t3+t7)*lz)+lx;
<a name="l00151"></a>00151 gy = 2*((t4+ t6)*lx+(t5 +t10)*ly+(t9-t2)*lz)+ly;
<a name="l00152"></a>00152 gz = 2*((t7- t3)*lx+(t2 + t9)*ly+(t5+t8)*lz)+lz;
<a name="l00153"></a>00153 }
<a name="l00154"></a>00154 <span class="comment"></span>
<a name="l00155"></a>00155 <span class="comment"> /** Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by the inverse (conjugate) of this quaternion</span>
<a name="l00156"></a>00156 <span class="comment"> */</span>
<a name="l00157"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ac0a646cae90d37412f950c6c09dc9115">00157</a> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ac0a646cae90d37412f950c6c09dc9115" title="Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by the inverse (conjugate) of this quaternion...">inverseRotatePoint</a>(<span class="keyword">const</span> <span class="keywordtype">double</span> lx,<span class="keyword">const</span> <span class="keywordtype">double</span> ly,<span class="keyword">const</span> <span class="keywordtype">double</span> lz, <span class="keywordtype">double</span> &gx,<span class="keywordtype">double</span> &gy,<span class="keywordtype">double</span> &gz )<span class="keyword"> const</span>
<a name="l00158"></a>00158 <span class="keyword"> </span>{
<a name="l00159"></a>00159 <span class="keyword">const</span> <span class="keywordtype">double</span> t2 =-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>(); <span class="keyword">const</span> <span class="keywordtype">double</span> t3 =-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>(); <span class="keyword">const</span> <span class="keywordtype">double</span> t4 =-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>(); <span class="keyword">const</span> <span class="keywordtype">double</span> t5 =-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>(); <span class="keyword">const</span> <span class="keywordtype">double</span> t6 = <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>();
<a name="l00160"></a>00160 <span class="keyword">const</span> <span class="keywordtype">double</span> t7 = <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>(); <span class="keyword">const</span> <span class="keywordtype">double</span> t8 =-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>(); <span class="keyword">const</span> <span class="keywordtype">double</span> t9 = <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>(); <span class="keyword">const</span> <span class="keywordtype">double</span> t10=-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>();
<a name="l00161"></a>00161 gx = 2*((t8+ t10)*lx+(t6 - t4)*ly+(t3+t7)*lz)+lx;
<a name="l00162"></a>00162 gy = 2*((t4+ t6)*lx+(t5 +t10)*ly+(t9-t2)*lz)+ly;
<a name="l00163"></a>00163 gz = 2*((t7- t3)*lx+(t2 + t9)*ly+(t5+t8)*lz)+lz;
<a name="l00164"></a>00164 }
<a name="l00165"></a>00165 <span class="comment"></span>
<a name="l00166"></a>00166 <span class="comment"> /** Return the squared norm of the quaternion */</span>
<a name="l00167"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a409be4f6b23e66c2bee70da319d05525">00167</a> <span class="keyword">inline</span> <span class="keywordtype">double</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a409be4f6b23e66c2bee70da319d05525" title="Return the squared norm of the quaternion.">normSqr</a>()<span class="keyword"> const </span>{ <span class="keywordflow">return</span> <a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">mrpt::utils::square</a>(<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()) + <a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">mrpt::utils::square</a>(<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()) + <a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">mrpt::utils::square</a>(<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()) + <a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">mrpt::utils::square</a>(<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()); }
<a name="l00168"></a>00168 <span class="comment"></span>
<a name="l00169"></a>00169 <span class="comment"> /** Normalize this quaternion, so its norm becomes the unitity.</span>
<a name="l00170"></a>00170 <span class="comment"> */</span>
<a name="l00171"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a554edb930bf463d2dc566552823b6ff6">00171</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a554edb930bf463d2dc566552823b6ff6" title="Normalize this quaternion, so its norm becomes the unitity.">normalize</a>()
<a name="l00172"></a>00172 {
<a name="l00173"></a>00173 <span class="keyword">const</span> T qq = 1.0/std::sqrt( <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a409be4f6b23e66c2bee70da319d05525" title="Return the squared norm of the quaternion.">normSqr</a>() );
<a name="l00174"></a>00174 <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i=0;i<4;i++)
<a name="l00175"></a>00175 (*<span class="keyword">this</span>)[i] *= qq;
<a name="l00176"></a>00176 }
<a name="l00177"></a>00177 <span class="comment"></span>
<a name="l00178"></a>00178 <span class="comment"> /** Calculate the 4x4 Jacobian of the normalization operation of this quaternion.</span>
<a name="l00179"></a>00179 <span class="comment"> * The output matrix can be a dynamic or fixed size (4x4) matrix.</span>
<a name="l00180"></a>00180 <span class="comment"> */</span>
<a name="l00181"></a>00181 <span class="keyword">template</span> <<span class="keyword">class</span> MATRIXLIKE>
<a name="l00182"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ac4ecfa54ce8f0a876eff9d1626eca609">00182</a> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ac4ecfa54ce8f0a876eff9d1626eca609" title="Calculate the 4x4 Jacobian of the normalization operation of this quaternion.">normalizationJacobian</a>(MATRIXLIKE &J)<span class="keyword"> const</span>
<a name="l00183"></a>00183 <span class="keyword"> </span>{
<a name="l00184"></a>00184 <span class="keyword">const</span> T n = 1.0/std::pow(<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a409be4f6b23e66c2bee70da319d05525" title="Return the squared norm of the quaternion.">normSqr</a>(),T(1.5));
<a name="l00185"></a>00185 J.setSize(4,4);
<a name="l00186"></a>00186 J.get_unsafe(0,0)=<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()+<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()+<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>();
<a name="l00187"></a>00187 J.get_unsafe(0,1)=-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>();
<a name="l00188"></a>00188 J.get_unsafe(0,2)=-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>();
<a name="l00189"></a>00189 J.get_unsafe(0,3)=-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>();
<a name="l00190"></a>00190
<a name="l00191"></a>00191 J.get_unsafe(1,0)=-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>();
<a name="l00192"></a>00192 J.get_unsafe(1,1)=<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()+<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()+<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>();
<a name="l00193"></a>00193 J.get_unsafe(1,2)=-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>();
<a name="l00194"></a>00194 J.get_unsafe(1,3)=-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>();
<a name="l00195"></a>00195
<a name="l00196"></a>00196 J.get_unsafe(2,0)=-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>();
<a name="l00197"></a>00197 J.get_unsafe(2,1)=-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>();
<a name="l00198"></a>00198 J.get_unsafe(2,2)=<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()+<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()+<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>();
<a name="l00199"></a>00199 J.get_unsafe(2,3)=-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>();
<a name="l00200"></a>00200
<a name="l00201"></a>00201 J.get_unsafe(3,0)=-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>();
<a name="l00202"></a>00202 J.get_unsafe(3,1)=-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>();
<a name="l00203"></a>00203 J.get_unsafe(3,2)=-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>();
<a name="l00204"></a>00204 J.get_unsafe(3,3)=<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()+<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()+<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>();
<a name="l00205"></a>00205 J *=n;
<a name="l00206"></a>00206 }
<a name="l00207"></a>00207 <span class="comment"></span>
<a name="l00208"></a>00208 <span class="comment"> /** Compute the Jacobian of the rotation composition operation \f$ p = f(\cdot) = q_{this} \times r \f$, that is the 4x4 matrix \f$ \frac{\partial f}{\partial q_{this} } \f$.</span>
<a name="l00209"></a>00209 <span class="comment"> * The output matrix can be a dynamic or fixed size (4x4) matrix.</span>
<a name="l00210"></a>00210 <span class="comment"> */</span>
<a name="l00211"></a>00211 <span class="keyword">template</span> <<span class="keyword">class</span> MATRIXLIKE>
<a name="l00212"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afffa513496f7f3545af9b437e7661f1a">00212</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afffa513496f7f3545af9b437e7661f1a" title="Compute the Jacobian of the rotation composition operation , that is the 4x4 matrix ...">rotationJacobian</a>(MATRIXLIKE &J)<span class="keyword"> const</span>
<a name="l00213"></a>00213 <span class="keyword"> </span>{
<a name="l00214"></a>00214 J.setSize(4,4);
<a name="l00215"></a>00215 J.get_unsafe(0,0)=<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>(); J.get_unsafe(0,1)=-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>(); J.get_unsafe(0,2)=-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>(); J.get_unsafe(0,3)=-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>();
<a name="l00216"></a>00216 J.get_unsafe(1,0)=<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>(); J.get_unsafe(1,1)= <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>(); J.get_unsafe(1,2)=-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>(); J.get_unsafe(1,3)= <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>();
<a name="l00217"></a>00217 J.get_unsafe(2,0)=<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>(); J.get_unsafe(2,1)= <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>(); J.get_unsafe(2,2)= <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>(); J.get_unsafe(2,3)=-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>();
<a name="l00218"></a>00218 J.get_unsafe(3,0)=<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>(); J.get_unsafe(3,1)=-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>(); J.get_unsafe(3,2)= <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>(); J.get_unsafe(3,3)= <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>();
<a name="l00219"></a>00219 }
<a name="l00220"></a>00220 <span class="comment"></span>
<a name="l00221"></a>00221 <span class="comment"> /** Calculate the 3x3 rotation matrix associated to this quaternion */</span>
<a name="l00222"></a>00222 <span class="keyword">template</span> <<span class="keyword">class</span> MATRIXLIKE>
<a name="l00223"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae56fd8f737fde43ba87f83e858c636d4">00223</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae56fd8f737fde43ba87f83e858c636d4" title="Calculate the 3x3 rotation matrix associated to this quaternion.">rotationMatrix</a>(MATRIXLIKE &M)<span class="keyword"> const</span>
<a name="l00224"></a>00224 <span class="keyword"> </span>{
<a name="l00225"></a>00225 M.setSize(3,3);
<a name="l00226"></a>00226 <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#aaf08e5c7f517bdc2b5eca9439174ccda" title="Fill out the top-left 3x3 block of the given matrix with the rotation matrix associated to this quate...">rotationMatrixNoResize</a>(M);
<a name="l00227"></a>00227 }
<a name="l00228"></a>00228 <span class="comment"></span>
<a name="l00229"></a>00229 <span class="comment"> /** Fill out the top-left 3x3 block of the given matrix with the rotation matrix associated to this quaternion (does not resize the matrix, for that, see rotationMatrix) */</span>
<a name="l00230"></a>00230 <span class="keyword">template</span> <<span class="keyword">class</span> MATRIXLIKE>
<a name="l00231"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#aaf08e5c7f517bdc2b5eca9439174ccda">00231</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#aaf08e5c7f517bdc2b5eca9439174ccda" title="Fill out the top-left 3x3 block of the given matrix with the rotation matrix associated to this quate...">rotationMatrixNoResize</a>(MATRIXLIKE &M)<span class="keyword"> const</span>
<a name="l00232"></a>00232 <span class="keyword"> </span>{
<a name="l00233"></a>00233 M.get_unsafe(0,0)=<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()+<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>(); M.get_unsafe(0,1)=2*(<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>() -<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()); M.get_unsafe(0,2)=2*(<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()+<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>());
<a name="l00234"></a>00234 M.get_unsafe(1,0)=2*(<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()+<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()); M.get_unsafe(1,1)=<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()+<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>(); M.get_unsafe(1,2)=2*(<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>());
<a name="l00235"></a>00235 M.get_unsafe(2,0)=2*(<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()); M.get_unsafe(2,1)=2*(<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()+<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()); M.get_unsafe(2,2)=<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()+<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>();
<a name="l00236"></a>00236 }
<a name="l00237"></a>00237
<a name="l00238"></a>00238 <span class="comment"></span>
<a name="l00239"></a>00239 <span class="comment"> /** Return the conjugate quaternion */</span>
<a name="l00240"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ad72a40f99f104ab7713fa96d5a1e13eb">00240</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a4b6e2c9948bfd52fe7ba57e44e54fa48" title="Return the conjugate quaternion.">conj</a>(<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html" title="A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...">CQuaternion</a> &q_out)<span class="keyword"> const</span>
<a name="l00241"></a>00241 <span class="keyword"> </span>{
<a name="l00242"></a>00242 q_out.r( <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>() );
<a name="l00243"></a>00243 q_out.x(-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>() );
<a name="l00244"></a>00244 q_out.y(-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>() );
<a name="l00245"></a>00245 q_out.z(-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>() );
<a name="l00246"></a>00246 }
<a name="l00247"></a>00247 <span class="comment"></span>
<a name="l00248"></a>00248 <span class="comment"> /** Return the conjugate quaternion */</span>
<a name="l00249"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a4b6e2c9948bfd52fe7ba57e44e54fa48">00249</a> <span class="keyword">inline</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html" title="A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...">CQuaternion</a> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a4b6e2c9948bfd52fe7ba57e44e54fa48" title="Return the conjugate quaternion.">conj</a>()<span class="keyword"> const</span>
<a name="l00250"></a>00250 <span class="keyword"> </span>{
<a name="l00251"></a>00251 <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html" title="A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...">CQuaternion</a> q_aux;
<a name="l00252"></a>00252 <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a4b6e2c9948bfd52fe7ba57e44e54fa48" title="Return the conjugate quaternion.">conj</a>(q_aux);
<a name="l00253"></a>00253 <span class="keywordflow">return</span> q_aux;
<a name="l00254"></a>00254 }
<a name="l00255"></a>00255 <span class="comment"></span>
<a name="l00256"></a>00256 <span class="comment"> /** Return the yaw, pitch & roll angles associated to quaternion</span>
<a name="l00257"></a>00257 <span class="comment"> * \sa For the equations, see The MRPT Book, or see http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/Quaternions.pdf</span>
<a name="l00258"></a>00258 <span class="comment"> * \sa rpy_and_jacobian</span>
<a name="l00259"></a>00259 <span class="comment"> */</span>
<a name="l00260"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a0b5137036a7dcae90740b9d49a55c24f">00260</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a0b5137036a7dcae90740b9d49a55c24f" title="Return the yaw, pitch & roll angles associated to quaternion.">rpy</a>(T &roll, T &pitch, T &yaw)<span class="keyword"> const</span>
<a name="l00261"></a>00261 <span class="keyword"> </span>{
<a name="l00262"></a>00262 <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ad624ce2e5d2880b5b7f96dfbc13293d9" title="Return the yaw, pitch & roll angles associated to quaternion, and (optionally) the 3x4 Jacobian of th...">rpy_and_jacobian</a>(roll,pitch,yaw,static_cast<mrpt::math::CMatrixDouble*>(NULL));
<a name="l00263"></a>00263 }
<a name="l00264"></a>00264 <span class="comment"></span>
<a name="l00265"></a>00265 <span class="comment"> /** Return the yaw, pitch & roll angles associated to quaternion, and (optionally) the 3x4 Jacobian of the transformation.</span>
<a name="l00266"></a>00266 <span class="comment"> * Note that both the angles and the Jacobian have one set of normal equations, plus other special formulas for the degenerated cases of |pitch|=90 degrees.</span>
<a name="l00267"></a>00267 <span class="comment"> * \sa For the equations, see The MRPT Book, or http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/Quaternions.pdf</span>
<a name="l00268"></a>00268 <span class="comment"> * \sa rpy</span>
<a name="l00269"></a>00269 <span class="comment"> */</span>
<a name="l00270"></a>00270 <span class="keyword">template</span> <<span class="keyword">class</span> MATRIXLIKE>
<a name="l00271"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ad624ce2e5d2880b5b7f96dfbc13293d9">00271</a> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ad624ce2e5d2880b5b7f96dfbc13293d9" title="Return the yaw, pitch & roll angles associated to quaternion, and (optionally) the 3x4 Jacobian of th...">rpy_and_jacobian</a>(T &roll, T &pitch, T &yaw, MATRIXLIKE *out_dr_dq = NULL, <span class="keywordtype">bool</span> resize_out_dr_dq_to3x4 = <span class="keyword">true</span> )<span class="keyword"> const</span>
<a name="l00272"></a>00272 <span class="keyword"> </span>{
<a name="l00273"></a>00273 <span class="keyword">using</span> mrpt<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">::utils::square</a>;
<a name="l00274"></a>00274 <span class="keyword">using</span> std::sqrt;
<a name="l00275"></a>00275
<a name="l00276"></a>00276 <span class="keywordflow">if</span> (out_dr_dq && resize_out_dr_dq_to3x4)
<a name="l00277"></a>00277 out_dr_dq->setSize(3,4);
<a name="l00278"></a>00278 <span class="keyword">const</span> T discr = <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()-<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>();
<a name="l00279"></a>00279 <span class="keywordflow">if</span> (fabs(discr)>0.49999)
<a name="l00280"></a>00280 { <span class="comment">// pitch = 90 deg</span>
<a name="l00281"></a>00281 pitch = 0.5*<a class="code" href="mrpt__macros_8h.html#ae71449b1cc6e6250b91f539153a7a0d3">M_PI</a>;
<a name="l00282"></a>00282 yaw =-2*atan2(<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>(),<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>());
<a name="l00283"></a>00283 roll = 0;
<a name="l00284"></a>00284 <span class="keywordflow">if</span> (out_dr_dq) {
<a name="l00285"></a>00285 out_dr_dq->zeros();
<a name="l00286"></a>00286 out_dr_dq->get_unsafe(0,0) = +2/<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>();
<a name="l00287"></a>00287 out_dr_dq->get_unsafe(0,2) = -2*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()/(<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>());
<a name="l00288"></a>00288 }
<a name="l00289"></a>00289 }
<a name="l00290"></a>00290 <span class="keywordflow">else</span> <span class="keywordflow">if</span> (discr<-0.49999)
<a name="l00291"></a>00291 { <span class="comment">// pitch =-90 deg</span>
<a name="l00292"></a>00292 pitch = -0.5*<a class="code" href="mrpt__macros_8h.html#ae71449b1cc6e6250b91f539153a7a0d3">M_PI</a>;
<a name="l00293"></a>00293 yaw =+2*atan2(<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>(),<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>());
<a name="l00294"></a>00294 roll = 0;
<a name="l00295"></a>00295 <span class="keywordflow">if</span> (out_dr_dq) {
<a name="l00296"></a>00296 out_dr_dq->zeros();
<a name="l00297"></a>00297 out_dr_dq->get_unsafe(0,0) = -2/<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>();
<a name="l00298"></a>00298 out_dr_dq->get_unsafe(0,2) = +2*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()/(<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>());
<a name="l00299"></a>00299 }
<a name="l00300"></a>00300 }
<a name="l00301"></a>00301 <span class="keywordflow">else</span>
<a name="l00302"></a>00302 { <span class="comment">// Non-degenerate case:</span>
<a name="l00303"></a>00303 yaw = atan2( 2*(<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()+<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()), 1-2*(<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()+<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()) );
<a name="l00304"></a>00304 pitch = asin ( 2*discr );
<a name="l00305"></a>00305 roll = atan2( 2*(<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()+<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()), 1-2*(<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()+<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()) );
<a name="l00306"></a>00306 <span class="keywordflow">if</span> (out_dr_dq) {
<a name="l00307"></a>00307 <span class="comment">// Auxiliary terms:</span>
<a name="l00308"></a>00308 <span class="keyword">const</span> <span class="keywordtype">double</span> val1=(2*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>() + 2*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>() - 1);
<a name="l00309"></a>00309 <span class="keyword">const</span> <span class="keywordtype">double</span> val12=<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(val1);
<a name="l00310"></a>00310 <span class="keyword">const</span> <span class="keywordtype">double</span> val2=(2*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>() + 2*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>());
<a name="l00311"></a>00311 <span class="keyword">const</span> <span class="keywordtype">double</span> val22=<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(val2);
<a name="l00312"></a>00312 <span class="keyword">const</span> <span class="keywordtype">double</span> xy2 = 2*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>();
<a name="l00313"></a>00313 <span class="keyword">const</span> <span class="keywordtype">double</span> rz2 = 2*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>();
<a name="l00314"></a>00314 <span class="keyword">const</span> <span class="keywordtype">double</span> ry2 = 2*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>();
<a name="l00315"></a>00315 <span class="keyword">const</span> <span class="keywordtype">double</span> val3 = (2*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>() + 2*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>() - 1);
<a name="l00316"></a>00316 <span class="keyword">const</span> <span class="keywordtype">double</span> val4 = ((<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(rz2 + xy2)/<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(val3) + 1)*val3);
<a name="l00317"></a>00317 <span class="keyword">const</span> <span class="keywordtype">double</span> val5 = (4*(rz2 + xy2))/<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(val3);
<a name="l00318"></a>00318 <span class="keyword">const</span> <span class="keywordtype">double</span> val6 = 1.0/(<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(rz2 + xy2)/<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(val3) + 1);
<a name="l00319"></a>00319 <span class="keyword">const</span> <span class="keywordtype">double</span> val7 = 2.0/ sqrt(1 - <a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(ry2 - 2*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()));
<a name="l00320"></a>00320 <span class="keyword">const</span> <span class="keywordtype">double</span> val8 = (val22/val12 + 1);
<a name="l00321"></a>00321 <span class="keyword">const</span> <span class="keywordtype">double</span> val9 = -2.0/val8;
<a name="l00322"></a>00322 <span class="comment">// row 1:</span>
<a name="l00323"></a>00323 out_dr_dq->get_unsafe(0,0) = -2*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()/val4;
<a name="l00324"></a>00324 out_dr_dq->get_unsafe(0,1) = -2*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()/val4;
<a name="l00325"></a>00325 out_dr_dq->get_unsafe(0,2) = -(2*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()/val3 - <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*val5)*val6 ;
<a name="l00326"></a>00326 out_dr_dq->get_unsafe(0,3) = -(2*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()/val3 - <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()*val5)*val6;
<a name="l00327"></a>00327 <span class="comment">// row 2:</span>
<a name="l00328"></a>00328 out_dr_dq->get_unsafe(1,0) = <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*val7 ;
<a name="l00329"></a>00329 out_dr_dq->get_unsafe(1,1) = -<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()*val7 ;
<a name="l00330"></a>00330 out_dr_dq->get_unsafe(1,2) = <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()*val7 ;
<a name="l00331"></a>00331 out_dr_dq->get_unsafe(1,3) = -<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*val7 ;
<a name="l00332"></a>00332 <span class="comment">// row 3:</span>
<a name="l00333"></a>00333 out_dr_dq->get_unsafe(2,0) = val9*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()/val1 ;
<a name="l00334"></a>00334 out_dr_dq->get_unsafe(2,1) = val9*(<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#afa2a076acbb5ccc3cf899747fbe1e1dc" title="Return r coordinate of the quaternion.">r</a>()/val1 - (2*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a3384d0bef93b72e6ded749fc178c479a" title="Return x coordinate of the quaternion.">x</a>()*val2)/val12) ;
<a name="l00335"></a>00335 out_dr_dq->get_unsafe(2,2) = val9*(<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a19dd28d34573a39dea1695900cb7a2e1" title="Return z coordinate of the quaternion.">z</a>()/val1 - (2*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()*val2)/val12) ;
<a name="l00336"></a>00336 out_dr_dq->get_unsafe(2,3) = val9*<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#ae4e990ee1f1d6ac67cc02a68a59ffba9" title="Return y coordinate of the quaternion.">y</a>()/val1 ;
<a name="l00337"></a>00337 }
<a name="l00338"></a>00338 }
<a name="l00339"></a>00339 }
<a name="l00340"></a>00340
<a name="l00341"></a><a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a09bdc4f9912b5759b4b75c6f39c62df4">00341</a> <span class="keyword">inline</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html" title="A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...">CQuaternion</a> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html#a09bdc4f9912b5759b4b75c6f39c62df4">operator * </a>(<span class="keyword">const</span> T &factor)
<a name="l00342"></a>00342 {
<a name="l00343"></a>00343 <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html" title="A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...">CQuaternion</a> q = *<span class="keyword">this</span>;
<a name="l00344"></a>00344 q*=factor;
<a name="l00345"></a>00345 <span class="keywordflow">return</span> q;
<a name="l00346"></a>00346 }
<a name="l00347"></a>00347
<a name="l00348"></a>00348 }; <span class="comment">// end class</span>
<a name="l00349"></a>00349
<a name="l00350"></a><a class="code" href="namespacemrpt_1_1math.html#ad0af3f20fa3e963153e6135431d597e3">00350</a> <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html">CQuaternion<double></a> <a class="code" href="namespacemrpt_1_1math.html#ad0af3f20fa3e963153e6135431d597e3" title="A quaternion of data type "double".">CQuaternionDouble</a>; <span class="comment">//!< A quaternion of data type "double"</span>
<a name="l00351"></a><a class="code" href="namespacemrpt_1_1math.html#a668fef3a57a53170ce8b74d7a3debdce">00351</a> <span class="comment"></span> <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html" title="A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...">CQuaternion<float></a> <a class="code" href="namespacemrpt_1_1math.html#a668fef3a57a53170ce8b74d7a3debdce" title="A quaternion of data type "float".">CQuaternionFloat</a>; <span class="comment">//!< A quaternion of data type "float"</span>
<a name="l00352"></a>00352 <span class="comment"></span>
<a name="l00353"></a>00353 } <span class="comment">// end namespace</span>
<a name="l00354"></a>00354
<a name="l00355"></a>00355 } <span class="comment">// end namespace mrpt</span>
<a name="l00356"></a>00356
<a name="l00357"></a>00357 <span class="preprocessor">#endif</span>
</pre></div></div>
</div>
<br><hr><br> <table border="0" width="100%"> <tr> <td> Page generated by <a href="http://www.doxygen.org" target="_blank">Doxygen 1.7.5</a> for MRPT 0.9.5 SVN: at Sun Sep 25 17:20:18 UTC 2011</td><td></td> <td width="100"> </td> <td width="150"> </td></tr> </table> </body></html>
