I am attaching the code for solution. P0 is considered as a shoulder point and P1 is considered as the point that lies in space (which I am supposed to grab given the length of my upper arm and forearm). Please comment on it and let me know what you think.
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
struct point {
double x, y, z;
};
/*
* Used to represent a vector in
* Xi + Yj + Zk format
*/
struct vector {
double i, j, k;
};
/*
* Used to represent a plane in
* Ax + By + Cz + D = 0 format
*/
struct plane {
double A, B, C, D;
};
/*
* Represents the final assembly of the configuration. When two spheres
* intersect they form a circle whose center is stored at "center" and radius is
* stored at "radius". The circle also has a support which is defined by a plane
* called as "radical plane" and its equation is stored at "p".
*/
struct configuration {
struct point center;
double radius;
struct plane p;
};
/*
* Conversion functions between vector and point
*/
struct vector get_vector_from_point(struct point p) {
static struct vector v;
v.i = p.x;
v.j = p.y;
v.k = p.z;
return v;
}
struct point get_point_from_vector(struct vector v) {
static struct point p;
p.x = v.i;
p.y = v.j;
p.z = v.k;
return p;
}
int check_if_same_points(struct point p1, struct point p2) {
return ((p1.x == p2.x) && (p1.y == p2.y) && (p1.z == p2.z));
}
/*
* Distance formula
*/
double distance(struct point p0, struct point p1) {
return sqrt(pow((fabs(p1.z - p0.z)), 2) + pow((fabs(p1.y - p0.y)), 2) + pow((fabs(p1.x - p0.x)), 2));
}
/*
* Customized utility functions used for vector mathematics
*/
double dot_product(struct vector p0, struct vector p1) {
return (p0.i * p1.i + p0.j * p1.j + p0.k * p1.k);
}
struct vector scale_vector(double scale, struct vector v) {
static struct vector scaled_vec;
scaled_vec.i = scale * v.i;
scaled_vec.j = scale * v.j;
scaled_vec.k = scale * v.k;
return scaled_vec;
}
struct vector add_vectors(struct vector v1, struct vector v2) {
static struct vector v;
v.i = v1.i + v2.i;
v.j = v1.j + v2.j;
v.k = v1.k + v2.k;
return v;
}
struct vector subtract_vectors(struct vector v1, struct vector v2) {
static struct vector v;
v.i = v1.i - v2.i;
v.j = v1.j - v2.j;
v.k = v1.k - v2.k;
return v;
}
/*
* Takes the given assembly of points and links. Returns object of configuration
* structure with necessary information. The center and radius from the returned
* structure can be used find possible locations of elbow.
* Client can use following parametric equation of circle in 3-D
* X(t) = C + r (cos(t) * U + sin(t) * V)
* where 0 <= t < 2 * pie, C is the center, r is the radius, U and V are unit
* normals to the plane such that if N is a unit length plane normal then
* {U,V,N} are mutually orthogonal.
*/
struct configuration return_config(struct point p0, double l0, struct point p1, double l1) {
struct vector p0_v = get_vector_from_point(p0);
struct vector p1_v = get_vector_from_point(p1);
double dot_prd_p0 = dot_product(p0_v, p0_v);
double dot_prd_p1 = dot_product(p1_v, p1_v);
struct vector sub_vec = subtract_vectors(p1_v, p0_v);
double D = ((l0 * l0) - (l1 * l1) + dot_prd_p1 - dot_prd_p0) / 2.0f;
static struct plane p;
p.A = sub_vec.i; p.B = sub_vec.j; p.C = sub_vec.k; p.D = D;
static struct configuration c;
/*
* Special case when object point and shoulder point are same.
*/
if(check_if_same_points(p0, p1)) {
printf("object and shoulder are at same location \n");
c.center.x = p0.x; c.center.y = p0.y; c.center.z = p0.z;
c.radius = l0;
c.p.A = c.p.B = c.p.C = c.p.D = 0.0f;
return c;
}
double a0 = (1.0f - (((l0 * l0) - (l1 * l1)) / (distance(p0, p1) * distance(p0, p1)))) / 2.0f;
struct vector lhs = scale_vector(a0,p0_v);
struct vector rhs = scale_vector(1.0f - a0, p1_v);
struct vector ans = add_vectors(lhs, rhs);
struct point center = get_point_from_vector(ans);
double radius = sqrt((l0 * l0) - (distance(center, p0) * distance(center, p0)));
c.center.x = center.x; c.center.y = center.y; c.center.z = center.z;
c.radius = radius;
c.p.A = p.A; c.p.B = p.B; c.p.C = p.C; c.p.D = D;
return c;
}
/*
* The logic is as follows - Point P0 generates a sphere of radius L0 around it,
* P1 generates another sphere of radius L1 around it. The intersection(if any)
* will be a circle with a plane. If I can return the center and radius of that
* circle and equation of the plane, then the client can find out any possible
* location of the elbow by varying the value of theta in the parametric
* equation of the circle. Thus if the spheres meet to generate a circle then
* there will be infinite elbow positions. If it does not generate a circle and
* meet "externally" then there will be only single elbow position. Otherwise
* there will be no solutions at all.
*/
int main() {
struct point p0, p1;
p0.x = 0, p0.y = 0, p0.z = 0;
p1.x = 50, p1.y = 50, p1.z = 0;
double l0 = 50, l1 = 50;
printf("Shoulder coordinates : (%lf, %lf, %lf) \n", p0.x, p0.y, p0.z);
printf("Object coordinates: (%lf, %lf, %lf) \n", p1.x, p1.y, p1.z);
printf("link0 = %lf, link1 = %lf \n", l0, l1);
if(distance(p0, p1) > (l0 + l1)) {
printf("The given combination of the points and links cannot make a valid configuration");
return -1;
}
struct configuration c = return_config(p0, l0, p1, l1);
printf("Center = (%lf, %lf, %lf), radius = %lf \n", c.center.x, c.center.y, c.center.z, c.radius);
printf("Equation of the radical plane = %lfA + (%lf)B + (%lf)C + %lf = 0 \n", c.p.A, c.p.B, c.p.C, c.p.D);
return 0;
}