E* Interpolated Graph Replanner Wed Dec 12 18:55:40 2007
00001 /* 00002 * Copyright (C) 2004 00003 * Swiss Federal Institute of Technology, Lausanne. All rights reserved. 00004 * 00005 * Developed at the Autonomous Systems Lab. 00006 * Visit our homepage at http://asl.epfl.ch/ 00007 * 00008 * This program is free software; you can redistribute it and/or modify 00009 * it under the terms of the GNU General Public License as published by 00010 * the Free Software Foundation; either version 2 of the License, or 00011 * (at your option) any later version. 00012 * 00013 * This program is distributed in the hope that it will be useful, 00014 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00015 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00016 * GNU General Public License for more details. 00017 * 00018 * You should have received a copy of the GNU General Public License 00019 * along with this program; if not, write to the Free Software 00020 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 00021 * USA 00022 */ 00023 00024 00025 #ifndef ESTAR_NUMERIC_HPP 00026 #define ESTAR_NUMERIC_HPP 00027 00028 00029 #include <limits> 00030 #include <cmath> 00031 00032 00033 namespace estar { 00034 00035 00036 static const double infinity = std::numeric_limits<double>::max(); 00037 static const double epsilon = 1e3 * std::numeric_limits<double>::epsilon(); 00038 //static const double epsilon = std::numeric_limits<double>::epsilon(); 00039 00040 00050 template<typename T> 00051 T minval(T a, T b) 00052 { 00053 return a < b ? a : b; 00054 } 00055 00056 00060 template<typename T> 00061 T maxval(T a, T b) 00062 { 00063 return a > b ? a : b; 00064 } 00065 00066 00071 template<typename T> 00072 T boundval(T lower_bound, T value, T upper_bound) 00073 { 00074 return maxval(minval(value, upper_bound), lower_bound); 00075 } 00076 00077 00081 template<typename T> 00082 T absval(const T & a) 00083 { 00084 return a > 0 ? T(a) : T(-a); 00085 } 00086 00087 00091 template<typename T> 00092 T square(T x) 00093 { 00094 return x * x; 00095 } 00096 00097 00101 inline double mod2pi(double x) 00102 { 00103 x = fmod(x, 2 * M_PI); 00104 if(x > M_PI) 00105 x -= 2 * M_PI; 00106 else if(x <= - M_PI) 00107 x += 2 * M_PI; 00108 00109 return x; 00110 } 00111 00112 00124 int QuadraticEquation(double a, double b, double c, 00125 double & x1, double & x2); 00126 00127 } 00128 00129 #endif // ESTAR_NUMERIC_HPP