API tools faq
paste
Advertisement
Swiftkill

mvector\coords

Swiftkill
Nov 16th, 2013
202
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 14.63 KB | None | 0 0
raw download clone embed print report
  1.  
  2. #ifndef __VECTOR_H
  3. #define __VECTOR_H
  4.  
  5. #include "debug.h"
  6. #include "defs.h"
  7. //____________________________________________________________________________
  8. //
  9. // Определение класса вектора
  10. // Vector of 3 double precision(F64) value.
  11. class tpVector
  12. {
  13. public:
  14.     // Module of the vector.
  15.     inline F64      Mod()const;
  16.     // Normalize of the vector.
  17.     inline tpVector Norm()  {return (*this) * (1.0f/Mod());};
  18.    
  19.     // Element of vector
  20.     F64 operator[] (int i) const { return v[i]; }
  21.     // Reference to element of the vector.
  22.     F64& operator[] (int i) {return v[i];}
  23.  
  24.     // Constructor.
  25.     tpVector( F64 x0, F64 x1, F64 x2)
  26.     {
  27.         v[0]=x0; v[1]=x1; v[2]=x2;
  28.     };
  29.  
  30. protected:
  31.     // Default constructor.
  32.     tpVector()
  33.     {
  34.         v[0] = 0.0;
  35.         v[1] = 0.0;
  36.         v[2] = 0.0;
  37.     };
  38.    
  39.     // Addition, Subtraction, Multiplication, Division assignment operator.
  40.     inline tpVector&    operator+=(const tpVector &a);
  41.     inline tpVector&    operator-=(const tpVector &a);
  42.     inline tpVector&    operator*=(const F64 a);
  43.     inline tpVector&    operator/=(const F64 a);
  44.     tpVector&   operator+(const tpVector &b)const
  45.         {tpVector   r(*this);return r+=b;};
  46.     tpVector&   operator-(const tpVector& b)const
  47.         {tpVector   r(*this);return r-=b;};
  48.     tpVector&   operator *(const F64 b) const
  49.         {tpVector   r(*this);return r*=b;};
  50.     tpVector&   operator /(const F64 b) const
  51.         {tpVector   r(*this);return r/=b;};
  52.     // Compare operator.
  53.     bool        operator <(const tpVector& a) const
  54.         {return (v[0] < a.v[0]) && (v[1] < a.v[1]) && (v[2] < a.v[2]);};
  55.     bool        operator <=(const tpVector& a) const
  56.         {return (v[0] <= a.v[0]) && (v[1] <= a.v[1]) && (v[2] <= a.v[2]);};
  57.  
  58.     friend class tpDecartC;
  59.     friend class tpDecartV;
  60.     friend class tpPolarC;
  61. protected:
  62.     /// Elements of the vector
  63.     TRM_ALIGN_16(F64 v[3]);
  64. };
  65.  
  66. F64     tpVector::Mod()const
  67. {
  68.     return sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
  69. };
  70.  
  71. tpVector& tpVector::operator +=(const tpVector& a)
  72. {
  73.     v[0] += a.v[0];
  74.     v[1] += a.v[1];
  75.     v[2] += a.v[2];
  76.     return *this;
  77. };
  78.  
  79. tpVector& tpVector::operator -=(const tpVector& a)
  80. {
  81.     v[0] -= a.v[0];
  82.     v[1] -= a.v[1];
  83.     v[2] -= a.v[2];
  84.     return *this;
  85. };
  86.  
  87. tpVector& tpVector::operator *=(const F64 a)
  88. {
  89.  
  90.     v[0] *= a;
  91.     v[1] *= a;
  92.     v[2] *= a;
  93.     return *this;
  94. };
  95.  
  96. tpVector& tpVector::operator /=(const F64 a)
  97. {
  98.  
  99.     v[0] /= a;
  100.     v[1] /= a;
  101.     v[2] /= a;
  102.     return *this;
  103. };
  104.  
  105.  
  106. /**************************************************************************/
  107. // 06.09.11 - <math.h> заменен на <qmath.h> для определения M_PI
  108. /**************************************************************************/
  109. #ifndef __COORD_H
  110. #define __COORD_H
  111.  
  112. #include <qmath.h>
  113. #include "params.h"
  114. #include "mvector.h"
  115. #include "debug.h"
  116. #include "defs.h"
  117.  
  118. /// type of the "time" value.
  119. typedef F64 tpTime;
  120. /// type of the "range" value.
  121. typedef F64 tpRange;
  122. /// type of the "velocity" value.
  123. typedef F64 tpVelocity;
  124. /// type of the "angle velocity" value.
  125. typedef F64 tpAngleVel;
  126.  
  127. /// Радиус земли - Radius of the Earth [m].
  128. const   tpRange GeoRadius = 6375245;
  129. /**
  130. * Эффективный радиус земли
  131. * const tpRange GeoRadiusEff = 4.*GeoRadius/3.;
  132. */
  133. #define GeoRadiusEff 8500326.6
  134.  
  135. /// Пересчет градусов в радианы
  136. template <class T> inline
  137. F64 GradToRad(T t)
  138.     {return t* F64_DEG_TO_RAD;}
  139. /// Пересчет радиан в градусы
  140. template <class T> inline
  141. F64 RadToGrad(T t)
  142.     {return t* F64_RAD_TO_DEG;}
  143.  
  144. //____________________________________________________________________________
  145. /**
  146. *   Класс, описывающий угловую величину.
  147. */
  148. class tpAngle
  149. {
  150. public:
  151.     /** Default constructor. */
  152.     tpAngle()
  153. #ifndef NDEBUG
  154.     :   flag(0)
  155. #endif
  156.     {};
  157.    
  158.     /** Constructor. */
  159.     tpAngle(const F64 x)
  160. #ifndef NDEBUG
  161.     :   flag(1)
  162. #endif
  163.     {
  164.         angle = x;
  165.         if(x<-2*M_PI || x>=2*M_PI)
  166.             angle = fmod(x,(F64)2*M_PI);
  167.         if(angle<-M_PI)
  168.             angle+=M_2PI;
  169.         else    if(angle>=M_PI)
  170.             angle-=M_2PI;
  171.     };
  172.    
  173.     operator    F64()   const   { c2assert(flag); return angle; };
  174.     /** Addition assignment operator. */
  175.     tpAngle&    operator +=(const tpAngle& d);
  176.     /** Subtraction assignment operator. */
  177.     tpAngle&    operator -=(const tpAngle& d);
  178.  
  179. private:
  180.     F64     angle;
  181. #ifndef NDEBUG
  182.     /** Init flag. */
  183.     int flag;
  184. #endif
  185. };
  186.  
  187.  
  188. inline  tpAngle&    tpAngle::operator +=(const tpAngle& d)
  189. {
  190.     angle = tpAngle(angle+=d);
  191.     return  *this;
  192. }
  193.  
  194. inline  tpAngle&    tpAngle::operator -=(const tpAngle& d)
  195. {
  196.     angle = tpAngle(angle-=d);
  197.     return  *this;
  198. }
  199.  
  200. //*******************************************
  201. //Функции преобразования углов
  202.  
  203. /** Приведение угла в пределы [0, 2*Pi] */
  204. inline F64 angleNorm(F64 angle)
  205. {
  206.     while (angle >= M_2PI)
  207.         angle -= M_2PI;
  208.     while (angle < 0)      
  209.         angle += M_2PI;
  210.     return angle;
  211. };
  212.  
  213. /**  Попадание угла в сектор */
  214. inline int angleInSector(F64 angle, F64 angle1, F64 angle2)
  215. {
  216.     if (angle2 > angle1)
  217.     {
  218.         if (angle >= angle1 && angle2 >= angle)
  219.             return 1;
  220.         return 0;
  221.     }
  222.     else
  223.     {
  224.         if (angle >= angle2 && angle1 >= angle)
  225.             return 1;
  226.         return 0;
  227.     }
  228. };
  229.  
  230. /** function-angle normalization: angle [rad] */
  231. inline F64  NORM_ANGLE_RAD(F64 angle)
  232. {
  233.     if(angle >= M_2PI)
  234.         return angle-M_2PI;
  235.     else if(angle < 0)
  236.         return angle+M_2PI;
  237.     return angle;
  238. }
  239.  
  240. /** function-angle normalization: angle [deg] */
  241. inline F64 NORM_ANGLE_GRAD(F64 angle)
  242. {
  243.     if(angle >= 360)
  244.         return angle-360;
  245.     else if(angle < 0)
  246.         return angle+360;
  247.     return angle;
  248. }
  249.  
  250. /** function - convert angle [deg] to [rad] */
  251. inline F64 TO_RAD(F64 angle)
  252. {
  253.     return angle * M_PI_DIV_180;     // /180.*M_PI;
  254. };
  255.  
  256. /** function - convert angle [rad] to [deg] */
  257. inline F64 TO_GRAD(F64 angle)
  258. {
  259.     return angle * M_180_DIV_PI;  // *180./M_PI;
  260. };
  261.  
  262. //******
  263.  
  264. class tpDecartV;
  265. class tpPolarC;
  266.  
  267. //____________________________________________________________________________
  268. // Определение класса вектора декартовых координат
  269. /**
  270.  *  Class of the Cartesian (Decart) coordinates vector.
  271.  *  - 1-st element - coordinate X (Nord) [m]
  272.  *  - 2-nd element - coordinate Y (Zenith) [m]
  273.  *  - 3-rd element - coordinate Z (East) [m]
  274.  */
  275. class tpDecartC : public tpVector
  276. {
  277. public:
  278.     // Default constructor.
  279.     tpDecartC() : tpVector(){};
  280.     tpDecartC(tpRange x, tpRange y, tpRange z) : tpVector(x,y,z){};
  281.     tpDecartC(const tpPolarC& d);
  282.    
  283.     //  Get X (Nord), Get Y (Zenith). Get Z (East)
  284.     tpRange     X()const    {return v[0];};
  285.     tpRange     Y()const    {return v[1];};
  286.     tpRange     Z()const    {return v[2];};
  287.     //  Get Altitude.
  288.     tpRange   H()
  289.     {
  290. //      return tpRange((*this)[1]
  291. //          + ((*this)[0]*(*this)[0]+(*this)[2]*(*this)[2])/(2.*GeoRadius));
  292.         return tpRange(sqrt( (v[1]+GeoRadiusEff)*(v[1]+GeoRadiusEff)+
  293.             v[0]*v[0]+v[2]*v[2] )- GeoRadiusEff) ;
  294.     };
  295.  
  296.     tpRange     Mod()const  {return tpVector::Mod();};
  297.     tpRange     Rhor() {return tpRange(sqrt(v[0]*v[0]+v[2]*v[2]));};
  298.  
  299.     // Addition, Subtraction assignment operator.
  300.     void    operator +=(const tpDecartC& d)const
  301.         {const_cast<tpDecartC*>(this)->tpVector::operator+=(d);};
  302.     void    operator -=(const tpDecartC& d) const
  303.         {const_cast<tpDecartC*>(this)->tpVector::operator-=(d);};
  304.     // Addition, Subtraction operator.
  305.     const   tpDecartC   operator+(const tpDecartC& d)const
  306.         {return tpDecartC( tpVector::operator+(d));};
  307.     const   tpDecartC   operator-(const tpDecartC& d)const
  308.         {return tpDecartC( tpVector::operator-(d));};
  309.     // Division operator.
  310.     const   tpDecartV   operator /(const tpTime t) const;
  311.  
  312. private:
  313.     tpDecartC(const tpVector& d) : tpVector(d){};
  314.  
  315. friend class tpDecartV;
  316. };
  317.  
  318.  
  319. //____________________________________________________________________________
  320. // Определение класса вектора декартовых скоростей
  321. /**
  322.  *  Class of the velocity vector in the Cartesian coordinate system.
  323.  *  - 1-st element - velocity along axis X (Nord) [m/s]
  324.  *  - 2-nd element - velocity along axis Y (Zenith) [m/s]
  325.  *  - 3-rd element - velocity along axis Z (East) [m/s]
  326.  */
  327. class tpDecartV : public tpVector
  328. {
  329. public:
  330.     // Default constructor.
  331.     tpDecartV() : tpVector(){};
  332.     // Constructor.
  333.     tpDecartV(const tpVelocity x0,const tpVelocity x1,const tpVelocity x2):
  334.                                                     tpVector(x0,x1,x2){};
  335.    
  336.     //  Get velocity along axis X (Nord), axis Y (Zenith), axis Z (East)
  337.     tpVelocity      Vx()const   {return v[0];};
  338.     tpVelocity      Vy()const   {return v[1];};
  339.     tpVelocity      Vz()const   {return v[2];};
  340.  
  341.     // Calculate full velocity (module of the velocity vector).
  342.     tpVelocity      Mod()const 
  343.     {
  344.         return tpVector::Mod();
  345.     };
  346.     // Calculate horizontal velocity
  347.     tpVelocity      ModXZ()const
  348.     {
  349.         return sqrt(v[0]*v[0] + v[2]*v[2]);
  350.     }
  351.  
  352.     // Addition, Subtraction assignment operator.
  353.     void    operator +=(const tpDecartV& d) const
  354.         {const_cast<tpDecartV*>(this)->tpVector::operator+=(d);};
  355.     void    operator -=(const tpDecartV& d) const
  356.         {const_cast<tpDecartV*>(this)->tpVector::operator-=(d);};
  357.     // Addition, Subtraction, Multiplication  operator.
  358.     tpDecartV   operator+(const tpDecartV& d)const
  359.         {return tpDecartV( tpVector::operator+(d));};
  360.     tpDecartV   operator-(const tpDecartV& d)const
  361.         {return tpDecartV( tpVector::operator-(d));};
  362.     tpDecartC   operator *(const tpTime t) const;
  363. //      {return tpDecartC(tpVector::operator*(t));};
  364. private:
  365.     tpDecartV(const tpVector& d) : tpVector(d){};
  366. friend class tpDecartC;
  367. };
  368.  
  369. inline tpDecartC    tpDecartV::operator *(const tpTime t) const
  370. {
  371.     return tpDecartC(tpVector::operator*(t));
  372. };
  373.  
  374. inline  const   tpDecartV   tpDecartC::operator /(const tpTime t) const
  375. {
  376.     return tpDecartV(tpVector::operator/(t));
  377. };
  378.  
  379.  
  380.  
  381.  
  382. //____________________________________________________________________________
  383. class tpAzimuth:public tpAngle
  384. {
  385. public:
  386.     tpAzimuth():tpAngle()   {};
  387.     tpAzimuth(F64 x):tpAngle(x) {};
  388. };
  389.  
  390. //____________________________________________________________________________
  391. class tpElevation:public tpAngle
  392. {
  393. public:
  394.     tpElevation():tpAngle() {};
  395.     tpElevation(F64 x):tpAngle(x)   {};
  396. };
  397.  
  398.  
  399. //____________________________________________________________________________
  400. // Определение класса вектора полярных координат
  401. /**
  402.  *   Class of the coordinates vector in the polar coordinate system.
  403.  *  It designate for operate witch the vector of the polar coordinates
  404.  *  a whole rather than witch separate its element.
  405.  *  - 1-vector element - Range [m]
  406.  *  - 2-vector element - Azimuth [rad]
  407.  *  - 3-vector element - Elevation [rad]
  408.  */
  409.  
  410. class tpPolarC : public tpVector
  411. {
  412. public:
  413.     // Default constructor.
  414.     tpPolarC() : tpVector(){};
  415.     // Constructor - Create vector of the Polar coordinates.
  416.     tpPolarC(const tpPolarC& c) : tpVector(c){};
  417.     tpPolarC(tpRange r, const tpAzimuth &a, const tpElevation &e):
  418.         tpVector(r,a,e){};
  419.     // Constructor - Create vector of the Decart coordinates.
  420.     tpPolarC(const tpDecartC& d);
  421.  
  422.     // Get range, azimuth, elevation
  423.     const   tpRange&            Range()const        {return v[0];}
  424.     const   tpAzimuth&          Azimuth()const      {return v[1];}
  425.     const   tpElevation&        Elevation()const    {return v[2];}
  426.  
  427.     tpPolarC    &operator +=(const tpPolarC& d);
  428.     tpPolarC    &operator -=(const tpPolarC& d);
  429.     tpPolarC    &operator +=(const tpRange& d);
  430.     tpPolarC    &operator -=(const tpRange& d);
  431.     tpPolarC    &operator +=(const tpAzimuth& d);
  432.     tpPolarC    &operator -=(const tpAzimuth& d);
  433.     tpPolarC    &operator +=(const tpElevation& d);
  434.     tpPolarC    &operator -=(const tpElevation& d);
  435.  
  436.     tpPolarC    operator +(const tpPolarC& d) const
  437.         {return tpPolarC( tpVector::operator+(d));};
  438.     tpPolarC    operator -(const tpPolarC& d) const
  439.         {return tpPolarC( tpVector::operator-(d));};
  440.     tpPolarC    operator +(const tpRange& d) const;
  441.     tpPolarC    operator -(const tpRange& d) const;
  442.     tpPolarC    operator +(const tpAzimuth& d) const;
  443.     tpPolarC    operator -(const tpAzimuth& d) const;
  444.     tpPolarC    operator +(const tpElevation& d) const;
  445.     tpPolarC    operator -(const tpElevation& d) const;
  446. protected:
  447.     F64 Mod()const;
  448.     tpVector Norm()const;
  449. private:
  450.     tpPolarC(const tpVector& d):tpVector(tpRange(d[0]),
  451.                         tpAzimuth(d[1]),tpElevation(d[2])){};
  452. };
  453.  
  454. //  inline реализация методов --------------------------------------------
  455.  
  456. inline  tpPolarC&       tpPolarC::operator +=(const tpPolarC& d)
  457. {
  458.     (*this)[0] += d.Range();
  459.     (*this)[1] = tpAzimuth((*this)[1] + d.Azimuth());
  460.     (*this)[2] = tpElevation((*this)[2] + d.Elevation());
  461.     return  *this;
  462. }
  463.  
  464. inline  tpPolarC&       tpPolarC::operator -=(const tpPolarC& d)
  465. {
  466.     (*this)[0] -= F64(d.Range());
  467.     (*this)[1] = tpAzimuth((*this)[1]-d.Azimuth());
  468.     (*this)[2] = tpElevation((*this)[2]-d.Elevation());
  469.     return  *this;
  470. }
  471.  
  472. inline  tpPolarC&       tpPolarC::operator +=(const tpRange& r)
  473. {
  474.     (*this)[0] += r;
  475.     return  *this;
  476. };
  477.  
  478. inline  tpPolarC&       tpPolarC::operator -=(const tpRange& r)
  479. {
  480.     (*this)[0] -= r;
  481.     return  *this;
  482. };
  483.  
  484. inline  tpPolarC&       tpPolarC::operator +=(const tpAzimuth& a)
  485. {
  486.     (*this)[1] = tpAzimuth((*this)[1]+F64(a));
  487.     return  *this;
  488. };
  489.  
  490. inline  tpPolarC&       tpPolarC::operator -=(const tpAzimuth& a)
  491. {
  492.     (*this)[1] = tpAzimuth((*this)[1]-F64(a));
  493.     return  *this;
  494. };
  495.  
  496. inline  tpPolarC&       tpPolarC::operator +=(const tpElevation& e)
  497. {
  498.     (*this)[2] = tpElevation((*this)[2]+F64(e));
  499.     return  *this;
  500. };
  501.  
  502. inline  tpPolarC&       tpPolarC::operator -=(const tpElevation& e)
  503. {
  504.     (*this)[2] = tpElevation((*this)[2]-F64(e));
  505.     return  *this;
  506. };
  507.  
  508.  
  509. inline  tpPolarC    tpPolarC::operator +(const tpRange& r) const
  510. {
  511.     return tpPolarC(Range()+r,Azimuth(),Elevation());
  512. };
  513.  
  514. inline  tpPolarC    tpPolarC::operator -(const tpRange& r) const
  515. {
  516.     return tpPolarC(Range()-r,Azimuth(),Elevation());
  517. };
  518.  
  519. inline  tpPolarC    tpPolarC::operator +(const tpAzimuth& a) const
  520. {
  521.     return tpPolarC(Range(),tpAzimuth(Azimuth()+a),Elevation());
  522. };
  523.  
  524. inline  tpPolarC    tpPolarC::operator -(const tpAzimuth& a) const
  525. {
  526.     return tpPolarC(Range(),tpAzimuth(Azimuth()-a),Elevation());
  527. };
  528.  
  529. inline  tpPolarC    tpPolarC::operator +(const tpElevation& e) const
  530. {
  531.     return tpPolarC(Range(),Azimuth(),tpElevation(Elevation()+e));
  532. };
  533.  
  534. inline  tpPolarC    tpPolarC::operator -(const tpElevation& e) const
  535. {
  536.     return tpPolarC(Range(),Azimuth(),tpElevation(Elevation()-e));
  537. };
  538.  
  539.  
  540. inline  tpDecartC::tpDecartC(const tpPolarC& d)
  541. {
  542.     register tpRange    d_hor   = d.Range()*cos(d.Elevation());
  543.     *this =  tpDecartC( d_hor*cos(d.Azimuth()),
  544.         d.Range()*sin(d.Elevation()),
  545.         d_hor*sin(d.Azimuth()));
  546. };
  547.  
  548. inline  tpPolarC::tpPolarC(const tpDecartC& a)
  549. {
  550.     // initialization intermediate variable
  551.     register F64    sq_d_1_hor= a.X() * a.X() + a.Z() * a.Z();
  552.     register F64    d_1_hor   = sqrt( sq_d_1_hor );
  553.     register F64    sq_d_1    = sq_d_1_hor + a.Y() * a.Y();
  554.     register F64    d_1       = sqrt( sq_d_1 );
  555.     // recalculation of coordinates in system of polar coordinates  
  556.     *this = tpPolarC(   d_1,
  557.         a.Z()!=0 || a.X()!=0 ? atan2(a.Z(),a.X()) : 0,
  558.         a.Y()!=0 || d_1_hor!=0 ? atan2(a.Y(),d_1_hor) : 0 );
  559. };
  560.  
  561. #endif //__COORD_H
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!