Esempio n. 1
0
void move_leg(int leg_i, float x, float y, float z)
{
	current_pos[leg_i][0] = x;
	current_pos[leg_i][1] = y;
	current_pos[leg_i][2] = z;
	next_pos[leg_i][0] = x;
	next_pos[leg_i][1] = y;
	next_pos[leg_i][2] = z;

	float relative_x = x - leg_space[leg_i][0];
	float relative_y = y - leg_space[leg_i][1];

	if (leg_i >= 3) {
		relative_y = -relative_y;
		relative_x = -relative_x;
	}


	float theta = todegrees(atan2f(relative_y, relative_x));
	float theta2 = 90 + theta;

	float len = lawofcosines(relative_x, relative_y, toradians(90)) - FEMURA_LEN;
	action_servo(leg_i*3, theta2);
	move_leg_xz(leg_i, len, z);
}
Esempio n. 2
0
void move_leg_xz(int leg_i, float x, float z)
{
	float c = lawofcosines(z, x, toradians(90));
	float under_alpha = acosf(lawofcosines2(x, z, c));
	
	float alpha = acosf(lawofcosines2(TARSUS_LEN, c, 70));
	float beta = acosf(lawofcosines2(c, 70, TARSUS_LEN));

	if (isnan(alpha) || isnan(beta)) {
		return;
	}

	if (leg_i < 3) {
		action_servo(leg_i*3+1, 180-(todegrees(alpha+under_alpha)));
		action_servo(leg_i*3+2, todegrees(beta)+30-TARSUS_DEG);
	} else {
		action_servo(leg_i*3+1, todegrees(alpha+under_alpha));
		action_servo(leg_i*3+2, 180-(todegrees(beta)+30-TARSUS_DEG));
	}
}
Esempio n. 3
0
File: geo.c Progetto: mmase/wgrib2
double *gauss2lats(int nlat, double *ylat) {
  
  const double xlim = 1.0E-7;
  
  double *cosc  = (double *) malloc(sizeof(double) * (nlat + 1));
  double *sinc  = (double *) malloc(sizeof(double) * (nlat + 1));
  double *colat = (double *) malloc(sizeof(double) * (nlat + 1));
  
  int nzero = (nlat / 2);
  
  int i;
  double fi = nlat;
  double fi1 = fi + 1.0;
  double a = fi * fi1/sqrt(4.0*fi1*fi1 - 1.0);
  double b = fi1 * fi/sqrt(4.0*fi*fi - 1.0);

    double g, gm, gp, gt, delta, d;

  for (i = 1; i <= nzero; i++) {
    cosc[i] = sin((i - 0.5)*M_PI/nlat + M_PI*0.5);
  }
  
  for (i = 1; i <= nzero; i++) {
    g = gord(nlat, cosc[i]);
    gm = gord(nlat - 1, cosc[i]);
    gp = gord(nlat + 1, cosc[i]);
    gt = (cosc[i]*cosc[i] - 1.0)/(a * gp - b * gm);
    delta = g*gt;
    cosc[i] = cosc[i] - delta;
    
    while ( fabs(delta) > xlim ) {
      g = gord(nlat,cosc[i]);
      gm = gord(nlat - 1, cosc[i]);
      gp = gord(nlat + 1, cosc[i]);
      gt = (cosc[i]*cosc[i] - 1.0)/(a * gp - b * gm);
      delta = g*gt;
      cosc[i] = cosc[i] - delta;
      
    } /* end while */
    
  } /* end for */
  
  for (i = 1; i <= nzero; i++) {
    colat[i] = acos(cosc[i]);
    sinc[i] = sin(colat[i]);
  }
  
  /*
   * ... deal with equator if odd number of points
   */
  if ( ( nlat % 2) != 0 ) {
    i = nzero + 1;
    cosc[i] = 0.0;
    d = gord(nlat - 1, cosc[i]);
    d = d*d*fi*fi;
    colat[i] = M_PI * 0.5;
    sinc[i] = 1.0;
  } /* end if() */
  
  /*
   *  ... deal with southern hemisphere by symmetry
   */
  for (i = nlat - nzero + 1; i <= nlat; i++) {
    cosc[i]  = -cosc[nlat + 1 - i];
    colat[i] = M_PI - colat[nlat + 1 - i];
    sinc[i]  = sinc[nlat + 1 - i];
  } /* end for(i) */
  
  for (i = 1; i <= nlat; i++) {
    ylat[i-1] = todegrees(acos(sinc[i]));
    if ( i > (nlat / 2) ) ylat[i-1] = -ylat[i-1];
    /* change from N-S to S-N */
    ylat[i-1] = -ylat[i-1];
  }

  free(cosc);
  free(sinc);
  free(colat);
  
  return ylat;
  
} /* end gauss2lats() */
Esempio n. 4
0
File: geo.c Progetto: mmase/wgrib2
int lambert2ll(unsigned char **sec, double **llat, double **llon) {


    double n;
    double *lat, *lon;

    double dx, dy, lat1r, lon1r, lon2d, lon2r, latin1r, latin2r;
    double lond, latd, d_lon;
    double f, rho, rhoref, theta, startx, starty;
    int j, nnx, nny, nres, nscan;
    double x, y, tmp;
    unsigned char *gds;
    double latDr;
    double earth_radius;
    unsigned int nnpnts;

    get_nxny(sec, &nnx, &nny, &nnpnts, &nres, &nscan);

    if (nnx <= 0 || nny <= 0) {
        fprintf(stderr,"Sorry code does not handle variable nx/ny yet\n");
        return 0;
    }

    earth_radius = radius_earth(sec);
    gds = sec[3];
    dy      = GDS_Lambert_dy(gds);
    dx      = GDS_Lambert_dx(gds);
    lat1r   = GDS_Lambert_La1(gds) * (M_PI / 180.0);
    lon1r   = GDS_Lambert_Lo1(gds) * (M_PI / 180.0);
    lon2d   = GDS_Lambert_Lov(gds);
    lon2r   = lon2d * (M_PI / 180.0);
    latin1r = GDS_Lambert_Latin1(gds) * (M_PI/180.0);
    latin2r = GDS_Lambert_Latin2(gds) * (M_PI/180.0);

//  fix for theta start value crossing 0 longitude
//    if ((lon1r - lon2r) > 0) lon2r = lon2r + 2*M_PI;


//
// Latitude of "false origin" where scales are defined.
// It is used to estimate "reference_R", rhoref.
// Often latDr == latin1r == latin2r and non-modified code is true and works fine.
// But could be different if intersection latitudes latin1r and latin2r are different.
// Usually latDr must be latin1r <=  latDr <= latin2r, other could be strange.
//
    latDr = GDS_Lambert_LatD(gds) * (M_PI/180.0);


    if (lon1r < 0) fatal_error("bad GDS, lon1r < 0.0","");

    if ( fabs(latin1r - latin2r) < 1E-09 ) {
        n = sin(latin1r);
    }
    else {
        n = log(cos(latin1r)/cos(latin2r)) / 
        log(tan(M_PI_4 + latin2r/2.0) / tan(M_PI_4 + latin1r/2.0));
    }
  
    f = (cos(latin1r) * pow(tan(M_PI_4 + latin1r/2.0), n)) / n;
  
    rho = earth_radius * f * pow(tan(M_PI_4 + lat1r/2.0),-n);
    // old rhoref = earth_radius * f * pow(tan(M_PI_4 + latin1r/2.0),-n);
    rhoref = earth_radius * f * pow(tan(M_PI_4 + latDr/2.0),-n);

    // 2/2009 .. new code
    d_lon = lon1r - lon2r;
    if (d_lon > M_PI) d_lon -= 2*M_PI;
    if (d_lon < -M_PI) d_lon += 2*M_PI;
    theta = n * d_lon; 
    // 2/2009 theta = n * (lon1r - lon2r); 

    startx = rho * sin(theta);
    starty = rhoref - rho * cos(theta);

    if ((*llat = (double *) malloc(nnpnts * sizeof(double))) == NULL) {
        fatal_error("lambert2ll memory allocation failed","");
    }
    if ((*llon = (double *) malloc(nnpnts * sizeof(double))) == NULL) {
        fatal_error("lambert2ll memory allocation failed","");
    }
    lat = *llat;
    lon = *llon;

    /* put x[] and y[] values in lon[] and lat[] */
    if (stagger(sec, nnpnts, lon, lat)) fatal_error("geo: stagger problem","");

    dx = fabs(dx);
    dy = fabs(dy);

#pragma omp parallel for private(j,x,y,tmp,theta,rho,lond,latd)
    for (j = 0; j < nnpnts; j++) {
	y = starty + lat[j]*dy;
        x = startx + lon[j]*dx;
	tmp = rhoref - y;
	theta = atan(x / tmp);
        rho = sqrt(x * x + tmp*tmp);
        rho = n > 0 ? rho : -rho;
        lond = lon2d + todegrees(theta/n);
        latd = todegrees(2.0 * atan(pow(earth_radius * f/rho,1.0/n)) - M_PI_2);
	lond = lond >= 360.0 ? lond - 360.0 : lond;
	lond = lond < 0.0 ? lond + 360.0 : lond;
        lon[j] = lond;
        lat[j] = latd;
    }
    return 0;
} /* end lambert2ll() */