Если заданы два отрезка и конечные точки, возвращаются конфигурации, в которых сегменты разделяют конечную точку

Я застрял в проблеме, где нам задают начальную позицию (P0) и длину (L0) одного отрезка, а также начальную позицию (P1) и длину (L1) другого отрезка, и мы возвращаем конфигурации, где оба сегмента заканчиваются в одной точке. Я кодирую функцию в C ++, которая возвращает одну допустимую конфигурацию для таких входных данных или указывает, что никакая конфигурация не является допустимой. Ниже приведены файлы, которые я сделал, которые используют библиотеки Eigen C ++.

SegmentConfigurations.cpp

 /* SegmentConfigurations.cpp : Defines a utility function that calculates the
* orientation for two input line segments such that they share a common
* endpoint, and the other endpoints are specified as inputs.
* Details on the implementation are described in the write-up document.
*/

#include "RotationMatrix.h"#include "SegmentConfigurations.h"using namespace std;
using namespace Eigen;

// calculate the square of the number "x"#define square(x) ((x)*(x))
// calculate the magnitude of the vector with coordinates (x,y,z)
#define magnitude(x,y,z) (sqrt((x)*(x) + (y)*(y) + (z)*(z)))

/* Returns a configuration such that the two input line segments share
* a common point (see previous explanation).
*
* Parameters:
* p0, p1: Two input points.
* l0: The length of the line segment that includes p0 as one endpoint.
* l1: The length of the line segment that includes p1 as one endpoint.
*     Function assumes l0 and l1 are both non-negative.
*
* Output:
* An Orientation structure containing two non-null 3x3 rotation matrices
* specifying the orientations for the line segments, or NULL if there is
* no configuration that allows the two line segments to share an endpoint.
*/
struct Orientations *segment_configurations(Vector3d p0, double l0,
Vector3d p1, double l1)
{
// vector pointing in the direction from p0 to p1
Vector3d vector_p0_to_p1 = p1 - p0;
// distance from p0 to 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 is p0 if points are the 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 from p0 to p1 so it has length d0
vector_p0_to_p1 = vector_p0_to_p1 * d0 / p0_p1_dist;
// final calculation to 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 is perpendicular to 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 to its 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])
{
// we know that 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])
{
// we know that 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 for point
}
// otherwise, use default value for elbow
/* Ensures that center_to_elbow has non-positive z-value and
* magnitude equal to "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());
// get point on circumference with lowest z-value; call this the "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} };
for (int i = 0; i < 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 is to see if rotation and translation matrices together reach p1 from 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 two 3x3 rotation matrices that orient the two line segments given as
* input to the 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 into 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 by 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 a 3x3 rotation matrix, representing a rotation in 3-dimensions
* from vector0 to vector1. The matrix is calculated using the equation
* in the Wikipedia article for "Rotation matrices" under the section
* titled "Rotation matrix from axis and angle" (link to article below).
* https://en.wikipedia.org/wiki/Rotation_matrix
*
* Parameters:
* vector0, vector1: The input vectors for which the 3-dimensional
*     rotation is calculated.
*
* Output:
* A 3x3 rotation matrix that represents how to rotate vector0
*     in order to 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 to both inputs
Vector3d u = vector0.cross(vector1);
if (!u.norm())
{
if (vector0 == vector1)
return Matrix3d::Identity();
// return rotation matrix that represents 180 degree rotation
Matrix3d m1;
m1 << -1, 0, 0,
0,-1, 0,
0, 0, 1;
return m1;
}

/* For the angle between both inputs:
* 1) The sine is the magnitude of their cross product.
* 2) The cosine equals their dot product.
*/
// sine must be 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 включает в себя основные функции для визуальной проверки правильности ориентации. Идея состоит в том, что функция «M3» представляет собой однородную матрицу преобразования из базового кадра в кадр конечного эффектора (источник которого — P1). Надежда состоит в том, что коэффициент смещения этой матрицы, добавленный к P0, приведет к P1, но это не всегда так, потому что «M3» не правильно, или, по крайней мере, я думаю. Я не уверен, что какая-то часть моей функции неверна или я ее тестирую, и это основная часть того, где я застрял. Любая помощь приветствуется.