空间直线与球面的相交算法

关注

转载 2024-07-09 20:56:19 575 阅读 AI 写同款· GEO 优化 ›

【CV】3D空间中椭球曲面与直线的交点问题首先，在 3D 空间XYZXYZ\text{XYZ}坐标系中，椭球曲面的方程为 (x−Cx)2R2x+(y−Cy)2R2y+(z−Cz)2R2z=1(x−Cx)2Rx2+(y−Cy)2Ry2+(z−Cz)2Rz2=1\frac{(x-C_x)^2}{R_x^2} + \frac{(y-C_y)^2}{R_y^2} + \frac{(z-C_z)^2}{R_z^2} = 1,其中 (Cx,Cy,... 阅读详情

1. 原理推导

1.1. 直线公式

空间直线的数学定义是，已知直线L上一点M0(x0,y0,c0)，以及直线L的方向向量s(m,n,p)，那么空间直线L的方程为：

以上是空间直线的标准式方程（点向式方程）。令上面式子的比值为t，那么直线的参数式方程为：

对于知道线段的起点O和终点E，显然方向向量为D=E−O。这时，根据射线的向量方程，线段上某一点P为

转化为参数方程就是：

并且，采取这种公式描述还有个好处，参数t的取值范围为0到1，否则就在直线的两个端点之外。当t取0时，表四起点O；当t取1时，表示终点E;当t取(0,1)范围表示位于O、E端点之间的点。

1.2. 求交

根据数学定义，已知球心坐标C(Cx,Cy,Cz)与球的半径R，球面的公式为：

联立(1)(2)两式，最终会得到一个关于t的一元二次方程：

展开合并同类项，得出关于t的一元二次方程如下：

解这个一元二次方程，该方程有无解，单个解，以及双解三种可能，这也符合空间直线与球面相交的直观认识，要么相切有一个交点，要么相交有两个交点，否则的话没有交点。

2、代码实现

2.1 关键代码

/* 两个向量的差, (vector1 - vector2). */
vec3d vdiff(const vec3d vector1, const vec3d vector2)
{
    vec3d v;
    v.x = vector1.x - vector2.x;
    v.y = vector1.y - vector2.y;
    v.z = vector1.z - vector2.z;
    return v;
}

/* 两个向量的和, (vector1 + vector2). */
vec3d vsum(const vec3d vector1, const vec3d vector2)
{
    vec3d v;
    v.x = vector1.x + vector2.x;
    v.y = vector1.y + vector2.y;
    v.z = vector1.z + vector2.z;
    return v;
}

/* 向量的数乘1. */
vec3d vmul(const vec3d vector, const double n)
{
    vec3d v;
    v.x = vector.x * n;
    v.y = vector.y * n;
    v.z = vector.z * n;
    return v;
}

/* 向量的数乘2. */
vec3d vdiv(const vec3d vector, const double n)
{
    vec3d v;
    v.x = vector.x / n;
    v.y = vector.y / n;
    v.z = vector.z / n;
    return v;
}

/* 向量的欧几里得范数 */
double vdist(const vec3d v1, const vec3d v2)
{
    double xd = v1.x - v2.x;
    double yd = v1.y - v2.y;
    double zd = v1.z - v2.z;
    return sqrt(xd * xd + yd * yd + zd * zd);
}

/* 向量的欧几里得范数  */
double vnorm(const vec3d vector)
{
    return sqrt(vector.x * vector.x + vector.y * vector.y + vector.z * vector.z);
}

/* 两个向量的点积*/
double dot(const vec3d vector1, const vec3d vector2)
{
    return vector1.x * vector2.x + vector1.y * vector2.y + vector1.z * vector2.z;
}

/*两个向量的叉积 */
vec3d cross(const vec3d vector1, const vec3d vector2)
{
    vec3d v;
    v.x = vector1.y * vector2.z - vector1.z * vector2.y;
    v.y = vector1.z * vector2.x - vector1.x * vector2.z;
    v.z = vector1.x * vector2.y - vector1.y * vector2.x;
    return v;
}


/* Intersecting a sphere sc with radius of r, with a line p1-p2.
 * Return zero if successful, negative error otherwise.
 * mu1 & mu2 are constant to find points of intersection.
 * 球和直线的相交
 *
*/
int sphereline(const vec3d p1, const vec3d p2, const vec3d sc, double r, double *const mu1, double *const mu2)
{
   double a,b,c;
   double bb4ac;
   vec3d dp;

   dp.x = p2.x - p1.x;
   dp.y = p2.y - p1.y;
   dp.z = p2.z - p1.z;

   a = dp.x * dp.x + dp.y * dp.y + dp.z * dp.z;

   b = 2 * (dp.x * (p1.x - sc.x) + dp.y * (p1.y - sc.y) + dp.z * (p1.z - sc.z));

   c = sc.x * sc.x + sc.y * sc.y + sc.z * sc.z;
   c += p1.x * p1.x + p1.y * p1.y + p1.z * p1.z;
   c -= 2 * (sc.x * p1.x + sc.y * p1.y + sc.z * p1.z);
   c -= r * r;

   bb4ac = b * b - 4 * a * c;

   if (fabs(a) == 0 || bb4ac < 0) {
      *mu1 = 0;
      *mu2 = 0;
      return -1;
   }

   *mu1 = (-b + sqrt(bb4ac)) / (2 * a);
   *mu2 = (-b - sqrt(bb4ac)) / (2 * a);

   return 0;
}

sphereline函数代码解释：

1) p1相当于前文说的O点；p2相当于E点；dp相当于OE线段。

