55,7 → 55,7 |
#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 LEG_CD_ANG 34.380 /* arctg(LEG_C/LEG_D) in gradi */ |
|
const float c0 = LEG_C * LEG_C; |
const float c1 = LEG_B * LEG_B; |
79,7 → 79,7 |
|
float pxz2 = px2 + pz2; |
|
float alfa1,beta1,alfa2,beta2,gamma1,gamma2; |
float alfa1,beta1,alfa2,beta2,gamma1; |
float m,dsqrt; |
|
float delta_xz = pxz2 - c0; |
88,15 → 88,15 |
if (delta_xz < 0.0) return -1; |
|
if (pz >= LEG_C) { |
gamma2 = acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2); |
gamma1 = gamma2 * todeg; |
gamma1 = acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2) * todeg; |
} else { |
gamma2 = -acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2); |
gamma1 = gamma2 * todeg; |
gamma1 = -acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2) * todeg; |
} |
|
m = pxz2 - LEG_CD_2IPO * (pz * sin(gamma2+LEG_CD_ANG) + px * cos(gamma2+LEG_CD_ANG) - LEG_CD_2IPO / 4.0); |
if (gamma1 < -90.0 || gamma1 > 90.0) return -1; |
|
m = pxz2 - LEG_CD_2IPO * (sin(gamma1+LEG_CD_ANG)+cos(gamma1+LEG_CD_ANG)); |
|
s = m + py2; |
k = c2 + s; |
k2 = k * k; |