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 undefined...
 198         if (mag == 0)
 199                 return Vector(0, 0, 0);
 200
 201         return Vector(x / mag, y / mag, z / mag);
 202 }
 203
 204
 205 double Vector::Magnitude(void)
 206 {
 207         return sqrt(x * x + y * y + z * z);
 208 }
 209
 210
 211 double Vector::Angle(void)
 212 {
 213         // acos returns a value between zero and PI, which means we don't know which
 214         // quadrant the angle is in... Though, if the y-coordinate of the vector is
 215         // negative, that means that the angle is in quadrants III - IV.
 216         double rawAngle = acos(Unit().x);
 217 #if 0
 218         double correctedAngle = (y < 0 ? (2.0 * PI) - rawAngle : rawAngle);
 219 #else
 220         double correctedAngle = (y < 0 ? TAU - rawAngle : rawAngle);
 221 #endif
 222
 223         return correctedAngle;
 224 }
 225
 226
 227 bool Vector::isZero(double epsilon/*= 1e-6*/)
 228 {
 229         return (fabs(x) < epsilon && fabs(y) < epsilon && fabs(z) < epsilon ? true : false);
 230 }
 231
 232
 233 // Class methods
 234
 235 /*static*/ double Vector::Dot(Vector v1, Vector v2)
 236 {
 237         return (v1.x * v2.x) + (v1.y * v2.y) + (v1.z * v2.z);
 238 }
 239
 240
 241 /*static*/ double Vector::Magnitude(Vector v1, Vector v2)
 242 {
 243         double xx = v1.x - v2.x;
 244         double yy = v1.y - v2.y;
 245         double zz = v1.z - v2.z;
 246         return sqrt((xx * xx) + (yy * yy) + (zz * zz));
 247 }
 248
 249
 250 //
 251 // Convenience function
 252 //
 253 /*static*/ double Vector::Angle(Point p1, Point p2)
 254 {
 255         return Vector(p1, p2).Angle();
 256 }
 257
 258
 259 // Returns the parameter of a point in space to this vector. If the parameter
 260 // is between 0 and 1, the normal of the vector to the point is on the vector.
 261 // Note: v1 is the tail, v2 is the head of the line (vector).
 262 /*static*/ double Vector::Parameter(Vector tail, Vector head, Vector p)
 263 {
 264         // Geometric interpretation:
 265         // The parameterized point on the vector lineSegment is where the normal of
 266         // the lineSegment to the point intersects lineSegment. If the pp < 0, then
 267         // the perpendicular lies beyond the 1st endpoint. If pp > 1, then the
 268         // perpendicular lies beyond the 2nd endpoint.
 269
 270         Vector lineSegment = head - tail;
 271         double magnitude = lineSegment.Magnitude();
 272         Vector pointSegment = p - tail;
 273         double t = lineSegment.Dot(pointSegment) / (magnitude * magnitude);
 274         return t;
 275 }
 276
 277
 278 // Return the 2D normal to the linesegment formed by the passed in points.
 279 // The normal thus calculated should rotate anti-clockwise.
 280 /*static*/ Vector Vector::Normal(Vector tail, Vector head)
 281 {
 282         Vector v = (head - tail).Unit();
 283         return Vector(-v.y, v.x);
 284 }
 285
 286
 287 /*static*/ double Vector::AngleBetween(Vector a, Vector b)
 288 {
 289         // This is done using the following formula:
 290         // (a . b) = ||a|| ||b|| cos(theta)
 291         // However, have to check for two degenerate cases, where a = cb:
 292         // 1, if c > 0, theta = 0; 2, if c < 0, theta = 180°.
 293         // Also, the vectors a & b have to be non-zero.
 294         // Also, have to check using an epsilon because acos will not return an
 295         // exact value if the vectors are orthogonal
 296         if (a.isZero() || b.isZero())
 297                 return 0;
 298
 299         return acos(a.Dot(b) / (a.Magnitude() * b.Magnitude()));
 300 }
 301