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: 2009/05/14 13:54:34 $
   7   Version:   $Revision: 1.2 $
   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 < points.size( ); i++ ) {
 117             for( j = 0; j < 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     int subId, cellId, returnVal;
 149     double t, pcoords[ 3 ], x[ 3 ];
 150     double fpO[ 3 ], fpF[ 3 ];
 151     double p1[ 3 ], p2[ 3 ], p3[ 3 ];
 152     vtkPolyData* data = _mCubes->GetOutput( );
 153     vtkCellLocator* locator = vtkCellLocator::New( );
 154
 155     locator->SetDataSet( data );
 156     locator->Initialize( );
 157     locator->Update( );
 158
 159     fpO[ 0 ] = pO[ 0 ]; fpO[ 1 ] = pO[ 1 ]; fpO[ 2 ] = pO[ 2 ];
 160     fpF[ 0 ] = pF[ 0 ]; fpF[ 1 ] = pF[ 1 ]; fpF[ 2 ] = pF[ 2 ];
 161     returnVal = locator->IntersectWithLine( fpO, fpF, 0.1, t, x, pcoords, subId, cellId );
 162     locator->Delete( );
 163
 164     if( returnVal )
 165     {
 166         vtkCell* cell = data->GetCell( cellId );
 167         vtkPoints* points = cell->GetPoints( );
 168
 169         data->GetPointData( )->GetNormals( )->GetTuple( cell->GetPointIds( )->GetId( 0 ), n1.GetAnsiRef( ) );
 170         data->GetPointData( )->GetNormals( )->GetTuple( cell->GetPointIds( )->GetId( 1 ), n2.GetAnsiRef( ) );
 171         data->GetPointData( )->GetNormals( )->GetTuple( cell->GetPointIds( )->GetId( 2 ), n3.GetAnsiRef( ) );
 172
 173         n1 += n2 + n3;
 174         n1 *= ( 1.0 / 3.0 );
 175         n1.Normalize( );
 176         n[ 0 ] = n1( 0 ); n[ 1 ] = n1( 1 ); n[ 2 ] = n1( 2 );
 177
 178         points->GetPoint( 0, p1 );
 179         points->GetPoint( 1, p2 );
 180         points->GetPoint( 2, p3 );
 181         this->IntersectPlaneWithLine( p, p1, p2, p3, pO, pF );
 182         return( true );
 183     } else return( false );
 184
 185 }
 186
 187 //----------------------------------------------------------------------------
 188 bool UtilVtk3DGeometriSelection::IntersectPlaneWithLine( double* p, double* x1, double* x2, double* x3, double* x4, double* x5 )
 189 {
 190     gtm::TVector< double > vx1( 3 ), vx2( 3 ), vx3( 3 );
 191     gtm::TVector< double > vx4( 3 ), vx5( 3 ), vx6( p, 3, false );
 192     gtm::TMatrix< double > mU( 4, 4 ), mD( 4, 4 );
 193     double t;
 194     bool ret;
 195
 196     vx1 = x1;
 197     vx2 = x2;
 198     vx3 = x3;
 199     vx4 = x4;
 200     vx5 = x5;
 201     vx6 = vx5 - vx4;
 202
 203     mD( 0, 0 ) = mU( 0, 0 ) = 1;
 204     mD( 0, 1 ) = mU( 0, 1 ) = vx1( 0 );
 205     mD( 0, 2 ) = mU( 0, 2 ) = vx1( 1 );
 206     mD( 0, 3 ) = mU( 0, 3 ) = vx1( 2 );
 207     mD( 1, 0 ) = mU( 1, 0 ) = 1;
 208     mD( 1, 1 ) = mU( 1, 1 ) = vx2( 0 );
 209     mD( 1, 2 ) = mU( 1, 2 ) = vx2( 1 );
 210     mD( 1, 3 ) = mU( 1, 3 ) = vx2( 2 );
 211     mD( 2, 0 ) = mU( 2, 0 ) = 1;
 212     mD( 2, 1 ) = mU( 2, 1 ) = vx3( 0 );
 213     mD( 2, 2 ) = mU( 2, 2 ) = vx3( 1 );
 214     mD( 2, 3 ) = mU( 2, 3 ) = vx3( 2 );
 215     mU( 3, 0 ) = 1;
 216     mU( 3, 1 ) = vx4( 0 );
 217     mU( 3, 2 ) = vx4( 1 );
 218     mU( 3, 3 ) = vx4( 2 );
 219     mD( 3, 0 ) = 0;
 220     mD( 3, 1 ) = vx6( 0 );
 221     mD( 3, 2 ) = vx6( 1 );
 222     mD( 3, 3 ) = vx6( 2 );
 223
 224     ret = ( mD.Det( ) != 0 );
 225     if( ret ) {
 226
 227         t = mU.Det( ) / mD.Det( );
 228         vx6 = ( ( vx4 - vx5 ) * t ) + vx4;
 229
 230     } // fi
 231     return( ret );
 232
 233 }
 234
 235
 236
 237 //----------------------------------------------------------------------------
 238 // pResult  Result
 239 // n        Normal to plane
 240 // p        Point of the plane
 241 // pA       point1 of the line
 242 // pB       point2 of the line
 243 void UtilVtk3DGeometriSelection::IntersectionPlaneAndLine(double *pResult,double *n,double *p,double *pA,double *pB)
 244 {
 245         double u,A,B,D;
 246         D = - (n[0]*p[0] + n[1]*p[1] + n[2]*p[2]) ;
 247         A= n[0]*pA[0] +  n[1]*pA[1] + n[2]*pA[2] + D;
 248         B= n[0]*(pA[0]-pB[0]) +  n[1]*(pA[1]-pB[1]) + n[2]*(pA[2]-pB[2]);
 249         if (B!=0)
 250         {
 251                 u = A / B ;
 252         } else {
 253                 u=-1;
 254         }
 255         pResult[0] = pA[0] + u*(pB[0] - pA[0]);
 256         pResult[1] = pA[1] + u*(pB[1] - pA[1]);
 257         pResult[2] = pA[2] + u*(pB[2] - pA[2]);
 258 }
 259
 260 //----------------------------------------------------------------------------
 261 // p   point
 262 // pA  point1 of the line
 263 // pB  point2 of the line
 264 double UtilVtk3DGeometriSelection::DistanceMinPointToLine(double *p,double *pA, double *pB)
 265 {
 266         double pResult[3];
 267         double normalToPlane[3];
 268         normalToPlane[0]=pA[0]-pB[0];
 269         normalToPlane[1]=pA[1]-pB[1];
 270         normalToPlane[2]=pA[2]-pB[2];
 271     IntersectionPlaneAndLine(pResult,normalToPlane,p,pA,pB);
 272
 273         double dx = p[0]-pResult[0];
 274         double dy = p[1]-pResult[1];
 275         double dz = p[2]-pResult[2];
 276
 277     double result=sqrt( dx*dx + dy*dy + dz*dz );
 278
 279         return result;
 280 }
 281
 282
 283
 284