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);
}
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));
}
}
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() */
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() */
