Blame |
Last modification |
View Log
| RSS feed
#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;
}