UtilVtk3DGeometriSelection.cxx

Go to the documentation of this file.

/*=========================================================================

  Program:   wxMaracas
  Module:    $RCSfile: UtilVtk3DGeometriSelection.cxx,v $
  Language:  C++
  Date:      $Date: 2010/03/01 11:11:01 $
  Version:   $Revision: 1.4 $

  Copyright: (c) 2002, 2003
  License:
  
     This software is distributed WITHOUT ANY WARRANTY; without even 
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/


#include "UtilVtk3DGeometriSelection.h"

#include <vtkPolyData.h>
#include <vtkCell.h>
#include <vtkCellLocator.h>
#include <vtkPointData.h>

#include <vector>
#include "matrix.h"


//----------------------------------------------------------------------------
//----------------------------------------------------------------------------
//----------------------------------------------------------------------------
UtilVtk3DGeometriSelection::UtilVtk3DGeometriSelection()
{
        _width  =       0;
        _height =       0;
        _depth  =       0;
        _mCubes =       NULL;
}
//----------------------------------------------------------------------------
UtilVtk3DGeometriSelection::~UtilVtk3DGeometriSelection()
{
}
//----------------------------------------------------------------------------
void UtilVtk3DGeometriSelection::SetDimentions(int w,int h,int d)
{
        _width  =       w;
        _height =       h;
        _depth  =       d;
}
//----------------------------------------------------------------------------
void UtilVtk3DGeometriSelection::SetMarchingCube(vtkMarchingCubes *mCubes)
{
        _mCubes = mCubes;
}
//----------------------------------------------------------------------------
bool UtilVtk3DGeometriSelection::FindCubePointsFromPoints( double* pO, double* pF, double* pickPoint, double* cameraPos )
{
    gtm::TVector< double > p1( 3 ), p2( 3 ), p3( 3 ), p4( 3 ), p5( 3 ), p6( 3 );
    gtm::TVector< double > c( 3 );
    gtm::TVector< double >* swp;
    gtm::TVector< double > tpO( pO, 3, false ), tpF( pF, 3, false );
    std::vector< gtm::TVector< double >* > points;
    double x1[ 3 ], x2[ 3 ], x3[ 3 ], d1, d2;
    int i, j;

    // 1st plane intersection
    x1[ 0 ] = 0; x1[ 1 ] = _height; x1[ 2 ] =      0;
    x2[ 0 ] = 0; x2[ 1 ] =       0; x2[ 2 ] =      0;
    x3[ 0 ] = 0; x3[ 1 ] =       0; x3[ 2 ] = _depth;
    if( this->IntersectPlaneWithLine( p1.GetAnsiRef( ), x1, x2, x3, cameraPos, pickPoint ) )
        points.push_back( &p1 );

    // 2nd plane intersection
    x1[ 0 ] =      0; x1[ 1 ] = _height; x1[ 2 ] = _depth;
    x2[ 0 ] =      0; x2[ 1 ] =       0; x2[ 2 ] = _depth;
    x3[ 0 ] = _width; x3[ 1 ] =       0; x3[ 2 ] = _depth;
    if( this->IntersectPlaneWithLine( p2.GetAnsiRef( ), x1, x2, x3, cameraPos, pickPoint ) )
        points.push_back( &p2 );

    // 3rd plane intersection
    x1[ 0 ] =      0; x1[ 1 ] = 0; x1[ 2 ] = _depth;
    x2[ 0 ] =      0; x2[ 1 ] = 0; x2[ 2 ] =      0;
    x3[ 0 ] = _width; x3[ 1 ] = 0; x3[ 2 ] =      0;
    if( this->IntersectPlaneWithLine( p3.GetAnsiRef( ), x1, x2, x3, cameraPos, pickPoint ) )
        points.push_back( &p3 );

    // 4th plane intersection
    x1[ 0 ] = _width; x1[ 1 ] = _height; x1[ 2 ] = _depth;
    x2[ 0 ] = _width; x2[ 1 ] =       0; x2[ 2 ] = _depth;
    x3[ 0 ] = _width; x3[ 1 ] =       0; x3[ 2 ] =      0;
    if( this->IntersectPlaneWithLine( p4.GetAnsiRef( ), x1, x2, x3, cameraPos, pickPoint ) )
        points.push_back( &p4 );

    // 5th plane intersection
    x1[ 0 ] = _width; x1[ 1 ] =       0; x1[ 2 ] = 0;
    x2[ 0 ] =      0; x2[ 1 ] =       0; x2[ 2 ] = 0;
    x3[ 0 ] =      0; x3[ 1 ] = _height; x3[ 2 ] = 0;
    if( this->IntersectPlaneWithLine( p5.GetAnsiRef( ), x1, x2, x3, cameraPos, pickPoint ) )
        points.push_back( &p5 );

    // 6th plane intersection
    x1[ 0 ] =      0; x1[ 1 ] = _height; x1[ 2 ] =      0;
    x2[ 0 ] =      0; x2[ 1 ] = _height; x2[ 2 ] = _depth;
    x3[ 0 ] = _width; x3[ 1 ] = _height; x3[ 2 ] = _depth;
    if( this->IntersectPlaneWithLine( p6.GetAnsiRef( ), x1, x2, x3, cameraPos, pickPoint ) )
        points.push_back( &p6 );

    if( points.size( ) >= 2 ) { // Did I find at least 2 points?

        c( 0 ) = ( double )_width  / 2.0;
        c( 1 ) = ( double )_height / 2.0;
        c( 2 ) = ( double )_depth  / 2.0;

        // Sort with bubble sort. Only 30 iterations!
        for( i = 0; i <(int) (points.size( )); i++ ) {
            for( j = 0; j < (int)(points.size( )) - 1; j++ ) {

                d1 = ( c - *points[ j ] ).GetNorm( );
                d2 = ( c - *points[ j + 1 ] ).GetNorm( );
                if( d2 < d1 ) {

                    swp = points[ j ];
                    points[ j ] = points[ j + 1 ];
                    points[ j + 1 ] = swp;

                } // fi

    } // rof

        } // rof

        // Order the two points according to distance to camera.
        c = cameraPos;
        d1 = ( c - *points[ 0 ] ).GetNorm( );
        d2 = ( c - *points[ 1 ] ).GetNorm( );
        tpO = ( d1 < d2 )? *points[ 0 ]: *points[ 1 ];
        tpF = ( d1 > d2 )? *points[ 0 ]: *points[ 1 ];
        return( true );

    } else return( false );

}
//----------------------------------------------------------------------------
bool UtilVtk3DGeometriSelection::GetPointAndNormalIntersection( double* p, double* n, double* pO, double* pF )
{
    gtm::TVector< double > n1( 3 ), n2( 3 ), n3( 3 );

        //EED 01/03/2010
        //int     cellId,
        vtkIdType cellId;

        int subId,  returnVal;

    double t, pcoords[ 3 ], x[ 3 ];
    double fpO[ 3 ], fpF[ 3 ];
    double p1[ 3 ], p2[ 3 ], p3[ 3 ];
    vtkPolyData* data = _mCubes->GetOutput( );
    vtkCellLocator* locator = vtkCellLocator::New( );

    locator->SetDataSet( data );
    locator->Initialize( );
    locator->Update( );

    fpO[ 0 ] = pO[ 0 ]; fpO[ 1 ] = pO[ 1 ]; fpO[ 2 ] = pO[ 2 ];
    fpF[ 0 ] = pF[ 0 ]; fpF[ 1 ] = pF[ 1 ]; fpF[ 2 ] = pF[ 2 ];
    returnVal = locator->IntersectWithLine( fpO, fpF, 0.1, t, x, pcoords, subId, cellId );
    locator->Delete( );

    if( returnVal )
    {
        vtkCell* cell = data->GetCell( cellId );
        vtkPoints* points = cell->GetPoints( );

        data->GetPointData( )->GetNormals( )->GetTuple( cell->GetPointIds( )->GetId( 0 ), n1.GetAnsiRef( ) );
        data->GetPointData( )->GetNormals( )->GetTuple( cell->GetPointIds( )->GetId( 1 ), n2.GetAnsiRef( ) );
        data->GetPointData( )->GetNormals( )->GetTuple( cell->GetPointIds( )->GetId( 2 ), n3.GetAnsiRef( ) );
   
        n1 += n2 + n3;
        n1 *= ( 1.0 / 3.0 );
        n1.Normalize( );
        n[ 0 ] = n1( 0 ); n[ 1 ] = n1( 1 ); n[ 2 ] = n1( 2 );

        points->GetPoint( 0, p1 );
        points->GetPoint( 1, p2 );
        points->GetPoint( 2, p3 );
        this->IntersectPlaneWithLine( p, p1, p2, p3, pO, pF );
        return( true );
    } else return( false );

}

//----------------------------------------------------------------------------
bool UtilVtk3DGeometriSelection::IntersectPlaneWithLine( double* p, double* x1, double* x2, double* x3, double* x4, double* x5 )
{
    gtm::TVector< double > vx1( 3 ), vx2( 3 ), vx3( 3 );
    gtm::TVector< double > vx4( 3 ), vx5( 3 ), vx6( p, 3, false );
    gtm::TMatrix< double > mU( 4, 4 ), mD( 4, 4 );
    double t;
    bool ret;

    vx1 = x1;
    vx2 = x2;
    vx3 = x3;
    vx4 = x4;
    vx5 = x5;
    vx6 = vx5 - vx4;

    mD( 0, 0 ) = mU( 0, 0 ) = 1;
    mD( 0, 1 ) = mU( 0, 1 ) = vx1( 0 );
    mD( 0, 2 ) = mU( 0, 2 ) = vx1( 1 );
    mD( 0, 3 ) = mU( 0, 3 ) = vx1( 2 );
    mD( 1, 0 ) = mU( 1, 0 ) = 1;
    mD( 1, 1 ) = mU( 1, 1 ) = vx2( 0 );
    mD( 1, 2 ) = mU( 1, 2 ) = vx2( 1 );
    mD( 1, 3 ) = mU( 1, 3 ) = vx2( 2 );
    mD( 2, 0 ) = mU( 2, 0 ) = 1;
    mD( 2, 1 ) = mU( 2, 1 ) = vx3( 0 );
    mD( 2, 2 ) = mU( 2, 2 ) = vx3( 1 );
    mD( 2, 3 ) = mU( 2, 3 ) = vx3( 2 );
    mU( 3, 0 ) = 1;
    mU( 3, 1 ) = vx4( 0 );
    mU( 3, 2 ) = vx4( 1 );
    mU( 3, 3 ) = vx4( 2 );
    mD( 3, 0 ) = 0;
    mD( 3, 1 ) = vx6( 0 );
    mD( 3, 2 ) = vx6( 1 );
    mD( 3, 3 ) = vx6( 2 );

    ret = ( mD.Det( ) != 0 );
    if( ret ) {

        t = mU.Det( ) / mD.Det( );
        vx6 = ( ( vx4 - vx5 ) * t ) + vx4;

    } // fi
    return( ret );

}



//----------------------------------------------------------------------------
// pResult  Result
// n        Normal to plane
// p        Point of the plane
// pA       point1 of the line
// pB       point2 of the line
void UtilVtk3DGeometriSelection::IntersectionPlaneAndLine(double *pResult,double *n,double *p,double *pA,double *pB)
{
        double u,A,B,D;
        D = - (n[0]*p[0] + n[1]*p[1] + n[2]*p[2]) ;
        A= n[0]*pA[0] +  n[1]*pA[1] + n[2]*pA[2] + D;
        B= n[0]*(pA[0]-pB[0]) +  n[1]*(pA[1]-pB[1]) + n[2]*(pA[2]-pB[2]);
        if (B!=0)
        {
                u = A / B ;
        } else {
                u=-1;
        }
        pResult[0] = pA[0] + u*(pB[0] - pA[0]);
        pResult[1] = pA[1] + u*(pB[1] - pA[1]);
        pResult[2] = pA[2] + u*(pB[2] - pA[2]);
}

//----------------------------------------------------------------------------
// p   point
// pA  point1 of the line
// pB  point2 of the line
double UtilVtk3DGeometriSelection::DistanceMinPointToLine(double *p,double *pA, double *pB)
{
        double pResult[3];
        double normalToPlane[3];
        normalToPlane[0]=pA[0]-pB[0];
        normalToPlane[1]=pA[1]-pB[1];
        normalToPlane[2]=pA[2]-pB[2];
    IntersectionPlaneAndLine(pResult,normalToPlane,p,pA,pB);

        double dx = p[0]-pResult[0];
        double dy = p[1]-pResult[1];
        double dz = p[2]-pResult[2];

    double result=sqrt( dx*dx + dy*dy + dz*dz );

        return result;
}