00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055 #ifndef __VCGLIB_POINT
00056 #define __VCGLIB_POINT
00057
00058 #include <assert.h>
00059 #include <vcg/math/base.h>
00060 #include <vcg/space/space.h>
00061
00062 namespace vcg {
00063
00064 namespace ndim{
00065
00066
00067
00068
00069
00079 template <int N, class S>
00080 class Point
00081 {
00082 public:
00083 typedef S ScalarType;
00084 typedef VoidType ParamType;
00085 typedef Point<N,S> PointType;
00086 enum {Dimension=N};
00087
00088
00089 protected:
00091 S _v[N];
00092
00093 public:
00094
00096
00101 inline Point () { };
00102
00103
00106 inline S Ext( const int i ) const
00107 {
00108 if(i>=0 && i<=N) return _v[i];
00109 else return 0;
00110 }
00111
00113 template <int N2, class S2>
00114 inline void Import( const Point<N2,S2> & b )
00115 {
00116 _v[0] = ScalarType(b[0]);
00117 _v[1] = ScalarType(b[1]);
00118 if (N>2) { if (N2>2) _v[2] = ScalarType(b[2]); else _v[2] = 0;};
00119 if (N>3) { if (N2>3) _v[3] = ScalarType(b[3]); else _v[3] = 0;};
00120 }
00121
00123 template <int N2, class S2>
00124 static inline PointType Construct( const Point<N2,S2> & b )
00125 {
00126 PointType p; p.Import(b);
00127 return p;
00128 }
00129
00131 template <class S2>
00132 inline void ImportHomo( const Point<N-1,S2> & b )
00133 {
00134 _v[0] = ScalarType(b[0]);
00135 _v[1] = ScalarType(b[1]);
00136 if (N>2) { _v[2] = ScalarType(_v[2]); };
00137 _v[N-1] = 1.0;
00138 }
00139
00141 template <int N2, class S2>
00142 static inline PointType Construct( const Point<N-1,S2> & b )
00143 {
00144 PointType p; p.ImportHomo(b);
00145 return p;
00146 }
00147
00149
00151
00155 inline S & operator [] ( const int i )
00156 {
00157 assert(i>=0 && i<N);
00158 return _v[i];
00159 }
00160 inline const S & operator [] ( const int i ) const
00161 {
00162 assert(i>=0 && i<N);
00163 return _v[i];
00164 }
00165 inline const S &X() const { return _v[0]; }
00166 inline const S &Y() const { return _v[1]; }
00167 inline const S &Z() const { static_assert(N>2); return _v[2]; }
00171 inline const S &W() const { return _v[N-1]; }
00172 inline S &X() { return _v[0]; }
00173 inline S &Y() { return _v[1]; }
00174 inline S &Z() { static_assert(N>2); return _v[2]; }
00175 inline S &W() { return _v[N-1]; }
00176 inline const S * V() const
00177 {
00178 return _v;
00179 }
00180 inline S & V( const int i )
00181 {
00182 assert(i>=0 && i<N);
00183 return _v[i];
00184 }
00185 inline const S & V( const int i ) const
00186 {
00187 assert(i>=0 && i<N);
00188 return _v[i];
00189 }
00191
00192
00196
00197 inline void SetZero()
00198 {
00199 for(unsigned int ii = 0; ii < Dimension;++ii)
00200 V(ii) = S();
00201 }
00202
00203 inline PointType operator + ( PointType const & p) const
00204 {
00205 PointType res;
00206 for(unsigned int ii = 0; ii < Dimension;++ii)
00207 res[ii] = V(ii) + p[ii];
00208 return res;
00209 }
00210
00211 inline PointType operator - ( PointType const & p) const
00212 {
00213 PointType res;
00214 for(unsigned int ii = 0; ii < Dimension;++ii)
00215 res[ii] = V(ii) - p[ii];
00216 return res;
00217 }
00218
00219 inline PointType operator * ( const S s ) const
00220 {
00221 PointType res;
00222 for(unsigned int ii = 0; ii < Dimension;++ii)
00223 res[ii] = V(ii) * s;
00224 return res;
00225 }
00226
00227 inline PointType operator / ( const S s ) const
00228 {
00229 PointType res;
00230 for(unsigned int ii = 0; ii < Dimension;++ii)
00231 res[ii] = V(ii) / s;
00232 return res;
00233 }
00234
00235 inline PointType & operator += ( PointType const & p)
00236 {
00237 for(unsigned int ii = 0; ii < Dimension;++ii)
00238 V(ii) += p[ii];
00239 return *this;
00240 }
00241
00242 inline PointType & operator -= ( PointType const & p)
00243 {
00244 for(unsigned int ii = 0; ii < Dimension;++ii)
00245 V(ii) -= p[ii];
00246 return *this;
00247 }
00248
00249 inline PointType & operator *= ( const S s )
00250 {
00251 for(unsigned int ii = 0; ii < Dimension;++ii)
00252 V(ii) *= s;
00253 return *this;
00254 }
00255
00256 inline PointType & operator /= ( const S s )
00257 {
00258 for(unsigned int ii = 0; ii < Dimension;++ii)
00259 V(ii) *= s;
00260 return *this;
00261 }
00262
00263 inline PointType operator - () const
00264 {
00265 PointType res;
00266 for(unsigned int ii = 0; ii < Dimension;++ii)
00267 res[ii] = - V(ii);
00268 return res;
00269 }
00271
00272
00275
00276 inline S operator * ( PointType const & p ) const;
00277
00279 inline S StableDot ( const PointType & p ) const;
00280
00282
00284
00288
00289 inline S Norm() const;
00291 template <class PT> static S Norm(const PT &p );
00293 inline S SquaredNorm() const;
00295 template <class PT> static S SquaredNorm(const PT &p );
00297 inline PointType & Normalize();
00299 template <class PT> static PointType & Normalize(const PT &p);
00301 inline PointType & HomoNormalize();
00302
00304 inline S NormInfinity() const;
00306 inline S NormOne() const;
00307
00309
00310
00311 inline S operator % ( PointType const & p ) const;
00312
00314 inline S Sum() const;
00316 inline S Max() const;
00318 inline S Min() const;
00320 inline int MaxI() const;
00322 inline int MinI() const;
00323
00324
00326 inline PointType & Scale( const PointType & p );
00327
00328
00330 void ToPolar( S & ro, S & tetha, S & fi ) const
00331 {
00332 ro = Norm();
00333 tetha = (S)atan2( _v[1], _v[0] );
00334 fi = (S)acos( _v[2]/ro );
00335 }
00336
00338
00343 inline bool operator == ( PointType const & p ) const;
00344 inline bool operator != ( PointType const & p ) const;
00345 inline bool operator < ( PointType const & p ) const;
00346 inline bool operator > ( PointType const & p ) const;
00347 inline bool operator <= ( PointType const & p ) const;
00348 inline bool operator >= ( PointType const & p ) const;
00350
00352
00358 inline PointType LocalToGlobal(ParamType p) const{
00359 return *this; }
00360
00361 inline ParamType GlobalToLocal(PointType p) const{
00362 ParamType p(); return p; }
00364
00365 };
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375 template <class S>
00376 class Point2 : public Point<2,S> {
00377 public:
00378 typedef S ScalarType;
00379 typedef Point2 PointType;
00380 using Point<2,S>::_v;
00381 using Point<2,S>::V;
00382 using Point<2,S>::W;
00383
00385
00387
00388 inline Point2 (){}
00389
00391 inline Point2 ( const S a, const S b){
00392 _v[0]=a; _v[1]=b; };
00393
00396 inline Point2 operator ~ () const {
00397 return Point2 ( -_v[2], _v[1] );
00398 }
00399
00401 inline ScalarType &Angle(){
00402 return math::Atan2(_v[1],_v[0]);}
00403
00405 inline Point2 & ToPolar(){
00406 ScalarType t = Angle();
00407 _v[0] = Norm();
00408 _v[1] = t;
00409 return *this;}
00410
00412 inline Point2 & ToCartesian() {
00413 ScalarType l = _v[0];
00414 _v[0] = (ScalarType)(l*math::Cos(_v[1]));
00415 _v[1] = (ScalarType)(l*math::Sin(_v[1]));
00416 return *this;}
00417
00419 inline Point2 & Rotate( const ScalarType rad ){
00420 ScalarType t = _v[0];
00421 ScalarType s = math::Sin(rad);
00422 ScalarType c = math::Cos(rad);
00423 _v[0] = _v[0]*c - _v[1]*s;
00424 _v[1] = t *s + _v[1]*c;
00425 return *this;}
00426
00428
00430
00432 inline void Zero(){
00433 _v[0]=0; _v[1]=0; };
00434
00435 inline Point2 ( const S nv[2] ){
00436 _v[0]=nv[0]; _v[1]=nv[1]; };
00437
00438 inline Point2 operator + ( Point2 const & p) const {
00439 return Point2( _v[0]+p._v[0], _v[1]+p._v[1]); }
00440
00441 inline Point2 operator - ( Point2 const & p) const {
00442 return Point2( _v[0]-p._v[0], _v[1]-p._v[1]); }
00443
00444 inline Point2 operator * ( const S s ) const {
00445 return Point2( _v[0]*s, _v[1]*s ); }
00446
00447 inline Point2 operator / ( const S s ) const {
00448 S t=1.0/s;
00449 return Point2( _v[0]*t, _v[1]*t ); }
00450
00451 inline Point2 operator - () const {
00452 return Point2 ( -_v[0], -_v[1] ); }
00453
00454 inline Point2 & operator += ( Point2 const & p ) {
00455 _v[0] += p._v[0]; _v[1] += p._v[1]; return *this; }
00456
00457 inline Point2 & operator -= ( Point2 const & p ) {
00458 _v[0] -= p._v[0]; _v[1] -= p._v[1]; return *this; }
00459
00460 inline Point2 & operator *= ( const S s ) {
00461 _v[0] *= s; _v[1] *= s; return *this; }
00462
00463 inline Point2 & operator /= ( const S s ) {
00464 S t=1.0/s; _v[0] *= t; _v[1] *= t; return *this; }
00465
00466 inline S Norm() const {
00467 return math::Sqrt( _v[0]*_v[0] + _v[1]*_v[1] );}
00468
00469 template <class PT> static S Norm(const PT &p ) {
00470 return math::Sqrt( p.V(0)*p.V(0) + p.V(1)*p.V(1) );}
00471
00472 inline S SquaredNorm() const {
00473 return ( _v[0]*_v[0] + _v[1]*_v[1] );}
00474
00475 template <class PT> static S SquaredNorm(const PT &p ) {
00476 return ( p.V(0)*p.V(0) + p.V(1)*p.V(1) );}
00477
00478 inline S operator * ( Point2 const & p ) const {
00479 return ( _v[0]*p._v[0] + _v[1]*p._v[1]) ; }
00480
00481 inline bool operator == ( Point2 const & p ) const {
00482 return _v[0]==p._v[0] && _v[1]==p._v[1] ;}
00483
00484 inline bool operator != ( Point2 const & p ) const {
00485 return _v[0]!=p._v[0] || _v[1]!=p._v[1] ;}
00486
00487 inline bool operator < ( Point2 const & p ) const{
00488 return (_v[1]!=p._v[1])?(_v[1]< p._v[1]) : (_v[0]<p._v[0]); }
00489
00490 inline bool operator > ( Point2 const & p ) const {
00491 return (_v[1]!=p._v[1])?(_v[1]> p._v[1]) : (_v[0]>p._v[0]); }
00492
00493 inline bool operator <= ( Point2 const & p ) {
00494 return (_v[1]!=p._v[1])?(_v[1]< p._v[1]) : (_v[0]<=p._v[0]); }
00495
00496 inline bool operator >= ( Point2 const & p ) const {
00497 return (_v[1]!=p._v[1])?(_v[1]> p._v[1]) : (_v[0]>=p._v[0]); }
00498
00499 inline Point2 & Normalize() {
00500 PointType n = Norm(); if(n!=0.0) { n=1.0/n; _v[0]*=n; _v[1]*=n;} return *this;};
00501
00502 template <class PT> Point2 & Normalize(const PT &p){
00503 PointType n = Norm(); if(n!=0.0) { n=1.0/n; V(0)*=n; V(1)*=n; }
00504 return *this;};
00505
00506 inline Point2 & HomoNormalize(){
00507 if (_v[2]!=0.0) { _v[0] /= W(); W()=1.0; } return *this;};
00508
00509 inline S NormInfinity() const {
00510 return math::Max( math::Abs(_v[0]), math::Abs(_v[1]) ); }
00511
00512 inline S NormOne() const {
00513 return math::Abs(_v[0])+ math::Abs(_v[1]);}
00514
00515 inline S operator % ( Point2 const & p ) const {
00516 return _v[0] * p._v[1] - _v[1] * p._v[0]; }
00517
00518 inline S Sum() const {
00519 return _v[0]+_v[1];}
00520
00521 inline S Max() const {
00522 return math::Max( _v[0], _v[1] ); }
00523
00524 inline S Min() const {
00525 return math::Min( _v[0], _v[1] ); }
00526
00527 inline int MaxI() const {
00528 return (_v[0] < _v[1]) ? 1:0; };
00529
00530 inline int MinI() const {
00531 return (_v[0] > _v[1]) ? 1:0; };
00532
00533 inline PointType & Scale( const PointType & p ) {
00534 _v[0] *= p._v[0]; _v[1] *= p._v[1]; return *this; }
00535
00536 inline S StableDot ( const PointType & p ) const {
00537 return _v[0]*p._v[0] +_v[1]*p._v[1]; }
00539
00540 };
00541
00542 template <typename S>
00543 class Point3 : public Point<3,S> {
00544 public:
00545 typedef S ScalarType;
00546 typedef Point3<S> PointType;
00547 using Point<3,S>::_v;
00548 using Point<3,S>::V;
00549 using Point<3,S>::W;
00550
00551
00553
00555
00556 inline Point3 ():Point<3,S>(){}
00558 inline Point3 ( const S a, const S b, const S c){
00559 _v[0]=a; _v[1]=b; _v[2]=c; };
00560
00562 inline PointType operator ^ ( PointType const & p ) const {
00563 return Point3 (
00564 _v[1]*p._v[2] - _v[2]*p._v[1],
00565 _v[2]*p._v[0] - _v[0]*p._v[2],
00566 _v[0]*p._v[1] - _v[1]*p._v[0] );
00567 }
00569
00571
00573 inline void Zero(){
00574 _v[0]=0; _v[1]=0; _v[2]=0; };
00575
00576 inline Point3 ( const S nv[3] ){
00577 _v[0]=nv[0]; _v[1]=nv[1]; _v[2]=nv[2]; };
00578
00579 inline Point3 operator + ( Point3 const & p) const{
00580 return Point3( _v[0]+p._v[0], _v[1]+p._v[1], _v[2]+p._v[2]); }
00581
00582 inline Point3 operator - ( Point3 const & p) const {
00583 return Point3( _v[0]-p._v[0], _v[1]-p._v[1], _v[2]-p._v[2]); }
00584
00585 inline Point3 operator * ( const S s ) const {
00586 return Point3( _v[0]*s, _v[1]*s , _v[2]*s ); }
00587
00588 inline Point3 operator / ( const S s ) const {
00589 S t=1.0/s;
00590 return Point3( _v[0]*t, _v[1]*t , _v[2]*t ); }
00591
00592 inline Point3 operator - () const {
00593 return Point3 ( -_v[0], -_v[1] , -_v[2] ); }
00594
00595 inline Point3 & operator += ( Point3 const & p ) {
00596 _v[0] += p._v[0]; _v[1] += p._v[1]; _v[2] += p._v[2]; return *this; }
00597
00598 inline Point3 & operator -= ( Point3 const & p ) {
00599 _v[0] -= p._v[0]; _v[1] -= p._v[1]; _v[2] -= p._v[2]; return *this; }
00600
00601 inline Point3 & operator *= ( const S s ) {
00602 _v[0] *= s; _v[1] *= s; _v[2] *= s; return *this; }
00603
00604 inline Point3 & operator /= ( const S s ) {
00605 S t=1.0/s; _v[0] *= t; _v[1] *= t; _v[2] *= t; return *this; }
00606
00607 inline S Norm() const {
00608 return math::Sqrt( _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] );}
00609
00610 template <class PT> static S Norm(const PT &p ) {
00611 return math::Sqrt( p.V(0)*p.V(0) + p.V(1)*p.V(1) + p.V(2)*p.V(2) );}
00612
00613 inline S SquaredNorm() const {
00614 return ( _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] );}
00615
00616 template <class PT> static S SquaredNorm(const PT &p ) {
00617 return ( p.V(0)*p.V(0) + p.V(1)*p.V(1) + p.V(2)*p.V(2) );}
00618
00619 inline S operator * ( PointType const & p ) const {
00620 return ( _v[0]*p._v[0] + _v[1]*p._v[1] + _v[2]*p._v[2]) ; }
00621
00622 inline bool operator == ( PointType const & p ) const {
00623 return _v[0]==p._v[0] && _v[1]==p._v[1] && _v[2]==p._v[2] ;}
00624
00625 inline bool operator != ( PointType const & p ) const {
00626 return _v[0]!=p._v[0] || _v[1]!=p._v[1] || _v[2]!=p._v[2] ;}
00627
00628 inline bool operator < ( PointType const & p ) const{
00629 return (_v[2]!=p._v[2])?(_v[2]< p._v[2]):
00630 (_v[1]!=p._v[1])?(_v[1]< p._v[1]) : (_v[0]<p._v[0]); }
00631
00632 inline bool operator > ( PointType const & p ) const {
00633 return (_v[2]!=p._v[2])?(_v[2]> p._v[2]):
00634 (_v[1]!=p._v[1])?(_v[1]> p._v[1]) : (_v[0]>p._v[0]); }
00635
00636 inline bool operator <= ( PointType const & p ) {
00637 return (_v[2]!=p._v[2])?(_v[2]< p._v[2]):
00638 (_v[1]!=p._v[1])?(_v[1]< p._v[1]) : (_v[0]<=p._v[0]); }
00639
00640 inline bool operator >= ( PointType const & p ) const {
00641 return (_v[2]!=p._v[2])?(_v[2]> p._v[2]):
00642 (_v[1]!=p._v[1])?(_v[1]> p._v[1]) : (_v[0]>=p._v[0]); }
00643
00644 inline PointType & Normalize() {
00645 S n = Norm();
00646 if(n!=0.0) {
00647 n=S(1.0)/n;
00648 _v[0]*=n; _v[1]*=n; _v[2]*=n; }
00649 return *this;};
00650
00651 template <class PT> PointType & Normalize(const PT &p){
00652 S n = Norm(); if(n!=0.0) { n=1.0/n; V(0)*=n; V(1)*=n; V(2)*=n; }
00653 return *this;};
00654
00655 inline PointType & HomoNormalize(){
00656 if (_v[2]!=0.0) { _v[0] /= W(); _v[1] /= W(); W()=1.0; }
00657 return *this;};
00658
00659 inline S NormInfinity() const {
00660 return math::Max( math::Max( math::Abs(_v[0]), math::Abs(_v[1]) ),
00661 math::Abs(_v[3]) ); }
00662
00663 inline S NormOne() const {
00664 return math::Abs(_v[0])+ math::Abs(_v[1])+math::Max(math::Abs(_v[2]));}
00665
00666 inline S operator % ( PointType const & p ) const {
00667 S t = (*this)*p;
00668 return math::Sqrt( SquaredNorm() * p.SquaredNorm() - (t*t) );};
00669
00670 inline S Sum() const {
00671 return _v[0]+_v[1]+_v[2];}
00672
00673 inline S Max() const {
00674 return math::Max( math::Max( _v[0], _v[1] ), _v[2] ); }
00675
00676 inline S Min() const {
00677 return math::Min( math::Min( _v[0], _v[1] ), _v[2] ); }
00678
00679 inline int MaxI() const {
00680 int i= (_v[0] < _v[1]) ? 1:0; if (_v[i] < _v[2]) i=2; return i;};
00681
00682 inline int MinI() const {
00683 int i= (_v[0] > _v[1]) ? 1:0; if (_v[i] > _v[2]) i=2; return i;};
00684
00685 inline PointType & Scale( const PointType & p ) {
00686 _v[0] *= p._v[0]; _v[1] *= p._v[1]; _v[2] *= p._v[2]; return *this; }
00687
00688 inline S StableDot ( const PointType & p ) const {
00689 S k0=_v[0]*p._v[0], k1=_v[1]*p._v[1], k2=_v[2]*p._v[2];
00690 int exp0,exp1,exp2;
00691 frexp( double(k0), &exp0 );
00692 frexp( double(k1), &exp1 );
00693 frexp( double(k2), &exp2 );
00694 if( exp0<exp1 )
00695 if(exp0<exp2) return (k1+k2)+k0; else return (k0+k1)+k2;
00696 else
00697 if(exp1<exp2) return (k0+k2)+k1; else return (k0+k1)+k2;
00698 }
00700
00701 };
00702
00703 template <typename S>
00704 class Point4 : public Point<4,S> {
00705 public:
00706 typedef S ScalarType;
00707 typedef Point4<S> PointType;
00708 using Point<3,S>::_v;
00709 using Point<3,S>::V;
00710 using Point<3,S>::W;
00711
00713
00714
00715 inline Point4 (){}
00716
00718
00719 inline Point4 ( const S a, const S b, const S c, const S d){
00720 _v[0]=a; _v[1]=b; _v[2]=c; _v[3]=d; };
00722
00724 inline void Zero(){
00725 _v[0]=0; _v[1]=0; _v[2]=0; _v[3]=0; };
00726
00727 inline Point4 ( const S nv[4] ){
00728 _v[0]=nv[0]; _v[1]=nv[1]; _v[2]=nv[2]; _v[3]=nv[3]; };
00729
00730 inline Point4 operator + ( Point4 const & p) const {
00731 return Point4( _v[0]+p._v[0], _v[1]+p._v[1], _v[2]+p._v[2], _v[3]+p._v[3] ); }
00732
00733 inline Point4 operator - ( Point4 const & p) const {
00734 return Point4( _v[0]-p._v[0], _v[1]-p._v[1], _v[2]-p._v[2], _v[3]-p._v[3] ); }
00735
00736 inline Point4 operator * ( const S s ) const {
00737 return Point4( _v[0]*s, _v[1]*s , _v[2]*s , _v[3]*s ); }
00738
00739 inline PointType operator ^ ( PointType const & p ) const {
00740 assert(0);
00741 return *this;
00742 }
00743
00744 inline Point4 operator / ( const S s ) const {
00745 S t=1.0/s;
00746 return Point4( _v[0]*t, _v[1]*t , _v[2]*t , _v[3]*t ); }
00747
00748 inline Point4 operator - () const {
00749 return Point4 ( -_v[0], -_v[1] , -_v[2] , -_v[3] ); }
00750
00751 inline Point4 & operator += ( Point4 const & p ) {
00752 _v[0] += p._v[0]; _v[1] += p._v[1]; _v[2] += p._v[2]; _v[3] += p._v[3]; return *this; }
00753
00754 inline Point4 & operator -= ( Point4 const & p ) {
00755 _v[0] -= p._v[0]; _v[1] -= p._v[1]; _v[2] -= p._v[2]; _v[3] -= p._v[3]; return *this; }
00756
00757 inline Point4 & operator *= ( const S s ) {
00758 _v[0] *= s; _v[1] *= s; _v[2] *= s; _v[3] *= s; return *this; }
00759
00760 inline Point4 & operator /= ( const S s ) {
00761 S t=1.0/s; _v[0] *= t; _v[1] *= t; _v[2] *= t; _v[3] *= t; return *this; }
00762
00763 inline S Norm() const {
00764 return math::Sqrt( _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] + _v[3]*_v[3] );}
00765
00766 template <class PT> static S Norm(const PT &p ) {
00767 return math::Sqrt( p.V(0)*p.V(0) + p.V(1)*p.V(1) + p.V(2)*p.V(2) + p.V(3)*p.V(3) );}
00768
00769 inline S SquaredNorm() const {
00770 return ( _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] + _v[3]*_v[3] );}
00771
00772 template <class PT> static S SquaredNorm(const PT &p ) {
00773 return ( p.V(0)*p.V(0) + p.V(1)*p.V(1) + p.V(2)*p.V(2) + p.V(3)*p.V(3) );}
00774
00775 inline S operator * ( PointType const & p ) const {
00776 return ( _v[0]*p._v[0] + _v[1]*p._v[1] + _v[2]*p._v[2] + _v[3]*p._v[3] ); }
00777
00778 inline bool operator == ( PointType const & p ) const {
00779 return _v[0]==p._v[0] && _v[1]==p._v[1] && _v[2]==p._v[2] && _v[3]==p._v[3];}
00780
00781 inline bool operator != ( PointType const & p ) const {
00782 return _v[0]!=p._v[0] || _v[1]!=p._v[1] || _v[2]!=p._v[2] || _v[3]!=p._v[3];}
00783
00784 inline bool operator < ( PointType const & p ) const{
00785 return (_v[3]!=p._v[3])?(_v[3]< p._v[3]) : (_v[2]!=p._v[2])?(_v[2]< p._v[2]):
00786 (_v[1]!=p._v[1])?(_v[1]< p._v[1]) : (_v[0]<p._v[0]); }
00787
00788 inline bool operator > ( PointType const & p ) const {
00789 return (_v[3]!=p._v[3])?(_v[3]> p._v[3]) : (_v[2]!=p._v[2])?(_v[2]> p._v[2]):
00790 (_v[1]!=p._v[1])?(_v[1]> p._v[1]) : (_v[0]>p._v[0]); }
00791
00792 inline bool operator <= ( PointType const & p ) {
00793 return (_v[3]!=p._v[3])?(_v[3]< p._v[3]) : (_v[2]!=p._v[2])?(_v[2]< p._v[2]):
00794 (_v[1]!=p._v[1])?(_v[1]< p._v[1]) : (_v[0]<=p._v[0]); }
00795
00796 inline bool operator >= ( PointType const & p ) const {
00797 return (_v[3]!=p._v[3])?(_v[3]> p._v[3]) : (_v[2]!=p._v[2])?(_v[2]> p._v[2]):
00798 (_v[1]!=p._v[1])?(_v[1]> p._v[1]) : (_v[0]>=p._v[0]); }
00799
00800 inline PointType & Normalize() {
00801 PointType n = Norm(); if(n!=0.0) { n=1.0/n; _v[0]*=n; _v[1]*=n; _v[2]*=n; _v[3]*=n; }
00802 return *this;};
00803
00804 template <class PT> PointType & Normalize(const PT &p){
00805 PointType n = Norm(); if(n!=0.0) { n=1.0/n; V(0)*=n; V(1)*=n; V(2)*=n; V(3)*=n; }
00806 return *this;};
00807
00808 inline PointType & HomoNormalize(){
00809 if (_v[3]!=0.0) { _v[0] /= W(); _v[1] /= W(); _v[2] /= W(); W()=1.0; }
00810 return *this;};
00811
00812 inline S NormInfinity() const {
00813 return math::Max( math::Max( math::Abs(_v[0]), math::Abs(_v[1]) ),
00814 math::Max( math::Abs(_v[2]), math::Abs(_v[3]) ) ); }
00815
00816 inline S NormOne() const {
00817 return math::Abs(_v[0])+ math::Abs(_v[1])+math::Max(math::Abs(_v[2]),math::Abs(_v[3]));}
00818
00819 inline S operator % ( PointType const & p ) const {
00820 S t = (*this)*p;
00821 return math::Sqrt( SquaredNorm() * p.SquaredNorm() - (t*t) );};
00822
00823 inline S Sum() const {
00824 return _v[0]+_v[1]+_v[2]+_v[3];}
00825
00826 inline S Max() const {
00827 return math::Max( math::Max( _v[0], _v[1] ), math::Max( _v[2], _v[3] )); }
00828
00829 inline S Min() const {
00830 return math::Min( math::Min( _v[0], _v[1] ), math::Min( _v[2], _v[3] )); }
00831
00832 inline int MaxI() const {
00833 int i= (_v[0] < _v[1]) ? 1:0; if (_v[i] < _v[2]) i=2; if (_v[i] < _v[3]) i=3;
00834 return i;};
00835
00836 inline int MinI() const {
00837 int i= (_v[0] > _v[1]) ? 1:0; if (_v[i] > _v[2]) i=2; if (_v[i] > _v[3]) i=3;
00838 return i;};
00839
00840 inline PointType & Scale( const PointType & p ) {
00841 _v[0] *= p._v[0]; _v[1] *= p._v[1]; _v[2] *= p._v[2]; _v[3] *= p._v[3]; return *this; }
00842
00843 inline S StableDot ( const PointType & p ) const {
00844 S k0=_v[0]*p._v[0], k1=_v[1]*p._v[1], k2=_v[2]*p._v[2], k3=_v[3]*p._v[3];
00845 int exp0,exp1,exp2,exp3;
00846 frexp( double(k0), &exp0 );frexp( double(k1), &exp1 );
00847 frexp( double(k2), &exp2 );frexp( double(k3), &exp3 );
00848 if (exp0>exp1) { math::Swap(k0,k1); math::Swap(exp0,exp1); }
00849 if (exp2>exp3) { math::Swap(k2,k3); math::Swap(exp2,exp3); }
00850 if (exp0>exp2) { math::Swap(k0,k2); math::Swap(exp0,exp2); }
00851 if (exp1>exp3) { math::Swap(k1,k3); math::Swap(exp1,exp3); }
00852 if (exp2>exp3) { math::Swap(k2,k3); math::Swap(exp2,exp3); }
00853 return ( (k0 + k1) + k2 ) +k3; }
00854
00856 };
00857
00858
00859 template <class S>
00860 inline S Angle( Point3<S> const & p1, Point3<S> const & p2 )
00861 {
00862 S w = p1.Norm()*p2.Norm();
00863 if(w==0) return -1;
00864 S t = (p1*p2)/w;
00865 if(t>1) t = 1;
00866 else if(t<-1) t = -1;
00867 return (S) acos(t);
00868 }
00869
00870
00871 template <class S>
00872 inline S AngleN( Point3<S> const & p1, Point3<S> const & p2 )
00873 {
00874 S w = p1*p2;
00875 if(w>1)
00876 w = 1;
00877 else if(w<-1)
00878 w=-1;
00879 return (S) acos(w);
00880 }
00881
00882
00883 template <int N,class S>
00884 inline S Norm( Point<N,S> const & p )
00885 {
00886 return p.Norm();
00887 }
00888
00889 template <int N,class S>
00890 inline S SquaredNorm( Point<N,S> const & p )
00891 {
00892 return p.SquaredNorm();
00893 }
00894
00895 template <int N,class S>
00896 inline Point<N,S> & Normalize( Point<N,S> & p )
00897 {
00898 p.Normalize();
00899 return p;
00900 }
00901
00902 template <int N, class S>
00903 inline S Distance( Point<N,S> const & p1,Point<N,S> const & p2 )
00904 {
00905 return (p1-p2).Norm();
00906 }
00907
00908 template <int N, class S>
00909 inline S SquaredDistance( Point<N,S> const & p1,Point<N,S> const & p2 )
00910 {
00911 return (p1-p2).SquaredNorm();
00912 }
00913
00914
00915
00916
00917
00918
00919
00920
00921
00922
00923
00924
00925
00926
00927
00928
00929
00930
00931
00932
00933
00934
00935
00936
00937 typedef Point2<short> Point2s;
00938 typedef Point2<int> Point2i;
00939 typedef Point2<float> Point2f;
00940 typedef Point2<double> Point2d;
00941 typedef Point2<short> Vector2s;
00942 typedef Point2<int> Vector2i;
00943 typedef Point2<float> Vector2f;
00944 typedef Point2<double> Vector2d;
00945
00946 typedef Point3<short> Point3s;
00947 typedef Point3<int> Point3i;
00948 typedef Point3<float> Point3f;
00949 typedef Point3<double> Point3d;
00950 typedef Point3<short> Vector3s;
00951 typedef Point3<int> Vector3i;
00952 typedef Point3<float> Vector3f;
00953 typedef Point3<double> Vector3d;
00954
00955
00956 typedef Point4<short> Point4s;
00957 typedef Point4<int> Point4i;
00958 typedef Point4<float> Point4f;
00959 typedef Point4<double> Point4d;
00960 typedef Point4<short> Vector4s;
00961 typedef Point4<int> Vector4i;
00962 typedef Point4<float> Vector4f;
00963 typedef Point4<double> Vector4d;
00964
00967
00968 template<unsigned int N,typename S>
00969 struct PointBase : Point<N,S>
00970 {
00971 PointBase()
00972 :Point<N,S>()
00973 {
00974 }
00975 };
00976
00977 }
00978 }
00979 #endif
00980
