/* * compute_s_normconst: Compute the s orbital normalization constant. * (8a^3/PI^3)^(1/4). */ double compute_s_normconst(double alpha) { double val; val = 8 * pow(alpha, 3) / M_PI_3; val = pow(val, 1.0/4.0); return val; }

//Func: ini_astr() //This function set initial values for every asteroid void ini_astr(int num_astr, obj *astr){ for(int i = 0; i < num_astr; ++i){ do{ (astr[i]).m = rand()%(int) pow(10,rand()%7); }while((astr[i]).m == 0); (astr[i]).x = rand()%(int) pow(10,rand()%4); (astr[i]).y = rand()%(int) pow(10,rand()%4); (astr[i]).z = rand()%(int) pow(10,rand()%4); (astr[i]).sx = (astr[i]).x; (astr[i]).sy = (astr[i]).y; (astr[i]).sz = (astr[i]).z; (astr[i]).vx = 0; (astr[i]).vy = 0; (astr[i]).vz = 0; (astr[i]).ax = 0; (astr[i]).ay = 0; (astr[i]).az = 0; (astr[i]).fx = 0; (astr[i]).fy = 0; (astr[i]).fz = 0; (astr[i]).theta = 0; (astr[i]).phi = 0; } }

Color Colors::gammaCorrect(Color color, float gamma) { float gc = 1 / gamma; uint8_t red = 127.0f * pow((float)Colors::red(color) / 127.0f, gc); uint8_t green = 127.0f * pow((float)Colors::green(color) / 127.0f, gc); uint8_t blue = 127.0f * pow((float)Colors::blue(color) / 127.0f, gc); return Colors::rgb(red, green, blue); }

void Splines::Solve_eq(unsigned long n, t_pt *pts) { t_eq eq; unsigned long i, j; eq.dim = n - 1; eq.matrice = solver.Allocmat(n - 1, n - 1); eq.vecteur = solver.Allocmat(1, n - 1); eq.vectres = solver.Allocmat(1, n - 1); for (i = 1; i < n; i++) { for (j = 1; j < n; j++) eq.matrice->mat[(i - 1) * (n - 1) + (j - 1)] = C(n, j) * pow(1 - ((double)i)/n, n - j) * pow(((double)i) / n, j); eq.vecteur->mat[i - 1] = pts[i].x - pts[0].x * pow(1 - ((double)i)/n, n) - pts[n].x * pow(((double)i)/n, n); } solver.Solve(&eq); for (i = 1; i < n; i++) pts[i].x = (int)eq.vectres->mat[i - 1]; for (i = 1; i < n; i++) { for (j = 1; j < n; j++) eq.matrice->mat[(i - 1) * (n - 1) + (j - 1)] = C(n, j) * pow(1 - ((double)i)/n, n - j) * pow(((double)i) / n, j); eq.vecteur->mat[i - 1] = pts[i].y - pts[0].y * pow(1 - ((double)i)/n, n) - pts[n].y * pow(((double)i)/n, n); } solver.Solve(&eq); for (i = 1; i < n; i++) pts[i].y = (int)eq.vectres->mat[i - 1]; solver.Freemat(eq.matrice); solver.Freemat(eq.vecteur); solver.Freemat(eq.vectres); }

