2013-08-08 05:34:54 +08:00
|
|
|
#ifndef UTILITY_H
|
|
|
|
#define UTILITY_H
|
|
|
|
|
|
|
|
#include <iostream>
|
|
|
|
#include <ctime>
|
|
|
|
#include <limits>
|
|
|
|
#undef near
|
|
|
|
|
|
|
|
#include <vector>
|
|
|
|
#include <fstream>
|
|
|
|
#include <cstring>
|
|
|
|
#include <string>
|
|
|
|
#include <sstream>
|
|
|
|
#include <iomanip>
|
|
|
|
#include <algorithm>
|
2018-05-08 12:05:18 +08:00
|
|
|
#include <cmath>
|
2013-08-08 05:34:54 +08:00
|
|
|
|
|
|
|
#include "ATC_Error.h"
|
|
|
|
|
|
|
|
namespace ATC_Utility
|
|
|
|
{
|
|
|
|
/** constants */
|
2014-11-21 02:59:03 +08:00
|
|
|
static const double Pi_ = 4.0*atan(1.0);
|
|
|
|
static const double Big_ = 1.e20;
|
|
|
|
const static double parsetol_ = 1.0e-8;
|
|
|
|
//const static double parsetol_ = 1.0e-10;
|
|
|
|
const static double parsebig_ = 1.0e10;
|
2013-08-08 05:34:54 +08:00
|
|
|
|
|
|
|
/** scalar triple product */
|
|
|
|
inline double det3(double * v1, double * v2, double * v3) {
|
|
|
|
return
|
|
|
|
-v1[2]*v2[1]*v3[0] + v1[1]*v2[2]*v3[0] + v1[2]*v2[0]*v3[1]
|
|
|
|
-v1[0]*v2[2]*v3[1] - v1[1]*v2[0]*v3[2] + v1[0]*v2[1]*v3[2];
|
|
|
|
}
|
|
|
|
inline void plane_coords(int i, int & i1, int & i2) {
|
|
|
|
if (i==0) { i1 = 1; i2 = 2; }
|
|
|
|
else if (i==1) { i1 = 2; i2 = 0; }
|
|
|
|
else if (i==2) { i1 = 0; i2 = 1; }
|
|
|
|
}
|
|
|
|
/** 2d cross product */
|
|
|
|
inline double cross2(double * v1, double * v2) {
|
|
|
|
return v1[0]*v2[1] - v1[1]*v2[0];
|
|
|
|
}
|
|
|
|
/** 3d cross product */
|
|
|
|
inline void cross3(double * v1, double * v2, double * v) {
|
|
|
|
v[0] = v1[1]*v2[2] - v1[2]*v2[1];
|
|
|
|
v[1] = v1[2]*v2[0] - v1[0]*v2[3];
|
|
|
|
v[2] = v1[0]*v2[1] - v1[1]*v2[0];
|
|
|
|
}
|
|
|
|
inline double norm3(double * v) {return sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]); }
|
|
|
|
|
|
|
|
inline int sgn(double x) { return (int) ((x>0) - (x<0)); }
|
2014-11-21 02:59:03 +08:00
|
|
|
// template <typename T> int sgn(T val) { return (T(0) < val) - (val < T(0)); }
|
2013-08-08 05:34:54 +08:00
|
|
|
inline int rnd(double x) { return (int) (x+sgn(x)*0.5); }
|
|
|
|
|
|
|
|
/** Compares doubles acceptably */
|
|
|
|
inline bool dbl_geq(double dblL, double dblR, int tolMult=2)
|
|
|
|
{
|
|
|
|
return ( (dblL > dblR) ||
|
|
|
|
((dblL <= (dblR + \
|
|
|
|
std::numeric_limits<double>::epsilon() * \
|
2019-10-20 23:24:13 +08:00
|
|
|
tolMult * std::max(fabs(dblL), fabs(dblR)))) &&
|
2013-08-08 05:34:54 +08:00
|
|
|
(dblR <= (dblL + \
|
|
|
|
std::numeric_limits<double>::epsilon() * \
|
2019-10-20 23:24:13 +08:00
|
|
|
tolMult * std::max(fabs(dblL), fabs(dblR))))));
|
2013-08-08 05:34:54 +08:00
|
|
|
}
|
|
|
|
|
2014-11-21 02:59:03 +08:00
|
|
|
inline double tolerance(double x, double tol = parsetol_) {
|
|
|
|
return std::max(tol,tol*fabs(x));
|
|
|
|
}
|
|
|
|
inline double nudge_up(double x) { return x+tolerance(x); }
|
|
|
|
inline double nudge_down(double x) { return x-tolerance(x); }
|
2013-08-08 05:34:54 +08:00
|
|
|
inline double parse_min(const char * arg) {
|
2014-11-21 02:59:03 +08:00
|
|
|
if (std::strcmp(arg,"INF") == 0) return -parsebig_;
|
|
|
|
else if (std::strcmp(arg,"-INF") == 0) return -parsebig_;
|
|
|
|
else return (atof(arg));
|
|
|
|
}
|
2013-08-08 05:34:54 +08:00
|
|
|
inline double parse_max(const char * arg) {
|
2014-11-21 02:59:03 +08:00
|
|
|
if (std::strcmp(arg,"INF") == 0) return parsebig_;
|
|
|
|
else return (atof(arg));
|
2013-08-08 05:34:54 +08:00
|
|
|
}
|
|
|
|
inline double parse_minmax(const char * arg) {
|
2014-11-21 02:59:03 +08:00
|
|
|
if (std::strcmp(arg,"-INF") == 0) return -parsebig_;
|
|
|
|
else if (std::strcmp(arg,"INF") == 0) return parsebig_;
|
|
|
|
else return atof(arg);
|
|
|
|
}
|
2013-08-08 05:34:54 +08:00
|
|
|
inline void split_values(double & min, double & max) {
|
2014-11-21 02:59:03 +08:00
|
|
|
min = nudge_down(min);
|
|
|
|
max = nudge_up(max);
|
2013-08-08 05:34:54 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/** Returns true if the value v is between min & max */
|
|
|
|
template<typename T>
|
|
|
|
inline bool in_range(const T &v, const T &min, const T &max)
|
|
|
|
{
|
|
|
|
return v >= min && v <= max;
|
|
|
|
}
|
|
|
|
/** Returns true if the value v is between min & max within tolerance TOL */
|
|
|
|
template<typename T>
|
|
|
|
inline bool in_range(const T &v, const T &min, const T &max, const T &tol)
|
|
|
|
{
|
|
|
|
return in_range(v, min-tol, max+tol);
|
|
|
|
}
|
|
|
|
/** Returns the value with the larger absolute value */
|
|
|
|
template<typename T>
|
|
|
|
inline T max_abs(const T &a, const T &b)
|
|
|
|
{
|
|
|
|
return (a*a > b*b) ? a : b;
|
|
|
|
}
|
|
|
|
/** Returns the value with the smaller absolute value */
|
|
|
|
template<typename T>
|
|
|
|
inline T min_abs(const T &a, const T &b)
|
|
|
|
{
|
|
|
|
return (a*a < b*b) ? a : b;
|
|
|
|
}
|
|
|
|
/** A simple Matlab like timing function */
|
|
|
|
inline double tictoc()
|
|
|
|
{
|
|
|
|
double t_new = clock() / (double) CLOCKS_PER_SEC;
|
|
|
|
static double t = t_new;
|
|
|
|
double dt = t_new - t;
|
|
|
|
t = t_new;
|
|
|
|
return dt;
|
|
|
|
}
|
|
|
|
/** A simple timer */
|
|
|
|
inline double timer()
|
|
|
|
{
|
|
|
|
double t_new = clock() / (double) CLOCKS_PER_SEC;
|
|
|
|
static double t = t_new; // done once at first time the function is evoked
|
|
|
|
double dt = t_new - t;
|
|
|
|
return dt;
|
|
|
|
}
|
|
|
|
/** Binary search between low & high for value (assumes array is sorted) */
|
|
|
|
|
|
|
|
template<typename T>
|
|
|
|
inline int search_sorted(const T* A, T value, int low, int high)
|
|
|
|
{
|
|
|
|
int mid;
|
|
|
|
while (low < high)
|
|
|
|
{
|
|
|
|
mid = (low + high) >> 1; // set mid to mean of high and low, ">>" is a bit shift which divides by 2, rounding down
|
|
|
|
if (A[mid] > value) high = mid; // mid was too high, reduce high
|
|
|
|
else if (A[mid] < value) low = mid+1; // mid was too low, increase low
|
|
|
|
else return mid;
|
|
|
|
}
|
|
|
|
return -1; // value not found in array, return -1
|
|
|
|
}
|
|
|
|
/** Flat search between low & high for value (assumes array is NOT sorted) */
|
|
|
|
template<typename T>
|
|
|
|
inline int search_unsorted(const T* A, T value, int low, int high)
|
|
|
|
{
|
|
|
|
for (int i=low; i<high; i++) if (A[i] == value) return i;
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
/** Regular swap */
|
|
|
|
template<typename T>
|
|
|
|
inline void swap(T &a, T &b)
|
|
|
|
{
|
|
|
|
T temp = a;
|
|
|
|
a = b;
|
|
|
|
b = temp;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
//===================================================================
|
|
|
|
/** A collection of string convesion/manipulation routines */
|
|
|
|
//===================================================================
|
|
|
|
|
2013-08-22 07:06:07 +08:00
|
|
|
|
2013-08-08 05:34:54 +08:00
|
|
|
/** converts anything that has iostream::<< defined to a string */
|
|
|
|
template<typename T>
|
2013-08-22 07:06:07 +08:00
|
|
|
inline std::string to_string(const T &v, int precision=0)
|
2013-08-08 05:34:54 +08:00
|
|
|
{
|
|
|
|
std::ostringstream out;
|
2014-11-21 02:59:03 +08:00
|
|
|
if (precision) {
|
|
|
|
out << std::setprecision(precision);
|
|
|
|
out << std::setw(precision+3);
|
|
|
|
out << std::showpoint;
|
|
|
|
}
|
2013-08-08 05:34:54 +08:00
|
|
|
out << v;
|
2014-11-21 02:59:03 +08:00
|
|
|
out << std::noshowpoint;
|
2013-08-08 05:34:54 +08:00
|
|
|
return out.str();
|
|
|
|
}
|
|
|
|
|
2014-11-21 02:59:03 +08:00
|
|
|
/** formatted double to a string */
|
|
|
|
inline std::string to_string(int precision, const double v)
|
|
|
|
{
|
|
|
|
char b[50];
|
|
|
|
sprintf(b, "%*.*f",4+precision,precision, v);
|
|
|
|
std::string s(b);
|
|
|
|
return s;
|
|
|
|
}
|
|
|
|
|
2013-08-08 05:34:54 +08:00
|
|
|
/** conversion to string */
|
2013-08-22 07:06:07 +08:00
|
|
|
inline std::string true_false(const double &v)
|
2013-08-08 05:34:54 +08:00
|
|
|
{
|
|
|
|
if (v) return "TRUE";
|
|
|
|
else return "FALSE";
|
|
|
|
}
|
|
|
|
|
|
|
|
/** test conversion to double */
|
2013-08-22 07:06:07 +08:00
|
|
|
inline bool is_numeric(const std::string &s)
|
2013-08-08 05:34:54 +08:00
|
|
|
{
|
|
|
|
double v;
|
|
|
|
std::istringstream in(s);
|
2016-03-22 00:10:48 +08:00
|
|
|
in >> v;
|
|
|
|
// in.good() == true indicates, that not the whole string was converted.
|
|
|
|
return !(in.fail() || in.good());
|
2013-08-08 05:34:54 +08:00
|
|
|
}
|
|
|
|
|
2013-08-22 07:06:07 +08:00
|
|
|
/** convert a string to anything that has iostream::>> defined, second arg is to disambibuate type */
|
2013-08-08 05:34:54 +08:00
|
|
|
template<typename T>
|
2013-08-22 07:06:07 +08:00
|
|
|
inline T str2T(const std::string &s, T v)
|
2013-08-08 05:34:54 +08:00
|
|
|
{
|
|
|
|
std::istringstream in(s);
|
|
|
|
if (!(in >> v)) throw ATC::ATC_Error("str2T invalid string conversion");
|
|
|
|
return v;
|
|
|
|
}
|
|
|
|
|
|
|
|
/** convert a string to a double */
|
2013-08-22 07:06:07 +08:00
|
|
|
inline double str2dbl(const std::string &s) { return str2T(s, double(0.0)); }
|
2013-08-08 05:34:54 +08:00
|
|
|
|
|
|
|
/** tests if conversion to double is possible */
|
2013-08-22 07:06:07 +08:00
|
|
|
inline bool is_dbl(const std::string &s)
|
2013-08-08 05:34:54 +08:00
|
|
|
{
|
|
|
|
char *endptr;
|
|
|
|
strtod(s.c_str(), &endptr);
|
|
|
|
if(endptr != NULL && *endptr == '\0') return true;
|
|
|
|
return false;
|
|
|
|
}
|
2014-11-21 02:59:03 +08:00
|
|
|
|
|
|
|
inline bool is_number(const std::string& s)
|
|
|
|
{
|
|
|
|
std::string::const_iterator it = s.begin();
|
|
|
|
while (it != s.end() && std::isdigit(*it)) ++it;
|
|
|
|
return !s.empty() && it == s.end();
|
|
|
|
}
|
2013-08-08 05:34:54 +08:00
|
|
|
|
|
|
|
/** convert a string to an int */
|
2013-08-22 07:06:07 +08:00
|
|
|
inline int str2int(const std::string &s) { return str2T(s, int(4)); }
|
2013-08-08 05:34:54 +08:00
|
|
|
|
|
|
|
//* replaces all characters in a set of characters with a character
|
|
|
|
//* @param str input string
|
|
|
|
//* @param *s pointer to array of characters that will be replaced
|
|
|
|
//* @param r character to replace characters in s[] with
|
2013-08-22 07:06:07 +08:00
|
|
|
static void replace_all_of(std::string &str, const char *s, const char r)
|
2013-08-08 05:34:54 +08:00
|
|
|
{
|
|
|
|
size_t found;
|
|
|
|
found = str.find_first_of(s);
|
2013-08-22 07:06:07 +08:00
|
|
|
while (found != std::string::npos)
|
2013-08-08 05:34:54 +08:00
|
|
|
{
|
|
|
|
str[found] = r;
|
|
|
|
found = str.find_first_of(s, found+1);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/** converts the string to lowercase */
|
2013-08-22 07:06:07 +08:00
|
|
|
inline std::string& to_lower(std::string &s)
|
2013-08-08 05:34:54 +08:00
|
|
|
{
|
2013-08-22 07:06:07 +08:00
|
|
|
std::transform(s.begin(),s.end(),s.begin(),static_cast<int(*)(int)>(tolower));
|
2013-08-08 05:34:54 +08:00
|
|
|
return s;
|
|
|
|
}
|
|
|
|
|
|
|
|
/** converts the string to uppercase */
|
2013-08-22 07:06:07 +08:00
|
|
|
inline std::string& to_upper(std::string &s)
|
2013-08-08 05:34:54 +08:00
|
|
|
{
|
2013-08-22 07:06:07 +08:00
|
|
|
std::transform(s.begin(),s.end(),s.begin(),static_cast<int(*)(int)>(toupper));
|
2013-08-08 05:34:54 +08:00
|
|
|
return s;
|
|
|
|
}
|
|
|
|
|
|
|
|
/** removes any whitespace from the beginning or end of string */
|
2013-08-22 07:06:07 +08:00
|
|
|
static std::string& trim(std::string &s)
|
2013-08-08 05:34:54 +08:00
|
|
|
{
|
|
|
|
if (s.empty()) return s;
|
|
|
|
size_t found = s.find_last_not_of(" \t\n");
|
|
|
|
if (found < s.size()-1) s.erase(found+1);
|
|
|
|
found = s.find_first_not_of(" \t\n");
|
2013-08-22 07:06:07 +08:00
|
|
|
if (found != std::string::npos) s = s.substr(found);
|
2013-08-08 05:34:54 +08:00
|
|
|
return s;
|
|
|
|
}
|
|
|
|
|
|
|
|
/** splits delimited string into a vector of strings */
|
2013-08-22 07:06:07 +08:00
|
|
|
static void split(const std::string &s, std::vector<std::string> &ss, char del=' ')
|
2013-08-08 05:34:54 +08:00
|
|
|
{
|
|
|
|
ss.clear();
|
|
|
|
size_t begin=0, end=0;
|
2013-08-22 07:06:07 +08:00
|
|
|
while (end != std::string::npos)
|
2013-08-08 05:34:54 +08:00
|
|
|
{
|
|
|
|
begin = s.find_first_not_of(del, end); // find beginning of fragment
|
|
|
|
end = s.find_first_of(del,begin);
|
2013-08-22 07:06:07 +08:00
|
|
|
if (begin != std::string::npos) // safe if end is npos-1
|
2013-08-08 05:34:54 +08:00
|
|
|
ss.push_back(s.substr(begin,end-begin));
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2013-08-22 07:06:07 +08:00
|
|
|
static std::string cap(std::string & s) {
|
2013-08-08 05:34:54 +08:00
|
|
|
s[0] = toupper(s[0]);
|
|
|
|
return s;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* turns Aa_Bb_Cc into aa_bb_cc */
|
2013-08-22 07:06:07 +08:00
|
|
|
static std::string to_cap(const std::string &s) {
|
|
|
|
std::vector<std::string> words;
|
|
|
|
std::string delimiter = "_";
|
2013-08-08 05:34:54 +08:00
|
|
|
split(s,words,(delimiter.c_str())[0]);
|
2013-08-22 07:06:07 +08:00
|
|
|
std::string name = "";
|
2013-08-08 05:34:54 +08:00
|
|
|
for (unsigned int i = 0; i < words.size(); i++) {
|
|
|
|
name = name + cap(words[i]);
|
|
|
|
}
|
|
|
|
return name;
|
|
|
|
}
|
|
|
|
|
|
|
|
//* scans a string for a list of commands delimited by ', \t' with # comments
|
|
|
|
//* @param line The input line to be parsed
|
|
|
|
//* @cs A vector of strings parsed from the input line
|
2013-08-22 07:06:07 +08:00
|
|
|
static void command_strings(std::string line, std::vector<std::string> &cs)
|
2013-08-08 05:34:54 +08:00
|
|
|
{
|
2013-08-22 07:06:07 +08:00
|
|
|
if (line.find('#') != std::string::npos) line.erase(line.find("#"));
|
2013-08-08 05:34:54 +08:00
|
|
|
replace_all_of(line, "\t,", ' ');
|
|
|
|
to_lower(trim(line));
|
|
|
|
split(line, cs);
|
|
|
|
}
|
|
|
|
|
|
|
|
//* reads a single line from a file and splits it into a vector of strings
|
|
|
|
//* returns the number of strings in the vector
|
2013-08-22 07:06:07 +08:00
|
|
|
inline int command_line(std::fstream &fid, std::vector<std::string> &cs)
|
2013-08-08 05:34:54 +08:00
|
|
|
{
|
2013-08-22 07:06:07 +08:00
|
|
|
std::string line;
|
2013-08-08 05:34:54 +08:00
|
|
|
getline(fid, line);
|
|
|
|
command_strings(line, cs);
|
|
|
|
return cs.size();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
#endif
|