2直線の最近点を求める

f:id:startrekker:20170613223313p:plain

これも内積(Dot Product)を使う。

d1, d2を求めれば各直線上での最近点が求まる。
それら最近点が同一になったとき、2直線は交差したと言える。

ベクトルが直交する場合、内積が0になる(※1)ことを
利用するのだが、長くなるので以下コードのみ。

using System;
using System.Windows.Media.Media3D;

namespace Calc
{
    public static class Logic
    {
        /// <summary>
        /// 直線ABと直線CDの最近点を得る
        /// </summary>
        /// <param name="a">点A(直線ABの1点目)</param>
        /// <param name="b">点B(直線ABの2点目)</param>
        /// <param name="c">点C(直線CDの1点目)</param>
        /// <param name="d">点D(直線CDの2点目)</param>
        /// <param name="p">点P(直線AB上で最も直線CDに近づく点)</param>
        /// <param name="q">点Q(直線CD上で最も直線ABに近づく点)</param>
        /// <returns>0:計算不能, 1:2直線が交わる, 2:2直線が交わらない</returns>
        static public int GetNearestPoint(Point3D a, Point3D b, Point3D c, Point3D d, 
                                         out Point3D p, out Point3D q)
        {
            p = new Point3D(0, 0, 0);
            q = new Point3D(0, 0, 0);

            // 2直線でない場合、計算不能
            if (Point3D.Equals(a, b) || Point3D.Equals(c,d))
            {
                return 0;
            }

            Vector3D vAB = b - a;
            Vector3D vCD = d - c;

            Vector3D vAB_Normal = vAB;
            vAB_Normal.Normalize();
            Vector3D vCD_Normal = vCD;
            vCD_Normal.Normalize();

            double dotProductVal = Vector3D.DotProduct(vAB_Normal, vCD_Normal);
            double workVal = 1.0 - dotProductVal * dotProductVal;

            // 2直線が平行なら計算不能
            if (Math.Abs(workVal) <= 0.001)
            {
                return 0;
            }

            Vector3D vAC = c - a;

            double d1 = (Vector3D.DotProduct(vAC, vAB_Normal) - dotProductVal * 
                        Vector3D.DotProduct(vAC, vCD_Normal)) / workVal;
            double d2 = (dotProductVal * Vector3D.DotProduct(vAC, vAB_Normal) - 
                        Vector3D.DotProduct(vAC, vCD_Normal)) / workVal;

            p = new Point3D(a.X + d1 * vAB_Normal.X, a.Y + d1 * vAB_Normal.Y, 
                            a.Z + d1 * vAB_Normal.Z);
            q = new Point3D(c.X + d2 * vCD_Normal.X, c.Y + d2 * vCD_Normal.Y,
                            c.Z + d2 * vCD_Normal.Z);

            double dist = GetDistance(p, q);
            if (dist <= 0.001)
            {
                return 1;
            }
            else
            {
                return 2;
            }
        }

        /// <summary>
        /// 2点間の距離を得る
        /// </summary>
        /// <param name="p1">1点目</param>
        /// <param name="p2">2点目</param>
        /// <returns></returns>
        static public double GetDistance(Point3D p1, Point3D p2)
        {
            return (p2.X - p1.X) * (p2.X - p1.X) + (p2.Y - p1.Y) * (p2.Y - p1.Y) 
                    + (p2.Z - p1.Z) * (p2.Z - p1.Z);
        }
    }
}