Shamusworld >> Repos - architektonas/blob - src/vector.cpp

   1 //
   2 // vector.cpp: Various structures used for 3 dimensional imaging
   3 //
   4 // by James Hammons
   5 // (C) 2006 Underground Software
   6 //
   7 // JLH = James L. Hammons <jlhamm@acm.org>
   8 //
   9 // WHO  WHEN        WHAT
  10 // ---  ----------  ------------------------------------------------------------
  11 // JLH  09/19/2006  Created this file
  12 // JLH  03/22/2011  Moved implementation of constructor from header to here
  13 // JLH  04/02/2011  Fixed divide-by-zero bug in Unit(), added Angle() function
  14 // JLH  08/04/2013  Added Parameter() function
  15 //
  16
  17 #include "vector.h"
  18
  19 #include <math.h>                                                               // For sqrt()
  20 #include "mathconstants.h"
  21
  22 // Vector implementation
  23
  24 Vector::Vector(double xx/*= 0*/, double yy/*= 0*/, double zz/*= 0*/): x(xx), y(yy), z(zz)
  25 {
  26 }
  27
  28 Vector::Vector(Vector tail, Vector head): x(head.x - tail.x), y(head.y - tail.y), z(head.z - tail.z)
  29 {
  30 }
  31
  32 Vector::Vector(const Vector &v): x(v.x), y(v.y), z(v.z)
  33 {
  34 }
  35
  36 // Create vector from angle + length (2D; z is set to zero)
  37 void Vector::SetAngleAndLength(double angle, double length)
  38 {
  39         x = cos(angle) * length;
  40         y = sin(angle) * length;
  41         z = 0;
  42 }
  43
  44 Vector Vector::operator=(Vector const v)
  45 {
  46         x = v.x, y = v.y, z = v.z;
  47
  48         return *this;
  49 }
  50
  51 Vector Vector::operator+(Vector const v)
  52 {
  53         return Vector(x + v.x, y + v.y, z + v.z);
  54 }
  55
  56 Vector Vector::operator-(Vector const v)
  57 {
  58         return Vector(x - v.x, y - v.y, z - v.z);
  59 }
  60
  61 // Unary negation
  62
  63 Vector Vector::operator-(void)
  64 {
  65         return Vector(-x, -y, -z);
  66 }
  67
  68 // Vector x constant
  69
  70 Vector Vector::operator*(double const v)
  71 {
  72         return Vector(x * v, y * v, z * v);
  73 }
  74
  75 // Vector x constant
  76
  77 Vector Vector::operator*(float const v)
  78 {
  79         return Vector(x * v, y * v, z * v);
  80 }
  81
  82 // Vector / constant
  83
  84 Vector Vector::operator/(double const v)
  85 {
  86         return Vector(x / v, y / v, z / v);
  87 }
  88
  89 // Vector / constant
  90
  91 Vector Vector::operator/(float const v)
  92 {
  93         return Vector(x / v, y / v, z / v);
  94 }
  95
  96 // Vector (cross) product
  97
  98 Vector Vector::operator*(Vector const v)
  99 {
 100         // a x b = [a2b3 - a3b2, a3b1 - a1b3, a1b2 - a2b1]
 101         return Vector((y * v.z) - (z * v.y), (z * v.x) - (x * v.z), (x * v.y) - (y * v.x));
 102 }
 103
 104 // Dot product
 105
 106 double Vector::Dot(Vector const v)
 107 {
 108         return (x * v.x) + (y * v.y) + (z * v.z);
 109 }
 110
 111 // Vector x constant, self assigned
 112
 113 Vector& Vector::operator*=(double const v)
 114 {
 115         x *= v, y *= v, z *= v;
 116
 117         return *this;
 118 }
 119
 120 // Vector / constant, self assigned
 121
 122 Vector& Vector::operator/=(double const v)
 123 {
 124         x /= v, y /= v, z /= v;
 125
 126         return *this;
 127 }
 128
 129 // Vector + vector, self assigned
 130
 131 Vector& Vector::operator+=(Vector const v)
 132 {
 133         x += v.x, y += v.y, z += v.z;
 134
 135         return *this;
 136 }
 137
 138 // Vector + constant, self assigned
 139
 140 Vector& Vector::operator+=(double const v)
 141 {
 142         x += v, y += v, z += v;
 143
 144         return *this;
 145 }
 146
 147 // Vector - vector, self assigned
 148
 149 Vector& Vector::operator-=(Vector const v)
 150 {
 151         x -= v.x, y -= v.y, z -= v.z;
 152
 153         return *this;
 154 }
 155
 156 // Vector - constant, self assigned
 157
 158 Vector& Vector::operator-=(double const v)
 159 {
 160         x -= v, y -= v, z -= v;
 161
 162         return *this;
 163 }
 164
 165 // Check for equality
 166 bool Vector::operator==(Vector const v)
 167 {
 168         return (x == v.x && y == v.y && z == v.z ? true : false);
 169 }
 170
 171 // Check for inequality
 172 bool Vector::operator!=(Vector const v)
 173 {
 174         return (x != v.x || y != v.y || z != v.z ? true : false);
 175 }
 176
 177 Vector Vector::Unit(void)
 178 {
 179         double mag = Magnitude();
 180
 181         // If the magnitude of the vector is zero, then the Unit vector is
 182         // undefined...
 183         if (mag == 0)
 184                 return Vector(0, 0, 0);
 185
 186         return Vector(x / mag, y / mag, z / mag);
 187 }
 188
 189 double Vector::Magnitude(void)
 190 {
 191         return sqrt((x * x) + (y * y) + (z * z));
 192 }
 193
 194 double Vector::Angle(void)
 195 {
 196         // acos returns a value between zero and TAU/2, which means we don't know
 197         // which quadrant the angle is in... However, if the y-coordinate of the
 198         // vector is negative, that means that the angle is in quadrants III - IV.
 199         double rawAngle = acos(Unit().x);
 200         double correctedAngle = (y < 0 ? TAU - rawAngle : rawAngle);
 201
 202         return correctedAngle;
 203 }
 204
 205 bool Vector::isZero(double epsilon/*= 1e-6*/)
 206 {
 207         return (fabs(x) < epsilon && fabs(y) < epsilon && fabs(z) < epsilon ? true : false);
 208 }
 209
 210 // Class methods
 211
 212 /*static*/ double Vector::Dot(Vector v1, Vector v2)
 213 {
 214         return (v1.x * v2.x) + (v1.y * v2.y) + (v1.z * v2.z);
 215 }
 216
 217 /*static*/ double Vector::Magnitude(Vector v1, Vector v2)
 218 {
 219         double xx = v1.x - v2.x;
 220         double yy = v1.y - v2.y;
 221         double zz = v1.z - v2.z;
 222
 223         return sqrt((xx * xx) + (yy * yy) + (zz * zz));
 224 }
 225
 226 //
 227 // Convenience function
 228 //
 229 /*static*/ Vector Vector::Unit(Point p1, Point p2)
 230 {
 231         return Vector(p1, p2).Unit();
 232 }
 233
 234 //
 235 // Convenience function
 236 //
 237 /*static*/ double Vector::Angle(Point p1, Point p2)
 238 {
 239         return Vector(p1, p2).Angle();
 240 }
 241
 242 // Returns the parameter of a point in space to this vector. If the parameter
 243 // is between 0 and 1, the normal of the vector to the point is on the vector.
 244 // Note: v1 is the tail, v2 is the head of the line (vector).
 245 /*static*/ double Vector::Parameter(Vector tail, Vector head, Vector p)
 246 {
 247         // Geometric interpretation:
 248         // The parameterized point on the vector lineSegment is where the normal of
 249         // the lineSegment to the point intersects lineSegment. If the pp < 0, then
 250         // the perpendicular lies beyond the 1st endpoint. If pp > 1, then the
 251         // perpendicular lies beyond the 2nd endpoint.
 252
 253         Vector lineSegment = head - tail;
 254         double magnitude = lineSegment.Magnitude();
 255         Vector pointSegment = p - tail;
 256         double t = lineSegment.Dot(pointSegment) / (magnitude * magnitude);
 257
 258         return t;
 259 }
 260
 261 // Return the 2D normal to the linesegment formed by the passed in points.
 262 // The normal thus calculated should rotate anti-clockwise.
 263 /*static*/ Vector Vector::Normal(Vector tail, Vector head)
 264 {
 265         Vector v = (head - tail).Unit();
 266
 267         return Vector(-v.y, v.x);
 268 }
 269
 270 /*static*/ double Vector::AngleBetween(Vector a, Vector b)
 271 {
 272         // This is done using the following formula:
 273         // (a . b) = ||a|| ||b|| cos(theta)
 274         // However, have to check for two degenerate cases, where a = cb:
 275         // 1) if c > 0, theta = 0; 2) if c < 0, theta = 180°.
 276         // Also, the vectors a & b have to be non-zero.
 277         // Also, have to check using an epsilon because acos will not return an
 278         // exact value if the vectors are orthogonal.
 279         if (a.isZero() || b.isZero())
 280                 return 0;
 281
 282         return acos(a.Dot(b) / (a.Magnitude() * b.Magnitude()));
 283 }