２直線の交点を求める

２直線の交点を求める方法とプログラミング例

２直線の交点を求める方法

線上最近点を求めて交点を導く

３次元上の２直線は、ねじれにより交点が無い場合があります。
そこで、計算としては交点を求めるのでなく線それぞれの線上最近点を求めることになります。
それぞれの線上最近点が同じ位置であれば交差しているとみなします。

図のように定義すると、線上最近点を求めるにはd1d2の長さを知ることが計算の鍵になります。

[ n1:ABの単位ベクトル　n2:CDの単位ベクトル　d1:AB上の最近点　d2:CD上の最近点 ]
[ d1:AとP1の距離　d2:CとP2の距離 ]

２直線の最短距離を結ぶ線分はどちらの直線に対しても直交

ベクトルが直交する場合は内積が0になるので、これを元に計算式を作ります。

※内積の展開、変形が分からない場合参考にしてください
n1・C - n1・A = n1・(C - A)
n1・d1*n1 = d1*(n1・n1)
n1・n1 = 1 ...(n1が単位ベクトルなので)

直交内積0をあらわす式です。

n1・(p2-p1) = 0 　...式A
n2・(p2-p1) = 0 　...式B

式ABを変形します。
p1とp2は次のように置き換えることができます。
p1 = A + d1*n1
p2 = C + d2*n2

p1p2を代入して...

n1・( C + d2*n2 ) - n1・( A + d1*n1 ) = 0 ...式A'
n2・( C + d2*n2 ) - n2・( A + d1*n1 ) = 0 ...式B'

さらに変形します

n1・(ACベクトル) + d2*(n1・n2) - d1 = 0 ...式A''
n2・(ACベクトル) - d1*(n1・n2) + d2 = 0 ...式B''

式A''と式B''を使って、d1d2を解きます..

d1 = ( n1・ACベクトル - (n1・n2)*( n2・ACベクトル ) ) / ( 1 - (n1・n2)*(n1・n2) )
d2 = ( (n1・n2)*( n1・ACベクトル ) - ( n2・ACベクトル ) ) / ( 1 - (n1・n2)*(n1・n2) )

d1d2が解ければp1p2の正確な座標値が分かります。

p1 = A + d1*n1
p2 = C + d2*n2

２線の交点を求めるプログラミング例

#include <math.h>

//ベクトルの定義
struct Vector3D{
    double x;
    double y;
    double z;
};
//頂点の定義(ベクトルと同じ)
#define Vertex3D Vector3D

//２点間の距離（ただしニ乗されてます）
double dist_vertex_np(const Vertex3D& v1, const Vertex3D& v2) {
    return ( v2.x - v1.x )*( v2.x - v1.x ) + ( v2.y - v1.y )*( v2.y - v1.y ) +( v2.z - v1.z )*( v2.z - v1.z );
}

//内積
double dot_product(const Vector3D& vl, const Vector3D& vr) {
    return vl.x * vr.x + vl.y * vr.y + vl.z * vr.z;
}

//ベクトルab　(b-a)を作成する
Vector3D create_vector( const Vector3D& a, const Vector3D& b ) {
    Vector3D ret;
    ret.x = b.x-a.x;
    ret.y = b.y-a.y;
    ret.z = b.z-a.z;
    return ret;
}

//単位ベクトルを作成する
Vector3D get_unit_vector( const Vector3D& v ) {
    //ベクトルの長さ
    double length = pow( ( v.x * v.x ) + ( v.y * v.y ) + ( v.z * v.z ), 0.5 );

    //XY各成分を長さで割る
    Vector3D unit;
    unit.x = v.x / length;
    unit.y = v.y / length;
    unit.z = v.z / length;

    return unit;
}

//AB CDで構成される２直線の交点(あるいは最近点)を求める
/*
    resultはVertex3D ２個の配列

    戻り値
    0 計算できず（平行であったりA=B C=Dのばあい）
    1 交点があった    resultに交点を格納
    2 交点がない　    resultには最近点を格納
*/
int intersect_lines( Vertex3D* result,
                     const Vertex3D& A, const Vertex3D& B, const Vertex3D& C, const Vertex3D& D )
{
    //A=B C=Dのときは計算できない
    if( dist_vertex_np( A,B ) == 0 || dist_vertex_np( C,D ) == 0 ) {
        return 0;
    }

    Vertex3D AB = create_vector(A,B);
    Vertex3D CD = create_vector(C,D);

    Vertex3D n1 = get_unit_vector(AB);
    Vertex3D n2 = get_unit_vector(CD);

    double work1 = dot_product( n1, n2 );
    double work2 = 1 - work1*work1;

    //直線が平行な場合は計算できない 平行だとwork2が0になる
    if( work2 == 0 ) { return 0; }

    Vertex3D AC = create_vector(A,C);

    double d1 = ( dot_product(AC, n1 ) - work1 * dot_product( AC, n2 ) ) / work2;
    double d2 = ( work1 * dot_product(AC, n1 ) - dot_product( AC, n2 ) ) / work2;

    //AB上の最近点
    result[0].x = A.x + d1 * n1.x;
    result[0].y = A.y + d1 * n1.y;
    result[0].z = A.z + d1 * n1.z;

    //BC上の最近点
    result[1].x = C.x + d2 * n2.x;
    result[1].y = C.y + d2 * n2.y;
    result[1].z = C.z + d2 * n2.z;

    //交差の判定 誤差は用途に合わせてください
    if( dist_vertex_np( result[0], result[1] ) < 0.000001 ) {
        //交差した
        return 1;
    }

    //交差しなかった。
    return 2;
}

戻る　　　次へ