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);
}
