00001
00005 #pragma once
00006
00007 #include<cmath>
00008 #include<complex>
00009 #include<boost/numeric/ublas/vector.hpp>
00010 #include<boost/numeric/ublas/matrix.hpp>
00011 #include<boost/numeric/ublas/io.hpp>
00012 #include <usml/usml_config.h>
00013
00014 # define TWO_PI (2.0*M_PI)
00015
00016 namespace usml {
00017 namespace ublas {
00018
00019 using std::cout;
00020 using std::operator<<;
00021 using std::endl;
00022
00023 using std::complex;
00024 using std::size_t;
00025
00026 using std::max;
00027 using std::min;
00028 using std::floor;
00029 using std::ceil;
00030
00031 using std::abs;
00032 using std::arg;
00033 using std::sqrt;
00034
00035 using std::cos;
00036 using std::cosh;
00037 using std::sin;
00038 using std::sinh;
00039 using std::tan;
00040 using std::tanh;
00041
00042 using std::acos;
00043 using std::asin;
00044 using std::atan;
00045 using std::atan2;
00046
00047 using std::exp;
00048 using std::log;
00049 using std::log10;
00050 using std::pow;
00051
00052 using namespace boost::numeric::ublas;
00053
00054 const complex<double> DOUBLE_I = complex<double> (0.0, 1.0);
00055 const complex<float> FLOAT_I = complex<float> (0.0, 1.0);
00056
00075 template<class T> struct math_traits
00076 {
00077 typedef math_traits<T> self_type;
00078 typedef T value_type;
00079 typedef const T & const_reference;
00080 typedef T & reference;
00081 typedef T real_type;
00082
00083
00084
00085
00086 static inline value_type to_degrees(const_reference t);
00087 static inline value_type to_radians(const_reference t);
00088 static inline value_type to_latitude(const_reference t);
00089 static inline value_type to_colatitude(const_reference t);
00090
00091
00092
00093
00094 static inline value_type sqrt(const_reference t);
00095
00096
00097
00098
00099 static inline value_type cos(const_reference t);
00100 static inline value_type cosh(const_reference t);
00101 static inline value_type sin(const_reference t);
00102 static inline value_type sinh(const_reference t);
00103 static inline value_type tan(const_reference t);
00104 static inline value_type tanh(const_reference t);
00105
00106
00107
00108
00109 static inline value_type acos(const_reference t);
00110 static inline value_type acosh(const_reference t);
00111 static inline value_type asin(const_reference t);
00112 static inline value_type asinh(const_reference t);
00113 static inline value_type atan(const_reference t);
00114 static inline value_type atan2(const_reference y, const_reference x);
00115 static inline value_type atanh(const_reference t);
00116
00117
00118
00119
00120 static inline value_type exp(const_reference t);
00121 static inline value_type log(const_reference t);
00122 static inline value_type log10(const_reference t);
00123 static inline value_type pow(const_reference t, int e);
00124 static inline value_type pow(const_reference t, const_reference e);
00125 };
00126
00132 template<> struct math_traits<double>
00133 {
00134 typedef math_traits<double> self_type;
00135 typedef double value_type;
00136 typedef const double & const_reference;
00137 typedef double & reference;
00138 typedef double real_type;
00139
00140
00141
00142
00143 static inline value_type max(const_reference t1, const_reference t2)
00144 {
00145 return std::max(t1, t2);
00146 }
00147 static inline value_type min(const_reference t1, const_reference t2)
00148 {
00149 return std::min(t1, t2);
00150 }
00151 static inline value_type floor(const_reference t)
00152 {
00153 return std::floor(t);
00154 }
00155 static inline value_type ceil(const_reference t)
00156 {
00157 return std::ceil(t);
00158 }
00159
00160
00161
00162
00163 static inline value_type to_degrees(const_reference t)
00164 {
00165 return t * (180.0 / M_PI);
00166 }
00167
00168 static inline value_type to_radians(const_reference t)
00169 {
00170 return t * (M_PI / 180.0);
00171 }
00172
00173 static inline value_type to_latitude(const_reference t)
00174 {
00175 return 90.0 - to_degrees(t);
00176 }
00177
00178 static inline value_type to_colatitude(const_reference t)
00179 {
00180 return to_radians(90.0 - t);
00181 }
00182
00183
00184
00185
00186 static inline value_type sqrt(const_reference t)
00187 {
00188 return std::sqrt(t);
00189 }
00190 static inline value_type copysign(const_reference t1, const_reference t2)
00191 {
00192 return ( t2 < 0.0 ) ? -t1 : t1 ;
00193 }
00194
00195
00196
00197
00198 static inline value_type cos(const_reference t)
00199 {
00200 return std::cos(t);
00201 }
00202 static inline value_type cosh(const_reference t)
00203 {
00204 return std::cosh(t);
00205 }
00206 static inline value_type sin(const_reference t)
00207 {
00208 return std::sin(t);
00209 }
00210 static inline value_type sinh(const_reference t)
00211 {
00212 return std::sinh(t);
00213 }
00214 static inline value_type tan(const_reference t)
00215 {
00216 return std::tan(t);
00217 }
00218 static inline value_type tanh(const_reference t)
00219 {
00220 return std::tanh(t);
00221 }
00222
00223
00224
00225
00226
00227
00228 static inline value_type acos(const_reference t)
00229 {
00230 return std::acos(t);
00231 }
00232 static inline value_type acosh(const_reference t)
00233 {
00234 return log(t + sqrt(t * t - 1.0));
00235 }
00236 static inline value_type asin(const_reference t)
00237 {
00238 return std::asin(t);
00239 }
00240 static inline value_type asinh(const_reference t)
00241 {
00242 return log(t + sqrt(t * t + 1.0));
00243 }
00244 static inline value_type atan(const_reference t)
00245 {
00246 return std::atan(t);
00247 }
00248 static inline value_type atan2(const_reference y, const_reference x)
00249 {
00250 return std::atan2(y, x);
00251 }
00252 static inline value_type atanh(const_reference t)
00253 {
00254 return 0.5 * log((1.0 + t) / (1.0 - t));
00255 }
00256
00257
00258
00259
00260 static inline value_type exp(const_reference t)
00261 {
00262 return std::exp(t);
00263 }
00264 static inline value_type log(const_reference t)
00265 {
00266 return std::log(t);
00267 }
00268 static inline value_type log10(const_reference t)
00269 {
00270 return std::log10(t);
00271 }
00272 static inline value_type pow(const_reference t, int e)
00273 {
00274 return std::pow(t, e);
00275 }
00276 static inline value_type pow(const_reference t, const_reference e)
00277 {
00278 return std::pow(t, e);
00279 }
00280
00281
00282
00283
00284 static inline real_type abs(const_reference t)
00285 {
00286 return std::abs(t);
00287 }
00288 static inline real_type arg(const_reference t)
00289 {
00290 return 0.0;
00291 }
00292 static inline real_type abs2(const_reference t)
00293 {
00294 return t * t;
00295 }
00296
00297 static inline real_type abs(const complex<value_type> &t)
00298 {
00299 return std::abs(t);
00300 }
00301 static inline real_type arg(const complex<value_type> &t)
00302 {
00303 return std::arg(t);
00304 }
00305 static inline real_type abs2(const complex<value_type> &t)
00306 {
00307 return t.real() * t.real() + t.imag() * t.imag();
00308 }
00309 };
00310
00316 template<> struct math_traits<float>
00317 {
00318 typedef math_traits<float> self_type;
00319 typedef float value_type;
00320 typedef const float & const_reference;
00321 typedef float & reference;
00322 typedef float real_type;
00323
00324
00325
00326
00327 static inline value_type max(const_reference t1, const_reference t2)
00328 {
00329 return std::max(t1, t2);
00330 }
00331 static inline value_type min(const_reference t1, const_reference t2)
00332 {
00333 return std::min(t1, t2);
00334 }
00335 static inline value_type floor(const_reference t)
00336 {
00337 return std::floor(t);
00338 }
00339 static inline value_type ceil(const_reference t)
00340 {
00341 return std::ceil(t);
00342 }
00343
00344
00345
00346
00347 static inline value_type to_degrees(const_reference t)
00348 {
00349 return t * (180.0 / M_PI);
00350 }
00351
00352 static inline value_type to_radians(const_reference t)
00353 {
00354 return t * (M_PI / 180.0);
00355 }
00356
00357 static inline value_type to_latitude(const_reference t)
00358 {
00359 return 90.0 - to_degrees(t);
00360 }
00361
00362 static inline value_type to_colatitude(const_reference t)
00363 {
00364 return to_radians(90.0 - t);
00365 }
00366
00367
00368
00369
00370 static inline value_type sqrt(const_reference t)
00371 {
00372 return std::sqrt(t);
00373 }
00374 static inline value_type copysign(const_reference t1, const_reference t2)
00375 {
00376 return ( t2 < 0.0 ) ? -t1 : t1 ;
00377 }
00378
00379
00380
00381
00382 static inline value_type cos(const_reference t)
00383 {
00384 return std::cos(t);
00385 }
00386 static inline value_type cosh(const_reference t)
00387 {
00388 return std::cosh(t);
00389 }
00390 static inline value_type sin(const_reference t)
00391 {
00392 return std::sin(t);
00393 }
00394 static inline value_type sinh(const_reference t)
00395 {
00396 return std::sinh(t);
00397 }
00398 static inline value_type tan(const_reference t)
00399 {
00400 return std::tan(t);
00401 }
00402 static inline value_type tanh(const_reference t)
00403 {
00404 return std::tanh(t);
00405 }
00406
00407
00408
00409
00410
00411
00412 static inline value_type acos(const_reference t)
00413 {
00414 return std::acos(t);
00415 }
00416 static inline value_type acosh(const_reference t)
00417 {
00418 return log(t + sqrt(t * t - 1.0f));
00419 }
00420 static inline value_type asin(const_reference t)
00421 {
00422 return std::asin(t);
00423 }
00424 static inline value_type asinh(const_reference t)
00425 {
00426 return log(t + sqrt(t * t + 1.0f));
00427 }
00428 static inline value_type atan(const_reference t)
00429 {
00430 return std::atan(t);
00431 }
00432 static inline value_type atan2(const_reference y, const_reference x)
00433 {
00434 return std::atan2(y, x);
00435 }
00436 static inline value_type atanh(const_reference t)
00437 {
00438 return 0.5 * log((1.0f + t) / (1.0f - t));
00439 }
00440
00441
00442
00443
00444 static inline value_type exp(const_reference t)
00445 {
00446 return std::exp(t);
00447 }
00448 static inline value_type log(const_reference t)
00449 {
00450 return std::log(t);
00451 }
00452 static inline value_type log10(const_reference t)
00453 {
00454 return std::log10(t);
00455 }
00456 static inline value_type pow(const_reference t, int e)
00457 {
00458 return std::pow(t, e);
00459 }
00460 static inline value_type pow(const_reference t, const_reference e)
00461 {
00462 return std::pow(t, e);
00463 }
00464
00465
00466
00467
00468 static inline real_type abs(const_reference t)
00469 {
00470 return std::abs(t);
00471 }
00472 static inline real_type arg(const_reference t)
00473 {
00474 return 0.0;
00475 }
00476 static inline real_type abs2(const_reference t)
00477 {
00478 return t * t;
00479 }
00480
00481 static inline real_type abs(const complex<value_type> &t)
00482 {
00483 return std::abs(t);
00484 }
00485 static inline real_type arg(const complex<value_type> &t)
00486 {
00487 return std::arg(t);
00488 }
00489 static inline real_type abs2(const complex<value_type> &t)
00490 {
00491 return t.real() * t.real() + t.imag() * t.imag();
00492 }
00493 };
00494
00501 template<> struct math_traits<complex<double> >
00502 {
00503 typedef math_traits<complex<double> > self_type;
00504 typedef complex<double> value_type;
00505 typedef const complex<double> & const_reference;
00506 typedef complex<double> & reference;
00507 typedef double real_type;
00508
00509
00510
00511
00512 static inline value_type sqrt(const_reference t)
00513 {
00514 return std::sqrt(t);
00515 }
00516
00517
00518
00519 static inline value_type cos(const_reference t)
00520 {
00521 return std::cos(t);
00522 }
00523 static inline value_type cosh(const_reference t)
00524 {
00525 return std::cosh(t);
00526 }
00527 static inline value_type sin(const_reference t)
00528 {
00529 return std::sin(t);
00530 }
00531 static inline value_type sinh(const_reference t)
00532 {
00533 return std::sinh(t);
00534 }
00535 static inline value_type tan(const_reference t)
00536 {
00537 return std::tan(t);
00538 }
00539 static inline value_type tanh(const_reference t)
00540 {
00541 return std::tanh(t);
00542 }
00543
00544
00545
00546
00547
00548
00549 static inline value_type acos(const_reference t)
00550 {
00551 return DOUBLE_I * log(t + sqrt(t * t - 1.0));
00552 }
00553 static inline value_type acosh(const_reference t)
00554 {
00555 return log(t + sqrt(t * t - 1.0));
00556 }
00557 static inline value_type asin(const_reference t)
00558 {
00559 return -DOUBLE_I * log(DOUBLE_I * t + sqrt(1.0 - t * t));
00560 }
00561 static inline value_type asinh(const_reference t)
00562 {
00563 return log(t + sqrt(t * t + 1.0));
00564 }
00565 static inline value_type atan(const_reference t)
00566 {
00567 return log((1.0 + DOUBLE_I * t) / (1.0 - DOUBLE_I * t)) / (2.0
00568 * DOUBLE_I);
00569 }
00570 static inline value_type atan2(const_reference y, const_reference x)
00571 {
00572 return atan(y / x);
00573 }
00574 static inline value_type atanh(const_reference t)
00575 {
00576 return 0.5 * log((1.0 + t) / (1.0 - t));
00577 }
00578
00579
00580
00581
00582 static inline value_type exp(const_reference t)
00583 {
00584 return std::exp(t);
00585 }
00586 static inline value_type log(const_reference t)
00587 {
00588 return std::log(t);
00589 }
00590 static inline value_type log10(const_reference t)
00591 {
00592 return std::log10(t);
00593 }
00594 static inline value_type pow(const_reference t, int e)
00595 {
00596 return std::pow(t, e);
00597 }
00598 static inline value_type pow(const_reference t, const_reference e)
00599 {
00600 return std::pow(t, e);
00601 }
00602
00603
00604
00605
00606 static inline value_type pow(const_reference t, real_type e)
00607 {
00608 return std::pow(t, e);
00609 }
00610 static inline value_type pow(real_type t, const_reference e)
00611 {
00612 return std::pow(t, e);
00613 }
00614
00615 };
00616
00623 template<> struct math_traits<complex<float> >
00624 {
00625 typedef math_traits<complex<float> > self_type;
00626 typedef complex<float> value_type;
00627 typedef const complex<float> & const_reference;
00628 typedef complex<float> & reference;
00629 typedef float real_type;
00630
00631
00632
00633
00634 static inline value_type sqrt(const_reference t)
00635 {
00636 return std::sqrt(t);
00637 }
00638
00639
00640
00641 static inline value_type cos(const_reference t)
00642 {
00643 return std::cos(t);
00644 }
00645 static inline value_type cosh(const_reference t)
00646 {
00647 return std::cosh(t);
00648 }
00649 static inline value_type sin(const_reference t)
00650 {
00651 return std::sin(t);
00652 }
00653 static inline value_type sinh(const_reference t)
00654 {
00655 return std::sinh(t);
00656 }
00657 static inline value_type tan(const_reference t)
00658 {
00659 return std::tan(t);
00660 }
00661 static inline value_type tanh(const_reference t)
00662 {
00663 return std::tanh(t);
00664 }
00665
00666
00667
00668
00669
00670
00671 static inline value_type acos(const_reference t)
00672 {
00673 return FLOAT_I * log(t + sqrt(t * t - 1.0f));
00674 }
00675 static inline value_type acosh(const_reference t)
00676 {
00677 return log(t + sqrt(t * t - 1.0f));
00678 }
00679 static inline value_type asin(const_reference t)
00680 {
00681 return -FLOAT_I * log(FLOAT_I * t + sqrt(1.0f - t * t));
00682 }
00683 static inline value_type asinh(const_reference t)
00684 {
00685 return log(t + sqrt(t * t + 1.0f));
00686 }
00687 static inline value_type atan(const_reference t)
00688 {
00689 return log((1.0f + FLOAT_I * t) / (1.0f - FLOAT_I * t)) / (2.0f
00690 * FLOAT_I);
00691 }
00692 static inline value_type atan2(const_reference y, const_reference x)
00693 {
00694 return atan(y / x);
00695 }
00696 static inline value_type atanh(const_reference t)
00697 {
00698 return 0.5f * log((1.0f + t) / (1.0f - t));
00699 }
00700
00701
00702
00703
00704 static inline value_type exp(const_reference t)
00705 {
00706 return std::exp(t);
00707 }
00708 static inline value_type log(const_reference t)
00709 {
00710 return std::log(t);
00711 }
00712 static inline value_type log10(const_reference t)
00713 {
00714 return std::log10(t);
00715 }
00716 static inline value_type pow(const_reference t, int e)
00717 {
00718 return std::pow(t, e);
00719 }
00720 static inline value_type pow(const_reference t, const_reference e)
00721 {
00722 return std::pow(t, e);
00723 }
00724
00725
00726
00727
00728 static inline value_type pow(const_reference t, real_type e)
00729 {
00730 return std::pow(t, e);
00731 }
00732 static inline value_type pow(real_type t, const_reference e)
00733 {
00734 return std::pow(t, e);
00735 }
00736
00737 };
00738
00739
00740
00741
00742 inline math_traits<double>::value_type to_degrees(
00743 math_traits<double>::const_reference t)
00744 {
00745 return math_traits<double>::to_degrees(t);
00746 }
00747
00748 inline math_traits<double>::value_type to_radians(
00749 math_traits<double>::const_reference t)
00750 {
00751 return math_traits<double>::to_radians(t);
00752 }
00753
00754 inline math_traits<double>::value_type to_latitude(
00755 math_traits<double>::const_reference t)
00756 {
00757 return math_traits<double>::to_latitude(t);
00758 }
00759
00760 inline math_traits<double>::value_type to_colatitude(
00761 math_traits<double>::const_reference t)
00762 {
00763 return math_traits<double>::to_colatitude(t);
00764 }
00765
00766 inline math_traits<double>::value_type copysign(
00767 math_traits<double>::const_reference t,
00768 math_traits<double>::const_reference v )
00769 {
00770 return math_traits<double>::copysign(t,v);
00771 }
00772
00773
00774
00775
00776 #ifdef _MSC_VER // Microsoft Visual C++
00777 #ifndef NAN
00778 #define NAN std::numeric_limits<double>::quiet_NaN()
00779 #endif
00780 inline int isnan(double x) { return _isnan(x); }
00781
00782 #if (_MSC_VER < 1800 ) // Visual Sudio eariler than 2013
00783
00784 inline int round(double x) { return floor(x + 0.5); }
00785
00786 inline math_traits<double>::value_type acosh(
00787 math_traits<double>::const_reference t)
00788 {
00789 return math_traits<double>::acosh(t);
00790 }
00791 inline math_traits<double>::value_type asinh(
00792 math_traits<double>::const_reference t)
00793 {
00794 return math_traits<double>::asinh(t);
00795 }
00796 inline math_traits<double>::value_type atanh(
00797 math_traits<double>::const_reference t)
00798 {
00799 return math_traits<double>::atanh(t);
00800 }
00801
00802 #endif
00803
00804 #endif
00805 }
00806 }