2 // vector.cpp: Various structures used for 3 dimensional imaging
5 // (C) 2006 Underground Software
7 // JLH = James L. Hammons <jlhamm@acm.org>
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
19 #include <math.h> // For sqrt()
20 #include "mathconstants.h"
22 // Vector implementation
24 Vector::Vector(double xx/*= 0*/, double yy/*= 0*/, double zz/*= 0*/): x(xx), y(yy), z(zz)
28 Vector::Vector(Vector tail, Vector head): x(head.x - tail.x), y(head.y - tail.y), z(head.z - tail.z)
32 Vector::Vector(const Vector &v): x(v.x), y(v.y), z(v.z)
36 // Create vector from angle + length (2D; z is set to zero)
37 void Vector::SetAngleAndLength(double angle, double length)
39 x = cos(angle) * length;
40 y = sin(angle) * length;
44 Vector Vector::operator=(Vector const v)
46 x = v.x, y = v.y, z = v.z;
51 Vector Vector::operator+(Vector const v)
53 return Vector(x + v.x, y + v.y, z + v.z);
56 Vector Vector::operator-(Vector const v)
58 return Vector(x - v.x, y - v.y, z - v.z);
63 Vector Vector::operator-(void)
65 return Vector(-x, -y, -z);
70 Vector Vector::operator*(double const v)
72 return Vector(x * v, y * v, z * v);
77 Vector Vector::operator*(float const v)
79 return Vector(x * v, y * v, z * v);
84 Vector Vector::operator/(double const v)
86 return Vector(x / v, y / v, z / v);
91 Vector Vector::operator/(float const v)
93 return Vector(x / v, y / v, z / v);
96 // Vector (cross) product
98 Vector Vector::operator*(Vector const v)
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));
106 double Vector::Dot(Vector const v)
108 return (x * v.x) + (y * v.y) + (z * v.z);
111 // Vector x constant, self assigned
113 Vector& Vector::operator*=(double const v)
115 x *= v, y *= v, z *= v;
120 // Vector / constant, self assigned
122 Vector& Vector::operator/=(double const v)
124 x /= v, y /= v, z /= v;
129 // Vector + vector, self assigned
131 Vector& Vector::operator+=(Vector const v)
133 x += v.x, y += v.y, z += v.z;
138 // Vector + constant, self assigned
140 Vector& Vector::operator+=(double const v)
142 x += v, y += v, z += v;
147 // Vector - vector, self assigned
149 Vector& Vector::operator-=(Vector const v)
151 x -= v.x, y -= v.y, z -= v.z;
156 // Vector - constant, self assigned
158 Vector& Vector::operator-=(double const v)
160 x -= v, y -= v, z -= v;
165 // Check for equality
166 bool Vector::operator==(Vector const v)
168 return (x == v.x && y == v.y && z == v.z ? true : false);
171 // Check for inequality
172 bool Vector::operator!=(Vector const v)
174 return (x != v.x || y != v.y || z != v.z ? true : false);
177 Vector Vector::Unit(void)
179 double mag = Magnitude();
181 // If the magnitude of the vector is zero, then the Unit vector is
184 return Vector(0, 0, 0);
186 return Vector(x / mag, y / mag, z / mag);
189 double Vector::Magnitude(void)
191 return sqrt((x * x) + (y * y) + (z * z));
194 double Vector::Angle(void)
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);
202 return correctedAngle;
205 bool Vector::isZero(double epsilon/*= 1e-6*/)
207 return (fabs(x) < epsilon && fabs(y) < epsilon && fabs(z) < epsilon ? true : false);
212 /*static*/ double Vector::Dot(Vector v1, Vector v2)
214 return (v1.x * v2.x) + (v1.y * v2.y) + (v1.z * v2.z);
217 /*static*/ double Vector::Magnitude(Vector v1, Vector v2)
219 double xx = v1.x - v2.x;
220 double yy = v1.y - v2.y;
221 double zz = v1.z - v2.z;
223 return sqrt((xx * xx) + (yy * yy) + (zz * zz));
227 // Convenience function
229 /*static*/ double Vector::Angle(Point p1, Point p2)
231 return Vector(p1, p2).Angle();
234 // Returns the parameter of a point in space to this vector. If the parameter
235 // is between 0 and 1, the normal of the vector to the point is on the vector.
236 // Note: v1 is the tail, v2 is the head of the line (vector).
237 /*static*/ double Vector::Parameter(Vector tail, Vector head, Vector p)
239 // Geometric interpretation:
240 // The parameterized point on the vector lineSegment is where the normal of
241 // the lineSegment to the point intersects lineSegment. If the pp < 0, then
242 // the perpendicular lies beyond the 1st endpoint. If pp > 1, then the
243 // perpendicular lies beyond the 2nd endpoint.
245 Vector lineSegment = head - tail;
246 double magnitude = lineSegment.Magnitude();
247 Vector pointSegment = p - tail;
248 double t = lineSegment.Dot(pointSegment) / (magnitude * magnitude);
253 // Return the 2D normal to the linesegment formed by the passed in points.
254 // The normal thus calculated should rotate anti-clockwise.
255 /*static*/ Vector Vector::Normal(Vector tail, Vector head)
257 Vector v = (head - tail).Unit();
259 return Vector(-v.y, v.x);
262 /*static*/ double Vector::AngleBetween(Vector a, Vector b)
264 // This is done using the following formula:
265 // (a . b) = ||a|| ||b|| cos(theta)
266 // However, have to check for two degenerate cases, where a = cb:
267 // 1) if c > 0, theta = 0; 2) if c < 0, theta = 180°.
268 // Also, the vectors a & b have to be non-zero.
269 // Also, have to check using an epsilon because acos will not return an
270 // exact value if the vectors are orthogonal.
271 if (a.isZero() || b.isZero())
274 return acos(a.Dot(b) / (a.Magnitude() * b.Magnitude()));