void apf(short *in, short *coeff, short *out, long delayi, short alpha, short beta, short u, short agc, short ltgain, short order, short length, short br) { static int FirstTime = 1; static short FIRmem[ORDER]; /* FIR filter memory */ static short IIRmem[ORDER]; /* IIR filter memory */ static short last; static short Residual[ACBMemSize + SubFrameSize]; /* local residual */ short wcoef1[ORDER]; short wcoef2[ORDER]; short scratch[SubFrameSize]; short temp[SubFrameSize]; short mem[ORDER]; long sum1, sum2; long gamma, APFgain; short i, j, n, best; short Stemp, shift1, shift2; long Ltemp; /* initialization -- should be done in init routine for implementation */ if (FirstTime) { FirstTime = 0; for (i = 0; i < ORDER; i++) FIRmem[i] = 0; for (i = 0; i < ORDER; i++) IIRmem[i] = 0; for (i = 0; i < ACBMemSize; i++) Residual[i] = 0; last = 0; } /* Compute weighted LPC coefficients */ weight(wcoef1, coeff, alpha, order); weight(wcoef2, coeff, beta, order); /* Tilt speech */ /*...no tilt in non-voiced regions...*/ for (i = 0, sum2 = 0; i < length - 1; i++) sum2 = L_mac(sum2, in[i], in[i + 1]); if (sum2 < 0) u = 0; /*...no tilt...*/ for (i = 0; i < length; i++) { scratch[i] = msu_r(L_deposit_h(in[i]), u, last); last = in[i]; } /* Compute residual */ fir(scratch, scratch, wcoef1, FIRmem, order, length); for (i = 0; i < SubFrameSize ; i++) Residual[ACBMemSize+i] = scratch[i]; /* long term filtering */ /* Find best integer delay around delayi */ j = extract_h(L_add(delayi, 32768)); sum1 = 0; shift1 = 1; best = j; for (i = Max(DMIN, j - 3); i <= Min(DMAX, j + 3); i++) { shift2 = 1; for (n = ACBMemSize, sum2 = 0; n < ACBMemSize + length; n++) { Ltemp = L_mult(Residual[n], Residual[n - i]); Ltemp = L_shr(Ltemp, shift2); sum2 = L_add(sum2, Ltemp); if (sum2 >= 0x40000000) { sum2 = L_shr(sum2, 1); shift2++; } } if( ((shift1 >= shift2) && (L_shr(sum2,sub(shift1,shift2)) > sum1)) || ((shift1 < shift2) && (sum2 > L_shr(sum1,sub(shift2,shift1))))) { sum1 = sum2; shift1 = shift2; best = i; } } /* Get beta for delayi */ shift1 = 1; for (i = ACBMemSize, sum1 = 0; i < ACBMemSize + length; i++) { Ltemp = L_mult(Residual[i - best], Residual[i - best]); Ltemp = L_shr(Ltemp, shift1); sum1 = L_add(sum1, Ltemp); if (sum1 >= 0x40000000) { sum1 = L_shr(sum1, 1); shift1++; } } shift2 = 1; for (i = ACBMemSize, sum2 = 0; i < ACBMemSize + length; i++) { Ltemp = L_mult(Residual[i], Residual[i - best]); Ltemp = L_shr(Ltemp, shift2); sum2 = L_add(sum2, Ltemp); if (sum2 >= 0x40000000) { sum2 = L_shr(sum2, 1); shift2++; } } if (shift1 > shift2) { shift1 = sub(shift1, shift2); sum2 = L_shr(sum2, shift1); } else if (shift1 < shift2) { shift2 = sub(shift2, shift1); sum1 = L_shr(sum1, shift2); } if ((sum2 == 0) || (sum1 == 0) || (br == 1)) for (i = 0; i < length; i++) temp[i] = Residual[i + ACBMemSize]; else { if (sum2 >= sum1) gamma = 0x7fffffff; /* Clip gamma at 1.0 */ else if (sum2 < 0) gamma = 0; else { shift1 = norm_l(sum1); sum1 = L_shl(sum1, shift1); sum2 = L_shl(sum2, shift1); gamma = L_divide(sum2, sum1); } if (gamma < 0x40000000) for (i = 0; i < length; i++) temp[i] = Residual[i + ACBMemSize]; else { /* Do actual filtering */ for (i = 0; i < length; i++) { Ltemp = L_mpy_ls(gamma, ltgain); Ltemp = L_mpy_ls(Ltemp, Residual[ACBMemSize + i - best]); temp[i] = add(Residual[ACBMemSize + i], round(Ltemp)); } } } /* iir short term filter - first run */ for (i = 0; i < length; i++) scratch[i] = temp[i]; for (i = 0; i < order; i++) mem[i] = IIRmem[i]; iir(scratch, scratch, wcoef2, mem, order, length); /* Get filter gain */ shift1 = 1; for (i = 0, sum1 = 0; i < length; i++) { Ltemp = L_mult(in[i], in[i]); Ltemp = L_shr(Ltemp, shift1); sum1 = L_add(sum1, Ltemp); if (sum1 >= 0x40000000) { sum1 = L_shr(sum1, 1); shift1++; } } shift2 = 1; for (i = 0, sum2 = 0; i < length; i++) { Ltemp = L_mult(scratch[i], scratch[i]); Ltemp = L_shr(Ltemp, shift2); sum2 = L_add(sum2, Ltemp); if (sum2 >= 0x40000000) { sum2 = L_shr(sum2, 1); shift2++; } } if (shift1 > shift2) { shift1 = sub(shift1, shift2); sum2 = L_shr(sum2, shift1); } else if (shift1 < shift2) { shift2 = sub(shift2, shift1); sum1 = L_shr(sum1, shift2); } if (sum2 != 0) { shift1 = norm_l(sum2); sum2 = L_shl(sum2, shift1); shift1 = sub(shift1, 2); /* For (1. < APFgain < 2.) */ sum1 = L_shl(sum1, shift1); Ltemp = L_divide(sum1, sum2); shift1 = norm_l(Ltemp); Ltemp = L_shl(Ltemp, shift1); Stemp = sqroot(Ltemp); if (shift1 & 1) APFgain = L_mult(0x5a82, Stemp); else APFgain = L_deposit_h(Stemp); shift1 = shr(shift1, 1); APFgain = L_shr(APFgain, shift1); /* Re-normalize the speech signal */ for (i = 0; i < length; i++) { Ltemp = L_mpy_ls(APFgain, temp[i]); Ltemp = L_shl(Ltemp, 1); /* For (1. < APFgain < 2.) */ temp[i] = round(Ltemp); } } else APFgain = 0x40000000; /* iir short term filter - second run */ iir(out, temp, wcoef2, IIRmem, order, length); /* Update residual buffer */ for (i = 0; i < ACBMemSize; i++) Residual[i] = Residual[i + length]; }
void oblicz_pierwiastki(struct rownanie * e) { if (e->wsp[0]!=0) { float d=oblicz_d(e); if (d>0) { e->x1r=-e->wsp[1]-sqroot(d)/(2*e->wsp[0]); e->x2r=-e->wsp[1]+sqroot(d)/(2*e->wsp[0]); printf("\n2 pierwiastki x1=%f, x2=%f", e->x1r, e->x2r); } else if (d==0) { e->x1r=-e->wsp[1]/(2*e->wsp[0]); printf("\n1 pierwiastek xo=%f",e->x1r); } else { e->x1r=(-e->wsp[1]/(float)(2*e->wsp[0])); e->x2r=e->x1r; float absd=d<0?-d:d; e->u.x1u=-sqroot(absd)/(2*e->wsp[0]); e->u.x2u=-e->u.x1u; printf("\nPierwiastki zespolone:\n"); formatuj_zesp(e->x1r, e->u.x1u); formatuj_zesp(e->x2r, e->u.x2u); } } else { if (e->wsp[1]!=0) { e->x1r=-e->wsp[2]/e->wsp[1]; printf("\nx1=%f",e->x1r); } else if(e->wsp[1]==0 && e->wsp[2]!=0) printf("\nr.sprzeczne"); else if (e->wsp[1]==0 && e->wsp[2]==0) printf("\nrnieoznaczone"); } }
int checkprime(int x) { int i; for(i=2;i<=sqroot(x); i++) { if((x/i)*i==x){return 0;} } return 1; }
void fndppf(short *delay, short *beta, short *buf, short dmin, short dmax, short length) { static short b = -10224; /* rom storage */ static short a[3] = {-18739, 16024, -4882}; /* a[] scaled down by 4 */ short dnew = 0; short sum; long Lsum; register short m, i, n; static short DECbuf[FrameSize / 4]; long Lcorrmax, Lcmax, Ltmp; short tap1; short M1, M2, dnewtmp = 0; static short lastgoodpitch = 0; static short lastbeta = 0; static short memory[3]; static int FirstTime = 1; short Lsum_scale; short shift, Lcorr_scale, Lcmax_scale; short n1, n2, nq, nq1; long Ltempf; /* init static variables (should be in init routine for implementation) */ if (FirstTime) { FirstTime = 0; n1 = (shr(FrameSize, 2)); for (i = 0; i < n1; i++) DECbuf[i] = 0; memory[0] = memory[1] = memory[2] = 0; } /* Shift memory of DECbuf */ for (i = 0; i < shr(length, 3); i++) { DECbuf[i] = DECbuf[i + shr(length, 3)]; } /* filter signal and decimate */ for (i = 0, n = shr(length, 3); i < shr(length, 1); i++) { Ltempf = L_shr(L_deposit_h(buf[i + shr(length, 1)]), 4); Ltempf = L_msu(Ltempf, memory[0], a[0]); Ltempf = L_msu(Ltempf, memory[1], a[1]); Ltempf = L_msu(Ltempf, memory[2], a[2]); Ltempf = L_shl(Ltempf, 2); shift = 0; if ((i + 1) % 4 == 0) { Lsum = L_add(Ltempf, L_deposit_h(memory[2])); Lsum = L_mac(Lsum, memory[0], b); Lsum = L_mac(Lsum, memory[1], b); DECbuf[n++] = round(L_shl(Lsum, 1)); } memory[2] = memory[1]; memory[1] = memory[0]; memory[0] = round(Ltempf); } /* perform first search for best delay value in decimated domain */ Lcorrmax = (LW_MIN); Lcorr_scale = 1; for (m = shr(dmin, 2); m <= shr(dmax, 2); m++) { n1 = 1; for (i = 0, Lsum = 0; i < sub(shr(length, 2), m); i++) { Ltempf = L_mult(DECbuf[i], DECbuf[i + m]); Ltempf = L_shr(Ltempf, n1); Lsum = L_add(Lsum, Ltempf); if (L_abs(Lsum) >= 0x40000000) { Lsum = L_shr(Lsum, 1); n1++; } } if ( ((Lcorr_scale >= n1) && (L_shr(Lsum, sub(Lcorr_scale, n1)) > Lcorrmax)) || ((Lcorr_scale < n1) && (Lsum > L_shr(Lcorrmax, sub(n1, Lcorr_scale)))) ) { Lcorrmax = Lsum; Lcorr_scale = n1; dnew = m; } } /* Compare against lastgoodpitch */ if (lastgoodpitch != 0 && (abs_s(sub(lastgoodpitch, shl(dnew, 2))) > 2)) { M1 = sub(shr(lastgoodpitch, 2), 2); if (M1 < shr(dmin, 2)) M1 = shr(dmin, 2); M2 = add(M1, 4); if (M2 > shr(dmax, 2)) M2 = shr(dmax, 2); Lcmax = LW_MIN; Lcmax_scale = 1; for (m = M1; m <= M2; m++) { n1 = 1; for (i = 0, Lsum = 0; i < sub(shr(length, 2), m); i++) { Ltempf = L_mult(DECbuf[i], DECbuf[i + m]); Ltempf = L_shr(Ltempf, n1); Lsum = L_add(Lsum, Ltempf); if (L_abs(Lsum) >= 0x40000000) { Lsum = L_shr(Lsum, 1); n1++; } } if ( ((Lcmax_scale >= n1) && (L_shr(Lsum, sub(Lcmax_scale, n1)) > Lcmax)) || ((Lcmax_scale < n1) && (Lsum > L_shr(Lcmax, sub(n1, Lcmax_scale)))) ) { /* Gives some bias to low delays */ Lcmax = Lsum; Lcmax_scale = n1; dnewtmp = m; } } Lsum = L_mpy_ls(Lcorrmax, 27361); if ( ((Lcmax_scale >= Lcorr_scale) && (L_shr(Lsum, sub(Lcmax_scale, Lcorr_scale)) < Lcmax)) || ((Lcmax_scale < Lcorr_scale) && (Lsum < L_shr(Lcmax, sub(Lcorr_scale, Lcmax_scale)))) ) { dnew = dnewtmp; } } /* perform first search for best delay value in non-decimated buffer */ M1 = Max(sub(shl(dnew, 2), 3), dmin); if (M1 < dmin) M1 = dmin; M2 = Min(add(shl(dnew, 2), 3), dmax); if (M2 > dmax) M2 = dmax; Lcorrmax = LW_MIN; Lcorr_scale = 1; for (m = M1; m <= M2; m++) { n1 = 1; for (i = 0, Lsum = 0; i < sub(length, m); i++) { Ltempf = L_mult(buf[i], buf[i + m]); Ltempf = L_shr(Ltempf, n1); Lsum = L_add(Lsum, Ltempf); if (L_abs(Lsum) >= 0x40000000) { Lsum = L_shr(Lsum, 1); n1++; } } if ( ((Lcorr_scale >= n1) && (L_shr(Lsum, sub(Lcorr_scale, n1)) > Lcorrmax)) || ((Lcorr_scale < n1) && (Lsum > L_shr(Lcorrmax, sub(n1, Lcorr_scale)))) ) { Lcorrmax = Lsum; Lcorr_scale = n1; dnew = m; } } Lsum_scale = 1; for (i = 0, Lsum = 0; i < sub(length, dnew); i++) { Ltempf = L_mult(buf[i + dnew], buf[i + dnew]); Ltempf = L_shr(Ltempf, Lsum_scale); Lsum = L_add(Lsum, Ltempf); if (L_abs(Lsum) >= 0x40000000) { Lsum = L_shr(Lsum, 1); Lsum_scale++; } } Lcmax_scale = 1; for (i = 0, Lcmax = 0; i < length - dnew; i++) { Ltempf = L_mult(buf[i], buf[i]); Ltempf = L_shr(Ltempf, Lcmax_scale); Lcmax = L_add(Lcmax, Ltempf); if (L_abs(Lcmax) >= 0x40000000) { Lcmax = L_shr(Lcmax, 1); Lcmax_scale++; } } nq = norm_l(Lsum); Lsum = L_shl(Lsum, nq); nq1 = norm_l(Lcmax); Lcmax = L_shl(Lcmax, nq1); Lsum = L_mpy_ll(Lsum, Lcmax); n1 = norm_l(Lsum); Lsum = L_shl(Lsum, n1); sum = sqroot(Lsum); n1 = add(add(n1, nq), nq1); n1 = sub(sub(n1, Lcmax_scale), Lsum_scale); n2 = shr(n1, 1); if (n1 & 1) Lsum = L_mult(sum, 23170); else Lsum = L_deposit_h(sum); n2 = add(n2, Lcorr_scale); Lcorrmax = L_shl(Lcorrmax, n2); if ((Lsum == 0) || (Lcorrmax <= 0)) *beta = 0; else if (Lcorrmax > Lsum) *beta = 0x7fff; else *beta = round(L_divide(Lcorrmax, Lsum)); /* perform search for best delay value in around old pitch delay */ if (lastgoodpitch != 0) { M1 = lastgoodpitch - 6; M2 = lastgoodpitch + 6; if (M1 < dmin) M1 = dmin; if (M2 > dmax) M2 = dmax; if (dnew > M2 || dnew < M1) { Lcmax = LW_MIN; Lcmax_scale = 1; for (m = M1; m <= M2; m++) { n1 = 1; for (i = 0, Lsum = 0; i < length - m; i++) { Ltempf = L_mult(buf[i], buf[i + m]); Ltempf = L_shr(Ltempf, n1); Lsum = L_add(Lsum, Ltempf); if (L_abs(Lsum) >= 0x40000000) { Lsum = L_shr(Lsum, 1); n1++; } } if ( ((Lcmax_scale >= n1) && (L_shr(Lsum, sub(Lcmax_scale, n1)) > Lcmax)) || ((Lcmax_scale < n1) && (Lsum > L_shr(Lcmax, sub(n1, Lcmax_scale)))) ) { Lcmax = Lsum; dnewtmp = m; Lcmax_scale = n1; } } Lcorr_scale = 1; for (i = 0, Ltmp = 0; i < length - dnewtmp; i++) { Ltempf = L_mult(buf[i + dnewtmp], buf[i + dnewtmp]); Ltempf = L_shr(Ltempf, Lcorr_scale); Ltmp = L_add(Ltmp, Ltempf); if (L_abs(Ltmp) >= 0x40000000) { Ltmp = L_shr(Ltmp, 1); Lcorr_scale++; } } Lsum_scale = 1; for (i = 0, Lsum = 0; i < length - dnewtmp; i++) { Ltempf = L_mult(buf[i], buf[i]); Ltempf = L_shr(Ltempf, Lsum_scale); Lsum = L_add(Lsum, Ltempf); if (L_abs(Lsum) >= 0x40000000) { Lsum = L_shr(Lsum, 1); Lsum_scale++; } } nq = norm_l(Ltmp); Ltmp = L_shl(Ltmp, nq); nq1 = norm_l(Lsum); Lsum = L_shl(Lsum, nq1); Ltmp = L_mpy_ll(Ltmp, Lsum); n1 = norm_l(Ltmp); Ltmp = L_shl(Ltmp, n1); sum = sqroot(Ltmp); n1 = add(add(n1, nq), nq1); n1 = sub(sub(n1, Lsum_scale), Lcorr_scale); n2 = shr(n1, 1); if (n1 & 1) Ltmp = L_mult(sum, 23170); else Ltmp = L_deposit_h(sum); n2 = add(n2, Lcmax_scale); Lcmax = L_shl(Lcmax, n2); if ((Ltmp == 0) || (Lcmax <= 0)) tap1 = 0; else if (Lcmax >= Ltmp) tap1 = 0x7fff; else tap1 = round(L_divide(Lcmax, Ltmp)); /* Replace dnew with dnewtmp if tap1 is large enough */ if ((dnew > M2 && (shr(tap1, 1) > mult_r(9830, *beta))) || (dnew < M1 && (shr(tap1, 1) > mult_r(19661, *beta)))) { dnew = dnewtmp; *beta = (tap1); } } } *delay = dnew; if (*beta > 13107) { lastgoodpitch = dnew; lastbeta = *beta; } else { lastbeta = mult_r(24576, lastbeta); if (lastbeta < 9830) lastgoodpitch = 0; } }
short e_ran_g(short *seed0) { static int iset = 0; static long gset; long rsq, ltemp1, ltemp2; short sv1, sv2, rsq_s; short shft_fctr, stemp1; short shft_fctr1; /* ======================================================================== */ long ltmp1, ltmp2; short ans = 0; short stmp2; /* ======================================================================== */ if (iset == 0) { sv1 = shl(sub(ran0(seed0), 16384), 1); sv2 = shl(sub(ran0(seed0), 16384), 1); /* rsq = sv1 * sv1 + sv2 * sv2; */ ltemp1 = L_mult(sv1, sv1); ltemp2 = L_mult(sv2, sv2); rsq = L_add(L_shr(ltemp1, 1), L_shr(ltemp2, 1)); if (rsq >= 1073741824 || rsq == 0){ /* If condition not met, don't iterate; use */ /* rough approximation. */ ans = shr(sv1,3); ans = add(ans, shr(sv2,3)); ans = add(ans, shr(sub(ran0(seed0), 16384),2)); return (ans); } /* * error in rsq doesn't seem to contribute to the final error in e_ran_g */ /* * rsq scale down by two: input to fnLog must be scaled up by 2. */ rsq = L_shl(rsq, 1); /* stemp1 = round(L_negate(fnLog(rsq))); */ ltmp1 = L_negate(fnLog(rsq)); /* * rsq must be greater than the log of lsq for the fractional * divide to work. therfore normalize rsq. */ shft_fctr = norm_l(rsq); rsq_s = round(L_shl(rsq, shft_fctr)); stmp2 = (divide_s(round(ltmp1), rsq_s)); /* * stemp2 must be normalized before taking its square root. * (increases precision). */ shft_fctr1 = norm_s(stmp2); ltmp2 = L_deposit_h(shl(stmp2, shft_fctr1)); stemp1 = sqroot(ltmp2); /* * shifting involved before taking the square root: * LEFT << shft_fctr. (LEFT because rsq is in the denominator * of ltemp2 quotion). * LEFT << 6. (multiply by 2 in original code and multiply by 32 * because output of fnLog scaled down by 32). * RIGHT >> shft_fctr1. (normalization taken before sqroot). */ shft_fctr = shft_fctr + 6 - shft_fctr1; /* * PROPERTY: sqrt(2^n) = 2^(n/2) * if shft_fctr is odd; multiply stemp1 by sqrt(2)/2 and * increment number of shifts by 1. Can now use shft_fctr / 2. */ if (shft_fctr & 0x0001) { stemp1 = mult(stemp1, 23170); shft_fctr++; } shft_fctr = shr(shft_fctr, 1); /* * normalize stemp1 for the following multiplication. * adjust shft_fctr accordingly. */ shft_fctr1 = norm_s(stemp1); stemp1 = shl(stemp1, shft_fctr1); shft_fctr = shft_fctr - shft_fctr1; gset = L_mult(sv1, stemp1); /* * final output is scaled down by 4, therefore shift up by * shft_fctr - 2. */ gset = L_shl(gset, shft_fctr - 2); iset = 1; return round(L_shl(L_mult(sv2, stemp1), shft_fctr - 2)); } else { iset = 0; return round(gset); } }
float distance(float x, float y, float z) { float d1 = sqroot((x * x) + (y * y)); float d2 = sqroot((d1 * d1) + (z * z)); return d2; }