| 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; |