Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

Util.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 /*=======================================================================
00003     Util.h
00004   -----------------------------------------------------------------------
00005     Numerical utility functions
00006   -----------------------------------------------------------------------
00007     Copyright 1999, 2000, 2001 James R. Bruce
00008     School of Computer Science, Carnegie Mellon University
00009   -----------------------------------------------------------------------
00010     This program is free software; you can redistribute it and/or
00011     modify it under the terms of the GNU General Public License as
00012     published by the Free Software Foundation; either version 2 of the
00013     License, or (at your option) any later version.
00014 
00015     This program is distributed in the hope that it will be useful,
00016     but WITHOUT ANY WARRANTY; without even the implied warranty of
00017     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018     General Public License for more details.
00019 
00020     You should have received a copy of the GNU General Public License
00021     along with this program; if not, write to: Free Software Foundation,
00022     Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
00023   =======================================================================*/
00024 
00025 #ifndef __UTIL_H__
00026 #define __UTIL_H__
00027 
00028 #include <math.h>
00029 #include <string.h>
00030 
00031 const double TODEG = (180.0 / M_PI);  // 57.295779513082;
00032 
00033 #if (!defined(__SGI_STL_ALGOBASE_H) && !defined(ALGOBASE_H))
00034 /*
00035 template <class num>
00036 inline num max(num a,num b)
00037 {
00038   return((a > b)? a : b);
00039 }
00040 
00041 template <class num>
00042 inline num min(num a,num b)
00043 {
00044   return((a < b)? a : b);
00045 }
00046 
00047 
00048 template <class data>
00049 inline void swap(data &a,data &b)
00050 {
00051   data t;
00052   t = a;
00053   a = b;
00054   b = t;
00055 }
00056 */
00057 
00058 #endif
00059 
00060 template <class num1,class num2>
00061 inline num1 bound(num1 x,num2 low,num2 high)
00062 {
00063   if(x < low ) x = low;
00064   if(x > high) x = high;
00065   return(x);
00066 }
00067 
00068 template <class num>
00069 inline num max3(num a,num b,num c)
00070 {
00071   if(a > b){
00072     return((a > c)? a : c);
00073   }else{
00074     return((b > c)? b : c);
00075   }
00076 }
00077 
00078 template <class num>
00079 inline num min3(num a,num b,num c)
00080 {
00081   if(a < b){
00082     return((a < c)? a : c);
00083   }else{
00084     return((b < c)? b : c);
00085   }
00086 }
00087 
00088 template <class num>
00089 inline void sort(num &a,num &b,num &c)
00090 {
00091   if(a > b) swap(a,b);
00092   if(b > c) swap(b,c);
00093   if(a > b) swap(a,b);
00094 }
00095 
00096 template <class real>
00097 real fmodt(real x,real m)
00098 // Does a real modulus the *right* way, using
00099 // truncation instead of round to zero.
00100 {
00101   return(x - floor(x / m)*m);
00102 }
00103 
00104 template <class num1,class num2>
00105 inline num1 setbits(num1 val,num2 bits)
00106 {
00107   return(val |= bits);
00108 }
00109 
00110 template <class num1,class num2>
00111 inline num1 clearbits(num1 val,num2 bits)
00112 {
00113   return(val &= ~bits);
00114 }
00115 
00116 template <class real>
00117 real saw(real t)
00118 {
00119   t -= floor(t);
00120   return(2 * ((t < 0.5)? t : (1.0-t)));
00121 }
00122 
00123 template <class data>
00124 inline int mcopy(data *dest,data *src,int num)
00125 {
00126   int i;
00127 
00128   for(i=0; i<num; i++) dest[i] = src[i];
00129 
00130   return(num);
00131 }
00132 
00133 template <class data>
00134 inline data mset(data *dest,data val,int num)
00135 {
00136   int i;
00137 
00138   for(i=0; i<num; i++) dest[i] = val;
00139 
00140   return(val);
00141 }
00142 
00143 template <class data>
00144 inline void mzero(data &d)
00145 {
00146   memset(&d,0,sizeof(d));
00147 }
00148 
00149 
00150 template <class node>
00151 int list_length(node *list)
00152 {
00153   node *p = list;
00154   int num = 0;
00155 
00156   while(p){
00157     num++;
00158     p = p->next;
00159   }
00160 
00161   return(num);
00162 }
00163 
00164 // normalize the supplied angle to lie in (-M_PI, M_PI]
00165 // NOTE: this one passes value and returns the adjusted angle
00166 inline double norm_angle(double angle)
00167 {
00168   angle=fmod(angle,2*M_PI);
00169 
00170   if(angle <   0.0) angle += 2*M_PI;
00171   if(angle >= M_PI) angle -= 2*M_PI;
00172 
00173   return(angle);
00174 }
00175 
00176 // NOTE: This one passed by reference
00177 inline void angle_wrap(double &angle) {
00178   angle=fmod(angle,2*M_PI);
00179   if (angle < 0.0) angle+=2*M_PI;
00180   if (angle >= M_PI) angle-=2*M_PI;
00181 }
00182 
00183 // return the normalized angle halfway between these two
00184 // assumes the arguments are already normalized to (-M_PI, M_PI].
00185 inline double avg_angle(double left, double right) {
00186   if (left < right) {
00187     left += 2*M_PI;
00188   }
00189   double result = (left+right)/2;
00190   if (result > M_PI) {
00191     result -= 2*M_PI;
00192   }
00193   
00194   return result;
00195 }
00196 
00197 // wrapper for handling edge cases in atan2
00198 // TODO: is this really neccessary?
00199 inline double atan2a(double y, double x) {
00200   if (x == 0.0) {
00201     if (y == 0.0) {
00202       return 0.0;
00203     } else if (y > 0) return M_PI/2.0; 
00204     else return -M_PI/2.0;
00205   }
00206   else return atan2(y, x);
00207 }
00208 
00209 inline double atan2b(double y, double x)
00210 {
00211   if(fabs(x) < 1.0E-4){
00212     if(y == 0.0) return(0.0);
00213     return((y > 0)? M_PI/2.0 : -M_PI/2.0);
00214   }
00215   else return atan2(y, x);
00216 }
00217 
00218 template <class num>
00219 inline int sign(num n)
00220 {
00221   if(n > 0) return( 1);
00222   if(n < 0) return(-1);
00223   return(0);
00224 }
00225 
00226 // value less than minimum is clipped to 0.0 to avoid underflow
00227 inline double gaussian_with_min(double x,double min_value)
00228 {
00229   double val;
00230 
00231   val = exp(-(x*x)/2);
00232 
00233   if(val < min_value)
00234     val = 0.0;
00235 
00236   return(val);
00237 }
00238 
00239 const double gaussian_constant = sqrt(2*M_PI);
00240 
00241 inline double gaussian_prob(double x, double mean, double sigma) {
00242   double offset = x-mean;
00243   double top = exp(-offset*offset/(2*sigma*sigma));
00244   double bottom = gaussian_constant*sigma;
00245   
00246   if (bottom < 1e-10) {
00247     if (offset < 1e-10) {
00248       return 1/1e-10;
00249     } else {
00250       return 0;
00251     }
00252   }
00253   
00254   return top/bottom;
00255 }
00256 
00257 /*
00258 template <class node>
00259 node *random_element(node *list)
00260 {
00261   node *p = list;
00262   int num = 0;
00263 
00264   while(p){
00265     num++;
00266     p = p->next;
00267   }
00268 
00269   return(num);
00270 }
00271 */
00272 
00273 /*! @file
00274  * @brief Numerical Utilities
00275  * @author James R. Bruce (Creator)
00276  *
00277  * @verbatim
00278   =======================================================================
00279     Util.h
00280   -----------------------------------------------------------------------
00281     Numerical utility functions
00282   -----------------------------------------------------------------------
00283     Copyright 1999, 2000, 2001 James R. Bruce
00284     School of Computer Science, Carnegie Mellon University
00285   -----------------------------------------------------------------------
00286     This program is free software; you can redistribute it and/or
00287     modify it under the terms of the GNU General Public License as
00288     published by the Free Software Foundation; either version 2 of the
00289     License, or (at your option) any later version.
00290 
00291     This program is distributed in the hope that it will be useful,
00292     but WITHOUT ANY WARRANTY; without even the implied warranty of
00293     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00294     General Public License for more details.
00295 
00296     You should have received a copy of the GNU General Public License
00297     along with this program; if not, write to: Free Software Foundation,
00298     Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
00299     =======================================================================
00300  * @endverbatim
00301  *
00302  * $Author: ejt $
00303  * $Name: tekkotsu-1_4_1 $
00304  * $Revision: 1.4 $
00305  * $State: Exp $
00306  * $Date: 2003/01/23 18:14:11 $
00307  */
00308 
00309 #endif

Tekkotsu v1.4
Generated Sat Jul 19 00:06:32 2003 by Doxygen 1.3.2