RSC  0.16.0
MathUtils.cpp
Go to the documentation of this file.
1 /* ============================================================
2  *
3  * This file is a part of the RSC project.
4  *
5  * Copyright (C) 2010 by Johannes Wienke <jwienke at techfak dot uni-bielefeld dot de>
6  *
7  * This file may be licensed under the terms of the
8  * GNU Lesser General Public License Version 3 (the ``LGPL''),
9  * or (at your option) any later version.
10  *
11  * Software distributed under the License is distributed
12  * on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
13  * express or implied. See the LGPL for the specific language
14  * governing rights and limitations.
15  *
16  * You should have received a copy of the LGPL along with this
17  * program. If not, go to http://www.gnu.org/licenses/lgpl.html
18  * or write to the Free Software Foundation, Inc.,
19  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
20  *
21  * The development of this software was supported by:
22  * CoR-Lab, Research Institute for Cognition and Robotics
23  * Bielefeld University
24  *
25  * ============================================================ */
26 
27 #include "MathUtils.h"
28 
29 #include <cmath>
30 
31 using namespace std;
32 
33 namespace rsc {
34 namespace math {
35 
36 double MathUtils::getTwoPi() {
37  return 2.0 * M_PI;
38 }
39 
40 double MathUtils::getDefaultAnglePrecision() {
41  return 0.001;
42 }
43 
44 double MathUtils::normalizeAngle(const double& angleInRad) {
45 
46  double mod = fmod(angleInRad, getTwoPi());
47 
48  if (mod < 0) {
49  return getTwoPi() + mod;
50  } else {
51  return mod;
52  }
53 
54 }
55 
56 bool MathUtils::isSameAngle(const double& firstInRad,
57  const double& secondInRad, const double& precision) {
58 
59  return cos(normalizeAngle(firstInRad) - normalizeAngle(secondInRad)) > cos(
60  precision);
61 
62 }
63 
64 double MathUtils::radianFromDegree(const double& degree) {
65  return degree * (M_PI / 180.0f);
66 }
67 
68 double MathUtils::degreeFromRadian(const double& radian) {
69  return radian * (180.0f / M_PI);
70 }
71 
72 double MathUtils::getDefaultClosePrecision() {
73  return 0.000001;
74 }
75 
76 bool MathUtils::isClose(const double& a, const double& b,
77  const double& precision) {
78  return abs(a - b) < precision;
79 }
80 
81 }
82 }
STL namespace.