lib/maracasVisuLib/src/interface/wxWindows/widgets/UtilVtk3DGeometriSelection.cxx

   1 /*=========================================================================
   2
   3   Program:   wxMaracas
   4   Module:    $RCSfile: UtilVtk3DGeometriSelection.cxx,v $
   5   Language:  C++
   6   Date:      $Date: 2010/03/01 11:11:01 $
   7   Version:   $Revision: 1.4 $
   8
   9   Copyright: (c) 2002, 2003
  10   License:
  11
  12      This software is distributed WITHOUT ANY WARRANTY; without even
  13      the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  14      PURPOSE.  See the above copyright notice for more information.
  15
  16 =========================================================================*/
  17
  18
  19 #include "UtilVtk3DGeometriSelection.h"
  20
  21 #include <vtkPolyData.h>
  22 #include <vtkCell.h>
  23 #include <vtkCellLocator.h>
  24 #include <vtkPointData.h>
  25
  26 #include <vector>
  27 #include "matrix.h"
  28
  29
  30 //----------------------------------------------------------------------------
  31 //----------------------------------------------------------------------------
  32 //----------------------------------------------------------------------------
  33 UtilVtk3DGeometriSelection::UtilVtk3DGeometriSelection()
  34 {
  35         _width  =       0;
  36         _height =       0;
  37         _depth  =       0;
  38         _mCubes =       NULL;
  39 }
  40 //----------------------------------------------------------------------------
  41 UtilVtk3DGeometriSelection::~UtilVtk3DGeometriSelection()
  42 {
  43 }
  44 //----------------------------------------------------------------------------
  45 void UtilVtk3DGeometriSelection::SetDimentions(int w,int h,int d)
  46 {
  47         _width  =       w;
  48         _height =       h;
  49         _depth  =       d;
  50 }
  51 //----------------------------------------------------------------------------
  52 void UtilVtk3DGeometriSelection::SetMarchingCube(vtkMarchingCubes *mCubes)
  53 {
  54         _mCubes = mCubes;
  55 }
  56 //----------------------------------------------------------------------------
  57 bool UtilVtk3DGeometriSelection::FindCubePointsFromPoints( double* pO, double* pF, double* pickPoint, double* cameraPos )
  58 {
  59     gtm::TVector< double > p1( 3 ), p2( 3 ), p3( 3 ), p4( 3 ), p5( 3 ), p6( 3 );
  60     gtm::TVector< double > c( 3 );
  61     gtm::TVector< double >* swp;
  62     gtm::TVector< double > tpO( pO, 3, false ), tpF( pF, 3, false );
  63     std::vector< gtm::TVector< double >* > points;
  64     double x1[ 3 ], x2[ 3 ], x3[ 3 ], d1, d2;
  65     int i, j;
  66
  67     // 1st plane intersection
  68     x1[ 0 ] = 0; x1[ 1 ] = _height; x1[ 2 ] =      0;
  69     x2[ 0 ] = 0; x2[ 1 ] =       0; x2[ 2 ] =      0;
  70     x3[ 0 ] = 0; x3[ 1 ] =       0; x3[ 2 ] = _depth;
  71     if( this->IntersectPlaneWithLine( p1.GetAnsiRef( ), x1, x2, x3, cameraPos, pickPoint ) )
  72         points.push_back( &p1 );
  73
  74     // 2nd plane intersection
  75     x1[ 0 ] =      0; x1[ 1 ] = _height; x1[ 2 ] = _depth;
  76     x2[ 0 ] =      0; x2[ 1 ] =       0; x2[ 2 ] = _depth;
  77     x3[ 0 ] = _width; x3[ 1 ] =       0; x3[ 2 ] = _depth;
  78     if( this->IntersectPlaneWithLine( p2.GetAnsiRef( ), x1, x2, x3, cameraPos, pickPoint ) )
  79         points.push_back( &p2 );
  80
  81     // 3rd plane intersection
  82     x1[ 0 ] =      0; x1[ 1 ] = 0; x1[ 2 ] = _depth;
  83     x2[ 0 ] =      0; x2[ 1 ] = 0; x2[ 2 ] =      0;
  84     x3[ 0 ] = _width; x3[ 1 ] = 0; x3[ 2 ] =      0;
  85     if( this->IntersectPlaneWithLine( p3.GetAnsiRef( ), x1, x2, x3, cameraPos, pickPoint ) )
  86         points.push_back( &p3 );
  87
  88     // 4th plane intersection
  89     x1[ 0 ] = _width; x1[ 1 ] = _height; x1[ 2 ] = _depth;
  90     x2[ 0 ] = _width; x2[ 1 ] =       0; x2[ 2 ] = _depth;
  91     x3[ 0 ] = _width; x3[ 1 ] =       0; x3[ 2 ] =      0;
  92     if( this->IntersectPlaneWithLine( p4.GetAnsiRef( ), x1, x2, x3, cameraPos, pickPoint ) )
  93         points.push_back( &p4 );
  94
  95     // 5th plane intersection
  96     x1[ 0 ] = _width; x1[ 1 ] =       0; x1[ 2 ] = 0;
  97     x2[ 0 ] =      0; x2[ 1 ] =       0; x2[ 2 ] = 0;
  98     x3[ 0 ] =      0; x3[ 1 ] = _height; x3[ 2 ] = 0;
  99     if( this->IntersectPlaneWithLine( p5.GetAnsiRef( ), x1, x2, x3, cameraPos, pickPoint ) )
 100         points.push_back( &p5 );
 101
 102     // 6th plane intersection
 103     x1[ 0 ] =      0; x1[ 1 ] = _height; x1[ 2 ] =      0;
 104     x2[ 0 ] =      0; x2[ 1 ] = _height; x2[ 2 ] = _depth;
 105     x3[ 0 ] = _width; x3[ 1 ] = _height; x3[ 2 ] = _depth;
 106     if( this->IntersectPlaneWithLine( p6.GetAnsiRef( ), x1, x2, x3, cameraPos, pickPoint ) )
 107         points.push_back( &p6 );
 108
 109     if( points.size( ) >= 2 ) { // Did I find at least 2 points?
 110
 111         c( 0 ) = ( double )_width  / 2.0;
 112         c( 1 ) = ( double )_height / 2.0;
 113         c( 2 ) = ( double )_depth  / 2.0;
 114
 115         // Sort with bubble sort. Only 30 iterations!
 116         for( i = 0; i <(int) (points.size( )); i++ ) {
 117             for( j = 0; j < (int)(points.size( )) - 1; j++ ) {
 118
 119                 d1 = ( c - *points[ j ] ).GetNorm( );
 120                 d2 = ( c - *points[ j + 1 ] ).GetNorm( );
 121                 if( d2 < d1 ) {
 122
 123                     swp = points[ j ];
 124                     points[ j ] = points[ j + 1 ];
 125                     points[ j + 1 ] = swp;
 126
 127                 } // fi
 128
 129     } // rof
 130
 131         } // rof
 132
 133         // Order the two points according to distance to camera.
 134         c = cameraPos;
 135         d1 = ( c - *points[ 0 ] ).GetNorm( );
 136         d2 = ( c - *points[ 1 ] ).GetNorm( );
 137         tpO = ( d1 < d2 )? *points[ 0 ]: *points[ 1 ];
 138         tpF = ( d1 > d2 )? *points[ 0 ]: *points[ 1 ];
 139         return( true );
 140
 141     } else return( false );
 142
 143 }
 144 //----------------------------------------------------------------------------
 145 bool UtilVtk3DGeometriSelection::GetPointAndNormalIntersection( double* p, double* n, double* pO, double* pF )
 146 {
 147     gtm::TVector< double > n1( 3 ), n2( 3 ), n3( 3 );
 148
 149         //EED 01/03/2010
 150         //int     cellId,
 151         vtkIdType cellId;
 152
 153         int subId,  returnVal;
 154
 155     double t, pcoords[ 3 ], x[ 3 ];
 156     double fpO[ 3 ], fpF[ 3 ];
 157     double p1[ 3 ], p2[ 3 ], p3[ 3 ];
 158     vtkPolyData* data = _mCubes->GetOutput( );
 159     vtkCellLocator* locator = vtkCellLocator::New( );
 160
 161     locator->SetDataSet( data );
 162     locator->Initialize( );
 163     locator->Update( );
 164
 165     fpO[ 0 ] = pO[ 0 ]; fpO[ 1 ] = pO[ 1 ]; fpO[ 2 ] = pO[ 2 ];
 166     fpF[ 0 ] = pF[ 0 ]; fpF[ 1 ] = pF[ 1 ]; fpF[ 2 ] = pF[ 2 ];
 167     returnVal = locator->IntersectWithLine( fpO, fpF, 0.1, t, x, pcoords, subId, cellId );
 168     locator->Delete( );
 169
 170     if( returnVal )
 171     {
 172         vtkCell* cell = data->GetCell( cellId );
 173         vtkPoints* points = cell->GetPoints( );
 174
 175         data->GetPointData( )->GetNormals( )->GetTuple( cell->GetPointIds( )->GetId( 0 ), n1.GetAnsiRef( ) );
 176         data->GetPointData( )->GetNormals( )->GetTuple( cell->GetPointIds( )->GetId( 1 ), n2.GetAnsiRef( ) );
 177         data->GetPointData( )->GetNormals( )->GetTuple( cell->GetPointIds( )->GetId( 2 ), n3.GetAnsiRef( ) );
 178
 179         n1 += n2 + n3;
 180         n1 *= ( 1.0 / 3.0 );
 181         n1.Normalize( );
 182         n[ 0 ] = n1( 0 ); n[ 1 ] = n1( 1 ); n[ 2 ] = n1( 2 );
 183
 184         points->GetPoint( 0, p1 );
 185         points->GetPoint( 1, p2 );
 186         points->GetPoint( 2, p3 );
 187         this->IntersectPlaneWithLine( p, p1, p2, p3, pO, pF );
 188         return( true );
 189     } else return( false );
 190
 191 }
 192
 193 //----------------------------------------------------------------------------
 194 bool UtilVtk3DGeometriSelection::IntersectPlaneWithLine( double* p, double* x1, double* x2, double* x3, double* x4, double* x5 )
 195 {
 196     gtm::TVector< double > vx1( 3 ), vx2( 3 ), vx3( 3 );
 197     gtm::TVector< double > vx4( 3 ), vx5( 3 ), vx6( p, 3, false );
 198     gtm::TMatrix< double > mU( 4, 4 ), mD( 4, 4 );
 199     double t;
 200     bool ret;
 201
 202     vx1 = x1;
 203     vx2 = x2;
 204     vx3 = x3;
 205     vx4 = x4;
 206     vx5 = x5;
 207     vx6 = vx5 - vx4;
 208
 209     mD( 0, 0 ) = mU( 0, 0 ) = 1;
 210     mD( 0, 1 ) = mU( 0, 1 ) = vx1( 0 );
 211     mD( 0, 2 ) = mU( 0, 2 ) = vx1( 1 );
 212     mD( 0, 3 ) = mU( 0, 3 ) = vx1( 2 );
 213     mD( 1, 0 ) = mU( 1, 0 ) = 1;
 214     mD( 1, 1 ) = mU( 1, 1 ) = vx2( 0 );
 215     mD( 1, 2 ) = mU( 1, 2 ) = vx2( 1 );
 216     mD( 1, 3 ) = mU( 1, 3 ) = vx2( 2 );
 217     mD( 2, 0 ) = mU( 2, 0 ) = 1;
 218     mD( 2, 1 ) = mU( 2, 1 ) = vx3( 0 );
 219     mD( 2, 2 ) = mU( 2, 2 ) = vx3( 1 );
 220     mD( 2, 3 ) = mU( 2, 3 ) = vx3( 2 );
 221     mU( 3, 0 ) = 1;
 222     mU( 3, 1 ) = vx4( 0 );
 223     mU( 3, 2 ) = vx4( 1 );
 224     mU( 3, 3 ) = vx4( 2 );
 225     mD( 3, 0 ) = 0;
 226     mD( 3, 1 ) = vx6( 0 );
 227     mD( 3, 2 ) = vx6( 1 );
 228     mD( 3, 3 ) = vx6( 2 );
 229
 230     ret = ( mD.Det( ) != 0 );
 231     if( ret ) {
 232
 233         t = mU.Det( ) / mD.Det( );
 234         vx6 = ( ( vx4 - vx5 ) * t ) + vx4;
 235
 236     } // fi
 237     return( ret );
 238
 239 }
 240
 241
 242
 243 //----------------------------------------------------------------------------
 244 // pResult  Result
 245 // n        Normal to plane
 246 // p        Point of the plane
 247 // pA       point1 of the line
 248 // pB       point2 of the line
 249 void UtilVtk3DGeometriSelection::IntersectionPlaneAndLine(double *pResult,double *n,double *p,double *pA,double *pB)
 250 {
 251         double u,A,B,D;
 252         D = - (n[0]*p[0] + n[1]*p[1] + n[2]*p[2]) ;
 253         A= n[0]*pA[0] +  n[1]*pA[1] + n[2]*pA[2] + D;
 254         B= n[0]*(pA[0]-pB[0]) +  n[1]*(pA[1]-pB[1]) + n[2]*(pA[2]-pB[2]);
 255         if (B!=0)
 256         {
 257                 u = A / B ;
 258         } else {
 259                 u=-1;
 260         }
 261         pResult[0] = pA[0] + u*(pB[0] - pA[0]);
 262         pResult[1] = pA[1] + u*(pB[1] - pA[1]);
 263         pResult[2] = pA[2] + u*(pB[2] - pA[2]);
 264 }
 265
 266 //----------------------------------------------------------------------------
 267 // p   point
 268 // pA  point1 of the line
 269 // pB  point2 of the line
 270 double UtilVtk3DGeometriSelection::DistanceMinPointToLine(double *p,double *pA, double *pB)
 271 {
 272         double pResult[3];
 273         double normalToPlane[3];
 274         normalToPlane[0]=pA[0]-pB[0];
 275         normalToPlane[1]=pA[1]-pB[1];
 276         normalToPlane[2]=pA[2]-pB[2];
 277     IntersectionPlaneAndLine(pResult,normalToPlane,p,pA,pB);
 278
 279         double dx = p[0]-pResult[0];
 280         double dy = p[1]-pResult[1];
 281         double dz = p[2]-pResult[2];
 282
 283     double result=sqrt( dx*dx + dy*dy + dz*dz );
 284
 285         return result;
 286 }
 287
 288
 289
 290