1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46
|
/**
* Converts pitch, roll, and heave to lengths a, b, and c
* @return 0 if OK, non-zero if failure.
* @param pitch the pitch angle
* @param roll the roll angle
* @param heave the distance off the floor to the center of the platform
* @param len an array of three floats that will store the lengths of the cables.
*/
__fastcall TFormJoystick::IKinetics(float pitch,float roll,float heave,float *len)
{
if(abs(pitch)>PI/2) return 1;
if(abs(roll)>PI/2) return 2;
CPoint p[3];
int i;
for(i=0;i<3;++i) {
// Find the roll component
// We do this first so that when we calculate the pitch the roll
// will still be relative to the platform, rather than the world.
p[i].x=platform[i].x;
p[i].y=platform[i].y * cos(roll) + platform[i].z * -sin(roll);
p[i].z=platform[i].y * sin(roll) + platform[i].z * cos(roll);
// Find the pitch component
p[i].x=platform[i].x * cos(pitch) + platform[i].z * -sin(pitch);
p[i].z=platform[i].x * sin(pitch) + platform[i].z * cos(pitch);
// Find the heave component
p[i].z+=heave;
if(p[i].z<0) return 3+i;
if(p[i].z>WINCH_HEIGHT) return 6+i;
// Get the lengths
float x = p[i].x - base[i].x;
float y = p[i].y - base[i].y;
float z = p[i].z - base[i].z;
len[i] = sqrt(x*x + y*y + z*z);
}
return 0;
}
// -------------------------------------------------------------------------- |
Partager