void boardDumperThread::run() { //printf("Entering the main thread\n"); //enc->getEncoders(data); //Bottle bData; //for (int i = 0; i < numberOfJointsRead; i++) // { // dataRead[i] = data[dataMap[i]]; // bData.addDouble(dataRead[i]); // } //port->write(bData); if (getter) { //printf("Getter is getting something\n"); getter -> getData(data); //fprintf(stderr, "Time is %lf \n", stmp.getTime()); Bottle bData; for (int i = 0; i < numberOfJointsRead; i++) { //printf("%.2f \n", data[dataMap[i]]); dataRead[i] = data[dataMap[i]]; bData.addDouble(dataRead[i]); } if (getter->getStamp(stmp)) { if (stmp.isValid()) { port->setEnvelope(stmp); } else { //stmp.update(); stmp=Stamp(-1,0.0); port->setEnvelope(stmp); } } else { fprintf(stderr, "boardDumperThread::warning. Trying to get a stamp without a proper IPreciselyTimed defined. \n"); } if (logFile) { char buff [20]; sprintf(buff,"%d ",stmp.getCount()); fputs (buff,logFile); sprintf(buff,"%f ",stmp.getTime()); fputs (buff,logFile); fputs (bData.toString().c_str(),logFile); fputs ("\n",logFile); } port->write(bData); } }
double FormJf(Mode* ms, Geometry geo, Vec v, Vec f, double ftol, int printnewton){ Mode m = ms[0]; int lasing = m->lasing, Nm; VecGetSize(v, &Nm); Nm /= NJ(geo); Mat J = m->J; // for multimode, all m share same J if(lasing) ComputeGain(geo, ms, Nm); // do this before naming scratch vectors! // ================== name scratch vectors ================== // Vec vpsisq = geo->vscratch[3], // only form this later if needed vIpsi = geo->vscratch[2]; Vec dfR, dfI; if(Nm == 1){ dfR = geo->vscratch[0]; dfI = geo->vscratch[1]; }else{ dfR = geo->vNhscratch[0]; dfI = geo->vNhscratch[1]; } // =========== linear J to compute residual ========= // MatRetrieveValues(J); int ih, jh, kh, ir, jr, jc; for(ih=0; ih<Nm; ih++){ m = ms[ih]; VecSet(dfR, 0.0); VecSet(dfI, 0.0); LinearDerivative(m, geo, dfR, dfI, ih); SetJacobian(geo, J, dfR, -2, 0, ih); SetJacobian(geo, J, dfI, -2, 1, ih); } // row derivatives already added in add placeholders! AssembleMat(J); MatMult(J, v, f); for(kh = 0; kh<Nm; kh++) for(ir=0; ir<2; ir++) VecSetValue(f, kh*NJ(geo) + Nxyzcr(geo)+ir, 0.0, INSERT_VALUES); // assume psi(ifix) = something + i 0. Note we do not explicitly assume what "something" is, so in principle psi can have any normalization, as long as Im psi(ifix) =0. AssembleVec(f); double fnorm; VecNorm(f, NORM_2, &fnorm); if( printnewton ) PetscPrintf(PETSC_COMM_WORLD, "|f| = %1.6e;", fnorm); // no \n here to make room for timing printf statement immediately afterwards if(Nm==2) //DEBUG PetscPrintf(PETSC_COMM_WORLD, " DEBUG: |yw| c = (%g, %g)", get_c(ms[0]) / cabs(gamma_w(ms[0], geo)), get_c(ms[1]) / cabs(gamma_w(ms[1], geo)) ); // see note under "%s at D = %g:" print statement in Newton.c if(fnorm < ftol ) return fnorm; // TODO: deleted old integral routine. Write new one here. // =============== column derivatives ==================== for(jh=0; jh<Nm; jh++){ Mode mj = ms[jh]; VecSet(dfR, 0.0); VecSet(dfI, 0.0); if(Nm > 1) // hack: only recompute vpsisq if ComputeGain didn't already do it, i.e. for multimode VecDotMedium(geo, mj->vpsi, mj->vpsi, vpsisq, geo->vMscratch[0]); for(ih=0; ih<Nm; ih++){ Mode mi = ms[ih]; TimesI(geo, mi->vpsi, vIpsi); ColumnDerivative(mi, mj, geo, dfR, dfI, vIpsi, vpsisq, ih); } SetJacobian(geo, J, dfR, -1, 0, jh); SetJacobian(geo, J, dfI, -1, 1, jh); } //================ tensor derivatives ================ if(lasing){ Vec vpsibra = vpsisq; vpsisq = 0; for(jh=0; jh<Nm; jh++){ Mode mj = ms[jh]; for(jr=0; jr<2; jr++) for(jc=0; jc< geo->gN.Nc; jc++){ VecCopy(mj->vpsi, vpsibra); Stamp(geo, vpsibra, jc, jr, geo->vMscratch[0]); VecSet(dfR, 0.0); for(ih=0; ih<Nm; ih++){ Mode mi = ms[ih]; TimesI(geo, mi->vpsi, vIpsi); TensorDerivative(mi, mj, geo, dfR, vpsibra, vIpsi, ih); } SetJacobian(geo, J, dfR, jc, jr, jh); } } } if(geo->interference != 0.0 && Nm == 2){ // column interference derivative for(jh=0; jh<Nm; jh++){ Mode mj = ms[jh]; VecSet(dfR, 0.0); VecSet(dfI, 0.0); for(ih=0; ih<Nm; ih++){ Mode mi = ms[ih]; TimesI(geo, mi->vpsi, vIpsi); // assume vscratch[6] computed above in ComputeGain ColumnDerivative(mi, mj, geo, dfR, dfI, vIpsi, geo->vscratch[6], ih); } double thisc = get_c(mj), otherc = get_c(ms[1-jh]); AssembleVec(dfI); // hack, so VecScale can work. Usually SetJacobian does the assembling VecScale( dfI, otherc / thisc); // factor of two already included AssembleVec(dfR); // hack; this is usually called in SetJacobian, but here we are not setting the dfR part. If we don't assemble here, then ColumnDerivative will complain the next time we try to insert values into dfR. SetJacobian(geo, J, dfI, -1, 1, jh); } // tensor interference derivative for(jc=0; jc< geo->gN.Nc; jc++){ // shifted order of loops around for convenience, but this is actually unnecessary Vec vpsibra = geo->vscratch[3], vcos = geo->vscratch[4], vsin = geo->vscratch[5]; for(jh=0; jh<Nm; jh++) for(jr=0; jr<2; jr++) { VecCopy( ms[1-jh]->vpsi, vcos); Stamp(geo, vcos, jc, jr, geo->vMscratch[0]); VecCopy( ms[1-jh]->vpsi, vsin); Stamp(geo, vsin, jc, 1-jr, geo->vMscratch[0]); //d/dE1R = cos E2R - sin E2I //d/dE1I = cos E2I + sin E2R //the other two are (1 <-> 2, sin <-> -sin) double sign = (jh == jr ? -1.0 : 1.0); double costh = cos(geo->interference), sinth = sin(geo->interference); VecScale( vsin, sign * sinth); VecWAXPY( vpsibra, costh, vcos, vsin); // this function has awkward syntax, and VecAXPBY is even worse VecSet(dfR, 0.0); for(ih=0; ih<Nm; ih++){ Mode mi = ms[ih]; TimesI(geo, mi->vpsi, vIpsi); TensorDerivative(mi, ms[0], geo, dfR, vpsibra, vIpsi, ih); } // will have factor of c[0]^2 double thisc = get_c(ms[0]), otherc = get_c(ms[1]); AssembleVec(dfR); // same hack. see above VecScale( dfR, otherc / thisc); // factor of two already included SetJacobian(geo, J, dfR, jc, jr, jh); } } PetscPrintf(PETSC_COMM_WORLD, "DEBUG: interference code called"); } AssembleMat(J); return fnorm; }