Line data Source code
1 : /** \file geo.hpp
2 : * \author Jared R. Males
3 : * \brief Utilities for working with angles
4 : * \ingroup gen_math_files
5 : *
6 : */
7 :
8 : //***********************************************************************//
9 : // Copyright 2015, 2016, 2017, 2018 Jared R. Males (jaredmales@gmail.com)
10 : //
11 : // This file is part of mxlib.
12 : //
13 : // mxlib is free software: you can redistribute it and/or modify
14 : // it under the terms of the GNU General Public License as published by
15 : // the Free Software Foundation, either version 3 of the License, or
16 : // (at your option) any later version.
17 : //
18 : // mxlib is distributed in the hope that it will be useful,
19 : // but WITHOUT ANY WARRANTY; without even the implied warranty of
20 : // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
21 : // GNU General Public License for more details.
22 : //
23 : // You should have received a copy of the GNU General Public License
24 : // along with mxlib. If not, see <http://www.gnu.org/licenses/>.
25 : //***********************************************************************//
26 :
27 : #ifndef math_geo_hpp
28 : #define math_geo_hpp
29 :
30 : #include <vector>
31 :
32 : #include <cmath>
33 :
34 : #include "constants.hpp"
35 :
36 : namespace mx
37 : {
38 : namespace math
39 : {
40 :
41 : /// Calculate the semi-latus rectum of a <a href="http://en.wikipedia.org/wiki/Conic_section">conic section</a>
42 : /**
43 : * \ingroup geo
44 : */
45 : #define semilatrect( a, e ) \
46 : ( e == 0.0 ? a : ( e == 1.0 ? 2. * a : ( e < 1. ? a * ( 1 - e * e ) : a * ( e * e - 1 ) ) ) )
47 :
48 : /// Calculate the focal parameter of a <a href="http://en.wikipedia.org/wiki/Conic_section">conic section</a>
49 : /**
50 : * \ingroup geo
51 : */
52 : #define focus( a, e ) \
53 : ( e == 0.0 ? 1e34 : ( e == 1.0 ? 2. * a : ( e < 1. ? a * ( 1 - e * e ) / e : a * ( e * e - 1 ) / e ) ) )
54 :
55 : /// Calculate the semi-major axis of a <a href="http://en.wikipedia.org/wiki/Conic_section">conic section</a>, given the
56 : /// focal parameter and the eccentricity
57 : /**
58 : * \ingroup geo
59 : */
60 : #define semimaj( p, e ) ( e == 1.0 ? 1e34 : ( e < 1 ? p * e / ( 1 - e * e ) : p * e / ( e * e - 1 ) ) )
61 :
62 : /// Calculate the eccentricity of a <a href="http://en.wikipedia.org/wiki/Conic_section">conic section</a> given the
63 : /// semi-major axis and the focal parameter
64 : /**
65 : * \ingroup geo
66 : */
67 : #define eccent( a, p ) \
68 : ( a == 0.0 ? 1e34 \
69 : : ( p >= 1e9 ? 0.0 \
70 : : ( p > 0 ? ( -p / ( 2 * a ) + 0.5 * std::sqrt( p * p / ( a * a ) + 4 ) ) \
71 : : ( p / ( 2 * a ) + 0.5 * std::sqrt( p * p / ( a * a ) + 4 ) ) ) ) )
72 :
73 : /// Type specifying angles in radians
74 : /**
75 : * \test Verify compilation and calculations of math::angleDiff \ref tests_math_geo_angleDiff "[test doc]"
76 : */
77 : struct radians;
78 :
79 : /// Type specifying angles in degrees
80 : /**
81 : * \test Verify compilation and calculations of math::angleDiff \ref tests_math_geo_angleDiff "[test doc]"
82 : */
83 : struct degrees;
84 :
85 : /// Type holding constants related to angle calculations in degrees
86 : template <typename degrad, typename realT>
87 : struct degradT;
88 :
89 : /// Type holding constants related to angle calculations in degrees
90 : /**
91 : * \test Verify compilation and calculations of math::angleDiff \ref tests_math_geo_angleDiff "[test doc]"
92 : */
93 : template <typename _realT>
94 : struct degradT<degrees, _realT>
95 : {
96 : typedef _realT realT;
97 : static constexpr realT scale =
98 : static_cast<realT>( 180 ) / pi<realT>(); // Scale factor to convert from radians to this unit.
99 : static constexpr realT degrees = 1;
100 : static constexpr realT radians = pi<realT>() / static_cast<realT>( 180 );
101 : static constexpr realT full = static_cast<realT>( 360.0 );
102 : static constexpr realT half = static_cast<realT>( 180.0 );
103 : };
104 :
105 : template <typename realT>
106 : using degreesT = degradT<degrees, realT>;
107 :
108 : /// Type holding constants related to angle calculations in radians
109 : /**
110 : * \test Verify compilation and calculations of math::angleDiff \ref tests_math_geo_angleDiff "[test doc]"
111 : */
112 : template <typename _realT>
113 : struct degradT<radians, _realT>
114 : {
115 : typedef _realT realT;
116 : static constexpr realT scale = 1; // Scale factor to convert from radians to this unit.
117 : static constexpr realT degrees = static_cast<realT>( 180 ) / pi<realT>();
118 : static constexpr realT radians = 1;
119 : static constexpr realT full = two_pi<realT>();
120 : static constexpr realT half = pi<realT>();
121 : };
122 :
123 : template <typename realT>
124 : using radiansT = degradT<radians, realT>;
125 :
126 : /// Convert from degrees to radians
127 : /**
128 : *
129 : * \param q is the angle to convert
130 : *
131 : * \return the angle q converted to radians
132 : *
133 : * \test Verify compilation and calculations of math::angleDiff \ref tests_math_geo_angleDiff "[test doc]"
134 : *
135 : * \ingroup geo
136 : */
137 : template <typename realT>
138 38 : realT dtor( realT q )
139 : {
140 38 : return q * degradT<degrees, realT>::radians;
141 : }
142 :
143 : /// Convert from radians to degrees
144 : /**
145 : *
146 : * \param q is the angle to convert
147 : *
148 : * \return the angle q converted to degrees
149 : *
150 : * \ingroup geo
151 : */
152 : template <typename realT>
153 2 : realT rtod( realT q )
154 : {
155 2 : return q * degradT<radians, realT>::degrees;
156 : }
157 :
158 : /// Calculate the angle modulo full-circle, normalizing to a positive value.
159 : /** The output will be betweeen 0 and 360 (or 0 and 2pi).
160 : *
161 : * \returns the value of q normalized to 0 <= q < 360[2pi]
162 : *
163 : * \tparam degrad controls whether this is in degrees (0, default) or radians (1)
164 : * \tparam realT is the type in which to do arithmetic
165 : *
166 : * \ingroup geo
167 : */
168 : template <class angleT>
169 8 : typename angleT::realT angleMod( typename angleT::realT q /**< [in] the angle */ )
170 : {
171 : static_assert( std::is_floating_point<typename angleT::realT>::value,
172 : "angleMod: angleT::realT must be floating point" );
173 :
174 8 : q = fmod( q, angleT::full );
175 :
176 8 : if( q < 0 )
177 0 : q += angleT::full;
178 :
179 8 : return q;
180 : }
181 :
182 : /// Calculate the difference between two angles, correctly across 0/360.
183 : /** Calculates \f$ dq = q2- q1 \f$, but accounts for crossing 0/360. This implies
184 : * that \f$ dq \le 180 \f$.
185 : *
186 : *
187 : * \returns the difference of q2 and q1
188 : *
189 : * \tparam angleT controls whether this is in degrees (0, default) or radians (1)
190 : * \tparam realT is the type in which to do arithmetic
191 : *
192 : * \test Verify compilation and calculations of math::angleDiff \ref tests_math_geo_angleDiff "[test doc]"
193 : *
194 : * \ingroup geo
195 : */
196 : template <class angleT>
197 16 : typename angleT::realT angleDiff( typename angleT::realT q1, ///< [in] angle to subtract from q2.
198 : typename angleT::realT q2 ///< [in] angle to subtract q1 from.
199 : )
200 : {
201 : static_assert( std::is_floating_point<typename angleT::realT>::value, "angleDiff: realT must be floating point" );
202 :
203 16 : typename angleT::realT dq = q2 - q1;
204 :
205 16 : if( std::abs( dq ) > angleT::half )
206 : {
207 8 : if( dq < 0 )
208 : {
209 4 : dq = dq + static_cast<typename angleT::realT>( 2 ) * angleT::half;
210 : }
211 : else
212 : {
213 4 : dq = dq - static_cast<typename angleT::realT>( 2 ) * angleT::half;
214 : }
215 : }
216 :
217 16 : return dq;
218 : }
219 :
220 : /// Calculate the mean of a set of angles, correctly across 0/360.
221 : /** Calculates the mean by decomposing into vectors and averaging the components. This accounts for crossing 0/360.
222 : *
223 : * \returns the mean angle
224 : *
225 : * \tparam angleT is the angle type, either radians<realT> or degrees<realT>. angleT::realT is the type in which to do
226 : * arithmetic.
227 : *
228 : * \ingroup geo
229 : */
230 : template <class angleT>
231 : typename angleT::realT
232 : angleMean( const std::vector<typename angleT::realT> &q /**< [in] vector of angles to average.*/ )
233 : {
234 : static_assert( std::is_floating_point<typename angleT::realT>::value, "angleMean: realT must be floating point" );
235 :
236 : typename angleT::realT s = 0;
237 : typename angleT::realT c = 0;
238 :
239 : for( int i = 0; i < q.size(); ++i )
240 : {
241 : s += sin( q[i] / angleT::scale );
242 : c += cos( q[i] / angleT::scale );
243 : }
244 :
245 : s /= q.size();
246 : c /= q.size();
247 :
248 : return atan2( s, c ) * angleT::scale;
249 : }
250 :
251 : /// Make a vector of angles continuous, fixing the 0/360 crossing.
252 : /** The vector is modified so it is continuous.
253 : *
254 : *
255 : * \tparam degrad controls whether angles are degrees (false) or radians (true)
256 : * \tparam realT is the type in which to do arithmetic
257 : *
258 : * \ingroup geo
259 : */
260 : template <int degrad = 0, typename realT>
261 : int continueAngles( std::vector<realT> &angles, ///< [in] the vector of angles
262 : realT threshold = 0.75 ///< [in] [optional] the fraction of a full circle at which to consider a
263 : ///< difference in angle discontinuous.
264 : )
265 : {
266 : realT full;
267 :
268 : if( degrad )
269 : full = two_pi<realT>();
270 : else
271 : full = static_cast<realT>( 360 );
272 :
273 : threshold *= full;
274 :
275 : realT adj = 0;
276 :
277 : if( fabs( angles[1] - angles[0] ) > threshold )
278 : {
279 : if( angles[1] > angles[0] )
280 : adj = -full;
281 : else
282 : adj = full;
283 :
284 : angles[1] += adj;
285 : }
286 :
287 : if( angles.size() == 2 )
288 : return 0;
289 :
290 : for( int i = 2; i < angles.size(); ++i )
291 : {
292 : angles[i] += adj;
293 :
294 : if( fabs( angles[i] - angles[i - 1] ) > threshold )
295 : {
296 : if( angles[i] > angles[i - 1] )
297 : adj += -full;
298 : else
299 : adj += full;
300 :
301 : angles[i] += adj;
302 : }
303 : }
304 :
305 : return 0;
306 : }
307 :
308 : /// Rotate a point about the origin.
309 : /** The rotation is counter-clockwise for positive angles.
310 : *
311 : * \tparam realT a real floating point type
312 : *
313 : * \ingroup geo
314 : */
315 : template <typename realT>
316 : void rotatePoint( realT &x0, ///< [in.out] the x-coordinate of the point to rotate. On exit contains the rotated value.
317 : realT &y0, ///< [in.out] the y-coordinate of the point to rotate. On exit contains the rotated value.
318 : realT angle ///< [in] the angle by which to rotate [radians]
319 : )
320 : {
321 : realT x1;
322 : realT y1;
323 :
324 : realT cq = cos( angle );
325 : realT sq = sin( angle );
326 :
327 : x1 = x0 * cq - y0 * sq;
328 : y1 = x0 * sq + y0 * cq;
329 :
330 : x0 = x1;
331 : y0 = y1;
332 : }
333 :
334 : } // namespace math
335 : } // namespace mx
336 :
337 : #endif // math_geo_hpp
|
