1 /* -*- Mode:C++; c-file-style:"gnu"; indent-tabs-mode:nil; -*- */
2 /*
3 * Copyright (c) 2011, 2012 CTTC
4 *
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License version 2 as
7 * published by the Free Software Foundation;
8 *
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
13 *
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
17 *
18 * Author: Nicola Baldo <nbaldo@cttc.es>
19 */
20
21
22 #include <ns3/log.h>
23 #include <cmath>
24 #include "angles.h"
25
26
27 namespace ns3 {
28
29 NS_LOG_COMPONENT_DEFINE ("Angles");
30
31 bool Angles::m_printDeg = false;
32
33 const double DEG_TO_RAD = M_PI / 180.0;
34 const double RAD_TO_DEG = 180.0 / M_PI;
35
36
37 double
DegreesToRadians(double degrees)38 DegreesToRadians (double degrees)
39 {
40 return degrees * DEG_TO_RAD;
41 }
42
43
44 double
RadiansToDegrees(double radians)45 RadiansToDegrees (double radians)
46 {
47 return radians * RAD_TO_DEG;
48 }
49
50
51 std::vector<double>
DegreesToRadians(const std::vector<double> & degrees)52 DegreesToRadians (const std::vector<double> °rees)
53 {
54 std::vector<double> radians;
55 radians.reserve (degrees.size ());
56 for (size_t i = 0; i < degrees.size (); i++)
57 {
58 radians.push_back (DegreesToRadians (degrees[i]));
59 }
60 return radians;
61
62 }
63
64
65 std::vector<double>
RadiansToDegrees(const std::vector<double> & radians)66 RadiansToDegrees (const std::vector<double> &radians)
67 {
68 std::vector<double> degrees;
69 degrees.reserve (radians.size ());
70 for (size_t i = 0; i < radians.size (); i++)
71 {
72 degrees.push_back (RadiansToDegrees (radians[i]));
73 }
74 return degrees;
75 }
76
77
78 double
WrapTo360(double a)79 WrapTo360 (double a)
80 {
81 a = fmod (a, 360);
82 if (a < 0)
83 {
84 a += 360;
85 }
86
87 NS_ASSERT_MSG (0 <= a && a < 360, "Invalid wrap, a=" << a);
88 return a;
89 }
90
91
92 double
WrapTo180(double a)93 WrapTo180 (double a)
94 {
95 a = fmod (a + 180, 360);
96 if (a < 0)
97 {
98 a += 360;
99 }
100 a -= 180;
101
102 NS_ASSERT_MSG (-180 <= a && a < 180, "Invalid wrap, a=" << a);
103 return a;
104 }
105
106
107 double
WrapTo2Pi(double a)108 WrapTo2Pi (double a)
109 {
110 a = fmod (a, 2 * M_PI);
111 if (a < 0)
112 {
113 a += 2 * M_PI;
114 }
115
116 NS_ASSERT_MSG (0 <= a && a < 2 * M_PI, "Invalid wrap, a=" << a);
117 return a;
118 }
119
120
121 double
WrapToPi(double a)122 WrapToPi (double a)
123 {
124 a = fmod (a + M_PI, 2 * M_PI);
125 if (a < 0)
126 {
127 a += 2 * M_PI;
128 }
129 a -= M_PI;
130
131 NS_ASSERT_MSG (-M_PI <= a && a < M_PI, "Invalid wrap, a=" << a);
132 return a;
133 }
134
135
136 std::ostream&
operator <<(std::ostream & os,const Angles & a)137 operator<< (std::ostream& os, const Angles& a)
138 {
139 double azim, incl;
140 std::string unit;
141
142 if (a.m_printDeg)
143 {
144 azim = RadiansToDegrees (a.m_azimuth);
145 incl = RadiansToDegrees (a.m_inclination);
146 unit = "deg";
147 }
148 else
149 {
150 azim = a.m_azimuth;
151 incl = a.m_inclination;
152 unit = "rad";
153 }
154
155 os << "(" << azim << ", " << incl << ") " << unit;
156 return os;
157 }
158
159 std::istream&
operator >>(std::istream & is,Angles & a)160 operator>> (std::istream& is, Angles& a)
161 {
162 char c;
163 is >> a.m_azimuth >> c >> a.m_inclination;
164 if (c != ':')
165 {
166 is.setstate (std::ios_base::failbit);
167 }
168 return is;
169 }
170
171
Angles()172 Angles::Angles ()
173 : Angles (NAN, NAN)
174 {}
175
176
Angles(double azimuth,double inclination)177 Angles::Angles (double azimuth, double inclination)
178 : m_azimuth (azimuth),
179 m_inclination (inclination)
180 {
181 NormalizeAngles ();
182 }
183
184
Angles(Vector v)185 Angles::Angles (Vector v)
186 : m_azimuth (std::atan2 (v.y, v.x)),
187 m_inclination (std::acos (v.z / v.GetLength ()))
188 {
189 // azimuth and inclination angles for zero-length vectors are not defined
190 if (v.x == 0.0 && v.y == 0.0 && v.z == 0.0)
191 {
192 m_azimuth = NAN;
193 m_inclination = NAN;
194 }
195
196 NormalizeAngles ();
197 }
198
Angles(Vector v,Vector o)199 Angles::Angles (Vector v, Vector o)
200 : Angles (v - o)
201 {}
202
203
204
205 void
SetAzimuth(double azimuth)206 Angles::SetAzimuth (double azimuth)
207 {
208 m_azimuth = azimuth;
209 NormalizeAngles ();
210 }
211
212
213 void
SetInclination(double inclination)214 Angles::SetInclination (double inclination)
215 {
216 m_inclination = inclination;
217 NormalizeAngles ();
218 }
219
220
221 double
GetAzimuth(void) const222 Angles::GetAzimuth (void) const
223 {
224 return m_azimuth;
225 }
226
227
228 double
GetInclination(void) const229 Angles::GetInclination (void) const
230 {
231 return m_inclination;
232 }
233
234
235 void
NormalizeAngles(void)236 Angles::NormalizeAngles (void)
237 {
238 CheckIfValid ();
239
240 // Normalize azimuth angle
241 if (std::isnan (m_azimuth))
242 {
243 return;
244 }
245
246 m_azimuth = WrapToPi (m_azimuth);
247 }
248
249
250 void
CheckIfValid(void) const251 Angles::CheckIfValid (void) const
252 {
253 if (std::isfinite (m_inclination) || std::isfinite (m_azimuth))
254 {
255 NS_ASSERT_MSG (0.0 <= m_inclination && m_inclination <= M_PI,
256 "m_inclination=" << m_inclination << " not valid, should be in [0, pi] rad");
257 }
258 else
259 {
260 // infinite or nan inclination or azimuth angle
261 NS_LOG_WARN ("Undefined angle: " << *this);
262 }
263
264 }
265
266 }
267
268