WFMath  1.0.2
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 <limits>
38 
39 #include <cmath>
40 
41 #include <cassert>
42 
43 namespace WFMath {
44 
45 template<int dim>
46 Vector<dim>::Vector(const Vector<dim>& v) : m_valid(v.m_valid)
47 {
48  for(int i = 0; i < dim; ++i) {
49  m_elem[i] = v.m_elem[i];
50  }
51 }
52 
53 template<int dim>
54 Vector<dim>::Vector(const Point<dim>& p) : m_valid(p.isValid())
55 {
56  for(int i = 0; i < dim; ++i) {
57  m_elem[i] = p.elements()[i];
58  }
59 }
60 
61 template<int dim>
63 {
64  static ZeroPrimitive<Vector<dim> > zeroVector(dim);
65  return zeroVector.getShape();
66 }
67 
68 
69 template<int dim>
71 {
72  m_valid = v.m_valid;
73 
74  for(int i = 0; i < dim; ++i) {
75  m_elem[i] = v.m_elem[i];
76  }
77 
78  return *this;
79 }
80 
81 template<int dim>
82 bool Vector<dim>::isEqualTo(const Vector<dim>& v, CoordType epsilon) const
83 {
84  double delta = _ScaleEpsilon(m_elem, v.m_elem, dim, epsilon);
85 
86  for(int i = 0; i < dim; ++i) {
87  if(std::fabs(m_elem[i] - v.m_elem[i]) > delta) {
88  return false;
89  }
90  }
91 
92  return true;
93 }
94 
95 template <int dim>
96 Vector<dim>& operator+=(Vector<dim>& v1, const Vector<dim>& v2)
97 {
98  v1.m_valid = v1.m_valid && v2.m_valid;
99 
100  for(int i = 0; i < dim; ++i) {
101  v1.m_elem[i] += v2.m_elem[i];
102  }
103 
104  return v1;
105 }
106 
107 template <int dim>
108 Vector<dim>& operator-=(Vector<dim>& v1, const Vector<dim>& v2)
109 {
110  v1.m_valid = v1.m_valid && v2.m_valid;
111 
112  for(int i = 0; i < dim; ++i) {
113  v1.m_elem[i] -= v2.m_elem[i];
114  }
115 
116  return v1;
117 }
118 
119 template <int dim>
121 {
122  for(int i = 0; i < dim; ++i) {
123  v.m_elem[i] *= d;
124  }
125 
126  return v;
127 }
128 
129 template <int dim>
131 {
132  for(int i = 0; i < dim; ++i) {
133  v.m_elem[i] /= d;
134  }
135 
136  return v;
137 }
138 
139 
140 template <int dim>
141 Vector<dim> operator-(const Vector<dim>& v)
142 {
143  Vector<dim> ans;
144 
145  ans.m_valid = v.m_valid;
146 
147  for(int i = 0; i < dim; ++i) {
148  ans.m_elem[i] = -v.m_elem[i];
149  }
150 
151  return ans;
152 }
153 
154 template<int dim>
156 {
157  CoordType mag = sloppyMag();
158 
159  assert("need nonzero length vector" && mag > norm / std::numeric_limits<CoordType>::max());
160 
161  return (*this *= norm / mag);
162 }
163 
164 template<int dim>
166 {
167  m_valid = true;
168 
169  for(int i = 0; i < dim; ++i) {
170  m_elem[i] = 0;
171  }
172 
173  return *this;
174 }
175 
176 template<int dim>
177 CoordType Angle(const Vector<dim>& v, const Vector<dim>& u)
178 {
179  // Adding numbers with large magnitude differences can cause
180  // a loss of precision, but Dot() checks for this now
181 
182  CoordType dp = FloatClamp(Dot(u, v) / std::sqrt(u.sqrMag() * v.sqrMag()),
183  -1.0, 1.0);
184 
185  CoordType angle = std::acos(dp);
186 
187  return angle;
188 }
189 
190 template<int dim>
191 Vector<dim>& Vector<dim>::rotate(int axis1, int axis2, CoordType theta)
192 {
193  assert(axis1 >= 0 && axis2 >= 0 && axis1 < dim && axis2 < dim && axis1 != axis2);
194 
195  CoordType tmp1 = m_elem[axis1], tmp2 = m_elem[axis2];
196  CoordType stheta = std::sin(theta),
197  ctheta = std::cos(theta);
198 
199  m_elem[axis1] = tmp1 * ctheta - tmp2 * stheta;
200  m_elem[axis2] = tmp2 * ctheta + tmp1 * stheta;
201 
202  return *this;
203 }
204 
205 template<int dim>
207  CoordType theta)
208 {
209  RotMatrix<dim> m;
210  return operator=(Prod(*this, m.rotation(v1, v2, theta)));
211 }
212 
213 template<int dim>
215 {
216  return *this = Prod(*this, m);
217 }
218 
219 template<> Vector<3>& Vector<3>::rotate(const Vector<3>& axis, CoordType theta);
220 template<> Vector<3>& Vector<3>::rotate(const Quaternion& q);
221 
222 template<int dim>
223 CoordType Dot(const Vector<dim>& v1, const Vector<dim>& v2)
224 {
225  double delta = _ScaleEpsilon(v1.m_elem, v2.m_elem, dim);
226 
227  CoordType ans = 0;
228 
229  for(int i = 0; i < dim; ++i) {
230  ans += v1.m_elem[i] * v2.m_elem[i];
231  }
232 
233  return (std::fabs(ans) >= delta) ? ans : 0;
234 }
235 
236 template<int dim>
238 {
239  CoordType ans = 0;
240 
241  for(int i = 0; i < dim; ++i) {
242  // all terms > 0, no loss of precision through cancelation
243  ans += m_elem[i] * m_elem[i];
244  }
245 
246  return ans;
247 }
248 
249 template<int dim>
250 bool Perpendicular(const Vector<dim>& v1, const Vector<dim>& v2)
251 {
252  CoordType max1 = 0, max2 = 0;
253 
254  for(int i = 0; i < dim; ++i) {
255  CoordType val1 = std::fabs(v1[i]), val2 = std::fabs(v2[i]);
256  if(val1 > max1) {
257  max1 = val1;
258  }
259  if(val2 > max2) {
260  max2 = val2;
261  }
262  }
263 
264  // Need to scale by both, since Dot(v1, v2) goes like the product of the magnitudes
265  int exp1, exp2;
266  (void) std::frexp(max1, &exp1);
267  (void) std::frexp(max2, &exp2);
268 
269  return std::fabs(Dot(v1, v2)) < std::ldexp(numeric_constants<CoordType>::epsilon(), exp1 + exp2);
270 }
271 
272 // Note for people trying to compute the above numbers
273 // more accurately:
274 
275 // The worst value for dim == 2 occurs when the ratio of the components
276 // of the vector is sqrt(2) - 1. The value above is equal to sqrt(4 - 2 * sqrt(2)).
277 
278 // The worst value for dim == 3 occurs when the two smaller components
279 // are equal, and their ratio with the large component is the
280 // (unique, real) solution to the equation q x^3 + (q-1) x + p == 0,
281 // where p = sqrt(2) - 1, and q = sqrt(3) + 1 - 2 * sqrt(2).
282 // Running the script bc_sloppy_mag_3 provided with the WFMath source
283 // will calculate the above number.
284 
285 template<> Vector<2>& Vector<2>::polar(CoordType r, CoordType theta);
286 template<> void Vector<2>::asPolar(CoordType& r, CoordType& theta) const;
287 
288 template<> Vector<3>& Vector<3>::polar(CoordType r, CoordType theta,
289  CoordType z);
290 template<> void Vector<3>::asPolar(CoordType& r, CoordType& theta,
291  CoordType& z) const;
292 template<> Vector<3>& Vector<3>::spherical(CoordType r, CoordType theta,
293  CoordType phi);
294 template<> void Vector<3>::asSpherical(CoordType& r, CoordType& theta,
295  CoordType& phi) const;
296 
297 template<> CoordType Vector<2>::sloppyMag() const;
298 template<> CoordType Vector<3>::sloppyMag() const;
299 
300 template<> CoordType Vector<1>::sloppyMag() const
301  {return std::fabs(m_elem[0]);}
302 
303 template<> Vector<2>::Vector(CoordType x, CoordType y) : m_valid(true)
304  {m_elem[0] = x; m_elem[1] = y;}
305 template<> Vector<3>::Vector(CoordType x, CoordType y, CoordType z) : m_valid(true)
306  {m_elem[0] = x; m_elem[1] = y; m_elem[2] = z;}
307 
308 template<> Vector<2>& Vector<2>::rotate(CoordType theta)
309  {return rotate(0, 1, theta);}
310 
311 template<> Vector<3>& Vector<3>::rotateX(CoordType theta)
312  {return rotate(1, 2, theta);}
313 template<> Vector<3>& Vector<3>::rotateY(CoordType theta)
314  {return rotate(2, 0, theta);}
315 template<> Vector<3>& Vector<3>::rotateZ(CoordType theta)
316  {return rotate(0, 1, theta);}
317 
318 
319 } // namespace WFMath
320 
321 #endif // WFMATH_VECTOR_FUNCS_H
Vector()
Construct an uninitialized vector.
Definition: vector.h:125
Generic library namespace.
Definition: atlasconv.h:45
Vector & rotateZ(CoordType theta)
3D only: rotate a vector about the z axis by an angle theta
Vector & polar(CoordType r, CoordType theta)
2D only: construct a vector from polar coordinates
void asSpherical(CoordType &r, CoordType &theta, CoordType &phi) const
3D only: convert a vector to shperical coordinates
A dim dimensional rotation matrix. Technically, a member of the group O(dim).
Definition: const.h:53
Vector & rotateY(CoordType theta)
3D only: rotate a vector about the y axis by an angle theta
CoordType sqrMag() const
The squared magnitude of a vector.
Definition: vector_funcs.h:237
Vector & rotate(int axis1, int axis2, CoordType theta)
Rotate the vector in the (axis1, axis2) plane by the angle theta.
Definition: vector_funcs.h:191
Vector & rotateX(CoordType theta)
3D only: rotate a vector about the x axis by an angle theta
bool Perpendicular(const Vector< dim > &v1, const Vector< dim > &v2)
Check if two vectors are perpendicular.
Definition: vector_funcs.h:250
Vector & spherical(CoordType r, CoordType theta, CoordType phi)
3D only: construct a vector from shperical coordinates
void asPolar(CoordType &r, CoordType &theta) const
2D only: convert a vector to polar coordinates
const Shape & getShape() const
Gets the zeroed shape.
Definition: zero.h:53
A dim dimensional vector.
Definition: const.h:55
RotMatrix & rotation(const int i, const int j, CoordType theta)
set the matrix to a rotation by the angle theta in the (i, j) plane
Definition: rotmatrix_funcs.h:360
float CoordType
Basic floating point type.
Definition: const.h:140
CoordType sloppyMag() const
An approximation to the magnitude of a vector.
Utility class for providing zero primitives. This class will only work with simple structures such as...
Definition: point.h:87
A normalized quaterion.
Definition: quaternion.h:35
RotMatrix< dim > Prod(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2
Definition: rotmatrix_funcs.h:89
A dim dimensional point.
Definition: const.h:50