void AxesLib::goToRads(float rx, float ry){ float degsH = (360.0 - _rad2deg(rx)); if(degsH >= 360.0) degsH = degsH - 360.0; _moveTo((float) degsH*_pgrad_x, (float) _rad2deg(ry)*_pgrad_y); }
static double _rts_sun_altitude(double latitude, double delta_prime, double h_prime) { double latitude_rad = _deg2rad(latitude); double delta_prime_rad = _deg2rad(delta_prime); return _rad2deg (asin (sin(latitude_rad)*sin(delta_prime_rad) + cos(latitude_rad)*cos(delta_prime_rad)*cos(_deg2rad(h_prime)))); }
static double _top_azimuth_angle_astro(double h_prime, double latitude, double delta_prime) { double h_prime_rad = _deg2rad(h_prime); double lat_rad = _deg2rad(latitude); return _wrap (_rad2deg (atan2(sin(h_prime_rad), cos(h_prime_rad)*sin(lat_rad) - tan(_deg2rad(delta_prime))*cos(lat_rad))), 0, 360); }
static double _top_elevation_angle (double latitude, double delta_prime, double h_prime) { double lat_rad = _deg2rad (latitude); double delta_prime_rad = _deg2rad (delta_prime); return _rad2deg (asin(sin(lat_rad)*sin(delta_prime_rad) + cos(lat_rad)*cos(delta_prime_rad) * cos(_deg2rad(h_prime)))); }
/*! * \brief * Calculate geocentric declination * \return [deg] */ static double _geo_declination(double beta, double epsilon, double lamda) { double beta_rad = _deg2rad (beta); double epsilon_rad = _deg2rad (epsilon); return _rad2deg (asin (sin(beta_rad)*cos(epsilon_rad) + cos(beta_rad)*sin(epsilon_rad)*sin(_deg2rad(lamda)))); }
static double _incidence_angle (spa_output_t so, spa_location_t sl) { double zenith_rad = _deg2rad (so.zenith); double slope_rad = _deg2rad (sl.slope); return _rad2deg(acos(cos(zenith_rad)*cos(slope_rad) + sin(slope_rad )*sin(zenith_rad) * cos(_deg2rad(so.azimuth_astro - sl.azm_rotation)))); }
/* * \brief * Calculate topocentric declination and right ascension parallax at once * \return [dec] */ static void _delta_alpha_prime(spa_location_t sl, double xi, double h, double delta, double *delta_alpha, double *delta_prime) { double delta_alpha_rad; double lat_rad = _deg2rad (sl.latitude); double xi_rad = _deg2rad (xi); double h_rad = _deg2rad (h); double delta_rad = _deg2rad (delta); double u = atan(0.99664719 * tan(lat_rad)); double y = 0.99664719 * sin(u) + sl.elevation*sin(lat_rad)/6378140.0; double x = cos(u) + sl.elevation*cos(lat_rad)/6378140.0; delta_alpha_rad = atan2( - x*sin(xi_rad) *sin(h_rad), cos(delta_rad) - x*sin(xi_rad) *cos(h_rad)); *delta_prime = _rad2deg (atan2((sin(delta_rad) - y*sin(xi_rad))*cos(delta_alpha_rad), cos(delta_rad) - x*sin(xi_rad) *cos(h_rad))); *delta_alpha = _rad2deg (delta_alpha_rad); }
/*! * \brief * Calculate geocentric right ascension * \return [deg] */ static double _geo_right_ascension(double lamda, double epsilon, double beta) { double lamda_rad = _deg2rad (lamda); double epsilon_rad = _deg2rad (epsilon); return _wrap (_rad2deg ( atan2( sin (lamda_rad)*cos (epsilon_rad) - tan (_deg2rad (beta))*sin (epsilon_rad), cos (lamda_rad))), 0, 360); }
/*! * \brief * Calculate Earth's heliocentric latitude * \return [deg] */ static double _earth_hel_latitude (double jme) { double sum[B_COUNT]; int i; for (i = 0; i < B_COUNT; i++) sum[i] = _periodic_term_sum (B_TERMS[i], b_subcount[i], jme); return _rad2deg (_earth_values (sum, B_COUNT, jme)); }
/*! * \brief * Calculate Earth's Heliocentric longitude * \return [deg] */ static double _earth_hel_longitude (double jme) { double sum[L_COUNT]; int i; for (i = 0; i < L_COUNT; i++) sum[i] = _periodic_term_sum (L_TERMS[i], l_subcount[i], jme); return _wrap (_rad2deg (_earth_values (sum, L_COUNT, jme)), 0, 360); }
static double _rts_hour_angle_at_rise_set (double latitude, double delta_zero, double h0_prime) { double h0 = -99999; double latitude_rad = _deg2rad (latitude); double delta_zero_rad = _deg2rad (delta_zero); double argument = (sin(_deg2rad(h0_prime)) - sin(latitude_rad)*sin(delta_zero_rad)) / (cos(latitude_rad)*cos(delta_zero_rad)); if (fabs(argument) <= 1) h0 = _wrap (_rad2deg(acos(argument)), 0, 180); return h0; }
/** * Calculate the distance between two geodetic location in miles. */ double shgeo_dist(shgeo_t *f_geo, shgeo_t *t_geo) { static const shnum_t mile_mod = 90.9; shnum_t theta, dist; shnum_t lat1, lat2; shnum_t lon1, lon2; shgeo_loc(f_geo, &lat1, &lon1, NULL); shgeo_loc(t_geo, &lat2, &lon2, NULL); theta = lon1 - lon2; dist = (sinl(_deg2rad(lat1)) * sinl(_deg2rad(lat2))) + (cosl(_deg2rad(lat1)) * cosl(_deg2rad(lat2)) * cosl(_deg2rad(theta))); dist = acosl(dist); dist = _rad2deg(dist); dist = dist * mile_mod; return ((double)dist); }
int main(int argc, char* argv[]) { int boardN = 0; int ret = 0; int rhoSize = 256; int thetaSize = 180; YARPImageOf<YarpPixelMono> hough_img; char c; printf("\n[eyeCalib] Setting up the sensor.."); ret = _grabber.initialize(boardN, _sizeX, _sizeY); if (ret == YARP_FAIL) { printf("[eyeCalib] ERROR in _grabber.initialize(). Quitting.\n"); exit (1); } initializeHead(); printf("\n[eyeCalib] Allocating images and filter (%d x %d)...", _sizeX, _sizeY); for (int i=0; i<N_IMAGES; i++) _imgBuffer[i].Resize (_sizeX, _sizeY); _susanImg.Resize (_sizeX, _sizeY); _susanFlt.resize(_sizeX, _sizeY); printf("\n[eyeCalib] Allocating Hough Filter (theta %d, rho %d)..", thetaSize, rhoSize); _houghFlt.resize(thetaSize, rhoSize); char fileName[255]; FILE *outFile = NULL; double orientation; double posVector[4]; double vel; printf("\n[eyeCalib] Do you want to save results to file ? [Y/n] "); c = getch(); if (c != 'n') { printf("\n[eyeCalib] File name ? "); scanf("%s", fileName); printf("[eyeCalib] Append data (Y/n)?"); c = getch(); if (c == 'n') { outFile = fopen(fileName,"w"); fprintf(outFile, "velocity;J0;J1;J2;J3;orientation\n"); } else { outFile = fopen(fileName,"a"); } if (!outFile) { printf("[eyeCalib] ERROR opening the file %s. Saving aboorted.\n", fileName); } } printf("\n[eyeCalib] Interactive calibration or Load data form file? [I/l] "); c = getch(); if ( c == 'l') { printf("\n[eyeCalib] File to load? "); scanf("%s", fileName); FILE *dataFile = NULL; dataFile = fopen(fileName,"r"); if (dataFile == NULL) { printf("[eyeCalib] ERROR opening the file %s. Quitting.\n", fileName); releaseSensor(); if (outFile != NULL) fclose(outFile); exit(1); } bool ret = false; while (readLine(dataFile, &vel, posVector) == true) { printf("\n[eyeCalib] Moving [%.2lf %.2lf %.2lf %.2lf * %.2lf]..", posVector[0], posVector[1], posVector[2], posVector[3], vel); printf("\n[eyeCalib] Hit any key when Position has been reached.."); moveHead(vel,posVector); c = getch(); orientation = calibrate(); printf("\n[eyeCalib] Main orientation is %.3frad (%.3fdeg) - variance %.2f", orientation, _rad2deg(orientation)); if (outFile != NULL) fprintf(outFile, "%lf;%lf;%lf;%lf;%lf;%lf\n", vel, posVector[0], posVector[1], posVector[2], posVector[3], orientation); } fclose(dataFile); } else { do { printf("\n[eyeCalib] Avaible Joints:\n\t0 - Left Pan\n\t1 - Left Tilt\n\t2 - Right Pan\n\t3 - Left Tilt"); printf("\n[EyeCalib] Move to? [J0;J1;J2;J3] "); scanf("%lf;%lf;%lf;%lf", &(posVector[0]), &(posVector[1]), &(posVector[2]), &(posVector[3])); printf("\n[eyeCalib] Move with velocity? "); scanf("%lf", &vel); printf("\n[eyeCalib] Moving [%.2lf %.2lf %.2lf %.2lf * %.2lf]..", posVector[0], posVector[1], posVector[2], posVector[3], vel); printf("\n[eyeCalib] Hit any key when Position has been reached.."); moveHead(vel, posVector); c = getch(); orientation = calibrate(); printf("\n[eyeCalib] Main orientation is %.3frad (%.3fdeg) - variance %.2f", orientation, _rad2deg(orientation)); if (outFile != NULL) fprintf(outFile, "%lf;%lf;%lf;%lf;%lf;%lf\n", vel, posVector[0], posVector[1], posVector[2], posVector[3], orientation); printf("\n[eyeCalib] Another loop ? [Y/n] "); c = getch(); } while (c != 'n'); } if (outFile != NULL) fclose(outFile); releaseSensor(); releaseHead(); printf("\n[eyeCalib] Bye.\n"); return 0; }