<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<title>KatanaNativeInterface: include/common/MathHelperFunctions.h Source File</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<link href="navtree.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="navtree.js"></script>
<script type="text/javascript" src="resize.js"></script>
<script type="text/javascript">
$(document).ready(initResizable);
</script>
<link href="doxygen.css" rel="stylesheet" type="text/css"/>
</head>
<body>
<!-- Generated by Doxygen 1.7.3 -->
<div id="top">
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
<tbody>
<tr style="height: 56px;">
<td style="padding-left: 0.5em;">
<div id="projectname">KatanaNativeInterface <span id="projectnumber">$VERSION$</span></div>
</td>
</tr>
</tbody>
</table>
</div>
<div id="navrow1" class="tabs">
<ul class="tablist">
<li><a href="index.html"><span>Main Page</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><a href="dirs.html"><span>Directories</span></a></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>
<div id="side-nav" class="ui-resizable side-nav-resizable">
<div id="nav-tree">
<div id="nav-tree-contents">
</div>
</div>
<div id="splitbar" style="-moz-user-select:none;"
class="ui-resizable-handle">
</div>
</div>
<script type="text/javascript">
initNavTree('MathHelperFunctions_8h.html','');
</script>
<div id="doc-content">
<div class="header">
<div class="headertitle">
<h1>MathHelperFunctions.h</h1> </div>
</div>
<div class="contents">
<a href="MathHelperFunctions_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"> * Copyright (C) 2006 by Tiziano Mueller *</span>
<a name="l00003"></a>00003 <span class="comment"> * tiziano.mueller@neuronics.ch *</span>
<a name="l00004"></a>00004 <span class="comment"> * *</span>
<a name="l00005"></a>00005 <span class="comment"> * This program is free software; you can redistribute it and/or modify *</span>
<a name="l00006"></a>00006 <span class="comment"> * it under the terms of the GNU General Public License as published by *</span>
<a name="l00007"></a>00007 <span class="comment"> * the Free Software Foundation; either version 2 of the License, or *</span>
<a name="l00008"></a>00008 <span class="comment"> * (at your option) any later version. *</span>
<a name="l00009"></a>00009 <span class="comment"> * *</span>
<a name="l00010"></a>00010 <span class="comment"> * This program is distributed in the hope that it will be useful, *</span>
<a name="l00011"></a>00011 <span class="comment"> * but WITHOUT ANY WARRANTY; without even the implied warranty of *</span>
<a name="l00012"></a>00012 <span class="comment"> * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *</span>
<a name="l00013"></a>00013 <span class="comment"> * GNU General Public License for more details. *</span>
<a name="l00014"></a>00014 <span class="comment"> * *</span>
<a name="l00015"></a>00015 <span class="comment"> * You should have received a copy of the GNU General Public License *</span>
<a name="l00016"></a>00016 <span class="comment"> * along with this program; if not, write to the *</span>
<a name="l00017"></a>00017 <span class="comment"> * Free Software Foundation, Inc., *</span>
<a name="l00018"></a>00018 <span class="comment"> * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *</span>
<a name="l00019"></a>00019 <span class="comment"> ***************************************************************************/</span>
<a name="l00020"></a>00020
<a name="l00021"></a><a class="code" href="MathHelperFunctions_8h.html#ae71449b1cc6e6250b91f539153a7a0d3">00021</a> <span class="preprocessor">#define M_PI 3.14159265358979323846</span>
<a name="l00022"></a>00022 <span class="preprocessor"></span>
<a name="l00023"></a>00023 <span class="preprocessor">#include <cmath></span>
<a name="l00024"></a>00024 <span class="preprocessor">#include <vector></span>
<a name="l00025"></a>00025 <span class="preprocessor">#include <functional></span>
<a name="l00026"></a>00026 <span class="preprocessor">#include <cassert></span>
<a name="l00027"></a>00027
<a name="l00028"></a>00028 <span class="preprocessor">#ifndef KNI_MATH_HELPER_FUNCTIONS</span>
<a name="l00029"></a>00029 <span class="preprocessor"></span><span class="preprocessor">#define KNI_MATH_HELPER_FUNCTIONS</span>
<a name="l00030"></a>00030 <span class="preprocessor"></span>
<a name="l00031"></a>00031
<a name="l00032"></a>00032
<a name="l00033"></a><a class="code" href="namespaceKNI__MHF.html">00033</a> <span class="keyword">namespace </span>KNI_MHF {
<a name="l00034"></a>00034
<a name="l00035"></a>00035 <span class="comment">//*****************************************************************</span>
<a name="l00036"></a>00036
<a name="l00037"></a><a class="code" href="namespaceKNI__MHF.html#a522b4f0516e082c5b14e417b1f2f462f">00037</a> <span class="keyword">template</span><<span class="keyword">typename</span> _T> <span class="keyword">inline</span> <span class="keywordtype">short</span> <a class="code" href="namespaceKNI__MHF.html#a522b4f0516e082c5b14e417b1f2f462f">sign</a>(_T x) { <span class="keywordflow">return</span> ( (x<0) ? -1 : 1 ); }
<a name="l00038"></a>00038
<a name="l00039"></a>00039 <span class="comment">//*****************************************************************</span>
<a name="l00040"></a>00040
<a name="l00044"></a><a class="code" href="structKNI__MHF_1_1unary__precalc__sin.html">00044</a> <span class="keyword">template</span><<span class="keyword">typename</span> _T> <span class="keyword">struct </span><a class="code" href="structKNI__MHF_1_1unary__precalc__sin.html" title="function-object which calculates sinus for n-elements of a container if used together with a STL algo...">unary_precalc_sin</a> : <span class="keyword">public</span> std::unary_function<_T, _T> {
<a name="l00045"></a><a class="code" href="structKNI__MHF_1_1unary__precalc__sin.html#ae85547495f3d42b3382bd3134df6a947">00045</a> _T <a class="code" href="structKNI__MHF_1_1unary__precalc__sin.html#ae85547495f3d42b3382bd3134df6a947">operator() </a>(_T &x) {
<a name="l00046"></a>00046 <span class="keywordflow">return</span> sin(x);
<a name="l00047"></a>00047 }
<a name="l00048"></a>00048 };
<a name="l00049"></a>00049
<a name="l00053"></a><a class="code" href="structKNI__MHF_1_1unary__precalc__cos.html">00053</a> <span class="keyword">template</span><<span class="keyword">typename</span> _T> <span class="keyword">struct </span><a class="code" href="structKNI__MHF_1_1unary__precalc__cos.html">unary_precalc_cos</a> : <span class="keyword">public</span> std::unary_function<_T, _T> {
<a name="l00054"></a><a class="code" href="structKNI__MHF_1_1unary__precalc__cos.html#abad73ed06344ea9a4df43d223a4f64ae">00054</a> _T <a class="code" href="structKNI__MHF_1_1unary__precalc__cos.html#abad73ed06344ea9a4df43d223a4f64ae">operator() </a>(_T x) {
<a name="l00055"></a>00055 <span class="keywordflow">return</span> cos(x);
<a name="l00056"></a>00056 }
<a name="l00057"></a>00057 };
<a name="l00058"></a>00058
<a name="l00059"></a>00059
<a name="l00060"></a>00060
<a name="l00061"></a>00061 <span class="comment">//*****************************************************************</span>
<a name="l00062"></a><a class="code" href="namespaceKNI__MHF.html#addb30747327355c9ff2dcfe58451dc23">00062</a> <span class="keyword">template</span><<span class="keyword">typename</span> _T> <span class="keyword">inline</span> _T <a class="code" href="namespaceKNI__MHF.html#addb30747327355c9ff2dcfe58451dc23">atan1</a>(_T in1, _T in2) {
<a name="l00063"></a>00063
<a name="l00064"></a>00064 <span class="keywordflow">if</span>(in1==0.0)
<a name="l00065"></a>00065 <span class="keywordflow">return</span> <a class="code" href="MathHelperFunctions_8h.html#ae71449b1cc6e6250b91f539153a7a0d3">M_PI</a>+<a class="code" href="namespaceKNI__MHF.html#a522b4f0516e082c5b14e417b1f2f462f">sign</a>(in2)*<a class="code" href="MathHelperFunctions_8h.html#ae71449b1cc6e6250b91f539153a7a0d3">M_PI</a>/2;
<a name="l00066"></a>00066
<a name="l00067"></a>00067 <span class="keywordflow">if</span>(in1<0.0)
<a name="l00068"></a>00068 <span class="keywordflow">return</span> atan(in2/in1)+<a class="code" href="MathHelperFunctions_8h.html#ae71449b1cc6e6250b91f539153a7a0d3">M_PI</a>;
<a name="l00069"></a>00069
<a name="l00070"></a>00070 <span class="keywordflow">if</span>( (in1>0.0) && (in2<0.0) )
<a name="l00071"></a>00071 <span class="keywordflow">return</span> atan(in2/in1)+2.0*<a class="code" href="MathHelperFunctions_8h.html#ae71449b1cc6e6250b91f539153a7a0d3">M_PI</a>;
<a name="l00072"></a>00072
<a name="l00073"></a>00073 <span class="keywordflow">return</span> atan(in2/in1);
<a name="l00074"></a>00074 }
<a name="l00075"></a>00075
<a name="l00076"></a>00076 <span class="comment">//*****************************************************************</span>
<a name="l00077"></a><a class="code" href="namespaceKNI__MHF.html#a52c5a9edb5cd944492f95e7863ff20b3">00077</a> <span class="keyword">template</span><<span class="keyword">typename</span> _T> <span class="keyword">inline</span> _T <a class="code" href="namespaceKNI__MHF.html#a52c5a9edb5cd944492f95e7863ff20b3">acotan</a>(<span class="keyword">const</span> _T in) {
<a name="l00078"></a>00078 <span class="keywordflow">if</span>(in == 0.0)
<a name="l00079"></a>00079 <span class="keywordflow">return</span> <a class="code" href="MathHelperFunctions_8h.html#ae71449b1cc6e6250b91f539153a7a0d3">M_PI</a>/2;
<a name="l00080"></a>00080 <span class="keywordflow">else</span>
<a name="l00081"></a>00081 <span class="keywordflow">return</span> atan(1/in);
<a name="l00082"></a>00082 }
<a name="l00083"></a>00083
<a name="l00084"></a>00084 <span class="comment">//*************************************************</span>
<a name="l00085"></a><a class="code" href="namespaceKNI__MHF.html#a205b65427a897c6a9017c27f585d60c9">00085</a> <span class="keyword">template</span><<span class="keyword">typename</span> _T> <span class="keyword">inline</span> _T <a class="code" href="namespaceKNI__MHF.html#a205b65427a897c6a9017c27f585d60c9">atan0</a>(<span class="keyword">const</span> _T in1, <span class="keyword">const</span> _T in2) {
<a name="l00086"></a>00086 <span class="keywordflow">if</span>(in1 == 0.0)
<a name="l00087"></a>00087 <span class="keywordflow">return</span> <a class="code" href="MathHelperFunctions_8h.html#ae71449b1cc6e6250b91f539153a7a0d3">M_PI</a>/2;
<a name="l00088"></a>00088 <span class="keywordflow">return</span> atan(in2/in1);
<a name="l00089"></a>00089 }
<a name="l00090"></a>00090
<a name="l00091"></a>00091 <span class="comment">//*************************************************</span>
<a name="l00092"></a><a class="code" href="namespaceKNI__MHF.html#a2c795afb8d206422e3aa2e5e5fe812e8">00092</a> <span class="keyword">template</span><<span class="keyword">typename</span> _T> <span class="keyword">inline</span> _T <a class="code" href="namespaceKNI__MHF.html#a2c795afb8d206422e3aa2e5e5fe812e8">pow2</a>(<span class="keyword">const</span> _T in) {
<a name="l00093"></a>00093 <span class="keywordflow">return</span> pow(in,2);
<a name="l00094"></a>00094 }
<a name="l00095"></a>00095
<a name="l00096"></a>00096
<a name="l00100"></a><a class="code" href="namespaceKNI__MHF.html#aead8f230476531d670a146a55f3fd05d">00100</a> <span class="keyword">template</span><<span class="keyword">typename</span> _T> <span class="keyword">inline</span> _T <a class="code" href="namespaceKNI__MHF.html#aead8f230476531d670a146a55f3fd05d" title="conversion from radian to degree">rad2deg</a>(<span class="keyword">const</span> _T a) {
<a name="l00101"></a>00101 <span class="keywordflow">return</span> a*(180.0/<a class="code" href="MathHelperFunctions_8h.html#ae71449b1cc6e6250b91f539153a7a0d3">M_PI</a>);
<a name="l00102"></a>00102 }
<a name="l00103"></a>00103
<a name="l00107"></a><a class="code" href="structKNI__MHF_1_1unary__rad2deg.html">00107</a> <span class="keyword">template</span><<span class="keyword">typename</span> _T> <span class="keyword">struct </span><a class="code" href="structKNI__MHF_1_1unary__rad2deg.html" title="a function-object version of rad2deg">unary_rad2deg</a> : <span class="keyword">public</span> std::unary_function<_T, _T> {
<a name="l00108"></a><a class="code" href="structKNI__MHF_1_1unary__rad2deg.html#a3c2d9994482410b1a0b41f318f66640e">00108</a> _T <a class="code" href="structKNI__MHF_1_1unary__rad2deg.html#a3c2d9994482410b1a0b41f318f66640e">operator() </a>(<span class="keyword">const</span> _T a) { <span class="keywordflow">return</span> <a class="code" href="namespaceKNI__MHF.html#aead8f230476531d670a146a55f3fd05d" title="conversion from radian to degree">rad2deg</a>(a); }
<a name="l00109"></a>00109 };
<a name="l00110"></a>00110
<a name="l00114"></a><a class="code" href="namespaceKNI__MHF.html#addad2ed2f4a9f6f13a56554df726bde5">00114</a> <span class="keyword">template</span><<span class="keyword">typename</span> _T> <span class="keyword">inline</span> _T <a class="code" href="namespaceKNI__MHF.html#addad2ed2f4a9f6f13a56554df726bde5" title="conversion from degree to radian">deg2rad</a>(<span class="keyword">const</span> _T a) {
<a name="l00115"></a>00115 <span class="keywordflow">return</span> a*(<a class="code" href="MathHelperFunctions_8h.html#ae71449b1cc6e6250b91f539153a7a0d3">M_PI</a>/180.0);
<a name="l00116"></a>00116 }
<a name="l00117"></a>00117
<a name="l00121"></a><a class="code" href="structKNI__MHF_1_1unary__deg2rad.html">00121</a> <span class="keyword">template</span><<span class="keyword">typename</span> _T> <span class="keyword">struct </span><a class="code" href="structKNI__MHF_1_1unary__deg2rad.html" title="a function-object version of rad2deg">unary_deg2rad</a> : <span class="keyword">public</span> std::unary_function<_T, _T> {
<a name="l00122"></a><a class="code" href="structKNI__MHF_1_1unary__deg2rad.html#abdad19791a23807376fc099eb4eb9392">00122</a> _T <a class="code" href="structKNI__MHF_1_1unary__deg2rad.html#abdad19791a23807376fc099eb4eb9392">operator() </a>(<span class="keyword">const</span> _T a) { <a class="code" href="namespaceKNI__MHF.html#addad2ed2f4a9f6f13a56554df726bde5" title="conversion from degree to radian">deg2rad</a>(a); }
<a name="l00123"></a>00123 };
<a name="l00124"></a>00124
<a name="l00125"></a>00125 <span class="comment">//*************************************************</span>
<a name="l00126"></a><a class="code" href="namespaceKNI__MHF.html#a202b976b4e5b854db64e26fd89f627aa">00126</a> <span class="keyword">template</span><<span class="keyword">typename</span> _T> _T <span class="keyword">inline</span> <a class="code" href="namespaceKNI__MHF.html#a202b976b4e5b854db64e26fd89f627aa">anglereduce</a>(<span class="keyword">const</span> _T a) {
<a name="l00127"></a>00127 <span class="keywordflow">return</span> a - floor( a/(2*<a class="code" href="MathHelperFunctions_8h.html#ae71449b1cc6e6250b91f539153a7a0d3">M_PI</a>) )*2*<a class="code" href="MathHelperFunctions_8h.html#ae71449b1cc6e6250b91f539153a7a0d3">M_PI</a>;
<a name="l00128"></a>00128 }
<a name="l00129"></a>00129 <span class="comment">//*************************************************</span>
<a name="l00130"></a>00130
<a name="l00134"></a><a class="code" href="namespaceKNI__MHF.html#a1821c355f23ed1eebd67a0d1b0805db4">00134</a> <span class="keyword">template</span><<span class="keyword">typename</span> _angleT, <span class="keyword">typename</span> _encT> <span class="keyword">inline</span> _encT <a class="code" href="namespaceKNI__MHF.html#a1821c355f23ed1eebd67a0d1b0805db4" title="converts absolute angles in radian to encoders.">rad2enc</a>(_angleT <span class="keyword">const</span>& angle, _angleT <span class="keyword">const</span>& angleOffset, _encT <span class="keyword">const</span>& epc, _encT <span class="keyword">const</span>& encOffset, _encT <span class="keyword">const</span>& rotDir) {
<a name="l00135"></a>00135 <span class="comment">// converting all parameters to _angleT (usually =double)</span>
<a name="l00136"></a>00136 _angleT _epc = epc, _rotDir = rotDir, _angleOffset = angleOffset, _encOffset = encOffset;
<a name="l00137"></a>00137 <span class="preprocessor">#ifdef WIN32</span>
<a name="l00138"></a>00138 <span class="preprocessor"></span> <span class="keywordtype">double</span> temp = _encOffset + (_angleOffset-angle)*_epc*_rotDir/(2*<a class="code" href="MathHelperFunctions_8h.html#ae71449b1cc6e6250b91f539153a7a0d3">M_PI</a>);
<a name="l00139"></a>00139 <span class="keywordflow">return</span> <span class="keyword">static_cast<</span>_encT<span class="keyword">></span>( (temp >= 0) ? floor(temp + 0.5) : floor(temp - 0.5) );
<a name="l00140"></a>00140 <span class="preprocessor">#else</span>
<a name="l00141"></a>00141 <span class="preprocessor"></span> <span class="keywordflow">return</span> <span class="keyword">static_cast<</span>_encT<span class="keyword">></span>( round( _encOffset + (_angleOffset-angle)*_epc*_rotDir/(2*<a class="code" href="MathHelperFunctions_8h.html#ae71449b1cc6e6250b91f539153a7a0d3">M_PI</a>) ) );
<a name="l00142"></a>00142 <span class="preprocessor">#endif</span>
<a name="l00143"></a>00143 <span class="preprocessor"></span>}
<a name="l00144"></a>00144
<a name="l00148"></a><a class="code" href="namespaceKNI__MHF.html#a759be0245c1703c4413960d6431519c8">00148</a> <span class="keyword">template</span><<span class="keyword">typename</span> _angleT, <span class="keyword">typename</span> _encT> <span class="keyword">inline</span> _angleT <a class="code" href="namespaceKNI__MHF.html#a759be0245c1703c4413960d6431519c8" title="converts encoders to absolute angles in radian">enc2rad</a>(_encT <span class="keyword">const</span>& enc, _angleT <span class="keyword">const</span>& angleOffset, _encT <span class="keyword">const</span>& epc, _encT <span class="keyword">const</span>& encOffset, _encT <span class="keyword">const</span>& rotDir) {
<a name="l00149"></a>00149 <span class="comment">// converting all parameters to _angleT (usually = double)</span>
<a name="l00150"></a>00150 _angleT _epc = epc, _rotDir = rotDir, _angleOffset = angleOffset, _encOffset = encOffset, _enc = enc;
<a name="l00151"></a>00151 <span class="keywordflow">return</span> _angleOffset - (_enc - _encOffset)*2.0*<a class="code" href="MathHelperFunctions_8h.html#ae71449b1cc6e6250b91f539153a7a0d3">M_PI</a>/(_epc*_rotDir);
<a name="l00152"></a>00152 }
<a name="l00153"></a>00153
<a name="l00157"></a><a class="code" href="namespaceKNI__MHF.html#abae2c20b6da9322319c82d64724eb163">00157</a> <span class="keyword">inline</span> <span class="keywordtype">double</span> <a class="code" href="namespaceKNI__MHF.html#abae2c20b6da9322319c82d64724eb163" title="Find the first equal angle.">findFirstEqualAngle</a>(<span class="keywordtype">double</span> cosValue, <span class="keywordtype">double</span> sinValue, <span class="keywordtype">double</span> tolerance) {
<a name="l00158"></a>00158 <span class="keywordtype">double</span> v1[2], v2[2];
<a name="l00159"></a>00159
<a name="l00160"></a>00160 v1[0] = acos(cosValue);
<a name="l00161"></a>00161 v1[1] = -v1[0];
<a name="l00162"></a>00162 v2[0] = asin(sinValue);
<a name="l00163"></a>00163 v2[1] = <a class="code" href="MathHelperFunctions_8h.html#ae71449b1cc6e6250b91f539153a7a0d3">M_PI</a> - v2[0];
<a name="l00164"></a>00164
<a name="l00165"></a>00165 <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=0;i<2;++i) {
<a name="l00166"></a>00166 <span class="keywordflow">for</span>(<span class="keywordtype">int</span> j=0;j<2;++j) {
<a name="l00167"></a>00167 <span class="keywordflow">if</span>(std::abs(<a class="code" href="namespaceKNI__MHF.html#a202b976b4e5b854db64e26fd89f627aa">anglereduce</a>(v1[i]) - <a class="code" href="namespaceKNI__MHF.html#a202b976b4e5b854db64e26fd89f627aa">anglereduce</a>(v2[j])) < tolerance) <span class="keywordflow">return</span> v1[i];
<a name="l00168"></a>00168 }
<a name="l00169"></a>00169 }
<a name="l00170"></a>00170 assert(!<span class="stringliteral">"precondition for findFirstEqualAngle failed -> no equal angles found"</span>);
<a name="l00171"></a>00171 <span class="keywordflow">return</span> 0;
<a name="l00172"></a>00172 }
<a name="l00173"></a>00173
<a name="l00174"></a>00174
<a name="l00175"></a>00175 }
<a name="l00176"></a>00176
<a name="l00177"></a>00177
<a name="l00178"></a>00178
<a name="l00179"></a>00179 <span class="preprocessor">#endif</span>
</pre></div></div>
</div>
<div id="nav-path" class="navpath">
<ul>
<li class="navelem"><a class="el" href="MathHelperFunctions_8h.html">MathHelperFunctions.h</a> </li>
<li class="footer">Generated on Tue Feb 8 2011 for KatanaNativeInterface by 
<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/></a> 1.7.3 </li>
</ul>
</div>
</body>
</html>
