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 /// component wise min
 208 template<typename T>
 209 inline
 210 SGVec2<T>
 211 min(const SGVec2<T>& v1, const SGVec2<T>& v2)
 212 {return SGVec2<T>(SGMisc<T>::min(v1(0), v2(0)), SGMisc<T>::min(v1(1), v2(1)));}
 213 template<typename S, typename T>
 214 inline
 215 SGVec2<T>
 216 min(const SGVec2<T>& v, S s)
 217 { return SGVec2<T>(SGMisc<T>::min(s, v(0)), SGMisc<T>::min(s, v(1))); }
 218 template<typename S, typename T>
 219 inline
 220 SGVec2<T>
 221 min(S s, const SGVec2<T>& v)
 222 { return SGVec2<T>(SGMisc<T>::min(s, v(0)), SGMisc<T>::min(s, v(1))); }
 223
 224 /// component wise max
 225 template<typename T>
 226 inline
 227 SGVec2<T>
 228 max(const SGVec2<T>& v1, const SGVec2<T>& v2)
 229 {return SGVec2<T>(SGMisc<T>::max(v1(0), v2(0)), SGMisc<T>::max(v1(1), v2(1)));}
 230 template<typename S, typename T>
 231 inline
 232 SGVec2<T>
 233 max(const SGVec2<T>& v, S s)
 234 { return SGVec2<T>(SGMisc<T>::max(s, v(0)), SGMisc<T>::max(s, v(1))); }
 235 template<typename S, typename T>
 236 inline
 237 SGVec2<T>
 238 max(S s, const SGVec2<T>& v)
 239 { return SGVec2<T>(SGMisc<T>::max(s, v(0)), SGMisc<T>::max(s, v(1))); }
 240
 241 /// Scalar dot product
 242 template<typename T>
 243 inline
 244 T
 245 dot(const SGVec2<T>& v1, const SGVec2<T>& v2)
 246 { return v1(0)*v2(0) + v1(1)*v2(1); }
 247
 248 /// The euclidean norm of the vector, that is what most people call length
 249 template<typename T>
 250 inline
 251 T
 252 norm(const SGVec2<T>& v)
 253 { return sqrt(dot(v, v)); }
 254
 255 /// The euclidean norm of the vector, that is what most people call length
 256 template<typename T>
 257 inline
 258 T
 259 length(const SGVec2<T>& v)
 260 { return sqrt(dot(v, v)); }
 261
 262 /// The 1-norm of the vector, this one is the fastest length function we
 263 /// can implement on modern cpu's
 264 template<typename T>
 265 inline
 266 T
 267 norm1(const SGVec2<T>& v)
 268 { return fabs(v(0)) + fabs(v(1)); }
 269
 270 /// The inf-norm of the vector
 271 template<typename T>
 272 inline
 273 T
 274 normI(const SGVec2<T>& v)
 275 { return SGMisc<T>::max(fabs(v(0)), fabs(v(1))); }
 276
 277 /// The euclidean norm of the vector, that is what most people call length
 278 template<typename T>
 279 inline
 280 SGVec2<T>
 281 normalize(const SGVec2<T>& v)
 282 { return (1/norm(v))*v; }
 283
 284 /// Return true if exactly the same
 285 template<typename T>
 286 inline
 287 bool
 288 operator==(const SGVec2<T>& v1, const SGVec2<T>& v2)
 289 { return v1(0) == v2(0) && v1(1) == v2(1); }
 290
 291 /// Return true if not exactly the same
 292 template<typename T>
 293 inline
 294 bool
 295 operator!=(const SGVec2<T>& v1, const SGVec2<T>& v2)
 296 { return ! (v1 == v2); }
 297
 298 /// Return true if equal to the relative tolerance tol
 299 template<typename T>
 300 inline
 301 bool
 302 equivalent(const SGVec2<T>& v1, const SGVec2<T>& v2, T rtol, T atol)
 303 { return norm1(v1 - v2) < rtol*(norm1(v1) + norm1(v2)) + atol; }
 304
 305 /// Return true if equal to the relative tolerance tol
 306 template<typename T>
 307 inline
 308 bool
 309 equivalent(const SGVec2<T>& v1, const SGVec2<T>& v2, T rtol)
 310 { return norm1(v1 - v2) < rtol*(norm1(v1) + norm1(v2)); }
 311
 312 /// Return true if about equal to roundoff of the underlying type
 313 template<typename T>
 314 inline
 315 bool
 316 equivalent(const SGVec2<T>& v1, const SGVec2<T>& v2)
 317 {
 318   T tol = 100*SGLimits<T>::epsilon();
 319   return equivalent(v1, v2, tol, tol);
 320 }
 321
 322 /// The euclidean distance of the two vectors
 323 template<typename T>
 324 inline
 325 T
 326 dist(const SGVec2<T>& v1, const SGVec2<T>& v2)
 327 { return norm(v1 - v2); }
 328
 329 /// The squared euclidean distance of the two vectors
 330 template<typename T>
 331 inline
 332 T
 333 distSqr(const SGVec2<T>& v1, const SGVec2<T>& v2)
 334 { SGVec2<T> tmp = v1 - v2; return dot(tmp, tmp); }
 335
 336 #ifndef NDEBUG
 337 template<typename T>
 338 inline
 339 bool
 340 isNaN(const SGVec2<T>& v)
 341 {
 342   return SGMisc<T>::isNaN(v(0)) || SGMisc<T>::isNaN(v(1));
 343 }
 344 #endif
 345
 346 /// Output to an ostream
 347 template<typename char_type, typename traits_type, typename T>
 348 inline
 349 std::basic_ostream<char_type, traits_type>&
 350 operator<<(std::basic_ostream<char_type, traits_type>& s, const SGVec2<T>& v)
 351 { return s << "[ " << v(0) << ", " << v(1) << " ]"; }
 352
 353 inline
 354 SGVec2f
 355 toVec2f(const SGVec2d& v)
 356 { return SGVec2f((float)v(0), (float)v(1)); }
 357
 358 inline
 359 SGVec2d
 360 toVec2d(const SGVec2f& v)
 361 { return SGVec2d(v(0), v(1)); }
 362
 363 #endif