double sinh(double x) { int sign; ip_number ix,r; ix=_d2e(x); if (fp_uncommon(ix)) goto dom_error; sign=fp_sign(ix); fp_abs(ix); if (fp_grpow(ix,0)) { /* _sinh_lnv is REQUIRED to read in as a number with the lower part of */ /* its floating point representation zero. */ ip_number w,z,z2; w=_esub(ix,&sinh_lnv); z=_exp(w); if (fp_error(z)) goto range_error; if (!fp_grpow(z,32)) { ip_number t; z2=z; t=_erdv(z,&sinh_vm2); fp_negate(t); z=_eadd(t,&z2); } z2=z; r=_eadd(_emul(z,&sinh_v2m1),&z2); fp_abs(r); r.word.hi|=sign; } else if (!fp_grpow(ix,-32)) return x; else { ip_number g,t,ix2; ix2=ix; g=_esquare(ix); /* Use a (minimax) rational approximation. See Cody & Waite. */ t=__fp_poly1(g,4,sinhp_poly); r=_erdv(__fp_poly0(g,3,sinhq_poly),&t); ix2.word.hi|=sign; /* put sign back */ r=__fp_spike(r,&ix2); /* r=r*ix+ix */ } if (fp_error(r)) goto range_error; return _e2d(r); dom_error: return __fp_edom(sign_bit, TRUE); range_error: return __fp_erange(ix.word.hi, TRUE); }
double fmod(double x,double y) { /* floating point remainder of (x/y) for integral quotient. Remainder */ /* has same sign as x. */ dp_number fx,fy,fr; double q; fx.d=x; fy.d=y; if (!dp_uncommon(fx) && !dp_uncommon(fy) && !dp_iszero(fy)) { if (dp_iszero(fx)) return x; fp_abs(fy); y=fy.d; fr.d=modf(fx.d/y,&q); fr.d=fx.d-q*y; /* The next few lines are an ultra-cautious scheme to ensure that the */ /* result is less than fabs(y) in value and that it has the sign of x. */ if (!fp_sign(fx)) { while (fr.d>=y) fr.d-=y; while (fp_sign(fr) && !dp_iszero(fr)) fr.d+=y; } else { y=-y; while (fr.d<=y) fr.d-=y; while (!fp_sign(fr) && !dp_iszero(fr)) fr.d+=y; } return fr.d; } return __fp_edom(sign_bit, TRUE); }
/* c = (a, b) */ void fp_gcd(fp_int *a, fp_int *b, fp_int *c) { fp_int u, v, r; /* either zero than gcd is the largest */ if (fp_iszero (a) == 1 && fp_iszero (b) == 0) { fp_abs (b, c); return; } if (fp_iszero (a) == 0 && fp_iszero (b) == 1) { fp_abs (a, c); return; } /* optimized. At this point if a == 0 then * b must equal zero too */ if (fp_iszero (a) == 1) { fp_zero(c); return; } /* sort inputs */ if (fp_cmp_mag(a, b) != FP_LT) { fp_init_copy(&u, a); fp_init_copy(&v, b); } else { fp_init_copy(&u, b); fp_init_copy(&v, a); } fp_zero(&r); while (fp_iszero(&v) == FP_NO) { fp_mod(&u, &v, &r); fp_copy(&v, &u); fp_copy(&r, &v); } fp_copy(&u, c); }
double tanh(double x) { /* The first two exits avoid premature overflow as well as needless use */ /* of the exp() function. */ int sign; dp_number fx; fx.d=x; sign=fp_sign(fx); fp_abs(fx); if (fx.d<=27.0) { ip_number ix=_d2e(fx.d); if (fp_uncommon(ix)) goto uncommon; if (_egr(ix,&log3_by_2)) { ix=_emul(ix,&two); ix=_exp(ix); if (fp_error(ix)) goto error; ix=_erdv(_eadd(ix,&one),&two); fp_negate(ix); ix=_eadd(ix,&one); } else { if (fp_grpow(ix,-32)) { ip_number g,t,r1,r2,ix2; ix2=ix; g=_esquare(ix2); r1=__fp_poly1(g,3,tanhp_poly); r2=__fp_poly0(g,3,tanhq_poly); t=_erdv(r2,&r1); ix=__fp_spike(t,&ix2); /* ix=g*ix+ix */ } } ix.word.hi|=sign; if (fp_error(ix)) goto error; return _e2d(ix); } else { dp_number result; result.word.hi=0x3ff00000 | sign; result.word.lo=0; return result.d; } error: return __fp_erange(sign, TRUE); uncommon: return __fp_edom(0, TRUE); }
static int path_func_test() { char *p; test_ffp (fp_dir (""), "."); test_ffp (fp_dir ("gtest.h"), "."); test_ffp (fp_base ("gtest.h"), "gtest.h"); test_ffp (fp_dir ("./gtest.h"), "."); test_ffp (fp_base ("./gtest.h"), "gtest.h"); test_ffp (fp_dir ("/tmp/gtest/gtest.h"), "/tmp/gtest"); test_ffp (fp_base ("/tmp/gtest/gtest.h"), "gtest.h"); test_ffp (fp_abs ("/tmp/gtest/gtest.h"), "/tmp/gtest/gtest.h"); BUG_ON (!fp_hasprefix ("/tmp/gtest", "/tmp") ); BUG_ON (!fp_hasprefix ("/tmp/gtest", "") ); BUG_ON (!fp_hasprefix ("/tmp/gtest", "/tmp/gtest") ); BUG_ON (fp_hasprefix ("/tmp/gtest", "/tmp/gtest/") ); BUG_ON (!fp_hassuffix ("/tmp/gtest", "gtest") ); BUG_ON (!fp_hassuffix ("/tmp/gtest", "") ); BUG_ON (!fp_hassuffix ("/tmp/gtest", "/tmp/gtest") ); BUG_ON (fp_hassuffix ("/tmp/gtest", "/tmp/gtest/") ); return 0; }
/** * Calculate the lid angle using two acceleration vectors, one recorded in * the base and one in the lid. * * @param base Base accel vector * @param lid Lid accel vector * @param lid_angle Pointer to location to store lid angle result * * @return flag representing if resulting lid angle calculation is reliable. */ static int calculate_lid_angle(const vector_3_t base, const vector_3_t lid, int *lid_angle) { vector_3_t v; fp_t ang_lid_to_base, cos_lid_90, cos_lid_270; fp_t lid_to_base, base_to_hinge; fp_t denominator; int reliable = 1; /* * The angle between lid and base is: * acos((cad(base, lid) - cad(base, hinge)^2) /(1 - cad(base, hinge)^2)) * where cad() is the cosine_of_angle_diff() function. * * Make sure to check for divide by 0. */ lid_to_base = cosine_of_angle_diff(base, lid); base_to_hinge = cosine_of_angle_diff(base, p_acc_orient->hinge_axis); /* * If hinge aligns too closely with gravity, then result may be * unreliable. */ if (fp_abs(base_to_hinge) > HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD) reliable = 0; base_to_hinge = fp_sq(base_to_hinge); /* Check divide by 0. */ denominator = FLOAT_TO_FP(1.0) - base_to_hinge; if (fp_abs(denominator) < FLOAT_TO_FP(0.01)) { *lid_angle = 0; return 0; } ang_lid_to_base = arc_cos(fp_div(lid_to_base - base_to_hinge, denominator)); /* * The previous calculation actually has two solutions, a positive and * a negative solution. To figure out the sign of the answer, calculate * the cosine of the angle between the actual lid angle and the * estimated vector if the lid were open to 90 deg, cos_lid_90. Also * calculate the cosine of the angle between the actual lid angle and * the estimated vector if the lid were open to 270 deg, * cos_lid_270. The smaller of the two angles represents which one is * closer. If the lid is closer to the estimated 270 degree vector then * the result is negative, otherwise it is positive. */ rotate(base, p_acc_orient->rot_hinge_90, v); cos_lid_90 = cosine_of_angle_diff(v, lid); rotate(v, p_acc_orient->rot_hinge_180, v); cos_lid_270 = cosine_of_angle_diff(v, lid); /* * Note that cos_lid_90 and cos_lid_270 are not in degrees, because * the arc_cos() was never performed. But, since arc_cos() is * monotonically decreasing, we can do this comparison without ever * taking arc_cos(). But, since the function is monotonically * decreasing, the logic of this comparison is reversed. */ if (cos_lid_270 > cos_lid_90) ang_lid_to_base = -ang_lid_to_base; /* Place lid angle between 0 and 360 degrees. */ if (ang_lid_to_base < 0) ang_lid_to_base += FLOAT_TO_FP(360); /* * Round to nearest int by adding 0.5. Note, only works because lid * angle is known to be positive. */ *lid_angle = FP_TO_INT(ang_lid_to_base + FLOAT_TO_FP(0.5)); return reliable; }
double _asnacs(double x,int acs_flag) { ip_number ix,g,r,t1; int sign,flags; ix=_d2e(x); if (!fp_uncommon(ix)) { sign=fp_sign(ix); fp_abs(ix); if (fp_geqpow(ix,-1)) { /* ix>=0.5 */ /* Filter out domain errors and 1.0 */ if (fp_geqpow(ix,0)) goto dom_error; /* ix<1.0 */ /* ix is in the range [0.5,1.0] (acos(1.0) already catered for) */ flags=acs_flag ? sign ? 0xe : 0x0 : sign ? 0xd : 0xc; /* Do the range reduction g := (1-Y)/2; Y := 2*SQT(g) as done by the FPE. */ g=ix; fp_negate(g); g=_eadd(g,&one); g.word.hi--; /* /2 - we can use exp arithmetic because */ ix=_esqrt(g); ix.word.hi++; /* *2 - we know ix is in the range [0.5,1.0] */ } else { flags=acs_flag ? sign ? 0x4 : 0xc : sign ? 0x1 : 0x0; if (!fp_geqpow(ix,-32)) goto very_small; g=_esquare(ix); } t1=__fp_poly0(g,5,asnacsq_poly); r=_ediv(__fp_poly1(g,5,asnacsp_poly),&t1); ix=__fp_spike(r,&ix); /* ix=r*ix+ix */ very_small: ix.word.hi^=(flags & 0x8) << 28; /* negate if bit 3 of flags is set */ if (flags & 0x4) { /* If bit 2 is set... */ /* ...add a constant */ ip_number constant=pi_by_2; if (flags & 0x2) constant.word.hi++; /* Turn into pi if bit 1 set */ ix=_eadd(constant,&ix); } ix.word.hi^=flags<<31; /* Negate final answer if bit 0 set */ return _e2d(ix); dom_error: /* Either a domain error or an argument of 1.0 */ if (fp_eqpow(ix,0)) { dp_number res; /* This is an exact result of -pi/2, 0, pi/2 or pi depending... */ res.d=_pi_2; if (acs_flag) { if (sign) res.word.hi+=1<<DExp_pos; /* change pi/2 to pi */ else res.d=0.0; } else { res.word.hi|=sign; } return res.d; } /* Fall through to EDOM code */ } /* asin/acos(Inf/QNaN/SNaN) => DOMAIN ERROR */ return __fp_edom(sign_bit, TRUE); }
/* c = 1/a (mod b) for odd b only */ int fp_invmod(fp_int *a, fp_int *b, fp_int *c) { fp_int x, y, u, v, B, D; int neg; /* 2. [modified] b must be odd */ if (fp_iseven (b) == FP_YES) { return fp_invmod_slow(a,b,c); } /* init all our temps */ fp_init(&x); fp_init(&y); fp_init(&u); fp_init(&v); fp_init(&B); fp_init(&D); /* x == modulus, y == value to invert */ fp_copy(b, &x); /* we need y = |a| */ fp_abs(a, &y); /* 3. u=x, v=y, A=1, B=0, C=0,D=1 */ fp_copy(&x, &u); fp_copy(&y, &v); fp_set (&D, 1); top: /* 4. while u is even do */ while (fp_iseven (&u) == FP_YES) { /* 4.1 u = u/2 */ fp_div_2 (&u, &u); /* 4.2 if B is odd then */ if (fp_isodd (&B) == FP_YES) { fp_sub (&B, &x, &B); } /* B = B/2 */ fp_div_2 (&B, &B); } /* 5. while v is even do */ while (fp_iseven (&v) == FP_YES) { /* 5.1 v = v/2 */ fp_div_2 (&v, &v); /* 5.2 if D is odd then */ if (fp_isodd (&D) == FP_YES) { /* D = (D-x)/2 */ fp_sub (&D, &x, &D); } /* D = D/2 */ fp_div_2 (&D, &D); } /* 6. if u >= v then */ if (fp_cmp (&u, &v) != FP_LT) { /* u = u - v, B = B - D */ fp_sub (&u, &v, &u); fp_sub (&B, &D, &B); } else { /* v - v - u, D = D - B */ fp_sub (&v, &u, &v); fp_sub (&D, &B, &D); } /* if not zero goto step 4 */ if (fp_iszero (&u) == FP_NO) { goto top; } /* now a = C, b = D, gcd == g*v */ /* if v != 1 then there is no inverse */ if (fp_cmp_d (&v, 1) != FP_EQ) { return FP_VAL; } /* b is now the inverse */ neg = a->sign; while (D.sign == FP_NEG) { fp_add (&D, b, &D); } fp_copy (&D, c); c->sign = neg; return FP_OKAY; }