wfmath  1.0.3
A math library for the Worldforge system.
vector.cpp
1 // vector.cpp (Vector<> implementation)
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 #ifdef HAVE_CONFIG_H
31 #include "config.h"
32 #endif
33 
34 #include "vector_funcs.h"
35 #include "basis.h"
36 #include "point.h"
37 #include "quaternion.h"
38 
39 namespace WFMath {
40 
41 
42 template<> CoordType Vector<2>::sloppyMag() const
43 {
44  CoordType ax = std::fabs(m_elem[0]),
45  ay = std::fabs(m_elem[1]);
46  const CoordType p = numeric_constants<CoordType>::sqrt2() - 1;
47 
48  // Don't need float add, all terms > 0
49 
50  if(ax > ay)
51  return ax + p * ay;
52  else if (ay > 0)
53  return ay + p * ax;
54  else
55  return 0;
56 }
57 
58 template<> CoordType Vector<3>::sloppyMag() const
59 {
60  CoordType ax = std::fabs(m_elem[0]),
61  ay = std::fabs(m_elem[1]),
62  az = std::fabs(m_elem[2]);
63  const CoordType p = numeric_constants<CoordType>::sqrt2() - 1;
64  const CoordType q = numeric_constants<CoordType>::sqrt3() + 1 - 2 * numeric_constants<CoordType>::sqrt2();
65 
66  // Don't need FloatAdd, only term < 0 is q, it's very small,
67  // and amin1 * amin2 / amax < amax.
68 
69  if(ax > ay && ax > az)
70  return ax + p * (ay + az) + q * ay * az / ax;
71  else if (ay > az)
72  return ay + p * (ax + az) + q * ax * az / ay;
73  else if (az > 0)
74  return az + p * (ax + ay) + q * ax * ay / az;
75  else
76  return 0;
77 }
78 
79 template<> Vector<3>& Vector<3>::rotate(const Vector<3>& axis, CoordType theta)
80 {
81  CoordType axis_sqr_mag = axis.sqrMag();
82 
83  assert(axis_sqr_mag != 0);
84 
85  Vector<3> perp_part = *this - axis * Dot(*this, axis) / axis_sqr_mag;
86  Vector<3> rot90 = Cross(axis, perp_part) / std::sqrt(axis_sqr_mag);
87 
88  *this += perp_part * (std::cos(theta) - 1) + rot90 * std::sin(theta);
89 
90  return *this;
91 }
92 
93 template<> Vector<3>& Vector<3>::rotate(const Quaternion& q)
94 {
95  *this = (2 * q.scalar() * q.scalar() - 1) * *this +
96  2 * q.vector() * Dot(q.vector(), *this) +
97  2 * q.scalar() * Cross(q.vector(), *this);
98 
99  return *this;
100 }
101 
102 CoordType Cross(const Vector<2>& v1, const Vector<2>& v2)
103 {
104  CoordType ans = v1[0] * v2[1] - v2[0] * v1[1];
105 
106  return (ans >= v1._scaleEpsilon(v2)) ? ans : 0;
107 }
108 
109 Vector<3> Cross(const Vector<3>& v1, const Vector<3>& v2)
110 {
111  Vector<3> ans;
112 
113  ans.setValid(v1.isValid() && v2.isValid());
114 
115  ans[0] = v1[1] * v2[2] - v2[1] * v1[2];
116  ans[1] = v1[2] * v2[0] - v2[2] * v1[0];
117  ans[2] = v1[0] * v2[1] - v2[0] * v1[1];
118 
119  double delta = v1._scaleEpsilon(v2);
120 
121  for(int i = 0; i < 3; ++i)
122  if(std::fabs(ans[i]) < delta)
123  ans[i] = 0;
124 
125  return ans;
126 }
127 
128 template<>
130 {
131  CoordType d[2] = {r, theta};
132  _PolarToCart(d, m_elem);
133  m_valid = true;
134  return *this;
135 }
136 
137 template<>
138 void Vector<2>::asPolar(CoordType& r, CoordType& theta) const
139 {
140  CoordType d[2];
141  _CartToPolar(m_elem, d);
142  r = d[0];
143  theta = d[1];
144 }
145 
146 template<>
147 Vector<3>& Vector<3>::polar(CoordType r, CoordType theta, CoordType z)
148 {
149  CoordType d[2] = {r, theta};
150  _PolarToCart(d, m_elem);
151  m_elem[2] = z;
152  m_valid = true;
153  return *this;
154 }
155 
156 template<>
157 void Vector<3>::asPolar(CoordType& r, CoordType& theta, CoordType& z) const
158 {
159  CoordType d[2];
160  _CartToPolar(m_elem, d);
161  r = d[0];
162  theta = d[1];
163  z = m_elem[2];
164 }
165 
166 template<>
167 Vector<3>& Vector<3>::spherical(CoordType r, CoordType theta, CoordType phi)
168 {
169  CoordType d[3] = {r, theta, phi};
170  _SphericalToCart(d, m_elem);
171  m_valid = true;
172  return *this;
173 }
174 
175 template<>
176 void Vector<3>::asSpherical(CoordType& r, CoordType& theta,
177  CoordType& phi) const
178 {
179  CoordType d[3];
180  _CartToSpherical(m_elem, d);
181  r = d[0];
182  theta = d[1];
183  phi = d[2];
184 }
185 
186 template class Vector<3>;
187 static_assert(std::is_standard_layout<Vector<3>>::value, "Vector should be standard layout.");
188 static_assert(std::is_trivially_copyable<Vector<3>>::value, "Vector should be trivially copyable.");
189 template class Vector<2>;
190 static_assert(std::is_standard_layout<Vector<2>>::value, "Vector should be standard layout.");
191 static_assert(std::is_trivially_copyable<Vector<2>>::value, "Vector should be trivially copyable.");
192 
193 template Vector<3>& operator-=(Vector<3>& v1, const Vector<3>& v2);
194 template Vector<2>& operator-=(Vector<2>& v1, const Vector<2>& v2);
195 
196 template Vector<3>& operator+=(Vector<3>& v1, const Vector<3>& v2);
197 template Vector<2>& operator+=(Vector<2>& v1, const Vector<2>& v2);
198 
199 template Vector<3>& operator*=(Vector<3>& v1, CoordType d);
200 template Vector<2>& operator*=(Vector<2>& v1, CoordType d);
201 
202 template Vector<3>& operator/=(Vector<3>& v1, CoordType d);
203 template Vector<2>& operator/=(Vector<2>& v1, CoordType d);
204 
205 template CoordType Dot<3>(const Vector<3> &, const Vector<3> &);
206 template CoordType Dot<2>(const Vector<2> &, const Vector<2> &);
207 template CoordType Angle<3>(const Vector<3> &, const Vector<3> &);
208 
209 template Vector<3> operator-<3>(const Vector<3> &);
210 
211 template Vector<3> operator*<3>(CoordType, const Vector<3> &);
212 template Vector<2> operator*<2>(CoordType, const Vector<2> &);
213 template Vector<3> operator*<3>(const Vector<3> &, CoordType);
214 template Vector<2> operator*<2>(const Vector<2> &, CoordType);
215 template Vector<3> operator/<3>(const Vector<3> &, CoordType);
216 template Vector<2> operator/<2>(const Vector<2> &, CoordType);
217 
218 template Vector<3> operator+<3>(const Vector<3> &, const Vector<3> &);
219 template Vector<2> operator+<2>(const Vector<2> &, const Vector<2> &);
220 
221 template Vector<3> operator-<3>(const Vector<3> &, const Vector<3> &);
222 template Vector<2> operator-<2>(const Vector<2> &, const Vector<2> &);
223 
224 }
Vector & polar(CoordType r, CoordType theta)
2D only: construct a vector from polar coordinates
void asPolar(CoordType &r, CoordType &theta) const
2D only: convert a vector to polar coordinates
void asSpherical(CoordType &r, CoordType &theta, CoordType &phi) const
3D only: convert a vector to shperical coordinates
CoordType sloppyMag() const
An approximation to the magnitude of a vector.
void setValid(bool valid=true)
make isValid() return true if you've initialized the vector by hand
Definition: vector.h:154
Vector & spherical(CoordType r, CoordType theta, CoordType phi)
3D only: construct a vector from shperical coordinates
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
Generic library namespace.
Definition: shape.h:41
double CoordType
Basic floating point type.
Definition: const.h:140
CoordType Cross(const Vector< 2 > &v1, const Vector< 2 > &v2)
2D only: get the z component of the cross product of two vectors
Definition: vector.cpp:102