util.cpp

00001 #include <math.h>
00002 #include <string.h>
00003 
00004 #define MAXBUF 32
00005 #define MAXDIGITS 10
00006 
00007 /** 
00008  * Clamp a value (angle) between min (non-inclusive) and max (inclusive)
00009  * e.g. clamp(v, 0, 360) or clamp(v, -180, 180)
00010  */
00011 float clamp(float v, float min, float max, bool flip) 
00012 {
00013     float i;
00014     float f;
00015     float mod = (max - min);
00016 
00017     f = modff((v/mod), &i) * mod;
00018     if (flip) {
00019         if (f > max) f -= mod;
00020         if (f <= min) f += mod;
00021     } else {
00022         if (f < min) f += mod;
00023         if (f >= max) f -= mod;
00024     }
00025     return f;
00026 }
00027 
00028 // convert character to an int
00029 //
00030 int ctoi(char c)
00031 {
00032   int i=-1;
00033   
00034   if (c >= '0' && c <= '9') {
00035     i = c - '0';
00036   }
00037 
00038   //printf("char: %c  int %d\n", c, i); 
00039  
00040   return i;
00041 }
00042 
00043 
00044 // convert string to floating point
00045 //
00046 double cvstof(char *s)
00047 {
00048   double f=0.0;
00049   double mult = 0.1;
00050   bool neg = false;
00051   //char dec = 1;
00052   
00053   // leading spaces
00054   while (*s == ' ' || *s == '\t') {
00055     s++;
00056     if (*s == 0) break;
00057   }
00058 
00059   // What about negative numbers?
00060   if (*s == '-') {
00061     neg = true;
00062     s++;
00063   }
00064 
00065   // before the decimal
00066   //
00067   while (*s != 0) {
00068     if (*s == '.') {
00069       s++;
00070       break;
00071     }
00072     f = (f * 10.0) + (double) ctoi(*s);
00073     s++;
00074   }
00075   // after the decimal
00076   while (*s != 0 && *s >= '0' && *s <= '9') {
00077     f += (double) ctoi(*s) * mult;
00078     mult /= 10;
00079     s++;
00080   }
00081   
00082   // if we were negative...
00083   if (neg) f = -f;
00084   
00085   return f;
00086 }
00087 
00088 
00089 char *cvntos(unsigned long n)
00090 {
00091     static char buf[MAXBUF+1]; // +1 null termination
00092     char *str = buf+MAXBUF;
00093     int i;
00094 
00095     // ensure null termination
00096     *str-- = '\0';
00097 
00098     for (i = 0; i < MAXBUF; i++) {
00099         unsigned long m = n;
00100         n /= 10;
00101         char c = m - 10 * n;
00102         *--str = c + '0';
00103         if (n == 0) break;
00104     }
00105 
00106     return str;
00107 }
00108 
00109 
00110 
00111 char *cvitos(long n)
00112 {
00113     static char buf[MAXBUF+2]; // +1 for sign, +1 for null termination
00114     char *str = buf;
00115 
00116     *str = '\0';
00117     if (n < 0) {
00118         *str++ = '-';
00119         *str = '\0';
00120         n = -n;
00121     }
00122     strcat(str, cvntos(n));
00123 
00124     return buf;
00125 }
00126 
00127 
00128 
00129 char *cvftos(double number, int digits)
00130 {
00131     static char buf[MAXBUF+3+MAXDIGITS]; // +1 for termination, +1 for sign, +1 for ., +MAXDIGITS for digits
00132     char *str = buf;
00133     int i;
00134 
00135     // ensure proper null termination
00136     *str = '\0';
00137 
00138     // Limited buffer space for decimals
00139     if (digits > MAXDIGITS)
00140         digits = MAXDIGITS;
00141 
00142     if (isnan(number)) {
00143         strcpy(buf, "nan");
00144     } else if (isinf(number)) {
00145         strcpy(buf, "inf");
00146     } else if (number > 4294967040.0 || number < -4294967040.0) {  // constant determined empirically
00147         strcpy(buf, "ovf");
00148     } else {
00149 
00150         // Handle negative numbers
00151         if (number < 0.0) {
00152             // Add the sign
00153             strcat(str, "-");
00154             number = -number;
00155         }
00156 
00157         // Round correctly so that print(1.999, 2) prints as "2.00"
00158         double rounding = 0.5;
00159         for (i=0; i < digits; i++)
00160             rounding /= 10.0;
00161         number += rounding;
00162 
00163         // Extract the integer part of the number and print it
00164         unsigned long int_part = (unsigned long)number;
00165         double remainder = number - (double)int_part;
00166 
00167         // Add the integer part
00168         strcat(str, cvntos(int_part));
00169         // Add the decimal point
00170         char *s = str + strlen(str);
00171         *s++ = '.';
00172 
00173         // Extract digits from the remainder one at a time
00174         while (digits-- > 0) {
00175             remainder *= 10.0;
00176             int toPrint = (int) remainder;
00177             *s++ = toPrint + '0';
00178             remainder -= (double) toPrint;
00179         }
00180         *s = '\0';
00181     }
00182 
00183     return str;
00184 }
00185 
00186 
00187 // copy t to s until delimiter is reached
00188 // return location of delimiter+1 in t
00189 // if s or t null, return null
00190 char *split(char *s, char *t, int max, char delim)
00191 {
00192   int i = 0;
00193 
00194   if (s == 0 || t == 0)
00195     return 0;
00196 
00197   while (*t != 0 && *t != '\n' && *t != delim && i < max) {
00198     *s++ = *t++;
00199     i++;
00200   }
00201   *s = 0;
00202 
00203   return t+1;
00204 }