2) 根据1）我们知道p1.x = Ox(此时t = 0); p2.x = Ox + Dx(此时t = 1);则dp.x = p2.x - p1.x = Ox + Dx - Ox = Dx;同理：dp.y = Dy;dp.z = Dz,所以 a相当于前文公式(3)中t平方的系数。

3) b相当于公式(3)中t的系数。

4) c相当于公式(3)中的常数项。

5)之后就用一元二次方程的求根公式求线段dp和球的交点。解这个一元二次方程，该方程有无解，单个解，以及双解三种可能，这也符合空间直线与球面相交的直观认识，要么相切有一个交点，要么相交有两个交点，否则的话没有交点

2.2 main（）函数的调用

  int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);

    vec3d O;
    O.x = 30;
    O.y = 40;
    O.z = 70;

    vec3d E;
    E.x = 30;
    E.y = -40;
    E.z = 70;

    vec3d Center;
    Center.x = 0;
    Center.y = 0;
    Center.z = 70;


    double R = 50;
    double res1,res2;

    int  result = 1;
    result = sphereline(O,E,Center,R,&res1,&res2);

    qDebug()<<"求解得到的参数t为：";
    qDebug()<<"t1:"<< QString::number(res1,'g',5);
    qDebug()<<"t2:" <<QString::number(res2,'g',5);

    vec3d ex,t2,t3;
    double h;

    ex = vdiff(E, O); // vector result1-result2
    h = vnorm(ex); // scalar result1-result2
    ex = vdiv(ex, h); // unit vector ex with respect to result1 (new coordinate system)
    /* t2 points to the intersection */
    t2 = vmul(ex, res1*h);
    t2 = vsum(O, t2);

    t3 = vmul(ex, res2*h);
    t3 = vsum(O, t3);

    qDebug()<<"求解得到如下的交点:";
    qDebug()<< QString::number(t2.x,'g',5);
    qDebug()<< QString::number(t2.y,'g',5);
    qDebug()<< QString::number(t2.z,'g',5)<<endl;

    qDebug()<< QString::number(t3.x,'g',5);
    qDebug()<< QString::number(t3.y,'g',5);
    qDebug()<< QString::number(t3.z,'g',5)<<endl;


    return a.exec();
}

调试输出：