Last active
September 25, 2018 13:22
-
-
Save doleron/c19fe5a0b8582a5eeadc18811d93e321 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// inverse kinematics | |
// helper functions, calculates angle theta1 (for YZ-pane) | |
int delta_calcAngleYZ(float x0, float y0, float z0, float &theta) { | |
float y1 = -0.5 * 0.57735 * f; // f/2 * tg 30 | |
y0 -= 0.5 * 0.57735 * e; // shift center to edge | |
// z = a + b*y | |
float a = (x0*x0 + y0*y0 + z0*z0 +rf*rf - re*re - y1*y1)/(2*z0); | |
float b = (y1-y0)/z0; | |
// discriminant | |
float d = -(a+b*y1)*(a+b*y1)+rf*(b*b*rf+rf); | |
if (d < 0) return -1; // non-existing point | |
float yj = (y1 - a*b - sqrt(d))/(b*b + 1); // choosing outer point | |
float zj = a + b*yj; | |
theta = 180.0*atan(-zj/(y1 - yj))/pi + ((yj>y1)?180.0:0.0); | |
return 0; | |
} | |
// inverse kinematics: (x0, y0, z0) -> (theta1, theta2, theta3) | |
// returned status: 0=OK, -1=non-existing position | |
int delta_calcInverse(float x0, float y0, float z0, float &theta1, float &theta2, float &theta3) { | |
theta1 = theta2 = theta3 = 0; | |
int status = delta_calcAngleYZ(x0, y0, z0, theta1); | |
if (status == 0) status = delta_calcAngleYZ(x0*cos120 + y0*sin120, y0*cos120-x0*sin120, z0, theta2); // rotate coords to +120 deg | |
if (status == 0) status = delta_calcAngleYZ(x0*cos120 - y0*sin120, y0*cos120+x0*sin120, z0, theta3); // rotate coords to -120 deg | |
return status; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment