C:/cmcintos/defOrgs/source/geometrical/Geom_MeshSpatialObject.cxx

#ifndef _Geom_MeshSpatialObject_cxx
#define _Geom_MeshSpatialObject_cxx
#include "Geom_MeshSpatialObject.h"

namespace mial
{

        template <class dType, int nDims,class MType, class VType >
        Geom_MeshSpatialObject<dType, nDims,MType,VType>
                ::Geom_MeshSpatialObject(dType p)
        {
                theMeshSpatialObject = MeshSpatialObjectType::New();
                theMesh = theMeshSpatialObject->GetMesh();
                writer = WriterType::New();
                m_IsInsidePrecision = p;
        }

        template <class dType, int nDims,class MType, class VType >
        MType 
                Geom_MeshSpatialObject<dType, nDims,MType,VType>
		::getMatrixNodePositions()
        {
                this->mNodes.set_size(this->numNodes,nDims);
                this->updateMatrixNodePositions();
                return this->mNodes;
        }

        /*template <class dType, int nDims,class MType, class VType >
        vType Geom_MeshSpatialObject<dType, nDims,MType,VType>::getVectorNodePositions()
        {
        // TODO finish this function ;)
        this.updateVectorNodePositions();
        //double a[nDims];
        /*for(int i=0; i<nDims; i++)
        {
        container[i] = nodes(index,i);
        }
        return vNodes;
        //return a;
        }*/

        template <class dType, int nDims,class MType, class VType >
        bool 
                Geom_MeshSpatialObject<dType, nDims,MType,VType>
		::setMatrixNodePositions(MatrixType pos, int * inds)
        {
                for(int i=0; i< sizeof(inds)/sizeof(int); i++)
                        this->mNodes.set_row(i,pos.get_row(i));
                this->updateMeshNodePositions();
                return true;
        }

        template <class dType, int nDims,class MType, class VType >
        bool 
                Geom_MeshSpatialObject<dType, nDims,MType,VType>
		::addNodes(MatrixType pos, VectorType classes)
        {
                
                MatrixType tmp;
                tmp.set_size(this->numNodes+pos.rows(),nDims);
                VectorType tmpC;
                tmpC.set_size(this->numNodes+pos.rows());

                //Copy the information
                for(int i=0;i<this->numNodes;i++)
                {
                        tmp.set_row(i,this->mNodes.get_row(i));
                        tmpC(i) = this->nodeClass(i);
                }
                //Add the new information
                for(int i=this->numNodes, c=0;i<this->numNodes+pos.rows();i++,c++)
                {
                        tmp.set_row(i,pos.get_row(c));
                        tmpC(i) = classes(c);
                }

                this->numNodes +=pos.rows();

                this->mNodes.set_size(this->numNodes,nDims);
                this->mNodes = tmp;//Copy back the old information plus the new information
                this->nodeClass = tmpC;

                //Update the mesh node positions to include the new nodes
                this->updateMeshNodePositions();
                return true;
        }
        template <class dType, int nDims,class MType, class VType >
        bool 
                Geom_MeshSpatialObject<dType, nDims,MType,VType>
		::setMatrixNodePositions(MatrixType pos)
        {
                this->mNodes = pos;
                this->updateMeshNodePositions();
                return true;
        }

        template <class dType, int nDims,class MType, class VType >
        void 
                Geom_MeshSpatialObject<dType, nDims,MType,VType>
		::updateMatrixNodePositions()
        {
                typename MeshType::PointsContainer::Iterator pointIterator;
                pointIterator = theMesh->GetPoints()->Begin();
                typename MeshType::PointsContainer::Iterator end = theMesh->GetPoints()->End();

                int count =0;
                this->numNodes =0;
                //TODO just get the # of points from the mes
                while( pointIterator != end )
                {       ++pointIterator; // advance to next point
                this->numNodes++;
                }


                pointIterator = theMesh->GetPoints()->Begin();
                this->mNodes.set_size(this->numNodes,nDims);
                while( pointIterator != end )
                {       
                        typename MeshType::PointType p = pointIterator.Value(); // access the point
                        for(int i=0;i<nDims;i++)
                        {
                                this->mNodes(count,i)=p[i];
                        }
                        ++pointIterator; // advance to next point
                        count++;
                }
        }

        template <class dType, int nDims,class MType, class VType >
        void 
                Geom_MeshSpatialObject<dType, nDims,MType,VType>
                ::updateMeshNodePositions()
        {

                PointType tmp;
                for(int count=0;count < this->numNodes; count++)
                {
                        for(int i=0; i<nDims; i++)
                        {
                                tmp[i] = this->mNodes(count,i);
                        }
                        theMesh->SetPoint(count,tmp); //Add the point to the mesh
                }
                theMeshSpatialObject->SetMesh(theMesh);
        }

