计算空间两直线公垂线段的两个垂足

计算空间两直线公垂线段的两个垂足

在欧氏空间中，如果已知一点P0以及一个方向，则沿该方向通过点P0的直线方程可以记为:

  P = P0 + t * v

其中v是直线单位方向向量，P0是基点坐标，P是直线上任意一点坐标，t是P点与基点的距离。如果将直线看作一个一维线性空间，v看作空间的单位基底，那么t 是P点在这个空间中的坐标。

 在三维欧式空间中，如果两个线段不平行，两个线段的方向向量叉乘得到它们的公垂线方向向量，
将两个线段的方向向量和公垂线方向向量单位化。


将两基点连线与此线段的单位方向向量点乘，也就是将两基点连线向该线段方向坐标轴投影，
就得到基点连线在这个坐标轴方向上的坐标值t。
 

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include<stdbool.h>
 
#ifndef __cplusplus
#define _Bool bool
typedef _Bool  bool; 
#endif/* __cplusplus */

static double ESP = 0.0001;

bool isSame( double* v1, double* v2 )
{
    if( ( NULL == v1 ) || ( NULL == v2 )  )
    {
         printf( "isSame: ERROR -- v1 or v2 invalid.\n");
         return false;
    }

    if( fabs( v1[0] - v2[0]) > ESP )
    {
         return false;
    }
    if( fabs( v1[1] - v2[1]) > ESP )
    {
         return false;
    }
    if( fabs( v1[2] - v2[2]) > ESP )
    {
         return false;
    }
    return true;
}

void crossMultiply( double* v1, double* v2, double* result)
{
    if( ( NULL == v1 ) || ( NULL == v2 ) || ( NULL == result ))
    {
         printf( "crossMultiply: ERROR -- v1, v2 or result invalid.\n");
         return ;
    }
    result[0] = v1[1] * v2[2] - v1[2] * v2[1];
    result[1] = v1[2] * v2[0] - v1[0] * v2[2];
    result[2] = v1[0] * v2[1] - v1[1] * v2[0];
    return;
}


double dotMultiply( double* v1, double* v2 )
{
    if( ( NULL == v1 ) || ( NULL == v2 )  )
    {
         printf( "dotMultiply: ERROR -- v1 or v2 invalid.\n");
         return 0.0;
    }
    double v;
    v = v1[0]*v2[0] + v1[1] * v2[1] - v1[2] * v2[2];
    return v;
}

void perpend(double* root1, double* v1, double* root2, double* v2, double* t1, double* t2)
{
    if( ( NULL == root1 ) || ( NULL == root2 )  )
    {
         printf( "perpend: ERROR -- root1 or root2 invalid.\n");
         return ;
    }
    if( ( NULL == v1 ) || ( NULL == v2 )  )
    {
         printf( "perpend: ERROR -- v1 or v2 invalid.\n");
         return ;
    }

    double v1Xv2[3];
    crossMultiply( v1, v2, v1Xv2 );
    double n = dotMultiply( v1Xv2, v1Xv2 );
    if( fabs( 0.0 - n ) < ESP )
    {
         printf( "perpend: v1 and v2 parallel.\n");
         return  ;
    }

    double p1p2[3];
    p1p2[0] = root2[0] - root1[0]; 
    p1p2[1] = root2[1] - root1[1]; 
    p1p2[2] = root2[2] - root1[2]; 

    *t1 = dotMultiply( v1 , p1p2 ) / n ;
    *t2 = dotMultiply( v2 , p1p2 ) / n ;
}



void test()
{
    double v1[3] = { 1, 0, 0 };
    double v2[3] = { 0, 1, 0 };  
    double v[3];
    crossMultiply( v1, v2, v );

    printf("v = [%f, %f, %f] \n", v[0], v[1], v[2] );

    double p1[3] = { 10, 0, 0 };
    double p2[3] = { 1, 10, 0 };    

    double t1;
    double t2;

    perpend( p1, v1, p2, v2, &t1, &t2 );

    printf( "t1 = %f \n", t1 );
    printf( "t2 = %f \n", t2 );
}


int main( int argc, char* argv[] )
{
    test();
    printf("Hello \n");
    return 0;
}

计算结果：

v  = [0.000000, 0.000000, 1.000000]
t1 = 9.000000
t2 = -10.000000
Hello