Example #1
0
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);
}
Example #2
0
/**
 * 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, &timestamps, 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;
}
Example #4
0
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);
}
Example #6
0
/* 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;
}
Example #7
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() */
Example #10
0
/**
 * 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)) ;
				  }


			}
}
Example #11
0
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);
}
Example #12
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;
   }
}
Example #13
0
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 );
//        }

	}
}
Example #14
0
/**************************************************************************

  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))) ;
}
Example #15
0
/* 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));
}
Example #16
0
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
}
Example #17
0
/* 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;
}
Example #18
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;
}
Example #19
0
/* 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));
}
Example #20
0
//-----------------------------------------------------------------------------
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)));
}
Example #21
0
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);
						}
					}
				}
			}
		}
	}
}
Example #22
0
/**
 * 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, &timestamps, 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() */
Example #23
0
//---------------------------------------------------------
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;
}
Example #24
0
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:
        ;
    }
}
Example #25
0
 Vect(P &a, P &b) {
     p = a;  q = b;
     dist = SQ(a.X - b.X) + SQ(a.Y - b.Y);
 }
Example #26
0
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;

}
Example #30
0
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;
}