c++ - Given Two line segments and endpoints, return configurations where segments share endpoint -

i'm stuck on problem we're given starting position (p0) , length (l0) of 1 line segment , starting position (p1) , length (l1) of line segment, , return configurations both segments end @ same point. i'm coding function in c++ returns 1 valid configuration such inputs, or indicates no configuration valid. below files made, utilize eigen c++ libraries.

segmentconfigurations.cpp

 /* segmentconfigurations.cpp : defines utility function calculates   * orientation 2 input line segments such share common   * endpoint, , other endpoints specified inputs.   * details on implementation described in write-up document.   */  #include "rotationmatrix.h" #include "segmentconfigurations.h" using namespace std; using namespace eigen;  // calculate square of number "x" #define square(x) ((x)*(x)) // calculate magnitude of vector coordinates (x,y,z) #define magnitude(x,y,z) (sqrt((x)*(x) + (y)*(y) + (z)*(z)))  /* returns configuration such 2 input line segments share  * common point (see previous explanation).  *  * parameters:  * p0, p1: 2 input points.  * l0: length of line segment includes p0 1 endpoint.  * l1: length of line segment includes p1 1 endpoint.  *     function assumes l0 , l1 both non-negative.  *  * output:  * orientation structure containing 2 non-null 3x3 rotation matrices  * specifying orientations line segments, or null if there  * no configuration allows 2 line segments share endpoint.  */ struct orientations *segment_configurations(vector3d p0, double l0,                                             vector3d p1, double l1) {     // vector pointing in direction p0 p1     vector3d vector_p0_to_p1 = p1 - p0;     // distance p0 p1     double p0_p1_dist = vector_p0_to_p1.norm();     if (l0 + l1 < p0_p1_dist || p0_p1_dist < fabs(l0 - l1))         return null; // no solution possible      struct orientations *result = (struct orientations*)calloc(1,                                    sizeof(struct orientations));      vector3d circle_center = p0; // center p0 if points same     if (!p0_p1_dist) // p0_p1_dist == 0     {         result->rotation1 = matrix3d::identity();         result->rotation2 = matrix3d::identity();         return result;     }     double d0 = (l0*l0 - l1*l1 + square(p0_p1_dist)) / (2 * p0_p1_dist);     double radius_of_circle = sqrt(l0*l0 - d0*d0);     // scale vector p0 p1 has length d0     vector_p0_to_p1 = vector_p0_to_p1 * d0 / p0_p1_dist;     // final calculation find center of circle     circle_center = vector_p0_to_p1 + circle_center;     printf("circle center: %f, %f, %f\n\n", circle_center[0], circle_center[1], circle_center[2]);     if (!radius_of_circle)     {         if (!l0) // l0 == 0         {             result->rotation1 = matrix3d::identity();             result->rotation2 = rotation_matrix(vector3d::unitx(), vector_p0_to_p1);             return result;         }         result->rotation1 = rotation_matrix(vector3d::unitx(), vector_p0_to_p1);         result->rotation2 = matrix3d::identity();         return result;     }      // note: vector_p0_to_p1 perpendicular circle      vector3d center_to_elbow(0, 0, -1);     if (vector_p0_to_p1[2])     {         vector3d center_to_point( 1, 0, 0 );         if (vector_p0_to_p1[0] && vector_p0_to_p1[1])         {             // ratio of y-component of circle normal vector x-component             double normal_y_x_ratio = vector_p0_to_p1[1] / vector_p0_to_p1[0];             center_to_point[0] = radius_of_circle*sqrt(1 / (square(normal_y_x_ratio) + 1));             center_to_point[1] = (-vector_p0_to_p1[0] * center_to_point[0]) / vector_p0_to_p1[1];             center_to_elbow = vector_p0_to_p1.cross(center_to_point);         }         else if (vector_p0_to_p1[0])         {             // know vector_p0_to_p1[1] == 0             center_to_point[0] = 0;             center_to_point[1] = 1;             center_to_elbow = vector_p0_to_p1.cross(center_to_point);         }         else if (vector_p0_to_p1[1])         {             // know vector_p0_to_p1[0] == 0             center_to_elbow = vector_p0_to_p1.cross(center_to_point);         }         else         {             center_to_elbow[0] = 1;             center_to_elbow[1] = 0;             center_to_elbow[2] = 0;         }         // otherwise, use default value point     }     // otherwise, use default value elbow     /* ensures center_to_elbow has non-positive z-value ,      * magnitude equal "radius".      */     if (center_to_elbow[2] > 0)         center_to_elbow = center_to_elbow * (-radius_of_circle / center_to_elbow.norm());     else         center_to_elbow = center_to_elbow * (radius_of_circle / center_to_elbow.norm());     // point on circumference lowest z-value; call "elbow"     printf("cte: %f, %f, %f\n\n", center_to_elbow[0], center_to_elbow[1], center_to_elbow[2]);          vector3d elbow_point = circle_center + center_to_elbow;         vector3d p0_to_elbow = elbow_point - p0;         vector3d elbow_to_p1 = p1 - elbow_point;         printf("elbow: %f, %f, %f\n\n", elbow_point[0], elbow_point[1], elbow_point[2]);         printf("p0te: %f, %f, %f\n\n", p0_to_elbow[0], p0_to_elbow[1], p0_to_elbow[2]);         printf("length: %f\n\n", p0_to_elbow.norm());         printf("etp1: %f, %f, %f\n\n", elbow_to_p1[0], elbow_to_p1[1], elbow_to_p1[2]);         printf("length: %f\n\n", elbow_to_p1.norm());          result->rotation1 = rotation_matrix(vector3d::unitx(), p0_to_elbow);         result->rotation2 = rotation_matrix(p0_to_elbow, elbow_to_p1);          return result;     }       int main()  {     double inputs[5][8] = { {1,5,2,3,4,5,6,4}, {1,2,5,3,4,6,5,4}, {2,1,2,5,15,1,2,12}, {5,-1,-7,15,-10,-9,-7,8}, {2,4,-1,7,-5,28,-1,24} };     (int = 0; < 2; i++)     {         double *values = inputs[i];         vector3d p0(values[0], values[1], values[2]);         vector3d p1(values[4], values[5], values[6]);         printf("point 1: %f,%f,%f,%f\n\n", values[0], values[1], values[2], values[3]);         printf("point 2: %f,%f,%f,%f\n\n", values[4], values[5], values[6], values[7]);         vector3d diff = p1 - p0;         printf("diff: %f,%f,%f\n\n", diff[0], diff[1], diff[2]);          struct orientations *output = segment_configurations(p0, values[3], p1, values[7]);         if (output != null)         {             matrix3d rotation1 = output->rotation1;             matrix3d rotation2 = output->rotation2;             printf("rotation 1:\n\n");             print_three_by_three(output->rotation1);             printf("rotation 2:\n\n");             print_three_by_three(output->rotation2);             matrix4d r_01 = rotation_to_homogeneous(output->rotation1);             matrix4d t_12 = translation(values[3], 0, 0);             matrix4d r_23 = rotation_to_homogeneous(output->rotation2);             matrix4d t_34 = translation(values[7], 0, 0);             printf("r01:\n\n");             print_four_by_four(r_01);             printf("t12:\n\n");             print_four_by_four(t_12);             printf("r23:\n\n");             print_four_by_four(r_23);             printf("t34:\n\n");             print_four_by_four(t_34);              // goal see if rotation , translation matrices reach p1 p0.             matrix4d m1 = r_01 * t_12;             matrix4d m2 = r_23 * m1;             matrix4d m3 = m2 * t_34;             printf("m1:\n\n");             print_four_by_four(m1);             printf("m2:\n\n");             print_four_by_four(m2);             printf("m3:\n\n");             print_four_by_four(m3);              vector3d m1_disp(m1(0, 3), m1(1, 3), m1(2, 3));             vector3d elbow = p0 + m1_disp;             printf("calc elbow:  %f,%f,%f\n\n", elbow[0], elbow[1], elbow[2]);             vector3d p0toelbow = elbow - p0;             printf("calc p0toe:  %f,%f,%f\n\n", p0toelbow[0], p0toelbow[1], p0toelbow[2]);             vector3d calc_elbowtop1 = output->rotation2 * p0toelbow;             printf("calc etop1:  %f,%f,%f\n\n", calc_elbowtop1[0], calc_elbowtop1[1], calc_elbowtop1[2]);              vector3d m3_disp(m3(0, 3), m3(1, 3), m3(2, 3));             vector3d endeffector = p0 + m3_disp;             printf("result:  %f,%f,%f\n", endeffector[0], endeffector[1], endeffector[2]);         }         else             printf("no answer!\n");         printf("----------------------------------------\n");     }     getchar();     return 0; } 

