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
43namespace WFMath {
44
45template<int dim>
46Vector<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
53template<int dim>
55{
56 static ZeroPrimitive<Vector<dim> > zeroVector(dim);
57 return zeroVector.getShape();
58}
59
60template<int dim>
61bool 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
78template <int dim>
79Vector<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
90template <int dim>
91Vector<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
102template <int dim>
104{
105 for(int i = 0; i < dim; ++i) {
106 v.m_elem[i] *= d;
107 }
108
109 return v;
110}
111
112template <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
123template <int dim>
124Vector<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}
137template<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
147template<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;
158
159template<int dim>
160CoordType 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
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
173template<int dim>
174Vector<dim>& Vector<dim>::rotate(int axis1, int axis2, CoordType theta)
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
188template<int dim>
190 CoordType theta)
191{
193 return operator=(Prod(*this, m.rotation(v1, v2, theta)));
194}
195
196template<int dim>
198{
199 return *this = Prod(*this, m);
200}
201
202template<> Vector<3>& Vector<3>::rotate(const Vector<3>& axis, CoordType theta);
203template<> Vector<3>& Vector<3>::rotate(const Quaternion& q);
204
205template<int dim>
206CoordType 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 }
216 return (std::fabs(ans) >= delta) ? ans : 0;
217}
218
219template<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
232template<int dim>
233bool 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;
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:
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
268template<> Vector<2>& Vector<2>::polar(CoordType r, CoordType theta);
269template<> void Vector<2>::asPolar(CoordType& r, CoordType& theta) const;
270
271template<> Vector<3>& Vector<3>::polar(CoordType r, CoordType theta,
272 CoordType z);
273template<> void Vector<3>::asPolar(CoordType& r, CoordType& theta,
274 CoordType& z) const;
276 CoordType phi);
277template<> void Vector<3>::asSpherical(CoordType& r, CoordType& theta,
278 CoordType& phi) const;
279
280template<> CoordType Vector<2>::sloppyMag() const;
281template<> CoordType Vector<3>::sloppyMag() const;
282
283template<> CoordType Vector<1>::sloppyMag() const
284 {return std::fabs(m_elem[0]);}
285
286template<> Vector<2>::Vector(CoordType x, CoordType y) : m_valid(true)
287 {m_elem[0] = x; m_elem[1] = y;}
288template<> 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
291template<> Vector<2>& Vector<2>::rotate(CoordType theta)
292 {return rotate(0, 1, theta);}
293
294template<> Vector<3>& Vector<3>::rotateX(CoordType theta)
295 {return rotate(1, 2, theta);}
296template<> Vector<3>& Vector<3>::rotateY(CoordType theta)
297 {return rotate(2, 0, theta);}
298template<> 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
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 & 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:54
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
CoordType sqrMag() const
The squared magnitude of a vector.
Definition: vector_funcs.h:220
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
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:233