simgear/math/SGVec3.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 Library General Public
  14 // License along with this library; if not, write to the
  15 // Free Software Foundation, Inc., 59 Temple Place - Suite 330,
  16 // Boston, MA  02111-1307, USA.
  17 //
  18
  19 #ifndef SGVec3_H
  20 #define SGVec3_H
  21
  22 /// 3D Vector Class
  23 template<typename T>
  24 class SGVec3 {
  25 public:
  26   typedef T value_type;
  27
  28   /// Default constructor. Does not initialize at all.
  29   /// If you need them zero initialized, use SGVec3::zeros()
  30   SGVec3(void)
  31   {
  32     /// Initialize with nans in the debug build, that will guarantee to have
  33     /// a fast uninitialized default constructor in the release but shows up
  34     /// uninitialized values in the debug build very fast ...
  35 #ifndef NDEBUG
  36     for (unsigned i = 0; i < 3; ++i)
  37       _data[i] = SGLimits<T>::quiet_NaN();
  38 #endif
  39   }
  40   /// Constructor. Initialize by the given values
  41   SGVec3(T x, T y, T z)
  42   { _data[0] = x; _data[1] = y; _data[2] = z; }
  43   /// Constructor. Initialize by the content of a plain array,
  44   /// make sure it has at least 3 elements
  45   explicit SGVec3(const T* data)
  46   { _data[0] = data[0]; _data[1] = data[1]; _data[2] = data[2]; }
  47   /// Constructor. Initialize by a geodetic coordinate
  48   /// Note that this conversion is relatively expensive to compute
  49   SGVec3(const SGGeod& geod)
  50   { SGGeodesy::SGGeodToCart(geod, *this); }
  51   /// Constructor. Initialize by a geocentric coordinate
  52   /// Note that this conversion is relatively expensive to compute
  53   SGVec3(const SGGeoc& geoc)
  54   { SGGeodesy::SGGeocToCart(geoc, *this); }
  55
  56   /// Access by index, the index is unchecked
  57   const T& operator()(unsigned i) const
  58   { return _data[i]; }
  59   /// Access by index, the index is unchecked
  60   T& operator()(unsigned i)
  61   { return _data[i]; }
  62
  63   /// Access raw data by index, the index is unchecked
  64   const T& operator[](unsigned i) const
  65   { return _data[i]; }
  66   /// Access raw data by index, the index is unchecked
  67   T& operator[](unsigned i)
  68   { return _data[i]; }
  69
  70   /// Access the x component
  71   const T& x(void) const
  72   { return _data[0]; }
  73   /// Access the x component
  74   T& x(void)
  75   { return _data[0]; }
  76   /// Access the y component
  77   const T& y(void) const
  78   { return _data[1]; }
  79   /// Access the y component
  80   T& y(void)
  81   { return _data[1]; }
  82   /// Access the z component
  83   const T& z(void) const
  84   { return _data[2]; }
  85   /// Access the z component
  86   T& z(void)
  87   { return _data[2]; }
  88
  89   /// Get the data pointer
  90   const T* data(void) const
  91   { return _data; }
  92   /// Get the data pointer
  93   T* data(void)
  94   { return _data; }
  95
  96   /// Readonly interface function to ssg's sgVec3/sgdVec3
  97   const T (&sg(void) const)[3]
  98   { return _data; }
  99   /// Interface function to ssg's sgVec3/sgdVec3
 100   T (&sg(void))[3]
 101   { return _data; }
 102
 103   /// Inplace addition
 104   SGVec3& operator+=(const SGVec3& v)
 105   { _data[0] += v(0); _data[1] += v(1); _data[2] += v(2); return *this; }
 106   /// Inplace subtraction
 107   SGVec3& operator-=(const SGVec3& v)
 108   { _data[0] -= v(0); _data[1] -= v(1); _data[2] -= v(2); return *this; }
 109   /// Inplace scalar multiplication
 110   template<typename S>
 111   SGVec3& operator*=(S s)
 112   { _data[0] *= s; _data[1] *= s; _data[2] *= s; return *this; }
 113   /// Inplace scalar multiplication by 1/s
 114   template<typename S>
 115   SGVec3& operator/=(S s)
 116   { return operator*=(1/T(s)); }
 117
 118   /// Return an all zero vector
 119   static SGVec3 zeros(void)
 120   { return SGVec3(0, 0, 0); }
 121   /// Return unit vectors
 122   static SGVec3 e1(void)
 123   { return SGVec3(1, 0, 0); }
 124   static SGVec3 e2(void)
 125   { return SGVec3(0, 1, 0); }
 126   static SGVec3 e3(void)
 127   { return SGVec3(0, 0, 1); }
 128
 129 private:
 130   /// The actual data
 131   T _data[3];
 132 };
 133
 134 /// Unary +, do nothing ...
 135 template<typename T>
 136 inline
 137 const SGVec3<T>&
 138 operator+(const SGVec3<T>& v)
 139 { return v; }
 140
 141 /// Unary -, do nearly nothing
 142 template<typename T>
 143 inline
 144 SGVec3<T>
 145 operator-(const SGVec3<T>& v)
 146 { return SGVec3<T>(-v(0), -v(1), -v(2)); }
 147
 148 /// Binary +
 149 template<typename T>
 150 inline
 151 SGVec3<T>
 152 operator+(const SGVec3<T>& v1, const SGVec3<T>& v2)
 153 { return SGVec3<T>(v1(0)+v2(0), v1(1)+v2(1), v1(2)+v2(2)); }
 154
 155 /// Binary -
 156 template<typename T>
 157 inline
 158 SGVec3<T>
 159 operator-(const SGVec3<T>& v1, const SGVec3<T>& v2)
 160 { return SGVec3<T>(v1(0)-v2(0), v1(1)-v2(1), v1(2)-v2(2)); }
 161
 162 /// Scalar multiplication
 163 template<typename S, typename T>
 164 inline
 165 SGVec3<T>
 166 operator*(S s, const SGVec3<T>& v)
 167 { return SGVec3<T>(s*v(0), s*v(1), s*v(2)); }
 168
 169 /// Scalar multiplication
 170 template<typename S, typename T>
 171 inline
 172 SGVec3<T>
 173 operator*(const SGVec3<T>& v, S s)
 174 { return SGVec3<T>(s*v(0), s*v(1), s*v(2)); }
 175
 176 /// Scalar dot product
 177 template<typename T>
 178 inline
 179 T
 180 dot(const SGVec3<T>& v1, const SGVec3<T>& v2)
 181 { return v1(0)*v2(0) + v1(1)*v2(1) + v1(2)*v2(2); }
 182
 183 /// The euclidean norm of the vector, that is what most people call length
 184 template<typename T>
 185 inline
 186 T
 187 norm(const SGVec3<T>& v)
 188 { return sqrt(dot(v, v)); }
 189
 190 /// The euclidean norm of the vector, that is what most people call length
 191 template<typename T>
 192 inline
 193 T
 194 length(const SGVec3<T>& v)
 195 { return sqrt(dot(v, v)); }
 196
 197 /// The 1-norm of the vector, this one is the fastest length function we
 198 /// can implement on modern cpu's
 199 template<typename T>
 200 inline
 201 T
 202 norm1(const SGVec3<T>& v)
 203 { return fabs(v(0)) + fabs(v(1)) + fabs(v(2)); }
 204
 205 /// Vector cross product
 206 template<typename T>
 207 inline
 208 SGVec3<T>
 209 cross(const SGVec3<T>& v1, const SGVec3<T>& v2)
 210 {
 211   return SGVec3<T>(v1(1)*v2(2) - v1(2)*v2(1),
 212                    v1(2)*v2(0) - v1(0)*v2(2),
 213                    v1(0)*v2(1) - v1(1)*v2(0));
 214 }
 215
 216 /// The euclidean norm of the vector, that is what most people call length
 217 template<typename T>
 218 inline
 219 SGVec3<T>
 220 normalize(const SGVec3<T>& v)
 221 { return (1/norm(v))*v; }
 222
 223 /// Return true if exactly the same
 224 template<typename T>
 225 inline
 226 bool
 227 operator==(const SGVec3<T>& v1, const SGVec3<T>& v2)
 228 { return v1(0) == v2(0) && v1(1) == v2(1) && v1(2) == v2(2); }
 229
 230 /// Return true if not exactly the same
 231 template<typename T>
 232 inline
 233 bool
 234 operator!=(const SGVec3<T>& v1, const SGVec3<T>& v2)
 235 { return ! (v1 == v2); }
 236
 237 /// Return true if equal to the relative tolerance tol
 238 template<typename T>
 239 inline
 240 bool
 241 equivalent(const SGVec3<T>& v1, const SGVec3<T>& v2, T rtol, T atol)
 242 { return norm1(v1 - v2) < rtol*(norm1(v1) + norm1(v2)) + atol; }
 243
 244 /// Return true if equal to the relative tolerance tol
 245 template<typename T>
 246 inline
 247 bool
 248 equivalent(const SGVec3<T>& v1, const SGVec3<T>& v2, T rtol)
 249 { return norm1(v1 - v2) < rtol*(norm1(v1) + norm1(v2)); }
 250
 251 /// Return true if about equal to roundoff of the underlying type
 252 template<typename T>
 253 inline
 254 bool
 255 equivalent(const SGVec3<T>& v1, const SGVec3<T>& v2)
 256 {
 257   T tol = 100*SGLimits<T>::epsilon();
 258   return equivalent(v1, v2, tol, tol);
 259 }
 260
 261 #ifndef NDEBUG
 262 template<typename T>
 263 inline
 264 bool
 265 isNaN(const SGVec3<T>& v)
 266 {
 267   return SGMisc<T>::isNaN(v(0)) ||
 268     SGMisc<T>::isNaN(v(1)) || SGMisc<T>::isNaN(v(2));
 269 }
 270 #endif
 271
 272 /// Output to an ostream
 273 template<typename char_type, typename traits_type, typename T>
 274 inline
 275 std::basic_ostream<char_type, traits_type>&
 276 operator<<(std::basic_ostream<char_type, traits_type>& s, const SGVec3<T>& v)
 277 { return s << "[ " << v(0) << ", " << v(1) << ", " << v(2) << " ]"; }
 278
 279 /// Two classes doing actually the same on different types
 280 typedef SGVec3<float> SGVec3f;
 281 typedef SGVec3<double> SGVec3d;
 282
 283 inline
 284 SGVec3f
 285 toVec3f(const SGVec3d& v)
 286 { return SGVec3f((float)v(0), (float)v(1), (float)v(2)); }
 287
 288 inline
 289 SGVec3d
 290 toVec3d(const SGVec3f& v)
 291 { return SGVec3d(v(0), v(1), v(2)); }
 292
 293 #endif