scalar vec_dist2(const vector *v1, const vector *v2) { return SQ(v1->x - v2->x) + SQ(v1->y - v2->y) + SQ(v1->z - v2->z); }
/** * Very simple test: pick random skyposition, compute a_i, b_i using * once LALComputeAM() and once LALNewGetAMCoeffs(), and look at the errors * sum_i (a_i - a_i')^2 */ int main(int argc, char *argv[]) { LALStatus XLAL_INIT_DECL(status); int opt; /* Command-line option. */ LIGOTimeGPS startTime = {714180733, 0}; REAL8 duration = 180000; /* 50 hours */ REAL8 Tsft = 1800; /* assume 30min SFTs */ LIGOTimeGPSVector *timestamps = NULL; DetectorStateSeries *detStates = NULL; SkyPosition XLAL_INIT_DECL(skypos); EphemerisData XLAL_INIT_DECL(edat); BarycenterInput XLAL_INIT_DECL(baryinput); LALDetector *det = NULL; AMCoeffs XLAL_INIT_DECL(AMold); AMCoeffs XLAL_INIT_DECL(AMnew1); AMCoeffs XLAL_INIT_DECL(AMnew2); REAL8 alpha, delta; AMCoeffsParams XLAL_INIT_DECL(amParams); EarthState earth; UINT4 i; REAL8 maxerr01, maxerr02, maxerr12, averr01, averr02, averr12; REAL8 tolerance = 1e-2; /* be generous: allow 1% error */ struct tms buf; const CHAR *sites[] = {"H1", "L1", "V2", "G1", "T1" }; REAL8 sinzeta; /* zeta = IFO opening angle */ UINT4 pickedSite; BOOLEAN ignoreErrors = 0; /* Don't fail if tolerance exceeded */ UINT4 numChecks = 1; /* Number of times to check */ char earthEphem[] = TEST_DATA_DIR "earth00-19-DE405.dat.gz"; char sunEphem[] = TEST_DATA_DIR "sun00-19-DE405.dat.gz"; /* ----- old testing code to use 9 degree earth rotations ----- */ /* startTime.gpsSeconds = 714275242; duration = 86164; Tsft = 2154.1; */ while ((opt = LALgetopt( argc, argv, "n:qv:" )) != -1) { switch (opt) { case 'v': /* set lalDebugLevel */ break; case 'q': /* don't fail if tolerance exceeded */ ignoreErrors = 1; break; case 'n': /* number of times to check */ numChecks = atoi( LALoptarg ); break; } } /* init random-generator */ srand ( times(&buf) ); /* ----- init ephemeris ----- */ edat.ephiles.earthEphemeris = earthEphem; edat.ephiles.sunEphemeris = sunEphem; SUB ( LALInitBarycenter(&status, &edat), &status); /* ----- get timestamps ----- */ SUB ( LALMakeTimestamps ( &status, ×tamps, startTime, duration, Tsft ), &status ); /* ----- allocate memory for AM-coeffs ----- */ AMold.a = XLALCreateREAL4Vector ( timestamps->length ); AMold.b = XLALCreateREAL4Vector ( timestamps->length ); AMnew1.a = XLALCreateREAL4Vector ( timestamps->length ); AMnew1.b = XLALCreateREAL4Vector ( timestamps->length ); AMnew2.a = XLALCreateREAL4Vector ( timestamps->length ); AMnew2.b = XLALCreateREAL4Vector ( timestamps->length ); while ( numChecks-- ) { /* ----- pick detector-site at random ----- */ pickedSite = floor( 5 * (1.0 * rand() / (RAND_MAX + 1.0) ) ); /* int in [0,5) */ /* NOTE: contrary to ComputeAM() and LALGetAMCoffs(), the new function LALNewGetAMCoeffs() * computes 'a * sinzeta' and 'b * sinzeta': for the comparison we therefore need to correct * for GEO's opening-angle of 94.33degrees [JKS98]: */ if ( ! strcmp ( sites[pickedSite], "G1" ) ) sinzeta = 0.997146; else sinzeta = 1; if ( ( det = XLALGetSiteInfo ( sites[pickedSite] )) == NULL ) { XLALPrintError ("\nCall to XLALGetSiteInfo() has failed for site = '%s'... \n\n", sites[pickedSite]); return NEWGETAMCOEFFSTEST_ESUB; } /* ----- pick skyposition at random ----- */ alpha = LAL_TWOPI * (1.0 * rand() / ( RAND_MAX + 1.0 ) ); /* uniform in [0, 2pi) */ delta = LAL_PI_2 - acos ( 1 - 2.0 * rand()/RAND_MAX ); /* sin(delta) uniform in [-1,1] */ /* ----- old testing code to put source overhead ----- */ /* alpha = det->frDetector.vertexLongitudeRadians; delta = det->frDetector.vertexLatitudeRadians; */ /* ===== compute AM-coeffs the 'old way': ===== */ baryinput.site.location[0] = det->location[0]/LAL_C_SI; baryinput.site.location[1] = det->location[1]/LAL_C_SI; baryinput.site.location[2] = det->location[2]/LAL_C_SI; baryinput.alpha = alpha; baryinput.delta = delta; baryinput.dInv = 0.e0; /* amParams structure to compute a(t) and b(t) */ amParams.das = (LALDetAndSource *)LALMalloc(sizeof(LALDetAndSource)); amParams.das->pSource = (LALSource *)LALMalloc(sizeof(LALSource)); amParams.baryinput = &baryinput; amParams.earth = &earth; amParams.edat = &edat; amParams.das->pDetector = det; amParams.das->pSource->equatorialCoords.longitude = alpha; amParams.das->pSource->equatorialCoords.latitude = delta; amParams.das->pSource->orientation = 0.0; amParams.das->pSource->equatorialCoords.system = COORDINATESYSTEM_EQUATORIAL; amParams.polAngle = 0; SUB (LALComputeAM ( &status, &AMold, timestamps->data, &amParams), &status); /* ===== compute AM-coeffs the 'new way' using LALNewGetAMCoeffs() */ /* ----- get detector-state series ----- */ SUB ( LALGetDetectorStates (&status, &detStates, timestamps, det, &edat, 0 ), &status ); skypos.system = COORDINATESYSTEM_EQUATORIAL; skypos.longitude = alpha; skypos.latitude = delta; /* the 'new' and the 'newer' way ... */ SUB ( LALGetAMCoeffs ( &status, &AMnew1, detStates, skypos ), &status ); /* 'new1' */ SUB ( LALNewGetAMCoeffs ( &status, &AMnew2, detStates, skypos ), &status ); /* 'new2' */ /* ===== analyse relative errors ===== */ maxerr01 = maxerr02 = maxerr12 = 0; /* errors between 0='old', 1='new1', 2='new2' */ averr01 = averr02 = averr12 = 0; for ( i=0; i < timestamps->length; i ++ ) { /* printf("GPS time: %d s %d ns; GMST in radians: %f\n", detStates->data[i].tGPS.gpsSeconds, detStates->data[i].tGPS.gpsNanoSeconds, fmod(detStates->data[i].earthState.gmstRad,LAL_TWOPI)); printf("Old AM coeffs: a=%f, b=%f\nNew AM coeffs: a=%f, b=%f\nNEWER AM coeffs: a=%f b=%f", AMold.a->data[i], AMold.b->data[i], AMnew.a->data[i], AMnew.b->data[i], AMnewer.a->data[i], AMnewer.b->data[i]); */ REAL8 thisErr; /* compare 0-1 */ thisErr = sqrt( SQ ( AMold.a->data[i] - AMnew1.a->data[i] ) / AMold.A ); averr01 += thisErr; maxerr01 = MYMAX( thisErr, maxerr01 ); thisErr = sqrt( SQ ( AMold.b->data[i] - AMnew1.b->data[i] ) / AMold.B ); averr01 += thisErr; maxerr01 = MYMAX( thisErr, maxerr01 ); /* compare 0-2 */ thisErr = sqrt( SQ ( AMold.a->data[i] - AMnew2.a->data[i]/sinzeta ) / AMold.A ); averr02 += thisErr; maxerr02 = MYMAX( thisErr, maxerr02 ); thisErr = sqrt( SQ ( AMold.b->data[i] - AMnew2.b->data[i]/sinzeta ) / AMold.B ); averr02 += thisErr; maxerr02 = MYMAX( thisErr, maxerr02 ); /* compare 1-2 */ thisErr = sqrt( SQ ( AMnew1.a->data[i] - AMnew2.a->data[i]/sinzeta ) / AMold.A ); averr12 += thisErr; maxerr12 = MYMAX( thisErr, maxerr12 ); thisErr = sqrt( SQ ( AMnew1.b->data[i] - AMnew2.b->data[i]/sinzeta ) / AMold.B ); averr12 += thisErr; maxerr12 = MYMAX( thisErr, maxerr12 ); } averr01 /= 2.0 * timestamps->length; averr02 /= 2.0 * timestamps->length; averr12 /= 2.0 * timestamps->length; if ( lalDebugLevel ) { printf ("Parameters: IFO = %s, skypos = [%g, %g]\n", sites[pickedSite], alpha, delta ); printf ("Maximal relative errors: maxerr(0-1) = %g %%, maxerr(0-2) = %g %% maxerr(1-2) = %g %%\n", 100.0 * maxerr01, 100.0 * maxerr02, 100.0 * maxerr12 ); printf ("Average relative errors: averr(0-1) = %g %%, averr(0-2) = %g %% averr(1-2) = %g %%\n", 100.0 * averr01, 100.0 * averr02, 100.0 * averr12 ); } else printf ("%d %g %g \t %g %g %g \t %g %g %g\n", pickedSite, alpha, delta, averr01, averr02, averr12, maxerr01, maxerr02, maxerr12); if ( (averr01 > tolerance) || (averr02 > tolerance) || (averr12 > tolerance) || (maxerr01 > tolerance) ||(maxerr02 > tolerance) || (maxerr12 > tolerance) ) { XLALPrintError ("Maximal error-tolerance of %g %% was exceeded!\n", 100.0 * tolerance ); if (!ignoreErrors) return 1; } if ( lalDebugLevel ) printf("%d checks left\n", numChecks); /* ---- Clean up things that were created in this loop ---- */ XLALDestroyDetectorStateSeries ( detStates ); detStates = NULL; LALFree ( det ); LALFree ( amParams.das->pSource ); LALFree ( amParams.das ); } /* ----- free memory ----- */ XLALDestroyTimestampVector ( timestamps ); XLALDestroyREAL4Vector ( AMold.a ); XLALDestroyREAL4Vector ( AMold.b ); XLALDestroyREAL4Vector ( AMnew1.a ); XLALDestroyREAL4Vector ( AMnew1.b ); XLALDestroyREAL4Vector ( AMnew2.a ); XLALDestroyREAL4Vector ( AMnew2.b ); LALFree(edat.ephemE); LALFree(edat.ephemS); LALCheckMemoryLeaks(); return 0; /* OK */ } /* main() */
void Conversions::ecef2wgs(double ecef_x, double ecef_y, double ecef_z, double* wgs_lat, double* wgs_lon, double* altitude) { // ECEF 2 Wold Grid System 1984 double Xe = ecef_x; double Ye = ecef_y; double Ze = ecef_z; // WGS 84 earth shape model double f = 1/298.257223563; // Ellipsoid's flatness double e = sqrt(f*(2-f)); // Ellipsoid's Eccentricity double rp = 6357752.3142; // [m] ; semiminor axis: radius polar // b=6356752:31424518 double re = 6378137; // [m] ; semimajor axis: radius equator // a=6378137 // Earth position from ECEF to NorthEastDown (Local) double lon = atan2(Ye,Xe); double p = sqrt( SQ(Xe)+SQ(Ye) ); double h[2] = {0, 0}; double lamdum[2] = {0, 0}; double N[2] = {re, re}; double alt=0, E, F, G, c, s, P, Q, r0, V, z0, e_a, lat; for (int i=0;i<100;i++) { lamdum[1] = atan( (Ze + SQ(e) * N[0] * sin( lamdum[0] ) ) / p); N[1] = re / sqrt(1 - SQ(e) * SQ( sin(lamdum[0]) ) ); h[1] = p / cos( lamdum[0] ) - N[0]; if ( (fabs(h[1])-h[0]) == 0 ) { alt=h[1]; break; } h[0]=h[1]; N[0]=N[1]; lamdum[0] = lamdum[1]; } E = sqrt(re*re-rp*rp); F = 54 * SQ(rp) * SQ(Ze); G = SQ(p) + (1 - SQ(e)) * SQ(Ze) - SQ(e) * SQ(E); c = ( SQ(SQ(e)) * F * SQ(p) ) / CUBIC(G); s = pow(( 1 + c + sqrt( SQ(c) + 2 * c) ), (1/3)); P = F / (3* SQ(s+(1/s)+1) * SQ(G)); Q = sqrt(1+2* SQ(SQ(e)) * P); r0= -(P * SQ(e) * p ) / (1+Q) + sqrt(0.5 * SQ(re) * (1+(1/Q))-(P*(1-SQ(e))*SQ(Ze))/(Q*(1+Q))-0.5 * P * SQ(p)); // U = sqrt( SQ(p- SQ(e) * r0) + SQ(Ze) ); V = sqrt( SQ(p- SQ(e) * r0) + (1-SQ(e)) * SQ(Ze) ); z0= ( SQ(rp) * Ze) / (re * V); e_a = re * e / rp; lat = atan( ( Ze + SQ(e_a) * z0) / p ); *wgs_lat = lat * 180.0 / 3.1415926535897932384626433832795; *wgs_lon = lon * 180.0 / 3.1415926535897932384626433832795; *altitude = alt; }
cdb_index_entry *find_best_candidate(cdb_edit_join_method *join_method) { int ipoints; double temp_distance, distance = 100; double candidate_start_lat, candidate_start_lon; double candidate_end_lat, candidate_end_lon; double candidate_x_start, candidate_y_start; double candidate_x_end, candidate_y_end; double temp_x, temp_y; cdb_index_entry *best_candidate = NULL; cdb_index_entry *candidate_segment = NULL; if (very_very_verbose) fprintf(stderr,">>> Searching candidates.\n"); candidate_segment = source->segment; best_candidate = NULL; for(next_segment_cdb(source); candidate_segment < last_segment_cdb(source); next_segment_cdb(source)) { /* begin find best candidate loop */ candidate_segment = source->segment; /* * Don't try to join a segment which has already been joined */ if((byte4)NULL == candidate_segment->addr) { if (very_very_verbose) fprintf(stderr,">>>Segment %d has already been joined.\n", source->segment->ID); continue; } /* * Don't create a segment which crosses 180 */ if(180 < fabs(current_segment->ilon_max - candidate_segment->ilon_min) * CDB_LON_SCALE) continue; /* * don't create a segment which is too long */ load_current_seg_data_cdb(source); if(MAX_SEGMENT_POINTS < dest->npoints + source->npoints) continue; /* * Set up for fine checks */ if (very_very_verbose) fprintf(stderr,"Checking candidate %d. \n", source->segment->ID); candidate_start_lat = (double)source->segment->ilat0 * CDB_LAT_SCALE; candidate_start_lon = (double)source->segment->ilon0 * CDB_LON_SCALE; if(!within_mapx(map, candidate_start_lat, candidate_start_lon)) continue; candidate_end_lat = candidate_start_lat; candidate_end_lon = candidate_start_lon; for(ipoints = 0; ipoints < source->npoints; ipoints++, source->data_ptr++) { candidate_end_lat += (double) source->data_ptr->dlat * CDB_LAT_SCALE; candidate_end_lon += (double) source->data_ptr->dlon * CDB_LON_SCALE; } forward_mapx(map, candidate_start_lat, candidate_start_lon, &candidate_x_start, &candidate_y_start); forward_mapx(map, candidate_end_lat, candidate_end_lon, &candidate_x_end, &candidate_y_end); /* * See if this candidate is a good one. * If so do fine checks to see if it is the best candidate so far. */ if(fabs(current_x_start - candidate_x_start) <= 2 && fabs(current_y_start - candidate_y_start) <= 2 ) { temp_x = current_x_start - candidate_x_start; temp_y = current_y_start - candidate_y_start; temp_distance = sqrt(SQ(temp_x) + SQ(temp_y)); if(temp_distance < distance) { *join_method = JOIN_START_TO_START; distance = temp_distance; best_candidate = candidate_segment; } } if(fabs(current_x_start - candidate_x_end) <= 2 && fabs(current_y_start - candidate_y_end) <= 2 ) { temp_x = current_x_start - candidate_x_end; temp_y = current_y_start - candidate_y_end; temp_distance = sqrt(SQ(temp_x) + SQ(temp_y)); if(temp_distance < distance) { *join_method = JOIN_START_TO_END; distance = temp_distance; best_candidate = candidate_segment; } } if(fabs(current_x_end - candidate_x_start) <= 2 && fabs(current_y_end - candidate_y_start) <= 2 ) { temp_x = current_x_end - candidate_x_start; temp_y = current_y_end - candidate_y_start; temp_distance = sqrt(SQ(temp_x) + SQ(temp_y)); if(temp_distance < distance) { *join_method = JOIN_END_TO_START; distance = temp_distance; best_candidate = candidate_segment; } } if(fabs(current_x_end - candidate_x_end) <= 2 && fabs(current_y_end - candidate_y_end) <= 2 ) { temp_x = current_x_end - candidate_x_end; temp_y = current_y_end - candidate_y_end; temp_distance = sqrt(SQ(temp_x) + SQ(temp_y)); if(temp_distance < distance) { *join_method = JOIN_END_TO_END; distance = temp_distance; best_candidate = candidate_segment; } } } /** end find best candidate loop **/ return(best_candidate); }
//--------------------------------------------------------- void NDG2D::BuildPeriodicMaps2D(double xperiod, double yperiod) //--------------------------------------------------------- { // function [] = BuildPeriodicMaps2D(xperiod, yperiod); // Purpose: Connectivity and boundary tables for with all // maps returned in Globals2D assuming periodicity // Find node to node connectivity vmapM.resize(Nfp*Nfaces*K); vmapP.resize(Nfp*Nfaces*K); DVec FxL,FyL,FxR,FyR; DMat x1,x2,y1,y2,D,xF1,yF1,xF2,yF2; IMat idLR; IVec idL,idR,vidL,vidR,fidL,fidR; int k1=0,f1=0, k2=0,f2=0; DVec onesNfp=ones(Nfp); double dx=0.0, dy=0.0, cx1=0.0,cx2=0.0,cy1=0.0,cy2=0.0; double dNfp=(double)Nfp; for (k1=1; k1<=K; ++k1) { for (f1=1; f1<=Nfaces; ++f1) { k2 = EToE(k1,f1); f2 = EToF(k1,f1); vidL = Fmask(All,f1); vidL += (k1-1)*Np; vidR = Fmask(All,f2); vidR += (k2-1)*Np; fidL = Range(1,Nfp) + (f1-1)*Nfp + (k1-1)*Nfp*Nfaces; fidR = Range(1,Nfp) + (f2-1)*Nfp + (k2-1)*Nfp*Nfaces; vmapM(fidL) = vidL; vmapP(fidL) = vidL; FxL=Fx(fidL); FyL=Fy(fidL); FxR=Fx(fidR); FyR=Fy(fidR); x1 = outer(FxL, onesNfp); y1 = outer(FyL, onesNfp); x2 = outer(FxR, onesNfp); y2 = outer(FyR, onesNfp); // Compute distance matrix D = sqr(x1-trans(x2)) + sqr(y1-trans(y2)); idLR = find2D(abs(D), '<', NODETOL); idL=idLR(All,1); idR=idLR(All,2); vmapP(fidL(idL)) = vidR(idR); } } for (k1=1; k1<=K; ++k1) { for (f1=1; f1<=Nfaces; ++f1) { //################################################### xF1=x(Fmask(All,f1), k1); cx1=xF1.sum()/dNfp; yF1=y(Fmask(All,f1), k1); cy1=yF1.sum()/dNfp; //################################################### k2 = EToE(k1,f1); f2 = EToF(k1,f1); if (k2==k1) { for (k2=1; k2<=K; ++k2) { if (k1!=k2) { for (f2=1; f2<=Nfaces; ++f2) { if (EToE(k2,f2)==k2) { //######################################### xF2=x(Fmask(All,f2), k2); cx2=xF2.sum()/dNfp; yF2=y(Fmask(All,f2), k2); cy2=yF2.sum()/dNfp; //######################################### dx = sqrt( SQ(abs(cx1-cx2)-xperiod) + SQ(cy1-cy2)); dy = sqrt( SQ(cx1-cx2) + SQ(abs(cy1-cy2)-yperiod)); if (dx<NODETOL || dy<NODETOL) { EToE(k1,f1) = k2; EToE(k2,f2) = k1; EToF(k1,f1) = f2; EToF(k2,f2) = f1; vidL = Fmask(All,f1); vidL += (k1-1)*Np; vidR = Fmask(All,f2); vidR += (k2-1)*Np; fidL = Range(1,Nfp) + (f1-1)*Nfp + (k1-1)*Nfp*Nfaces; fidR = Range(1,Nfp) + (f2-1)*Nfp + (k2-1)*Nfp*Nfaces; FxL=Fx(fidL); FyL=Fy(fidL); FxR=Fx(fidR); FyR=Fy(fidR); x1 = outer(FxL, onesNfp); y1 = outer(FyL, onesNfp); x2 = outer(FxR, onesNfp); y2 = outer(FyR, onesNfp); // Compute distance matrix if (dx<NODETOL) { D = sqr(abs(x1-trans(x2))-xperiod) + sqr(y1-trans(y2)); } else { D = sqr(x1-trans(x2)) + sqr(abs(y1-trans(y2))-yperiod); } idLR = find2D(abs(D), '<', NODETOL); idL=idLR(All,1); idR=idLR(All,2); //assert(idL.size() == Nfp); if (idL.size() != Nfp) { umERROR("NDG2D::BuildPeriodicMaps2D", "Nfp != idL.size() = %d", idL.size()); } vmapP(fidL(idL)) = vidR(idR); vmapP(fidR(idR)) = vidL(idL); } } } } } } } } // Create default list of boundary nodes mapB = find(vmapP, '=', vmapM); vmapB = vmapM(mapB); }
/* approximation of the first derivative of the square of v in y */ inline double dv2dy(double **m, int i, int j, double dy, double alpha){ return ( SQ(m[i][j]+m[i][j+1]) - SQ(m[i][j-1]+m[i][j]) + alpha * ( fabs(m[i][j]+m[i][j+1]) * (m[i][j]-m[i][j+1]) - fabs(m[i][j-1]+m[i][j]) * (m[i][j-1]-m[i][j])) )/dy/4.0; }
void rawAtom(Atom *atom, char *atomFileName) { const char routineName[] = "rawAtom"; register int kr, kf, la, n; char inputLine[MAX_LINE_SIZE], shapeStr[20], vdWstr[20], nuDepStr[20], errorStr[80], symmStr[20], optionStr[20], *match; bool_t exit_on_EOF; int i, j, Nlevel, Nrad, Nline, Ncont, Nfixed, Nread, Nrequired, checkPoint; double f, C, lambda0, lambdamin, dlamb; FILE *atomFile; AtomicLine *line; AtomicContinuum *continuum; FixedTransition *fixed; /* --- Open the data file for current model atom -- -------------- */ if ((atomFile = fopen(atomFileName, "r")) == NULL) { sprintf(messageStr, "Unable to open input file %s", atomFileName); Error(ERROR_LEVEL_2, routineName, messageStr); } /* --- Read atom ID and convert to uppercase -- -------------- */ getLine(atomFile, COMMENT_CHAR, inputLine, exit_on_EOF=TRUE); Nread = sscanf(inputLine, "%2s", atom->ID); checkNread(Nread, Nrequired=1, routineName, checkPoint=1); for (n = 0; n < (int) strlen(atom->ID); n++) atom->ID[n] = toupper(atom->ID[n]); if (strlen(atom->ID) == 1) strcat(atom->ID, " "); /* --- Get the atomic weight -- -------------- */ for (n = 0; n < sizeof(atomweight)/sizeof(struct AtomWeight); n++) { if ((match = strstr(atom->ID, atomweight[n].ID))) { atom->weight = atomweight[n].weight; break; } } if (!match) { sprintf(messageStr, "Found no matching element for %s", atom->ID); Error(ERROR_LEVEL_2, routineName, messageStr); } /* --- Get Number of levels, lines fixed transitions, and continua */ getLine(atomFile, COMMENT_CHAR, inputLine, exit_on_EOF=TRUE); Nread = sscanf(inputLine, "%d %d %d %d", &atom->Nlevel, &atom->Nline, &atom->Ncont, &atom->Nfixed); checkNread(Nread, Nrequired=4, routineName, checkPoint=2); Nlevel = atom->Nlevel; Nline = atom->Nline; Ncont = atom->Ncont; Nfixed = atom->Nfixed; Nrad = Nline + Ncont; atom->E = (double *) malloc(Nlevel * sizeof(double)); atom->g = (double *) malloc(Nlevel * sizeof(double)); atom->stage = (int *) malloc(Nlevel * sizeof(int)); atom->label = (char **) malloc(Nlevel * sizeof(char *)); /* --- Read in the level energies, statistical weights, labels, and ionization stage -- -------------- */ for (i = 0; i < Nlevel; i++) { atom->label[i] = (char *) calloc((ATOM_LABEL_WIDTH+1), sizeof(char)); getLine(atomFile, COMMENT_CHAR, inputLine , exit_on_EOF=TRUE); Nread = sscanf(inputLine, "%lf %lf '%20c' %d", &atom->E[i], &atom->g[i], atom->label[i], &atom->stage[i]); checkNread(Nread, Nrequired=4, routineName, checkPoint=3); atom->E[i] *= (HPLANCK * CLIGHT) / CM_TO_M; } if (atom->stage[Nlevel-1] != (atom->stage[Nlevel-2] + 1)) { sprintf(messageStr, "Found no overlying continuum for atom %s", atom->ID); Error(ERROR_LEVEL_2, routineName, messageStr); } C = 2 * PI * (Q_ELECTRON/EPSILON_0) * (Q_ELECTRON/M_ELECTRON) / CLIGHT; /* --- Go through the bound-bound transitions -- -------------- */ atom->Nprd = 0; atom->line = (AtomicLine *) malloc(Nline * sizeof(AtomicLine)); for (kr = 0; kr < Nline; kr++) { line = atom->line + kr; line->atom = atom; line->isotope_frac = 1.0; getLine(atomFile, COMMENT_CHAR, inputLine, exit_on_EOF=TRUE); Nread = sscanf(inputLine, "%d %d %lf %s %d %s %lf %lf %s %lf %lf %lf %lf %lf %lf %lf", &j, &i, &f, shapeStr, &line->Nlambda, symmStr, &line->qcore, &line->qwing, vdWstr, &line->cvdWaals[0], &line->cvdWaals[1], &line->cvdWaals[2], &line->cvdWaals[3], &line->Grad, &line->cStark, &line->g_Lande_eff); checkNread(Nread, Nrequired=15, routineName, checkPoint=4); if (Nread == 15) line->g_Lande_eff = 0.0; line->j = MAX(i, j); line->i = MIN(i, j); i = line->i; j = line->j; if (!strstr(shapeStr, "PRD") && !strstr(shapeStr, "VOIGT") && !strstr(shapeStr, "GAUSS")) { sprintf(messageStr, "Invalid value for line-shape string: %s", shapeStr); Error(ERROR_LEVEL_2, routineName, messageStr); } if (strstr(shapeStr, "PRD")) { atom->Nprd++; line->PRD = TRUE; } if (strstr(shapeStr, "GAUSS")) line->Voigt = FALSE; lambda0 = (HPLANCK * CLIGHT) / (atom->E[j] - atom->E[i]); line->Aji = C / SQ(lambda0) * (atom->g[i] / atom->g[j]) * f; line->Bji = CUBE(lambda0) / (2.0 * HPLANCK * CLIGHT) * line->Aji; line->Bij = (atom->g[j] / atom->g[i]) * line->Bji; line->lambda0 = lambda0 / NM_TO_M; if (strstr(vdWstr, "PARAMTR")) line->vdWaals = RIDDER_RENSBERGEN; else { line->vdWaals = UNSOLD; line->cvdWaals[3] = line->cvdWaals[1] = 0.0; } line->symmetric = (strstr(symmStr, "ASYMM")) ? FALSE : TRUE; } /* --- Go through the bound-free transitions -- -------------- */ atom->continuum = (AtomicContinuum *) malloc(Ncont * sizeof(AtomicContinuum)); for (kr = 0; kr < Ncont; kr++) { continuum = atom->continuum + kr; continuum->atom = atom; continuum->isotope_frac = 1.0; getLine(atomFile, COMMENT_CHAR, inputLine, exit_on_EOF=TRUE); Nread = sscanf(inputLine, "%d %d %lf %d %s %lf", &j, &i, &continuum->alpha0, &continuum->Nlambda, nuDepStr, &lambdamin); checkNread(Nread, Nrequired=6, routineName, checkPoint=5); continuum->j = MAX(i, j); continuum->i = MIN(i, j); j = continuum->j; i = continuum->i; lambda0 = (HPLANCK * CLIGHT)/(atom->E[j] - atom->E[i]); continuum->lambda0 = lambda0 / NM_TO_M; continuum->lambda = (double *) malloc(continuum->Nlambda * sizeof(double)); continuum->alpha = (double *) malloc(continuum->Nlambda * sizeof(double)); if (strstr(nuDepStr, "EXPLICIT")) { continuum->hydrogenic = FALSE; for (la = continuum->Nlambda-1; la >= 0; la--) { getLine(atomFile, COMMENT_CHAR, inputLine, exit_on_EOF=TRUE); Nread = sscanf(inputLine, "%lf %lf", &continuum->lambda[la], &continuum->alpha[la]); checkNread(Nread, Nrequired=2, routineName, checkPoint=6); } for (la = 1; la < continuum->Nlambda; la++) { if (continuum->lambda[la] < continuum->lambda[la-1]) { sprintf(messageStr, "Continuum %d does not have monotonous wavelengths", kr - Nline); Error(ERROR_LEVEL_2, routineName, messageStr); } } } else { continuum->hydrogenic = TRUE; if (lambdamin >= continuum->lambda0) { sprintf(messageStr, "Minimum wavelength for continuum %d too long", kr - Nline); Error(ERROR_LEVEL_2, routineName, messageStr); } dlamb = (continuum->lambda0 - lambdamin) / (continuum->Nlambda - 1); continuum->lambda[0] = lambdamin; for (la = 1; la < continuum->Nlambda; la++) continuum->lambda[la] = continuum->lambda[la-1] + dlamb; } } /* --- Go through fixed transitions -- -------------- */ if (atom->Nfixed > 0) { atom->ft = (FixedTransition *) malloc(Nfixed * sizeof(FixedTransition)); for (kf = 0; kf < Nfixed; kf++) { fixed = atom->ft + kf; getLine(atomFile, COMMENT_CHAR, inputLine, exit_on_EOF=TRUE); Nread = sscanf(inputLine, "%d %d %lf %lf %s", &j, &i, &fixed->strength, &fixed->Trad, optionStr); checkNread(Nread, Nrequired=5, routineName, checkPoint=7); fixed->j = MAX(i, j); fixed->i = MIN(i, j); j = fixed->j; i = fixed->i; for (kr = 0; kr < Nline; kr++) { line = atom->line + kr; if (line->i == i && line->j == j) { sprintf(messageStr, "Fixed transition j = %d, i = %d duplicates active line", j, i); Error(ERROR_LEVEL_2, routineName, messageStr); } } for (kr = 0; kr < Ncont; kr++) { continuum = atom->continuum + kr; if (continuum->i == i && continuum->j == j) { sprintf(messageStr, "Fixed transition j = %d, i = %d" " duplicates active continuum", j, i); Error(ERROR_LEVEL_2, routineName, messageStr); } } lambda0 = (HPLANCK * CLIGHT) / (atom->E[j] - atom->E[i]); fixed->lambda0 = lambda0 / NM_TO_M; if (atom->stage[j] == atom->stage[i]) fixed->type = FIXED_LINE; else fixed->type = FIXED_CONTINUUM; if (strstr(optionStr, "TRAD_ATMOSPHERIC")) fixed->option = TRAD_ATMOSPHERIC; else if (strstr(optionStr, "TRAD_PHOTOSPHERIC")) fixed->option = TRAD_PHOTOSPHERIC; else if (strstr(optionStr, "TRAD_CHROMOSPHERIC")) fixed->option = TRAD_CHROMOSPHERIC; else { sprintf(messageStr, "Inavlid value for TRAD option: %s", optionStr); Error(ERROR_LEVEL_2, routineName, messageStr); } } } fclose(atomFile); }
int main(int argc, char *argv[]) { ConfigVariables config = empty_ConfigVariables; UserVariables_t uvar = empty_UserVariables; /* register user-variables */ XLAL_CHECK ( XLALInitUserVars ( &uvar ) == XLAL_SUCCESS, XLAL_EFUNC ); /* read cmdline & cfgfile */ XLAL_CHECK ( XLALUserVarReadAllInput ( argc,argv ) == XLAL_SUCCESS, XLAL_EFUNC ); if (uvar.help) { /* help requested: we're done */ exit(0); } if ( uvar.version ) { XLALOutputVersionString ( stdout, lalDebugLevel ); exit(0); } /* basic setup and initializations */ XLAL_CHECK ( XLALInitCode( &config, &uvar, argv[0] ) == XLAL_SUCCESS, XLAL_EFUNC ); /* prepare output files */ FILE *fpOutab = NULL; if ( uvar.outab ) { XLAL_CHECK ( (fpOutab = fopen (uvar.outab, "wb")) != NULL, XLAL_EIO, "Error opening file '%s' for writing...", uvar.outab ); /* write header info in comments */ XLAL_CHECK ( XLAL_SUCCESS == XLALOutputVersionString ( fpOutab, 0 ), XLAL_EFUNC ); /* write the command-line */ for (int a = 0; a < argc; a++) { fprintf(fpOutab,"%%%% argv[%d]: '%s'\n", a, argv[a]); } /* write column headings */ fprintf(fpOutab, "%%%% columns:\n%%%% Alpha Delta tGPS "); if ( config.numDetectors == 1 ) { fprintf(fpOutab, " a(t) b(t)"); } else { for ( UINT4 X=0; X < config.numDetectors; X++ ) { fprintf(fpOutab, " a[%d](t) b[%d](t)", X, X); } } fprintf(fpOutab, "\n"); } FILE *fpOutABCD = NULL; if ( uvar.outABCD ) { XLAL_CHECK ( (fpOutABCD = fopen (uvar.outABCD, "wb")) != NULL, XLAL_EIO, "Error opening file '%s' for writing...", uvar.outABCD ); /* write header info in comments */ XLAL_CHECK ( XLAL_SUCCESS == XLALOutputVersionString ( fpOutABCD, 0 ), XLAL_EFUNC ); /* write the command-line */ for (int a = 0; a < argc; a++) { fprintf(fpOutABCD,"%%%% argv[%d]: '%s'\n", a, argv[a]); } /* write column headings */ fprintf(fpOutABCD, "%%%% columns:\n%%%% Alpha Delta"); fprintf(fpOutABCD, " A B C D"); if ( config.numDetectors > 1 ) { fprintf(fpOutABCD, " "); for ( UINT4 X=0; X < config.numDetectors; X++ ) { fprintf(fpOutABCD, " A[%d] B[%d] C[%d] D[%d]", X, X, X, X); } } fprintf(fpOutABCD, "\n"); } /* loop over sky positions (outer loop, could allow for buffering if necessary) */ for (UINT4 n = 0; n < config.numSkyPoints; n++) { SkyPosition skypos; skypos.system = COORDINATESYSTEM_EQUATORIAL; skypos.longitude = config.Alpha->data[n]; skypos.latitude = config.Delta->data[n]; /* do the actual computation of the antenna pattern functions */ MultiAMCoeffs *multiAM; XLAL_CHECK ( ( multiAM = XLALComputeMultiAMCoeffs ( config.multiDetStates, config.multiNoiseWeights, skypos ) ) != NULL, XLAL_EFUNC, "XLALComputeAMCoeffs() failed." ); /* for multi-IFO run with weights, do it again, without weights, to get single-IFO quantities consistent with single-IFO runs * FIXME: remove this temporary hack when MultiAmCoeffs have been changed to include non-weighted single-IFO quantities */ MultiAMCoeffs *multiAMforSingle = NULL; MultiAMCoeffs *multiAMunweighted = NULL; if ( ( config.numDetectors > 1 ) && ( config.multiNoiseWeights != NULL ) ) { XLAL_CHECK ( ( multiAMunweighted = XLALComputeMultiAMCoeffs ( config.multiDetStates, NULL, skypos ) ) != NULL, XLAL_EFUNC, "XLALComputeAMCoeffs() failed." ); multiAMforSingle = multiAMunweighted; } else { multiAMforSingle = multiAM; } /* write out the data for this sky point */ if ( uvar.outab ) { // output a(t), b(t) at each timestamp for (UINT4 t = 0; t < config.numTimeStamps; t++) { // FIXME: does not work for different multi-IFO numTimeStampsX fprintf (fpOutab, "%.7f %.7f %d", config.Alpha->data[n], config.Delta->data[n], config.multiTimestamps->data[0]->data[t].gpsSeconds ); for ( UINT4 X=0; X < config.numDetectors; X++ ) { fprintf(fpOutab, " %12.8f %12.8f", multiAMforSingle->data[X]->a->data[t], multiAMforSingle->data[X]->b->data[t]); } // for ( UINT4 X=0; X < config.numDetectors; X++ ) fprintf(fpOutab, "\n"); } // for (UINT4 t = 0; t < config.numTimeStamps; t++) } // if ( uvar.outab ) if ( uvar.outABCD ) { // output ABCD averaged over all timestamps // FIXME: stop doing average manually when AMCoeffs is changed to contain averaged values REAL8 A = multiAM->Mmunu.Ad/config.numTimeStamps; REAL8 B = multiAM->Mmunu.Bd/config.numTimeStamps; REAL8 C = multiAM->Mmunu.Cd/config.numTimeStamps; REAL8 D = A*B-SQ(C); fprintf (fpOutABCD, "%.7f %.7f %12.8f %12.8f %12.8f %12.8f", config.Alpha->data[n], config.Delta->data[n], A, B, C, D ); if ( config.numDetectors > 1 ) { for ( UINT4 X=0; X < config.numDetectors; X++ ) { REAL4 AX = multiAMforSingle->data[X]->A/config.numTimeStampsX->data[X]; REAL4 BX = multiAMforSingle->data[X]->B/config.numTimeStampsX->data[X]; REAL4 CX = multiAMforSingle->data[X]->C/config.numTimeStampsX->data[X]; REAL4 DX = AX*BX-SQ(CX); fprintf(fpOutABCD, " %12.8f %12.8f %12.8f %12.8f", AX, BX, CX, DX); } } fprintf(fpOutABCD, "\n"); } // if ( uvar.outABCD ) XLALDestroyMultiAMCoeffs ( multiAM ); if ( multiAMunweighted ) { XLALDestroyMultiAMCoeffs ( multiAMunweighted ); } } // for (UINT4 n = 0; n < config.numSkyPoints; n++) /* ----- close output files ----- */ if ( fpOutab ) { fprintf (fpOutab, "\n"); fclose ( fpOutab ); } if ( fpOutABCD ) { fprintf (fpOutABCD, "\n"); fclose ( fpOutABCD ); } /* ----- done: free all memory */ XLAL_CHECK ( XLALDestroyConfig( &config ) == XLAL_SUCCESS, XLAL_EFUNC ); LALCheckMemoryLeaks(); return 0; } /* main */
/** * basic initializations: deal with user input and return standardized 'ConfigVariables' */ int XLALInitCode ( ConfigVariables *cfg, const UserVariables_t *uvar, const char *app_name) { XLAL_CHECK ( cfg && uvar && app_name, XLAL_EINVAL, "Illegal NULL pointer input." ); /* init ephemeris data */ XLAL_CHECK ( ( cfg->edat = XLALInitBarycenter( uvar->ephemEarth, uvar->ephemSun ) ) != NULL, XLAL_EFUNC, "XLALInitBarycenter failed: could not load Earth ephemeris '%s' and Sun ephemeris '%s.", uvar->ephemEarth, uvar->ephemSun); cfg->numDetectors = uvar->IFOs->length; cfg->numTimeStamps = 0; XLAL_CHECK ( (cfg->numTimeStampsX = XLALCreateUINT4Vector ( cfg->numDetectors )) != NULL, XLAL_EFUNC, "XLALCreateREAL8Vector(%d) failed.", cfg->numDetectors ); BOOLEAN haveTimeGPS = XLALUserVarWasSet( &uvar->timeGPS ); BOOLEAN haveTimeStampsFile = XLALUserVarWasSet( &uvar->timeStampsFile ); BOOLEAN haveTimeStampsFiles = XLALUserVarWasSet( &uvar->timeStampsFiles ); XLAL_CHECK ( !(haveTimeStampsFiles && haveTimeStampsFile), XLAL_EINVAL, "Can't handle both timeStampsFiles and (deprecated) haveTimeStampsFiles input options." ); XLAL_CHECK ( !(haveTimeGPS && haveTimeStampsFile), XLAL_EINVAL, "Can't handle both (deprecated) timeStampsFile and timeGPS input options." ); XLAL_CHECK ( !(haveTimeGPS && haveTimeStampsFiles), XLAL_EINVAL, "Can't handle both timeStampsFiles and timeGPS input options." ); XLAL_CHECK ( haveTimeGPS || haveTimeStampsFiles || haveTimeStampsFile, XLAL_EINVAL, "Need either timeStampsFiles or timeGPS input option." ); if ( haveTimeStampsFiles ) { XLAL_CHECK ( (uvar->timeStampsFiles->length == 1 ) || ( uvar->timeStampsFiles->length == cfg->numDetectors ), XLAL_EINVAL, "Length of timeStampsFiles list is neither 1 (one file for all detectors) nor does it match the number of detectors. (%d != %d)", uvar->timeStampsFiles->length, cfg->numDetectors ); XLAL_CHECK ( (uvar->timeStampsFiles->length == 1 ) || !uvar->outab, XLAL_EINVAL, "At the moment, can't produce a(t), b(t) output (--outab) when given per-IFO --timeStampsFiles."); } if ( haveTimeStampsFiles && ( uvar->timeStampsFiles->length == cfg->numDetectors ) ) { XLAL_CHECK ( ( cfg->multiTimestamps = XLALReadMultiTimestampsFiles ( uvar->timeStampsFiles ) ) != NULL, XLAL_EFUNC ); XLAL_CHECK ( (cfg->multiTimestamps->length > 0) && (cfg->multiTimestamps->data != NULL), XLAL_EINVAL, "Got empty timestamps-list from '%s'.", uvar->timeStampsFiles ); } else { /* prepare multiTimestamps structure */ UINT4 nTS = 0; XLAL_CHECK ( ( cfg->multiTimestamps = XLALCalloc ( 1, sizeof(*cfg->multiTimestamps))) != NULL, XLAL_ENOMEM, "Allocating multiTimestamps failed." ); XLAL_CHECK ( ( cfg->multiTimestamps->data = XLALCalloc ( cfg->numDetectors, sizeof(cfg->multiTimestamps->data) )) != NULL, XLAL_ENOMEM, "Allocating multiTimestamps->data failed." ); cfg->multiTimestamps->length = cfg->numDetectors; if ( haveTimeGPS ) { /* set up timestamps vector from timeGPS, use same for all IFOs */ nTS = uvar->timeGPS->length; XLAL_CHECK ( (cfg->multiTimestamps->data[0] = XLALCreateTimestampVector ( nTS ) ) != NULL, XLAL_EFUNC, "XLALCreateTimestampVector( %d ) failed.", nTS ); /* convert input REAL8 times into LIGOTimeGPS for first detector */ for (UINT4 t = 0; t < nTS; t++) { REAL8 temp_real8_timestamp = 0; XLAL_CHECK ( 1 == sscanf ( uvar->timeGPS->data[t], "%" LAL_REAL8_FORMAT, &temp_real8_timestamp ), XLAL_EINVAL, "Illegal REAL8 commandline argument to --timeGPS[%d]: '%s'", t, uvar->timeGPS->data[t] ); XLAL_CHECK ( XLALGPSSetREAL8( &cfg->multiTimestamps->data[0]->data[t], temp_real8_timestamp ) != NULL, XLAL_EFUNC, "Failed to convert input GPS %g into LIGOTimeGPS", temp_real8_timestamp ); } // for (UINT4 t = 0; t < nTS; t++) } // if ( haveTimeGPS ) else { // haveTimeStampsFiles || haveTimeStampsFile CHAR *singleTimeStampsFile = NULL; if ( haveTimeStampsFiles ) { singleTimeStampsFile = uvar->timeStampsFiles->data[0]; } else if ( haveTimeStampsFile ) { singleTimeStampsFile = uvar->timeStampsFile; } XLAL_CHECK ( ( cfg->multiTimestamps->data[0] = XLALReadTimestampsFile ( singleTimeStampsFile ) ) != NULL, XLAL_EFUNC ); nTS = cfg->multiTimestamps->data[0]->length; } // else: haveTimeStampsFiles || haveTimeStampsFile /* copy timestamps from first detector to all others */ if ( cfg->numDetectors > 1 ) { for ( UINT4 X=1; X < cfg->numDetectors; X++ ) { XLAL_CHECK ( (cfg->multiTimestamps->data[X] = XLALCreateTimestampVector ( nTS ) ) != NULL, XLAL_EFUNC, "XLALCreateTimestampVector( %d ) failed.", nTS ); for (UINT4 t = 0; t < nTS; t++) { cfg->multiTimestamps->data[X]->data[t].gpsSeconds = cfg->multiTimestamps->data[0]->data[t].gpsSeconds; cfg->multiTimestamps->data[X]->data[t].gpsNanoSeconds = cfg->multiTimestamps->data[0]->data[t].gpsNanoSeconds; } // for (UINT4 t = 0; t < nTS; t++) } // for ( UINT4 X=1; X < cfg->numDetectors X++ ) } // if ( cfg->numDetectors > 1 ) } // if !( haveTimeStampsFiles && ( uvar->timeStampsFiles->length == cfg->numDetectors ) ) for ( UINT4 X=0; X < cfg->numDetectors; X++ ) { cfg->numTimeStampsX->data[X] = cfg->multiTimestamps->data[X]->length; cfg->numTimeStamps += cfg->numTimeStampsX->data[X]; } /* convert detector names into site-info */ MultiLALDetector multiDet; XLAL_CHECK ( XLALParseMultiLALDetector ( &multiDet, uvar->IFOs ) == XLAL_SUCCESS, XLAL_EFUNC ); /* get detector states */ XLAL_CHECK ( (cfg->multiDetStates = XLALGetMultiDetectorStates ( cfg->multiTimestamps, &multiDet, cfg->edat, 0.5 * uvar->Tsft )) != NULL, XLAL_EFUNC, "XLALGetDetectorStates() failed." ); BOOLEAN haveAlphaDelta = ( XLALUserVarWasSet(&uvar->Alpha) && XLALUserVarWasSet(&uvar->Delta) ); BOOLEAN haveSkyGrid = XLALUserVarWasSet( &uvar->skyGridFile ); XLAL_CHECK ( !(haveAlphaDelta && haveSkyGrid), XLAL_EINVAL, "Can't handle both Alpha/Delta and skyGridFile input options." ); XLAL_CHECK ( haveAlphaDelta || haveSkyGrid, XLAL_EINVAL, "Need either Alpha/Delta or skyGridFile input option." ); if (haveAlphaDelta) { /* parse this into one-element Alpha, Delta vectors */ XLAL_CHECK ( (cfg->Alpha = XLALCreateREAL8Vector ( 1 )) != NULL, XLAL_EFUNC, "XLALCreateREAL8Vector(1) failed." ); cfg->Alpha->data[0] = uvar->Alpha; XLAL_CHECK ( (cfg->Delta = XLALCreateREAL8Vector ( 1 )) != NULL, XLAL_EFUNC, "XLALCreateREAL8Vector(1) failed." ); cfg->Delta->data[0] = uvar->Delta; cfg->numSkyPoints = 1; } // if (haveAlphaDelta) else if ( haveSkyGrid ) { LALParsedDataFile *data = NULL; XLAL_CHECK ( XLALParseDataFile (&data, uvar->skyGridFile) == XLAL_SUCCESS, XLAL_EFUNC, "Failed to parse data file '%s'.", uvar->skyGridFile ); cfg->numSkyPoints = data->lines->nTokens; XLAL_CHECK ( (cfg->Alpha = XLALCreateREAL8Vector ( cfg->numSkyPoints )) != NULL, XLAL_EFUNC, "XLALCreateREAL8Vector( %d ) failed.", cfg->numSkyPoints ); XLAL_CHECK ( (cfg->Delta = XLALCreateREAL8Vector ( cfg->numSkyPoints )) != NULL, XLAL_EFUNC, "XLALCreateREAL8Vector( %d ) failed.", cfg->numSkyPoints ); for (UINT4 n=0; n < cfg->numSkyPoints; n++) { XLAL_CHECK ( 2 == sscanf( data->lines->tokens[n], "%" LAL_REAL8_FORMAT "%" LAL_REAL8_FORMAT, &cfg->Alpha->data[n], &cfg->Delta->data[n] ), XLAL_EDATA, "Could not parse 2 numbers from line %d in candidate-file '%s':\n'%s'", n, uvar->skyGridFile, data->lines->tokens[n] ); } // for (UINT4 n=0; n < cfg->numSkyPoints; n++) XLALDestroyParsedDataFile ( data ); } // else if ( haveSkyGrid ) if ( uvar->noiseSqrtShX ) { /* translate user-input PSD sqrt(SX) to noise-weights (this actually does not care whether they were normalized or not) */ if ( uvar->noiseSqrtShX->length != cfg->numDetectors ) { fprintf(stderr, "Length of noiseSqrtShX vector does not match number of detectors! (%d != %d)\n", uvar->noiseSqrtShX->length, cfg->numDetectors); XLAL_ERROR ( XLAL_EINVAL ); } REAL8Vector *noiseSqrtShX = NULL; if ( (noiseSqrtShX = XLALCreateREAL8Vector ( cfg->numDetectors )) == NULL ) { fprintf(stderr, "Failed call to XLALCreateREAL8Vector( %d )\n", cfg->numDetectors ); XLAL_ERROR ( XLAL_EFUNC ); } REAL8 psd_normalization = 0; for (UINT4 X = 0; X < cfg->numDetectors; X++) { if ( 1 != sscanf ( uvar->noiseSqrtShX->data[X], "%" LAL_REAL8_FORMAT, &noiseSqrtShX->data[X] ) ) { fprintf(stderr, "Illegal REAL8 commandline argument to --noiseSqrtShX[%d]: '%s'\n", X, uvar->noiseSqrtShX->data[X]); XLAL_ERROR ( XLAL_EINVAL ); } if ( noiseSqrtShX->data[X] <= 0.0 ) { fprintf(stderr, "Non-positive input PSD ratio for detector X=%d: noiseSqrtShX[X]=%f\n", X, noiseSqrtShX->data[X] ); XLAL_ERROR ( XLAL_EINVAL ); } psd_normalization += 1.0/SQ(noiseSqrtShX->data[X]); } /* for X < cfg->numDetectors */ psd_normalization = (REAL8)cfg->numDetectors/psd_normalization; /* S = NSFT / sum S_Xalpha^-1, no per-SFT variation here -> S = Ndet / sum S_X^-1 */ /* create multi noise weights */ if ( (cfg->multiNoiseWeights = XLALCalloc(1, sizeof(*cfg->multiNoiseWeights))) == NULL ) { XLALPrintError ("%s: failed to XLALCalloc ( 1, %d )\n", __func__, sizeof(*cfg->multiNoiseWeights) ); XLAL_ERROR ( XLAL_ENOMEM ); } if ( (cfg->multiNoiseWeights->data = XLALCalloc(cfg->numDetectors, sizeof(*cfg->multiNoiseWeights->data))) == NULL ) { XLALPrintError ("%s: failed to XLALCalloc ( %d, %d )\n", __func__, cfg->numDetectors, sizeof(*cfg->multiNoiseWeights->data) ); XLAL_ERROR ( XLAL_ENOMEM ); } cfg->multiNoiseWeights->length = cfg->numDetectors; for (UINT4 X = 0; X < cfg->numDetectors; X++) { REAL8 noise_weight_X = psd_normalization/SQ(noiseSqrtShX->data[X]); /* w_Xalpha = S_Xalpha^-1/S^-1 = S / S_Xalpha */ /* create k^th weights vector */ if( ( cfg->multiNoiseWeights->data[X] = XLALCreateREAL8Vector ( cfg->numTimeStampsX->data[X] ) ) == NULL ) { /* free weights vectors created previously in loop */ XLALDestroyMultiNoiseWeights ( cfg->multiNoiseWeights ); XLAL_ERROR ( XLAL_EFUNC, "Failed to allocate noiseweights for IFO X = %d\n", X ); } /* if XLALCreateREAL8Vector() failed */ /* loop over rngmeds and calculate weights -- one for each sft */ for ( UINT4 alpha = 0; alpha < cfg->numTimeStampsX->data[X]; alpha++) { cfg->multiNoiseWeights->data[X]->data[alpha] = noise_weight_X; } } /* for X < cfg->numDetectors */ XLALDestroyREAL8Vector ( noiseSqrtShX ); } /* if ( uvar->noiseSqrtShX ) */ else { cfg->multiNoiseWeights = NULL; } return XLAL_SUCCESS; } /* XLALInitCode() */
/** * Set the boundary conditions depending on the chosen model */ void boundaryvalues( int imax, int jmax, double dx, double dy, double **U, double **V, double **K, double **W, double nu, int *b, int **Flag ) { int i, j; int c, bound_now; for( c = 0; c < 4; c++ ){ bound_now = b[c]; /* treating different cases of boundaries * inflow treated separately * */ switch(bound_now){ case 1: no_slip( imax, jmax, U, V, K, W, c,dx,dy,nu); break; case 3: outflow( imax, jmax, U, V, K, W, c); break; default: free_slip( imax, jmax, U, V, K, W, c); break; } } /* Boundary conditions for the obstacle cells */ for( i = 1; i <= imax; i++ ) for( j = 1; j <= jmax; j++ ) if( IS_BOUNDARY(Flag[i][j]) ){ /* Boundary conditions for obstacles with North-Eastern fluid cell */ if( ( Flag[ i ][ j ] & B_NE ) == B_NE ){ U[ i ][ j ] = .0; V[ i ][ j ] = .0; U[ i-1 ][ j ] = -U[ i-1 ][ j+1 ]; V[ i ][ j-1 ] = -V[ i+1 ][ j-1 ]; K[ i ][ j ] = 0.00001; W[ i ][ j ] = 0.5*( 6.0*10.0*nu/(beta_1*SQ(0.5*dy)) + 6.0*10.0*nu/(beta_1*SQ(0.5*dx)) ); } else /* Boundary conditions for obstacles with North-Western fluid cell */ if( ( Flag[ i ][ j ] & B_NW ) == B_NW ){ U[ i-1 ][ j ] = .0; V[ i ][ j ] = .0; U[ i ][ j ] = -U[ i ][ j+1 ]; V[ i ][ j-1 ] = -V[ i-1 ][ j-1 ]; K[ i ][ j ] = 0.00001; W[ i ][ j ] = 0.5*( 6.0*10.0*nu/(beta_1*SQ(0.5*dy)) + 6.0*10.0*nu/(beta_1*SQ(0.5*dx)) ); } else /* Boundary conditions for obstacles with South-Eastern fluid cell */ if( ( Flag[ i ][ j ] & B_SE ) == B_SE ){ U[ i ][ j ] = .0; V[ i ][ j-1 ] = .0; U[ i-1 ][ j ] = -U[ i-1 ][ j-1 ]; V[ i ][ j ] = -V[ i+1 ][ j ]; K[ i ][ j ] = 0.00001; W[ i ][ j ] = 0.5*( 6.0*10.0*nu/(beta_1*SQ(0.5*dy)) + 6.0*10.0*nu/(beta_1*SQ(0.5*dx)) ); } else /* Boundary conditions for obstacles with South-Western fluid cell */ if( ( Flag[ i ][ j ] & B_SW ) == B_SW ){ U[ i-1 ][ j ] = .0; V[ i ][ j-1 ] = .0; U[ i ][ j ] = -U[ i ][ j-1 ]; V[ i ][ j ] = -V[ i-1 ][ j ]; K[ i ][ j ] = 0.00001; W[ i ][ j ] = 0.5*( 6.0*10.0*nu/(beta_1*SQ(0.5*dy)) + 6.0*10.0*nu/(beta_1*SQ(0.5*dx)) ); } else /* Boundary conditions for obstacles with Northern fluid cell */ if( ( Flag[ i ][ j ] & B_N ) == B_N ){ V[ i ][ j ] = .0; U[ i ][ j ] = -U[ i ][ j+1 ]; U[ i-1 ][ j ] = -U[ i-1 ][ j+1 ]; K[ i ][ j ] = 0.00001; W[ i ][ j ] = 6.0*10.0*nu/(beta_1*SQ(0.5*dy)) ; } else /* Boundary conditions for obstacles with Southern fluid cell */ if(( Flag[ i ][ j ] & B_S ) == B_S ){ V[ i ][ j-1 ] = .0; U[ i ][ j ] = -U[ i ][ j-1 ]; U[ i-1 ][ j ] = -U[ i-1 ][ j-1 ]; K[ i ][ j ] = 0.00001; W[ i ][ j ] = 6.0*10.0*nu/(beta_1*SQ(0.5*dy)) ; }else /* Boundary conditions for obstacles with Western fluid cell */ if( ( Flag[ i ][ j ] & B_W ) == B_W ){ U[ i-1 ][ j ] = .0; V[ i ][ j ] = -V[ i-1 ][ j ]; V[ i ][ j-1 ] = -V[ i-1 ][ j-1 ]; K[ i ][ j ] = 0.00001; W[ i ][ j ] = 6.0*10.0*nu/(beta_1*SQ(0.5*dx)) ; } else /* Boundary conditions for obstacles with Eastern fluid cell */ if( ( Flag[ i ][ j ] & B_E ) == B_E ){ U[ i ][ j ] = .0; V[ i ][ j ] = -V[ i+1 ][ j ]; V[ i ][ j-1 ] = -V[ i+1 ][ j-1 ]; K[ i ][ j ] = 0.00001; W[ i ][ j ] = 6.0*10.0*nu/(beta_1*SQ(0.5*dx)) ; } } }
int ReadDatad(int numdat, double *xin, double *yin, double *zin) { double temp[3], minx, maxx, miny, maxy, xtmp, ytmp, ztmp; double qtxy, qtyx, qtzx, qtzy; int i0, i1, n0; bigtri[0][0] = bigtri[0][1] = bigtri[1][1] = bigtri[2][0] = -1; bigtri[1][0] = bigtri[2][1] = 5; if (rootdat EQ NULL) { rootdat = IMakeDatum(); if (error_status) return (error_status); rootsimp = IMakeSimp(); if (error_status) return (error_status); roottemp = IMakeTemp(); if (error_status) return (error_status); rootneig = IMakeNeig(); if (error_status) return (error_status); rootdat->values[0] = rootdat->values[1] = rootdat->values[2] = 0; } else { FreeVecti(jndx); FreeMatrixd(points); FreeMatrixd(joints); } curdat = rootdat; datcnt = 0; minx = xstart - horilap; maxx = xend + horilap; miny = ystart - vertlap; maxy = yend + vertlap; for (n0 = 0 ; n0 < numdat ; n0++) { temp[0] = xin[n0]; temp[1] = yin[n0]; temp[2] = zin[n0]; if (temp[0] > minx AND temp[0] < maxx AND temp[1] > miny AND temp[1] < maxy) { if (curdat->nextdat EQ NULL) { curdat->nextdat = IMakeDatum(); if (error_status) return (error_status); } curdat = curdat->nextdat; datcnt++; for (i1 = 0; i1 < 3; i1++) curdat->values[i1] = temp[i1]; } } if (datcnt > 3) { datcnt3 = datcnt + 3; jndx = IntVect(datcnt3); if (error_status) return (error_status); sumx = sumy = sumz = sumx2 = sumy2 = sumxy = sumxz = sumyz = 0; iscale = 0; /* * Calculate minimums and maximums of the input data accounting for * the scale factors. * * For the initial calculations, we have: * * maxxy[0][0] = maximum x input data value * maxxy[1][0] = minimum x input data value * maxxy[0][1] = maximum y input data value * maxxy[1][1] = minimum y input data value * maxxy[0][2] = maximum z input data value * maxxy[1][2] = minimum z input data value * */ data_limits: maxxy[0][0] = maxxy[0][1] = maxxy[0][2] = -(maxxy[1][0] = maxxy[1][1] = maxxy[1][2] = BIGNUM); curdat = rootdat->nextdat; for (i0 = 0; i0 < datcnt; i0++) { xtmp = curdat->values[0] * magx; if (maxxy[0][0] < xtmp) maxxy[0][0] = xtmp; if (maxxy[1][0] > xtmp) maxxy[1][0] = xtmp; ytmp = curdat->values[1] * magy; if (maxxy[0][1] < ytmp) maxxy[0][1] = ytmp; if (maxxy[1][1] > ytmp) maxxy[1][1] = ytmp; ztmp = curdat->values[2] * magz; if (maxxy[0][2] < ztmp) maxxy[0][2] = ztmp; if (maxxy[1][2] > ztmp) maxxy[1][2] = ztmp; curdat = curdat->nextdat; } /* * Modify the mins and maxs based on the scale factors and overlap regions. * to get the actual minimums and maximums of the data under consideration. */ if (maxxy[0][0] < maxx * magx) maxxy[0][0] = maxx * magx; if (maxxy[1][0] > minx * magx) maxxy[1][0] = minx * magx; if (maxxy[0][1] < maxy * magy) maxxy[0][1] = maxy * magy; if (maxxy[1][1] > miny * magy) maxxy[1][1] = miny * magy; /* * Calculate the extents in x, y, and z. * * maxxy[0][0] = maximum x extent, including overlap regions. * maxxy[0][1] = maximum y extent, including overlap regions. * maxxy[0][2] = maximum z extent. */ for (i0 = 0 ; i0 < 3 ; i0++) { maxxy[0][i0] -= maxxy[1][i0]; } maxhoriz = maxxy[0][0]; if (maxhoriz < maxxy[0][1]) maxhoriz = maxxy[0][1]; wbit = maxhoriz * EPSILON; /* * Calculate the ratio of the x extent by the y extent (qtxy) and * the y extent by the x extent (qtyx) . */ qtxy = maxxy[0][0] / maxxy[0][1]; qtyx = 1./qtxy; if ( (qtxy > (2.+EPSILON)) OR (qtyx > (2.+EPSILON)) ) { if (auto_scale) { /* * Readjust the scaling and recompute the data limits. */ iscale = 1; if (qtxy > (2+EPSILON) ) { magy *= qtxy; } else { magx *= qtyx; } magx_auto = magx; magy_auto = magy; magz_auto = magz; goto data_limits; } else { /* * Issue a warning and turn off gradient estimation. */ TooNarrow(); } } if (igrad) { qtzx = maxxy[0][2] / maxxy[0][0]; qtzy = maxxy[0][2] / maxxy[0][1]; if ( (qtzx > 60) OR (qtzy > 60) ) { if (auto_scale) { /* * Readjust the scaling and recompute the data limits. The X and Y * scales have been appropriately adjusted by the time you get here, * so dividing magz by either qtzx or qtzy will bring it in line. */ iscale = 1; magz *= 1./qtzx; magx_auto = magx; magy_auto = magy; magz_auto = magz; goto data_limits; } else { /* * Issue a warning and turn off gradient estimation. */ TooSteep(); } } if ( (qtzx < .017) OR (qtzy < .017) ) { if (auto_scale) { /* * Readjust the scaling and recompute the data limits. The X and Y * scales have been appropriately adjusted by the time you get here, * so dividing magz by either qtzx or qtzy will bring it in line. */ iscale = 1; magz *= 1./qtzx; magx_auto = magx; magy_auto = magy; magz_auto = magz; goto data_limits; } else { /* * Issue a warning and turn off gradient estimation. */ TooShallow(); } } } if (igrad) { points = DoubleMatrix(datcnt+4, 6); if (error_status) return (error_status); } else { points = DoubleMatrix(datcnt+4, 3); if (error_status) return (error_status); } joints = DoubleMatrix(datcnt3, 2); if (error_status) return (error_status); curdat = rootdat->nextdat; rootdat->nextdat = NULL; for (i0 = 0; i0 < datcnt; i0++) { sumx += points[i0][0] = curdat->values[0] * magx; sumx2 += SQ(points[i0][0]); sumy += points[i0][1] = curdat->values[1] * magy; sumy2 += SQ(points[i0][1]); sumxy += points[i0][0] * points[i0][1]; if (densi) points[i0][2] = 1; else { sumz += points[i0][2] = curdat->values[2] * magz; sumxz += points[i0][0] * points[i0][2]; sumyz += points[i0][1] * points[i0][2]; } holddat = curdat; curdat = curdat->nextdat; free(holddat); } det = (datcnt * (sumx2 * sumy2 - sumxy * sumxy)) - (sumx * (sumx * sumy2 - sumy * sumxy)) + (sumy * (sumx * sumxy - sumy * sumx2)); aaa = ((sumz * (sumx2 * sumy2 - sumxy * sumxy)) - (sumxz * (sumx * sumy2 - sumy * sumxy)) + (sumyz * (sumx * sumxy - sumy * sumx2))) / det; bbb = ((datcnt * (sumxz * sumy2 - sumyz * sumxy)) - (sumz * (sumx * sumy2 - sumy * sumxy)) + (sumy * (sumx * sumyz - sumy * sumxz))) / det; ccc = ((datcnt * (sumx2 * sumyz - sumxy * sumxz)) - (sumx * (sumx * sumyz - sumy * sumxz)) + (sumz * (sumx * sumxy - sumy * sumx2))) / det; for (i0 = 0 ; i0 < 3 ; i0++) { points[datcnt+i0][0] = maxxy[1][0] + bigtri[i0][0] * maxxy[0][0] * RANGE; points[datcnt+i0][1] = maxxy[1][1] + bigtri[i0][1] * maxxy[0][1] * RANGE; if (densi) points[datcnt+i0][2] = 1; else points[datcnt+i0][2] = aaa + bbb * points[datcnt+i0][0] + ccc * points[datcnt+i0][1]; } rootdat = NULL; } else { ErrorHnd(1, "ReadData", filee, "\n"); error_status = 1; return (error_status); } /* * Determine if any input data coordinates are duplicated. */ if (nndup == 1) { for (i0 = 0 ; i0 < datcnt ; i0++) { for (i1 = i0+1 ; i1 < datcnt ; i1++) { if ( (points[i0][0] == points[i1][0]) && (points[i0][1] == points[i1][1]) ) { sprintf(emsg,"\n Coordinates %d and %d are identical.\n",i0,i1); ErrorHnd(2, "ReadData", filee, emsg); error_status = 2; return (error_status); } } } } /* * Introduce a small random perturbation into the coordinate values. */ srand(367); for (i0 = 0 ; i0 < datcnt ; i0++) { for (i1 = 0 ; i1 < 2 ; i1++) { points[i0][i1] += wbit * (0.5 - (double)rand() / RAND_MAX); } } if (sdip OR igrad) { piby2 = 2 * atan(1.0); nn_pi = piby2 * 2; piby32 = 3 * piby2; rad2deg = 90 / piby2; } return (0); }
/** * Determine type and flags from scratch. * * \param mat matrix. * * This is expensive enough to only want to do it once. */ static void analyse_from_scratch( GLmatrix *mat ) { const GLfloat *m = mat->m; GLuint mask = 0; GLuint i; for (i = 0 ; i < 16 ; i++) { if (m[i] == 0.0) mask |= (1<<i); } if (m[0] == 1.0F) mask |= (1<<16); if (m[5] == 1.0F) mask |= (1<<21); if (m[10] == 1.0F) mask |= (1<<26); if (m[15] == 1.0F) mask |= (1<<31); mat->flags &= ~MAT_FLAGS_GEOMETRY; /* Check for translation - no-one really cares */ if ((mask & MASK_NO_TRX) != MASK_NO_TRX) mat->flags |= MAT_FLAG_TRANSLATION; /* Do the real work */ if (mask == (GLuint) MASK_IDENTITY) { mat->type = MATRIX_IDENTITY; } else if ((mask & MASK_2D_NO_ROT) == (GLuint) MASK_2D_NO_ROT) { mat->type = MATRIX_2D_NO_ROT; if ((mask & MASK_NO_2D_SCALE) != MASK_NO_2D_SCALE) mat->flags |= MAT_FLAG_GENERAL_SCALE; } else if ((mask & MASK_2D) == (GLuint) MASK_2D) { GLfloat mm = DOT2(m, m); GLfloat m4m4 = DOT2(m+4,m+4); GLfloat mm4 = DOT2(m,m+4); mat->type = MATRIX_2D; /* Check for scale */ if (SQ(mm-1) > SQ(1e-6) || SQ(m4m4-1) > SQ(1e-6)) mat->flags |= MAT_FLAG_GENERAL_SCALE; /* Check for rotation */ if (SQ(mm4) > SQ(1e-6)) mat->flags |= MAT_FLAG_GENERAL_3D; else mat->flags |= MAT_FLAG_ROTATION; } else if ((mask & MASK_3D_NO_ROT) == (GLuint) MASK_3D_NO_ROT) { mat->type = MATRIX_3D_NO_ROT; /* Check for scale */ if (SQ(m[0]-m[5]) < SQ(1e-6) && SQ(m[0]-m[10]) < SQ(1e-6)) { if (SQ(m[0]-1.0) > SQ(1e-6)) { mat->flags |= MAT_FLAG_UNIFORM_SCALE; } } else { mat->flags |= MAT_FLAG_GENERAL_SCALE; } } else if ((mask & MASK_3D) == (GLuint) MASK_3D) { GLfloat c1 = DOT3(m,m); GLfloat c2 = DOT3(m+4,m+4); GLfloat c3 = DOT3(m+8,m+8); GLfloat d1 = DOT3(m, m+4); GLfloat cp[3]; mat->type = MATRIX_3D; /* Check for scale */ if (SQ(c1-c2) < SQ(1e-6) && SQ(c1-c3) < SQ(1e-6)) { if (SQ(c1-1.0) > SQ(1e-6)) mat->flags |= MAT_FLAG_UNIFORM_SCALE; /* else no scale at all */ } else { mat->flags |= MAT_FLAG_GENERAL_SCALE; } /* Check for rotation */ if (SQ(d1) < SQ(1e-6)) { CROSS3( cp, m, m+4 ); SUB_3V( cp, cp, (m+8) ); if (LEN_SQUARED_3FV(cp) < SQ(1e-6)) mat->flags |= MAT_FLAG_ROTATION; else mat->flags |= MAT_FLAG_GENERAL_3D; } else { mat->flags |= MAT_FLAG_GENERAL_3D; /* shear, etc */ } } else if ((mask & MASK_PERSPECTIVE) == MASK_PERSPECTIVE && m[11]==-1.0F) { mat->type = MATRIX_PERSPECTIVE; mat->flags |= MAT_FLAG_GENERAL; } else { mat->type = MATRIX_GENERAL; mat->flags |= MAT_FLAG_GENERAL; } }
void LiveSLAMWrapper::Loop() { std::list<visensor_node::visensor_imu>::reverse_iterator reverse_iterImu ; std::list<ImageMeasurement>::iterator pIter ; ros::Time imageTimeStamp ; cv::Mat image0 ; cv::Mat image1 ; ros::Rate r(1000.0); while ( nh.ok() ) { monoOdometry->tracking_mtx.lock(); bool tmpFlag = monoOdometry->lock_densetracking ; monoOdometry->tracking_mtx.unlock(); //printf("tmpFlag = %d\n", tmpFlag ) ; if ( tmpFlag == true ){ r.sleep() ; continue ; } image0_queue_mtx.lock(); image1_queue_mtx.lock(); imu_queue_mtx.lock(); pIter = pImage1Iter ; pIter++ ; if ( pIter == image1Buf.end() ){ image0_queue_mtx.unlock(); image1_queue_mtx.unlock(); imu_queue_mtx.unlock(); r.sleep() ; continue ; } pIter = pImage0Iter ; pIter++ ; if ( pIter == image0Buf.end() ){ image0_queue_mtx.unlock(); image1_queue_mtx.unlock(); imu_queue_mtx.unlock(); r.sleep() ; continue ; } imageTimeStamp = pIter->t ; reverse_iterImu = imuQueue.rbegin() ; // printf("%d %d\n", imuQueue.size() < 10, reverse_iterImu->header.stamp <= imageTimeStamp ) ; if ( imuQueue.size() < 1 || reverse_iterImu->header.stamp < imageTimeStamp ){ image0_queue_mtx.unlock(); image1_queue_mtx.unlock(); imu_queue_mtx.unlock(); r.sleep() ; continue ; } //std::cout << imageTimeStamp.toNSec() << "\n" ; //std::cout << "[dt-image] " << imageTimeStamp << std::endl ; //std::cout << "[dt-imu] " << reverse_iterImu->header.stamp << " " << imuQueue.size() << std::endl ; ros::Time preTime = pImage1Iter->t ; pImage1Iter++ ; pImage0Iter++ ; imu_queue_mtx.unlock(); image1 = pImage1Iter->image.clone(); image0 = pImage0Iter->image.clone(); image1_queue_mtx.unlock(); image0_queue_mtx.unlock(); imu_queue_mtx.lock(); Quaterniond q, dq ; q.setIdentity() ; while ( currentIMU_iter->header.stamp < imageTimeStamp ) { double pre_t = currentIMU_iter->header.stamp.toSec(); currentIMU_iter++ ; double next_t = currentIMU_iter->header.stamp.toSec(); double dt = next_t - pre_t ; //prediction for dense tracking dq.x() = currentIMU_iter->angular_velocity.x*dt*0.5 ; dq.y() = currentIMU_iter->angular_velocity.y*dt*0.5 ; dq.z() = currentIMU_iter->angular_velocity.z*dt*0.5 ; dq.w() = sqrt( 1 - SQ(dq.x()) * SQ(dq.y()) * SQ(dq.z()) ) ; q = (q * dq).normalized(); } imu_queue_mtx.unlock(); // process image //Util::displayImage("MyVideo", image.data); Matrix3d deltaR(q) ; //puts("444") ; // cv::imshow("img0", image0 ) ; // cv::imshow("img1", image1 ) ; // cv::waitKey(1) ; ++imageSeqNumber; assert(image0.elemSize() == 1); assert(image1.elemSize() == 1); assert(fx != 0 || fy != 0); // if(!isInitialized) // { // monoOdometry->insertFrame(imageSeqNumber, image1, imageTimeStamp, // Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero() ); // cv::Mat disparity, depth ; // monoOdometry->bm_(image1, image0, disparity, CV_32F); // calculateDepthImage(disparity, depth, 0.11, fx ); // monoOdometry->currentKeyFrame = monoOdometry->slidingWindow[0] ; // monoOdometry->currentKeyFrame->setDepthFromGroundTruth( (float*)depth.data ) ; // monoOdometry->currentKeyFrame->keyFrameFlag = true ; // monoOdometry->currentKeyFrame->cameraLinkList.clear() ; // monoOdometry->RefToFrame = Sophus::SE3() ; // isInitialized = true; // } // else if(isInitialized && monoOdometry != nullptr) // { monoOdometry->trackFrame(image0, imageSeqNumber, imageTimeStamp, deltaR, R_i_2_c, T_i_2_c ); // } } }
/************************************************************************** NR routine to do sqrt(a^2 + b^2) */ static double dpythag(double a, double b) { a = fabs(a) ; b = fabs(b) ; if (a > b) return a*sqrt(1.0+SQ(b/a)) ; return (b == 0.0 ? 0.0 : b*sqrt(1.0+SQ(a/b))) ; }
/* central difference approximation of the second derivative in y */ inline double d2dy(double **m, int i, int j, double dy){ return (m[i][j+1] - 2*m[i][j] + m[i][j-1]) / (SQ(dy)); }
void IDPNagato::sim(int cur, double t)//Magick_numbers { // assert(cur <= (n_divide-2)-2); #define SQ(x) ((x)*(x)) if (t < ts[1]) { v1.tha = Qa[1]; v1.thv = v1.tha*t; v1.th = 0.5*v1.tha*t*t + Qs_i; } if (ts[1] < t && t < ts[2]) { v1.tha = Qa[2]; v1.thv = Qv[1] + v1.tha*(t - ts[1]); v1.th = Qs[1] + Qv[1]*(t - ts[1]) + 0.5*v1.tha*SQ(t - ts[1]); } if (ts[2] < t && t < ts[3]) { v1.tha = Qa[3]; v1.thv = Qv[2] + v1.tha*(t - ts[2]); v1.th = Qs[2] + Qv[2]*(t - ts[2]) + 0.5*v1.tha*SQ(t - ts[2]); } if (1 == cur) return; if (ts[3] < t && t < ts[4]) { v1.tha = Qa[4]; v1.thv = Qv[3] + v1.tha*(t - ts[3]); v1.th = Qs[3] + Qv[3]*(t - ts[3]) + 0.5*v1.tha*SQ(t - ts[3]); } if (2 == cur) return; if (ts[4] < t && t < ts[5]) { v1.tha = Qa[5]; v1.thv = Qv[4] + v1.tha*(t - ts[4]); v1.th = Qs[4] + Qv[4]*(t - ts[4]) + 0.5*v1.tha*SQ(t - ts[4]); } if (3 == cur) return; if (ts[5] < t && t < ts[6]) { v1.tha = Qa[6]; v1.thv = Qv[5] + v1.tha*(t - ts[5]); v1.th = Qs[5] + Qv[5]*(t - ts[5]) + 0.5*v1.tha*SQ(t - ts[5]); } if (ts[6] < t && t < ts[7]) { v1.tha = Qa[7]; v1.thv = Qv[6] + v1.tha*(t - ts[6]); v1.th = Qs[6] + Qv[6]*(t - ts[6]) + 0.5*v1.tha*SQ(t - ts[6]); } if (ts[7] < t && t < ts[8]) { v1.tha = Qa[8]; v1.thv = Qv[7] + v1.tha*(t - ts[7]); v1.th = Qs[7] + Qv[7]*(t - ts[7]) + 0.5*v1.tha*SQ(t - ts[7]); } if (ts[8] <= t) {// Through path } if (4 == cur) return; #undef SQ }
/* approximation of the first derivative of the square of u in x */ inline double du2dx(double **m, int i, int j, double dx, double alpha){ return ( SQ(m[i][j]+m[i+1][j]) - SQ(m[i-1][j]+m[i][j]) + alpha * ( fabs(m[i][j]+m[i+1][j]) * (m[i][j]-m[i+1][j]) - fabs(m[i-1][j]+m[i][j]) * (m[i-1][j]-m[i][j]) ) )/dx/4.0; }
int cEval::ScoreChains(POS *p, int sd) { int mgResult = 0; int sq = p->king_sq[sd]; int op = Opp(sd); // basic pointy chain if (SqBb(sq) & bbKSCastle[sd]) { if (OPP_PAWN(E4)) { if (CONTAINS(opPawns, D5, C6)) { // c6-d5-e4 triad mgResult -= (CONTAINS(sdPawns, D4, E3)) ? bigChainScore : smallChainScore; } if (CONTAINS(opPawns, D5, F3)) { // d5-e4-f3 triad mgResult -= (OWN_PAWN(E3)) ? bigChainScore : smallChainScore; } } if (OPP_PAWN(E5)) { if (CONTAINS(opPawns, F4, D6)) { // d6-e5-f4 triad // storm of a "g" pawn in the King's Indian if (OPP_PAWN(G5)) { mgResult -= 4; if (OPP_PAWN(H4)) return 10; // opponent did us a favour! } if (OPP_PAWN(G4)) mgResult -= 12; mgResult -= (CONTAINS(sdPawns, E4, D5)) ? bigChainScore : smallChainScore; } if (CONTAINS(opPawns, G3, F4)) { // e5-f4-g3 triad mgResult -= (OWN_PAWN(F3)) ? bigChainScore : smallChainScore; } } } if (SqBb(sq) & bbQSCastle[sd]) { // basic pointy chain if (OPP_PAWN(D4)) { if (CONTAINS(opPawns, E5, F6)) { mgResult -= (CONTAINS(sdPawns, E4, D3)) ? bigChainScore : smallChainScore; } if (CONTAINS(opPawns, F5, C3)) { mgResult -= (SQ(D3) & sdPawns) ? bigChainScore : smallChainScore; } } if (OPP_PAWN(D5)) { if (CONTAINS(opPawns, C4, E6)) { // storm of a "b" pawn if (OPP_PAWN(B5)) { mgResult -= 4; if (OPP_PAWN(A4)) return 0; // this is not how you handle pawn chains } if (OPP_PAWN(B4)) mgResult -= 12; mgResult -= (CONTAINS(sdPawns, E4, D5)) ? bigChainScore : smallChainScore; } if (CONTAINS(opPawns, B3, C4)) { mgResult -= (OWN_PAWN(C3)) ? bigChainScore : smallChainScore; } } } return mgResult; }
/* central difference approximation of the second derivative in x */ inline double d2dx(double **m, int i, int j, double dx){ return (m[i+1][j] - 2*m[i][j] + m[i-1][j]) / (SQ(dx)); }
//----------------------------------------------------------------------------- double PointToPointDist (const SHAPE &Shape1, int iPoint1, const SHAPE &Shape2, int iPoint2) { return sqrt(SQ(Shape1(iPoint1, VX) - Shape2(iPoint2, VX)) + SQ(Shape1(iPoint1, VY) - Shape2(iPoint2, VY))); }
void RayTracingEnvironment::ComputeVirtualLightSources(void) { int start_pos=0; for(int b=0;b<3;b++) { int nl=LightList.Count(); int where_to_start=start_pos; start_pos=nl; for(int l=where_to_start;l<nl;l++) { DirectionalSampler_t sample_generator; int n_desired=1*LightList[l].m_Color.Length(); if (LightList[l].m_Type==MATERIAL_LIGHT_SPOT) n_desired*=LightList[l].m_Phi/2; for(int try1=0;try1<n_desired;try1++) { LightDesc_t const &li=LightList[l]; FourRays myrays; myrays.origin.DuplicateVector(li.m_Position); RayTracingResult rslt; Vector trial_dir=sample_generator.NextValue(); if (li.IsDirectionWithinLightCone(trial_dir)) { myrays.direction.DuplicateVector(trial_dir); Trace4Rays(myrays,all_zeros,ReplicateX4(1000.0), &rslt); if ((rslt.HitIds[0]!=-1)) { // make sure normal points back towards ray origin fltx4 ndoti=rslt.surface_normal*myrays.direction; fltx4 bad_dirs=AndSIMD(CmpGtSIMD(ndoti,Four_Zeros), LoadAlignedSIMD((float *) signmask)); // flip signs of all "wrong" normals rslt.surface_normal.x=XorSIMD(bad_dirs,rslt.surface_normal.x); rslt.surface_normal.y=XorSIMD(bad_dirs,rslt.surface_normal.y); rslt.surface_normal.z=XorSIMD(bad_dirs,rslt.surface_normal.z); // a hit! let's make a virtual light source // treat the virtual light as a disk with its center at the hit position // and its radius scaled by the amount of the solid angle this probe // represents. float area_of_virtual_light= 4.0*M_PI*SQ( SubFloat( rslt.HitDistance, 0 ) )*(1.0/n_desired); FourVectors intens; intens.DuplicateVector(Vector(0,0,0)); FourVectors surface_pos=myrays.direction; surface_pos*=rslt.HitDistance; surface_pos+=myrays.origin; FourVectors delta=rslt.surface_normal; delta*=0.1; surface_pos+=delta; LightList[l].ComputeLightAtPoints(surface_pos,rslt.surface_normal, intens); FourVectors surf_colors; surf_colors.DuplicateVector(TriangleColors[rslt.HitIds[0]]); intens*=surf_colors; // see if significant LightDesc_t l1; l1.m_Type=MATERIAL_LIGHT_SPOT; l1.m_Position=Vector(surface_pos.X(0),surface_pos.Y(0),surface_pos.Z(0)); l1.m_Direction=Vector(rslt.surface_normal.X(0),rslt.surface_normal.Y(0), rslt.surface_normal.Z(0)); l1.m_Color=Vector(intens.X(0),intens.Y(0),intens.Z(0)); if (l1.m_Color.Length()>0) { l1.m_Color*=area_of_virtual_light/M_PI; l1.m_Range=0.0; l1.m_Falloff=1.0; l1.m_Attenuation0=1.0; l1.m_Attenuation1=0.0; l1.m_Attenuation2=1.0; // intens falls off as 1/r^2 l1.m_Theta=0; l1.m_Phi=M_PI; l1.RecalculateDerivedValues(); LightList.AddToTail(l1); } } } } } } }
/** * Very simple test: pick random skyposition, compute a_i, b_i using * once LALComputeAM() and once LALGetAMCoeffs(), and look at the errors * sum_i (a_i - a_i')^2 */ int main(int argc, char *argv[]) { LALStatus XLAL_INIT_DECL(status); LIGOTimeGPS startTime = {714180733, 0}; REAL8 duration = 180000; /* 50 hours */ REAL8 Tsft = 1800; /* assume 30min SFTs */ LIGOTimeGPSVector *timestamps = NULL; DetectorStateSeries *detStates = NULL; SkyPosition XLAL_INIT_DECL(skypos); EphemerisData XLAL_INIT_DECL(edat); BarycenterInput XLAL_INIT_DECL(baryinput); LALDetector *det = NULL; AMCoeffs XLAL_INIT_DECL(AMold); AMCoeffs XLAL_INIT_DECL(AMnew); REAL8 alpha, delta; AMCoeffsParams XLAL_INIT_DECL(amParams); EarthState earth; UINT4 i; REAL8 maxerr_a, maxerr_b, averr_a, averr_b; REAL8 tolerance = 1e-2; /* be generous: allow 1% error */ struct tms buf; const CHAR *sites[] = {"H1", "L1", "V2", "G1", "T1" }; UINT4 pickedSite; char earthEphem[] = TEST_DATA_DIR "earth00-19-DE405.dat.gz"; char sunEphem[] = TEST_DATA_DIR "sun00-19-DE405.dat.gz"; if ( argc == 2 && !strcmp(argv[1], "-v1") ) /* init random-generator */ srand ( times(&buf) ); /* ----- init ephemeris ----- */ edat.ephiles.earthEphemeris = earthEphem; edat.ephiles.sunEphemeris = sunEphem; SUB ( LALInitBarycenter(&status, &edat), &status); /* ----- get timestamps ----- */ SUB ( LALMakeTimestamps ( &status, ×tamps, startTime, duration, Tsft ), &status ); /* ----- allocate memory for AM-coeffs ----- */ AMold.a = XLALCreateREAL4Vector ( timestamps->length ); AMold.b = XLALCreateREAL4Vector ( timestamps->length ); AMnew.a = XLALCreateREAL4Vector ( timestamps->length ); AMnew.b = XLALCreateREAL4Vector ( timestamps->length ); /* ----- pick detector-site at random ----- */ pickedSite = floor( 5 * (1.0 * rand() / (RAND_MAX + 1.0) ) ); /* int in [0,5) */ if ( ( det = XLALGetSiteInfo ( sites[pickedSite] )) == NULL ) { XLALPrintError ("\nCall to XLALGetSiteInfo() has failed for site = '%s'... \n\n", sites[pickedSite]); return GETAMCOEFFSTEST_ESUB; } /* ----- pick skyposition at random ----- */ alpha = LAL_TWOPI * (1.0 * rand() / ( RAND_MAX + 1.0 ) ); /* uniform in [0, 2pi) */ delta = LAL_PI_2 - acos ( 1 - 2.0 * rand()/RAND_MAX ); /* sin(delta) uniform in [-1,1] */ /* ===== compute AM-coeffs the 'old way': ===== */ baryinput.site.location[0] = det->location[0]/LAL_C_SI; baryinput.site.location[1] = det->location[1]/LAL_C_SI; baryinput.site.location[2] = det->location[2]/LAL_C_SI; baryinput.alpha = alpha; baryinput.delta = delta; baryinput.dInv = 0.e0; /* amParams structure to compute a(t) and b(t) */ amParams.das = (LALDetAndSource *)LALMalloc(sizeof(LALDetAndSource)); amParams.das->pSource = (LALSource *)LALMalloc(sizeof(LALSource)); amParams.baryinput = &baryinput; amParams.earth = &earth; amParams.edat = &edat; amParams.das->pDetector = det; amParams.das->pSource->equatorialCoords.longitude = alpha; amParams.das->pSource->equatorialCoords.latitude = delta; amParams.das->pSource->orientation = 0.0; amParams.das->pSource->equatorialCoords.system = COORDINATESYSTEM_EQUATORIAL; amParams.polAngle = 0; SUB (LALComputeAM ( &status, &AMold, timestamps->data, &amParams), &status); /* ===== compute AM-coeffs the 'new way' using LALGetAMCoeffs() */ /* ----- get detector-state series ----- */ SUB ( LALGetDetectorStates (&status, &detStates, timestamps, det, &edat, 0 ), &status ); skypos.system = COORDINATESYSTEM_EQUATORIAL; skypos.longitude = alpha; skypos.latitude = delta; SUB ( LALGetAMCoeffs ( &status, &AMnew, detStates, skypos ), &status ); /* ===== analyse relative error ===== */ maxerr_a = maxerr_b = averr_a = averr_b = 0; for ( i=0; i < timestamps->length; i ++ ) { REAL8 thisErr; thisErr = sqrt( SQ ( AMold.a->data[i] - AMnew.a->data[i] ) / AMold.A ); averr_a += thisErr; maxerr_a = MYMAX( thisErr, maxerr_a ); thisErr = sqrt( SQ ( AMold.b->data[i] - AMnew.b->data[i] ) / AMold.B ); averr_b += thisErr; maxerr_b = MYMAX( thisErr, maxerr_b ); } averr_a /= timestamps->length; averr_b /= timestamps->length; if ( lalDebugLevel ) { printf ("Parameters: IFO = %s, skypos = [%g, %g]\n", sites[pickedSite], alpha, delta ); printf ("Maximal relative errors: maxerr(a) = %g %%, maxerr(b) = %g %% \n", 100.0 * maxerr_a, 100.0 * maxerr_b); printf ("Average relative errors: averr(a) = %g %%, averr(b) = %g %% \n", 100.0 * averr_a, 100.0 * averr_b ); } else printf ("%d %g %g %g %g %g %g \n", pickedSite, alpha, delta, averr_a, averr_b, maxerr_a, maxerr_b); if ( (averr_a > tolerance) || (averr_b > tolerance) || (maxerr_a > tolerance) ||(maxerr_b > tolerance)) { XLALPrintError ("Maximal error-tolerance of %g %% was exceeded!\n", 100.0 * tolerance ); return 1; } /* ----- free memory ----- */ XLALDestroyTimestampVector ( timestamps ); XLALDestroyREAL4Vector ( AMold.a ); XLALDestroyREAL4Vector ( AMold.b ); XLALDestroyREAL4Vector ( AMnew.a ); XLALDestroyREAL4Vector ( AMnew.b ); LALFree ( det ); XLALDestroyDetectorStateSeries ( detStates ); LALFree ( amParams.das->pSource ); LALFree ( amParams.das ); LALFree(edat.ephemE); LALFree(edat.ephemS); LALCheckMemoryLeaks(); return 0; /* OK */ } /* main() */
//--------------------------------------------------------- bool EdgeIntersect3D ( const DVec& a1, // [in] const DVec& b1, // [in] const DVec& a2, // [in] const DVec& b2, // [in] DVec& xint // [out] ) //--------------------------------------------------------- { bool test = false; xint = -999.0; // set xint to dummy vaklue double cx,cy,cz, dx,dy,dz, ex,ey,ez, t1,t2,mag,d; cx = (b2(2)-a2(2))*(b1(3)-a1(3)) - (b2(3)-a2(3))*(b1(2)-a1(2)); cy = (b2(3)-a2(3))*(b1(1)-a1(1)) - (b2(1)-a2(1))*(b1(3)-a1(3)); cz = (b2(1)-a2(1))*(b1(2)-a1(2)) - (b2(2)-a2(2))*(b1(1)-a1(1)); dx = (b2(2)-a2(2))*(a1(3)-a2(3)) - (b2(3)-a2(3))*(a1(2)-a2(2)); dy = (b2(3)-a2(3))*(a1(1)-a2(1)) - (b2(1)-a2(1))*(a1(3)-a2(3)); dz = (b2(1)-a2(1))*(a1(2)-a2(2)) - (b2(2)-a2(2))*(a1(1)-a2(1)); ex = (b1(2)-a1(2))*(a2(3)-a1(3)) - (b1(3)-a1(3))*(a2(2)-a1(2)); ey = (b1(3)-a1(3))*(a2(1)-a1(1)) - (b1(1)-a1(1))*(a2(3)-a1(3)); ez = (b1(1)-a1(1))*(a2(2)-a1(2)) - (b1(2)-a1(2))*(a2(1)-a1(1)); t1 = -(cx*dx + cy*dy + cz*dz); t2 = cx*ex + cy*ey + cz*ez; mag = cx*cx + cy*cy + cz*cz; double tol1 = 1e-6; if (mag < tol1) { // lines are colinear -- should be caught by "in" test return false; } t1/=mag; t2/=mag; DVec x1(3), x2(3); x1(1) = a1(1) + t1*(b1(1)-a1(1)); x1(2) = a1(2) + t1*(b1(2)-a1(2)); x1(3) = a1(3) + t1*(b1(3)-a1(3)); x2(1) = a2(1) + t2*(b2(1)-a2(1)); x2(2) = a2(2) + t2*(b2(2)-a2(2)); x2(3) = a2(3) + t2*(b2(3)-a2(3)); // make sure the segments really intersect. d = SQ(x1(1)-x2(1)) + SQ(x1(2)-x2(2)) + SQ(x1(3)-x2(3)); double tol2 = 1e-6; if (d>tol2) { return false; } if ( (t1 > -tol2 ) && (t2 > -tol2 ) && (t1 < (1.0+tol2)) && (t2 < (1.0+tol2)) ) { xint = x1; // we have intersection! return true; } return false; }
void Layer7::LoadSlot(QNetworkReply *reply) { switch (op) { case lsoStart: emit UpdateSignal("Loading started..."); Logger::ins.Header() << "Loading registry data from " << http.Url() << "data/ started...\r\n"; http.Propfind(http.Url() + "data/"); op = lsoListDomains; break; case lsoListDomains: if (reply->error() == QNetworkReply::NoError) { QString replyString(reply->readAll()); sl::log << "lsoListDomains: " << SQ(replyString) << sl::end; Propfinds elements = http.ParsePropfindReply(replyString); for (Propfinds::iterator e = elements.begin(); e != elements.end(); ++e) if (e->href != http.Url()) { domains.push_back(GtDomain(SQ(e->href), SQ(e->name))); Logger::ins.Info() << "Service domain: " << e->name << "\r\n"; emit UpdateSignal("Service Domain: " + e->name); } if (SelectFirstDomain()) { loaded = true; // check if all domains match domains from the db for (unsigned int d = 0; d < domains.size(); ++d) if (!sdoms.contains(QS(domains[d].name))) { loaded = false; Logger::ins.Error() << "Repository does not contain service domain named " << QS(domains[d].name) << "\r\n"; emit DoneSignal("Repository does not contain service domain named " + QS(domains[d].name)); } if (loaded) { op = lsoListServices; http.Propfind(QS(SelectedDomain().href) + "web/"); } } } else emit UpdateSignal(reply->errorString()); break; case lsoListServices: if (reply->error() == QNetworkReply::NoError) { QString replyString(reply->readAll()); sl::log << "lsoListServices: " << SQ(replyString) << sl::end; Propfinds elements = http.ParsePropfindReply(replyString); for (Propfinds::iterator e = elements.begin(); e != elements.end(); ++e) if (e->href != http.Url() && e->name != "OLD" && e->name != "REMOVED") { endpoints.push_back(GtEndpoint(item++ * 100 + REPO_TB_IDX, SelectedDomain().name, SQ(e->href), SQ(e->name))); Logger::ins.Info() << "Service: " << e->name << "\r\n"; emit UpdateSignal("Service: " + e->name); } } else emit UpdateSignal(reply->errorString()); if (SelectNextDomain()) http.Propfind(QS(SelectedDomain().href) + "web/"); else if (SelectFirstEndpoint()) { Logger::ins.Info() << "Parsing index files...\r\n"; emit UpdateSignal("Parsing index files..."); op = lsoGetIndexXml; Logger::ins.Info() << "Parsing " << QS(SelectedEndpoint().urlRegistryFolder) << "index.xml...\r\n"; http.Get(QS(SelectedEndpoint().urlRegistryFolder) + "index.xml"); } break; case lsoGetIndexXml: if (reply->error() == QNetworkReply::NoError) { QString replyString(reply->readAll()); sl::log << "lsoGetIndexXml: " << SQ(replyString) << sl::end; QDomDocument doc; doc.setContent(replyString); for (QDomElement r = doc.firstChildElement(); !r.isNull(); r = r.nextSiblingElement()) { if (r.nodeName() == "service") { for (QDomElement e = r.firstChildElement(); !e.isNull(); e = e.nextSiblingElement()) { if (e.nodeName() == "files") { for (QDomElement f = e.firstChildElement(); !f.isNull(); f = f.nextSiblingElement()) { QDomAttr r = f.attributeNode("root"), t = f.attributeNode("type"); if (!r.isNull() && r.nodeValue() == "true" && !t.isNull() && t.value() == "WSDL") SelectedEndpoint().urlWSDL = SQ(f.text()); } } else if (e.nodeName() == "name") SelectedEndpoint().intGivenName = SQ(e.text()); else if (e.nodeName() == "version") SelectedEndpoint().intGivenVersion = SQ(e.text()); else if (e.nodeName() == "policyVersion") SelectedEndpoint().policyVersion = SQ(e.text()); else if (e.nodeName() == "enabled") SelectedEndpoint().enabled = SQ(e.text()); else if (e.nodeName() == "soap") SelectedEndpoint().soap = SQ(e.text()); else if (e.nodeName() == "resolutionPath") SelectedEndpoint().resolutionPath = SQ(e.text()); else if (e.nodeName() == "id") SelectedEndpoint().intGivenId = SQ(e.text()); else if (e.nodeName() == "protectedEndpoint") SelectedEndpoint().protectedEndpoint = SQ(e.text()); } break; } } if (SelectedEndpoint().urlWSDL == "") { Logger::ins.Warning() << "WSDL file url not found for endpoint '" << QS(SelectedEndpoint().intGivenName) << "'\r\n"; emit UpdateSignal("WSDL file url not found for endpoint '" + QS(SelectedEndpoint().intGivenName) + "'"); SelectedEndpoint().serviceName = SelectedEndpoint().intGivenName; } if (SelectedEndpoint().protectedEndpoint == "") { Logger::ins.Warning() << "Ignoring non-protected endpoint '" << QS(SelectedEndpoint().intGivenName) << "'\r\n"; emit UpdateSignal("Ignoring non-protected endpoint '" + QS(SelectedEndpoint().intGivenName) + "'"); RemoveSelectedEndpoint(); } } else { Logger::ins.Error() << reply->errorString() << "\r\n"; emit UpdateSignal(reply->errorString()); } if (SelectNextEndpoint()) { Logger::ins.Info() << "Parsing " << QS(SelectedEndpoint().urlRegistryFolder) << "index.xml...\r\n"; http.Get(QS(SelectedEndpoint().urlRegistryFolder) + "index.xml"); } else if (SelectFirstEndpoint()) { Logger::ins.Info() << "Parsing WSDL files...\r\n"; emit UpdateSignal("Parsing WSDL files..."); op = lsoGetWsdl; Logger::ins.Info() << "Parsing " << QS(SelectedEndpoint().urlRegistryFolder + SelectedEndpoint().urlWSDL) << "\r\n"; http.Get(QS(SelectedEndpoint().urlRegistryFolder + SelectedEndpoint().urlWSDL)); } break; case lsoGetWsdl: if (reply->error() == QNetworkReply::NoError) { QString replyString(reply->readAll()); sl::log << "lsoGetWsdl: " << SQ(replyString) << sl::end; QDomDocument doc; doc.setContent(replyString); for (QDomElement r = doc.firstChildElement(); !r.isNull(); r = r.nextSiblingElement()) { if (r.nodeName() == "definitions" || r.nodeName() == "wsdl:definitions") { QDomAttr n = r.attributeNode("targetNamespace"); if (!n.isNull()) SelectedEndpoint().targetNamespace = SQ(n.nodeValue()); n = r.attributeNode("name"); if (!n.isNull()) SelectedEndpoint().definitionName = SQ(n.nodeValue()); // find all service names std::vector<std::string> serviceNames; for (QDomElement e = r.firstChildElement(); !e.isNull(); e = e.nextSiblingElement()) { if (e.nodeName() == "service" || e.nodeName() == "wsdl:service") { QDomAttr a = e.attributeNode("name"); if (!a.isNull()) serviceNames.push_back(SQ(a.nodeValue())); } } // assign service name if (serviceNames.size() == 1) SelectedEndpoint().serviceName = serviceNames[0]; else if (serviceNames.size() > 1) { // search service node name with best fit to protected endpoint string std::map<int, std::string> m; for (std::vector<std::string>::iterator s = serviceNames.begin(); s != serviceNames.end(); ++s) m.insert(std::pair<int, std::string>(LevenshteinDistance(SelectedEndpoint().protectedEndpoint, *s, true), *s)); SelectedEndpoint().serviceName = m.begin()->second; } else { Logger::ins.Warning() << "Service name not found for endpoint '" << QS(SelectedEndpoint().intGivenName) + "'\r\n"; emit UpdateSignal("Service name not found for endpoint '" + QS(SelectedEndpoint().intGivenName) + "'"); SelectedEndpoint().serviceName = SelectedEndpoint().intGivenName; } break; } } } else { Logger::ins.Error() << reply->errorString() << "\r\n"; emit UpdateSignal(reply->errorString()); } if (SelectNextEndpoint()) { Logger::ins.Info() << "Parsing " << QS(SelectedEndpoint().urlRegistryFolder + SelectedEndpoint().urlWSDL) << "\r\n"; http.Get(QS(SelectedEndpoint().urlRegistryFolder + SelectedEndpoint().urlWSDL)); } else { Logger::ins.Info() << "Finished loading registry.\r\n"; emit DoneSignal("Finished loading."); } break; default: ; } }
Vect(P &a, P &b) { p = a; q = b; dist = SQ(a.X - b.X) + SQ(a.Y - b.Y); }
R norm2sq(V2<R> const& p) { return SQ(p.x)+SQ(p.y); }
/** Initialize amplitude-prior pdfs from the user-input */ int XLALInitAmplitudePrior ( AmplitudePrior_t *AmpPrior, const UserInput_t *uvar ) { const UINT4 AmpPriorBins = 100; // defines the binnning accuracy of our prior-pdfs /* consistency check */ if ( !AmpPrior || !uvar ) { XLALPrintError ( "%s: invalid NULL input 'AmpPrior' or 'uvar'\n", __func__ ); XLAL_ERROR ( XLAL_EINVAL ); } if ( AmpPrior->pdf_h0Nat || AmpPrior->pdf_cosi || AmpPrior->pdf_psi || AmpPrior->pdf_phi0 ) { XLALPrintError ("%s: AmplitudePriors must be set to NULL before calling this function!\n", __func__ ); XLAL_ERROR ( XLAL_EINVAL ); } /* first check that user only provided *one* method of determining the amplitude-prior range */ UINT4 numSets = 0; if ( uvar->fixedh0Nat >= 0 ) numSets ++; if ( uvar->fixedSNR >= 0 ) numSets ++; if ( uvar->fixedh0NatMax >= 0 ) numSets ++ ; if ( uvar->fixedRhohMax >= 0 ) numSets ++; if ( numSets != 1 ) { XLALPrintError ("%s: Specify (>=0) exactly *ONE* amplitude-prior range of {fixedh0Nat, fixedSNR, fixedh0NatMax, fixedRhohMax}\n", __func__); XLAL_ERROR ( XLAL_EINVAL ); } /* ===== first pass: deal with all user-supplied fixed values ==> singular priors! ===== */ /* ----- h0 ----- */ if ( uvar->fixedh0Nat >= 0 ) /* fix h0Nat */ if ( (AmpPrior->pdf_h0Nat = XLALCreateSingularPDF1D ( uvar->fixedh0Nat )) == NULL ) XLAL_ERROR ( XLAL_EFUNC ); if ( uvar->fixedSNR >= 0 ) /* dummy-pdf, as signal will be computed with h0Nat=1 then rescaled to fixedSNR */ if ( (AmpPrior->pdf_h0Nat = XLALCreateSingularPDF1D ( 1.0 )) == NULL ) XLAL_ERROR ( XLAL_EFUNC ); AmpPrior->fixedSNR = uvar->fixedSNR; AmpPrior->fixRhohMax = (uvar->fixedRhohMax >= 0); /* ----- cosi ----- */ if ( XLALUserVarWasSet ( &uvar->cosi ) ) if ( (AmpPrior->pdf_cosi = XLALCreateSingularPDF1D ( uvar->cosi )) == NULL ) XLAL_ERROR ( XLAL_EFUNC ); /* ----- psi ----- */ if ( XLALUserVarWasSet ( &uvar->psi ) ) if ( (AmpPrior->pdf_psi = XLALCreateSingularPDF1D ( uvar->psi )) == NULL ) XLAL_ERROR ( XLAL_EFUNC ); /* ----- phi0 ----- */ if ( XLALUserVarWasSet ( &uvar->phi0 ) ) if ( (AmpPrior->pdf_phi0 = XLALCreateSingularPDF1D ( uvar->phi0 )) == NULL ) XLAL_ERROR ( XLAL_EFUNC ); /* ===== second pass: deal with non-singular prior ranges, taking into account the type of priors to use */ REAL8 h0NatMax = 0; if ( uvar->fixedh0NatMax >= 0 ) /* draw h0Nat from [0, h0NatMax] */ h0NatMax = uvar->fixedh0NatMax; if ( uvar->fixedRhohMax >= 0 ) /* draw h0 from [0, rhohMax/(detM)^(1/8)] */ h0NatMax = uvar->fixedRhohMax; /* at first, will be rescaled by (detM)^(1/8) after the fact */ switch ( uvar->AmpPriorType ) { case AMP_PRIOR_TYPE_PHYSICAL: /* ----- h0 ----- */ // uniform in [0, h0NatMax] : not that 'physical', but simple if ( AmpPrior->pdf_h0Nat == NULL ) if ( (AmpPrior->pdf_h0Nat = XLALCreateUniformPDF1D ( 0, h0NatMax )) == NULL ) XLAL_ERROR ( XLAL_EFUNC ); /* ----- cosi ----- */ if ( AmpPrior->pdf_cosi == NULL ) if ( (AmpPrior->pdf_cosi = XLALCreateUniformPDF1D ( -1.0, 1.0 )) == NULL ) XLAL_ERROR ( XLAL_EFUNC ); /* ----- psi ----- */ if ( AmpPrior->pdf_psi == NULL ) if ( (AmpPrior->pdf_psi = XLALCreateUniformPDF1D ( -LAL_PI_4, LAL_PI_4 )) == NULL ) XLAL_ERROR ( XLAL_EFUNC ); /* ----- phi0 ----- */ if ( AmpPrior->pdf_phi0 == NULL ) if ( (AmpPrior->pdf_phi0 = XLALCreateUniformPDF1D ( 0, LAL_TWOPI )) == NULL ) XLAL_ERROR ( XLAL_EFUNC ); break; case AMP_PRIOR_TYPE_CANONICAL: /* ----- pdf(h0) ~ h0^3 ----- */ if ( AmpPrior->pdf_h0Nat == NULL ) { UINT4 i; pdf1D_t *pdf; if ( ( pdf = XLALCreateDiscretePDF1D ( 0, h0NatMax, AmpPriorBins )) == NULL ) XLAL_ERROR ( XLAL_EFUNC ); for ( i=0; i < pdf->probDens->length; i ++ ) { REAL8 xMid = 0.5 * ( pdf->xTics->data[i] + pdf->xTics->data[i+1] ); pdf->probDens->data[i] = CUBE( xMid ); // pdf(h0) ~ h0^3 } AmpPrior->pdf_h0Nat = pdf; } /* ----- pdf(cosi) ~ ( 1 - cosi^2)^3 ----- */ if ( AmpPrior->pdf_cosi == NULL ) { UINT4 i; pdf1D_t *pdf; if ( ( pdf = XLALCreateDiscretePDF1D ( -1.0, 1.0, AmpPriorBins )) == NULL ) XLAL_ERROR ( XLAL_EFUNC ); for ( i=0; i < pdf->probDens->length; i ++ ) { REAL8 xMid = 0.5 * ( pdf->xTics->data[i] + pdf->xTics->data[i+1] ); REAL8 y = 1.0 - SQ(xMid); pdf->probDens->data[i] = CUBE( y ); } AmpPrior->pdf_cosi = pdf; } /* ----- psi ----- */ if ( AmpPrior->pdf_psi == NULL ) if ( (AmpPrior->pdf_psi = XLALCreateUniformPDF1D ( -LAL_PI_4, LAL_PI_4 )) == NULL ) XLAL_ERROR ( XLAL_EFUNC ); /* ----- phi0 ----- */ if ( AmpPrior->pdf_phi0 == NULL ) if ( (AmpPrior->pdf_phi0 = XLALCreateUniformPDF1D ( 0, LAL_TWOPI )) == NULL ) XLAL_ERROR ( XLAL_EFUNC ); break; default: XLALPrintError ("%s: something went wrong ... unknown priorType = %d\n", __func__, uvar->AmpPriorType ); XLAL_ERROR ( XLAL_EINVAL ); break; } // switch( uvar->AmpPriorType ) return XLAL_SUCCESS; } /* XLALInitAmplitudePrior() */
void LiveSLAMWrapper::Loop() { /* unsigned int image0BufSize ; unsigned int image1BufSize ; unsigned int imuBufSize ; */ std::list<visensor_node::visensor_imu>::reverse_iterator reverse_iterImu ; std::list<ImageMeasurement>::iterator pIter ; ros::Time imageTimeStamp ; cv::Mat image0 ; cv::Mat image1 ; ros::Rate r(1000.0); while ( nh.ok() ) { monoOdometry->tracking_mtx.lock(); bool tmpFlag = monoOdometry->lock_densetracking ; monoOdometry->tracking_mtx.unlock(); //printf("tmpFlag = %d\n", tmpFlag ) ; if ( tmpFlag == true ){ r.sleep() ; continue ; } image0_queue_mtx.lock(); image1_queue_mtx.lock(); imu_queue_mtx.lock(); pIter = pImage1Iter ; pIter++ ; if ( pIter == image1Buf.end() ){ image0_queue_mtx.unlock(); image1_queue_mtx.unlock(); imu_queue_mtx.unlock(); r.sleep() ; continue ; } pIter = pImage0Iter ; pIter++ ; if ( pIter == image0Buf.end() ){ image0_queue_mtx.unlock(); image1_queue_mtx.unlock(); imu_queue_mtx.unlock(); r.sleep() ; continue ; } imageTimeStamp = pIter->t ; reverse_iterImu = imuQueue.rbegin() ; // printf("%d %d\n", imuQueue.size() < 10, reverse_iterImu->header.stamp <= imageTimeStamp ) ; if ( imuQueue.size() < 1 || reverse_iterImu->header.stamp < imageTimeStamp ){ image0_queue_mtx.unlock(); image1_queue_mtx.unlock(); imu_queue_mtx.unlock(); r.sleep() ; continue ; } //std::cout << imageTimeStamp.toNSec() << "\n" ; //std::cout << "[dt-image] " << imageTimeStamp << std::endl ; //std::cout << "[dt-imu] " << reverse_iterImu->header.stamp << " " << imuQueue.size() << std::endl ; ros::Time preTime = pImage1Iter->t ; pImage1Iter++ ; pImage0Iter++ ; imu_queue_mtx.unlock(); image1 = pImage1Iter->image.clone(); image0 = pImage0Iter->image.clone(); image1_queue_mtx.unlock(); image0_queue_mtx.unlock(); imu_queue_mtx.lock(); Quaterniond q, dq ; q.setIdentity() ; while ( currentIMU_iter->header.stamp < imageTimeStamp ) { double pre_t = currentIMU_iter->header.stamp.toSec(); currentIMU_iter++ ; double next_t = currentIMU_iter->header.stamp.toSec(); double dt = next_t - pre_t ; //prediction for dense tracking dq.x() = currentIMU_iter->angular_velocity.x*dt*0.5 ; dq.y() = currentIMU_iter->angular_velocity.y*dt*0.5 ; dq.z() = currentIMU_iter->angular_velocity.z*dt*0.5 ; dq.w() = sqrt( 1 - SQ(dq.x()) * SQ(dq.y()) * SQ(dq.z()) ) ; q = (q * dq).normalized(); } imu_queue_mtx.unlock(); // process image //Util::displayImage("MyVideo", image.data); Matrix3d deltaR(q) ; //puts("444") ; ++imageSeqNumber; assert(image0.elemSize() == 1); assert(image1.elemSize() == 1); assert(fx != 0 || fy != 0); monoOdometry->trackFrame(image1, imageSeqNumber, imageTimeStamp, deltaR ); } }
void Conversions::setOriginEcef(double _x0,double _y0,double _z0) { double Xe = _x0; double Ye = _y0; double Ze = _z0; x0_ecef(0,0,_x0); x0_ecef(1,0,_y0); x0_ecef(2,0,_z0); // WGS 84 earth shape model double f = 1/298.257223563; // Ellipsoid's flatness double e = sqrt(f*(2-f)); // Ellipsoid's Eccentricity double rp = 6357752.3142; // [m] ; semiminor axis: radius polar // b=6356752:31424518 double re = 6378137; // [m] ; semimajor axis: radius equator // a=6378137 // Earth position from ECEF to NorthEastDown (Local) double lon = atan2(Ye,Xe); double p = sqrt( SQ(Xe)+SQ(Ye) ); double h[2] = {0, 0}; double lamdum[2] = {0, 0}; double N[2] = {re, re}; double alt=0, E, F, G, c, s, P, Q, r0, V, z0, e_a, lat; for (int i=0;i<100;i++) { lamdum[1] = atan( (Ze + SQ(e) * N[0] * sin( lamdum[0] ) ) / p); N[1] = re / sqrt(1 - SQ(e) * SQ( sin(lamdum[0]) ) ); h[1] = p / cos( lamdum[0] ) - N[0]; if ( (fabs(h[1])-h[0]) == 0 ) { alt=h[1]; break; } h[0]=h[1]; N[0]=N[1]; lamdum[0] = lamdum[1]; } E = sqrt(re*re-rp*rp); F = 54 * SQ(rp) * SQ(Ze); G = SQ(p) + (1 - SQ(e)) * SQ(Ze) - SQ(e) * SQ(E); c = ( SQ(SQ(e)) * F * SQ(p) ) / CUBIC(G); s = pow(( 1 + c + sqrt( SQ(c) + 2 * c) ), (1/3)); P = F / (3* SQ(s+(1/s)+1) * SQ(G)); Q = sqrt(1+2* SQ(SQ(e)) * P); r0= -(P * SQ(e) * p ) / (1+Q) + sqrt(0.5 * SQ(re) * (1+(1/Q))-(P*(1-SQ(e))*SQ(Ze))/(Q*(1+Q))-0.5 * P * SQ(p)); // U = sqrt( SQ(p- SQ(e) * r0) + SQ(Ze) ); V = sqrt( SQ(p- SQ(e) * r0) + (1-SQ(e)) * SQ(Ze) ); z0= ( SQ(rp) * Ze) / (re * V); e_a = re * e / rp; lat = atan( ( Ze + SQ(e_a) * z0) / p ); // printf("Conversions::setOrigin(): X0[deg] %f N %f E %f h\n",lat*180/3.141596,lon*180/3.141596,alt); // Rotation from ECEF to Local T_el(0,0,-cos(lon)*sin(lat)); T_el(1,0,-sin(lon)); T_el(2,0,-cos(lon)*cos(lat)); T_el(0,1,-sin(lon)*sin(lat)); T_el(1,1, cos(lon)); T_el(2,1,-sin(lon)*cos(lat)); T_el(0,2, cos(lat)); T_el(1,2, 0); T_el(2,2,-sin(lat)); this->originset = true; // std::cout << "T_el = " << T_el << std::endl; }
double thcond1_lam0(FluidState state, FpropsError *err){ if(state.fluid->thcond->type != FPROPS_THCOND_1){*err = FPROPS_INVALID_REQUEST; return NAN;} const ThermalConductivityData1 *k1 = &(state.fluid->thcond->data.k1); double lam0 = 0; MSG("k1: %p",k1); // TODO FIXME need to re-factor this to be standardised and only use data from filedata.h structures. if(0==strcmp(state.fluid->name,"carbondioxide")){ MSG("lam0 for carbondioxide"); //#define USE_CP0_FOR_LAM0 #ifdef USE_CP0_FOR_LAM0 double cp0 = fprops_cp0(state,err); double R = state.fluid->data->R; //MSG("cp0 = %f, R = %f", cp0, R); double M = state.fluid->data->M; double sigma = 0.3751; // nm! double opr2 = 0.177568/0.475598 * cp0/R * 1 / SQ(sigma) / sqrt(M); //MSG("1 + r^2 = %f (by cp0)", opr2_2); //MSG("5/2 *(cp0(T)/R - 1) = %f\n", 5./2*(fprops_cp0(state,err)/state.fluid->data->R - 1)); #else int i; double sum1 = 0; double c[] = {2.387869e-2, 4.350794, -10.33404, 7.981590, -1.940558}; for(i=0; i<5; ++i){ sum1 += c[i] * pow(state.T/100, 2-(i+1)); } double cint_over_k = 1.0 + exp(-183.5/state.T)*sum1; //MSG("cint/k = %f",cint_over_k); //MSG("1 + r^2 = %f (by cint/k)",1+0.4*cint_over_k); double opr2 = 1+0.4*cint_over_k; //double r = sqrt(0.4*cint_over_k); //MSG("r = %f",r); #endif // FIXME convert the other way, convert the cint/k stuff to simply cp0/R, should be more generalised that way. double CS_star = thcond1_cs(k1, k1->eps_over_k/state.T); //MSG("CS_star = %f", CS_star); lam0 = 0.475598 * sqrt(state.T) * opr2 / CS_star; // 0.177568 (mW/m/K)*(nm^2) //lam0 = 0.177568 * sqrt(state.T) / sqrt(state.fluid->data->M) / SQ(sigma) * cp0 / R / CS_star; }else if(0==strcmp(state.fluid->name,"nitrogen")){ // this uses a rather different approach/formulation; see http://ascend4.org/FPROPS/Thermal_conductivity MSG("lam0 for nitrogen"); double N1 = 1.511; double N2 = 2.117, t2 = -1.; double N3 = -3.332, t3 = -0.7; double tau = k1->T_star / state.T; lam0 = N1 * (visc1_mu0(state,err)/1e-6) + N2 * pow(tau,t2) + N3 * pow(tau,t3); }else{ ERRMSG("lam0 not implemented"); *err = FPROPS_NOT_IMPLEMENTED; return 0; } MSG("lam0(T=%f) = %e",state.T, lam0); return lam0 * k1->k_star; }