void Tri::genFile (Curve *cur, double *x, double *y){ register int i; Geometry *g; Point p1, p2, a; double *z, *w, *eta, xoff, yoff; int fac; fac = cur->face; p1.x = vert[vnum(fac,0)].x; p1.y = vert[vnum(fac,0)].y; p2.x = vert[vnum(fac,1)].x; p2.y = vert[vnum(fac,1)].y; getzw(qa,&z,&w,'a'); eta = dvector (0, qa); if ((g = lookupGeom (cur->info.file.name)) == (Geometry *) NULL) g = loadGeom (cur->info.file.name); /* If the current edge has an offset, apply it now */ xoff = cur->info.file.xoffset; yoff = cur->info.file.yoffset; if (xoff != 0.0 || yoff != 0.0) { dsadd (g->npts, xoff, g->x, 1, g->x, 1); dsadd (g->npts, yoff, g->y, 1, g->y, 1); if (option("verbose") > 1) printf ("shifting current geometry by (%g,%g)\n", xoff, yoff); } /* get the end points which are assumed to lie on the curve */ /* set up search direction in normal to element point -- This assumes that vertices already lie on spline */ a.x = p1.x - (p2.y - p1.y); a.y = p1.y + (p2.x - p1.x); eta[0] = searchGeom (a, p1, g); a.x = p2.x - (p2.y - p1.y); a.y = p2.y + (p2.x - p1.x); eta[qa-1] = searchGeom (a, p2, g); /* Now generate the points where we'll evaluate the geometry */ for (i = 1; i < qa-1; i++) eta [i] = eta[0] + 0.5 * (eta[qa-1] - eta[0]) * (z[i] + 1.); for (i = 0; i < qa; i++) { x[i] = splint (g->npts, eta[i], g->arclen, g->x, g->sx); y[i] = splint (g->npts, eta[i], g->arclen, g->y, g->sy); } g->pos = 0; /* reset the geometry */ if (xoff != 0.) dvsub (g->npts, g->x, 1, &xoff, 0, g->x, 1); if (yoff != 0.) dvsub (g->npts, g->y, 1, &yoff, 0, g->y, 1); free (eta); /* free the workspace */ return; }
main() { /* test program for above utility routines */ double **a, **b, **c, **bT; double *x, *y, *z; FILE *infile, *outfile; int a_rows, a_cols, b_rows, b_cols, errors, xn, yn; infile = fopen("mat.in", "r"); outfile = fopen("mat.dat", "w"); a = dReadMatrix( infile, &a_rows, &a_cols, &errors); b = dReadMatrix( infile, &b_rows, &b_cols, &errors); x = dReadVector( infile, &xn, &errors); y = dReadVector( infile, &yn, &errors); getchar(); dmdump( stdout, "Matrix A", a, a_rows, a_cols, "%8.2lf"); dmdump( stdout, "Matrix B", b, b_rows, b_cols, "%8.2lf"); dvdump( stdout, "Vector x", x, xn, "%8.2lf"); dvdump( stdout, "Vector y", y, yn, "%8.2lf"); z = dvector( 1, xn ); dvadd( x, xn, y, z ); dvdump( stdout, "x + y", z, xn, "%8.2lf"); dvsub( x, xn, y, z ); dvdump( stdout, "x - y", z, xn, "%8.2lf"); dvsmy( x, xn, 2.0, z ); dvdump( stdout, "2x", z, xn, "%8.2lf"); printf("Magnitude of 2x: %7.2lf\n", dvmag( z, xn )); printf("dot product x.y: %7.2lf\n", dvdot( x, xn, y)); dmvmult( a, a_rows, a_cols, x, xn, z ); dvdump( stdout, "Ax", z, xn, "%8.2lf"); c = dmatrix( 1, a_rows, 1, b_cols ); bT = dmatrix( 1, b_cols, 1, b_rows ); dmtranspose( b, b_rows, b_cols, bT); dmdump( stdout, "Matrix B (transposed)", bT, b_cols, b_rows, "%8.2lf"); dmmult( a, a_rows, a_cols, bT, b_cols, b_rows, c); dmdump( stdout, "Matrix AB", c, a_rows, b_rows, "%8.2lf"); /* dmfillUT( a, a_rows, a_cols ); dmdump( stdout, "Symmetrified matrix A", a, a_rows, a_cols, "%8.2lf"); */ free_dmatrix( a, 1, a_rows, 1, a_cols); free_dmatrix( b, 1, b_rows, 1, b_cols); free_dmatrix( c, 1, a_rows, 1, b_cols); free_dvector( x, 1, xn ); free_dvector( y, 1, yn ); }
static void demean (Dump* a, Dump* f) /* ------------------------------------------------------------------------- * * Subtract mean values in "a" from field values in "f". * ------------------------------------------------------------------------- */ { int i, j; const int nfields = strlen (f -> field); const int npts = a -> np * a -> np * a -> nz * a -> nel; for (i = 0; i < nfields; i++) { j = _index (a -> field, f -> field[i]); dvsub (npts, f -> data[i], 1, a -> data[j], 1, f -> data[i], 1); } }
vec3 adv_att_control(quat setpoint, quat att, vec3 angle_rate) { static quat att_prev = UNIT_Q; static vec3 error_sat_prev = VEC0; static evec3 integral_out_prev = VEC0; vec3 error = qerror(setpoint, att); vec3 torques = VEC0; evec3 ctrl_signal; vec3 error_sat = vsat(error, integral_error_limit); evec3 integral_out; transmitData.x = PosGain.x; transmitData.y = VelGain.x; if (int_Disable) { evec3 v0 = VEC0; integral_out = v0; } else { integral_out = dvsum(dvsum(v_to_extended(error_sat_prev), v_to_extended(error_sat)), integral_out_prev); } integral_out_prev = integral_out; error_sat_prev = error_sat; integral_out.z = 0; ctrl_signal = dvsum( dvsub( v_to_extended(vfmul_e2e(error, PosGain)), //vimul2_e2e(angle_rate, VelGain) vmul8_8(angle_rate, VelGain) ), dvdiv(integral_out, int_gain_divide) ); att_prev = att; torques = evclip(ctrl_signal); // FIXME: esto esta mal, es para poder probar sin que moleste el yaw //torques.z = 0; return torques; }
void vad(float rc, float * lsf, float * rxx, float * sigpp, int frm_count, int prev_marker, int pprev_marker, int *marker, float * Energy_db) { float tmp[M]; float SD; float E_low; float dtemp; float dSE; float dSLE; float ZC; float COEF; float COEFZC; float COEFSD; float dSZC; float norm_energy; int i; /* compute the frame energy */ norm_energy = (float) 10.0 *(float) log10((float) (rxx[0] / (float) 240.0 + EPSI)); *Energy_db = norm_energy; /* compute the low band energy */ E_low = (float) 0.0; for (i = 1; i <= NP; i++) E_low = E_low + rxx[i] * lbf_corr[i]; E_low = rxx[0] * lbf_corr[0] + (float) 2.0 *E_low; if (E_low < (float) 0.0) E_low = (float) 0.0; E_low = (float) 10.0 *(float) log10((float) (E_low / (float) 240.0 + EPSI)); /* compute SD */ /* Normalize lsfs */ for (i = 0; i < M; i++) lsf[i] /= (float) 2. *PI; dvsub(lsf, MeanLSF, tmp, M); SD = dvdot(tmp, tmp, M); /* compute # zero crossing */ ZC = (float) 0.0f; dtemp = sigpp[ZC_START]; for (i = ZC_START + 1; i <= ZC_END; i++) { if (dtemp * sigpp[i] < (float) 0.0) { ZC = ZC + (float) 1.0; } dtemp = sigpp[i]; } ZC = ZC / (float) 80.0; /* Initialize and update Mins */ if (frm_count < 129) { if (norm_energy < Min) { Min = norm_energy; Prev_Min = norm_energy; } if ((frm_count % 8) == 0) { Min_buffer[(int)frm_count / 8 - 1] = Min; Min = FLT_MAX_G729; } } if ((frm_count % 8) == 0) { Prev_Min = Min_buffer[0]; for (i = 1; i < 15; i++) { if (Min_buffer[i] < Prev_Min) Prev_Min = Min_buffer[i]; } } if (frm_count >= 129) { if ((frm_count % 8) == 1) { Min = Prev_Min; Next_Min = FLT_MAX_G729; } if (norm_energy < Min) Min = norm_energy; if (norm_energy < Next_Min) Next_Min = norm_energy; if ((frm_count % 8) == 0) { for (i = 0; i < 15; i++) Min_buffer[i] = Min_buffer[i + 1]; Min_buffer[15] = Next_Min; Prev_Min = Min_buffer[0]; for (i = 1; i < 16; i++) { if (Min_buffer[i] < Prev_Min) Prev_Min = Min_buffer[i]; } } } if (frm_count <= INIT_FRAME) { if (norm_energy < (float) 21.0) { less_count++; *marker = NOISE; } else { *marker = VOICE; MeanE = (MeanE * ((float) (frm_count - less_count - 1)) + norm_energy) / (float) (frm_count - less_count); MeanSZC = (MeanSZC * ((float) (frm_count - less_count - 1)) + ZC) / (float) (frm_count - less_count); dvwadd(MeanLSF, (float) (frm_count - less_count - 1), lsf, (float) 1.0, MeanLSF, M); dvsmul(MeanLSF, (float) 1.0 / (float) (frm_count - less_count), MeanLSF, M); } } if (frm_count >= INIT_FRAME) { if (frm_count == INIT_FRAME) { MeanSE = MeanE - (float) 10.0; MeanSLE = MeanE - (float) 12.0; } dSE = MeanSE - norm_energy; dSLE = MeanSLE - E_low; dSZC = MeanSZC - ZC; if (norm_energy < (float) 21.0) { *marker = NOISE; } else { *marker = MakeDec(dSLE, dSE, SD, dSZC); } v_flag = 0; if ((prev_marker == VOICE) && (*marker == NOISE) && (norm_energy > MeanSE + (float) 2.0) && (norm_energy > (float) 21.0)) { *marker = VOICE; v_flag = 1; } if (flag == 1) { if ((pprev_marker == VOICE) && (prev_marker == VOICE) && (*marker == NOISE) && (fabs(prev_energy - norm_energy) <= (float) 3.0)) { count_ext++; *marker = VOICE; v_flag = 1; if (count_ext <= 4) flag = 1; else { flag = 0; count_ext = 0; } } } else flag = 1; if (*marker == NOISE) count_sil++; if ((*marker == VOICE) && (count_sil > 10) && ((norm_energy - prev_energy) <= (float) 3.0)) { *marker = NOISE; count_sil = 0; } if (*marker == VOICE) count_sil = 0; if ((norm_energy < MeanSE + (float) 3.0) && (frm_count > 128) && (!v_flag) && (rc < (float) 0.6)) *marker = NOISE; if ((norm_energy < MeanSE + (float) 3.0) && (rc < (float) 0.75) && (SD < (float) 0.002532959)) { count_update++; if (count_update < INIT_COUNT) { COEF = (float) 0.75; COEFZC = (float) 0.8; COEFSD = (float) 0.6; } else if (count_update < INIT_COUNT + 10) { COEF = (float) 0.95; COEFZC = (float) 0.92; COEFSD = (float) 0.65; } else if (count_update < INIT_COUNT + 20) { COEF = (float) 0.97; COEFZC = (float) 0.94; COEFSD = (float) 0.70; } else if (count_update < INIT_COUNT + 30) { COEF = (float) 0.99; COEFZC = (float) 0.96; COEFSD = (float) 0.75; } else if (count_update < INIT_COUNT + 40) { COEF = (float) 0.995; COEFZC = (float) 0.99; COEFSD = (float) 0.75; } else { COEF = (float) 0.995; COEFZC = (float) 0.998; COEFSD = (float) 0.75; } dvwadd(MeanLSF, COEFSD, lsf, (float) 1.0 - COEFSD, MeanLSF, M); MeanSE = COEF * MeanSE + ((float) 1.0 - COEF) * norm_energy; MeanSLE = COEF * MeanSLE + ((float) 1.0 - COEF) * E_low; MeanSZC = COEFZC * MeanSZC + ((float) 1.0 - COEFZC) * ZC; } if (((frm_count > 128) && ((MeanSE < Min) && (SD < (float) 0.002532959))) || (MeanSE > Min + (float) 10.0)) { MeanSE = Min; count_update = 0; } } prev_energy = norm_energy; return; }
void vad(struct vad_state_t * state, GFLOAT rc, GFLOAT *lsf, GFLOAT *rxx, GFLOAT *sigpp, int frm_count, int prev_marker, int pprev_marker, int *marker, GFLOAT *Energy_db) { GFLOAT tmp[M]; GFLOAT SD; GFLOAT E_low; GFLOAT dtemp; GFLOAT dSE; GFLOAT dSLE; GFLOAT ZC; GFLOAT COEF; GFLOAT COEFZC; GFLOAT COEFSD; GFLOAT dSZC; GFLOAT norm_energy; int i; /* compute the frame energy */ norm_energy = (F)10.0*(GFLOAT) log10((GFLOAT)( rxx[0]/(F)240.0 +EPSI)); *Energy_db = norm_energy ; /* compute the low band energy */ E_low = (F)0.0; for( i=1; i<= NP; i++) E_low = E_low + rxx[i]*lbf_corr[i]; E_low= rxx[0]*lbf_corr[0] + (F)2.0*E_low; if (E_low < (F)0.0) E_low = (F)0.0; E_low= (F)10.0*(GFLOAT) log10((GFLOAT) (E_low/(F)240.0+EPSI)); /* compute SD */ /* Normalize lsfs */ for(i=0; i<M; i++) lsf[i] /= (F)2.*PI; dvsub(lsf, state->MeanLSF,tmp,M); SD = dvdot(tmp,tmp,M); /* compute # zero crossing */ ZC = (F)0.0f; dtemp = sigpp[ZC_START]; for (i=ZC_START+1 ; i <= ZC_END ; i++) { if (dtemp*sigpp[i] < (F)0.0) { ZC= ZC +(F)1.0; } dtemp = sigpp[i]; } ZC = ZC/(F)80.0; /* Initialize and update Mins */ if( frm_count < 129 ) { if( norm_energy < state->Min ){ state->Min = norm_energy; state->Prev_Min = norm_energy; } if( (frm_count % 8) == 0){ state->Min_buffer[(int)frm_count/8 -1] = state->Min; state->Min = FLT_MAX_G729; } } if( (frm_count % 8) == 0){ state->Prev_Min = state->Min_buffer[0]; for ( i =1; i< 15; i++){ if ( state->Min_buffer[i] < state->Prev_Min ) state->Prev_Min = state->Min_buffer[i]; } } if( frm_count >= 129 ) { if( (frm_count % 8 ) == 1) { state->Min = state->Prev_Min; state->Next_Min = FLT_MAX_G729; } if( norm_energy < state->Min ) state->Min = norm_energy; if( norm_energy < state->Next_Min ) state->Next_Min = norm_energy; if( (frm_count % 8) == 0){ for ( i =0; i< 15; i++) state->Min_buffer[i] = state->Min_buffer[i+1]; state->Min_buffer[15] = state->Next_Min; state->Prev_Min = state->Min_buffer[0]; for ( i =1; i< 16; i++){ if ( state->Min_buffer[i] < state->Prev_Min ) state->Prev_Min = state->Min_buffer[i]; } } } if (frm_count <= INIT_FRAME){ if( norm_energy < (F)21.0){ state->less_count++; *marker = NOISE; } else{ *marker = VOICE; state->MeanE = (state->MeanE * ( (GFLOAT)(frm_count - state->less_count -1)) + norm_energy)/(GFLOAT) (frm_count - state->less_count); state->MeanSZC = (state->MeanSZC * ( (GFLOAT)(frm_count - state->less_count -1)) + ZC)/(GFLOAT) (frm_count - state->less_count); dvwadd(state->MeanLSF,(GFLOAT) (frm_count - state->less_count -1),lsf,(F)1.0,state->MeanLSF,M); dvsmul(state->MeanLSF,(F)1.0/(GFLOAT) (frm_count - state->less_count ), state->MeanLSF,M); } } if (frm_count >= INIT_FRAME ){ if (frm_count == INIT_FRAME ){ state->MeanSE = state->MeanE -(F)10.0; state->MeanSLE = state->MeanE -(F)12.0; } dSE = state->MeanSE - norm_energy; dSLE = state->MeanSLE - E_low; dSZC = state->MeanSZC - ZC; if( norm_energy < (F)21.0 ){ *marker = NOISE; } else{ *marker = vad_make_dec(dSLE, dSE, SD, dSZC ); } state->v_flag =0; if( (prev_marker == VOICE) && (*marker == NOISE) && (norm_energy > state->MeanSE + (F)2.0) && ( norm_energy>(F)21.0)){ *marker = VOICE; state->v_flag=1; } if((state->flag == 1) ){ if( (pprev_marker == VOICE) && (prev_marker == VOICE) && (*marker == NOISE) && (fabs(state->prev_energy - norm_energy)<= (F)3.0)){ state->count_ext++; *marker = VOICE; state->v_flag=1; if(state->count_ext <=4) state->flag =1; else{ state->flag =0; state->count_ext=0; } } } else state->flag =1; if(*marker == NOISE) state->count_sil++; if((*marker == VOICE) && (state->count_sil > 10) && ((norm_energy - state->prev_energy) <= (F)3.0)){ *marker = NOISE; state->count_sil=0; } if(*marker == VOICE) state->count_sil=0; if ((norm_energy < state->MeanSE + (F)3.0) && ( frm_count >128) &&( !state->v_flag) && (rc <(F)0.6) ) *marker = NOISE; if ((norm_energy < state->MeanSE + (F)3.0) && (rc <(F)0.75) && ( SD<(F)0.002532959)){ state->count_update++; if (state->count_update < INIT_COUNT){ COEF = (F)0.75; COEFZC = (F)0.8; COEFSD = (F)0.6; } else if (state->count_update < INIT_COUNT+10){ COEF = (F)0.95; COEFZC = (F)0.92; COEFSD = (F)0.65; } else if (state->count_update < INIT_COUNT+20){ COEF = (F)0.97; COEFZC = (F)0.94; COEFSD = (F)0.70; } else if (state->count_update < INIT_COUNT+30){ COEF = (F)0.99; COEFZC = (F)0.96; COEFSD = (F)0.75; } else if (state->count_update < INIT_COUNT+40){ COEF = (F)0.995; COEFZC = (F)0.99; COEFSD = (F)0.75; } else{ COEF = (F)0.995; COEFZC = (F)0.998; COEFSD = (F)0.75; } dvwadd(state->MeanLSF,COEFSD,lsf,(F)1.0 -COEFSD,state->MeanLSF,M); state->MeanSE = COEF * state->MeanSE+((F)1.0- COEF)*norm_energy; state->MeanSLE = COEF * state->MeanSLE+((F)1.0- COEF)*E_low; state->MeanSZC = COEFZC * state->MeanSZC+((F)1.0- COEFZC)*ZC; } if(((frm_count > 128) && (state->MeanSE < state->Min) && (SD < (F)0.002532959)) || (state->MeanSE > state->Min + (F)10.0) ) { state->MeanSE = state->Min; state->count_update = 0; } } state->prev_energy = norm_energy; return; }
void musdetect( int rate, FLOAT Energy, FLOAT *rc, int *lags, FLOAT *pgains, int stat_flg, int frm_count, int prev_vad, int *Vad, FLOAT LLenergy) { int i; static int count_music=0; static FLOAT Mcount_music=(F)0.0; static int count_consc=0; FLOAT sum1, sum2,std; static FLOAT MeanPgain =(F)0.5; short PFLAG1, PFLAG2, PFLAG; static int count_pflag=0; static FLOAT Mcount_pflag=(F)0.0; static int count_consc_pflag=0; static int count_consc_rflag=0; static FLOAT mrc[10]={(F)0.0,(F)0.0, (F)0.0, (F)0.0, (F)0.0, (F)0.0, (F)0.0, (F)0.0,(F)0.0,(F)0.0}; static FLOAT MeanSE =(F)0.0; FLOAT pderr, Lenergy , SD, tmp_vec[10]; FLOAT Thres; pderr =(F)1.0; for (i=0; i< 4; i++) pderr *= ((F)1.0 - rc[i]*rc[i]); dvsub(mrc,rc,tmp_vec,10); SD = dvdot(tmp_vec, tmp_vec,10); Lenergy = (F)10.0*(FLOAT)log10(pderr*Energy/(F)240.0 +EPSI); if( *Vad == NOISE ){ dvwadd(mrc,(F)0.9,rc,(F)0.1,mrc,10); MeanSE = (F)0.9*MeanSE + (F)0.1*Lenergy; } sum1 =(F)0.0; sum2 =(F)0.0; for(i=0; i<5; i++){ sum1 += (FLOAT) lags[i]; sum2 += pgains[i]; } sum1 = sum1/(F)5.0; sum2 = sum2/(F)5.0; std =(F)0.0; for(i=0; i<5; i++) std += sqr(((FLOAT) lags[i] - sum1)); std = (FLOAT)sqrt(std/(F)4.0); MeanPgain = (F)0.8*MeanPgain + (F)0.2*sum2; if ( rate == G729D) Thres = (F)0.73; else Thres = (F)0.63; if ( MeanPgain > Thres) PFLAG2 =1; else PFLAG2 =0; if ( std < (F)1.30 && MeanPgain > (F)0.45 ) PFLAG1 =1; else PFLAG1 =0; PFLAG= (INT16)( ((INT16)prev_vad & (INT16)(PFLAG1 | PFLAG2))| (INT16)(PFLAG2)); if( rc[1] <= (F)0.45 && rc[1] >= (F)0.0 && MeanPgain < (F)0.5) count_consc_rflag++; else count_consc_rflag =0; if( stat_flg== 1 && (*Vad == VOICE)) count_music++; if ((frm_count%64) == 0 ){ if( frm_count == 64) Mcount_music = (FLOAT)count_music; else Mcount_music = (F)0.9*Mcount_music + (F)0.1*(FLOAT)count_music; } if( count_music == 0) count_consc++; else count_consc = 0; if( count_consc > 500 || count_consc_rflag > 150) Mcount_music = (F)0.0; if ((frm_count%64) == 0) count_music = 0; if( PFLAG== 1 ) count_pflag++; if ((frm_count%64) == 0 ){ if( frm_count == 64) Mcount_pflag = (FLOAT)count_pflag; else{ if( count_pflag > 25) Mcount_pflag = (F)0.98*Mcount_pflag + (F)0.02*(FLOAT)count_pflag; else if( count_pflag > 20) Mcount_pflag = (F)0.95*Mcount_pflag + (F)0.05*(FLOAT)count_pflag; else Mcount_pflag = (F)0.90*Mcount_pflag + (F)0.10*(FLOAT)count_pflag; } } if( count_pflag == 0) count_consc_pflag++; else count_consc_pflag = 0; if( count_consc_pflag > 100 || count_consc_rflag > 150) Mcount_pflag = (F)0.0; if ((frm_count%64) == 0) count_pflag = 0; if (rate == G729E){ if( SD > (F)0.15 && (Lenergy -MeanSE)> (F)4.0 && (LLenergy> 50.0) ) *Vad =VOICE; else if( (SD > (F)0.38 || (Lenergy -MeanSE)> (F)4.0 ) && (LLenergy> 50.0)) *Vad =VOICE; else if( (Mcount_pflag >= (F)10.0 || Mcount_music >= (F)5.0 || frm_count < 64) && (LLenergy> 7.0)) *Vad =VOICE; } return; }
void Precon_Stokes(Element_List *U, Bsystem *B, double *r, double *z){ switch(B->Precon){ case Pre_Diag: dvmul(B->Pmat->info.diag.ndiag,B->Pmat->info.diag.idiag,1,r,1,z,1); break; case Pre_Block:{ register int i; int eDIM = U->fhead->dim(); int nedge = B->Pmat->info.block.nedge; int info,l,cnt; double *tmp = dvector(0,LGmax-1); static double *pinv; dcopy(B->nsolve,r,1,z,1); #ifdef BVERT dpptrs('U',B->Pmat->info.block.nvert,1,B->Pmat->info.block.ivert, z,B->Pmat->info.block.nvert,info); #else dvmul(B->Pmat->info.block.nvert,B->Pmat->info.block.ivert,1,z,1,z,1); #endif cnt = B->Pmat->info.block.nvert; for(i = 0; i < nedge; ++i){ l = B->Pmat->info.block.Ledge[i]; dpptrs('U',l,1,B->Pmat->info.block.iedge[i],z+cnt,l,info); cnt += l; } if(!pinv){ pinv = dvector(0,B->nsolve-cnt-1); for(i = 0; i < B->nsolve-cnt; ++i) pinv[i] = U->flist[i]->get_1diag_massmat(0); dvrecp(B->nsolve-cnt,pinv,1,pinv,1); } /* finally multiply pressure dof by inverse of mass matrix */ dvmul(B->nsolve-cnt,pinv,1,r+cnt,1,z+cnt,1); free(tmp); } break; case Pre_None: dcopy(B->nsolve,r,1,z,1); break; case Pre_Diag_Stokes:{ register int i,j,k; double *tmp = dvector(0,B->nglobal-1),*sc,*b,*p,*wk; int eDIM = U->fhead->dim(); int **bmap = B->bmap; int nel = B->nel,asize,Nbmodes,info; if(eDIM == 2) wk = dvector(0,eDIM*4*LGmax); else wk = dvector(0,eDIM*6*LGmax*LGmax); dzero(B->nglobal,tmp,1); dcopy(B->nsolve-nel,r,1,tmp,1); /* multiply by invc */ dvmul(B->nsolve-nel,B->Pmat->info.diagst.idiag,1,tmp,1,tmp,1); /* multiply by b^T */ sc = B->signchange; p = z + B->nsolve-nel; dcopy(nel,r+B->nsolve-nel,1,p,1); dneg (nel-1,p+1,1); /* start at 1 to omit singular pressure point */ if(!(B->singular)) error_msg(issue in setting singular modes in Stokes_Precon not resolved); for(j = 1; j < nel; ++j){ Nbmodes = U->flist[j]->Nbmodes; asize = Nbmodes*eDIM+1; dzero (asize,wk,1); dgathr(eDIM*Nbmodes, tmp, bmap[j], wk); dvmul (eDIM*Nbmodes, sc, 1, wk, 1, wk, 1); b = B->Gmat->a[U->flist[j]->geom->id] + asize*(asize-1)/2; p[j] += ddot(asize-1,b,1,wk,1); sc += asize; } /* solve for p */ dpptrs('L', nel, 1, B->Pmat->info.diagst.binvcb,p,nel,info); /* generate initial vector as tmp = B p */ sc = B->signchange; dzero(B->nglobal,tmp,1); /* start from 1 due to singular pressure system */ for(k = 1; k < nel; ++k){ Nbmodes = U->flist[k]->Nbmodes; asize = Nbmodes*eDIM+1; b = B->Gmat->a[U->flist[k]->geom->id] + asize*(asize-1)/2; dsmul(asize-1,p[k],b,1,wk,1); for(i = 0; i < eDIM*Nbmodes; ++i) tmp[bmap[k][i]] += sc[i]*wk[i]; sc += asize; } /* set z = r - tmp */ dvsub(B->nsolve-nel,r,1,tmp,1,z,1); /* u = invc*z */ dvmul(B->nsolve-nel,B->Pmat->info.diagst.idiag,1,z,1,z,1); free(tmp); free(wk); } break; case Pre_Block_Stokes:{ register int i,j,k; double *tmp = dvector(0,B->nglobal-1),*sc,*b,*p,*wk; int eDIM = U->fhead->dim(),cnt,l; int **bmap = B->bmap; int nel = B->nel,asize,Nbmodes,info; int nedge = B->Pmat->info.blockst.nedge; if(eDIM == 2) wk = dvector(0,eDIM*4*LGmax); else wk = dvector(0,eDIM*6*LGmax*LGmax); dzero(B->nglobal,tmp,1); dcopy(B->nsolve-nel,r,1,tmp,1); /* multiply by invc */ #ifdef BVERT dpptrs('U',B->Pmat->info.blockst.nvert,1,B->Pmat->info.blockst.ivert, tmp,B->Pmat->info.blockst.nvert,info); #else dvmul(B->Pmat->info.blockst.nvert,B->Pmat->info.blockst.ivert,1, tmp,1,tmp,1); #endif cnt = B->Pmat->info.blockst.nvert; for(i = 0; i < nedge; ++i){ l = B->Pmat->info.blockst.Ledge[i]; dpptrs('U',l,1,B->Pmat->info.blockst.iedge[i],tmp+cnt,l,info); cnt += l; } /* multiply by b^T */ sc = B->signchange; p = z + B->nsolve-nel; dcopy(nel,r+B->nsolve-nel,1,p,1); dneg(nel-1,p+1,1); if(!(B->singular)) error_msg(issue in setting singular modes in Stokes_Precon not resolved); /* start at 1 to omit singular pressure point */ for(j = 1; j < nel; ++j){ Nbmodes = U->flist[j]->Nbmodes; asize = Nbmodes*eDIM+1; dzero (asize,wk,1); dgathr(eDIM*Nbmodes, tmp, bmap[j], wk); dvmul (eDIM*Nbmodes, sc, 1, wk, 1, wk, 1); b = B->Gmat->a[U->flist[j]->geom->id] + asize*(asize-1)/2; p[j] += ddot(asize-1,b,1,wk,1); sc += asize; } /* solve for p */ dpptrs('L', nel, 1, B->Pmat->info.blockst.binvcb,p,nel,info); /* generate initial vector as tmp = B p */ sc = B->signchange; dzero(B->nglobal,tmp,1); /* start from 1 due to singular pressure system */ for(k = 1; k < nel; ++k){ Nbmodes = U->flist[k]->Nbmodes; asize = Nbmodes*eDIM+1; b = B->Gmat->a[U->flist[k]->geom->id] + asize*(asize-1)/2; dsmul(asize-1,p[k],b,1,wk,1); for(i = 0; i < eDIM*Nbmodes; ++i) tmp[bmap[k][i]] += sc[i]*wk[i]; sc += asize; } /* set z = r - tmp */ dvsub(B->nsolve-nel,r,1,tmp,1,z,1); /* u = invc*z */ #ifdef BVERT dpptrs('U',B->Pmat->info.blockst.nvert,1,B->Pmat->info.blockst.ivert, z,B->Pmat->info.blockst.nvert,info); #else dvmul(B->Pmat->info.blockst.nvert,B->Pmat->info.blockst.ivert,1,z,1,z,1); #endif cnt = B->Pmat->info.blockst.nvert; for(i = 0; i < nedge; ++i){ l = B->Pmat->info.blockst.Ledge[i]; dpptrs('U',l,1,B->Pmat->info.blockst.iedge[i],z+cnt,l,info); cnt += l; } free(tmp); free(wk); } break; default: fprintf(stderr,"Preconditioner (Precon=%d) not known\n",B->Precon); exit(-1); } }
static void setupRHS (Element_List **V, Element_List **Vf,double *rhs, double *u0, Bndry **Vbc, Bsystem **Vbsys){ register int i,k; int N,nbl; int eDIM = V[0]->flist[0]->dim(); Bsystem *PB = Vbsys[eDIM],*B = Vbsys[0]; int nel = B->nel,info; int **ipiv = B->Gmat->cipiv; double **binvc = B->Gmat->binvc; double **invc = B->Gmat->invc; double ***dbinvc = B->Gmat->dbinvc; double **p_binvc = PB->Gmat->binvc; Element *E,*E1; Bndry *Ebc; double *tmp; if(eDIM == 2) tmp = dvector(0,max(8*LGmax,(LGmax-2)*(LGmax-2))); else tmp = dvector(0,18*LGmax*LGmax); B = Vbsys[0]; PB = Vbsys[eDIM]; #ifdef __LIBCATAMOUNT__ st1 = dclock(); #else st1 = clock(); #endif /* save initial condition */ saveinit(V,u0,Vbsys); Timing1("saveinit.........."); /* take inner product if in physical space */ for(i = 0; i < eDIM; ++i){ if(Vf[i]->fhead->state == 'p') Vf[i]->Iprod(Vf[i]); } /* zero pressure field */ dzero(Vf[eDIM]->hjtot,Vf[eDIM]->base_hj,1); Timing1("zeroing..........."); /* condense out interior from u-vel + p */ for(i = 0; i < eDIM; ++i) for(E=Vf[i]->fhead;E;E=E->next){ nbl = E->Nbmodes; N = E->Nmodes - nbl; if(N) dgemv('T', N, nbl, -1., binvc[E->geom->id], N, E->vert->hj+nbl, 1, 1., E->vert->hj,1); } Timing1("first condense(v)."); for(i = 0; i < eDIM; ++i) for(E=Vf[i]->fhead;E;E=E->next){ nbl = E->Nbmodes; N = E->Nmodes - nbl; if(N) { E1 = Vf[eDIM]->flist[E->id]; if(B->lambda->wave){ dcopy(N,E->vert->hj+nbl,1,tmp,1); dgetrs('N', N, 1, invc[E->geom->id], N,ipiv[E->geom->id],tmp,N,info); dgemv('T', N, E1->Nmodes, -1., dbinvc[i][E->geom->id], N, tmp, 1, 1., E1->vert->hj,1); } else{ dgemv('T', N, E1->Nmodes, -1., dbinvc[i][E->geom->id], N, E->vert->hj+nbl, 1, 1., E1->vert->hj,1); } } } Timing1("first condense(p)."); /* add flux terms */ for(i = 0; i < eDIM; ++i) for(Ebc = Vbc[i]; Ebc; Ebc = Ebc->next) if(Ebc->type == 'F' || Ebc->type == 'R') Vf[i]->flist[Ebc->elmt->id]->Add_flux_terms(Ebc); /* second level of factorisation to orthogonalise basis to p */ for(E=Vf[eDIM]->fhead;E;E=E->next){ E1 = Vf[0]->flist[E->id]; nbl = eDIM*E1->Nbmodes + 1; N = E->Nmodes-1; dgemv('T', N, nbl, -1.0, p_binvc[E->geom->id], N, E->vert->hj+1, 1, 0.0, tmp,1); for(i = 0; i < eDIM; ++i){ E1 = Vf[i]->flist[E->id]; dvadd(E1->Nbmodes,tmp+i*E1->Nbmodes,1,E1->vert->hj,1,E1->vert->hj,1); } E->vert->hj[0] += tmp[nbl-1]; } Timing1("second condense..."); /* subtract boundary initial conditions */ if(PB->smeth == iterative){ double **wk; double **a = PB->Gmat->a; if(eDIM == 2) wk = dmatrix(0,1,0,eDIM*4*LGmax); else wk = dmatrix(0,1,0,eDIM*6*LGmax*LGmax); for(k = 0; k < nel; ++k){ nbl = V[0]->flist[k]->Nbmodes; /* gather vector */ for(i = 0; i < eDIM; ++i) dcopy(nbl,V[i]->flist[k]->vert->hj,1,wk[0]+i*nbl,1); dspmv('U',eDIM*nbl+1,1.0,a[V[0]->flist[k]->geom->id], wk[0],1,0.0,wk[1],1); /* subtract of Vf */ for(i = 0; i < eDIM; ++i) dvsub(nbl,Vf[i]->flist[k]->vert->hj,1,wk[1]+i*nbl,1, Vf[i]->flist[k]->vert->hj,1); Vf[eDIM]->flist[k]->vert->hj[0] -= wk[1][eDIM*nbl]; } GathrBndry_Stokes(Vf,rhs,Vbsys); free_dmatrix(wk,0,0); } else{ if(Vbc[0]->DirRHS){ GathrBndry_Stokes(Vf,rhs,Vbsys); /* subtract of bcs */ dvsub(PB->nsolve,rhs,1,Vbc[0]->DirRHS,1,rhs,1); /* zero ic vector */ dzero(PB->nsolve,u0,1); } else{ /* zero out interior components since only deal with boundary initial conditions (interior is always direct) */ for(i = 0; i < eDIM; ++i) for(E = V[i]->fhead; E; E = E->next){ nbl = E->Nbmodes; N = E->Nmodes - nbl; dzero(N, E->vert->hj + nbl, 1); } /* inner product of divergence for pressure forcing */ for(i = 0; i < eDIM; ++i) V[i]->Trans(V[i], J_to_Q); V[0]->Grad(V[eDIM],0,0,'x'); V[1]->Grad(0,Vf[eDIM],0,'y'); dvadd(V[1]->htot,V[eDIM]->base_h,1,Vf[eDIM]->base_h,1, V[eDIM]->base_h,1); if(eDIM == 3){ V[2]->Grad(0,V[eDIM],0,'z'); dvadd(V[2]->htot,V[eDIM]->base_h,1,Vf[eDIM]->base_h,1, V[eDIM]->base_h,1); } #ifndef PCONTBASE for(k = 0; k < nel; ++k) V[eDIM]->flist[k]->Ofwd(*V[eDIM]->flist[k]->h, V[eDIM]->flist[k]->vert->hj, V[eDIM]->flist[k]->dgL); #else V[eDIM]->Iprod(V[eDIM]); #endif for(i = 0; i < eDIM; ++i){ for(k = 0; k < nel; ++k){ E = V[i]->flist[k]; nbl = E->Nbmodes; N = E->Nmodes - nbl; E->HelmHoltz(PB->lambda+k); dscal(E->Nmodes, -B->lambda[k].d, E->vert->hj, 1); if(N) { /* condense out interior terms in velocity */ dgemv('T', N, nbl, -1., binvc[E->geom->id], N, E->vert->hj+nbl, 1, 1., E->vert->hj,1); /* condense out interior terms in pressure*/ E1 = V[eDIM]->flist[k]; if(B->lambda->wave){ dcopy(N,E->vert->hj+nbl,1,tmp,1); dgetrs('N',N,1,invc[E->geom->id],N,ipiv[E->geom->id],tmp,N,info); dgemv('T', N, E1->Nmodes, -1., dbinvc[i][E->geom->id], N, tmp, 1, 1., E1->vert->hj,1); } else{ dgemv('T', N, E1->Nmodes, -1., dbinvc[i][E->geom->id], N, E->vert->hj+nbl, 1, 1., E1->vert->hj,1); } } } } /* second level of factorisation to orthogonalise basis to p */ /* p - vel */ for(E=V[eDIM]->fhead;E;E=E->next){ E1 = V[0]->flist[E->id]; nbl = eDIM*E1->Nbmodes + 1; N = E->Nmodes-1; dgemv('T', N, nbl, -1.0, p_binvc[E->geom->id], N, E->vert->hj+1, 1, 0.0, tmp,1); for(i = 0; i < eDIM; ++i){ E1 = V[i]->flist[E->id]; dvadd(E1->Nbmodes,tmp+i*E1->Nbmodes,1,E1->vert->hj,1,E1->vert->hj,1); dvadd(E1->Nbmodes,E1->vert->hj,1,Vf[i]->flist[E->id]->vert->hj,1, Vf[i]->flist[E->id]->vert->hj,1); } Vf[eDIM]->flist[E->id]->vert->hj[0] += E->vert->hj[0] + tmp[nbl-1]; } Timing1("bc condense......."); GathrBndry_Stokes(Vf,rhs,Vbsys); Timing1("GatherBndry......."); } } /* finally copy inner product of f into v for inner solve */ for(i = 0; i < eDIM; ++i) for(E = V[i]->fhead; E; E= E->next){ nbl = E->Nbmodes; N = E->Nmodes - nbl; E1 = Vf[i]->flist[E->id]; dcopy(N, E1->vert->hj+nbl, 1, E->vert->hj+nbl, 1); } for(E = Vf[eDIM]->fhead; E; E = E->next){ E1 = V[eDIM]->flist[E->id]; dcopy(E->Nmodes,E->vert->hj,1,E1->vert->hj,1); } dzero(PB->nglobal-PB->nsolve, rhs + PB->nsolve, 1); free(tmp); }