        template <class dType, int nDims,class MType, class VType >
        void 
                Geom_MeshSpatialObject<dType, nDims,MType,VType>
                ::updateMatrixConnections()
        {
                CellIterator cellIterator = theMesh->GetCells()->Begin();
                CellIterator cellEnd = theMesh->GetCells()->End();

                cellIterator = theMesh->GetCells()->Begin();
                cellEnd = theMesh->GetCells()->End();

                typename TriangleType::PointIdIterator pit;

                //TODO dynamically figure out the number of connections
                //There are three connections per cell
                this->mConnections.set_size(3*theMesh->GetNumberOfCells(),2);
                this->numConnections =0;
                while( cellIterator != cellEnd )
                {
                        CellType * cell = cellIterator.Value();
                        switch( cell->GetType() )
                        {
                        case CellType::TRIANGLE_CELL:
                                {
                                        TriangleType * tri = dynamic_cast<TriangleType *>( cell );
                                        pit = tri->PointIdsBegin();
                                        //Create a spring for each edge of the triangle

                                        mConnections(this->numConnections,0) = *(pit);
                                        mConnections(this->numConnections,1) = *(pit+1);
                                        this->numConnections++;

                                        mConnections(this->numConnections,0) = *(pit+1);
                                        mConnections(this->numConnections,1) = *(pit+2);
                                        this->numConnections++;

                                        mConnections(this->numConnections,0) = *(pit+2);
                                        mConnections(this->numConnections,1) = *(pit);
                                        this->numConnections++;

                                        break;
                                }
                        }
                        ++cellIterator;
                }
        }

        template <class dType, int nDims,class MType, class VType >
        void 
                Geom_MeshSpatialObject<dType, nDims,MType,VType>
                ::writeNodesToFile( std::string fileName )
        {
                std::ofstream out(fileName.c_str());

                typename MeshType::PointsContainer::Iterator pointIterator;
                pointIterator = theMesh->GetPoints()->Begin();
                typename MeshType::PointsContainer::Iterator end = theMesh->GetPoints()->End();
                out << theMesh->GetNumberOfPoints() << std::endl;
                while( pointIterator != end )
                {
                        typename MeshType::PointType p = pointIterator.Value(); // access the point
                        out << p << std::endl; // print the point
                        ++pointIterator; // advance to next point
                }
        }

        template <class dType, int nDims,class MType, class VType >
        void 
                Geom_MeshSpatialObject<dType, nDims,MType,VType>
		::writeObjectToFile(std::string fileName)
        {
                writer->SetInput( theMeshSpatialObject );
                writer->SetFileName( fileName.c_str() );
                writer->Update();
        }

        template <class dType, int nDims,class MType, class VType >
        void 
                Geom_MeshSpatialObject<dType, nDims,MType,VType>
		::readNodesFromFile( std::string fileName )
        {
                std::ifstream in(fileName.c_str());
                int count = 0;
                int size =0;
                in >> size;
                //MeshType::CoordRepType nodes[size][nDims];
                PointType tmp;
                while(count < size)
                {
                        for(int i=0; i<nDims; i++)
                        {
                                in >> tmp[i];
                        }
                        theMesh->SetPoint(count,tmp); //Add the point to the mesh
                        count++;
                }
                this->numNodes = theMesh->GetNumberOfPoints();
                theMeshSpatialObject->SetMesh(theMesh);
                updateMatrixNodePositions();

                //Update the state variable
                this->nodesChange = true;
        }

