3次元の点と線分の距離

Question

前提

C言語のVisual studioで3次元の点と線分の距離を求めるプログラムを書いてます。ネットに合った2次元のプログラムを無理やり3次元にしてみました。
計算のやり方もいまいちわかってません。修正点などわかる方いたら情報が欲しいです。

実現したいこと

点と線分の距離の数値を的確に出したい。

発生している問題・エラーメッセージ

エラーは起こしていないが数値が正しくない
このプログラムでは（0，0，0）と（10，0，0）の線分と（5，10，15）との距離が13.1037と出ます。
2個目は6.29961と出るのですが、実際の計算では、1個目は18.028で2個目は18.7085になりました。

該当のソースコード

#include <iostream>

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

struct LineSegment
{
    Point start;
    Point end;
};

/**
   @brief 線分と点の距離を計算する。
   @param line 線分
   @param point 点
   @return ユークリッド距離
 */
double calc_dist(const LineSegment& line, const Point& point)
{
    double x0 = point.x, y0 = point.y, z0 = point.z;
    double x1 = line.start.x, y1 = line.start.y, z1 = line.start.z;
    double x2 = line.end.x, y2 = line.end.y, z2 = line.end.z;

    double a = x2 - x1;
    double b = y2 - y1;
    double c = z2 - z1;
    double a2 = a * a * a;
    double b2 = b * b * b;
    double c2 = c * c * c;
    double r2 = a2 + b2 + c2;
    double tt = -(a * (x1 - x0) + b * (y1 - y0) + c * (z1 - z0));

    if (tt < 0)
        return cbrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
    else if (tt > r2)
        return cbrt((x2 - x0) * (x2 - x0) + (y2 - y0) * (y2 - y0) + (z2 - z0) * (z2 - z0));

    double f1 = a * (y1 - y0) * (z1 - z0) - b * (x1 - x0) * (z1 - z0) - c * (x1 - x0) * (y1 - y0);
    return cbrt((f1 * f1) / r2);
}

int main()
{
    Point point1 = { 5, 10 ,15 };
    LineSegment line1 = { {0, 0, 0}, {10, 0, 0} };
    double dist = calc_dist(line1, point1);
    std::cout << "dist1: " << dist << std::endl;  


    Point point2 = { 20, 10, 5 };
    LineSegment line2 = { {0, 0, 0}, {10, 0, 0} };
    dist = calc_dist(line2, point2);
    std::cout << "dist2: " << dist << std::endl; 
}

試したこと

ここに問題に対して試したことを記載してください。

補足情報（FW/ツールのバージョンなど）

元の2次元のプログラムはこれです。
https://teratail.com/questions/155180

Comments

[actorbug]

元の「ネットに合った2次元のプログラム」のURLを載せてください。
わからないまま無理やり3次元にしたコードを見せられても、何をやっているのか分かりません。

[Von_1234]

すいません。こちらになります。

https://teratail.com/questions/155180

[actorbug]

元ソースでは外積を使っているようなので、「点と線分の距離 外積」あたりで検索すれば、解説など見つかるかと思います。

[fana]

> 計算のやり方もいまいちわかってません

数学の部分が解決してない状態なのであれば，その状態でてきとーなコードを書いてても仕方ない．
「距離の求め方」そのものが不明点だというなら「中学か高校レベルの数学の話を教えてください」っていうことになるから「プログラミングに関係ない話」ということになるような予感．

[fana]

ものすごくざっくりと説明するなら：
その点から線分（を無限に延長した直線）に対して垂線を下ろすことを考えればいい．
下ろした先が線分の範囲内にあるならば垂線の長さがそのまま求める距離ということになるし，
そうでないなら線分の端点と点との距離を求めればいい．

Answer 1

質問文の書き出しは

C言語の

とされているが，示されているコードはどう見てもC++なので，C++で回答．
（演算内容は関数 calc_dist のコードからわかると思うので説明は割愛する．）

• 線分のデータ表現は質問者のそれとは異なっている．（コード内 LineSeg 型の注釈を参照のこと）

//３次元のベクトル
struct Vec3
{
	double v[3];
	Vec3( double x=0, double y=0, double z=0 ) : v{x,y,z} {}
	//必要なoperatorだけを実装した
	Vec3 operator +( const Vec3 &rhs ) const {	return Vec3( v[0]+rhs[0], v[1]+rhs[1], v[2]+rhs[2] );	}
	Vec3 operator -( const Vec3 &rhs ) const {	return Vec3( v[0]-rhs[0], v[1]-rhs[1], v[2]-rhs[2] );	}
	Vec3 operator *( double s ) const {	return Vec3(s*v[0],s*v[1],s*v[2]);	}
	double operator[]( int i ) const {	return v[i];	}
};

//内積
inline double InnerProd( const Vec3 &A, const Vec3 &B )
{	return A[0]*B[0] + A[1]*B[1] + A[2]*B[2];	}

//L2ノルム
inline double L2Norm( const Vec3 &V )
{	return sqrt( InnerProd( V,V ) );	}

//線分．
//一方の端点をP0, 他方の端点は P0+U*t．
struct LineSeg
{
	Vec3 P0;	//一方の端点
	Vec3 U;	//線分に沿う（P0からもう一方の端点に向かう方向の）単位ベクトル
	double t;	//線分の長さ

	//他方の端点を得る
	Vec3 P1() const {	return P0 + U*t;	}
};

//件の計算
double calc_dist(const LineSeg &LS, const Vec3 &Point)
{
	Vec3 P = Point - LS.P0;
	double k = InnerProd( LS.U, P );

	if( k <= 0 ){	return L2Norm( P );	}
	if( k >= LS.t ){	return L2Norm( Point - LS.P1() );	}
	return L2Norm( P - LS.U*k );  //PointからLSに下した垂線の長さ
}

//main
int main( /*int argc, char *argv[]*/ )
{
	LineSeg LS;
	LS.P0 = Vec3{ 0,0,0 };
	LS.U = Vec3{ 1,0,0 };
	LS.t = 10;

	std::cout << calc_dist( LS, { 5, 10, 15 } ) << std::endl;	//18.0278
	std::cout << calc_dist( LS, { 20, 10, 5 } ) << std::endl;	//15
	
	return 0;
}