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