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 : */
76 : struct radians;
77 :
78 : /// Type specifying angles in degrees
79 : /**
80 : */
81 : struct degrees;
82 :
83 : /// Type holding constants related to angle calculations in degrees
84 : template <typename degrad, typename realT>
85 : struct degradT;
86 :
87 : /// Type holding constants related to angle calculations in degrees
88 : /**
89 : */
90 : template <typename _realT>
91 : struct degradT<degrees, _realT>
92 : {
93 : typedef _realT realT;
94 : static constexpr realT scale =
95 : static_cast<realT>( 180 ) / pi<realT>(); // Scale factor to convert from radians to this unit.
96 : static constexpr realT degrees = 1;
97 : static constexpr realT radians = pi<realT>() / static_cast<realT>( 180 );
98 : static constexpr realT full = static_cast<realT>( 360.0 );
99 : static constexpr realT half = static_cast<realT>( 180.0 );
100 : };
101 :
102 : template <typename realT>
103 : using degreesT = degradT<degrees, realT>;
104 :
105 : /// Type holding constants related to angle calculations in radians
106 : /**
107 : */
108 : template <typename _realT>
109 : struct degradT<radians, _realT>
110 : {
111 : typedef _realT realT;
112 : static constexpr realT scale = 1; // Scale factor to convert from radians to this unit.
113 : static constexpr realT degrees = static_cast<realT>( 180 ) / pi<realT>();
114 : static constexpr realT radians = 1;
115 : static constexpr realT full = two_pi<realT>();
116 : static constexpr realT half = pi<realT>();
117 : };
118 :
119 : template <typename realT>
120 : using radiansT = degradT<radians, realT>;
121 :
122 : /// Convert from degrees to radians
123 : /**
124 : *
125 : * \param q is the angle to convert
126 : *
127 : * \return the angle q converted to radians
128 : *
129 : *
130 : * \ingroup geo
131 : */
132 : template <typename realT>
133 45 : realT dtor( realT q )
134 : {
135 45 : return q * degradT<degrees, realT>::radians;
136 : }
137 :
138 : /// Convert from radians to degrees
139 : /**
140 : *
141 : * \param q is the angle to convert
142 : *
143 : * \return the angle q converted to degrees
144 : *
145 : * \ingroup geo
146 : */
147 : template <typename realT>
148 11534338 : realT rtod( realT q )
149 : {
150 11534338 : return q * degradT<radians, realT>::degrees;
151 : }
152 :
153 : /// Calculate the angle modulo full-circle, normalizing to a positive value.
154 : /** The output will be betweeen 0 and 360 (or 0 and 2pi).
155 : *
156 : * \returns the value of q normalized to 0 <= q < 360[2pi]
157 : *
158 : * \tparam degrad controls whether this is in degrees (0, default) or radians (1)
159 : * \tparam realT is the type in which to do arithmetic
160 : *
161 : * \ingroup geo
162 : */
163 : template <class angleT>
164 23 : typename angleT::realT angleMod( typename angleT::realT q /**< [in] the angle */ )
165 : {
166 : static_assert( std::is_floating_point<typename angleT::realT>::value,
167 : "angleMod: angleT::realT must be floating point" );
168 :
169 23 : q = fmod( q, angleT::full );
170 :
171 23 : if( q < 0 )
172 1 : q += angleT::full;
173 :
174 23 : return q;
175 : }
176 :
177 : /// Calculate the difference between two angles, correctly across 0/360.
178 : /** Calculates \f$ dq = q2- q1 \f$, but accounts for crossing 0/360. This implies
179 : * that \f$ dq \le 180 \f$.
180 : *
181 : *
182 : * \returns the difference of q2 and q1
183 : *
184 : * \tparam angleT controls whether this is in degrees (0, default) or radians (1)
185 : * \tparam realT is the type in which to do arithmetic
186 : *
187 : *
188 : * \ingroup geo
189 : */
190 : template <class angleT>
191 11535137 : typename angleT::realT angleDiff( typename angleT::realT q1, ///< [in] angle to subtract from q2.
192 : typename angleT::realT q2 ///< [in] angle to subtract q1 from.
193 : )
194 : {
195 : static_assert( std::is_floating_point<typename angleT::realT>::value, "angleDiff: realT must be floating point" );
196 :
197 11535137 : typename angleT::realT dq = q2 - q1;
198 :
199 11535137 : if( std::abs( dq ) > angleT::half )
200 : {
201 5241994 : if( dq < 0 )
202 : {
203 18 : dq = dq + static_cast<typename angleT::realT>( 2 ) * angleT::half;
204 : }
205 : else
206 : {
207 5241976 : dq = dq - static_cast<typename angleT::realT>( 2 ) * angleT::half;
208 : }
209 : }
210 :
211 11535137 : return dq;
212 : }
213 :
214 : /// Calculate the mean of a set of angles, correctly across 0/360.
215 : /** Calculates the mean by decomposing into vectors and averaging the components. This accounts for crossing 0/360.
216 : *
217 : * \returns the mean angle
218 : *
219 : * \tparam angleT is the angle type, either radians<realT> or degrees<realT>. angleT::realT is the type in which to do
220 : * arithmetic.
221 : *
222 : * \ingroup geo
223 : */
224 : template <class angleT>
225 : typename angleT::realT
226 : angleMean( const std::vector<typename angleT::realT> &q /**< [in] vector of angles to average.*/ )
227 : {
228 : static_assert( std::is_floating_point<typename angleT::realT>::value, "angleMean: realT must be floating point" );
229 :
230 : typename angleT::realT s = 0;
231 : typename angleT::realT c = 0;
232 :
233 : for( int i = 0; i < q.size(); ++i )
234 : {
235 : s += sin( q[i] / angleT::scale );
236 : c += cos( q[i] / angleT::scale );
237 : }
238 :
239 : s /= q.size();
240 : c /= q.size();
241 :
242 : return atan2( s, c ) * angleT::scale;
243 : }
244 :
245 : /// Make a vector of angles continuous, fixing the 0/360 crossing.
246 : /** The vector is modified so it is continuous.
247 : *
248 : *
249 : * \tparam degrad controls whether angles are degrees (false) or radians (true)
250 : * \tparam realT is the type in which to do arithmetic
251 : *
252 : * \ingroup geo
253 : */
254 : template <int degrad = 0, typename realT>
255 : int continueAngles( std::vector<realT> &angles, ///< [in] the vector of angles
256 : realT threshold = 0.75 ///< [in] [optional] the fraction of a full circle at which to consider a
257 : ///< difference in angle discontinuous.
258 : )
259 : {
260 : realT full;
261 :
262 : if( degrad )
263 : full = two_pi<realT>();
264 : else
265 : full = static_cast<realT>( 360 );
266 :
267 : threshold *= full;
268 :
269 : realT adj = 0;
270 :
271 : if( fabs( angles[1] - angles[0] ) > threshold )
272 : {
273 : if( angles[1] > angles[0] )
274 : adj = -full;
275 : else
276 : adj = full;
277 :
278 : angles[1] += adj;
279 : }
280 :
281 : if( angles.size() == 2 )
282 : return 0;
283 :
284 : for( int i = 2; i < angles.size(); ++i )
285 : {
286 : angles[i] += adj;
287 :
288 : if( fabs( angles[i] - angles[i - 1] ) > threshold )
289 : {
290 : if( angles[i] > angles[i - 1] )
291 : adj += -full;
292 : else
293 : adj += full;
294 :
295 : angles[i] += adj;
296 : }
297 : }
298 :
299 : return 0;
300 : }
301 :
302 : /// Rotate a point about the origin.
303 : /** The rotation is counter-clockwise for positive angles.
304 : *
305 : * \tparam realT a real floating point type
306 : *
307 : * \ingroup geo
308 : */
309 : template <typename realT>
310 : void rotatePoint( realT &x0, ///< [in.out] the x-coordinate of the point to rotate. On exit contains the rotated value.
311 : realT &y0, ///< [in.out] the y-coordinate of the point to rotate. On exit contains the rotated value.
312 : realT angle ///< [in] the angle by which to rotate [radians]
313 : )
314 : {
315 : realT x1;
316 : realT y1;
317 :
318 : realT cq = cos( angle );
319 : realT sq = sin( angle );
320 :
321 : x1 = x0 * cq - y0 * sq;
322 : y1 = x0 * sq + y0 * cq;
323 :
324 : x0 = x1;
325 : y0 = y1;
326 : }
327 :
328 : } // namespace math
329 : } // namespace mx
330 :
331 : #endif // math_geo_hpp
|
