1 // Copyright (C) 2009-2015 OPEN CASCADE
3 // This library is free software; you can redistribute it and/or
4 // modify it under the terms of the GNU Lesser General Public
5 // License as published by the Free Software Foundation; either
6 // version 2.1 of the License, or (at your option) any later version.
8 // This library is distributed in the hope that it will be useful,
9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11 // Lesser General Public License for more details.
13 // You should have received a copy of the GNU Lesser General Public
14 // License along with this library; if not, write to the Free Software
15 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
17 // See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
19 // File : DirectedBoundingBox.cxx
20 // Created : Mon Apr 12 14:41:22 2010
21 // Author : Edward AGAPOV (eap)
23 #include "DirectedBoundingBox.hxx"
25 #include "InterpolationUtils.hxx"
27 #define __TENSOR(i,j) tensor[(i)*_dim+(j)]
28 #define __AXIS(i) (&_axes[(i)*_dim])
29 #define __MIN(i) _minmax[i*2]
30 #define __MAX(i) _minmax[i*2+1]
31 #define __MYID (long(this)%10000)
33 // cout << msg << endl
39 //================================================================================
41 * \brief Add point coordinates to inertia tensor in 3D space
43 //================================================================================
45 inline void addPointToInertiaTensor3D(const double* coord,
47 vector<double>& tensor)
49 // we fill the upper triangle of tensor only
51 double x = coord[0] - gc[0], y = coord[1] - gc[1], z = coord[2] - gc[2];
52 __TENSOR(0,0) += y*y + z*z;
53 __TENSOR(1,1) += x*x + z*z;
54 __TENSOR(2,2) += x*x + y*y;
59 //================================================================================
61 * \brief Add point coordinates to inertia tensor in 2D space
63 //================================================================================
65 inline void addPointToInertiaTensor2D(const double* coord,
67 vector<double>& tensor)
69 // we fill the upper triangle of tensor only
71 double x = coord[0] - gc[0], y = coord[1] - gc[1];
77 //================================================================================
79 * \brief Find eigenvectors of tensor using Jacobi's method
81 //================================================================================
83 bool JacobiEigenvectorsSearch( const int _dim, vector<double>& tensor, vector<double>& _axes)
88 << "{ "<<__TENSOR(0,0) << ", "<<__TENSOR(0,1) << ", "<<__TENSOR(0,2) << "} "
89 << "{ "<<__TENSOR(1,0) << ", "<<__TENSOR(1,1) << ", "<<__TENSOR(1,2) << "} "
90 << "{ "<<__TENSOR(2,0) << ", "<<__TENSOR(2,1) << ", "<<__TENSOR(2,2) << "}} ");
95 << "{ "<<__TENSOR(0,0) << ", "<<__TENSOR(0,1) << "} "
96 << "{ "<<__TENSOR(1,0) << ", "<<__TENSOR(1,1) << "}} ");
99 const int maxRot = 5*_dim*_dim; // limit on number of rotations
100 const double tol = 1e-9;
102 // set _axes to identity
104 for ( i = 0; i < _dim; ++i )
105 for ( j = 0; j < _dim; ++j )
106 __AXIS(i)[j] = ( i==j ? 1. : 0 );
109 for ( int iRot = 0; iRot < maxRot; ++ iRot )
111 // find max off-diagonal element of the tensor
114 for ( i = 0; i < _dim-1; ++i )
115 for ( j = i+1; j < _dim; ++j )
116 if ( fabs( __TENSOR(i,j)) > max )
117 max = fabs( __TENSOR(i,j) ), k = i, l = j;
118 solved = ( max < tol );
122 // Rotate to make __TENSOR(k,l) == 0
124 double diff = __TENSOR(l,l) - __TENSOR(k,k);
125 double t; // tangent of rotation angle
126 if ( fabs(__TENSOR(k,l)) < abs(diff)*1.0e-36)
128 t = __TENSOR(k,l)/diff;
132 double phi = diff/(2.0*__TENSOR(k,l));
133 t = 1.0/(abs(phi) + sqrt(phi*phi + 1.0));
134 if ( phi < 0.0) t = -t;
136 double c = 1.0/sqrt(t*t + 1.0); // cosine of rotation angle
137 double s = t*c; // sine of rotation angle
138 double tau = s/(1.0 + c);
139 __TENSOR(k,k) -= t*__TENSOR(k,l);
140 __TENSOR(l,l) += t*__TENSOR(k,l);
143 #define __ROTATE(T,r1,c1,r2,c2) \
145 int i1 = r1*_dim+c1, i2 = r2*_dim+c2; \
146 double t1 = T[i1], t2 = T[i2]; \
147 T[i1] -= s * ( t2 + tau * t1);\
148 T[i2] += s * ( t1 - tau * t2);\
150 for ( i = 0; i < k; ++i ) // Case of i < k
151 __ROTATE(tensor, i,k,i,l);
153 for ( i = k+1; i < l; ++i ) // Case of k < i < l
154 __ROTATE(tensor, k,i,i,l);
156 for ( i = l + 1; i < _dim; ++i ) // Case of i > l
157 __ROTATE(tensor, k,i,l,i);
159 for ( i = 0; i < _dim; ++i ) // Update transformation matrix
160 __ROTATE(_axes, i,k,i,l);
163 __DMP( "Solved = " << solved );
165 __DMP( " Eigen " << __TENSOR(0,0)<<", "<<__TENSOR(1,1)<<", "<<__TENSOR(2,2) );
166 for ( int ii=0; ii <3; ++ii )
167 __DMP( ii << ": " << __AXIS(ii)[0] << ", " << __AXIS(ii)[1] << ", " << __AXIS(ii)[2] );
170 __DMP( " Eigen " << __TENSOR(0,0) << ", " << __TENSOR(1,1) );
171 for ( int ii=0; ii <2; ++ii )
172 __DMP( ii << ": " << __AXIS(ii)[0] << ", " << __AXIS(ii)[1] );
178 //================================================================================
180 * \brief Return true if two minmaxes do not intersect
182 //================================================================================
184 inline bool isMinMaxOut(const double* minmax1,
185 const double* minmax2,
188 for ( int i = 0; i < dim; ++i )
190 if ( minmax1[i*2] > minmax2[i*2+1] ||
191 minmax1[i*2+1] < minmax2[i*2] )
197 } // noname namespace
199 namespace INTERP_KERNEL
202 //================================================================================
204 * \brief Creates empty box intended to further initalization via setData()
206 //================================================================================
208 DirectedBoundingBox::DirectedBoundingBox():_dim(0)
212 //================================================================================
214 * \brief Creates bounding box of a mesh
215 * \param pts - coordinates of points in full interlace
216 * \param numPts - number of points in the mesh
217 * \param dim - space dimension
219 //================================================================================
221 DirectedBoundingBox::DirectedBoundingBox(const double* pts,
222 const unsigned numPts,
224 : _dim(dim), _axes(dim*dim), _minmax(2*dim)
226 // init box extremities
227 for ( unsigned i = 0; i < _dim; ++i )
228 _minmax[1+i*2] = -numeric_limits<double>::max(),
229 _minmax[i*2] = numeric_limits<double>::max();
231 if ( numPts < 1 ) return;
233 __DMP( "DirectedBoundingBox " << __MYID );
235 const double* coord = pts;
236 const double* coordEnd = coord + numPts * dim;
238 // compute gravity center of points
239 double gc[3] = {0,0,0};
242 for ( coord = pts; coord < coordEnd; )
243 for ( int i = 0; i < (int)dim; ++i )
245 for ( int j = 0; j < (int)dim; ++j )
250 // compute axes and box extremities
251 vector<double> tensor( dim * dim, 0.);
255 for ( coord = pts; coord < coordEnd; coord += dim )
256 addPointToInertiaTensor3D( coord, gc, tensor );
258 //computeAxes3D(tensor);
259 JacobiEigenvectorsSearch(_dim, tensor, _axes);
261 for ( coord = pts; coord < coordEnd; coord += dim )
262 addPointToBox( coord );
267 for ( coord = pts; coord < coordEnd; coord += dim )
268 addPointToInertiaTensor2D( coord, gc, tensor );
270 //computeAxes2D(tensor);
271 JacobiEigenvectorsSearch(_dim, tensor, _axes);
273 for ( coord = pts; coord < coordEnd; coord += dim )
274 addPointToBox( coord );
279 for ( coord = pts; coord < coordEnd; coord += dim )
281 if ( *coord < _minmax[0] ) _minmax[0] = *coord;
282 if ( *coord > _minmax[1] ) _minmax[1] = *coord;
287 //================================================================================
289 * \brief Creates bounding box of an element
290 * \param pts - coordinates of points of element
291 * \param numPts - number of points in the element
292 * \param dim - space dimension
294 //================================================================================
296 DirectedBoundingBox::DirectedBoundingBox(const double** pts,
297 const unsigned numPts,
299 : _dim(dim), _axes(dim*dim), _minmax(2*dim)
301 // init box extremities
302 for ( unsigned i = 0; i < _dim; ++i )
303 _minmax[1+i*2] = -numeric_limits<double>::max(),
304 _minmax[i*2] = numeric_limits<double>::max();
306 if ( numPts < 1 ) return;
308 __DMP( "DirectedBoundingBox " << __MYID );
310 // compute gravity center of points
311 double gc[3] = {0,0,0};
314 for ( unsigned i = 0; i < numPts; ++i )
315 for ( int j = 0; j < (int)dim; ++j )
317 for ( int j = 0; j < (int)dim; ++j )
321 // compute axes and box extremities
322 vector<double> tensor( dim * dim, 0.);
326 for ( unsigned i = 0; i < numPts; ++i )
327 addPointToInertiaTensor3D( pts[i], gc, tensor );
329 //computeAxes3D(tensor);
330 JacobiEigenvectorsSearch(_dim, tensor, _axes);
332 for ( unsigned i = 0; i < numPts; ++i )
333 addPointToBox( pts[i] );
337 for ( unsigned i = 0; i < numPts; ++i )
338 addPointToInertiaTensor2D( pts[i], gc, tensor );
340 //computeAxes2D(tensor);
341 JacobiEigenvectorsSearch(_dim, tensor, _axes);
343 for ( unsigned i = 0; i < numPts; ++i )
344 addPointToBox( pts[i] );
348 for ( unsigned i = 0; i < numPts; ++i )
350 if ( pts[i][0] < _minmax[0] ) _minmax[0] = pts[i][0];
351 if ( pts[i][0] > _minmax[1] ) _minmax[1] = pts[i][0];
357 //================================================================================
359 * \brief Compute eigenvectors of inertia tensor
361 //================================================================================
363 // void DirectedBoundingBox::computeAxes3D(const std::vector<double>& tensor)
365 // // compute principal moments of inertia which are eigenvalues of the tensor
368 // // coefficients of polynomial equation det(tensor-eig*I) = 0
370 // double b = __TENSOR(0,0)+__TENSOR(1,1)+__TENSOR(2,2);
372 // __TENSOR(0,1)*__TENSOR(0,1) +
373 // __TENSOR(0,2)*__TENSOR(0,2) +
374 // __TENSOR(1,2)*__TENSOR(1,2) -
375 // __TENSOR(0,0)*__TENSOR(1,1) -
376 // __TENSOR(0,0)*__TENSOR(2,2) -
377 // __TENSOR(1,1)*__TENSOR(2,2);
379 // __TENSOR(0,0)*__TENSOR(1,1)*__TENSOR(2,2) -
380 // __TENSOR(0,0)*__TENSOR(1,2)*__TENSOR(1,2) -
381 // __TENSOR(1,1)*__TENSOR(0,2)*__TENSOR(0,2) -
382 // __TENSOR(2,2)*__TENSOR(0,1)*__TENSOR(0,1) +
383 // __TENSOR(0,1)*__TENSOR(0,2)*__TENSOR(1,2)*2;
385 // // find eigenvalues which are roots of characteristic polynomial
386 // double x = (3*c/a - b*b/(a*a))/3;
387 // double y = (2*b*b*b/(a*a*a) - 9*b*c/(a*a) + 27*d/a)/27;
388 // double z = y*y/4 + x*x*x/27;
390 // double i = sqrt(y*y/4 - z) + 1e-300;
391 // double j = -pow(i,1/3.);
392 // double y2 = -y/(2*i);
393 // if ( y2 > 1.0) y2 = 1.; else if ( y2 < -1.0) y2 = -1.;
394 // double k = acos(y2);
395 // double m = cos(k/3);
396 // double n = sqrt(3)*sin(k/3);
397 // double p = -b/(3*a);
399 // eig[0] = -2*j*m + p;
400 // eig[1] = j *(m + n) + p;
401 // eig[2] = j *(m - n) + p;
403 // // compute eigenvector of the tensor at each eigenvalue
404 // // by solving system [tensor-eig*I]*[axis] = 0
406 // __DMP( "Tensor : {"
407 // << "{ "<<__TENSOR(0,0) << ", "<<__TENSOR(0,1) << ", "<<__TENSOR(0,2) << "} "
408 // << "{ "<<__TENSOR(1,0) << ", "<<__TENSOR(1,1) << ", "<<__TENSOR(1,2) << "} "
409 // << "{ "<<__TENSOR(2,0) << ", "<<__TENSOR(2,1) << ", "<<__TENSOR(2,2) << "}} ");
410 // for ( int i = 0; i < 3 && ok; ++i ) // loop on 3 eigenvalues
414 // {{ __TENSOR(0,0)-eig[i],__TENSOR(0,1), __TENSOR(0,2), },
415 // { __TENSOR(0,1), __TENSOR(1,1)-eig[i],__TENSOR(1,2), },
416 // { __TENSOR(0,2), __TENSOR(1,2), __TENSOR(2,2)-eig[i]}};
417 // // The determinant of T is zero, so that the equations are not linearly independent.
418 // // Therefore, we assign an arbitrary value (1.) to i-th component of eigenvector
419 // // and use two of the equations to compute the other two components
420 // double M[2][3], sol[2];
421 // for ( int j = 0, c = 0; j < 3; ++j )
423 // M[0][2] = -T[0][j], M[1][2] = -T[1][j];
425 // M[0][c] = T[0][j], M[1][c] = T[1][j], c++;
427 // ok = solveSystemOfEquations<2>( M, sol );
429 // double* eigenVec = __AXIS(i);
430 // for ( int j = 0, c = 0; j < 3; ++j )
431 // eigenVec[j] = ( i == j ) ? 1. : sol[c++];
434 // double size = sqrt(eigenVec[0]*eigenVec[0] +
435 // eigenVec[1]*eigenVec[1] +
436 // eigenVec[2]*eigenVec[2] );
437 // if ((ok = (size > numeric_limits<double>::min() )))
439 // eigenVec[0] /= size;
440 // eigenVec[1] /= size;
441 // eigenVec[2] /= size;
446 // __DMP( " solve3EquationSystem() - KO " );
447 // _axes = vector<double>( _dim*_dim, 0);
448 // __AXIS(0)[0] = __AXIS(1)[1] = __AXIS(2)[2] = 1.;
450 // __DMP( " Eigen " << eig[0] << ", " << eig[1] << ", " << eig[2] );
451 // for ( int i=0; i <3; ++i )
452 // __DMP( i << ": " << __AXIS(i)[0] << ", " << __AXIS(i)[1] << ", " << __AXIS(i)[2] );
454 // double* a0 = __AXIS(0), *a1 = __AXIS(1);
455 // double cross[3] = { a0[1]*a1[2]-a1[1]*a0[2],
456 // a0[2]*a1[0]-a1[2]*a0[0],
457 // a0[0]*a1[1]-a1[0]*a0[1] };
458 // __DMP( " Cross a1^a2 " << cross[0] << ", " << cross[1] << ", " << cross[2] );
461 //================================================================================
463 * \brief Compute eigenvectors of inertia tensor
465 //================================================================================
467 // void DirectedBoundingBox::computeAxes2D(const std::vector<double>& tensor)
469 // // compute principal moments of inertia which are eigenvalues of the tensor
470 // // by solving square equation det(tensor-eig*I)
471 // double X = (__TENSOR(0,0)+__TENSOR(1,1))/2;
472 // double Y = sqrt(4*__TENSOR(0,1)*__TENSOR(0,1) +
473 // (__TENSOR(0,0)-__TENSOR(1,1)) * (__TENSOR(0,0)-__TENSOR(1,1)))/2;
479 // // compute eigenvector of the tensor at each eigenvalue
480 // // by solving system [tensor-eig*I]*[axis] = 0
482 // for ( int i = 0; i < 2 && ok; ++i )
486 // {{ __TENSOR(0,0)-eig[i],__TENSOR(0,1) },
487 // { __TENSOR(0,1), __TENSOR(1,1)-eig[i] }};
489 // // The determinant of T is zero, so that the equations are not linearly independent.
490 // // Therefore, we assign an arbitrary value (1.) to i-th component of eigenvector
491 // // and use one equation to compute the other component
492 // double* eigenVec = __AXIS(i);
495 // if ((ok = ( fabs( T[j][j] ) > numeric_limits<double>::min() )))
496 // eigenVec[j] = -T[j][i] / T[j][j];
500 // _axes = vector<double>( _dim*_dim, 0);
501 // __AXIS(0)[0] = __AXIS(1)[1] = 1.;
505 //================================================================================
507 * \brief Convert point coordinates into local coordinate system of the box
509 //================================================================================
511 void DirectedBoundingBox::toLocalCS(const double* p, double* pLoc) const
516 pLoc[0] = dotprod<3>( p, __AXIS(0));
517 pLoc[1] = dotprod<3>( p, __AXIS(1));
518 pLoc[2] = dotprod<3>( p, __AXIS(2));
521 pLoc[0] = dotprod<2>( p, __AXIS(0));
522 pLoc[1] = dotprod<2>( p, __AXIS(1));
529 //================================================================================
531 * \brief Convert point coordinates from local coordinate system of the box to global CS
533 //================================================================================
535 void DirectedBoundingBox::fromLocalCS(const double* p, double* pGlob) const
540 pGlob[0] = p[0] * __AXIS(0)[0] + p[1] * __AXIS(1)[0] + p[2] * __AXIS(2)[0];
541 pGlob[1] = p[0] * __AXIS(0)[1] + p[1] * __AXIS(1)[1] + p[2] * __AXIS(2)[1];
542 pGlob[2] = p[0] * __AXIS(0)[2] + p[1] * __AXIS(1)[2] + p[2] * __AXIS(2)[2];
545 pGlob[0] = p[0] * __AXIS(0)[0] + p[1] * __AXIS(1)[0];
546 pGlob[1] = p[0] * __AXIS(0)[1] + p[1] * __AXIS(1)[1];
553 //================================================================================
555 * \brief Enlarge box size by given value
557 //================================================================================
559 void DirectedBoundingBox::enlarge(const double tol)
561 for ( unsigned i = 0; i < _dim; ++i )
562 __MIN(i) -= tol, __MAX(i) += tol;
565 //================================================================================
567 * \brief Return coordinates of corners of bounding box
569 //================================================================================
571 void DirectedBoundingBox::getCorners(std::vector<double>& corners,
572 const double* minmax) const
574 int iC, nbCorners = 1;
575 for ( int i=0;i<(int)_dim;++i ) nbCorners *= 2;
576 corners.resize( nbCorners * _dim );
577 // each coordinate is filled with either min or max, nbSwap is number of corners
578 // after which min and max swap
579 int nbSwap = nbCorners/2;
580 for ( unsigned i = 0; i < _dim; ++i )
583 while ( iC < nbCorners )
585 for (int j = 0; j < nbSwap; ++j, ++iC ) corners[iC*_dim+i] = minmax[i*2];
586 for (int j = 0; j < nbSwap; ++j, ++iC ) corners[iC*_dim+i] = minmax[i*2+1];
592 //================================================================================
594 * \brief Test if this box intersects with the other
595 * \retval bool - true if there is no intersection
597 //================================================================================
599 bool DirectedBoundingBox::isDisjointWith(const DirectedBoundingBox& box) const
601 if ( _dim < 1 || box._dim < 1 ) return false; // empty box includes all
603 return isMinMaxOut( &box._minmax[0], &this->_minmax[0], _dim );
605 // boxes are disjoined if their minmaxes in local CS of either of boxes do not intersect
606 for ( int isThisCS = 0; isThisCS < 2; ++isThisCS )
608 const DirectedBoundingBox* axisBox = isThisCS ? this : &box;
609 const DirectedBoundingBox* cornerBox = isThisCS ? &box : this;
611 // find minmax of cornerBox in the CS of axisBox
613 DirectedBoundingBox mmBox((double*)0,0,_dim); //!< empty box with CS == axisBox->_axes
614 mmBox._axes = axisBox->_axes;
616 vector<double> corners;
617 getCorners( corners, &cornerBox->_minmax[0] );
619 double globCorner[3];
620 for ( int iC = 0, nC = corners.size()/_dim; iC < nC; ++iC)
622 cornerBox->fromLocalCS( &corners[iC*_dim], globCorner );
623 mmBox.addPointToBox( globCorner );
625 if ( isMinMaxOut( &mmBox._minmax[0], &axisBox->_minmax[0], _dim ))
631 //================================================================================
633 * \brief Test if this box intersects with an non-directed box
634 * \retval bool - true if there is no intersection
636 //================================================================================
638 bool DirectedBoundingBox::isDisjointWith(const double* box) const
640 if ( _dim < 1 ) return false; // empty box includes all
642 return isMinMaxOut( &_minmax[0], box, _dim );
644 // boxes are disjoined if their minmaxes in local CS of either of boxes do not intersect
646 // compare minmaxes in locals CS of this directed box
648 vector<double> cornersOther;
649 getCorners( cornersOther, box );
650 DirectedBoundingBox mmBox((double*)0,0,_dim); //!< empty box with CS == this->_axes
651 mmBox._axes = this->_axes;
652 for ( int iC = 0, nC = cornersOther.size()/_dim; iC < nC; ++iC)
653 mmBox.addPointToBox( &cornersOther[iC*_dim] );
655 if ( isMinMaxOut( &mmBox._minmax[0], &this->_minmax[0], _dim ))
659 // compare minmaxes in global CS
661 vector<double> cornersThis;
662 getCorners( cornersThis, &_minmax[0] );
663 DirectedBoundingBox mmBox((double*)0,0,_dim); //!< initailized _minmax
664 double globCorner[3];
665 for ( int iC = 0, nC = cornersThis.size()/_dim; iC < nC; ++iC)
667 fromLocalCS( &cornersThis[iC*_dim], globCorner );
668 for ( int i = 0; i < (int)_dim; ++i )
670 if ( globCorner[i] < mmBox._minmax[i*2] ) mmBox._minmax[i*2] = globCorner[i];
671 if ( globCorner[i] > mmBox._minmax[i*2+1] ) mmBox._minmax[i*2+1] = globCorner[i];
674 if ( isMinMaxOut( &mmBox._minmax[0], box, _dim ))
680 //================================================================================
682 * \brief Return true if given point is out of this box
684 //================================================================================
686 bool DirectedBoundingBox::isOut(const double* point) const
688 if ( _dim < 1 ) return false; // empty box includes all
691 toLocalCS( point, pLoc );
692 bool out = isLocalOut( pLoc );
697 __DMP(__MYID<<": "<<point[0]<<", "<<point[1]<<", "<<point[2]<<" "<<(out?"OUT":"IN"));break;
699 __DMP(__MYID<<": "<<point[0]<<", "<<point[1]<<" "<<(out?"OUT":"IN"));break;
701 __DMP(__MYID<<": "<<point[0]<<" "<<(out?"OUT":"IN"));break;
707 //================================================================================
709 * \brief Return array of internal data
711 //================================================================================
713 vector<double> DirectedBoundingBox::getData() const
715 vector<double> data(1, _dim);
718 data.insert( data.end(), &_axes[0], &_axes[0] + _axes.size());
719 data.insert( data.end(), &_minmax[0], &_minmax[0] + _minmax.size());
721 if ( data.size() < (unsigned)dataSize( _dim ))
722 data.resize( dataSize( _dim ), 0 );
726 //================================================================================
728 * \brief Initializes self with data retrieved via getData()
730 //================================================================================
732 void DirectedBoundingBox::setData(const double* data)
734 _dim = unsigned( *data++ );
737 _axes.assign( data, data+_dim*_dim ); data += _dim*_dim;
738 _minmax.assign( data, data+2*_dim );
747 //================================================================================
749 * \brief Return size of internal data returned by getData() depending on space dim
751 //================================================================================
753 int DirectedBoundingBox::dataSize(int dim)
755 return 1 + dim*dim + 2*dim; // : _dim + _axes + _minmax