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
43namespace WFMath {
44
45template<int dim>
46Vector<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
53template<int dim>
54Vector<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
61template<int dim>
63{
64 static ZeroPrimitive<Vector<dim> > zeroVector(dim);
65 return zeroVector.getShape();
66}
67
68
69template<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
81template<int dim>
82bool 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
95template <int dim>
96Vector<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
107template <int dim>
108Vector<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
119template <int dim>
121{
122 for(int i = 0; i < dim; ++i) {
123 v.m_elem[i] *= d;
124 }
125
126 return v;
128
129template <int dim>
132 for(int i = 0; i < dim; ++i) {
133 v.m_elem[i] /= d;
134 }
135
136 return v;
137}
138
139
140template <int dim>
141Vector<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
154template<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);
163
164template<int dim>
167 m_valid = true;
169 for(int i = 0; i < dim; ++i) {
170 m_elem[i] = 0;
171 }
172
173 return *this;
174}
176template<int dim>
177CoordType 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
190template<int dim>
191Vector<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
205template<int dim>
207 CoordType theta)
208{
210 return operator=(Prod(*this, m.rotation(v1, v2, theta)));
211}
213template<int dim>
216 return *this = Prod(*this, m);
217}
218
219template<> Vector<3>& Vector<3>::rotate(const Vector<3>& axis, CoordType theta);
220template<> Vector<3>& Vector<3>::rotate(const Quaternion& q);
221
222template<int dim>
223CoordType 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
236template<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
249template<int dim>
250bool 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 }
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
285template<> Vector<2>& Vector<2>::polar(CoordType r, CoordType theta);
286template<> void Vector<2>::asPolar(CoordType& r, CoordType& theta) const;
287
288template<> Vector<3>& Vector<3>::polar(CoordType r, CoordType theta,
289 CoordType z);
290template<> void Vector<3>::asPolar(CoordType& r, CoordType& theta,
291 CoordType& z) const;
293 CoordType phi);
294template<> void Vector<3>::asSpherical(CoordType& r, CoordType& theta,
295 CoordType& phi) const;
296
297template<> CoordType Vector<2>::sloppyMag() const;
298template<> CoordType Vector<3>::sloppyMag() const;
299
300template<> CoordType Vector<1>::sloppyMag() const
301 {return std::fabs(m_elem[0]);}
302
303template<> Vector<2>::Vector(CoordType x, CoordType y) : m_valid(true)
304 {m_elem[0] = x; m_elem[1] = y;}
305template<> 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
308template<> Vector<2>& Vector<2>::rotate(CoordType theta)
309 {return rotate(0, 1, theta);}
310
311template<> Vector<3>& Vector<3>::rotateX(CoordType theta)
312 {return rotate(1, 2, theta);}
313template<> Vector<3>& Vector<3>::rotateY(CoordType theta)
314 {return rotate(2, 0, theta);}
315template<> 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
A dim dimensional point.
Definition point.h:96
A normalized quaterion.
Definition quaternion.h:36
A dim dimensional rotation matrix. Technically, a member of the group O(dim).
Definition rotmatrix.h:87
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
A dim dimensional vector.
Definition vector.h:121
Vector & rotateZ(CoordType theta)
3D only: rotate a vector about the z axis by an angle theta
CoordType z() const
Access the third component of a vector.
void asPolar(CoordType &r, CoordType &theta) const
2D only: convert a vector to polar coordinates
Vector()
Construct an uninitialized vector.
Definition vector.h:125
void asSpherical(CoordType &r, CoordType &theta, CoordType &phi) const
3D only: convert a vector to shperical coordinates
Vector & spherical(CoordType r, CoordType theta, CoordType phi)
3D only: construct a vector from shperical coordinates
Vector & zero()
Zero the components of a vector.
Definition vector_funcs.h:165
static const Vector< dim > & ZERO()
Provides a global instance preset to zero.
Definition vector_funcs.h:62
Vector & sloppyNorm(CoordType norm=1.0)
Approximately normalize a vector.
Definition vector_funcs.h:155
CoordType sloppyMag() const
An approximation to the magnitude of a vector.
CoordType x() const
Access the first component of a vector.
Definition vector.h:313
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
CoordType y() const
Access the second component of a vector.
Definition vector.h:317
CoordType sqrMag() const
The squared magnitude of a vector.
Definition vector_funcs.h:237
Vector & rotateX(CoordType theta)
3D only: rotate a vector about the x axis by an angle theta
Vector & rotateY(CoordType theta)
3D only: rotate a vector about the y axis by an angle theta
Vector & polar(CoordType r, CoordType theta)
2D only: construct a vector from polar coordinates
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 atlasconv.h:45
RotMatrix< dim > Prod(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2
Definition rotmatrix_funcs.h:89
float CoordType
Basic floating point type.
Definition const.h:140
bool Perpendicular(const Vector< dim > &v1, const Vector< dim > &v2)
Check if two vectors are perpendicular.
Definition vector_funcs.h:250