        template <class dType, int nDims,class MType, class VType >
        bool 
                Geom_MeshSpatialObject<dType, nDims,MType,VType>
		::readTopologyFromFile( std::string fileName )
        {
                //TODO error checking on correct input type

                typename ReaderType::Pointer reader = ReaderType::New();
                reader->SetFileName(fileName.c_str());
                reader->Update();


                /*typedef itk::SpatialObjectToImageFilter< GroupType,BinaryImageType >          SpatialObjectToImageFilterType;
                SpatialObjectToImageFilterType::Pointer objectToImageFilter = SpatialObjectToImageFilterType::New();
                objectToImageFilter->SetInput( reader->GetGroup());

                objectToImageFilter->Update();
                typedef typename itk::ImageFileWriter<BinaryImageType> writerType;
                writerType::Pointer writer = writerType::New();
                writer->SetFileName("C:\\testSpatial.mhd");
                writer->SetInput( objectToImageFilter->GetOutput() );
                writer->Update();*/



                /*      typedef itk::SceneSpatialObject<3> SceneSpatialObjectType;
                SceneSpatialObjectType::Pointer scene = SceneSpatialObjectType::New();
                typedef itk::EllipseSpatialObject<3> EllipseType;
                EllipseType::Pointer ellipse1 = EllipseType::New();
                ellipse1->SetRadius(1);
                ellipse1->SetId(1);
                EllipseType::Pointer ellipse2 = EllipseType::New();
                ellipse2->SetId(2);
                ellipse2->SetRadius(2);
                scene->AddSpatialObject(ellipse1);
                scene->AddSpatialObject(ellipse2);

                EllipseType* ellipse3;// = EllipseType::New();
                ellipse3 = dynamic_cast<EllipseType *> (scene->GetObjectById(1)); */






                //GroupType::ChildrenListPointer children = (reader->GetGroup())->GetChildren();
                //GroupType::ChildrenListType::iterator it = children->begin();

                //Get the first child's mesh
                MeshSpatialObjectType* t = dynamic_cast<MeshSpatialObjectType *> ( (reader->GetScene())->GetObjectById(0));
                theMeshSpatialObject->SetMesh( t->GetMesh());
                theMesh = theMeshSpatialObject->GetMesh();
                //theMeshSpatialObject->SetMesh( ->GetMesh() );

                //Update the points
                updateMatrixNodePositions();

                //Update the matrix connections
                updateMatrixConnections();

                //Set the state variable so other modules are aware of the change.
                this->topologyChange = true;

                /*
                int count = 0;
                int size =0;
                int start =0;
                int middle =0;
                int end =0;
                in >> size;
                //MeshType::CoordRepType nodes[size][nDims];
                PointType tmp;
                CellAutoPointer Lines;
                mConnections.set_size(size,2);//Store all of the connections in vector format
                while(count < size)
                {
                in >> start >> end;
                Lines.TakeOwnership( new LineType);
                Lines->SetPointId(0,start);
                Lines->SetPointId(1,end);
                //theMesh->SetCell(theMesh->GetNumberOfCells(),Lines);

                mConnections(count,0) = start;
                mConnections(count,1) = end;
                count++;
                }
                numConnections = count;
                theMeshSpatialObject->SetMesh(theMesh);
                //std::cout << "Mesh bounds : " << theMeshSpatialObject->GetBoundingBox()->GetBounds() << std::endl;
                topologyChange = true;*/
                return false;
        }

        template < class dType, int nDims, class MType, class VType >
        typename itk::Image< unsigned char, nDims>::Pointer
                Geom_MeshSpatialObject< dType, nDims, MType, VType >
		::generateBinaryImageFromTopology( typename Geom_MeshSpatialObject<dType,nDims,MType,VType>::BinaryImageType::SizeType size)
        {
//              std::cout << "Starting binary image generation from mesh..." << std::endl;
                typename BinaryImageType::Pointer image = BinaryImageType::New();

                typedef itk::TriangleMeshToBinaryImageFilter< MeshType, BinaryImageType >  meshToImageFilterType;
                typename meshToImageFilterType::Pointer meshToImageFilter = meshToImageFilterType::New();
                
                // TODO fix this workaround and pass BinaryImageType::SizeType variable instead of dType*
                /*BinaryImageType::SizeType size;
                for( unsigned int i=0; i<nDims; i++)
                {
                        size[i] = imageSize[i];
                }*/

                meshToImageFilter->SetSize( size );

                meshToImageFilter->SetInput( theMeshSpatialObject->GetMesh() );
                image = meshToImageFilter->GetOutput();
                meshToImageFilter->Update();

//              std::cout << "Finished binary image generation from mesh." << std::endl;
//              BinaryImageType::IndexType start;
//              BinaryImageType::SizeType size;
//              dType origin[ nDims ]; 
//
//              //Compute the size of the spatial object
//              theMeshSpatialObject->ComputeBoundingBox();
//              //std::cout << "Mesh bounds : " << theMeshSpatialObject->GetBoundingBox()->GetBounds() << std::endl;
//              for(unsigned int i=0;i<nDims;i++)
//              {
//                      size[i] = (long unsigned int)(theMeshSpatialObject->GetBoundingBox()->GetMaximum()[i]- theMeshSpatialObject->GetBoundingBox()->GetMinimum()[i]);
//                      start[i] =0;
//                      origin[i] =theMeshSpatialObject->GetBoundingBox()->GetMinimum()[i];
//              }
//              BinaryImageType::RegionType region;
//              region.SetSize( size );
//              region.SetIndex( start);
//
//              image->SetRegions( region );
//              image->Allocate();
//
//              //TODO get spacing from input image
//              /*dType spacing[ nDims ];
//              image->SetSpacing( spacing );*/
//
//              //TODO get correct origin (offset) to map to world coridinates
//              image->SetOrigin( origin );
//
//              //Define a point for the image and the iterators
//              BinaryImageType::PointType pointXYZ;
//              typedef itk::ImageRegionIterator< BinaryImageType> IteratorType;
//
//              IteratorType imgIt( image, region );
//              VectorType vPoint(nDims);
//              for ( imgIt.GoToBegin(); !imgIt.IsAtEnd();++imgIt)
//              {
//                      //Convert from index to 3-space
//                      image->TransformIndexToPhysicalPoint(imgIt.GetIndex(),pointXYZ);
//                      for(int i=0;i<nDims;i++)
//                      {
//                              vPoint(i) = pointXYZ[i];
//                      }
//                      imgIt.Set( 255*isInside(vPoint) );
//              }
//              MeshSpatialObjectType::PointType myPhysicalPoint; 
//              myPhysicalPoint.Fill(50);
//              std::cout << "Is my physical point inside? : " << theMeshSpatialObject->IsInside(myPhysicalPoint) << std::endl;
//
//              VectorType p(nDims);
//              p(0) = 50;
//              p(1) = 60;
//              p(2) = 30;
//              std::cout << "Is my physical point REALLY inside? : " << this->isInside(p) << std::endl;
//
//
//              p(0) = 5;
//              p(1) = 5;
//              p(2) = 5;
//              std::cout << "Is my other physical point REALLY inside? : " << this->isInside(p) << std::endl;
//
//
//              typedef typename itk::ImageFileWriter<BinaryImageType> writerType;
//              writerType::Pointer writer = writerType::New();
//              writer->SetFileName("C:\\testSpatial.mhd");
//              writer->SetInput( image );
//              writer->Update();
//
//              //end testing*/

                return image;
        }

