在C中找到3D线上的点

时间:2014-04-10 20:30:31

标签: c linear-algebra linear

我有两个3D点:(x1,y1,z1)和(x3,y3,z3)并想在给定z2的行上找到一个点(x2,y2,z2),它位于z1和z3之间。

以下是我目前的情况:

#include<math.h>
#include<stdlib.h>
#include<stdio.h>

double *findPoint (double x1, double y1, double z1, double x3, double y3, double z3, double z2)
{
  double *ret = malloc(3 * sizeof(double));
  double dot = (x1 * x3) + (y1 * y3) + (z1 * z3);
  printf("dot %e\n", dot);
  double magprd = ((x1 * x1) + (y1 * y1) + (z1 * z1)) * ((x3 * x3) + (y3 * y3) + (z3 * z3));
  printf("magprd %e\n", magprd);
  double angle = acos(dot / magprd);
  printf("angle %e\n", angle);
  double distance = z2 / asin(angle);
  printf("distance %e\n", distance);
  double x2 = x1 - ((distance * x1) / 3);
  double y2 = y1 - ((distance * y1) / 3);
  ret[0] = x2;
  ret[1] = y2;
  ret[2] = z2;

  return ret;
}

int main() {
    // return pointer to array containing x2, y2, z2 corresponding to
    // z=4 on line between point at x1, y1, z1 and x3, y3, z3
    double *p = findPoint(1, 2, 3, 11, 12, 13, 4);

    if(p) {
        printf("point on line at z=4 is %e, %e, %e\n", p[0], p[1], p[2]);
        free(p);
    }

    return 0;
}

但这不能正常工作:

$ clang -lm test.c -o test
$ ./test 
dot 7.400000e+01
magprd 6.076000e+03
angle 1.558617e+00
distance nan
point on line at z=4 is nan, nan, 4.000000e+00

如何修复findPoint以解决此问题?谢谢!

2 个答案:

答案 0 :(得分:2)

你的数学看起来很复杂。我会建议这段代码:

typedef struct { double x,y,z; } Point;

Point findPoint(Point a, Point b, double z) {
    double dx = b.x - a.x;
    double dy = b.y - a.y;
    double dz = b.z - a.z;
    if(dz == 0.0) {
        // ERROR
        return (Point){ 0,0,0 };
    }
    double p = (z - a.z) / dz;
    return (Point){ a.x + p*dx, a.y + p*dy, a.z + p*dz };
}

如果你不想按价值使用我建议:

int findPoint(const Point* a, const Point* b, double z, Point* c) {
    double dx = b->x - a->x;
    double dy = b->y - a->y;
    double dz = b->z - a->z;
    if(dz == 0.0) {
        // ERROR
        return 0;
    }
    double p = (z - a.z) / dz;
    c->x = a.x + p*dx;
    c->y = a.y + p*dy;
    c->z = a.z + p*dz };
    return 1;
}

Point* a = malloc(sizeof(Point));
a->x = 1;
a->y = 2;
a->z = 3;
Point* b = malloc(sizeof(Point));
b->x = 11;
b->y = 12;
b->z = 13;
Point* c = malloc(sizeof(Point));
if(findPoint(a,b,4,c)) {
    printf("Result: %e %e %e\n", c->x, c->y, c->z);
}
else {
    printf("Error!\n");
}

答案 1 :(得分:1)

您可以将功能简化为

double *findPoint (double x1, double y1, double z1, double x3, double y3, double z3, double z2)
{
    double *ret = malloc(3 * sizeof(double));

    double tmp = (z2 - z1)/(z3 - z1); // Assuming that z1 != z3
    double x2 = x1 + tmp * (x3 - x1);
    double y2 = y1 + tmp * (y3 - y1);
    ret[0] = x2;
    ret[1] = y2;
    ret[2] = z2;

    return ret;
}

得出结果

point on line at z=4 is 2.000000e+00, 3.000000e+00, 4.000000e+00