Subversion Repositories shark

Rev

Details | 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
}