simgear/math/SGVec2.hxx

   1 // Copyright (C) 2006  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 SGVec2_H
  19 #define SGVec2_H
  20
  21 #include <osg/Vec2f>
  22 #include <osg/Vec2d>
  23
  24 template<typename T>
  25 struct SGVec2Storage {
  26   /// Readonly raw storage interface
  27   const T (&data(void) const)[2]
  28   { return _data; }
  29   /// Readonly raw storage interface
  30   T (&data(void))[2]
  31   { return _data; }
  32
  33   void osg() const
  34   { }
  35
  36 private:
  37   T _data[2];
  38 };
  39
  40 template<>
  41 struct SGVec2Storage<float> : public osg::Vec2f {
  42   /// Access raw data by index, the index is unchecked
  43   const float (&data(void) const)[2]
  44   { return osg::Vec2f::_v; }
  45   /// Access raw data by index, the index is unchecked
  46   float (&data(void))[2]
  47   { return osg::Vec2f::_v; }
  48
  49   const osg::Vec2f& osg() const
  50   { return *this; }
  51   osg::Vec2f& osg()
  52   { return *this; }
  53 };
  54
  55 template<>
  56 struct SGVec2Storage<double> : public osg::Vec2d {
  57   /// Access raw data by index, the index is unchecked
  58   const double (&data(void) const)[2]
  59   { return osg::Vec2d::_v; }
  60   /// Access raw data by index, the index is unchecked
  61   double (&data(void))[2]
  62   { return osg::Vec2d::_v; }
  63
  64   const osg::Vec2d& osg() const
  65   { return *this; }
  66   osg::Vec2d& osg()
  67   { return *this; }
  68 };
  69
  70 /// 2D Vector Class
  71 template<typename T>
  72 class SGVec2 : protected SGVec2Storage<T> {
  73 public:
  74   typedef T value_type;
  75
  76   /// Default constructor. Does not initialize at all.
  77   /// If you need them zero initialized, use SGVec2::zeros()
  78   SGVec2(void)
  79   {
  80     /// Initialize with nans in the debug build, that will guarantee to have
  81     /// a fast uninitialized default constructor in the release but shows up
  82     /// uninitialized values in the debug build very fast ...
  83 #ifndef NDEBUG
  84     for (unsigned i = 0; i < 2; ++i)
  85       data()[i] = SGLimits<T>::quiet_NaN();
  86 #endif
  87   }
  88   /// Constructor. Initialize by the given values
  89   SGVec2(T x, T y)
  90   { data()[0] = x; data()[1] = y; }
  91   /// Constructor. Initialize by the content of a plain array,
  92   /// make sure it has at least 2 elements
  93   explicit SGVec2(const T* d)
  94   { data()[0] = d[0]; data()[1] = d[1]; }
  95   explicit SGVec2(const osg::Vec2f& d)
  96   { data()[0] = d[0]; data()[1] = d[1]; }
  97   explicit SGVec2(const osg::Vec2d& d)
  98   { data()[0] = d[0]; data()[1] = d[1]; }
  99
 100   /// Access by index, the index is unchecked
 101   const T& operator()(unsigned i) const
 102   { return data()[i]; }
 103   /// Access by index, the index is unchecked
 104   T& operator()(unsigned i)
 105   { return data()[i]; }
 106
 107   /// Access raw data by index, the index is unchecked
 108   const T& operator[](unsigned i) const
 109   { return data()[i]; }
 110   /// Access raw data by index, the index is unchecked
 111   T& operator[](unsigned i)
 112   { return data()[i]; }
 113
 114   /// Access the x component
 115   const T& x(void) const
 116   { return data()[0]; }
 117   /// Access the x component
 118   T& x(void)
 119   { return data()[0]; }
 120   /// Access the y component
 121   const T& y(void) const
 122   { return data()[1]; }
 123   /// Access the y component
 124   T& y(void)
 125   { return data()[1]; }
 126
 127   /// Get the data pointer
 128   using SGVec2Storage<T>::data;
 129
 130   /// Readonly interface function to ssg's sgVec2/sgdVec2
 131   const T (&sg(void) const)[2]
 132   { return data(); }
 133   /// Interface function to ssg's sgVec2/sgdVec2
 134   T (&sg(void))[2]
 135   { return data(); }
 136
 137   /// Interface function to osg's Vec2*
 138   using SGVec2Storage<T>::osg;
 139
 140   /// Inplace addition
 141   SGVec2& operator+=(const SGVec2& v)
 142   { data()[0] += v(0); data()[1] += v(1); return *this; }
 143   /// Inplace subtraction
 144   SGVec2& operator-=(const SGVec2& v)
 145   { data()[0] -= v(0); data()[1] -= v(1); return *this; }
 146   /// Inplace scalar multiplication
 147   template<typename S>
 148   SGVec2& operator*=(S s)
 149   { data()[0] *= s; data()[1] *= s; return *this; }
 150   /// Inplace scalar multiplication by 1/s
 151   template<typename S>
 152   SGVec2& operator/=(S s)
 153   { return operator*=(1/T(s)); }
 154
 155   /// Return an all zero vector
 156   static SGVec2 zeros(void)
 157   { return SGVec2(0, 0); }
 158   /// Return unit vectors
 159   static SGVec2 e1(void)
 160   { return SGVec2(1, 0); }
 161   static SGVec2 e2(void)
 162   { return SGVec2(0, 1); }
 163 };
 164
 165 /// Unary +, do nothing ...
 166 template<typename T>
 167 inline
 168 const SGVec2<T>&
 169 operator+(const SGVec2<T>& v)
 170 { return v; }
 171
 172 /// Unary -, do nearly nothing
 173 template<typename T>
 174 inline
 175 SGVec2<T>
 176 operator-(const SGVec2<T>& v)
 177 { return SGVec2<T>(-v(0), -v(1)); }
 178
 179 /// Binary +
 180 template<typename T>
 181 inline
 182 SGVec2<T>
 183 operator+(const SGVec2<T>& v1, const SGVec2<T>& v2)
 184 { return SGVec2<T>(v1(0)+v2(0), v1(1)+v2(1)); }
 185
 186 /// Binary -
 187 template<typename T>
 188 inline
 189 SGVec2<T>
 190 operator-(const SGVec2<T>& v1, const SGVec2<T>& v2)
 191 { return SGVec2<T>(v1(0)-v2(0), v1(1)-v2(1)); }
 192
 193 /// Scalar multiplication
 194 template<typename S, typename T>
 195 inline
 196 SGVec2<T>
 197 operator*(S s, const SGVec2<T>& v)
 198 { return SGVec2<T>(s*v(0), s*v(1)); }
 199
 200 /// Scalar multiplication
 201 template<typename S, typename T>
 202 inline
 203 SGVec2<T>
 204 operator*(const SGVec2<T>& v, S s)
 205 { return SGVec2<T>(s*v(0), s*v(1)); }
 206
 207 /// multiplication as a multiplicator, that is assume that the first vector
 208 /// represents a 2x2 diagonal matrix with the diagonal elements in the vector.
 209 /// Then the result is the product of that matrix times the second vector.
 210 template<typename T>
 211 inline
 212 SGVec2<T>
 213 mult(const SGVec2<T>& v1, const SGVec2<T>& v2)
 214 { return SGVec2<T>(v1(0)*v2(0), v1(1)*v2(1)); }
 215
 216 /// component wise min
 217 template<typename T>
 218 inline
 219 SGVec2<T>
 220 min(const SGVec2<T>& v1, const SGVec2<T>& v2)
 221 {return SGVec2<T>(SGMisc<T>::min(v1(0), v2(0)), SGMisc<T>::min(v1(1), v2(1)));}
 222 template<typename S, typename T>
 223 inline
 224 SGVec2<T>
 225 min(const SGVec2<T>& v, S s)
 226 { return SGVec2<T>(SGMisc<T>::min(s, v(0)), SGMisc<T>::min(s, v(1))); }
 227 template<typename S, typename T>
 228 inline
 229 SGVec2<T>
 230 min(S s, const SGVec2<T>& v)
 231 { return SGVec2<T>(SGMisc<T>::min(s, v(0)), SGMisc<T>::min(s, v(1))); }
 232
 233 /// component wise max
 234 template<typename T>
 235 inline
 236 SGVec2<T>
 237 max(const SGVec2<T>& v1, const SGVec2<T>& v2)
 238 {return SGVec2<T>(SGMisc<T>::max(v1(0), v2(0)), SGMisc<T>::max(v1(1), v2(1)));}
 239 template<typename S, typename T>
 240 inline
 241 SGVec2<T>
 242 max(const SGVec2<T>& v, S s)
 243 { return SGVec2<T>(SGMisc<T>::max(s, v(0)), SGMisc<T>::max(s, v(1))); }
 244 template<typename S, typename T>
 245 inline
 246 SGVec2<T>
 247 max(S s, const SGVec2<T>& v)
 248 { return SGVec2<T>(SGMisc<T>::max(s, v(0)), SGMisc<T>::max(s, v(1))); }
 249
 250 /// Scalar dot product
 251 template<typename T>
 252 inline
 253 T
 254 dot(const SGVec2<T>& v1, const SGVec2<T>& v2)
 255 { return v1(0)*v2(0) + v1(1)*v2(1); }
 256
 257 /// The euclidean norm of the vector, that is what most people call length
 258 template<typename T>
 259 inline
 260 T
 261 norm(const SGVec2<T>& v)
 262 { return sqrt(dot(v, v)); }
 263
 264 /// The euclidean norm of the vector, that is what most people call length
 265 template<typename T>
 266 inline
 267 T
 268 length(const SGVec2<T>& v)
 269 { return sqrt(dot(v, v)); }
 270
 271 /// The 1-norm of the vector, this one is the fastest length function we
 272 /// can implement on modern cpu's
 273 template<typename T>
 274 inline
 275 T
 276 norm1(const SGVec2<T>& v)
 277 { return fabs(v(0)) + fabs(v(1)); }
 278
 279 /// The inf-norm of the vector
 280 template<typename T>
 281 inline
 282 T
 283 normI(const SGVec2<T>& v)
 284 { return SGMisc<T>::max(fabs(v(0)), fabs(v(1))); }
 285
 286 /// The euclidean norm of the vector, that is what most people call length
 287 template<typename T>
 288 inline
 289 SGVec2<T>
 290 normalize(const SGVec2<T>& v)
 291 { return (1/norm(v))*v; }
 292
 293 /// Return true if exactly the same
 294 template<typename T>
 295 inline
 296 bool
 297 operator==(const SGVec2<T>& v1, const SGVec2<T>& v2)
 298 { return v1(0) == v2(0) && v1(1) == v2(1); }
 299
 300 /// Return true if not exactly the same
 301 template<typename T>
 302 inline
 303 bool
 304 operator!=(const SGVec2<T>& v1, const SGVec2<T>& v2)
 305 { return ! (v1 == v2); }
 306
 307 /// Return true if smaller, good for putting that into a std::map
 308 template<typename T>
 309 inline
 310 bool
 311 operator<(const SGVec2<T>& v1, const SGVec2<T>& v2)
 312 {
 313   if (v1(0) < v2(0)) return true;
 314   else if (v2(0) < v1(0)) return false;
 315   else return (v1(1) < v2(1));
 316 }
 317
 318 template<typename T>
 319 inline
 320 bool
 321 operator<=(const SGVec2<T>& v1, const SGVec2<T>& v2)
 322 {
 323   if (v1(0) < v2(0)) return true;
 324   else if (v2(0) < v1(0)) return false;
 325   else return (v1(1) <= v2(1));
 326 }
 327
 328 template<typename T>
 329 inline
 330 bool
 331 operator>(const SGVec2<T>& v1, const SGVec2<T>& v2)
 332 { return operator<(v2, v1); }
 333
 334 template<typename T>
 335 inline
 336 bool
 337 operator>=(const SGVec2<T>& v1, const SGVec2<T>& v2)
 338 { return operator<=(v2, v1); }
 339
 340 /// Return true if equal to the relative tolerance tol
 341 template<typename T>
 342 inline
 343 bool
 344 equivalent(const SGVec2<T>& v1, const SGVec2<T>& v2, T rtol, T atol)
 345 { return norm1(v1 - v2) < rtol*(norm1(v1) + norm1(v2)) + atol; }
 346
 347 /// Return true if equal to the relative tolerance tol
 348 template<typename T>
 349 inline
 350 bool
 351 equivalent(const SGVec2<T>& v1, const SGVec2<T>& v2, T rtol)
 352 { return norm1(v1 - v2) < rtol*(norm1(v1) + norm1(v2)); }
 353
 354 /// Return true if about equal to roundoff of the underlying type
 355 template<typename T>
 356 inline
 357 bool
 358 equivalent(const SGVec2<T>& v1, const SGVec2<T>& v2)
 359 {
 360   T tol = 100*SGLimits<T>::epsilon();
 361   return equivalent(v1, v2, tol, tol);
 362 }
 363
 364 /// The euclidean distance of the two vectors
 365 template<typename T>
 366 inline
 367 T
 368 dist(const SGVec2<T>& v1, const SGVec2<T>& v2)
 369 { return norm(v1 - v2); }
 370
 371 /// The squared euclidean distance of the two vectors
 372 template<typename T>
 373 inline
 374 T
 375 distSqr(const SGVec2<T>& v1, const SGVec2<T>& v2)
 376 { SGVec2<T> tmp = v1 - v2; return dot(tmp, tmp); }
 377
 378 #ifndef NDEBUG
 379 template<typename T>
 380 inline
 381 bool
 382 isNaN(const SGVec2<T>& v)
 383 {
 384   return SGMisc<T>::isNaN(v(0)) || SGMisc<T>::isNaN(v(1));
 385 }
 386 #endif
 387
 388 /// Output to an ostream
 389 template<typename char_type, typename traits_type, typename T>
 390 inline
 391 std::basic_ostream<char_type, traits_type>&
 392 operator<<(std::basic_ostream<char_type, traits_type>& s, const SGVec2<T>& v)
 393 { return s << "[ " << v(0) << ", " << v(1) << " ]"; }
 394
 395 inline
 396 SGVec2f
 397 toVec2f(const SGVec2d& v)
 398 { return SGVec2f((float)v(0), (float)v(1)); }
 399
 400 inline
 401 SGVec2d
 402 toVec2d(const SGVec2f& v)
 403 { return SGVec2d(v(0), v(1)); }
 404
 405 #endif