00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #ifndef SGVec3_H
00019 #define SGVec3_H
00020
00021 #ifndef NO_OPENSCENEGRAPH_INTERFACE
00022 #include <osg/Vec3f>
00023 #include <osg/Vec3d>
00024 #endif
00025
00027 template<typename T>
00028 class SGVec3 {
00029 public:
00030 typedef T value_type;
00031
00034 SGVec3(void)
00035 {
00039 #ifndef NDEBUG
00040 for (unsigned i = 0; i < 3; ++i)
00041 data()[i] = SGLimits<T>::quiet_NaN();
00042 #endif
00043 }
00045 SGVec3(T x, T y, T z)
00046 { data()[0] = x; data()[1] = y; data()[2] = z; }
00049 explicit SGVec3(const T* d)
00050 { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; }
00051 template<typename S>
00052 explicit SGVec3(const SGVec3<S>& d)
00053 { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; }
00054 explicit SGVec3(const SGVec2<T>& v2, const T& v3 = 0)
00055 { data()[0] = v2[0]; data()[1] = v2[1]; data()[2] = v3; }
00056
00058 const T& operator()(unsigned i) const
00059 { return data()[i]; }
00061 T& operator()(unsigned i)
00062 { return data()[i]; }
00063
00065 const T& operator[](unsigned i) const
00066 { return data()[i]; }
00068 T& operator[](unsigned i)
00069 { return data()[i]; }
00070
00072 const T& x(void) const
00073 { return data()[0]; }
00075 T& x(void)
00076 { return data()[0]; }
00078 const T& y(void) const
00079 { return data()[1]; }
00081 T& y(void)
00082 { return data()[1]; }
00084 const T& z(void) const
00085 { return data()[2]; }
00087 T& z(void)
00088 { return data()[2]; }
00089
00091 const T (&data(void) const)[3]
00092 { return _data; }
00094 T (&data(void))[3]
00095 { return _data; }
00096
00098 SGVec3& operator+=(const SGVec3& v)
00099 { data()[0] += v(0); data()[1] += v(1); data()[2] += v(2); return *this; }
00101 SGVec3& operator-=(const SGVec3& v)
00102 { data()[0] -= v(0); data()[1] -= v(1); data()[2] -= v(2); return *this; }
00104 template<typename S>
00105 SGVec3& operator*=(S s)
00106 { data()[0] *= s; data()[1] *= s; data()[2] *= s; return *this; }
00108 template<typename S>
00109 SGVec3& operator/=(S s)
00110 { return operator*=(1/T(s)); }
00111
00113 static SGVec3 zeros(void)
00114 { return SGVec3(0, 0, 0); }
00116 static SGVec3 e1(void)
00117 { return SGVec3(1, 0, 0); }
00118 static SGVec3 e2(void)
00119 { return SGVec3(0, 1, 0); }
00120 static SGVec3 e3(void)
00121 { return SGVec3(0, 0, 1); }
00122
00125 static SGVec3 fromGeod(const SGGeod& geod);
00128 static SGVec3 fromGeoc(const SGGeoc& geoc);
00129
00130 private:
00131 T _data[3];
00132 };
00133
00134 template<>
00135 inline
00136 SGVec3<double>
00137 SGVec3<double>::fromGeod(const SGGeod& geod)
00138 {
00139 SGVec3<double> cart;
00140 SGGeodesy::SGGeodToCart(geod, cart);
00141 return cart;
00142 }
00143
00144 template<>
00145 inline
00146 SGVec3<float>
00147 SGVec3<float>::fromGeod(const SGGeod& geod)
00148 {
00149 SGVec3<double> cart;
00150 SGGeodesy::SGGeodToCart(geod, cart);
00151 return SGVec3<float>(cart(0), cart(1), cart(2));
00152 }
00153
00154 template<>
00155 inline
00156 SGVec3<double>
00157 SGVec3<double>::fromGeoc(const SGGeoc& geoc)
00158 {
00159 SGVec3<double> cart;
00160 SGGeodesy::SGGeocToCart(geoc, cart);
00161 return cart;
00162 }
00163
00164 template<>
00165 inline
00166 SGVec3<float>
00167 SGVec3<float>::fromGeoc(const SGGeoc& geoc)
00168 {
00169 SGVec3<double> cart;
00170 SGGeodesy::SGGeocToCart(geoc, cart);
00171 return SGVec3<float>(cart(0), cart(1), cart(2));
00172 }
00173
00175 template<typename T>
00176 inline
00177 const SGVec3<T>&
00178 operator+(const SGVec3<T>& v)
00179 { return v; }
00180
00182 template<typename T>
00183 inline
00184 SGVec3<T>
00185 operator-(const SGVec3<T>& v)
00186 { return SGVec3<T>(-v(0), -v(1), -v(2)); }
00187
00189 template<typename T>
00190 inline
00191 SGVec3<T>
00192 operator+(const SGVec3<T>& v1, const SGVec3<T>& v2)
00193 { return SGVec3<T>(v1(0)+v2(0), v1(1)+v2(1), v1(2)+v2(2)); }
00194
00196 template<typename T>
00197 inline
00198 SGVec3<T>
00199 operator-(const SGVec3<T>& v1, const SGVec3<T>& v2)
00200 { return SGVec3<T>(v1(0)-v2(0), v1(1)-v2(1), v1(2)-v2(2)); }
00201
00203 template<typename S, typename T>
00204 inline
00205 SGVec3<T>
00206 operator*(S s, const SGVec3<T>& v)
00207 { return SGVec3<T>(s*v(0), s*v(1), s*v(2)); }
00208
00210 template<typename S, typename T>
00211 inline
00212 SGVec3<T>
00213 operator*(const SGVec3<T>& v, S s)
00214 { return SGVec3<T>(s*v(0), s*v(1), s*v(2)); }
00215
00219 template<typename T>
00220 inline
00221 SGVec3<T>
00222 mult(const SGVec3<T>& v1, const SGVec3<T>& v2)
00223 { return SGVec3<T>(v1(0)*v2(0), v1(1)*v2(1), v1(2)*v2(2)); }
00224
00226 template<typename T>
00227 inline
00228 SGVec3<T>
00229 min(const SGVec3<T>& v1, const SGVec3<T>& v2)
00230 {
00231 return SGVec3<T>(SGMisc<T>::min(v1(0), v2(0)),
00232 SGMisc<T>::min(v1(1), v2(1)),
00233 SGMisc<T>::min(v1(2), v2(2)));
00234 }
00235 template<typename S, typename T>
00236 inline
00237 SGVec3<T>
00238 min(const SGVec3<T>& v, S s)
00239 {
00240 return SGVec3<T>(SGMisc<T>::min(s, v(0)),
00241 SGMisc<T>::min(s, v(1)),
00242 SGMisc<T>::min(s, v(2)));
00243 }
00244 template<typename S, typename T>
00245 inline
00246 SGVec3<T>
00247 min(S s, const SGVec3<T>& v)
00248 {
00249 return SGVec3<T>(SGMisc<T>::min(s, v(0)),
00250 SGMisc<T>::min(s, v(1)),
00251 SGMisc<T>::min(s, v(2)));
00252 }
00253
00255 template<typename T>
00256 inline
00257 SGVec3<T>
00258 max(const SGVec3<T>& v1, const SGVec3<T>& v2)
00259 {
00260 return SGVec3<T>(SGMisc<T>::max(v1(0), v2(0)),
00261 SGMisc<T>::max(v1(1), v2(1)),
00262 SGMisc<T>::max(v1(2), v2(2)));
00263 }
00264 template<typename S, typename T>
00265 inline
00266 SGVec3<T>
00267 max(const SGVec3<T>& v, S s)
00268 {
00269 return SGVec3<T>(SGMisc<T>::max(s, v(0)),
00270 SGMisc<T>::max(s, v(1)),
00271 SGMisc<T>::max(s, v(2)));
00272 }
00273 template<typename S, typename T>
00274 inline
00275 SGVec3<T>
00276 max(S s, const SGVec3<T>& v)
00277 {
00278 return SGVec3<T>(SGMisc<T>::max(s, v(0)),
00279 SGMisc<T>::max(s, v(1)),
00280 SGMisc<T>::max(s, v(2)));
00281 }
00282
00284 template<typename T>
00285 inline
00286 T
00287 dot(const SGVec3<T>& v1, const SGVec3<T>& v2)
00288 { return v1(0)*v2(0) + v1(1)*v2(1) + v1(2)*v2(2); }
00289
00291 template<typename T>
00292 inline
00293 T
00294 norm(const SGVec3<T>& v)
00295 { return sqrt(dot(v, v)); }
00296
00298 template<typename T>
00299 inline
00300 T
00301 length(const SGVec3<T>& v)
00302 { return sqrt(dot(v, v)); }
00303
00306 template<typename T>
00307 inline
00308 T
00309 norm1(const SGVec3<T>& v)
00310 { return fabs(v(0)) + fabs(v(1)) + fabs(v(2)); }
00311
00313 template<typename T>
00314 inline
00315 T
00316 normI(const SGVec3<T>& v)
00317 { return SGMisc<T>::max(fabs(v(0)), fabs(v(1)), fabs(v(2))); }
00318
00320 template<typename T>
00321 inline
00322 SGVec3<T>
00323 cross(const SGVec3<T>& v1, const SGVec3<T>& v2)
00324 {
00325 return SGVec3<T>(v1(1)*v2(2) - v1(2)*v2(1),
00326 v1(2)*v2(0) - v1(0)*v2(2),
00327 v1(0)*v2(1) - v1(1)*v2(0));
00328 }
00329
00331 template<typename T>
00332 inline
00333 SGVec3<T>
00334 perpendicular(const SGVec3<T>& v)
00335 {
00336 T absv1 = fabs(v(0));
00337 T absv2 = fabs(v(1));
00338 T absv3 = fabs(v(2));
00339
00340 if (absv2 < absv1 && absv3 < absv1) {
00341 T quot = v(1)/v(0);
00342 return (1/sqrt(1+quot*quot))*SGVec3<T>(quot, -1, 0);
00343 } else if (absv3 < absv2) {
00344 T quot = v(2)/v(1);
00345 return (1/sqrt(1+quot*quot))*SGVec3<T>(0, quot, -1);
00346 } else if (SGLimits<T>::min() < absv3) {
00347 T quot = v(0)/v(2);
00348 return (1/sqrt(1+quot*quot))*SGVec3<T>(-1, 0, quot);
00349 } else {
00350
00351 return SGVec3<T>(0, 0, 0);
00352 }
00353 }
00354
00357 template<typename T>
00358 inline
00359 SGVec3<T>
00360 normalize(const SGVec3<T>& v)
00361 {
00362 T normv = norm(v);
00363 if (normv <= SGLimits<T>::min())
00364 return SGVec3<T>::zeros();
00365 return (1/normv)*v;
00366 }
00367
00369 template<typename T>
00370 inline
00371 bool
00372 operator==(const SGVec3<T>& v1, const SGVec3<T>& v2)
00373 { return v1(0) == v2(0) && v1(1) == v2(1) && v1(2) == v2(2); }
00374
00376 template<typename T>
00377 inline
00378 bool
00379 operator!=(const SGVec3<T>& v1, const SGVec3<T>& v2)
00380 { return ! (v1 == v2); }
00381
00383 template<typename T>
00384 inline
00385 bool
00386 operator<(const SGVec3<T>& v1, const SGVec3<T>& v2)
00387 {
00388 if (v1(0) < v2(0)) return true;
00389 else if (v2(0) < v1(0)) return false;
00390 else if (v1(1) < v2(1)) return true;
00391 else if (v2(1) < v1(1)) return false;
00392 else return (v1(2) < v2(2));
00393 }
00394
00395 template<typename T>
00396 inline
00397 bool
00398 operator<=(const SGVec3<T>& v1, const SGVec3<T>& v2)
00399 {
00400 if (v1(0) < v2(0)) return true;
00401 else if (v2(0) < v1(0)) return false;
00402 else if (v1(1) < v2(1)) return true;
00403 else if (v2(1) < v1(1)) return false;
00404 else return (v1(2) <= v2(2));
00405 }
00406
00407 template<typename T>
00408 inline
00409 bool
00410 operator>(const SGVec3<T>& v1, const SGVec3<T>& v2)
00411 { return operator<(v2, v1); }
00412
00413 template<typename T>
00414 inline
00415 bool
00416 operator>=(const SGVec3<T>& v1, const SGVec3<T>& v2)
00417 { return operator<=(v2, v1); }
00418
00420 template<typename T>
00421 inline
00422 bool
00423 equivalent(const SGVec3<T>& v1, const SGVec3<T>& v2, T rtol, T atol)
00424 { return norm1(v1 - v2) < rtol*(norm1(v1) + norm1(v2)) + atol; }
00425
00427 template<typename T>
00428 inline
00429 bool
00430 equivalent(const SGVec3<T>& v1, const SGVec3<T>& v2, T rtol)
00431 { return norm1(v1 - v2) < rtol*(norm1(v1) + norm1(v2)); }
00432
00434 template<typename T>
00435 inline
00436 bool
00437 equivalent(const SGVec3<T>& v1, const SGVec3<T>& v2)
00438 {
00439 T tol = 100*SGLimits<T>::epsilon();
00440 return equivalent(v1, v2, tol, tol);
00441 }
00442
00444 template<typename T>
00445 inline
00446 T
00447 dist(const SGVec3<T>& v1, const SGVec3<T>& v2)
00448 { return norm(v1 - v2); }
00449
00451 template<typename T>
00452 inline
00453 T
00454 distSqr(const SGVec3<T>& v1, const SGVec3<T>& v2)
00455 { SGVec3<T> tmp = v1 - v2; return dot(tmp, tmp); }
00456
00457
00458 template<typename T>
00459 inline
00460 SGVec3<T>
00461 projection(const SGVec3<T>& u, const SGVec3<T>& d)
00462 {
00463 T denom = dot(d, d);
00464 T ud = dot(u, d);
00465 if (SGLimits<T>::min() < denom) return u;
00466 else return d * (dot(u, d) / denom);
00467 }
00468
00469 #ifndef NDEBUG
00470 template<typename T>
00471 inline
00472 bool
00473 isNaN(const SGVec3<T>& v)
00474 {
00475 return SGMisc<T>::isNaN(v(0)) ||
00476 SGMisc<T>::isNaN(v(1)) || SGMisc<T>::isNaN(v(2));
00477 }
00478 #endif
00479
00481 template<typename char_type, typename traits_type, typename T>
00482 inline
00483 std::basic_ostream<char_type, traits_type>&
00484 operator<<(std::basic_ostream<char_type, traits_type>& s, const SGVec3<T>& v)
00485 { return s << "[ " << v(0) << ", " << v(1) << ", " << v(2) << " ]"; }
00486
00487 inline
00488 SGVec3f
00489 toVec3f(const SGVec3d& v)
00490 { return SGVec3f((float)v(0), (float)v(1), (float)v(2)); }
00491
00492 inline
00493 SGVec3d
00494 toVec3d(const SGVec3f& v)
00495 { return SGVec3d(v(0), v(1), v(2)); }
00496
00497 #ifndef NO_OPENSCENEGRAPH_INTERFACE
00498 inline
00499 SGVec3d
00500 toSG(const osg::Vec3d& v)
00501 { return SGVec3d(v[0], v[1], v[2]); }
00502
00503 inline
00504 SGVec3f
00505 toSG(const osg::Vec3f& v)
00506 { return SGVec3f(v[0], v[1], v[2]); }
00507
00508 inline
00509 osg::Vec3d
00510 toOsg(const SGVec3d& v)
00511 { return osg::Vec3d(v[0], v[1], v[2]); }
00512
00513 inline
00514 osg::Vec3f
00515 toOsg(const SGVec3f& v)
00516 { return osg::Vec3f(v[0], v[1], v[2]); }
00517 #endif
00518
00519 #endif
