$darkmode
VCG Library
point3.h
1 /****************************************************************************
2 * VCGLib o o *
3 * Visual and Computer Graphics Library o o *
4 * _ O _ *
5 * Copyright(C) 2004-2016 \/)\/ *
6 * Visual Computing Lab /\/| *
7 * ISTI - Italian National Research Council | *
8 * \ *
9 * All rights reserved. *
10 * *
11 * This program is free software; you can redistribute it and/or modify *
12 * it under the terms of the GNU General Public License as published by *
13 * the Free Software Foundation; either version 2 of the License, or *
14 * (at your option) any later version. *
15 * *
16 * This program is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
19 * GNU General Public License (http://www.gnu.org/licenses/gpl.txt) *
20 * for more details. *
21 * *
22 ****************************************************************************/
23 
24 #ifndef __VCGLIB_POINT3
25 #define __VCGLIB_POINT3
26 
27 #include <assert.h>
28 #include <algorithm>
29 #include <vcg/math/base.h>
30 
31 namespace vcg {
32 
40 template <class T> class Box3;
41 
42 template <class P3ScalarType> class Point3
43 {
44 protected:
46  P3ScalarType _v[3];
47 
48 public:
49  typedef P3ScalarType ScalarType;
50  enum {Dimension = 3};
51 
52 
54 
59  inline Point3 () { }
60  inline Point3 ( const P3ScalarType nx, const P3ScalarType ny, const P3ScalarType nz )
61  {
62  _v[0] = nx;
63  _v[1] = ny;
64  _v[2] = nz;
65  }
66 
68  inline Point3 ( Point3 const & p ) = default;
69 
71  template<class Q>
72  inline Point3 ( Point3<Q> const & p )
73  {
74  _v[0]= p[0];
75  _v[1]= p[1];
76  _v[2]= p[2];
77  }
78 
79  inline Point3 ( const P3ScalarType nv[3] )
80  {
81  _v[0] = nv[0];
82  _v[1] = nv[1];
83  _v[2] = nv[2];
84  }
85 
87  inline Point3 & operator =(Point3 const & p) = default;
88 
90  template<class Q>
91  inline Point3 & operator =(Point3<Q> const & p)
92  {
93  _v[0] = p[0]; _v[1] = p[1]; _v[2] = p[2];
94  return *this;
95  }
96  inline void SetZero()
97  {
98  _v[0] = 0;
99  _v[1] = 0;
100  _v[2] = 0;
101  }
102 
105  inline P3ScalarType Ext( const int i ) const
106  {
107  if(i>=0 && i<=2) return _v[i];
108  else return 0;
109  }
110 
111  template <class Q>
112  inline void Import( const Point3<Q> & b )
113  {
114  _v[0] = P3ScalarType(b[0]);
115  _v[1] = P3ScalarType(b[1]);
116  _v[2] = P3ScalarType(b[2]);
117  }
118  template <class EigenVector>
119  inline void FromEigenVector( const EigenVector & b )
120  {
121  _v[0] = P3ScalarType(b[0]);
122  _v[1] = P3ScalarType(b[1]);
123  _v[2] = P3ScalarType(b[2]);
124  }
125  template <class EigenVector>
126  inline void ToEigenVector( EigenVector & b ) const
127  {
128  b[0]=_v[0] ;
129  b[1]=_v[1] ;
130  b[2]=_v[2] ;
131  }
132  template <class EigenVector>
133  inline EigenVector ToEigenVector(void) const
134  {
135  static_assert(EigenVector::RowsAtCompileTime == 3 || EigenVector::RowsAtCompileTime == 4,
136  "EigenVector type has not 3 or 4 components");
137  EigenVector b = EigenVector::Zero();
138  b[0]=_v[0];
139  b[1]=_v[1];
140  b[2]=_v[2];
141  return b;
142  }
143  template <class Q>
144  static inline Point3 Construct( const Point3<Q> & b )
145  {
146  return Point3(P3ScalarType(b[0]),P3ScalarType(b[1]),P3ScalarType(b[2]));
147  }
148 
149  template <class Q>
150  static inline Point3 Construct( const Q & P0, const Q & P1, const Q & P2)
151  {
152  return Point3(P3ScalarType(P0),P3ScalarType(P1),P3ScalarType(P2));
153  }
154 
155  static inline Point3 Construct( const Point3<ScalarType> & b )
156  {
157  return b;
158  }
159 
160  static inline Point3 Zero(void)
161  {
162  return Point3(0,0,0);
163  }
164 
165  static inline Point3 One(void)
166  {
167  return Point3(1,1,1);
168  }
169 
171 
173 
177  inline P3ScalarType & operator [] ( const int i )
178  {
179  assert(i>=0 && i<3);
180  return _v[i];
181  }
182  inline const P3ScalarType & operator [] ( const int i ) const
183  {
184  assert(i>=0 && i<3);
185  return _v[i];
186  }
187  inline const P3ScalarType &X() const { return _v[0]; }
188  inline const P3ScalarType &Y() const { return _v[1]; }
189  inline const P3ScalarType &Z() const { return _v[2]; }
190  inline P3ScalarType &X() { return _v[0]; }
191  inline P3ScalarType &Y() { return _v[1]; }
192  inline P3ScalarType &Z() { return _v[2]; }
193  inline const P3ScalarType * V() const
194  {
195  return _v;
196  }
197  inline P3ScalarType * V()
198  {
199  return _v;
200  }
201  inline P3ScalarType & V( const int i )
202  {
203  assert(i>=0 && i<3);
204  return _v[i];
205  }
206  inline const P3ScalarType & V( const int i ) const
207  {
208  assert(i>=0 && i<3);
209  return _v[i];
210  }
211 
215  inline Point3 operator + ( Point3 const & p) const
216  {
217  return Point3<P3ScalarType>( _v[0]+p._v[0], _v[1]+p._v[1], _v[2]+p._v[2] );
218  }
219  inline Point3 operator - ( Point3 const & p) const
220  {
221  return Point3<P3ScalarType>( _v[0]-p._v[0], _v[1]-p._v[1], _v[2]-p._v[2] );
222  }
223  inline Point3 operator * ( const P3ScalarType s ) const
224  {
225  return Point3<P3ScalarType>( _v[0]*s, _v[1]*s, _v[2]*s );
226  }
227  inline Point3 operator / ( const P3ScalarType s ) const
228  {
229  return Point3<P3ScalarType>( _v[0]/s, _v[1]/s, _v[2]/s );
230  }
232  inline P3ScalarType operator * ( Point3 const & p ) const
233  {
234  return ( _v[0]*p._v[0] + _v[1]*p._v[1] + _v[2]*p._v[2] );
235  }
236  inline P3ScalarType dot( const Point3 & p ) const { return (*this) * p; }
238  inline Point3 operator ^ ( Point3 const & p ) const
239  {
240  return Point3 <P3ScalarType>
241  (
242  _v[1]*p._v[2] - _v[2]*p._v[1],
243  _v[2]*p._v[0] - _v[0]*p._v[2],
244  _v[0]*p._v[1] - _v[1]*p._v[0]
245  );
246  }
247 
248  inline Point3 & operator += ( Point3 const & p)
249  {
250  _v[0] += p._v[0];
251  _v[1] += p._v[1];
252  _v[2] += p._v[2];
253  return *this;
254  }
255  inline Point3 & operator -= ( Point3 const & p)
256  {
257  _v[0] -= p._v[0];
258  _v[1] -= p._v[1];
259  _v[2] -= p._v[2];
260  return *this;
261  }
262  inline Point3 & operator *= ( const P3ScalarType s )
263  {
264  _v[0] *= s;
265  _v[1] *= s;
266  _v[2] *= s;
267  return *this;
268  }
269  inline Point3 & operator /= ( const P3ScalarType s )
270  {
271  _v[0] /= s;
272  _v[1] /= s;
273  _v[2] /= s;
274  return *this;
275  }
276 
277  // Norms
278  inline P3ScalarType Norm() const
279  {
280  return math::Sqrt( _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] );
281  }
282  inline P3ScalarType SquaredNorm() const
283  {
284  return ( _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] );
285  }
286  // Scalatura differenziata
287  inline Point3 & Scale( const P3ScalarType sx, const P3ScalarType sy, const P3ScalarType sz )
288  {
289  _v[0] *= sx;
290  _v[1] *= sy;
291  _v[2] *= sz;
292  return *this;
293  }
294  inline Point3 & Scale( const Point3 & p )
295  {
296  _v[0] *= p._v[0];
297  _v[1] *= p._v[1];
298  _v[2] *= p._v[2];
299  return *this;
300  }
301 
302  // Normalization
303  inline Point3 & Normalize()
304  {
305  P3ScalarType n = P3ScalarType(math::Sqrt(_v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2]));
306  if (n > P3ScalarType(0)) { _v[0] /= n; _v[1] /= n; _v[2] /= n; }
307  return *this;
308  }
309 
310  inline void normalize(void)
311  {
312  this->Normalize();
313  }
314 
315  inline Point3 normalized(void) const
316  {
317  Point3<P3ScalarType> p = *this;
318  p.normalize();
319  return p;
320  }
321 
332  void ToPolarRad(P3ScalarType &ro, P3ScalarType &theta, P3ScalarType &phi) const
333  {
334  ro = Norm();
335  theta = (P3ScalarType)atan2(_v[2], _v[0]);
336  phi = (P3ScalarType)asin(_v[1]/ro);
337  }
338 
349  void FromPolarRad(const P3ScalarType &ro, const P3ScalarType &theta, const P3ScalarType &phi)
350  {
351  _v[0]= ro*cos(theta)*cos(phi);
352  _v[1]= ro*sin(phi);
353  _v[2]= ro*sin(theta)*cos(phi);
354  }
355 
356  Box3<P3ScalarType> GetBBox(Box3<P3ScalarType> &bb) const;
357 
358  size_t MaxCoeffId() const
359  {
360  if (_v[0]>_v[1])
361  return _v[0]>_v[2] ? 0 : 2;
362  else
363  return _v[1]>_v[2] ? 1 : 2;
364  }
369 inline bool operator == ( Point3 const & p ) const
370  {
371  return _v[0]==p._v[0] && _v[1]==p._v[1] && _v[2]==p._v[2];
372  }
373  inline bool operator != ( Point3 const & p ) const
374  {
375  return _v[0]!=p._v[0] || _v[1]!=p._v[1] || _v[2]!=p._v[2];
376  }
377  inline bool operator < ( Point3 const & p ) const
378  {
379  return (_v[2]!=p._v[2])?(_v[2]<p._v[2]):
380  (_v[1]!=p._v[1])?(_v[1]<p._v[1]):
381  (_v[0]<p._v[0]);
382  }
383  inline bool operator > ( Point3 const & p ) const
384  {
385  return (_v[2]!=p._v[2])?(_v[2]>p._v[2]):
386  (_v[1]!=p._v[1])?(_v[1]>p._v[1]):
387  (_v[0]>p._v[0]);
388  }
389  inline bool operator <= ( Point3 const & p ) const
390  {
391  return (_v[2]!=p._v[2])?(_v[2]< p._v[2]):
392  (_v[1]!=p._v[1])?(_v[1]< p._v[1]):
393  (_v[0]<=p._v[0]);
394  }
395  inline bool operator >= ( Point3 const & p ) const
396  {
397  return (_v[2]!=p._v[2])?(_v[2]> p._v[2]):
398  (_v[1]!=p._v[1])?(_v[1]> p._v[1]):
399  (_v[0]>=p._v[0]);
400  }
401 
402  inline Point3 operator - (void) const
403  {
404  return Point3<P3ScalarType> ( -_v[0], -_v[1], -_v[2] );
405  }
407 
408 }; // end class definition
409 
410 
411 template <class P3ScalarType>
412 inline P3ScalarType Angle( Point3<P3ScalarType> const & p1, Point3<P3ScalarType> const & p2 )
413 {
414  P3ScalarType w = p1.Norm()*p2.Norm();
415  if(w==0) return -1;
416  P3ScalarType t = (p1*p2)/w;
417  if(t>1) t = 1;
418  else if(t<-1) t = -1;
419  return (P3ScalarType) acos(t);
420 }
421 
422 // versione uguale alla precedente ma che assume che i due vettori sono unitari
423 template <class P3ScalarType>
424 inline P3ScalarType AngleN( Point3<P3ScalarType> const & p1, Point3<P3ScalarType> const & p2 )
425 {
426  P3ScalarType w = p1*p2;
427  if(w>1)
428  w = 1;
429  else if(w<-1)
430  w=-1;
431  return (P3ScalarType) acos(w);
432 }
433 
434 
435 template <class P3ScalarType>
436 inline P3ScalarType Norm( Point3<P3ScalarType> const & p )
437 {
438  return p.Norm();
439 }
440 
441 template <class P3ScalarType>
442 inline P3ScalarType SquaredNorm( Point3<P3ScalarType> const & p )
443 {
444  return p.SquaredNorm();
445 }
446 
447 template <class P3ScalarType>
448 inline Point3<P3ScalarType> & Normalize( Point3<P3ScalarType> & p )
449 {
450  return p.Normalize();
451 }
452 
453 template <typename Scalar>
454 inline Point3<Scalar> Normalized(const Point3<Scalar> & p)
455 {
456  return p.normalized();
457 }
458 
459 template <class P3ScalarType>
460 inline P3ScalarType Distance( Point3<P3ScalarType> const & p1,Point3<P3ScalarType> const & p2 )
461 {
462  return (p1-p2).Norm();
463 }
464 
465 template <class P3ScalarType>
466 inline P3ScalarType SquaredDistance( Point3<P3ScalarType> const & p1,Point3<P3ScalarType> const & p2 )
467 {
468  return (p1-p2).SquaredNorm();
469 }
470 
471 template <class P3ScalarType>
472 P3ScalarType ApproximateGeodesicDistance(const Point3<P3ScalarType>& p0, const Point3<P3ScalarType>& n0,
473  const Point3<P3ScalarType>& p1, const Point3<P3ScalarType>& n1)
474 {
475  Point3<P3ScalarType> V(p0-p1);
476  V.Normalize();
477  const P3ScalarType C0 = V*n0;
478  const P3ScalarType C1 = V*n1;
479  const P3ScalarType De = Distance(p0,p1);
480  if(fabs(C0-C1)<0.0001) return De/sqrt(1-C0*C1);
481  const P3ScalarType Dg = ((asin(C0) - asin(C1))/(C0-C1));
482  return Dg*De;
483 }
484 
485 
486  // Dot product preciso numericamente (solo double!!)
487  // Implementazione: si sommano i prodotti per ordine di esponente
488  // (prima le piu' grandi)
489 template<class P3ScalarType>
490 double stable_dot ( Point3<P3ScalarType> const & p0, Point3<P3ScalarType> const & p1 )
491 {
492  P3ScalarType k0 = p0._v[0]*p1._v[0];
493  P3ScalarType k1 = p0._v[1]*p1._v[1];
494  P3ScalarType k2 = p0._v[2]*p1._v[2];
495 
496  int exp0,exp1,exp2;
497 
498  frexp( double(k0), &exp0 );
499  frexp( double(k1), &exp1 );
500  frexp( double(k2), &exp2 );
501 
502  if( exp0<exp1 )
503  {
504  if(exp0<exp2)
505  return (k1+k2)+k0;
506  else
507  return (k0+k1)+k2;
508  }
509  else
510  {
511  if(exp1<exp2)
512  return(k0+k2)+k1;
513  else
514  return (k0+k1)+k2;
515  }
516 }
517 
518 
519 
521 template<class P3ScalarType>
522 P3ScalarType PSDist( const Point3<P3ScalarType> & p,
523  const Point3<P3ScalarType> & v1,
524  const Point3<P3ScalarType> & v2,
526 {
527  Point3<P3ScalarType> e = v2-v1;
528  P3ScalarType t = ((p-v1)*e)/e.SquaredNorm();
529  if(t<0) t = 0;
530  else if(t>1) t = 1;
531  q = v1+e*t;
532  return Distance(p,q);
533 }
534 
535 
536 template <class P3ScalarType>
537 void GetUV( Point3<P3ScalarType> &n,Point3<P3ScalarType> &u, Point3<P3ScalarType> &v, Point3<P3ScalarType> up=(Point3<P3ScalarType>(0,1,0)) )
538 {
539  n.Normalize();
540  const double LocEps=double(1e-7);
541  u=n^up;
542  double len = u.Norm();
543  if(len < LocEps)
544  {
545  if(fabs(n[0])<fabs(n[1])){
546  if(fabs(n[0])<fabs(n[2])) up=Point3<P3ScalarType>(1,0,0); // x is the min
547  else up=Point3<P3ScalarType>(0,0,1); // z is the min
548  }else {
549  if(fabs(n[1])<fabs(n[2])) up=Point3<P3ScalarType>(0,1,0); // y is the min
550  else up=Point3<P3ScalarType>(0,0,1); // z is the min
551  }
552  u=n^up;
553  }
554  u.Normalize();
555  v=n^u;
556  v.Normalize();
557 }
558 
559 
560 template <class SCALARTYPE>
561 inline Point3<SCALARTYPE> Abs(const Point3<SCALARTYPE> & p) {
562  return (Point3<SCALARTYPE>(math::Abs(p[0]), math::Abs(p[1]), math::Abs(p[2])));
563 }
564 // probably a more uniform naming should be defined...
565 template <class SCALARTYPE>
566 inline Point3<SCALARTYPE> LowClampToZero(const Point3<SCALARTYPE> & p) {
567  return (Point3<SCALARTYPE>(std::max(p[0], (SCALARTYPE)0), std::max(p[1], (SCALARTYPE)0), std::max(p[2], (SCALARTYPE)0)));
568 }
569 
570 template <typename Scalar>
571 inline Point3<Scalar> operator*(const Scalar s, const Point3<Scalar> & p)
572 {
573  return (p * s);
574 }
575 
576 typedef Point3<short> Point3s;
577 typedef Point3<int> Point3i;
578 typedef Point3<float> Point3f;
579 typedef Point3<double> Point3d;
580 
583 } // end namespace
584 
585 #endif
586 
Definition: box3.h:42
Definition: point3.h:43
Point3 & operator=(Point3 const &p)=default
Point3(Point3 const &p)=default
P3ScalarType _v[3]
The only data member. Hidden to user.
Definition: point3.h:46
void ToPolarRad(P3ScalarType &ro, P3ScalarType &theta, P3ScalarType &phi) const
Definition: point3.h:332
P3ScalarType Ext(const int i) const
Definition: point3.h:105
void FromPolarRad(const P3ScalarType &ro, const P3ScalarType &theta, const P3ScalarType &phi)
Definition: point3.h:349
Point3 operator^(Point3 const &p) const
Cross product.
Definition: point3.h:238
Point3(Point3< Q > const &p)
Definition: point3.h:72
Definition: namespaces.dox:6
P3ScalarType PSDist(const Point3< P3ScalarType > &p, const Point3< P3ScalarType > &v1, const Point3< P3ScalarType > &v2, Point3< P3ScalarType > &q)
Point(p) Edge(v1-v2) dist, q is the point in v1-v2 with min dist.
Definition: point3.h:522