二次平面のベクトル演算(C#実装)

二次平面幾何の復習として、内積や外積、線の交点などを計算するコードを書いてみた。どこかで再利用するかもしれないのでクラス化してある。

参考にしたのは蟻本とこのサイト。 

まず、X・Y座標の保持と、座標用の演算オペレーターを用意する。

    /// <summary>
    /// Pont/Vector class for 2D 
    /// </summary>
    public class Pt
    {
        public double X;
        public double Y;

        public Pt(double x, double y)
        {
            X = x;
            Y = y;
        }

        public static Pt operator +(Pt v1, Pt v2)
        {
            return new Pt(DoubleUtil.Add(v1.X, v2.X), DoubleUtil.Add(v1.Y, v2.Y));
        }
        public static Pt operator -(Pt v1, Pt v2)
        {
            return new Pt(DoubleUtil.Add(v1.X, -v2.X), DoubleUtil.Add(v1.Y, -v2.Y));
        }
        public static Pt operator *(Pt v, double d)
        {
            return new Pt(v.X * d, v.Y * d);
        }
    }

※なおDoubleUtil.Add()は加算のDouble誤差対応版で、実装はこんな感じ。

    public class DoubleUtil
    {
        private static double EPS = 1e-10;

        public static double Add(double a, double b)
        {
            if (Math.Abs(a + b) < EPS * (Math.Abs(a) + Math.Abs(b))) return 0;
            return a + b;
        }

        public static bool Eq(double a, double b)
        {
            return Math.Abs(a - b) < 1e-9;
        }
    }

次に、各点を原点Oからのベクトルとしたときの内積(dot product)と外積(cross product)を求めるメソッドを作成する。

内積：a・b ＝ |a| b| cosθ ＝ ax bx + ay by

外積：a × b ＝ |a| |b| sinθ ＝ ax by - ay bx

        /// <summary>
        /// Dot product of two vectors: O -> this and O -> other
        ///  a dot b = |a| |b| cos(theta) = ax bx + ax by
        ///  zero if two vectors run orthogonally
        /// </summary>
        public double Dot(Pt other)
        {
            return DoubleUtil.Add(this.X * other.X, this.Y * other.Y);
        }

        /// <summary>
        /// Cross(det) product of two vectors: O -> this and O -> other
        ///  a x b = |a| |b| sin(theta) = ax by - ay bx
        ///  zero if two vectors run parallelly
        /// </summary>
        public double Cross(Pt other)
        {
            return DoubleUtil.Add(this.X * other.Y, -this.Y * other.X);
        }

次は点qが線分p1-p2上にあるか判定するメソッド。

①ベクトルq→p1とq→p2が平行である ＝ 外積が0になる

②ベクトルq→p1とq→p2が反対を向いている ＝ cosθが負になる ＝ 内積が負になる

③qがp1またはp2と一致 ＝ cosθが0になる ＝ 内積が0になる

の条件で判定できる。

        /// <summary>
        /// point q exists on line p1-p2?
        /// </summary>
        public static bool OnSeg(Pt p1, Pt p2, Pt q)
        {
            return (p1 - q).Cross(p2 - q) == 0 && (p1 - q).Dot(p2 - q) <= 0;
        }

最後に、2直線の交点を求めるメソッドを作成する。

直前p1-p2上の点xは、変数tを使ってp1 + t(p2 - p1)と表現できる。これが直線q1-q2にのるための条件は、ベクトルq1→xがq1-q2上にあればよいので、この外積が0になればよい。

(q2 - q1) × (x - q1) ＝ 0

これからtをもとめ、次の条件式が導き出される。

p1 × ( (q2 - q1) × (q1 - p1) / ( q2 - q1 ) × ( p2 - p1 ) ) ( p2 - p1 )

        /// <summary>
        /// crosssing point of line p1-p2 and q1-q2
        /// </summary>
        public static Pt Intersect(Pt p1, Pt p2, Pt q1, Pt q2)
        {
            return p1 + (p2 - p1) * ((q2 - q1).Cross(q1 - p1) / (q2 - q1).Cross(p2 - p1));
        }

ただし、これは点が線分上にある条件① しか使っていない。そのため交点の有無を判定するメソッドを別に用意する必要がある。

線分q1-q2とp1-p2が平行な場合とそれ以外に分けて、OnSeg()とIntersect()を使って、点の位置関係ごとに判定する。

        public static bool HasIntersect(Pt p1, Pt p2, Pt q1, Pt q2)
        {
            if ((p1 - q1).Cross(p2 - q2) == 0)
            {
                return OnSeg(p1, q1, p2) || OnSeg(p1, q1, q2) || OnSeg(p2, q2, p1) || OnSeg(p2, q2, q1);
            }
            else
            {
                var r = Intersect(p1, q1, p2, q2);
                return OnSeg(p1, q1, r) && OnSeg(p2, q2, r);
            }
        }

これが完成版。2点間の距離を返すメソッドなども追加した。

    /// <summary>
    /// Pont/Vector class for 2D 
    /// </summary>
    public class Pt
    {
        public double X;
        public double Y;

        public Pt(double x, double y)
        {
            X = x;
            Y = y;
        }

        public static Pt operator +(Pt v1, Pt v2)
        {
            return new Pt(DoubleUtil.Add(v1.X, v2.X), DoubleUtil.Add(v1.Y, v2.Y));
        }
        public static Pt operator -(Pt v1, Pt v2)
        {
            return new Pt(DoubleUtil.Add(v1.X, -v2.X), DoubleUtil.Add(v1.Y, -v2.Y));
        }
        public static Pt operator *(Pt v, double d)
        {
            return new Pt(v.X * d, v.Y * d);
        }

        /// <summary>
        /// Dot product of two vectors: O -> this and O -> other
        ///  a dot b = |a| |b| cos(theta) = ax bx + ax by
        ///  zero if two vectors run orthogonally
        /// </summary>
        public double Dot(Pt other)
        {
            return DoubleUtil.Add(this.X * other.X, this.Y * other.Y);
        }

        /// <summary>
        /// Cross(det) product of two vectors: O -> this and O -> other
        ///  a x b = |a| |b| sin(theta) = ax by - ay bx
        ///  zero if two vectors run parallelly
        /// </summary>
        public double Cross(Pt other)
        {
            return DoubleUtil.Add(this.X * other.Y, -this.Y * other.X);
        }

        /// <summary>
        /// point q exists on line p1-p2?
        /// </summary>
        public static bool OnSeg(Pt p1, Pt p2, Pt q)
        {
            return (p1 - q).Cross(p2 - q) == 0 && (p1 - q).Dot(p2 - q) <= 0;
        }

        /// <summary>
        /// crosssing point of line p1-p2 and q1-q2
        /// </summary>
        public static Pt Intersect(Pt p1, Pt p2, Pt q1, Pt q2)
        {
            return p1 + (p2 - p1) * ((q2 - q1).Cross(q1 - p1) / (q2 - q1).Cross(p2 - p1));
        }
        public static bool HasIntersect(Pt p1, Pt p2, Pt q1, Pt q2)
        {
            if ((p1 - q1).Cross(p2 - q2) == 0)
            {
                return OnSeg(p1, q1, p2) || OnSeg(p1, q1, q2) || OnSeg(p2, q2, p1) || OnSeg(p2, q2, q1);
            }
            else
            {
                var r = Intersect(p1, q1, p2, q2);
                return OnSeg(p1, q1, r) && OnSeg(p2, q2, r);
            }
        }

        public static bool operator ==(Pt x, Pt y)
        {
            return (DoubleUtil.Eq(x.X, y.X) && DoubleUtil.Eq(x.Y, y.Y));
        }

        public static bool operator !=(Pt x, Pt y)
        {
            return (!DoubleUtil.Eq(x.X, y.X) || !DoubleUtil.Eq(x.Y, y.Y));
        }
        public double Norm()
        {
            return Math.Sqrt(X * X + Y * Y);
        }
        public double Dist(Pt other)
        {
            return (this - other).Norm();
        }
        public static double Dist(Pt v1, Pt v2)
        {
            return v1.Dist(v2);
        }
    }