0,0 → 1,130 |
#include <stdio.h> |
#include <math.h> |
|
#define LEG_A 100.0 |
#define LEG_B 66.0 |
#define LEG_C 26.0 |
#define LEG_D 38.0 |
#define LEG_CD_2IPO 92.087 /* 2 * sqrt(LEG_C^2 + LEG_D^2) */ |
#define LEG_CD_ANG 0.600 /* arctg(LEG_C/LEG_D) in radianti */ |
|
#define PI 3.1415 |
|
const float c0 = LEG_C * LEG_C; |
const float c1 = LEG_B * LEG_B; |
const float c2 = LEG_B * LEG_B - LEG_A * LEG_A; |
const float todeg = 180.0 / PI; |
const float torad = PI / 180.0; |
|
int leg_to_ang(float px, float py, float pz, int *alfa, int *beta, int *gamma) |
{ |
float px2 = px * px; |
float py2 = py * py; |
float pz2 = pz * pz; |
|
float pxz2 = px2 + pz2; |
|
float alfa1,beta1,alfa2,beta2,gamma1,gamma2; |
float m,dsqrt; |
|
float delta_xz = pxz2 - c0; |
float s,k,k2,y1,delta_xy; |
|
if (delta_xz < 0.0) return -1; |
|
if (pz >= LEG_C) { |
gamma2 = acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2); |
gamma1 = gamma2 * todeg; |
} else { |
gamma2 = -acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2); |
gamma1 = gamma2 * todeg; |
} |
|
m = pxz2 - LEG_CD_2IPO * (px * cos(gamma2+LEG_CD_ANG) + pz * sin(gamma2+LEG_CD_ANG) - LEG_CD_2IPO/4); |
|
printf("M = %f\n",sqrt(m)); |
|
s = m + py2; |
k = c2 + s; |
k2 = k * k; |
|
delta_xy = py2 * k2 - s * (k2 - 4.0 * m * c1); |
|
if (delta_xy >= 0.0) { |
dsqrt = sqrt(delta_xy); |
y1 = (py * k + dsqrt) / (2.0 * s); |
beta1 = asin(y1/LEG_B) * todeg; |
alfa1 = asin((y1 - py)/LEG_A) * todeg + beta1; |
y1 = (py * k - dsqrt) / (2.0 * s); |
beta2 = asin(y1/LEG_B) * todeg; |
alfa2 = asin((y1 - py)/LEG_A) * todeg + beta2; |
|
if ((alfa1 >= 0.0 && alfa1 <= 180.0) && (beta1 >= -90.0 && beta1 <= 90.0)) { |
*alfa = (int)(alfa1 * 3600.0); |
*beta = (int)(beta1 * 3600.0); |
*gamma = (int)(gamma1 * 3600.0); |
return 0; |
} else if ((alfa2 >= 0.0 && alfa2 <= 180.0) && (beta2 >= -90.0 && beta2 <= 90.0)) { |
*alfa = (int)(alfa2 * 3600.0); |
*beta = (int)(beta2 * 3600.0); |
*gamma = (int)(gamma1 * 3600.0); |
return 0; |
} else { |
return -1; |
} |
} else |
return -1; |
|
return -1; |
|
} |
|
int ang_to_leg(int alfa, int beta, int gamma, float *px, float *py, float *pz) { |
|
float alfa1 = (float)(alfa)/3600.0 * torad; |
float beta1 = (float)(beta)/3600.0 * torad; |
float sin_gamma = sin((float)(gamma)/3600.0 * torad); |
float cos_gamma = cos((float)(gamma)/3600.0 * torad); |
float m; |
|
m = LEG_B * cos(beta1) + LEG_A * cos(alfa1 - beta1); |
*py = LEG_B * sin(beta1) - LEG_A * sin(alfa1 - beta1); |
|
*pz = (LEG_D + m) * sin_gamma + LEG_C * cos_gamma; |
*px = (LEG_D + m) * cos_gamma - LEG_C * sin_gamma; |
|
return 0; |
|
} |
|
int main() { |
|
float px,py,pz; |
int a,b,c; |
|
while(1) { |
|
printf("Insert PX: "); |
scanf("%f",&px); |
|
printf("Insert PY: "); |
scanf("%f",&py); |
|
printf("Insert PZ: "); |
scanf("%f",&pz); |
|
printf("PX: %3.3f PY: %3.3f PZ: %3.3f\n",px,py,pz); |
|
if (leg_to_ang(px,py,pz,&a,&b,&c)) |
printf("Error leg position\n"); |
|
ang_to_leg(a,b,c,&px,&py,&pz); |
|
printf(" A: %7d B: %7d C: %7d\n",a/3600,b/3600,c/3600); |
printf("PX: %.3f PY: %.3f PZ: %.3f\n",px,py,pz); |
|
} |
|
return 0; |
|
} |