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
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094 #ifndef __VCGLIB_POINT3
00095 #define __VCGLIB_POINT3
00096
00097 #include <assert.h>
00098 #include <vcg/math/base.h>
00099
00100 namespace vcg {
00101
00109 template <class T> class Box3;
00110
00111 template <class P3ScalarType> class Point3
00112 {
00113 protected:
00115 P3ScalarType _v[3];
00116
00117 public:
00118 typedef P3ScalarType ScalarType;
00119 enum {Dimension = 3};
00120
00121
00123
00128 inline Point3 () { }
00129 inline Point3 ( const P3ScalarType nx, const P3ScalarType ny, const P3ScalarType nz )
00130 {
00131 _v[0] = nx;
00132 _v[1] = ny;
00133 _v[2] = nz;
00134 }
00135 inline Point3 ( Point3 const & p )
00136 {
00137 _v[0]= p._v[0];
00138 _v[1]= p._v[1];
00139 _v[2]= p._v[2];
00140 }
00141 inline Point3 ( const P3ScalarType nv[3] )
00142 {
00143 _v[0] = nv[0];
00144 _v[1] = nv[1];
00145 _v[2] = nv[2];
00146 }
00147 inline Point3 & operator =( Point3 const & p )
00148 {
00149 _v[0]= p._v[0]; _v[1]= p._v[1]; _v[2]= p._v[2];
00150 return *this;
00151 }
00152 inline void SetZero()
00153 {
00154 _v[0] = 0;
00155 _v[1] = 0;
00156 _v[2] = 0;
00157 }
00158
00161 inline P3ScalarType Ext( const int i ) const
00162 {
00163 if(i>=0 && i<=2) return _v[i];
00164 else return 0;
00165 }
00166
00167 template <class Q>
00168 inline void Import( const Point3<Q> & b )
00169 {
00170 _v[0] = P3ScalarType(b[0]);
00171 _v[1] = P3ScalarType(b[1]);
00172 _v[2] = P3ScalarType(b[2]);
00173 }
00174
00175 template <class Q>
00176 static inline Point3 Construct( const Point3<Q> & b )
00177 {
00178 return Point3(P3ScalarType(b[0]),P3ScalarType(b[1]),P3ScalarType(b[2]));
00179 }
00180
00181 template <class Q>
00182 static inline Point3 Construct( const Q & P0, const Q & P1, const Q & P2)
00183 {
00184 return Point3(P3ScalarType(P0),P3ScalarType(P1),P3ScalarType(P2));
00185 }
00186
00187 static inline Point3 Construct( const Point3<ScalarType> & b )
00188 {
00189 return b;
00190 }
00191
00193
00195
00199 inline P3ScalarType & operator [] ( const int i )
00200 {
00201 assert(i>=0 && i<3);
00202 return _v[i];
00203 }
00204 inline const P3ScalarType & operator [] ( const int i ) const
00205 {
00206 assert(i>=0 && i<3);
00207 return _v[i];
00208 }
00209 inline const P3ScalarType &X() const { return _v[0]; }
00210 inline const P3ScalarType &Y() const { return _v[1]; }
00211 inline const P3ScalarType &Z() const { return _v[2]; }
00212 inline P3ScalarType &X() { return _v[0]; }
00213 inline P3ScalarType &Y() { return _v[1]; }
00214 inline P3ScalarType &Z() { return _v[2]; }
00215 inline const P3ScalarType * V() const
00216 {
00217 return _v;
00218 }
00219 inline P3ScalarType * V()
00220 {
00221 return _v;
00222 }
00223 inline P3ScalarType & V( const int i )
00224 {
00225 assert(i>=0 && i<3);
00226 return _v[i];
00227 }
00228 inline const P3ScalarType & V( const int i ) const
00229 {
00230 assert(i>=0 && i<3);
00231 return _v[i];
00232 }
00234
00235
00240 inline Point3 operator + ( Point3 const & p) const
00241 {
00242 return Point3<P3ScalarType>( _v[0]+p._v[0], _v[1]+p._v[1], _v[2]+p._v[2] );
00243 }
00244 inline Point3 operator - ( Point3 const & p) const
00245 {
00246 return Point3<P3ScalarType>( _v[0]-p._v[0], _v[1]-p._v[1], _v[2]-p._v[2] );
00247 }
00248 inline Point3 operator * ( const P3ScalarType s ) const
00249 {
00250 return Point3<P3ScalarType>( _v[0]*s, _v[1]*s, _v[2]*s );
00251 }
00252 inline Point3 operator / ( const P3ScalarType s ) const
00253 {
00254 return Point3<P3ScalarType>( _v[0]/s, _v[1]/s, _v[2]/s );
00255 }
00257 inline P3ScalarType operator * ( Point3 const & p ) const
00258 {
00259 return ( _v[0]*p._v[0] + _v[1]*p._v[1] + _v[2]*p._v[2] );
00260 }
00261 inline P3ScalarType dot( const Point3 & p ) const { return (*this) * p; }
00263 inline Point3 operator ^ ( Point3 const & p ) const
00264 {
00265 return Point3 <P3ScalarType>
00266 (
00267 _v[1]*p._v[2] - _v[2]*p._v[1],
00268 _v[2]*p._v[0] - _v[0]*p._v[2],
00269 _v[0]*p._v[1] - _v[1]*p._v[0]
00270 );
00271 }
00272
00273 inline Point3 & operator += ( Point3 const & p)
00274 {
00275 _v[0] += p._v[0];
00276 _v[1] += p._v[1];
00277 _v[2] += p._v[2];
00278 return *this;
00279 }
00280 inline Point3 & operator -= ( Point3 const & p)
00281 {
00282 _v[0] -= p._v[0];
00283 _v[1] -= p._v[1];
00284 _v[2] -= p._v[2];
00285 return *this;
00286 }
00287 inline Point3 & operator *= ( const P3ScalarType s )
00288 {
00289 _v[0] *= s;
00290 _v[1] *= s;
00291 _v[2] *= s;
00292 return *this;
00293 }
00294 inline Point3 & operator /= ( const P3ScalarType s )
00295 {
00296 _v[0] /= s;
00297 _v[1] /= s;
00298 _v[2] /= s;
00299 return *this;
00300 }
00301
00302 inline P3ScalarType Norm() const
00303 {
00304 return math::Sqrt( _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] );
00305 }
00306 inline P3ScalarType SquaredNorm() const
00307 {
00308 return ( _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] );
00309 }
00310
00311 inline Point3 & Scale( const P3ScalarType sx, const P3ScalarType sy, const P3ScalarType sz )
00312 {
00313 _v[0] *= sx;
00314 _v[1] *= sy;
00315 _v[2] *= sz;
00316 return *this;
00317 }
00318 inline Point3 & Scale( const Point3 & p )
00319 {
00320 _v[0] *= p._v[0];
00321 _v[1] *= p._v[1];
00322 _v[2] *= p._v[2];
00323 return *this;
00324 }
00325
00326
00327 inline Point3 & Normalize()
00328 {
00329 P3ScalarType n = P3ScalarType(math::Sqrt(_v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2]));
00330 if (n > P3ScalarType(0)) { _v[0] /= n; _v[1] /= n; _v[2] /= n; }
00331 return *this;
00332 }
00333
00334
00335 inline Point3 & normalized() { return Normalize(); }
00336
00347 void ToPolarRad(P3ScalarType &ro, P3ScalarType &theta, P3ScalarType &phi) const
00348 {
00349 ro = Norm();
00350 theta = (P3ScalarType)atan2(_v[2], _v[0]);
00351 phi = (P3ScalarType)asin(_v[1]/ro);
00352 }
00353
00364 void FromPolarRad(const P3ScalarType &ro, const P3ScalarType &theta, const P3ScalarType &phi)
00365 {
00366 _v[0]= ro*cos(theta)*cos(phi);
00367 _v[1]= ro*sin(phi);
00368 _v[2]= ro*sin(theta)*cos(phi);
00369 }
00370
00371 Box3<P3ScalarType> GetBBox(Box3<P3ScalarType> &bb) const;
00373
00374
00379 inline bool operator == ( Point3 const & p ) const
00380 {
00381 return _v[0]==p._v[0] && _v[1]==p._v[1] && _v[2]==p._v[2];
00382 }
00383 inline bool operator != ( Point3 const & p ) const
00384 {
00385 return _v[0]!=p._v[0] || _v[1]!=p._v[1] || _v[2]!=p._v[2];
00386 }
00387 inline bool operator < ( Point3 const & p ) const
00388 {
00389 return (_v[2]!=p._v[2])?(_v[2]<p._v[2]):
00390 (_v[1]!=p._v[1])?(_v[1]<p._v[1]):
00391 (_v[0]<p._v[0]);
00392 }
00393 inline bool operator > ( Point3 const & p ) const
00394 {
00395 return (_v[2]!=p._v[2])?(_v[2]>p._v[2]):
00396 (_v[1]!=p._v[1])?(_v[1]>p._v[1]):
00397 (_v[0]>p._v[0]);
00398 }
00399 inline bool operator <= ( Point3 const & p ) const
00400 {
00401 return (_v[2]!=p._v[2])?(_v[2]< p._v[2]):
00402 (_v[1]!=p._v[1])?(_v[1]< p._v[1]):
00403 (_v[0]<=p._v[0]);
00404 }
00405 inline bool operator >= ( Point3 const & p ) const
00406 {
00407 return (_v[2]!=p._v[2])?(_v[2]> p._v[2]):
00408 (_v[1]!=p._v[1])?(_v[1]> p._v[1]):
00409 (_v[0]>=p._v[0]);
00410 }
00411
00412
00413 inline Point3 operator - () const
00414 {
00415 return Point3<P3ScalarType> ( -_v[0], -_v[1], -_v[2] );
00416 }
00418
00419 };
00420
00421
00422 template <class P3ScalarType>
00423 inline P3ScalarType Angle( Point3<P3ScalarType> const & p1, Point3<P3ScalarType> const & p2 )
00424 {
00425 P3ScalarType w = p1.Norm()*p2.Norm();
00426 if(w==0) return -1;
00427 P3ScalarType t = (p1*p2)/w;
00428 if(t>1) t = 1;
00429 else if(t<-1) t = -1;
00430 return (P3ScalarType) acos(t);
00431 }
00432
00433
00434 template <class P3ScalarType>
00435 inline P3ScalarType AngleN( Point3<P3ScalarType> const & p1, Point3<P3ScalarType> const & p2 )
00436 {
00437 P3ScalarType w = p1*p2;
00438 if(w>1)
00439 w = 1;
00440 else if(w<-1)
00441 w=-1;
00442 return (P3ScalarType) acos(w);
00443 }
00444
00445
00446 template <class P3ScalarType>
00447 inline P3ScalarType Norm( Point3<P3ScalarType> const & p )
00448 {
00449 return p.Norm();
00450 }
00451
00452 template <class P3ScalarType>
00453 inline P3ScalarType SquaredNorm( Point3<P3ScalarType> const & p )
00454 {
00455 return p.SquaredNorm();
00456 }
00457
00458 template <class P3ScalarType>
00459 inline Point3<P3ScalarType> & Normalize( Point3<P3ScalarType> & p )
00460 {
00461 p.Normalize();
00462 return p;
00463 }
00464
00465 template <class P3ScalarType>
00466 inline P3ScalarType Distance( Point3<P3ScalarType> const & p1,Point3<P3ScalarType> const & p2 )
00467 {
00468 return (p1-p2).Norm();
00469 }
00470
00471 template <class P3ScalarType>
00472 inline P3ScalarType SquaredDistance( Point3<P3ScalarType> const & p1,Point3<P3ScalarType> const & p2 )
00473 {
00474 return (p1-p2).SquaredNorm();
00475 }
00476
00477
00478
00479
00480 template<class P3ScalarType>
00481 double stable_dot ( Point3<P3ScalarType> const & p0, Point3<P3ScalarType> const & p1 )
00482 {
00483 P3ScalarType k0 = p0._v[0]*p1._v[0];
00484 P3ScalarType k1 = p0._v[1]*p1._v[1];
00485 P3ScalarType k2 = p0._v[2]*p1._v[2];
00486
00487 int exp0,exp1,exp2;
00488
00489 frexp( double(k0), &exp0 );
00490 frexp( double(k1), &exp1 );
00491 frexp( double(k2), &exp2 );
00492
00493 if( exp0<exp1 )
00494 {
00495 if(exp0<exp2)
00496 return (k1+k2)+k0;
00497 else
00498 return (k0+k1)+k2;
00499 }
00500 else
00501 {
00502 if(exp1<exp2)
00503 return(k0+k2)+k1;
00504 else
00505 return (k0+k1)+k2;
00506 }
00507 }
00508
00509
00510
00512 template<class P3ScalarType>
00513 P3ScalarType PSDist( const Point3<P3ScalarType> & p,
00514 const Point3<P3ScalarType> & v1,
00515 const Point3<P3ScalarType> & v2,
00516 Point3<P3ScalarType> & q )
00517 {
00518 Point3<P3ScalarType> e = v2-v1;
00519 P3ScalarType t = ((p-v1)*e)/e.SquaredNorm();
00520 if(t<0) t = 0;
00521 else if(t>1) t = 1;
00522 q = v1+e*t;
00523 return Distance(p,q);
00524 }
00525
00526
00527 template <class P3ScalarType>
00528 void GetUV( Point3<P3ScalarType> &n,Point3<P3ScalarType> &u, Point3<P3ScalarType> &v, Point3<P3ScalarType> up=(Point3<P3ScalarType>(0,1,0)) )
00529 {
00530 n.Normalize();
00531 const double LocEps=double(1e-7);
00532 u=n^up;
00533 double len = u.Norm();
00534 if(len < LocEps)
00535 {
00536 if(fabs(n[0])<fabs(n[1])){
00537 if(fabs(n[0])<fabs(n[2])) up=Point3<P3ScalarType>(1,0,0);
00538 else up=Point3<P3ScalarType>(0,0,1);
00539 }else {
00540 if(fabs(n[1])<fabs(n[2])) up=Point3<P3ScalarType>(0,1,0);
00541 else up=Point3<P3ScalarType>(0,0,1);
00542 }
00543 u=n^up;
00544 }
00545 u.Normalize();
00546 v=n^u;
00547 v.Normalize();
00548 Point3<P3ScalarType> uv=u^v;
00549 }
00550
00551
00552 template <class SCALARTYPE>
00553 inline Point3<SCALARTYPE> Abs(const Point3<SCALARTYPE> & p) {
00554 return (Point3<SCALARTYPE>(math::Abs(p[0]), math::Abs(p[1]), math::Abs(p[2])));
00555 }
00556
00557 template <class SCALARTYPE>
00558 inline Point3<SCALARTYPE> LowClampToZero(const Point3<SCALARTYPE> & p) {
00559 return (Point3<SCALARTYPE>(math::Max(p[0], (SCALARTYPE)0), math::Max(p[1], (SCALARTYPE)0), math::Max(p[2], (SCALARTYPE)0)));
00560 }
00561
00562 typedef Point3<short> Point3s;
00563 typedef Point3<int> Point3i;
00564 typedef Point3<float> Point3f;
00565 typedef Point3<double> Point3d;
00566
00569 }
00570
00571 #endif
00572
