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