Rev 1624 | Details | Compare with Previous | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1624 | giacomo | 1 | #include <stdio.h> |
| 2 | #include <math.h> |
||
| 3 | |||
| 4 | #define LEG_A 100.0 |
||
| 5 | #define LEG_B 66.0 |
||
| 6 | #define LEG_C 26.0 |
||
| 7 | #define LEG_D 38.0 |
||
| 8 | #define LEG_CD_2IPO 92.087 /* 2 * sqrt(LEG_C^2 + LEG_D^2) */ |
||
| 9 | #define LEG_CD_ANG 0.600 /* arctg(LEG_C/LEG_D) in radianti */ |
||
| 10 | |||
| 11 | #define PI 3.1415 |
||
| 12 | |||
| 13 | const float c0 = LEG_C * LEG_C; |
||
| 14 | const float c1 = LEG_B * LEG_B; |
||
| 15 | const float c2 = LEG_B * LEG_B - LEG_A * LEG_A; |
||
| 16 | const float todeg = 180.0 / PI; |
||
| 17 | const float torad = PI / 180.0; |
||
| 18 | |||
| 19 | int leg_to_ang(float px, float py, float pz, int *alfa, int *beta, int *gamma) |
||
| 20 | { |
||
| 21 | float px2 = px * px; |
||
| 22 | float py2 = py * py; |
||
| 23 | float pz2 = pz * pz; |
||
| 24 | |||
| 25 | float pxz2 = px2 + pz2; |
||
| 26 | |||
| 27 | float alfa1,beta1,alfa2,beta2,gamma1,gamma2; |
||
| 28 | float m,dsqrt; |
||
| 29 | |||
| 30 | float delta_xz = pxz2 - c0; |
||
| 31 | float s,k,k2,y1,delta_xy; |
||
| 32 | |||
| 33 | if (delta_xz < 0.0) return -1; |
||
| 34 | |||
| 35 | if (pz >= LEG_C) { |
||
| 36 | gamma2 = acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2); |
||
| 37 | gamma1 = gamma2 * todeg; |
||
| 38 | } else { |
||
| 39 | gamma2 = -acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2); |
||
| 40 | gamma1 = gamma2 * todeg; |
||
| 41 | } |
||
| 42 | |||
| 43 | m = pxz2 - LEG_CD_2IPO * (px * cos(gamma2+LEG_CD_ANG) + pz * sin(gamma2+LEG_CD_ANG) - LEG_CD_2IPO/4); |
||
| 44 | |||
| 45 | printf("M = %f\n",sqrt(m)); |
||
| 46 | |||
| 47 | s = m + py2; |
||
| 48 | k = c2 + s; |
||
| 49 | k2 = k * k; |
||
| 50 | |||
| 51 | delta_xy = py2 * k2 - s * (k2 - 4.0 * m * c1); |
||
| 52 | |||
| 53 | if (delta_xy >= 0.0) { |
||
| 54 | dsqrt = sqrt(delta_xy); |
||
| 55 | y1 = (py * k + dsqrt) / (2.0 * s); |
||
| 56 | beta1 = asin(y1/LEG_B) * todeg; |
||
| 57 | alfa1 = asin((y1 - py)/LEG_A) * todeg + beta1; |
||
| 58 | y1 = (py * k - dsqrt) / (2.0 * s); |
||
| 59 | beta2 = asin(y1/LEG_B) * todeg; |
||
| 60 | alfa2 = asin((y1 - py)/LEG_A) * todeg + beta2; |
||
| 61 | |||
| 62 | if ((alfa1 >= 0.0 && alfa1 <= 180.0) && (beta1 >= -90.0 && beta1 <= 90.0)) { |
||
| 63 | *alfa = (int)(alfa1 * 3600.0); |
||
| 64 | *beta = (int)(beta1 * 3600.0); |
||
| 65 | *gamma = (int)(gamma1 * 3600.0); |
||
| 66 | return 0; |
||
| 67 | } else if ((alfa2 >= 0.0 && alfa2 <= 180.0) && (beta2 >= -90.0 && beta2 <= 90.0)) { |
||
| 68 | *alfa = (int)(alfa2 * 3600.0); |
||
| 69 | *beta = (int)(beta2 * 3600.0); |
||
| 70 | *gamma = (int)(gamma1 * 3600.0); |
||
| 71 | return 0; |
||
| 72 | } else { |
||
| 73 | return -1; |
||
| 74 | } |
||
| 75 | } else |
||
| 76 | return -1; |
||
| 77 | |||
| 78 | return -1; |
||
| 79 | |||
| 80 | } |
||
| 81 | |||
| 82 | int ang_to_leg(int alfa, int beta, int gamma, float *px, float *py, float *pz) { |
||
| 83 | |||
| 84 | float alfa1 = (float)(alfa)/3600.0 * torad; |
||
| 85 | float beta1 = (float)(beta)/3600.0 * torad; |
||
| 86 | float sin_gamma = sin((float)(gamma)/3600.0 * torad); |
||
| 87 | float cos_gamma = cos((float)(gamma)/3600.0 * torad); |
||
| 88 | float m; |
||
| 89 | |||
| 90 | m = LEG_B * cos(beta1) + LEG_A * cos(alfa1 - beta1); |
||
| 91 | *py = LEG_B * sin(beta1) - LEG_A * sin(alfa1 - beta1); |
||
| 92 | |||
| 93 | *pz = (LEG_D + m) * sin_gamma + LEG_C * cos_gamma; |
||
| 94 | *px = (LEG_D + m) * cos_gamma - LEG_C * sin_gamma; |
||
| 95 | |||
| 96 | return 0; |
||
| 97 | |||
| 98 | } |
||
| 99 | |||
| 100 | int main() { |
||
| 101 | |||
| 102 | float px,py,pz; |
||
| 103 | int a,b,c; |
||
| 104 | |||
| 105 | while(1) { |
||
| 106 | |||
| 107 | printf("Insert PX: "); |
||
| 108 | scanf("%f",&px); |
||
| 109 | |||
| 110 | printf("Insert PY: "); |
||
| 111 | scanf("%f",&py); |
||
| 112 | |||
| 113 | printf("Insert PZ: "); |
||
| 114 | scanf("%f",&pz); |
||
| 115 | |||
| 116 | printf("PX: %3.3f PY: %3.3f PZ: %3.3f\n",px,py,pz); |
||
| 117 | |||
| 118 | if (leg_to_ang(px,py,pz,&a,&b,&c)) |
||
| 119 | printf("Error leg position\n"); |
||
| 120 | |||
| 121 | ang_to_leg(a,b,c,&px,&py,&pz); |
||
| 122 | |||
| 123 | printf(" A: %7d B: %7d C: %7d\n",a/3600,b/3600,c/3600); |
||
| 124 | printf("PX: %.3f PY: %.3f PZ: %.3f\n",px,py,pz); |
||
| 125 | |||
| 126 | } |
||
| 127 | |||
| 128 | return 0; |
||
| 129 | |||
| 130 | } |