WFMath  0.3.12
vector_funcs.h
1 // vector_funcs.h (Vector<> template functions)
2 //
3 // The WorldForge Project
4 // Copyright (C) 2001 The WorldForge Project
5 //
6 // This program is free software; you can redistribute it and/or modify
7 // it under the terms of the GNU General Public License as published by
8 // the Free Software Foundation; either version 2 of the License, or
9 // (at your option) any later version.
10 //
11 // This program is distributed in the hope that it will be useful,
12 // but WITHOUT ANY WARRANTY; without even the implied warranty of
13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 // GNU General Public License for more details.
15 //
16 // You should have received a copy of the GNU General Public License
17 // along with this program; if not, write to the Free Software
18 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
19 //
20 // For information about WorldForge and its authors, please contact
21 // the Worldforge Web Site at http://www.worldforge.org.
22 
23 // Author: Ron Steinke
24 // Created: 2001-12-7
25 
26 // Extensive amounts of this material come from the Vector2D
27 // and Vector3D classes from stage/math, written by Bryce W.
28 // Harrington, Kosh, and Jari Sundell (Rakshasa).
29 
30 #ifndef WFMATH_VECTOR_FUNCS_H
31 #define WFMATH_VECTOR_FUNCS_H
32 
33 #include <wfmath/vector.h>
34 #include <wfmath/rotmatrix.h>
35 #include <wfmath/zero.h>
36 
37 #include <cmath>
38 
39 #include <cassert>
40 
41 namespace WFMath {
42 
43 template<int dim>
44 Vector<dim>::Vector(const Vector<dim>& v) : m_valid(v.m_valid)
45 {
46  for(int i = 0; i < dim; ++i) {
47  m_elem[i] = v.m_elem[i];
48  }
49 }
50 
51 template<int dim>
52 Vector<dim>::Vector(const Point<dim>& p) : m_valid(p.isValid())
53 {
54  for(int i = 0; i < dim; ++i) {
55  m_elem[i] = p.elements()[i];
56  }
57 }
58 
59 template<int dim>
61 {
62  static ZeroPrimitive<Vector<dim> > zeroVector(dim);
63  return zeroVector.getShape();
64 }
65 
66 
67 template<int dim>
69 {
70  m_valid = v.m_valid;
71 
72  for(int i = 0; i < dim; ++i) {
73  m_elem[i] = v.m_elem[i];
74  }
75 
76  return *this;
77 }
78 
79 template<int dim>
80 bool Vector<dim>::isEqualTo(const Vector<dim>& v, double epsilon) const
81 {
82  double delta = _ScaleEpsilon(m_elem, v.m_elem, dim, epsilon);
83 
84  for(int i = 0; i < dim; ++i) {
85  if(fabs(m_elem[i] - v.m_elem[i]) > delta) {
86  return false;
87  }
88  }
89 
90  return true;
91 }
92 
93 template <int dim>
94 Vector<dim>& operator+=(Vector<dim>& v1, const Vector<dim>& v2)
95 {
96  v1.m_valid = v1.m_valid && v2.m_valid;
97 
98  for(int i = 0; i < dim; ++i) {
99  v1.m_elem[i] += v2.m_elem[i];
100  }
101 
102  return v1;
103 }
104 
105 template <int dim>
106 Vector<dim>& operator-=(Vector<dim>& v1, const Vector<dim>& v2)
107 {
108  v1.m_valid = v1.m_valid && v2.m_valid;
109 
110  for(int i = 0; i < dim; ++i) {
111  v1.m_elem[i] -= v2.m_elem[i];
112  }
113 
114  return v1;
115 }
116 
117 template <int dim>
118 Vector<dim>& operator*=(Vector<dim>& v, CoordType d)
119 {
120  for(int i = 0; i < dim; ++i) {
121  v.m_elem[i] *= d;
122  }
123 
124  return v;
125 }
126 
127 template <int dim>
128 Vector<dim>& operator/=(Vector<dim>& v, CoordType d)
129 {
130  for(int i = 0; i < dim; ++i) {
131  v.m_elem[i] /= d;
132  }
133 
134  return v;
135 }
136 
137 template <int dim>
138 Vector<dim> operator+(const Vector<dim>& v1, const Vector<dim>& v2)
139 {
140  Vector<dim> ans(v1);
141 
142  ans += v2;
143 
144  return ans;
145 }
146 
147 template <int dim>
148 Vector<dim> operator-(const Vector<dim>& v1, const Vector<dim>& v2)
149 {
150  Vector<dim> ans(v1);
151 
152  ans -= v2;
153 
154  return ans;
155 }
156 
157 template <int dim>
158 Vector<dim> operator*(const Vector<dim>& v, CoordType d)
159 {
160  Vector<dim> ans(v);
161 
162  ans *= d;
163 
164  return ans;
165 }
166 
167 template<int dim>
168 Vector<dim> operator*(CoordType d, const Vector<dim>& v)
169 {
170  Vector<dim> ans(v);
171 
172  ans *= d;
173 
174  return ans;
175 }
176 
177 template <int dim>
178 Vector<dim> operator/(const Vector<dim>& v, CoordType d)
179 {
180  Vector<dim> ans(v);
181 
182  ans /= d;
183 
184  return ans;
185 }
186 
187 template <int dim>
188 Vector<dim> operator-(const Vector<dim>& v)
189 {
190  Vector<dim> ans;
191 
192  ans.m_valid = v.m_valid;
193 
194  for(int i = 0; i < dim; ++i) {
195  ans.m_elem[i] = -v.m_elem[i];
196  }
197 
198  return ans;
199 }
200 
201 template<int dim>
203 {
204  CoordType mag = sloppyMag();
205 
206  assert("need nonzero length vector" && mag > norm / WFMATH_MAX);
207 
208  return (*this *= norm / mag);
209 }
210 
211 template<int dim>
213 {
214  m_valid = true;
215 
216  for(int i = 0; i < dim; ++i) {
217  m_elem[i] = 0;
218  }
219 
220  return *this;
221 }
222 
223 template<int dim>
224 CoordType Angle(const Vector<dim>& v, const Vector<dim>& u)
225 {
226  // Adding numbers with large magnitude differences can cause
227  // a loss of precision, but Dot() checks for this now
228 
229  CoordType dp = FloatClamp(Dot(u, v) / std::sqrt(u.sqrMag() * v.sqrMag()),
230  -1.0, 1.0);
231 
232  CoordType angle = std::acos(dp);
233 
234  return angle;
235 }
236 
237 template<int dim>
238 Vector<dim>& Vector<dim>::rotate(int axis1, int axis2, CoordType theta)
239 {
240  assert(axis1 >= 0 && axis2 >= 0 && axis1 < dim && axis2 < dim && axis1 != axis2);
241 
242  CoordType tmp1 = m_elem[axis1], tmp2 = m_elem[axis2];
243  CoordType stheta = std::sin(theta),
244  ctheta = std::cos(theta);
245 
246  m_elem[axis1] = tmp1 * ctheta - tmp2 * stheta;
247  m_elem[axis2] = tmp2 * ctheta + tmp1 * stheta;
248 
249  return *this;
250 }
251 
252 template<int dim>
254  CoordType theta)
255 {
256  RotMatrix<dim> m;
257  return operator=(Prod(*this, m.rotation(v1, v2, theta)));
258 }
259 
260 template<int dim>
262 {
263  return *this = Prod(*this, m);
264 }
265 
266 template<> Vector<3>& Vector<3>::rotate(const Vector<3>& axis, CoordType theta);
267 template<> Vector<3>& Vector<3>::rotate(const Quaternion& q);
268 
269 template<int dim>
270 CoordType Dot(const Vector<dim>& v1, const Vector<dim>& v2)
271 {
272  double delta = _ScaleEpsilon(v1.m_elem, v2.m_elem, dim);
273 
274  CoordType ans = 0;
275 
276  for(int i = 0; i < dim; ++i) {
277  ans += v1.m_elem[i] * v2.m_elem[i];
278  }
279 
280  return (fabs(ans) >= delta) ? ans : 0;
281 }
282 
283 template<int dim>
285 {
286  CoordType ans = 0;
287 
288  for(int i = 0; i < dim; ++i) {
289  // all terms > 0, no loss of precision through cancelation
290  ans += m_elem[i] * m_elem[i];
291  }
292 
293  return ans;
294 }
295 
296 template<int dim>
297 bool Parallel(const Vector<dim>& v1, const Vector<dim>& v2, bool& same_dir)
298 {
299  CoordType dot = Dot(v1, v2);
300 
301  same_dir = (dot > 0);
302 
303  return Equal(dot * dot, v1.sqrMag() * v2.sqrMag());
304 }
305 
306 template<int dim>
307 bool Parallel(const Vector<dim>& v1, const Vector<dim>& v2)
308 {
309  bool same_dir;
310 
311  return Parallel(v1, v2, same_dir);
312 }
313 
314 template<int dim>
315 bool Perpendicular(const Vector<dim>& v1, const Vector<dim>& v2)
316 {
317  double max1 = 0, max2 = 0;
318 
319  for(int i = 0; i < dim; ++i) {
320  double val1 = fabs(v1[i]), val2 = fabs(v2[i]);
321  if(val1 > max1) {
322  max1 = val1;
323  }
324  if(val2 > max2) {
325  max2 = val2;
326  }
327  }
328 
329  // Need to scale by both, since Dot(v1, v2) goes like the product of the magnitudes
330  int exp1, exp2;
331  (void) frexp(max1, &exp1);
332  (void) frexp(max2, &exp2);
333 
334  return fabs(Dot(v1, v2)) < ldexp(WFMATH_EPSILON, exp1 + exp2);
335 }
336 
337 template<>
339 {
340  return (CoordType) 1;
341 }
342 
343 template<>
345 {
346  return (CoordType) 1.082392200292393968799446410733;
347 }
348 
349 template<>
350 const CoordType Vector<3>::sloppyMagMax()
351 {
352  return (CoordType) 1.145934719303161490541433900265;
353 }
354 
355 template<>
356 const CoordType Vector<1>::sloppyMagMaxSqrt()
357 {
358  return (CoordType) 1;
359 }
360 
361 template<>
362 const CoordType Vector<2>::sloppyMagMaxSqrt()
363 {
364  return (CoordType) 1.040380795811030899095785063701;
365 }
366 
367 template<>
368 const CoordType Vector<3>::sloppyMagMaxSqrt()
369 {
370  return (CoordType) 1.070483404496847625250328653179;
371 }
372 
373 // Note for people trying to compute the above numbers
374 // more accurately:
375 
376 // The worst value for dim == 2 occurs when the ratio of the components
377 // of the vector is sqrt(2) - 1. The value above is equal to sqrt(4 - 2 * sqrt(2)).
378 
379 // The worst value for dim == 3 occurs when the two smaller components
380 // are equal, and their ratio with the large component is the
381 // (unique, real) solution to the equation q x^3 + (q-1) x + p == 0,
382 // where p = sqrt(2) - 1, and q = sqrt(3) + 1 - 2 * sqrt(2).
383 // Running the script bc_sloppy_mag_3 provided with the WFMath source
384 // will calculate the above number.
385 
386 template<> Vector<2>& Vector<2>::polar(CoordType r, CoordType theta);
387 template<> void Vector<2>::asPolar(CoordType& r, CoordType& theta) const;
388 
389 template<> Vector<3>& Vector<3>::polar(CoordType r, CoordType theta,
390  CoordType z);
391 template<> void Vector<3>::asPolar(CoordType& r, CoordType& theta,
392  CoordType& z) const;
393 template<> Vector<3>& Vector<3>::spherical(CoordType r, CoordType theta,
394  CoordType phi);
395 template<> void Vector<3>::asSpherical(CoordType& r, CoordType& theta,
396  CoordType& phi) const;
397 
398 template<> CoordType Vector<2>::sloppyMag() const;
399 template<> CoordType Vector<3>::sloppyMag() const;
400 
401 template<> CoordType Vector<1>::sloppyMag() const
402  {return std::fabs(m_elem[0]);}
403 
404 template<> Vector<2>::Vector(CoordType x, CoordType y) : m_valid(true)
405  {m_elem[0] = x; m_elem[1] = y;}
406 template<> Vector<3>::Vector(CoordType x, CoordType y, CoordType z) : m_valid(true)
407  {m_elem[0] = x; m_elem[1] = y; m_elem[2] = z;}
408 
409 template<> Vector<2>& Vector<2>::rotate(CoordType theta)
410  {return rotate(0, 1, theta);}
411 
412 template<> Vector<3>& Vector<3>::rotateX(CoordType theta)
413  {return rotate(1, 2, theta);}
414 template<> Vector<3>& Vector<3>::rotateY(CoordType theta)
415  {return rotate(2, 0, theta);}
416 template<> Vector<3>& Vector<3>::rotateZ(CoordType theta)
417  {return rotate(0, 1, theta);}
418 
419 
420 } // namespace WFMath
421 
422 #endif // WFMATH_VECTOR_FUNCS_H