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
39namespace WFMath {
40
41
42template<> 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
58template<> 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
79template<> 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
93template<> 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
102CoordType 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
109Vector<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
128template<>
130{
131 CoordType d[2] = {r, theta};
132 _PolarToCart(d, m_elem);
133 m_valid = true;
134 return *this;
135}
136
137template<>
138void 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
146template<>
147Vector<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
156template<>
157void 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
166template<>
167Vector<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
175template<>
176void 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
186template class Vector<3>;
187static_assert(std::is_standard_layout<Vector<3>>::value, "Vector should be standard layout.");
188static_assert(std::is_trivially_copyable<Vector<3>>::value, "Vector should be trivially copyable.");
189template class Vector<2>;
190static_assert(std::is_standard_layout<Vector<2>>::value, "Vector should be standard layout.");
191static_assert(std::is_trivially_copyable<Vector<2>>::value, "Vector should be trivially copyable.");
192
193template Vector<3>& operator-=(Vector<3>& v1, const Vector<3>& v2);
194template Vector<2>& operator-=(Vector<2>& v1, const Vector<2>& v2);
195
196template Vector<3>& operator+=(Vector<3>& v1, const Vector<3>& v2);
197template Vector<2>& operator+=(Vector<2>& v1, const Vector<2>& v2);
198
199template Vector<3>& operator*=(Vector<3>& v1, CoordType d);
200template Vector<2>& operator*=(Vector<2>& v1, CoordType d);
201
202template Vector<3>& operator/=(Vector<3>& v1, CoordType d);
203template Vector<2>& operator/=(Vector<2>& v1, CoordType d);
204
205template CoordType Dot<3>(const Vector<3> &, const Vector<3> &);
206template CoordType Dot<2>(const Vector<2> &, const Vector<2> &);
207template CoordType Angle<3>(const Vector<3> &, const Vector<3> &);
208
209template Vector<3> operator-<3>(const Vector<3> &);
210
211template Vector<3> operator*<3>(CoordType, const Vector<3> &);
212template Vector<2> operator*<2>(CoordType, const Vector<2> &);
213template Vector<3> operator*<3>(const Vector<3> &, CoordType);
214template Vector<2> operator*<2>(const Vector<2> &, CoordType);
215template Vector<3> operator/<3>(const Vector<3> &, CoordType);
216template Vector<2> operator/<2>(const Vector<2> &, CoordType);
217
218template Vector<3> operator+<3>(const Vector<3> &, const Vector<3> &);
219template Vector<2> operator+<2>(const Vector<2> &, const Vector<2> &);
220
221template Vector<3> operator-<3>(const Vector<3> &, const Vector<3> &);
222template Vector<2> operator-<2>(const Vector<2> &, const Vector<2> &);
223
224}
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
Vector & spherical(CoordType r, CoordType theta, CoordType phi)
3D only: construct a vector from 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 & rotate(int axis1, int axis2, CoordType theta)
Rotate the vector in the (axis1, axis2) plane by the angle theta.
Definition: vector_funcs.h:174
Vector & polar(CoordType r, CoordType theta)
2D only: construct a vector from polar coordinates
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