Мне даны координаты двух точек в 3D. точка плеча и точка объекта (до которой я должен дойти). Мне также дают длину от плеча до локтя и длину предплечья. Я пытаюсь найти неизвестную позицию (положение локтевого сустава). Я использую правило косинуса, чтобы узнать угол локтя. Вот мой код -
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
struct point {
double x, y, z;
};
struct angles {
double clock_wise;
double counter_clock_wise;
};
double max(double a, double b) {
return (a > b) ? a : b;
}
/*
* Check if the combination can make a triangle by considering the fact that sum
* of any two sides of a triangle is greater than the remaining side. The
* overlapping condition of links is handled separately in main().
*/
int valid_triangle(struct point p0, double l0, struct point p1, double l1) {
double dist = sqrt(pow((fabs(p1.z - p0.z)), 2) + pow((fabs(p1.y - p0.y)), 2) + pow((fabs(p1.x - p0.x)), 2));
if((max(dist, l0) == dist) && max(dist, l1) == dist) {
return (dist < (l0 + l1));
}
else if((max(dist, l0) == l0) && (max(l0, l1) == l0)) {
return (l0 < (dist + l1));
}
else {
return (l1 < (dist + l0));
}
}
/*
* Cosine rule is used to find the elbow angle. Positive value indicates a
* counter clockwise angle while negative value indicates a clockwise angle.
* Since this problem has at max 2 solutions for any given position of P0 and
* P1, I am returning a structure of angles which can be used to consider angles
* from both direction viz. clockwise-negative and counter-clockwise-positive
*/
void return_config(struct point p0, double l0, struct point p1, double l1, struct angles *a) {
double dist = sqrt(pow((fabs(p1.z - p0.z)), 2) + pow((fabs(p1.y - p0.y)), 2) + pow((fabs(p1.x - p0.x)), 2));
double degrees = (double) acos((l0 * l0 + l1 * l1 - dist * dist) / (2 * l0 * l1)) * (180.0f / 3.1415f);
a->clock_wise = -degrees;
a->counter_clock_wise = degrees;
}
int main() {
struct point p0, p1;
struct angles a;
p0.x = 15, p0.y = 4, p0.z = 0;
p1.x = 20, p1.y = 4, p1.z = 0;
double l0 = 5, l1 = 8;
if(valid_triangle(p0, l0, p1, l1)) {
printf("Three lengths can make a valid configuration \n");
return_config(p0, l0, p1, l1, &a);
printf("Angle of the elbow point (clockwise) = %lf, (counter clockwise) = %lf \n", a.clock_wise, a.counter_clock_wise);
}
else {
double dist = sqrt(pow((fabs(p1.z - p0.z)), 2) + pow((fabs(p1.y - p0.y)), 2) + pow((fabs(p1.x - p0.x)), 2));
if((dist <= (l0 + l1)) && (dist > l0)) {
a.clock_wise = -180.0f;
a.counter_clock_wise = 180.0f;
printf("Angle of the elbow point (clockwise) = %lf, (counter clockwise) = %lf \n", a.clock_wise, a.counter_clock_wise);
}
else if((dist <= fabs(l0 - l1)) && (dist < l0)){
a.clock_wise = -0.0f;
a.counter_clock_wise = 0.0f;
printf("Angle of the elbow point (clockwise) = %lf, (counter clockwise) = %lf \n", a.clock_wise, a.counter_clock_wise);
}
else
printf("Given combination cannot make a valid configuration\n");
}
return 0;
}
Однако это решение имеет смысл только в 2-D. Потому что по часовой стрелке и против часовой стрелки бессмысленно без оси и направления вращения. Возвращение только угла технически правильно, но клиенту этой функции требуется много работы, чтобы осмысленно использовать результат. Как я могу внести изменения, чтобы получить ось и направление вращения? Кроме того, я хочу знать, сколько возможных решений этой проблемы может быть.
Пожалуйста, дайте мне знать ваши мысли! Любая помощь высоко ценится ...