1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121

/****
 * Copyright 2008 Monkey Wrench Software, Inc.
 *
 *   This program is free software: you can redistribute it and/or modify
 *   it under the terms of the GNU General Public License as published by
 *   the Free Software Foundation, either version 3 of the License, or
 *   (at your option) any later version.
 *
 *   This program is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *****/

using System;

namespace Mwsw.Geom {

  /**
   * A vector -- represents a magnitude (length) and a direction.
   */
  public struct Vector {
    private double m_x, m_y;
    public double X { get { return m_x; } }
    public double Y { get { return m_y; } }

    public Vector(double x, double y) { m_x = x; m_y = y; }

    /// The dot product of two vectors.
    public static double Dot(Vector a, Vector b) {
      return (a.X * b.X) + (a.Y * b.Y);
    }

    /// The length, squared.
    public double LengthSq { get { return Dot(this,this); } }
    
    /// The length.
    public double Length { get { return Math.Sqrt(LengthSq); } }

    /// A perpendicular: (easy in 2d)
    public Vector Perp { get { return new Vector(-m_y,m_x); } }

    /// Normalize to length one:
    public Vector Normalize() { 
      if (m_x == 0.0 && m_y == 0.0) // Avoid a divide-by-zero.
	return this;

      double len = Length;
      return new Vector(m_x/len,m_y/len);
    }

    /// Return which side of the line the given point lays on.
    ///  -1 for left; 0 for an intersect; 1 for positive.
    public int Side(Vector ov) {
      double val = Vector.Dot(ov, this.Perp);
      return Math.Sign(val);
    }

    /// Get the angle of the vector (in radians)
    public double Angle { get { return Math.Atan2(m_y,m_x); } } 

    /// Get the angle between two vectors (in radians)
    public static double AngleBetween(Vector a, Vector b) {
      double num = Dot(a,b);
      double den = a.Length * b.Length;
      return Math.Acos(num/den);
    }

    /// Rotate the vector by the given angle (in radians)
    public Vector Rotate(double theta) {
      double xamount = Math.Cos(theta);
      double yamount = Math.Sin(theta);
      return new Vector(m_x * xamount - m_y * yamount,
			m_x * yamount + m_y * xamount);
    }

    /// A parallel check w/ a tolerance. Tolerance for a specific angle theta
    ///  is expressed as cos(theta)^2. Tolerances >= 90 degrees are 
    public static bool AreParallel(Vector a, Vector b, double costol) {
      double dp = Dot(a,b); // (a.Length * b.Length * cos(theta))
      double dpsq = dp * dp; // (a.LengthSq * b.LengthSq * cos(theta)^2)

      // Consider the first quadrant only.
      //  Cos(theta) strictly decreases for 0 <= theta <= pi/2 as theta grows.
      //  Thus if theta_a <= theta_b, cos(theta_a) >= cos(theta_b)
      //                            , cos(theta_a)^2 >= cos(theta_b)^2
      //                            , x * cos(theta_a)^2 >= x * cos(theta_b)^2
      //                               (for x > 0)
      return (dpsq >= costol * a.LengthSq * b.LengthSq);
    }

    /// Helper for AreParallel: generate a tolerance value for a given angle
    ///  in radians:
    public static double GetParTolerance(double theta) {
      double v = Math.Cos(theta);
      return v*v;
    }

    public static Vector operator -(Vector a, Vector b) {
      return new Vector(a.X - b.X,a.Y-b.Y);
    }

    public static Vector operator +(Vector a, Vector b) {
      return new Vector(a.X + b.X,a.Y+b.Y);
    }

    public static Vector operator * (Vector a, double b) {
      return new Vector(a.X * b, a.Y * b);
    }
    public static Vector operator * (double a, Vector b) { return b*a; }
    

    public override string ToString() {
      return "[" + X + "," + Y + "]";
    }
  }

}