double Sphere::findIntersection(Ray* ray) { // https://en.wikipedia.org/wiki/Line%E2%80%93sphere_intersection double distance; double r = size / 2; Vect o_c = ray->getOrigin().subtract(position); double discriminant = pow(ray->getDirection().dot(o_c),2) - pow(o_c.magnitude(),2) + pow(r,2); if(discriminant > 0) { // there is one point of intersection double a = ray->getDirection().dot(o_c) * -1; double b = sqrt(discriminant); double amb = a-b; double apb = a+b; if(amb > 0) distance = amb; else if (amb < 0 && apb > 0) distance = apb; else distance = -1; } else { // there is no intersection distance = -1; } return distance; };

void rayCasting::setTransferFn(glm::vec4 inputArray[tableEntries], int te, double attenuation, float over){ minTFIndex = maxTFIndex = -1; if (attenuation < -1. || attenuation > 1.) std::cout << "Bad attenuation value !!!" << std::endl; if (te != tableEntries) std::cout << "Wrong number of entries !!!" << std::endl; for (int i=0; i<te; i++){ double bp = tan(1.570796327 * (0.5 - attenuation*0.49999)); double alpha = pow(inputArray[i].r / 255.f, (float)bp); alpha = 1.0 - pow((1.0 - alpha), 1.0/over); transferFn1D[i].r = inputArray[i].r*alpha; transferFn1D[i].g = inputArray[i].g*alpha; transferFn1D[i].b = inputArray[i].b*alpha; transferFn1D[i].a = alpha; if (minTFIndex == -1) if (alpha > 0) minTFIndex = i; if (alpha > 0) maxTFIndex = i; } if (minScalar != maxScalar) rangeScalar = maxScalar - minScalar; else rangeScalar = 1.0f; }

bool FAST_Laplace::isScaleExtremum(int row, int col, int octave, int interval) { double size = sigma[0] * pow(2.0, octave + (double)interval / intervals); //double size = sigma[interval];// * pow(2.0, octave); double val = getPixelLaplacian(gaussPyramid[octave][interval + 1], row, col) * size * size; if (std::abs(val) < 1e-1) return false; if (val > 0) for (int scale = -1; scale <= 1; ++ scale) { double size = sigma[0] * pow(2.0, octave + (double)(interval + scale) / intervals); // double size = sigma[interval]; double neighbour = getPixelLaplacian(gaussPyramid[octave][interval + scale + 1], row, col) * size * size; if (val < neighbour) return false; } else for (int scale = -1; scale <= 1; ++ scale) { double size = sigma[0] * pow(2.0, octave + (double)(interval + scale) / intervals); // double size = sigma[interval]; double neighbour = getPixelLaplacian(gaussPyramid[octave][interval + scale + 1], row, col) * size * size; if (val > neighbour) return false; } return true; }

void FattalToneMapping::calculateGradientPy(cv::Mat& pImage, int level, cv::Mat& pGradX, cv::Mat& pGradY) { pGradX = cv::Mat::zeros(pImage.rows, pImage.cols, CV_32FC1); pGradY = cv::Mat::zeros(pImage.rows, pImage.cols, CV_32FC1); for (int i = 0; i < pImage.rows; i++) for (int j = 0; j < pImage.cols; j++) { if ( i == 0 ) { pGradY.at<float>(i, j) = (pImage.at<float>(i + 1, j) - pImage.at<float>(i, j)) / pow(2, level + 1); } else if ( i == pImage.rows - 1) { pGradY.at<float>(i, j) = (pImage.at<float>(i, j) - pImage.at<float>(i - 1, j)) / pow(2, level + 1); } else { pGradY.at<float>(i, j) = (pImage.at<float>(i + 1, j) - pImage.at<float>(i - 1, j)) / pow(2, level + 1); } if ( j == 0 ) { pGradX.at<float>(i, j) = (pImage.at<float>(i, j + 1) - pImage.at<float>(i, j)) / pow(2, level + 1); } else if ( j == pImage.cols - 1) { pGradX.at<float>(i, j) = (pImage.at<float>(i, j) - pImage.at<float>(i, j - 1)) / pow(2, level + 1); } else { pGradX.at<float>(i, j) = (pImage.at<float>(i, j + 1) - pImage.at<float>(i, j - 1)) / pow(2, level + 1); } } }

bool CFXBoxBlur::LoadData(CResourceList* pResourceList) { // Force width and height to be powers of two. log2(x) = log10(x) / log10(2) CVarInt::CValueInt valueInt; EvaluateVar("BlurTexture Width", 0.0f, &valueInt); if(valueInt.GetValue() < 1) valueInt.SetValue(1); int nWidthPwr = MYROUND(log10f(valueInt.GetValue()) / log10f(2)); EvaluateVar("BlurTexture Height", 0.0f, &valueInt); if(valueInt.GetValue() < 1) valueInt.SetValue(1); int nHeightPwr = MYROUND(log10f(valueInt.GetValue()) / log10f(2)); int nWidth = pow(2, nWidthPwr); int nHeight = pow(2, nHeightPwr); // Create texture UtilGL::Texturing::STexLoadOptions texOptions; texOptions.SetDefaults(); texOptions.eFilter = UtilGL::Texturing::FILTER_LINEAR; m_texture.LoadFlat(nWidth, nHeight, CVector4(0.0f, 0.0f, 0.0f, 1.0f), false, false, &texOptions); return true; }

double lineProfile(const double vy, const double x, const double vz, void * params){ struct LPPar * par = (struct LPPar*) params; double Ve = par->Ve; double *Rz = par->Rz; double vx = par->vx; double R = sqrt(pow(Rz[0],2) + pow(x * (ar[NR-1]),2)), z = Rz[1], y = Rz[0]; // double vphi = sgn<double>(vx)*sqrt(vx*vx+vy*vy) * sin(atan(y/x)); // double vr = vy; double v_abs = sqrt(vx*vx + vy*vy); double alpha = acos(vx/v_abs), theta = atan2(y, x); double vr = v_abs * cos(alpha+theta), vphi = v_abs * sin(alpha+theta); //Rz[0]=R; //Rz[2]=(*p)(R,z); //Ve=sqrt(-2*((*p)(R,z)-(*p)(100 * ar[NR-1], 100 * ar[NR-1]))); /* rescaling the integral to [0,1]x[-1,1]x[0,1] */ double V[3] = {1e-6+Ve*vr, 1e-6+Ve*vphi, 1e-6+Ve*vz}; double jacob = Ve*Ve*(fabs(x) * pow(ar[NR-1],2))/R; //if (x<0.) // printf("%e %e %e %e %e %e\n",vr,vphi,vz,vx,vy,Ve); return jacob*df(Rz,V); }

double sigma(double beta, double p) { double fac2, fac3; double term1, term2, term3; double sum2, sum3, gama; double delta, result; delta = 1 + 4*quad(beta) + 3*qquad(beta); gama = 1 + 2*quad(beta); term1 = 2*pow((1 + 4*quad(beta)),-p/2.0); fac2 = 2*pow(gama,-p); sum2 = (1 + 2*p*qquad(beta)/quad(gama)); sum2 += (3*p*(p + 2)*qqquad(beta)/(4.0*qquad(gama))); term2 = fac2 * sum2; fac3 = (-4*pow(delta,-p/2.0)); sum3 = 1 + 3*p*qquad(beta)/(2*delta); sum3 += p*(p+2)*qqquad(beta)/(2*quad(delta)); term3 = fac3 * sum3; result = term1 + term2 + term3; return result; }

string formattedDuration(sf::Time time, unsigned int nb_dec) { float secondsF = time.asSeconds(); int s = (int) secondsF; int tot_s = s; int d = s/86400; s -= d*86400; int h = s/3600; s -= h*3600; int m = s/60; s -= m*60; string str = ""; if (d>0) str += to_string(d) + "j "; if (h>0 || d>0) str += to_string(h, 2) + "h "; if (m>0 || h>0 || d>0) str += to_string(m, 2) + "m "; str += to_string(s, 2); if (nb_dec > 0) { int subsec = secondsF * pow(10.0, (double) nb_dec) - tot_s * pow(10.0, (double) nb_dec); str += "." + to_string(subsec, nb_dec); } str += "s"; return str; }

//Function to verify if a pose is in the robot's workspace. bool Kinematics::rCheckWorkspace(double Pose[6], bool ID) { // - If Pose is inside the workspace, it returns the same pose // - If it is not, it returns the closest pose in the workspace (not yet implemented) //dReal xr=Pose[0], yr=Pose[1], zr=Pose[2]; // Current position dReal xr=Pose[0], zr=Pose[2]; // Current position bool InWorkspace; // To indicate if it is inside the workspace //--------------------- Verify workspace in xz plane ---------------------- dReal Rad = L4 + L5 + L6 + L7 + L8; dReal zo = -(L2 + L3); //It is inside the circunference and z is in the down part if ( ( pow(zr-zo,2) + pow(xr,2) - pow(Rad,2) )<=0 && zr<=zo ) { //PoseOut = PoseIn; InWorkspace = true; } else { //PoseOut = PoseIn; InWorkspace = false; printf("Workspace note: (x=%5.2f,z=%5.2f) is out of reachable space\n", xr, zr); } return (InWorkspace); }

void bfdesign (float fpass, float apass, float fstop, float astop, int *npoles, float *f3db) /***************************************************************************** Butterworth filter: compute number of poles and -3 db frequency for a low-pass or high-pass filter, given a frequency response constrained at two frequencies. ****************************************************************************** Input: fpass frequency in pass band at which amplitude is >= apass apass amplitude in pass band corresponding to frequency fpass fstop frequency in stop band at which amplitude is <= astop astop amplitude in stop band corresponding to frequency fstop Output: npoles number of poles f3db frequency at which amplitude is sqrt(0.5) (-3 db) ****************************************************************************** Notes: (1) Nyquist frequency equals 0.5 (2) The following conditions must be true: (0.0<fpass && fpass<0.5) && (0.0<fstop && fstop<0.5) && (fpass!=fstop) && (0.0<astop && astop<apass && apass<1.0) (3) if (fpass<fstop) a low-pass filter is assumed else a high-pass filter is assumed ****************************************************************************** Author: Dave Hale, Colorado School of Mines, 06/02/89 *****************************************************************************/ { float wpass,wstop,fnpoles,w3db; /* warp frequencies according to bilinear transform */ wpass = 2.0*tan(PI*fpass); wstop = 2.0*tan(PI*fstop); /* if lowpass filter, then */ if (fstop>fpass) { fnpoles = log((1.0/(apass*apass)-1.0)/(1.0/(astop*astop)-1.0)) / log(pow(wpass/wstop,2.0)); w3db = wpass/pow((1.0/(apass*apass)-1.0),0.5/fnpoles); /* else, if highpass filter, then */ } else { fnpoles = log((1.0/(apass*apass)-1.0)/(1.0/(astop*astop)-1.0)) / log(pow(wstop/wpass,2.0)); w3db = wpass*pow((1.0/(apass*apass)-1.0),0.5/fnpoles); } /* determine integer number of poles */ *npoles = 1+(int)fnpoles; /* determine (unwarped) -3 db frequency */ *f3db = atan(0.5*w3db)/PI; }

double minimize_merit_function(double res0, double lambda1, double res1, double lambda2, double res2) { double lambda; double a, b, c; //interpolating polynomial: ax2 + bx + c //calculate three-point quadratic polynomial interpolating the merit function c = res0; Cramer_rule(pow(lambda1, 2.0), lambda1, res1-res0, pow(lambda2, 2.0), lambda2, res2-res0, &a, &b); //minimize ax^2+bx+c if(a>0){ lambda = -b/(2*a); if(lambda < lambda1*thmin){ lambda = lambda1*thmin; }else if(lambda > lambda1*thmax){ lambda = lambda1*thmax; } }else{ if(a * lambda1*thmin*lambda1*thmin + b * lambda1*thmin + c < a * lambda1*thmax*lambda1*thmax + b * lambda1*thmax + c){ lambda = lambda1*thmin; }else{ lambda = lambda1*thmax; } } return(lambda); }

int main(void) { int num=0; int n[7]; scanf("%d", &num); int k =0; for(k=0; k<7; k++){ if(num>=(int)pow(10, k)) n[k]=num%(int)pow(10, k+1)/(int)pow(10, k); } /* n[0]=num%10; n[1]=num%100/10; n[2]=num%1000/100; n[3]=num%10000/1000; n[4]=num%100000/10000; n[5]=num/100000; int h=0; for(h=0; h<6; h++){ printf("%d", n[h]); printf(""); } */ int sum=0; int i, j; for( i=6,j=64; i>=0, j>=1; i--, j/=2){ if(n[i] !=0 && (n[i]%2) ==((i+1)%2) ){ sum+=j; } } printf("%d", sum); return 0; }

void GammaCompressor::compressUnsignedInt(ofstream * invertedFile, unsigned int number, unsigned char * bitCode, int * currentBit) { int numberSizeByBit; int cpt; if (number > 0) { numberSizeByBit = int(floor(log(number) / log(2)))+1; } else { numberSizeByBit = 0; } cpt = numberSizeByBit; while (cpt > 1) { writeBit(invertedFile, bitCode, currentBit, true); cpt--; } writeBit(invertedFile, bitCode, currentBit, false); if (number == 0) { writeBit(invertedFile, bitCode, currentBit, false); } else if (number == 1) { writeBit(invertedFile, bitCode, currentBit, true); } else { cpt = numberSizeByBit-1; number =number % int(pow(2, cpt)); while (cpt > 0) { writeBit(invertedFile, bitCode, currentBit, number / int(pow(2, cpt - 1)) == 1); number = number % int(pow(2, cpt-1)); cpt--; } } }

void AACQuantizeInit(CoderInfo *coderInfo, unsigned int numChannels, AACQuantCfg *aacquantCfg) { unsigned int channel, i; aacquantCfg->pow43 = (double*)AllocMemory(PRECALC_SIZE*sizeof(double)); aacquantCfg->adj43 = (double*)AllocMemory(PRECALC_SIZE*sizeof(double)); aacquantCfg->pow43[0] = 0.0; for(i=1;i<PRECALC_SIZE;i++) aacquantCfg->pow43[i] = pow((double)i, 4.0/3.0); #if TAKEHIRO_IEEE754_HACK aacquantCfg->adj43[0] = 0.0; for (i = 1; i < PRECALC_SIZE; i++) aacquantCfg->adj43[i] = i - 0.5 - pow(0.5 * (aacquantCfg->pow43[i - 1] + aacquantCfg->pow43[i]),0.75); #else // !TAKEHIRO_IEEE754_HACK for (i = 0; i < PRECALC_SIZE-1; i++) aacquantCfg->adj43[i] = (i + 1) - pow(0.5 * (aacquantCfg->pow43[i] + aacquantCfg->pow43[i + 1]), 0.75); aacquantCfg->adj43[i] = 0.5; #endif for (channel = 0; channel < numChannels; channel++) { coderInfo[channel].requantFreq = (double*)AllocMemory(BLOCK_LEN_LONG*sizeof(double)); } }

int main () { int N, i; printf("How many numbers: "); scanf("%d", &N); int n; int count_of_odd = 0, count_of_even = 0; int sum_of_odd = 0, sum_of_even = 0; for (i = 0; i < N; i++) { printf("Input number #%d:", i + 1); scanf("%d", &n); if (n % 2 == 0) { count_of_even++; sum_of_even += n; } else { count_of_odd++; sum_of_odd += n; } } puts(""); printf("Count of even = %d\nCount of odd = %d\n", count_of_even, count_of_odd); printf("Sum of even = %d\nSum of odd = %d\n", sum_of_even, sum_of_odd); printf("Square of sum of even = %d\nSquare of sum of odd = %d\n", (int)pow(sum_of_even, 2), (int)pow(sum_of_odd, 2)); return 0; }

// sea float sea_octave(vec2 uv, float choppy) { uv += noise(uv); vec2 wv = 1.0-abs(sin(uv)); vec2 swv = abs(cos(uv)); wv = mix(wv,swv,wv); return pow(1.0-pow(wv.x * wv.y,0.65),choppy); }

void FAST_Laplace::detector(cv::Mat& img, std::vector<cv::KeyPoint>& keypoints) { buildGaussPyr(img); std::vector< std::vector< std::vector<cv::KeyPoint> > > fastPoints; fastPoints.resize(octaves); for (int oct = 0; oct < octaves; ++oct ) fastPoints[oct].resize(intervals + 1); for (int oct = 0; oct < octaves; ++oct ) for (int inter = 0; inter < intervals + 1; ++inter ) { cv::Mat img8bit; gaussPyramid[oct][inter].convertTo(img8bit, CV_8UC1); double size = sigma[0] * pow(2.0, oct + (double)inter / intervals); cv::FAST(img8bit, fastPoints[oct][inter], this->threshold); for (auto& point : fastPoints[oct][inter]) // if (isScaleExtremum(point.pt.y, point.pt.x, oct, inter)) { point.pt.x *= pow( 2.0, oct-1 ); point.pt.y *= pow( 2.0, oct-1 ); point.size = size * 2; point.octave = oct; point.class_id = inter; keypoints.push_back(point); } } return; }

gint handler_src_dst_ip_compare_nodes(tree_node_t *node_a, tree_node_t *node_b) { guint src_mask_a = pow(2, 32 - node_a->src_dst_ip.src_network_bits) - 1; guint src_mask_b = pow(2, 32 - node_b->src_dst_ip.src_network_bits) - 1; guint src_addr_a = g_ntohl(node_a->src_dst_ip.src_addr.s_addr) & ~src_mask_a; guint src_addr_b = g_ntohl(node_b->src_dst_ip.src_addr.s_addr) & ~src_mask_b; // check the src addresses first if (src_addr_a < src_addr_b) { return -1; } else if (src_addr_a > src_addr_b) { return 1; } guint dst_mask_a = pow(2, 32 - node_a->src_dst_ip.dst_network_bits) - 1; guint dst_mask_b = pow(2, 32 - node_b->src_dst_ip.dst_network_bits) - 1; guint dst_addr_a = g_ntohl(node_a->src_dst_ip.dst_addr.s_addr) & ~dst_mask_a; guint dst_addr_b = g_ntohl(node_b->src_dst_ip.dst_addr.s_addr) & ~dst_mask_b; // if we hit here, we know src_addresses are equal, so compare dst_address if (dst_addr_a < dst_addr_b) { return -1; } else if (dst_addr_a > dst_addr_b) { return 1; } // if we hit here, we know both the src and dst addresses are equal return 0; }

float BLOBcalculateVelocity(pos oldPos, pos newPos, int deltaTime){ float s = sqrtf(pow((newPos.getX()-oldPos.getX()),2)+pow(newPos.getY()-oldPos.getY(),2)); float outVelocity = s/deltaTime; return outVelocity; }

/** * Determines the likelihood sum based on the formula: SUM( (IK[IND] - 100)^2 - (IK[IND] - 228)^2)/ 100 * @param I The 3D matrix * @param ind The current ind array * @param numOnes The length of ind array * @return A double representing the sum */ double calcLikelihoodSum(int * I, int * ind, int numOnes){ double likelihoodSum = 0.0; int y; for(y = 0; y < numOnes; y++) likelihoodSum += (pow((I[ind[y]] - 100),2) - pow((I[ind[y]]-228),2))/50.0; return likelihoodSum; }

void generateProbabilityTable(struct probabilityTable_s* probabilitiesTable, float standardDeviation){ for(uint8_t i = 0; i < 127; i++) { probabilitiesTable->probabilities[i] = (0.39894f*(1.0f/standardDeviation)*pow(e,(-pow((i),2.0f))/(2.0f*pow(standardDeviation,2.0f)))) / (0.39894f*(1.0f/standardDeviation)*pow(e,(-pow((0),2.0f))/(2.0f*pow(standardDeviation,2.0f)))); probabilitiesTable->standardDeviation = standardDeviation; } }

// FUNCTION: receiveInput // Recieves input as a string and returns it as a double (if valid) double Converter::receiveInput() { std::string inputString; std::cout << "\nINPUT:\n"; std::getline(std::cin, inputString); bool invalidInput = true; double inputValue; if (stringIsNumber(inputString)) { inputValue = std::stod(inputString); if (inputValue >= -pow(10, 13) && inputValue <= pow(10, 13)) invalidInput = false; } while (invalidInput) { std::cout << "Invalid input. Please try again: "; std::getline(std::cin, inputString); if (stringIsNumber(inputString)) { inputValue = std::stod(inputString); if (inputValue >= -pow(10, 13) && inputValue <= pow(10, 13)) invalidInput = false; } } return inputValue; }

CvSeq *reghand::filthull2(CvSeq *filted_elimhull) { //CvSeq *filtedhullseq=cvCloneSeq(filted_elimhull); float maxdis=0;CvPoint **fingpt;CvScalar mean,std=cvScalarAll(0); CvMat *dismat=cvCreateMat(1,filted_elimhull->total,CV_32FC1); CvPoint2D32f center=minrect.center; for (int i=0;i<filted_elimhull->total;i++) { CvPoint **data=CV_GET_SEQ_ELEM(CvPoint*,filted_elimhull,i); CvPoint pt=**data; float dis=sqrt(pow(pt.x-center.x,2)+pow(pt.y-center.y,2)); dismat->data.fl[i]=dis; if(dis>maxdis){maxdis=dis;fingpt=data;} } cvAvgSdv(dismat,&mean,&std); if(filted_elimhull->total==1&&maxdis>fingerTh*0.5) return filted_elimhull; if(filted_elimhull->total==2&&maxdis>fingerTh*0.5&&std.val[0]<10) { CvPoint startpt=**CV_GET_SEQ_ELEM(CvPoint*,filted_elimhull,0); CvPoint endpt=**CV_GET_SEQ_ELEM(CvPoint*,filted_elimhull,1);; double bfang=atan(double(startpt.y-handcenter.y)/(startpt.x-handcenter.x))*180/PI; if(bfang<0)bfang+=180; double afang=atan(double(endpt.y-handcenter.y)/(endpt.x-handcenter.x))*180/PI; if(afang<0)afang+=180; if(fabs(bfang-afang)>60) {cvClearSeq(filted_elimhull);cvSeqPush(filted_elimhull,fingpt);return filted_elimhull;} else return filted_elimhull; }

static char *value_string(char *buf, int buf_size, double val, const char *unit) { if (unit == unit_second_str && use_value_sexagesimal_format) { double secs; int hours, mins; secs = val; mins = (int)secs / 60; secs = secs - mins * 60; hours = mins / 60; mins %= 60; snprintf(buf, buf_size, "%d:%02d:%09.6f", hours, mins, secs); } else if (use_value_prefix) { const char *prefix_string; int index; if (unit == unit_byte_str && use_byte_value_binary_prefix) { index = (int) (log(val)/log(2)) / 10; index = av_clip(index, 0, FF_ARRAY_ELEMS(binary_unit_prefixes) -1); val /= pow(2, index*10); prefix_string = binary_unit_prefixes[index]; } else { index = (int) (log10(val)) / 3; index = av_clip(index, 0, FF_ARRAY_ELEMS(decimal_unit_prefixes) -1); val /= pow(10, index*3); prefix_string = decimal_unit_prefixes[index]; } snprintf(buf, buf_size, "%.3f %s%s", val, prefix_string, show_value_unit ? unit : ""); } else { snprintf(buf, buf_size, "%f %s", val, show_value_unit ? unit : ""); } return buf; }

// it computes the cmd vel error void ControlDataLogger::computeCostCallback(const ros::TimerEvent&) { double linear_error_component, angular_error_component; //linear error linear_error_ = cmdvel_optimal_.linear.x - cmdvel_robot_.linear.x; linear_error_msg_.data = fabs(linear_error_); if (linear_error_msg_.data > 0.1) {linear_error_msg_.data = 0.1; } linear_error_pub_.publish(linear_error_msg_); // angular error angular_error_ = fabs(cmdvel_optimal_.angular.z) - fabs(cmdvel_robot_.angular.z) ; angular_error_ = fabs(angular_error_); angular_error_msg_.data = angular_error_; angluar_error_pub_.publish(angular_error_msg_); // vector magnitute error linear_error_component = fabs(cmdvel_optimal_.linear.x - cmdvel_robot_.linear.x); if (linear_error_component > 0.1) {linear_error_component = 0.1; } angular_error_component = fabs(fabs(cmdvel_optimal_.angular.z) - fabs(cmdvel_robot_.angular.z)); vector_vel_error_msg_.data = sqrt( pow(linear_error_component, 2) + pow(angular_error_component, 2) ); vector_vel_error_pub_.publish(vector_vel_error_msg_); }

/* * compute_f_normconst: Compute the d orbital normalization constant. * (32768a^9/Pi^3)^(1/4). */ double compute_f_normconst(double alpha) { double val; val = 32768 * pow(alpha, 9) / M_PI_3; val = pow(val, 1.0/4.0); return val; }