        template < class dType, int nDims, class MType, class VType >
        void 
                Geom_MeshSpatialObject< dType, nDims, MType, VType >
                ::generateTopologyFromBinaryImage( BinaryImageTypePointer binaryInputImage )
        {
                typedef itk::BinaryMask3DMeshSource< BinaryImageType, MeshType >                MeshSourceType;
                typename MeshSourceType::Pointer imgToMeshFilter = MeshSourceType::New();

                imgToMeshFilter->SetInput( binaryInputImage );

                // Get the max binary image value so we can define what is "inside" the mesh:
                typedef itk::MinimumMaximumImageFilter< BinaryImageType >                               MinMaxFilterType;
                typename MinMaxFilterType::Pointer minMaxFilter = MinMaxFilterType::New();
                minMaxFilter->SetInput( binaryInputImage );
                minMaxFilter->Update();

                imgToMeshFilter->SetObjectValue( minMaxFilter->GetMaximum() ); 

                imgToMeshFilter->Update();

                std::cout << "BinaryMask3DMeshSource update completed." << std::endl; // DEBUG
                theMesh = imgToMeshFilter->GetOutput();

                // TODO: find out why CellData is bad and use a better solution
                // for some reason, unknown CellData is being written to file; temporary workaround:
                typename MeshType::CellDataContainer::Pointer emptyCellDataContainer = MeshType::CellDataContainer::New();
                theMesh->SetCellData( emptyCellDataContainer );
                theMeshSpatialObject->SetMesh( theMesh );
        }
        template < class dType, int nDims, class MType, class VType >
        bool
                Geom_MeshSpatialObject< dType, nDims, MType, VType >
		::isInside( VectorType p)
        {//Check if a given point is inside the object

                //For all triangles, check if a ray extending from the object in both directions intersects.
                //If the ray intersects an odd number of triangles on both sides in it is inside.
                CellIterator cellIterator = theMesh->GetCells()->Begin();
                CellIterator cellEnd = theMesh->GetCells()->End();

                cellIterator = theMesh->GetCells()->Begin();
                cellEnd = theMesh->GetCells()->End();

                TriangleType * tri;
                int pCount = 0;
                int nCount = 0;
                VectorType ray(nDims);

                //Since any arbritary ray can be checked always use x-axis
                ray.fill(0);
                ray(0)=1;
                ray(1)=0;
                ray(2)=0;

                double minDist =0;
                CoordRepType closestPoint[nDims];
                CoordRepType position[nDims]; //A point

                for(int i=0; i<nDims;i++)
                        position[i] = p(i);

                while( cellIterator != cellEnd )
                {
                        CellType * cell = cellIterator.Value();
                        if( cell->GetType() == CellType::TRIANGLE_CELL )
                        {
                                tri = dynamic_cast<TriangleType *>( cell );
                                tri->EvaluatePosition(position,theMesh->GetPoints(),closestPoint,NULL,&minDist,NULL);
                                if(abs(minDist) <= m_IsInsidePrecision)
                                {//If the point is on the surface return true
                                        return true;
                                }else
                                {//otherwise check if it is within the object.
                                        if(intersectWithLine(ray,p,tri))
                                                pCount++;
                                        if(intersectWithLine(-ray,p,tri))
                                                nCount++;
                                }
                        }
                        ++cellIterator;
                }
                return ( (pCount%2 !=0)&&(nCount%2!=0));//Return true iff both are odd
        }

