Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 1337 → Rev 1338

/demos/trunk/chimera/linux.c
0,0 → 1,130
#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;
 
}
/demos/trunk/chimera/send.c
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 34.380 /* arctg(LEG_C/LEG_D) in gradi */
#define LEG_CD_ANG 0.600 /* arctg(LEG_C/LEG_D) in radianti */
 
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;
float alfa1,beta1,alfa2,beta2,gamma1,gamma2;
float m,dsqrt;
 
float delta_xz = pxz2 - c0;
88,15 → 88,15
if (delta_xz < 0.0) return -1;
 
if (pz >= LEG_C) {
gamma1 = acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2) * todeg;
gamma2 = acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2);
gamma1 = gamma2 * todeg;
} else {
gamma1 = -acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2) * todeg;
gamma2 = -acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2);
gamma1 = gamma2 * todeg;
}
 
if (gamma1 < -90.0 || gamma1 > 90.0) return -1;
m = pxz2 - LEG_CD_2IPO * (pz * sin(gamma2+LEG_CD_ANG) + px * cos(gamma2+LEG_CD_ANG) - LEG_CD_2IPO / 4.0);
 
m = pxz2 - LEG_CD_2IPO * (sin(gamma1+LEG_CD_ANG)+cos(gamma1+LEG_CD_ANG));
 
s = m + py2;
k = c2 + s;
k2 = k * k;