simgear/math/SGVec3.hxx

   1 // Copyright (C) 2006-2009  Mathias Froehlich - Mathias.Froehlich@web.de
   2 //
   3 // This library is free software; you can redistribute it and/or
   4 // modify it under the terms of the GNU Library General Public
   5 // License as published by the Free Software Foundation; either
   6 // version 2 of the License, or (at your option) any later version.
   7 //
   8 // This library is distributed in the hope that it will be useful,
   9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11 // Library General Public License for more details.
  12 //
  13 // You should have received a copy of the GNU General Public License
  14 // along with this program; if not, write to the Free Software
  15 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  16 //
  17
  18 #ifndef SGVec3_H
  19 #define SGVec3_H
  20
  21 #ifndef NO_OPENSCENEGRAPH_INTERFACE
  22 #include <osg/Vec3f>
  23 #include <osg/Vec3d>
  24 #endif
  25
  26 /// 3D Vector Class
  27 template<typename T>
  28 class SGVec3 {
  29 public:
  30   typedef T value_type;
  31
  32 #ifdef __GNUC__
  33 // Avoid "_data not initialized" warnings (see comment below).
  34 #   pragma GCC diagnostic push
  35 #   pragma GCC diagnostic ignored "-Wuninitialized"
  36 #endif
  37
  38   /// Default constructor. Does not initialize at all.
  39   /// If you need them zero initialized, use SGVec3::zeros()
  40   SGVec3(void)
  41   {
  42     /// Initialize with nans in the debug build, that will guarantee to have
  43     /// a fast uninitialized default constructor in the release but shows up
  44     /// uninitialized values in the debug build very fast ...
  45 #ifndef NDEBUG
  46     for (unsigned i = 0; i < 3; ++i)
  47       data()[i] = SGLimits<T>::quiet_NaN();
  48 #endif
  49   }
  50
  51 #ifdef __GNUC__
  52   // Restore warning settings.
  53 #   pragma GCC diagnostic pop
  54 #endif
  55
  56   /// Constructor. Initialize by the given values
  57   SGVec3(T x, T y, T z)
  58   { data()[0] = x; data()[1] = y; data()[2] = z; }
  59   /// Constructor. Initialize by the content of a plain array,
  60   /// make sure it has at least 3 elements
  61   explicit SGVec3(const T* d)
  62   { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; }
  63   template<typename S>
  64   explicit SGVec3(const SGVec3<S>& d)
  65   { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; }
  66   explicit SGVec3(const SGVec2<T>& v2, const T& v3 = 0)
  67   { data()[0] = v2[0]; data()[1] = v2[1]; data()[2] = v3; }
  68
  69   /// Access by index, the index is unchecked
  70   const T& operator()(unsigned i) const
  71   { return data()[i]; }
  72   /// Access by index, the index is unchecked
  73   T& operator()(unsigned i)
  74   { return data()[i]; }
  75
  76   /// Access raw data by index, the index is unchecked
  77   const T& operator[](unsigned i) const
  78   { return data()[i]; }
  79   /// Access raw data by index, the index is unchecked
  80   T& operator[](unsigned i)
  81   { return data()[i]; }
  82
  83   /// Access the x component
  84   const T& x(void) const
  85   { return data()[0]; }
  86   /// Access the x component
  87   T& x(void)
  88   { return data()[0]; }
  89   /// Access the y component
  90   const T& y(void) const
  91   { return data()[1]; }
  92   /// Access the y component
  93   T& y(void)
  94   { return data()[1]; }
  95   /// Access the z component
  96   const T& z(void) const
  97   { return data()[2]; }
  98   /// Access the z component
  99   T& z(void)
 100   { return data()[2]; }
 101
 102   /// Readonly raw storage interface
 103   const T (&data(void) const)[3]
 104   { return _data; }
 105   /// Readonly raw storage interface
 106   T (&data(void))[3]
 107   { return _data; }
 108
 109   /// Inplace addition
 110   SGVec3& operator+=(const SGVec3& v)
 111   { data()[0] += v(0); data()[1] += v(1); data()[2] += v(2); return *this; }
 112   /// Inplace subtraction
 113   SGVec3& operator-=(const SGVec3& v)
 114   { data()[0] -= v(0); data()[1] -= v(1); data()[2] -= v(2); return *this; }
 115   /// Inplace scalar multiplication
 116   template<typename S>
 117   SGVec3& operator*=(S s)
 118   { data()[0] *= s; data()[1] *= s; data()[2] *= s; return *this; }
 119   /// Inplace scalar multiplication by 1/s
 120   template<typename S>
 121   SGVec3& operator/=(S s)
 122   { return operator*=(1/T(s)); }
 123
 124   /// Return an all zero vector
 125   static SGVec3 zeros(void)
 126   { return SGVec3(0, 0, 0); }
 127   /// Return unit vectors
 128   static SGVec3 e1(void)
 129   { return SGVec3(1, 0, 0); }
 130   static SGVec3 e2(void)
 131   { return SGVec3(0, 1, 0); }
 132   static SGVec3 e3(void)
 133   { return SGVec3(0, 0, 1); }
 134
 135   /// Constructor. Initialize by a geodetic coordinate
 136   /// Note that this conversion is relatively expensive to compute
 137   static SGVec3 fromGeod(const SGGeod& geod);
 138   /// Constructor. Initialize by a geocentric coordinate
 139   /// Note that this conversion is relatively expensive to compute
 140   static SGVec3 fromGeoc(const SGGeoc& geoc);
 141
 142 private:
 143   T _data[3];
 144 };
 145
 146 template<>
 147 inline
 148 SGVec3<double>
 149 SGVec3<double>::fromGeod(const SGGeod& geod)
 150 {
 151   SGVec3<double> cart;
 152   SGGeodesy::SGGeodToCart(geod, cart);
 153   return cart;
 154 }
 155
 156 template<>
 157 inline
 158 SGVec3<float>
 159 SGVec3<float>::fromGeod(const SGGeod& geod)
 160 {
 161   SGVec3<double> cart;
 162   SGGeodesy::SGGeodToCart(geod, cart);
 163   return SGVec3<float>(cart(0), cart(1), cart(2));
 164 }
 165
 166 template<>
 167 inline
 168 SGVec3<double>
 169 SGVec3<double>::fromGeoc(const SGGeoc& geoc)
 170 {
 171   SGVec3<double> cart;
 172   SGGeodesy::SGGeocToCart(geoc, cart);
 173   return cart;
 174 }
 175
 176 template<>
 177 inline
 178 SGVec3<float>
 179 SGVec3<float>::fromGeoc(const SGGeoc& geoc)
 180 {
 181   SGVec3<double> cart;
 182   SGGeodesy::SGGeocToCart(geoc, cart);
 183   return SGVec3<float>(cart(0), cart(1), cart(2));
 184 }
 185
 186 /// Unary +, do nothing ...
 187 template<typename T>
 188 inline
 189 const SGVec3<T>&
 190 operator+(const SGVec3<T>& v)
 191 { return v; }
 192
 193 /// Unary -, do nearly nothing
 194 template<typename T>
 195 inline
 196 SGVec3<T>
 197 operator-(const SGVec3<T>& v)
 198 { return SGVec3<T>(-v(0), -v(1), -v(2)); }
 199
 200 /// Binary +
 201 template<typename T>
 202 inline
 203 SGVec3<T>
 204 operator+(const SGVec3<T>& v1, const SGVec3<T>& v2)
 205 { return SGVec3<T>(v1(0)+v2(0), v1(1)+v2(1), v1(2)+v2(2)); }
 206
 207 /// Binary -
 208 template<typename T>
 209 inline
 210 SGVec3<T>
 211 operator-(const SGVec3<T>& v1, const SGVec3<T>& v2)
 212 { return SGVec3<T>(v1(0)-v2(0), v1(1)-v2(1), v1(2)-v2(2)); }
 213
 214 /// Scalar multiplication
 215 template<typename S, typename T>
 216 inline
 217 SGVec3<T>
 218 operator*(S s, const SGVec3<T>& v)
 219 { return SGVec3<T>(s*v(0), s*v(1), s*v(2)); }
 220
 221 /// Scalar multiplication
 222 template<typename S, typename T>
 223 inline
 224 SGVec3<T>
 225 operator*(const SGVec3<T>& v, S s)
 226 { return SGVec3<T>(s*v(0), s*v(1), s*v(2)); }
 227
 228 /// multiplication as a multiplicator, that is assume that the first vector
 229 /// represents a 3x3 diagonal matrix with the diagonal elements in the vector.
 230 /// Then the result is the product of that matrix times the second vector.
 231 template<typename T>
 232 inline
 233 SGVec3<T>
 234 mult(const SGVec3<T>& v1, const SGVec3<T>& v2)
 235 { return SGVec3<T>(v1(0)*v2(0), v1(1)*v2(1), v1(2)*v2(2)); }
 236
 237 /// component wise min
 238 template<typename T>
 239 inline
 240 SGVec3<T>
 241 min(const SGVec3<T>& v1, const SGVec3<T>& v2)
 242 {
 243   return SGVec3<T>(SGMisc<T>::min(v1(0), v2(0)),
 244                    SGMisc<T>::min(v1(1), v2(1)),
 245                    SGMisc<T>::min(v1(2), v2(2)));
 246 }
 247 template<typename S, typename T>
 248 inline
 249 SGVec3<T>
 250 min(const SGVec3<T>& v, S s)
 251 {
 252   return SGVec3<T>(SGMisc<T>::min(s, v(0)),
 253                    SGMisc<T>::min(s, v(1)),
 254                    SGMisc<T>::min(s, v(2)));
 255 }
 256 template<typename S, typename T>
 257 inline
 258 SGVec3<T>
 259 min(S s, const SGVec3<T>& v)
 260 {
 261   return SGVec3<T>(SGMisc<T>::min(s, v(0)),
 262                    SGMisc<T>::min(s, v(1)),
 263                    SGMisc<T>::min(s, v(2)));
 264 }
 265
 266 /// component wise max
 267 template<typename T>
 268 inline
 269 SGVec3<T>
 270 max(const SGVec3<T>& v1, const SGVec3<T>& v2)
 271 {
 272   return SGVec3<T>(SGMisc<T>::max(v1(0), v2(0)),
 273                    SGMisc<T>::max(v1(1), v2(1)),
 274                    SGMisc<T>::max(v1(2), v2(2)));
 275 }
 276 template<typename S, typename T>
 277 inline
 278 SGVec3<T>
 279 max(const SGVec3<T>& v, S s)
 280 {
 281   return SGVec3<T>(SGMisc<T>::max(s, v(0)),
 282                    SGMisc<T>::max(s, v(1)),
 283                    SGMisc<T>::max(s, v(2)));
 284 }
 285 template<typename S, typename T>
 286 inline
 287 SGVec3<T>
 288 max(S s, const SGVec3<T>& v)
 289 {
 290   return SGVec3<T>(SGMisc<T>::max(s, v(0)),
 291                    SGMisc<T>::max(s, v(1)),
 292                    SGMisc<T>::max(s, v(2)));
 293 }
 294
 295 /// Scalar dot product
 296 template<typename T>
 297 inline
 298 T
 299 dot(const SGVec3<T>& v1, const SGVec3<T>& v2)
 300 { return v1(0)*v2(0) + v1(1)*v2(1) + v1(2)*v2(2); }
 301
 302 /// The euclidean norm of the vector, that is what most people call length
 303 template<typename T>
 304 inline
 305 T
 306 norm(const SGVec3<T>& v)
 307 { return sqrt(dot(v, v)); }
 308
 309 /// The euclidean norm of the vector, that is what most people call length
 310 template<typename T>
 311 inline
 312 T
 313 length(const SGVec3<T>& v)
 314 { return sqrt(dot(v, v)); }
 315
 316 /// The 1-norm of the vector, this one is the fastest length function we
 317 /// can implement on modern cpu's
 318 template<typename T>
 319 inline
 320 T
 321 norm1(const SGVec3<T>& v)
 322 { return fabs(v(0)) + fabs(v(1)) + fabs(v(2)); }
 323
 324 /// The inf-norm of the vector
 325 template<typename T>
 326 inline
 327 T
 328 normI(const SGVec3<T>& v)
 329 { return SGMisc<T>::max(fabs(v(0)), fabs(v(1)), fabs(v(2))); }
 330
 331 /// Vector cross product
 332 template<typename T>
 333 inline
 334 SGVec3<T>
 335 cross(const SGVec3<T>& v1, const SGVec3<T>& v2)
 336 {
 337   return SGVec3<T>(v1(1)*v2(2) - v1(2)*v2(1),
 338                    v1(2)*v2(0) - v1(0)*v2(2),
 339                    v1(0)*v2(1) - v1(1)*v2(0));
 340 }
 341
 342 /// return any normalized vector perpendicular to v
 343 template<typename T>
 344 inline
 345 SGVec3<T>
 346 perpendicular(const SGVec3<T>& v)
 347 {
 348   T absv1 = fabs(v(0));
 349   T absv2 = fabs(v(1));
 350   T absv3 = fabs(v(2));
 351
 352   if (absv2 < absv1 && absv3 < absv1) {
 353     T quot = v(1)/v(0);
 354     return (1/sqrt(1+quot*quot))*SGVec3<T>(quot, -1, 0);
 355   } else if (absv3 < absv2) {
 356     T quot = v(2)/v(1);
 357     return (1/sqrt(1+quot*quot))*SGVec3<T>(0, quot, -1);
 358   } else if (SGLimits<T>::min() < absv3) {
 359     T quot = v(0)/v(2);
 360     return (1/sqrt(1+quot*quot))*SGVec3<T>(-1, 0, quot);
 361   } else {
 362     // the all zero case ...
 363     return SGVec3<T>(0, 0, 0);
 364   }
 365 }
 366
 367 /// Construct a unit vector in the given direction.
 368 /// or the zero vector if the input vector is zero.
 369 template<typename T>
 370 inline
 371 SGVec3<T>
 372 normalize(const SGVec3<T>& v)
 373 {
 374   T normv = norm(v);
 375   if (normv <= SGLimits<T>::min())
 376     return SGVec3<T>::zeros();
 377   return (1/normv)*v;
 378 }
 379
 380 /// Return true if exactly the same
 381 template<typename T>
 382 inline
 383 bool
 384 operator==(const SGVec3<T>& v1, const SGVec3<T>& v2)
 385 { return v1(0) == v2(0) && v1(1) == v2(1) && v1(2) == v2(2); }
 386
 387 /// Return true if not exactly the same
 388 template<typename T>
 389 inline
 390 bool
 391 operator!=(const SGVec3<T>& v1, const SGVec3<T>& v2)
 392 { return ! (v1 == v2); }
 393
 394 /// Return true if smaller, good for putting that into a std::map
 395 template<typename T>
 396 inline
 397 bool
 398 operator<(const SGVec3<T>& v1, const SGVec3<T>& v2)
 399 {
 400   if (v1(0) < v2(0)) return true;
 401   else if (v2(0) < v1(0)) return false;
 402   else if (v1(1) < v2(1)) return true;
 403   else if (v2(1) < v1(1)) return false;
 404   else return (v1(2) < v2(2));
 405 }
 406
 407 template<typename T>
 408 inline
 409 bool
 410 operator<=(const SGVec3<T>& v1, const SGVec3<T>& v2)
 411 {
 412   if (v1(0) < v2(0)) return true;
 413   else if (v2(0) < v1(0)) return false;
 414   else if (v1(1) < v2(1)) return true;
 415   else if (v2(1) < v1(1)) return false;
 416   else return (v1(2) <= v2(2));
 417 }
 418
 419 template<typename T>
 420 inline
 421 bool
 422 operator>(const SGVec3<T>& v1, const SGVec3<T>& v2)
 423 { return operator<(v2, v1); }
 424
 425 template<typename T>
 426 inline
 427 bool
 428 operator>=(const SGVec3<T>& v1, const SGVec3<T>& v2)
 429 { return operator<=(v2, v1); }
 430
 431 /// Return true if equal to the relative tolerance tol
 432 template<typename T>
 433 inline
 434 bool
 435 equivalent(const SGVec3<T>& v1, const SGVec3<T>& v2, T rtol, T atol)
 436 { return norm1(v1 - v2) < rtol*(norm1(v1) + norm1(v2)) + atol; }
 437
 438 /// Return true if equal to the relative tolerance tol
 439 template<typename T>
 440 inline
 441 bool
 442 equivalent(const SGVec3<T>& v1, const SGVec3<T>& v2, T rtol)
 443 { return norm1(v1 - v2) < rtol*(norm1(v1) + norm1(v2)); }
 444
 445 /// Return true if about equal to roundoff of the underlying type
 446 template<typename T>
 447 inline
 448 bool
 449 equivalent(const SGVec3<T>& v1, const SGVec3<T>& v2)
 450 {
 451   T tol = 100*SGLimits<T>::epsilon();
 452   return equivalent(v1, v2, tol, tol);
 453 }
 454
 455 /// The euclidean distance of the two vectors
 456 template<typename T>
 457 inline
 458 T
 459 dist(const SGVec3<T>& v1, const SGVec3<T>& v2)
 460 { return norm(v1 - v2); }
 461
 462 /// The squared euclidean distance of the two vectors
 463 template<typename T>
 464 inline
 465 T
 466 distSqr(const SGVec3<T>& v1, const SGVec3<T>& v2)
 467 { SGVec3<T> tmp = v1 - v2; return dot(tmp, tmp); }
 468
 469 // calculate the projection of u along the direction of d.
 470 template<typename T>
 471 inline
 472 SGVec3<T>
 473 projection(const SGVec3<T>& u, const SGVec3<T>& d)
 474 {
 475   T denom = dot(d, d);
 476   T ud = dot(u, d);
 477   if (SGLimits<T>::min() < denom) return u;
 478   else return d * (dot(u, d) / denom);
 479 }
 480
 481 #ifndef NDEBUG
 482 template<typename T>
 483 inline
 484 bool
 485 isNaN(const SGVec3<T>& v)
 486 {
 487   return SGMisc<T>::isNaN(v(0)) ||
 488     SGMisc<T>::isNaN(v(1)) || SGMisc<T>::isNaN(v(2));
 489 }
 490 #endif
 491
 492 /// Output to an ostream
 493 template<typename char_type, typename traits_type, typename T>
 494 inline
 495 std::basic_ostream<char_type, traits_type>&
 496 operator<<(std::basic_ostream<char_type, traits_type>& s, const SGVec3<T>& v)
 497 { return s << "[ " << v(0) << ", " << v(1) << ", " << v(2) << " ]"; }
 498
 499 inline
 500 SGVec3f
 501 toVec3f(const SGVec3d& v)
 502 { return SGVec3f((float)v(0), (float)v(1), (float)v(2)); }
 503
 504 inline
 505 SGVec3d
 506 toVec3d(const SGVec3f& v)
 507 { return SGVec3d(v(0), v(1), v(2)); }
 508
 509 #ifndef NO_OPENSCENEGRAPH_INTERFACE
 510 inline
 511 SGVec3d
 512 toSG(const osg::Vec3d& v)
 513 { return SGVec3d(v[0], v[1], v[2]); }
 514
 515 inline
 516 SGVec3f
 517 toSG(const osg::Vec3f& v)
 518 { return SGVec3f(v[0], v[1], v[2]); }
 519
 520 inline
 521 osg::Vec3d
 522 toOsg(const SGVec3d& v)
 523 { return osg::Vec3d(v[0], v[1], v[2]); }
 524
 525 inline
 526 osg::Vec3f
 527 toOsg(const SGVec3f& v)
 528 { return osg::Vec3f(v[0], v[1], v[2]); }
 529 #endif
 530
 531 #endif