home *** CD-ROM | disk | FTP | other *** search
/ GameStar 2006 March / Gamestar_82_2006-03_dvd.iso / DVDStar / Editace / quake4_sdkv10.exe / source / idlib / math / Pluecker.h < prev    next >
C/C++ Source or Header  |  2005-11-14  |  11KB  |  383 lines
  1.  
  2. #ifndef __MATH_PLUECKER_H__
  3. #define __MATH_PLUECKER_H__
  4.  
  5. /*
  6. ===============================================================================
  7.  
  8.     Pluecker coordinate
  9.  
  10. ===============================================================================
  11. */
  12.  
  13. class idPluecker {
  14. public:    
  15.                     idPluecker( void );
  16.                     explicit idPluecker( const float *a );
  17.                     explicit idPluecker( const idVec3 &start, const idVec3 &end );
  18.                     explicit idPluecker( const float a1, const float a2, const float a3, const float a4, const float a5, const float a6 );
  19.  
  20.     float            operator[]( const int index ) const;
  21.     float &            operator[]( const int index );
  22.     idPluecker        operator-() const;                                            // flips the direction
  23.     idPluecker        operator*( const float a ) const;
  24.     idPluecker        operator/( const float a ) const;
  25.     float            operator*( const idPluecker &a ) const;                        // permuted inner product
  26.     idPluecker        operator-( const idPluecker &a ) const;
  27.     idPluecker        operator+( const idPluecker &a ) const;
  28.     idPluecker &    operator*=( const float a );
  29.     idPluecker &    operator/=( const float a );
  30.     idPluecker &    operator+=( const idPluecker &a );
  31.     idPluecker &    operator-=( const idPluecker &a );
  32.  
  33.     bool            Compare( const idPluecker &a ) const;                        // exact compare, no epsilon
  34.     bool            Compare( const idPluecker &a, const float epsilon ) const;    // compare with epsilon
  35.     bool            operator==(    const idPluecker &a ) const;                    // exact compare, no epsilon
  36.     bool            operator!=(    const idPluecker &a ) const;                    // exact compare, no epsilon
  37.  
  38.     void             Set( const float a1, const float a2, const float a3, const float a4, const float a5, const float a6 );
  39.     void            Zero( void );
  40.  
  41.     idPluecker        Rotate( const idMat3 &rotation ) const;
  42.     idPluecker        Translate( const idVec3 &translation ) const;
  43.     idPluecker        TranslateAndRotate( const idVec3 &translation, const idMat3 &rotation ) const;
  44.  
  45.     void            FromLine( const idVec3 &start, const idVec3 &end );            // pluecker from line
  46.     void            FromRay( const idVec3 &start, const idVec3 &dir );            // pluecker from ray
  47.     bool            FromPlanes( const idPlane &p1, const idPlane &p2 );            // pluecker from intersection of planes
  48.     bool            ToLine( idVec3 &start, idVec3 &end ) const;                    // pluecker to line
  49.     bool            ToRay( idVec3 &start, idVec3 &dir ) const;                    // pluecker to ray
  50.     void            ToDir( idVec3 &dir ) const;                                    // pluecker to direction
  51.     float            PermutedInnerProduct( const idPluecker &a ) const;            // pluecker permuted inner product
  52.     float            Distance3DSqr( const idPluecker &a ) const;                    // pluecker line distance
  53.  
  54.     float            Length( void ) const;                                        // pluecker length
  55.     float            LengthSqr( void ) const;                                    // pluecker squared length
  56.     idPluecker        Normalize( void ) const;                                    // pluecker normalize
  57.     float            NormalizeSelf( void );                                        // pluecker normalize
  58.  
  59.     int                GetDimension( void ) const;
  60.  
  61.     const float *    ToFloatPtr( void ) const;
  62.     float *            ToFloatPtr( void );
  63.     const char *    ToString( int precision = 2 ) const;
  64.  
  65. private:
  66.     float            p[6];
  67. };
  68.  
  69. extern idPluecker pluecker_origin;
  70. #define pluecker_zero pluecker_origin
  71.  
  72. ID_INLINE idPluecker::idPluecker( void ) {
  73. }
  74.  
  75. ID_INLINE idPluecker::idPluecker( const float *a ) {
  76.     memcpy( p, a, 6 * sizeof( float ) );
  77. }
  78.  
  79. ID_INLINE idPluecker::idPluecker( const idVec3 &start, const idVec3 &end ) {
  80.     FromLine( start, end );
  81. }
  82.  
  83. ID_INLINE idPluecker::idPluecker( const float a1, const float a2, const float a3, const float a4, const float a5, const float a6 ) {
  84.     p[0] = a1;
  85.     p[1] = a2;
  86.     p[2] = a3;
  87.     p[3] = a4;
  88.     p[4] = a5;
  89.     p[5] = a6;
  90. }
  91.  
  92. ID_INLINE idPluecker idPluecker::operator-() const {
  93.     return idPluecker( -p[0], -p[1], -p[2], -p[3], -p[4], -p[5] );
  94. }
  95.  
  96. ID_INLINE float idPluecker::operator[]( const int index ) const {
  97.     return p[index];
  98. }
  99.  
  100. ID_INLINE float &idPluecker::operator[]( const int index ) {
  101.     return p[index];
  102. }
  103.  
  104. ID_INLINE idPluecker idPluecker::operator*( const float a ) const {
  105.     return idPluecker( p[0]*a, p[1]*a, p[2]*a, p[3]*a, p[4]*a, p[5]*a );
  106. }
  107.  
  108. ID_INLINE float idPluecker::operator*( const idPluecker &a ) const {
  109.     return p[0] * a.p[4] + p[1] * a.p[5] + p[2] * a.p[3] + p[4] * a.p[0] + p[5] * a.p[1] + p[3] * a.p[2];
  110. }
  111.  
  112. ID_INLINE idPluecker idPluecker::operator/( const float a ) const {
  113.     float inva;
  114.  
  115.     assert( a != 0.0f );
  116.     inva = 1.0f / a;
  117.     return idPluecker( p[0]*inva, p[1]*inva, p[2]*inva, p[3]*inva, p[4]*inva, p[5]*inva );
  118. }
  119.  
  120. ID_INLINE idPluecker idPluecker::operator+( const idPluecker &a ) const {
  121.     return idPluecker( p[0] + a[0], p[1] + a[1], p[2] + a[2], p[3] + a[3], p[4] + a[4], p[5] + a[5] );
  122. }
  123.  
  124. ID_INLINE idPluecker idPluecker::operator-( const idPluecker &a ) const {
  125.     return idPluecker( p[0] - a[0], p[1] - a[1], p[2] - a[2], p[3] - a[3], p[4] - a[4], p[5] - a[5] );
  126. }
  127.  
  128. ID_INLINE idPluecker &idPluecker::operator*=( const float a ) {
  129.     p[0] *= a;
  130.     p[1] *= a;
  131.     p[2] *= a;
  132.     p[3] *= a;
  133.     p[4] *= a;
  134.     p[5] *= a;
  135.     return *this;
  136. }
  137.  
  138. ID_INLINE idPluecker &idPluecker::operator/=( const float a ) {
  139.     float inva;
  140.  
  141.     assert( a != 0.0f );
  142.     inva = 1.0f / a;
  143.     p[0] *= inva;
  144.     p[1] *= inva;
  145.     p[2] *= inva;
  146.     p[3] *= inva;
  147.     p[4] *= inva;
  148.     p[5] *= inva;
  149.     return *this;
  150. }
  151.  
  152. ID_INLINE idPluecker &idPluecker::operator+=( const idPluecker &a ) {
  153.     p[0] += a[0];
  154.     p[1] += a[1];
  155.     p[2] += a[2];
  156.     p[3] += a[3];
  157.     p[4] += a[4];
  158.     p[5] += a[5];
  159.     return *this;
  160. }
  161.  
  162. ID_INLINE idPluecker &idPluecker::operator-=( const idPluecker &a ) {
  163.     p[0] -= a[0];
  164.     p[1] -= a[1];
  165.     p[2] -= a[2];
  166.     p[3] -= a[3];
  167.     p[4] -= a[4];
  168.     p[5] -= a[5];
  169.     return *this;
  170. }
  171.  
  172. ID_INLINE bool idPluecker::Compare( const idPluecker &a ) const {
  173.     return ( ( p[0] == a[0] ) && ( p[1] == a[1] ) && ( p[2] == a[2] ) &&
  174.             ( p[3] == a[3] ) && ( p[4] == a[4] ) && ( p[5] == a[5] ) );
  175. }
  176.  
  177. ID_INLINE bool idPluecker::Compare( const idPluecker &a, const float epsilon ) const {
  178.     if ( idMath::Fabs( p[0] - a[0] ) > epsilon ) {
  179.         return false;
  180.     }
  181.             
  182.     if ( idMath::Fabs( p[1] - a[1] ) > epsilon ) {
  183.         return false;
  184.     }
  185.  
  186.     if ( idMath::Fabs( p[2] - a[2] ) > epsilon ) {
  187.         return false;
  188.     }
  189.  
  190.     if ( idMath::Fabs( p[3] - a[3] ) > epsilon ) {
  191.         return false;
  192.     }
  193.  
  194.     if ( idMath::Fabs( p[4] - a[4] ) > epsilon ) {
  195.         return false;
  196.     }
  197.  
  198.     if ( idMath::Fabs( p[5] - a[5] ) > epsilon ) {
  199.         return false;
  200.     }
  201.  
  202.     return true;
  203. }
  204.  
  205. ID_INLINE bool idPluecker::operator==( const idPluecker &a ) const {
  206.     return Compare( a );
  207. }
  208.  
  209. ID_INLINE bool idPluecker::operator!=( const idPluecker &a ) const {
  210.     return !Compare( a );
  211. }
  212.  
  213. ID_INLINE void idPluecker::Set( const float a1, const float a2, const float a3, const float a4, const float a5, const float a6 ) {
  214.     p[0] = a1;
  215.     p[1] = a2;
  216.     p[2] = a3;
  217.     p[3] = a4;
  218.     p[4] = a5;
  219.     p[5] = a6;
  220. }
  221.  
  222. ID_INLINE void idPluecker::Zero( void ) {
  223.     p[0] = p[1] = p[2] = p[3] = p[4] = p[5] = 0.0f;
  224. }
  225.  
  226. ID_INLINE idPluecker idPluecker::Rotate( const idMat3 &rotation ) const {
  227.     idPluecker result;
  228.     result[0] =   rotation[ 0 ].z * p[3] - rotation[ 1 ].z * p[1] + rotation[ 2 ].z * p[0];
  229.     result[1] = - rotation[ 0 ].y * p[3] + rotation[ 1 ].y * p[1] - rotation[ 2 ].y * p[0];
  230.     result[2] =   rotation[ 0 ].x * p[2] - rotation[ 1 ].x * p[5] + rotation[ 2 ].x * p[4];
  231.     result[3] =   rotation[ 0 ].x * p[3] - rotation[ 1 ].x * p[1] + rotation[ 2 ].x * p[0];
  232.     result[4] =   rotation[ 0 ].z * p[2] - rotation[ 1 ].z * p[5] + rotation[ 2 ].z * p[4];
  233.     result[5] = - rotation[ 0 ].y * p[2] + rotation[ 1 ].y * p[5] - rotation[ 2 ].y * p[4];
  234.     return result;
  235. }
  236.  
  237. ID_INLINE idPluecker idPluecker::Translate( const idVec3 &translation ) const {
  238.     idPluecker result;
  239.     result[0] = p[0] + translation[0] * p[5] + p[2] * translation[1];
  240.     result[1] = p[1] - translation[0] * p[4] + p[2] * translation[2];
  241.     result[2] = p[2];
  242.     result[3] = p[3] - translation[1] * p[4] - p[5] * translation[2];
  243.     result[4] = p[4];
  244.     result[5] = p[5];
  245.     return result;
  246. }
  247.  
  248. ID_INLINE idPluecker idPluecker::TranslateAndRotate( const idVec3 &translation, const idMat3 &rotation ) const {
  249.     idVec3 t;
  250.     idPluecker result;
  251.     t[0] = p[0] + translation[0] * p[5] + p[2] * translation[1];
  252.     t[1] = p[1] - translation[0] * p[4] + p[2] * translation[2];
  253.     t[2] = p[3] - translation[1] * p[4] - p[5] * translation[2];
  254.     result[0] =   rotation[ 0 ].z * t[2] - rotation[ 1 ].z * t[1] + rotation[ 2 ].z * t[0];
  255.     result[1] = - rotation[ 0 ].y * t[2] + rotation[ 1 ].y * t[1] - rotation[ 2 ].y * t[0];
  256.     result[2] =   rotation[ 0 ].x * p[2] - rotation[ 1 ].x * p[5] + rotation[ 2 ].x * p[4];
  257.     result[3] =   rotation[ 0 ].x * t[2] - rotation[ 1 ].x * t[1] + rotation[ 2 ].x * t[0];
  258.     result[4] =   rotation[ 0 ].z * p[2] - rotation[ 1 ].z * p[5] + rotation[ 2 ].z * p[4];
  259.     result[5] = - rotation[ 0 ].y * p[2] + rotation[ 1 ].y * p[5] - rotation[ 2 ].y * p[4];
  260.     return result;
  261. }
  262.  
  263. ID_INLINE void idPluecker::FromLine( const idVec3 &start, const idVec3 &end ) {
  264.     p[0] = start[0] * end[1] - end[0] * start[1];
  265.     p[1] = start[0] * end[2] - end[0] * start[2];
  266.     p[2] = start[0] - end[0];
  267.     p[3] = start[1] * end[2] - end[1] * start[2];
  268.     p[4] = start[2] - end[2];
  269.     p[5] = end[1] - start[1];
  270. }
  271.  
  272. ID_INLINE void idPluecker::FromRay( const idVec3 &start, const idVec3 &dir ) {
  273.     p[0] = start[0] * dir[1] - dir[0] * start[1];
  274.     p[1] = start[0] * dir[2] - dir[0] * start[2];
  275.     p[2] = -dir[0];
  276.     p[3] = start[1] * dir[2] - dir[1] * start[2];
  277.     p[4] = -dir[2];
  278.     p[5] = dir[1];
  279. }
  280.  
  281. ID_INLINE bool idPluecker::ToLine( idVec3 &start, idVec3 &end ) const {
  282.     idVec3 dir1, dir2;
  283.     float d;
  284.  
  285.     dir1[0] = p[3];
  286.     dir1[1] = -p[1];
  287.     dir1[2] = p[0];
  288.  
  289.     dir2[0] = -p[2];
  290.     dir2[1] = p[5];
  291.     dir2[2] = -p[4];
  292.  
  293.     d = dir2 * dir2;
  294.     if ( d == 0.0f ) {
  295.         return false; // pluecker coordinate does not represent a line
  296.     }
  297.  
  298.     start = dir2.Cross(dir1) * (1.0f / d);
  299.     end = start + dir2;
  300.     return true;
  301. }
  302.  
  303. ID_INLINE bool idPluecker::ToRay( idVec3 &start, idVec3 &dir ) const {
  304.     idVec3 dir1;
  305.     float d;
  306.  
  307.     dir1[0] = p[3];
  308.     dir1[1] = -p[1];
  309.     dir1[2] = p[0];
  310.  
  311.     dir[0] = -p[2];
  312.     dir[1] = p[5];
  313.     dir[2] = -p[4];
  314.  
  315.     d = dir * dir;
  316.     if ( d == 0.0f ) {
  317.         return false; // pluecker coordinate does not represent a line
  318.     }
  319.  
  320.     start = dir.Cross(dir1) * (1.0f / d);
  321.     return true;
  322. }
  323.  
  324. ID_INLINE void idPluecker::ToDir( idVec3 &dir ) const {
  325.     dir[0] = -p[2];
  326.     dir[1] = p[5];
  327.     dir[2] = -p[4];
  328. }
  329.  
  330. ID_INLINE float idPluecker::PermutedInnerProduct( const idPluecker &a ) const {
  331.     return p[0] * a.p[4] + p[1] * a.p[5] + p[2] * a.p[3] + p[4] * a.p[0] + p[5] * a.p[1] + p[3] * a.p[2];
  332. }
  333.  
  334. ID_INLINE float idPluecker::Length( void ) const {
  335.     return ( float )idMath::Sqrt( p[5] * p[5] + p[4] * p[4] + p[2] * p[2] );
  336. }
  337.  
  338. ID_INLINE float idPluecker::LengthSqr( void ) const {
  339.     return ( p[5] * p[5] + p[4] * p[4] + p[2] * p[2] );
  340. }
  341.  
  342. ID_INLINE float idPluecker::NormalizeSelf( void ) {
  343.     float l, d;
  344.  
  345.     l = LengthSqr();
  346.     if ( l == 0.0f ) {
  347.         return l; // pluecker coordinate does not represent a line
  348.     }
  349.     d = idMath::InvSqrt( l );
  350.     p[0] *= d;
  351.     p[1] *= d;
  352.     p[2] *= d;
  353.     p[3] *= d;
  354.     p[4] *= d;
  355.     p[5] *= d;
  356.     return d * l;
  357. }
  358.  
  359. ID_INLINE idPluecker idPluecker::Normalize( void ) const {
  360.     float d;
  361.  
  362.     d = LengthSqr();
  363.     if ( d == 0.0f ) {
  364.         return *this; // pluecker coordinate does not represent a line
  365.     }
  366.     d = idMath::InvSqrt( d );
  367.     return idPluecker( p[0]*d, p[1]*d, p[2]*d, p[3]*d, p[4]*d, p[5]*d );
  368. }
  369.  
  370. ID_INLINE int idPluecker::GetDimension( void ) const {
  371.     return 6;
  372. }
  373.  
  374. ID_INLINE const float *idPluecker::ToFloatPtr( void ) const {
  375.     return p;
  376. }
  377.  
  378. ID_INLINE float *idPluecker::ToFloatPtr( void ) {
  379.     return p;
  380. }
  381.  
  382. #endif /* !__MATH_PLUECKER_H__ */
  383.