Diff from to

# File ik.c

`  * http://deepblue.lib.umich.edu/bitstream/handle/2027.42/6192/bac6709.0001.001.pdf?sequence=5`
`  */`
` #include <stdio.h>`
`+#include <stdlib.h>`
` #include <math.h>`
`+#include "ik.h"`
` `
` int`
` ik_first(`
` 	const double d4 = 225; // mm along second arm to center of wrist`
` 	const double a3 = 0; // mm between center of rotation of elbow?`
` `
`+	if (px*px + py*py < d2*d2)`
`+		return 0;`
`+`
` 	const double r = sqrt(px*px + py*py - d2*d2);`
` 	const double R = sqrt(px*px + py*py + pz*pz - d2*d2);`
` `
` 		const double sin_alpha = -pz / R;`
` 		const double cos_alpha = -right * r / R;`
` 		const double cos_beta = (a2*a2 + R*R - (d4*d4 + a3*a3)) / (2*a2*R);`
`+		if (cos_beta > 1 || cos_beta < -1)`
`+			return 0;`
`+`
` 		const double sin_beta = sqrt(1 - cos_beta*cos_beta);`
` 		const double sin_t2 = sin_alpha * cos_beta + right * above * cos_alpha * sin_beta;`
` 		const double cos_t2 = cos_alpha * cos_beta - right * above * sin_alpha * sin_beta;`
` 		const double t2 = d4*d4 + a3*a3;`
` 		const double t = sqrt(t2);`
` 		const double cos_phi = (a2*a2 + t2 - R*R) / (2 * a2 * t);`
`+		if (cos_phi > 1 || cos_phi < -1)`
`+			return 0;`
` 		const double sin_phi = right * above * sqrt(1 - cos_phi*cos_phi);`
` 		const double sin_beta = d4 / t;`
` 		const double cos_beta = fabs(a3) / t;`
` 		theta[2] = atan2(sin_t3, cos_t3);`
` 	}`
` `
`-	return 0;`
`+	return 1;`
` }`
` `
` `
`-#if 0`
`-int main(void)`
`+int main(int argc, char **argv)`
` {`
`-	const double xyz[3] = { 250, 0, 0 };`
`+	if (argc != 4)`
`+	{`
`+		fprintf(stderr, "need xyz args\n");`
`+		return -1;`
`+	}`
`+`
`+	const double xyz[3] = {`
`+		atof(argv[1]),`
`+		atof(argv[2]),`
`+		atof(argv[3])`
`+	};`
` 	double theta[6];`
` `
`-	ik_first(theta, xyz, 1, 1); // right, above`
`+	// right, above`
`+	if (!ik_first(theta, xyz, 1, 1))`
`+	{`
`+		fprintf(stderr, "[%f,%f,%f] unreachable\n", xyz[0], xyz[1], xyz[2]);`
`+		return -1;`
`+	}`
`+`
` 	for(int i = 0 ; i < 3 ; i++)`
` 		printf("%f\n", theta[i] * 180 / M_PI);`
` `
` 	return 0;`
` }`
`-#endif`