0

3-D viz の 2 点の座標が与えられます。ショルダーポイントとオブジェクトポイント（到達するはずのポイント）。また、肩から肘までの長さと前腕の長さも与えられます。未知の位置(関節肘の位置)を解こうとしています。コサイン ルールを使用して肘の角度を調べています。ここに私のコードがあります -

#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 次元でのみ意味があります。時計回りと反時計回りは、軸と回転方向がなければ意味がないからです。角度のみを返すことは技術的には正しいですが、この関数のクライアントが結果を意味のある方法で使用するには多くの作業が必要になります。回転の軸と方向を取得するにはどうすればよいですか? また、この問題に対して考えられる解決策がいくつあるか知りたいです。

あなたの考えを教えてください ！どんな助けでも大歓迎です...

4

1 に答える 1

2

基本的な問題は、平面上の 2 つのアーム パーツ間の角度を (必然的に) 解決しているが、平面自体が指定されていないことです。解決策には、平面自体を拘束するか、エルボーの位置を「可能な限り低い位置」などの状態に拘束することが含まれます (これらの 2 つの解決策は同じになる可能性があります)。

したがって、平面を定義し、その平面上に 3 つの端点を配置します。

于 2010-05-07T02:02:00.913 に答える