segmentconfigurations.h

// segmentconfigurations.h  #include <eigen\dense> using namespace eigen;  /* stores 2 3x3 rotation matrices orient 2 line segments given * input function "segment_configurations" (see previous explanation). */ struct orientations {     matrix3d rotation1;     matrix3d rotation2; }; 

rotationmatrix.h

#include <stdio.h> #include <eigen\dense> using namespace std; using namespace eigen;  void print_three_by_three(matrix3d m) {     printf("[ %f  %f  %f\n\n", m(0,0), m(0,1), m(0,2));     printf("  %f  %f  %f\n\n", m(1,0), m(1,1), m(1,2));     printf("  %f  %f  %f ]\n\n", m(2,0), m(2,1), m(2,2)); }  void print_four_by_four(matrix4d m) {     printf("[ %f  %f  %f  %f\n\n", m(0, 0), m(0, 1), m(0, 2), m(0,3));     printf("  %f  %f  %f  %f\n\n", m(1, 0), m(1, 1), m(1, 2), m(1,3));     printf("  %f  %f  %f  %f\n\n", m(2, 0), m(2, 1), m(2, 2), m(2,3));     printf("  %f  %f  %f  %f]\n\n",  m(3, 0), m(3, 1), m(3, 2), m(3,3)); }  // change 3x3 rotation matrix 4x4 homogeneous matrix representing same rotation matrix4d rotation_to_homogeneous(matrix3d m) {     matrix4d result;     result << m(0, 0), m(0, 1), m(0, 2), 0,               m(1, 0), m(1, 1), m(1, 2), 0,               m(2, 0), m(2, 1), m(2, 2), 0,               0, 0, 0, 1;     return result; }  // create 4x4 homogeneous matrix representing translation amount (x,y,z) matrix4d translation(double x, double y, double z) {     matrix4d result;     result << 1, 0, 0, x,               0, 1, 0, y,               0, 0, 1, z,               0, 0, 0, 1;     return result; }  /* return 3x3 rotation matrix, representing rotation in 3-dimensions * vector0 vector1. matrix calculated using equation * in wikipedia article "rotation matrices" under section * titled "rotation matrix axis , angle" (link article below). * https://en.wikipedia.org/wiki/rotation_matrix * * parameters: * vector0, vector1: input vectors 3-dimensional *     rotation calculated. * * output: * 3x3 rotation matrix represents how rotate vector0 *     in order produce vector1. */ matrix3d rotation_matrix(vector3d vector0, vector3d vector1) {     printf("in function\n");     printf("p0te: %f, %f, %f\n\n", vector0[0], vector0[1], vector0[2]);     printf("etp1: %f, %f, %f\n\n", vector1[0], vector1[1], vector1[2]);     vector0.normalize();     vector1.normalize();     printf("normalize\n");     printf("p0te: %f, %f, %f\n\n", vector0[0], vector0[1], vector0[2]);     printf("etp1: %f, %f, %f\n\n", vector1[0], vector1[1], vector1[2]);     // vector orthogonal both inputs     vector3d u = vector0.cross(vector1);     if (!u.norm())     {         if (vector0 == vector1)             return matrix3d::identity();         // return rotation matrix represents 180 degree rotation         matrix3d m1;         m1 << -1, 0, 0,                0,-1, 0,                0, 0, 1;         return m1;     }      /* angle between both inputs:      * 1) sine magnitude of cross product.      * 2) cosine equals dot product.      */     // sine must calculated using original cross product     printf("u: %f, %f, %f\n\n", u[0], u[1], u[2]);     double sine = u.norm();     double cosine = vector0.dot(vector1);     printf("sine: %f\n\n", sine);     printf("cosine: %f\n\n", cosine);     u.normalize();     printf("u (norm'd): %f, %f, %f\n\n", u[0], u[1], u[2]);      matrix3d tensor_product_matrix;     double ux = u[0];     double uy = u[1];     double uz = u[2];     tensor_product_matrix << ux*ux, ux*uy, ux*uz,                              ux*uy, uy*uy, uy*uz,                              ux*uz, uy*uz, uz*uz;     matrix3d cross_product_matrix;     cross_product_matrix << 0, -uz, uy,                             uz,   0,-ux,                            -uy,  ux,  0;       matrix3d part1 = matrix3d::identity();     matrix3d part2 = cross_product_matrix * sine;     matrix3d part3 = cross_product_matrix*cross_product_matrix * (1 - cosine);     return part1 + part2 + part3; } 

segmentconfigurations.cpp includes tests in main function visually checking orientations correct. idea function "m3" homogeneous transformation matrix base frame the frame of end-effector (whose origin p1). hope displacement factor of matrix added p0 result in p1, isn't case because "m3" isn't correct, or @ least think. i'm not sure if part of function wrong, or if i'm testing wrong, , main part of i'm stuck. welcomed.