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