        template < class dType, int nDims, class MType, class VType >
        bool
                Geom_MeshSpatialObject< dType, nDims, MType, VType >
		::intersectWithLine( VectorType L, VectorType P, TriangleType* tri)
        {//Check if a line L(s) = P + s*L intersects a triangle tri

                if(this->nodesChange) //Make sure the positions are up to date
                        updateMatrixNodePositions();

                typename TriangleType::PointIdIterator pit;
                pit = tri->PointIdsBegin();

                //Compute the normal to triangle
                VectorType v1(nDims);
                VectorType v2(nDims);
                VectorType n(nDims);
                VectorType w(nDims);
                VectorType intersectionPoint(nDims);

                v1 = this->mNodes.get_row(*(pit+1))-this->mNodes.get_row(*pit);
                v2 = this->mNodes.get_row(*(pit+2))-this->mNodes.get_row(*pit);
                n = vnl_cross_3d(v1,v2);
                n.normalize(); //Ensure that n is unit length
                L.normalize(); //Ensure that L is unit length

                /*std::cout << "v1 : " << v1(0) << " " << v1(1) << " " << v1(2) <<std::endl;
                std::cout << "v2 : " << v2(0) << " " << v2(1) << " " << v2(2) <<std::endl;
                std::cout << "n : " << n(0) << " " << n(1) << " " << n(2) <<std::endl;
                std::cout << "L : " << L(0) << " " << L(1) << " " << L(2) <<std::endl;*/
                dType s;
                CoordRepType position[nDims]; //A point
                double minDist;
                CoordRepType closestPoint[nDims];
                //Are L and the normal orthogonal, if so return false
                double t;
                t = dot_product(n,L);
                if( t != 0)
                {
                        w = P - this->mNodes.get_row(*(pit));//Vector from a point on the line to a point on the plane
                        s = -dot_product(n,w)/dot_product(n,L);
                        if(s>=0)
                        {
                                intersectionPoint = P+ s*L;
                                //std::cout << "Intersection at : " << intersectionPoint(0) << " " << intersectionPoint(1) << " " << intersectionPoint(2) <<std::endl;
                                for(int i=0;i<nDims;i++)
                                        position[i] = intersectionPoint(i);

                                //Does the point lie within the triangle?
                                //Check how close the intersection point is to the triangle, accept if its within a tolerance.
                                //The distance will be returned in minDist;
                                tri->EvaluatePosition(position,theMesh->GetPoints(),closestPoint,NULL,&minDist,NULL);
                                //std::cout << "Min dist: " << minDist << std::endl;
                                if(abs(minDist) <= m_IsInsidePrecision)
                                        return true;
                                else
                                {
                                        return false;
                                }
                        }
                        else
                                return false;
                }
                else
                {
                        return false;
                }
        }

        template < class dType, int nDims, class MType, class VType >
        bool
                Geom_MeshSpatialObject< dType, nDims, MType, VType >
                ::addConnection(int start,int end)
        {
                MatrixType tmp;
                tmp.set_size(this->numConnections+1,2);

                //Copy the information
                for(int i=0;i<this->numConnections;i++)
                {
                        tmp(i,0) = this->mConnections(i,0);
                        tmp(i,1) = this->mConnections(i,1);
                }
                tmp(this->numConnections,0) = start;
                tmp(this->numConnections,1) = end;

                this->numConnections++;

                this->mConnections.set_size(this->numConnections,2);
                this->mConnections = tmp;//Copy back the old information plus the new information

                CellAutoPointer Lines;
                //Add a new line
                
                //Can't use lines, as SOV will not render them
                /*Lines.TakeOwnership( new LineType);
                Lines->SetPointId(0,start);
                Lines->SetPointId(1,end);
                this->theMesh->SetCell(theMesh->GetNumberOfCells(),Lines);*/

                //Need triangles instead.


                this->topologyChange = true;

                return false;
        }

}//end mial
#endif

---