fgms 0.11.8
The
FlightGear MultiPlayer Server
project
fg_geometry.cxx
Go to the documentation of this file.
1 /**
2  * @file fg_geometry.cxx
3  */
4 // This program is free software; you can redistribute it and/or
5 // modify it under the terms of the GNU General Public License as
6 // published by the Free Software Foundation; either version 2 of the
7 // License, or (at your option) any later version.
8 //
9 // This program is distributed in the hope that it will be useful, but
10 // WITHOUT ANY WARRANTY; without even the implied warranty of
11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 // 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, U$
17 //
18 
19 //////////////////////////////////////////////////////////////////////
20 //
21 // Server for FlightGear, geometry functions
22 // taken from simgear and other sources
23 //
24 //////////////////////////////////////////////////////////////////////
25 
26 #include <math.h>
27 #ifndef _MSC_VER
28  #include <strings.h>
29 #endif // _MSC_VER
30 #include <assert.h>
31 #include "fg_geometry.hxx"
32 
33 /**
34  * High-precision versions of the above produced with an arbitrary
35  * precision calculator (the compiler might lose a few bits in the FPU
36  * operations). These are specified to 81 bits of mantissa, which is
37  * higher than any FPU known to me:
38  */
39 static const double SQUASH = 0.9966471893352525192801545;
40 static const double STRETCH = 1.0033640898209764189003079;
41 static const double POLRAD = 6356752.3142451794975639668;
42 
43 /** @brief Radians To Nautical Miles */
44 static const double SG_RAD_TO_NM = 3437.7467707849392526;
45 
46 /** @brief Nautical Miles in a Meter */
47 static const double SG_NM_TO_METER = 1852.0000;
48 
49 /** @brief Meters to Feet */
50 static const double SG_METER_TO_FEET = 3.28083989501312335958;
51 
52 /** @brief PI2 */
53 static const double SGD_PI_2 = 1.57079632679489661923;
54 
56 {
57  clear ();
58 }
59 
61 {
62  m_X = P.m_X;
63  m_Y = P.m_Y;
64  m_Z = P.m_Z;
65 }
66 
67 Point3D::Point3D ( const t_Point3D& X, const t_Point3D& Y, const t_Point3D& Z )
68 {
69  m_X = X;
70  m_Y = Y;
71  m_Z = Z;
72 }
73 
74 void Point3D::Set ( const t_Point3D& X, const t_Point3D& Y,
75  const t_Point3D& Z )
76 {
77  m_X = X;
78  m_Y = Y;
79  m_Z = Z;
80 }
81 
82 void
84 {
85  Point3D tmp (*this);
86  t_Point3D sqr_X = m_X*m_X;
87  t_Point3D sqr_Y = m_Y*m_Y;
88  t_Point3D sqr_Z = m_Z*m_Z;
89 
90  m_X = atan2 (tmp[Y], tmp[X]);
91  m_Y = SGD_PI_2 - atan2( sqrt(sqr_X + sqr_Y), tmp[Z]);
92  m_Z = sqrt(sqr_X + sqr_Y + sqr_Z);
93 }
94 
95 void
97 {
98  double dummy = cos (m_X) * m_Z;
99 
100  m_X = cos (m_Y) * dummy;
101  m_Y = sin (m_Y) * dummy;
102  m_Z = sin (m_X) * m_Z;
103 }
104 
106 {
107  m_X = P.m_X;
108  m_Y = P.m_Y;
109  m_Z = P.m_Z;
110 }
111 
113 {
114  m_X = P[0];
115  m_Y = P[1];
116  m_Z = P[2];
117 }
118 
120 {
121  m_X += P.m_X;
122  m_Y += P.m_Y;
123  m_Z += P.m_Z;
124 }
125 
127 {
128  m_X -= P.m_X;
129  m_Y -= P.m_Y;
130  m_Z -= P.m_Z;
131 }
132 
134 {
135  m_X *= P.m_X;
136  m_Y *= P.m_Y;
137  m_Z *= P.m_Z;
138 }
139 
141 {
142  m_X /= P.m_X;
143  m_Y /= P.m_Y;
144  m_Z /= P.m_Z;
145 }
146 
148 {
149  m_X = (m_Y * P.m_Z) - (m_Z * P.m_Y);
150  m_Y = (m_Z * P.m_X) - (m_Z * P.m_Z);
151  m_Z = (m_X * P.m_Y) - (m_Z * P.m_X);
152 
153 }
154 
156 {
157  m_X *= nV;
158  m_Y *= nV;
159  m_Z *= nV;
160 }
161 
163 {
164  m_X /= nV;
165  m_Y /= nV;
166  m_Z /= nV;
167 }
168 
170 {
171  if ((m_X == P.m_X) && (m_Y == P.m_Y) && (m_Z == P.m_Z))
172  return (true);
173  return (false);
174 }
175 
177 {
178  if ((m_X == P.m_X) && (m_Y == P.m_Y) && (m_Z == P.m_Z))
179  return (false);
180  return (true);
181 }
182 
183 t_Point3D Point3D::operator[] ( const int Index ) const
184 {
185  assert (! (Index < X || Index > Z));
186  switch (Index)
187  {
188  case X: return m_X;
189  case Y: return m_Y;
190  case Z: return m_Z;
191  }
192  return (0.0); // never reached
193 }
194 
195 t_Point3D& Point3D::operator[] ( const int Index )
196 {
197  assert (! (Index < X || Index > Z));
198  switch (Index)
199  {
200  case X: return m_X;
201  case Y: return m_Y;
202  case Z: return m_Z;
203  }
204  return (m_X);
205 }
206 
208 {
209  return (sqrt ((m_X * m_X) + (m_Y * m_Y) + (m_Z * m_Z)));
210 }
211 
213 {
214  t_Point3D len;
215 
216  len = length();
217  m_X /= len;
218  m_Y /= len;
219  m_Z /= len;
220 }
221 
223 {
224  return ((m_X * m_X) + (m_Y * m_Y) + (m_Z * m_Z));
225 }
226 
228 {
229  m_X = -m_X;
230  m_Y = -m_Y;
231  m_Z = -m_Z;
232 }
233 
235 {
236  m_X = 0.0;
237  m_Y = 0.0;
238  m_Z = 0.0;
239 }
240 
241 Point3D operator + ( const Point3D& P1, const Point3D& P2 )
242 {
243  return (Point3D (
244  P1.m_X + P2.m_X,
245  P1.m_Y + P2.m_Y,
246  P1.m_Z + P2.m_Z
247  ));
248 }
249 
250 Point3D operator - ( const Point3D& P1, const Point3D& P2 )
251 {
252  return (Point3D (
253  P1.m_X - P2.m_X,
254  P1.m_Y - P2.m_Y,
255  P1.m_Z - P2.m_Z
256  ));
257 }
258 
259 Point3D operator * ( const Point3D& P1, const Point3D& P2 )
260 {
261  return (Point3D (
262  P1.m_X * P2.m_X,
263  P1.m_Y * P2.m_Y,
264  P1.m_Z * P2.m_Z
265  ));
266 }
267 
268 Point3D operator / ( const Point3D& P1, const Point3D& P2 )
269 {
270  return (Point3D (
271  P1.m_X / P2.m_X,
272  P1.m_Y / P2.m_Y,
273  P1.m_Z / P2.m_Z
274  ));
275 }
276 
277 Point3D operator ^ ( const Point3D& P1, const Point3D& P2 )
278 {
279  return (Point3D (
280  (P1.m_Y * P2.m_Z) - (P1.m_Z * P2.m_Y),
281  (P1.m_Z * P2.m_X) - (P1.m_Z * P2.m_Z),
282  (P1.m_X * P2.m_Y) - (P1.m_Z * P2.m_X)
283  ));
284 }
285 
286 Point3D operator * ( const t_Point3D& nV, const Point3D& P1 )
287 {
288  return (Point3D (
289  P1.m_X * nV,
290  P1.m_Y * nV,
291  P1.m_Z * nV
292  ));
293 }
294 
295 Point3D operator / ( const t_Point3D& nV, const Point3D& P1 )
296 {
297  return (Point3D (
298  P1.m_X / nV,
299  P1.m_Y / nV,
300  P1.m_Z / nV
301  ));
302 }
303 
305 {
306  return (Point3D (
307  -P.m_X,
308  -P.m_Y,
309  -P.m_Z
310  ));
311 }
312 
313 t_Point3D sqr ( const Point3D& P )
314 {
315  return (
316  (P.m_X * P.m_X) +
317  (P.m_Y * P.m_Y) +
318  (P.m_Z * P.m_Z)
319  );
320 }
321 
322 
323 
324 //////////////////////////////////////////////////////////////////////
325 /**
326  * @brief Normalize P
327  */
328 Point3D
329 normalize ( const Point3D& P )
330 {
331  t_Point3D len;
332 
333  len = P.length();
334  return (Point3D (
335  (P.m_X / len),
336  (P.m_Y / len),
337  (P.m_Z / len)
338  ));
339 } // normalize ( const Point3D& P )
340 
341 
342 
343 //////////////////////////////////////////////////////////////////////
344 /**
345  * @brief Return the length of P
346  */
347 t_Point3D
348 length ( const Point3D& P )
349 {
350  return (sqrt (
351  (P.m_X * P.m_X) +
352  (P.m_Y * P.m_Y) +
353  (P.m_Z * P.m_Z)
354  ));
355 } // length ( const Point3D& P )
356 
357 
358 
359 
360 //////////////////////////////////////////////////////////////////////
361 /**
362  * @brief Calculate distance of clients
363  */
364 float
365 Distance ( const Point3D & P1, const Point3D & P2 )
366 {
367  Point3D P;
368 
369  P = P1 - P2;
370  return (float)(P.length() / SG_NM_TO_METER);
371 } // Distance ( const Point3D & P1, const Point3D & P2 )
372 
373 
374 
375 //////////////////////////////////////////////////////////////////////
376 /**
377  * @brief This is the inverse of the algorithm in localLat(). It
378  * returns the (cylindrical) coordinates of a surface latitude
379  * expressed as an "up" unit vector.
380  */
381 static void
382 surfRZ (double upr, double upz, double* r, double* z)
383 {
384  // We are
385  // converting a (2D, cylindrical) "up" vector defined by the
386  // geodetic latitude into unitless R and Z coordinates in
387  // cartesian space.
388  double R = upr * STRETCH;
389  double Z = upz * SQUASH;
390  // Now we need to turn R and Z into a surface point. That is,
391  // pick a coefficient C for them such that the point is on the
392  // surface when converted to "squashed" space:
393  // (C*R*SQUASH)^2 + (C*Z)^2 = POLRAD^2
394  // C^2 = POLRAD^2 / ((R*SQUASH)^2 + Z^2)
395  double sr = R * SQUASH;
396  double c = POLRAD / sqrt(sr*sr + Z*Z);
397  R *= c;
398  Z *= c;
399  *r = R;
400  *z = Z;
401 } // surfRZ()
402 
403 
404 
405 //////////////////////////////////////////////////////////////////////
406 //
407 // Convert a cartexian XYZ coordinate to a geodetic lat/lon/alt.
408 // This function is a copy of what's in SimGear,
409 // simgear/math/SGGeodesy.cxx.
410 //
411 ////////////////////////////////////////////////////////////////////////
412 
413 #define _EQURAD (6378137.0)
414 #define E2 fabs(1 - SQUASH*SQUASH)
415 static double ra2 = 1/(_EQURAD*_EQURAD);
416 static double e2 = E2;
417 static double e4 = E2*E2;
418 
419 /**
420  * @brief Convert a cartexian XYZ coordinate to a geodetic lat/lon/alt.
421  * This function is a copy of what's in SimGear,
422  * simgear/math/SGGeodesy.cxx
423  */
424 void
425 sgCartToGeod ( const Point3D& CartPoint , Point3D& GeodPoint )
426 {
427  // according to
428  // H. Vermeille,
429  // Direct transformation from geocentric to geodetic cordinates,
430  // Journal of Geodesy (2002) 76:451-454
431  double x = CartPoint[X];
432  double y = CartPoint[Y];
433  double z = CartPoint[Z];
434  double XXpYY = x*x+y*y;
435  double sqrtXXpYY = sqrt(XXpYY);
436  double p = XXpYY*ra2;
437  double q = z*z*(1-e2)*ra2;
438  double r = 1/6.0*(p+q-e4);
439  double s = e4*p*q/(4*r*r*r);
440  double t = pow(1+s+sqrt(s*(2+s)), 1/3.0);
441  double u = r*(1+t+1/t);
442  double v = sqrt(u*u+e4*q);
443  double w = e2*(u+v-q)/(2*v);
444  double k = sqrt(u+v+w*w)-w;
445  double D = k*sqrtXXpYY/(k+e2);
446  GeodPoint[Lon] = (2*atan2(y, x+sqrtXXpYY)) * SG_RADIANS_TO_DEGREES;
447  double sqrtDDpZZ = sqrt(D*D+z*z);
448  GeodPoint[Lat] = (2*atan2(z, D+sqrtDDpZZ)) * SG_RADIANS_TO_DEGREES;
449  GeodPoint[Alt] = ((k+e2-1)*sqrtDDpZZ/k) * SG_METER_TO_FEET;
450 } // sgCartToGeod()
451 
452 
453 
454 //////////////////////////////////////////////////////////////////////
455 /**
456  * @brief Opposite of sgCartToGeod
457  */
458 void sgGeodToCart ( double lat, double lon, double alt, double* xyz )
459 {
460  // This is the inverse of the algorithm in localLat(). We are
461  // converting a (2D, cylindrical) "up" vector defined by the
462  // geodetic latitude into unitless R and Z coordinates in
463  // cartesian space.
464  double upr = cos(lat);
465  double upz = sin(lat);
466  double r, z;
467  surfRZ(upr, upz, &r, &z);
468 
469  // Add the altitude using the "up" unit vector we calculated
470  // initially.
471  r += upr * alt;
472  z += upz * alt;
473 
474  // Finally, convert from cylindrical to cartesian
475  xyz[0] = r * cos(lon);
476  xyz[1] = r * sin(lon);
477  xyz[2] = z;
478 } // sgGeodToCart()
479 //////////////////////////////////////////////////////////////////////
480 
static double ra2
t_Point3D sqr()
static double e4
t_Point3D m_Y
Definition: fg_geometry.hxx:93
Point3D operator/(const Point3D &P1, const Point3D &P2)
static char D[28]
Definition: crypt-win.c:87
void sgCartToGeod(const Point3D &CartPoint, Point3D &GeodPoint)
Convert a cartexian XYZ coordinate to a geodetic lat/lon/alt. This function is a copy of what's in Si...
void operator*=(const Point3D &P)
bool operator!=(const Point3D &P)
void operator=(const Point3D &P)
t_Point3D operator[](const int Index) const
t_Point3D length() const
void operator-=(const Point3D &P)
bool operator==(const Point3D &P)
static const double SG_NM_TO_METER
Nautical Miles in a Meter.
Definition: fg_geometry.cxx:47
void operator^=(const Point3D &P)
double sgdVec3[3]
Definition: SGMath.hxx:8
static char P[]
Definition: crypt-win.c:209
#define E2
void PolarToCart()
Definition: fg_geometry.cxx:96
Point3D operator^(const Point3D &P1, const Point3D &P2)
static const double SQUASH
Definition: fg_geometry.cxx:39
Point3D operator+(const Point3D &P1, const Point3D &P2)
static double e2
Point3D operator-(const Point3D &P1, const Point3D &P2)
static const double STRETCH
Definition: fg_geometry.cxx:40
void clear()
t_Point3D m_X
Definition: fg_geometry.hxx:92
Point3D operator*(const Point3D &P1, const Point3D &P2)
Point3D invert(const Point3D &P)
t_Point3D m_Z
Definition: fg_geometry.hxx:94
void operator+=(const Point3D &P)
#define _EQURAD
static const double SG_RAD_TO_NM
Radians To Nautical Miles.
Definition: fg_geometry.cxx:44
static char R[32]
Definition: crypt-win.c:223
void normalize()
static const double SG_METER_TO_FEET
Meters to Feet.
Definition: fg_geometry.cxx:50
static const double SGD_PI_2
PI2.
Definition: fg_geometry.cxx:53
void CartToPolar()
Definition: fg_geometry.cxx:83
void sgGeodToCart(double lat, double lon, double alt, double *xyz)
Opposite of sgCartToGeod.
void operator/=(const Point3D &P)
double t_Point3D
Definition: fg_geometry.hxx:35
t_Point3D sqr(const Point3D &P)
t_Point3D length(const Point3D &P)
Return the length of P.
void Set(const t_Point3D &X, const t_Point3D &Y, const t_Point3D &Z)
Definition: fg_geometry.cxx:74
Point3D normalize(const Point3D &P)
Normalize P.
static void surfRZ(double upr, double upz, double *r, double *z)
This is the inverse of the algorithm in localLat(). It returns the (cylindrical) coordinates of a sur...
float Distance(const Point3D &P1, const Point3D &P2)
Calculate distance of clients.
void invert()
#define SG_RADIANS_TO_DEGREES
Definition: fg_geometry.hxx:31
static const double POLRAD
Definition: fg_geometry.cxx:41