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