00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include "UtilVtk3DGeometriSelection.h"
00020
00021 #include <vtkPolyData.h>
00022 #include <vtkCell.h>
00023 #include <vtkCellLocator.h>
00024 #include <vtkPointData.h>
00025
00026 #include <vector>
00027 #include "matrix.h"
00028
00029
00030
00031
00032
00033 UtilVtk3DGeometriSelection::UtilVtk3DGeometriSelection()
00034 {
00035 _width = 0;
00036 _height = 0;
00037 _depth = 0;
00038 _mCubes = NULL;
00039 }
00040
00041 UtilVtk3DGeometriSelection::~UtilVtk3DGeometriSelection()
00042 {
00043 }
00044
00045 void UtilVtk3DGeometriSelection::SetDimentions(int w,int h,int d)
00046 {
00047 _width = w;
00048 _height = h;
00049 _depth = d;
00050 }
00051
00052 void UtilVtk3DGeometriSelection::SetMarchingCube(vtkMarchingCubes *mCubes)
00053 {
00054 _mCubes = mCubes;
00055 }
00056
00057 bool UtilVtk3DGeometriSelection::FindCubePointsFromPoints( double* pO, double* pF, double* pickPoint, double* cameraPos )
00058 {
00059 gtm::TVector< double > p1( 3 ), p2( 3 ), p3( 3 ), p4( 3 ), p5( 3 ), p6( 3 );
00060 gtm::TVector< double > c( 3 );
00061 gtm::TVector< double >* swp;
00062 gtm::TVector< double > tpO( pO, 3, false ), tpF( pF, 3, false );
00063 std::vector< gtm::TVector< double >* > points;
00064 double x1[ 3 ], x2[ 3 ], x3[ 3 ], d1, d2;
00065 int i, j;
00066
00067
00068 x1[ 0 ] = 0; x1[ 1 ] = _height; x1[ 2 ] = 0;
00069 x2[ 0 ] = 0; x2[ 1 ] = 0; x2[ 2 ] = 0;
00070 x3[ 0 ] = 0; x3[ 1 ] = 0; x3[ 2 ] = _depth;
00071 if( this->IntersectPlaneWithLine( p1.GetAnsiRef( ), x1, x2, x3, cameraPos, pickPoint ) )
00072 points.push_back( &p1 );
00073
00074
00075 x1[ 0 ] = 0; x1[ 1 ] = _height; x1[ 2 ] = _depth;
00076 x2[ 0 ] = 0; x2[ 1 ] = 0; x2[ 2 ] = _depth;
00077 x3[ 0 ] = _width; x3[ 1 ] = 0; x3[ 2 ] = _depth;
00078 if( this->IntersectPlaneWithLine( p2.GetAnsiRef( ), x1, x2, x3, cameraPos, pickPoint ) )
00079 points.push_back( &p2 );
00080
00081
00082 x1[ 0 ] = 0; x1[ 1 ] = 0; x1[ 2 ] = _depth;
00083 x2[ 0 ] = 0; x2[ 1 ] = 0; x2[ 2 ] = 0;
00084 x3[ 0 ] = _width; x3[ 1 ] = 0; x3[ 2 ] = 0;
00085 if( this->IntersectPlaneWithLine( p3.GetAnsiRef( ), x1, x2, x3, cameraPos, pickPoint ) )
00086 points.push_back( &p3 );
00087
00088
00089 x1[ 0 ] = _width; x1[ 1 ] = _height; x1[ 2 ] = _depth;
00090 x2[ 0 ] = _width; x2[ 1 ] = 0; x2[ 2 ] = _depth;
00091 x3[ 0 ] = _width; x3[ 1 ] = 0; x3[ 2 ] = 0;
00092 if( this->IntersectPlaneWithLine( p4.GetAnsiRef( ), x1, x2, x3, cameraPos, pickPoint ) )
00093 points.push_back( &p4 );
00094
00095
00096 x1[ 0 ] = _width; x1[ 1 ] = 0; x1[ 2 ] = 0;
00097 x2[ 0 ] = 0; x2[ 1 ] = 0; x2[ 2 ] = 0;
00098 x3[ 0 ] = 0; x3[ 1 ] = _height; x3[ 2 ] = 0;
00099 if( this->IntersectPlaneWithLine( p5.GetAnsiRef( ), x1, x2, x3, cameraPos, pickPoint ) )
00100 points.push_back( &p5 );
00101
00102
00103 x1[ 0 ] = 0; x1[ 1 ] = _height; x1[ 2 ] = 0;
00104 x2[ 0 ] = 0; x2[ 1 ] = _height; x2[ 2 ] = _depth;
00105 x3[ 0 ] = _width; x3[ 1 ] = _height; x3[ 2 ] = _depth;
00106 if( this->IntersectPlaneWithLine( p6.GetAnsiRef( ), x1, x2, x3, cameraPos, pickPoint ) )
00107 points.push_back( &p6 );
00108
00109 if( points.size( ) >= 2 ) {
00110
00111 c( 0 ) = ( double )_width / 2.0;
00112 c( 1 ) = ( double )_height / 2.0;
00113 c( 2 ) = ( double )_depth / 2.0;
00114
00115
00116 for( i = 0; i <(int) (points.size( )); i++ ) {
00117 for( j = 0; j < (int)(points.size( )) - 1; j++ ) {
00118
00119 d1 = ( c - *points[ j ] ).GetNorm( );
00120 d2 = ( c - *points[ j + 1 ] ).GetNorm( );
00121 if( d2 < d1 ) {
00122
00123 swp = points[ j ];
00124 points[ j ] = points[ j + 1 ];
00125 points[ j + 1 ] = swp;
00126
00127 }
00128
00129 }
00130
00131 }
00132
00133
00134 c = cameraPos;
00135 d1 = ( c - *points[ 0 ] ).GetNorm( );
00136 d2 = ( c - *points[ 1 ] ).GetNorm( );
00137 tpO = ( d1 < d2 )? *points[ 0 ]: *points[ 1 ];
00138 tpF = ( d1 > d2 )? *points[ 0 ]: *points[ 1 ];
00139 return( true );
00140
00141 } else return( false );
00142
00143 }
00144
00145 bool UtilVtk3DGeometriSelection::GetPointAndNormalIntersection( double* p, double* n, double* pO, double* pF )
00146 {
00147 gtm::TVector< double > n1( 3 ), n2( 3 ), n3( 3 );
00148 int subId, cellId, returnVal;
00149 double t, pcoords[ 3 ], x[ 3 ];
00150 double fpO[ 3 ], fpF[ 3 ];
00151 double p1[ 3 ], p2[ 3 ], p3[ 3 ];
00152 vtkPolyData* data = _mCubes->GetOutput( );
00153 vtkCellLocator* locator = vtkCellLocator::New( );
00154
00155 locator->SetDataSet( data );
00156 locator->Initialize( );
00157 locator->Update( );
00158
00159 fpO[ 0 ] = pO[ 0 ]; fpO[ 1 ] = pO[ 1 ]; fpO[ 2 ] = pO[ 2 ];
00160 fpF[ 0 ] = pF[ 0 ]; fpF[ 1 ] = pF[ 1 ]; fpF[ 2 ] = pF[ 2 ];
00161 returnVal = locator->IntersectWithLine( fpO, fpF, 0.1, t, x, pcoords, subId, cellId );
00162 locator->Delete( );
00163
00164 if( returnVal )
00165 {
00166 vtkCell* cell = data->GetCell( cellId );
00167 vtkPoints* points = cell->GetPoints( );
00168
00169 data->GetPointData( )->GetNormals( )->GetTuple( cell->GetPointIds( )->GetId( 0 ), n1.GetAnsiRef( ) );
00170 data->GetPointData( )->GetNormals( )->GetTuple( cell->GetPointIds( )->GetId( 1 ), n2.GetAnsiRef( ) );
00171 data->GetPointData( )->GetNormals( )->GetTuple( cell->GetPointIds( )->GetId( 2 ), n3.GetAnsiRef( ) );
00172
00173 n1 += n2 + n3;
00174 n1 *= ( 1.0 / 3.0 );
00175 n1.Normalize( );
00176 n[ 0 ] = n1( 0 ); n[ 1 ] = n1( 1 ); n[ 2 ] = n1( 2 );
00177
00178 points->GetPoint( 0, p1 );
00179 points->GetPoint( 1, p2 );
00180 points->GetPoint( 2, p3 );
00181 this->IntersectPlaneWithLine( p, p1, p2, p3, pO, pF );
00182 return( true );
00183 } else return( false );
00184
00185 }
00186
00187
00188 bool UtilVtk3DGeometriSelection::IntersectPlaneWithLine( double* p, double* x1, double* x2, double* x3, double* x4, double* x5 )
00189 {
00190 gtm::TVector< double > vx1( 3 ), vx2( 3 ), vx3( 3 );
00191 gtm::TVector< double > vx4( 3 ), vx5( 3 ), vx6( p, 3, false );
00192 gtm::TMatrix< double > mU( 4, 4 ), mD( 4, 4 );
00193 double t;
00194 bool ret;
00195
00196 vx1 = x1;
00197 vx2 = x2;
00198 vx3 = x3;
00199 vx4 = x4;
00200 vx5 = x5;
00201 vx6 = vx5 - vx4;
00202
00203 mD( 0, 0 ) = mU( 0, 0 ) = 1;
00204 mD( 0, 1 ) = mU( 0, 1 ) = vx1( 0 );
00205 mD( 0, 2 ) = mU( 0, 2 ) = vx1( 1 );
00206 mD( 0, 3 ) = mU( 0, 3 ) = vx1( 2 );
00207 mD( 1, 0 ) = mU( 1, 0 ) = 1;
00208 mD( 1, 1 ) = mU( 1, 1 ) = vx2( 0 );
00209 mD( 1, 2 ) = mU( 1, 2 ) = vx2( 1 );
00210 mD( 1, 3 ) = mU( 1, 3 ) = vx2( 2 );
00211 mD( 2, 0 ) = mU( 2, 0 ) = 1;
00212 mD( 2, 1 ) = mU( 2, 1 ) = vx3( 0 );
00213 mD( 2, 2 ) = mU( 2, 2 ) = vx3( 1 );
00214 mD( 2, 3 ) = mU( 2, 3 ) = vx3( 2 );
00215 mU( 3, 0 ) = 1;
00216 mU( 3, 1 ) = vx4( 0 );
00217 mU( 3, 2 ) = vx4( 1 );
00218 mU( 3, 3 ) = vx4( 2 );
00219 mD( 3, 0 ) = 0;
00220 mD( 3, 1 ) = vx6( 0 );
00221 mD( 3, 2 ) = vx6( 1 );
00222 mD( 3, 3 ) = vx6( 2 );
00223
00224 ret = ( mD.Det( ) != 0 );
00225 if( ret ) {
00226
00227 t = mU.Det( ) / mD.Det( );
00228 vx6 = ( ( vx4 - vx5 ) * t ) + vx4;
00229
00230 }
00231 return( ret );
00232
00233 }
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243 void UtilVtk3DGeometriSelection::IntersectionPlaneAndLine(double *pResult,double *n,double *p,double *pA,double *pB)
00244 {
00245 double u,A,B,D;
00246 D = - (n[0]*p[0] + n[1]*p[1] + n[2]*p[2]) ;
00247 A= n[0]*pA[0] + n[1]*pA[1] + n[2]*pA[2] + D;
00248 B= n[0]*(pA[0]-pB[0]) + n[1]*(pA[1]-pB[1]) + n[2]*(pA[2]-pB[2]);
00249 if (B!=0)
00250 {
00251 u = A / B ;
00252 } else {
00253 u=-1;
00254 }
00255 pResult[0] = pA[0] + u*(pB[0] - pA[0]);
00256 pResult[1] = pA[1] + u*(pB[1] - pA[1]);
00257 pResult[2] = pA[2] + u*(pB[2] - pA[2]);
00258 }
00259
00260
00261
00262
00263
00264 double UtilVtk3DGeometriSelection::DistanceMinPointToLine(double *p,double *pA, double *pB)
00265 {
00266 double pResult[3];
00267 double normalToPlane[3];
00268 normalToPlane[0]=pA[0]-pB[0];
00269 normalToPlane[1]=pA[1]-pB[1];
00270 normalToPlane[2]=pA[2]-pB[2];
00271 IntersectionPlaneAndLine(pResult,normalToPlane,p,pA,pB);
00272
00273 double dx = p[0]-pResult[0];
00274 double dy = p[1]-pResult[1];
00275 double dz = p[2]-pResult[2];
00276
00277 double result=sqrt( dx*dx + dy*dy + dz*dz );
00278
00279 return result;
00280 }
00281
00282
00283
00284