Biomechanical Joint Model  Author: Anderson Maciel

---

comemesh.cpp

Go to the documentation of this file.

/***************************************************************************
 *   Copyright (C) 2002 by Anderson Maciel                                 *
 *   andi.maciel@gmail.com                                                 *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 **************************************************************************/

#include <algebra/comemesh.h>
#include <algebra/comepoint3d.h>
#include <algebra/comevector3d.h>
#include <bio/comebiostructure.h>
#include <bio/comemoleculescartilage.h>
//#include <bio/comemoleculestissue.h>
#include <VCOLLIDE/RAPID/RAPID.H>
#include <math.h>

#ifndef VTK_USE_ANSI_STDLIB
#define VTK_USE_ANSI_STDLIB
#endif
#include <vtkPolyDataReader.h>
#include <vtkPolyData.h>
#include <vtkCellArray.h>

int COME_Mesh::textureCount = 0;
COME_Mesh::COME_Mesh(){

        texture         = NULL; 
        textureID = 0;
}

COME_Mesh :: COME_Mesh( const vector<COME_Vertex3D> verticesN, const vector<COME_Face> facesN, COME_BioStructure* parentN ){

        vertices        = verticesN; 
        faces           = facesN;
        parent          = parentN;
        texture         = NULL;
}

COME_Mesh ::~COME_Mesh(){

        
}


void                            
COME_Mesh :: addFaceAsIndices( vector<int> indicesN ){

        COME_Face face( indicesN, this );
        face.updateNormal();
        //face.setNearestMolecule( ((COME_MoleculesCartilage*)parent)->getTissue()->nearestTo( face.getCenter() ) );

        faces.push_back( face );
}

void
COME_Mesh :: addVertex( COME_Vertex3D vertexN ){

        vertices.push_back( vertexN );
        verticesGlobal.push_back( vertexN ); 
}

void
COME_Mesh :: setAVertex( COME_Vertex3D vertN, int iPosN ){

        vertices[iPosN] = vertN;

}

void
COME_Mesh :: setCollisionDetectableFaces() {

        vector<COME_Face*> result;
        for (int i = 0; i < faces.size();i++){
                
                for ( int iPos = 0; iPos < faces[i].getNumberVertices(); iPos ++ ){  
                        
                        if( faces[i].getVertex( iPos ).vpDistance( COME_Point3D() ) < ((COME_BioStructure*)parent)->getCollisionRadius() ){
                                faces[i].setCollisionDetectable( true );
                                break;
                        }
                        faces[i].setCollisionDetectable( false );
                }
        }
}

void
COME_Mesh :: setFileName( string newName ){

        meshFile = newName;
}
        


string
COME_Mesh :: getFileName(){

        return meshFile;
}

vector<COME_Vertex3D>&  
COME_Mesh :: getVertices(){

        return vertices;
}

vector<COME_Vertex3D>*
COME_Mesh :: getVerticesPt(){

        return &vertices;
}
vector<COME_Vertex3D>   
COME_Mesh :: getVerticesGlobal() {
        return verticesGlobal;
}

vector<COME_Vertex3D>*
COME_Mesh::getVerticesGlobalPt(){
        return &verticesGlobal;
}

COME_Matrix
COME_Mesh:: getGIM(){
        return ((COME_BioStructure*)parent)->getGIM();
}

COME_Vertex3D*
COME_Mesh :: getAVertexGlobalPositionPt( int iPosN ) {
 return &verticesGlobal[iPosN];
}

COME_Vertex3D
COME_Mesh :: getAVertexGlobalPosition( int iPosN ) {

        return verticesGlobal[ iPosN ];
}

COME_Vertex3D
COME_Mesh :: getClosestVertex( COME_Point3D givenPoint ) const {

        double minDist  = givenPoint.vpDistance( verticesGlobal[ 0 ] );
        int nearest             = 0;
        //search the vertex nearest of the currentPoint
        for ( int i = 1; i < verticesGlobal.size(); i++ ){
        
                double currDist = givenPoint.vpDistance( verticesGlobal[ i ] );
                if( currDist < minDist ){
                        minDist = currDist;
                        nearest = i;
                }
        }
        return verticesGlobal[ nearest ];
}

int
COME_Mesh :: getClosestVertexIndex( COME_Point3D givenPoint ) const {

        double minDist  = givenPoint.vpDistance( verticesGlobal[ 0 ] );
        int nearest     = 0;
        //search the vertex nearest of the currentPoint
        for ( int i = 1; i < verticesGlobal.size(); i++ ){
        
                double currDist = givenPoint.vpDistance( verticesGlobal[ i ] );
                if( currDist < minDist ){
                        minDist = currDist;
                        nearest = i;
                }
        }
        return nearest;
}

int
COME_Mesh :: getClosestFaceIndex( COME_Point3D givenPoint ) {

        double minDist  = faces[0].distancePointFaceGlobal( givenPoint );
        int nearestF    = 0;

        for( int iF = 1; iF < faces.size(); iF++ ){
                double currDist = faces[iF].distancePointFaceGlobal( givenPoint );
                
                if( currDist < minDist ){
                        minDist = currDist;
                        nearestF = iF;
                }
        }
        
        return nearestF;
}

vector<COME_Face>       
COME_Mesh :: getFaces() const {
        return faces;
}

vector<COME_Face>       *
COME_Mesh :: getFacesPt() {
        return &faces;
}

vector<COME_Face*>      
COME_Mesh :: getPtFaces() {

        vector<COME_Face*> result;
        for (int i = 0; i < faces.size();i++){
                result.push_back(&faces[i]);
        }
        return result;
}

vector<COME_Vertex3D>
COME_Mesh :: getVerticesFace( int iNumberFace ) const{
        
        vector<COME_Vertex3D> vertex;

        COME_Face faceN = faces[ iNumberFace ];
        for ( int iPos = 0; iPos < faceN.getNumberVertices(); iPos ++ ){  
                        
                vertex.push_back( faceN.getVertex( iPos ) );    
        }

        return vertex;
}

vector<COME_Vertex3D>
COME_Mesh :: getVerticesFaceGlobalPosition( int iNumberFace ) const{
        
        vector<COME_Vertex3D> vertex;

        COME_Face faceN = faces[ iNumberFace ];
        for ( int iPos = 0; iPos < faceN.getNumberVertices(); iPos ++ ){  
                        
                vertex.push_back( faceN.getVertexGlobalPosition( iPos ) );      
        }

        return vertex;
}

COME_Vertex3D
COME_Mesh :: getAVertex( int iPosN ) const{

        return vertices[ iPosN ];
}

COME_Vertex3D*
COME_Mesh :: getAVertexPt( int iPosN ){

        return &vertices[ iPosN ];
}


COME_Face
COME_Mesh :: getAFace( int iNumberFace ) const{

        return faces[ iNumberFace ];
}

COME_Face*
COME_Mesh :: getAFacePt( int iNumberFace ){

        return &faces[ iNumberFace ];
}

int                     
COME_Mesh :: getNumberFaces() const{

        return faces.size();
}

void 
COME_Mesh :: getEnvelop( COME_Point3D& mins, COME_Point3D& maxs ){

        double xMin = 0.0,yMin = 0.0,zMin = 0.0,xMax = 0.0,yMax = 0.0,zMax = 0.0;

        if( vertices.size() > 0 ){
                //initialization
                xMin  = vertices[ 0 ].getX();
                yMin  = vertices[ 0 ].getY();   
                zMin  = vertices[ 0 ].getZ();
                xMax  = xMin; 
                yMax  = yMin;
                zMax  = zMin;
                
                for ( int i = 1; i < vertices.size(); i++ ){
                
                        //min
                        if ( xMin > vertices[ i ].getX() ){
                        
                                xMin = vertices[ i ].getX();            
                        }
        
                        if ( yMin > vertices[ i ].getY() ){
                        
                                yMin = vertices[ i ].getY();            
                        }
        
                        if ( zMin > vertices[ i ].getZ() ){
                        
                                zMin = vertices[ i ].getZ();            
                        }
        
        
                        //max
                        if ( xMax < vertices[ i ].getX() ){
                        
                                xMax = vertices[ i ].getX();            
                        }
        
                        if ( yMax < vertices[ i ].getY() ){
                        
                                yMax = vertices[ i ].getY();            
                        }
        
                        if ( zMax < vertices[ i ].getZ() ){
                        
                                zMax = vertices[ i ].getZ();            
                        }
        
        
                }
        } else {
                printf( "Mesh is empty, envelop is set to (0 0 0) (0 0 0). \n" );
        }

        mins.setXYZ( xMin, yMin, zMin );
        maxs.setXYZ( xMax, yMax, zMax );

}

COME_Point3D
COME_Mesh::medialPoint(){

        COME_Point3D medial;
        for ( int i = 0; i < vertices.size(); i++ ){
        
                medial = medial + vertices[i];
        }
        medial = medial * ( 1.0 / (double)vertices.size() );
        return medial;
}

void 
COME_Mesh :: resetCollisionDisps(){

        for ( int i = 0; i < vertices.size(); i++ ){
        
                verticesGlobal[i].setCollisionDisp( COME_Vector3D() );
                verticesGlobal[i].setVelocityDisp( COME_Vector3D() );
        }
        
}


// Recalculates all the vertices position based on the anchoring molecules
// and updates the faces normals as well as the Vertices normals
void 
COME_Mesh:: updateSkin(){
        if (COME::flagSkinning ){
                //printf( "Updating skinning\n" );
                updateGlobalPositions();
                updateFacesNormalsGlobalPosition();
                //Would be good to calculate the vertices normals only if we realy need them
                //if (calcNormals){
                        updateVerticesNormalsGlobalPosition();
                //}
        }
}

void    
COME_Mesh :: updateGlobalPositions() {

        COME_Matrix localGIM = ((COME_BioStructure*)parent)->getGIM();

        //printf( " BEFORE %f %f %f\n", verticesGlobal[1].getX(), verticesGlobal[1].getY(), verticesGlobal[1].getZ());
        for( int i = 0; i < vertices.size(); i++ ){
                if( vertices[i].localPositions.size() > 0 ){
                        verticesGlobal[i] = /*localGIM **/ vertices[i].getBlendedPosition();
                        verticesGlobal[i].setTexCoord( vertices[i].getTexCoord() );
                        verticesGlobal[i].setOwnerFace( vertices[i].getOwnerFace() );
                        verticesGlobal[i].anchors = vertices[i].anchors;
                        verticesGlobal[i].localPositions = vertices[i].localPositions;
                        //printf("skinning ON\n");
                } else {
                        verticesGlobal[i] = localGIM * vertices[i];
                        //printf("NO skinning\n");
                }
                //verticesGlobal[i].setCollisionDisp( COME_Vector3D() );
        }
        //printf( " AFTER_ %f %f %f\n", verticesGlobal[1].getX(), verticesGlobal[1].getY(), verticesGlobal[1].getZ());
}

void
COME_Mesh::updateFacesNormalsGlobalPosition(){
        
        for (int i = 0; i < faces.size();i++){
                faces[i].updateNormalGlobalPosition();
                //printf(" FACES NORMALS: %f %f %f \n", faces[i].getNormalGlobalPosition().x, faces[i].getNormalGlobalPosition().y, faces[i].getNormalGlobalPosition().z );
                
                /*double lacor[4]; lacor[0]=1; lacor[1]=0.2; lacor[2]=0.2; lacor[3]=1;
                faces[i].getVertexGlobalPositionPt(0)->setColor( lacor );
                faces[i].getVertexGlobalPositionPt(1)->setColor( lacor );
                faces[i].getVertexGlobalPositionPt(2)->setColor( lacor );*/
        }
}

void
COME_Mesh :: updateVerticesNormals(){
        
        for( int i = 0; i < vertices.size(); i++ ){
                
                vector <int> neighbourFaces = verticesGlobal[i].getNeighbourFaces();
                COME_Vector3D totalNormals;
                
                for( int j = 0; j < neighbourFaces.size(); j++ ){
                        totalNormals += faces[neighbourFaces[j]].getNormal();
                        //printf("NEIGHBOUR FACES NORMALS: %f %f %f \n", faces[neighbourFaces[j]].getNormalGlobalPosition().x, faces[neighbourFaces[j]].getNormalGlobalPosition().y, faces[neighbourFaces[j]].getNormalGlobalPosition().z );
                }
                totalNormals = totalNormals / neighbourFaces.size();
                totalNormals.vpNormalize();
                verticesGlobal[i].setNormal(totalNormals);
                //printf(" Vertices Globals: %f %f %f \n", totalNormals.x, totalNormals.y, totalNormals.z );
 
        }
}

void
COME_Mesh :: updateVerticesNormalsGlobalPosition(){
        
        for( int i = 0; i < vertices.size(); i++ ){
                
                vector <int> neighbourFaces = verticesGlobal[i].getNeighbourFaces();
                COME_Vector3D totalNormals;
                
                for( int j = 0; j < neighbourFaces.size(); j++ ){
                        totalNormals += faces[neighbourFaces[j]].getNormalGlobalPosition();
                        //printf("NEIGHBOUR FACES NORMALS: %f %f %f \n", faces[neighbourFaces[j]].getNormalGlobalPosition().x, faces[neighbourFaces[j]].getNormalGlobalPosition().y, faces[neighbourFaces[j]].getNormalGlobalPosition().z );
                }
                totalNormals = totalNormals / neighbourFaces.size();
                totalNormals.vpNormalize();
                verticesGlobal[i].setNormalGlobalPosition(totalNormals);
                //printf(" Vertices Globals: %f %f %f \n", totalNormals.x, totalNormals.y, totalNormals.z );
 
        }
}



bool
COME_Mesh :: isInside( COME_Point3D currentPoint ){

        double area = 0.0;
        for ( int f = 0; f < faces.size(); f++ ){
                faces[f].setParent(this); // This is not good, used because parent was lost
                area += faces[f].geoSolidAngle( currentPoint );
        }
        return ( ( ( area > 2*M_PI ) || ( area < -2*M_PI ) ) ? true : false );
        

}

void
COME_Mesh::findIntersections( COME_Point3D pi, COME_Point3D pf, vector<COME_Point3D> *allIntersections ){

        COME_Vector3D n, v, vl, vll;
        double t, denom, U, V;
        
        vector<COME_Vertex3D>* allVertices = getVerticesGlobalPt();

        for (int i = 0; i < faces.size();i++){

                v = allVertices->at(faces[i].getIndexVertex(0));
                vl = allVertices->at(faces[i].getIndexVertex(1));
                vll = allVertices->at(faces[i].getIndexVertex(2));
                n = faces[i].getNormal();
                
                // If segment and face are parallel the average position of the vertices should be the intersecting point, but the code is commented below to avoid indesirable points when faces are very large
                denom = COME_Vector3D( pf - pi ).vpDotProduct(n);
                /*if( denom == 0.0 ){
                        // point is put in the face center
                        allIntersections->push_back( COME_Vector3D( v + vl + vll ) / 3.0 );
                        continue;
                }
                */
                
                t = -(COME_Vector3D( pi - v ).vpDotProduct(n) / denom);
                COME_Vector3D VPt = ( pi - v ) + ( ( pf - pi ) * t );
                
                denom = ((vl-v)*(vll-v)).maxK();
                U = (VPt*(vll-v)).maxK()/denom;
                V = ((vl-v)*VPt).maxK()/denom;
                                
                if( ( U >= 0.0 ) && ( U <= 1.0 ) && ( V >= 0.0 ) && ( V <= 1.0 ) && ( U+V >= 0.0 ) && ( U+V <= 1.0 ) ){
                        allIntersections->push_back( v+VPt );
                }
        }
}

bool
COME_Mesh :: loadFile( string fileName, double scaleFactor, COME_Point3D originTranslation ){

        FILE *file;
        float x1, y1, z1, x2, y2, z2, t1, t2;
        int v1, v2, v3, terminator;

        setFileName( fileName );
        //Split here to load vtk files

        if( !( fileName.find(".vtk",0) == string::npos ) ){

                        // open and load the file
                        vtkPolyDataReader *vtkComeLoader = vtkPolyDataReader::New();
                        vtkComeLoader->SetFileName( fileName.c_str() );
                        vtkComeLoader->Update();
                        vtkPolyData *vtkData = vtkComeLoader->GetOutput();

                        vtkIdType *pts = 0;
                        vtkIdType npts = 0;
                        float *floatArray;

                        // parse the polydata

                        npts = vtkData->GetNumberOfPoints();
                        for (int i = 0;  i < npts; i++)
                        {
                                floatArray = vtkData->GetPoint( i );
                                addVertex( COME_Vertex3D( ( floatArray[0]-originTranslation.getX() )* scaleFactor, ( floatArray[1]-originTranslation.getY() )* scaleFactor, ( floatArray[2]-originTranslation.getZ() )* scaleFactor ) );
                        }

                        vtkCellArray *inputPolys = vtkData->GetPolys();
                        inputPolys->InitTraversal();
                        
                        while(inputPolys->GetNextCell(npts,pts))
                        {
                                vector<int> thisFace;
                                thisFace.push_back( pts[0] );
                                thisFace.push_back( pts[1] );
                                thisFace.push_back( pts[2] );
                                addFaceAsIndices( thisFace );
                        }
                
        } else if( file = fopen( fileName.c_str(), "r" ) ){
                char buffer[90];
                string str_buffer;
                do {
                        fgets( buffer, 80, file ); 
                        str_buffer = buffer;

                        if( feof( file ) ){
                                printf( "Wrong format file. \"point [\\r|n\" or \"vertex [\\r|n\" section not found.");
                                exit(0);
                        }
                } 
//#ifndef _WIN32
                while( ( ( str_buffer.find( "point [\r", 0 ) == string::npos ) && ( str_buffer.find( "vertex [\r", 0 ) == string::npos ) ) && ( ( str_buffer.find( "point [\n", 0 ) == string::npos ) && ( str_buffer.find( "vertex [\n", 0 ) == string::npos ) ) ) ;
//#else
                //while( ( ( str_buffer.find( "point [\n", 0 ) == string::npos ) && ( str_buffer.find( "vertex [\n", 0 ) == string::npos ) ) && ( ( str_buffer.find( "point [\n", 0 ) == string::npos ) && ( str_buffer.find( "vertex [\n", 0 ) == string::npos ) ) ) ;
                //while( strcmp( buffer, "              point [\n") && strcmp( buffer, "                vertex [\n") );
//#endif

                printf( "Start reading points.\n" );
                while( fscanf( file, "                  %f %f %f, %f %f %f, ", &x1, &y1, &z1, &x2, &y2, &z2 ) ){

                        addVertex( COME_Vertex3D( ( x1-originTranslation.getX() )* scaleFactor, ( y1-originTranslation.getY() )* scaleFactor, ( z1-originTranslation.getZ() )* scaleFactor ) );
                        addVertex( COME_Vertex3D( ( x2-originTranslation.getX() )* scaleFactor, ( y2-originTranslation.getY() )* scaleFactor, ( z2-originTranslation.getZ() )* scaleFactor ) );
                }
                printf( "Finished reading points.\n" );

                do {
                        fgets( buffer, 80, file ); 
                        str_buffer = buffer;
                        if( feof( file ) ){
                                printf( "Wrong format file. \"coordIndex [\\r\" section not found.");
                                exit(0);
                        }
                }while( ( str_buffer.find( "coordIndex [\r", 0 ) == string::npos ) && ( str_buffer.find( "coordIndex [\n", 0 ) == string::npos ) );

                printf( "Start reading faces.\n" );
                while( fscanf( file, "                  %d, %d, %d, %d,", &v1, &v2, &v3, &terminator ) ){

                        vector<int> thisFace;
                        thisFace.push_back( v1 );
                        thisFace.push_back( v2 );
                        thisFace.push_back( v3 );
                        addFaceAsIndices( thisFace );
                }
                printf( "Finished reading faces.\n" );
                
                rewind(file);
                bool noTexture = false;
                do {
                        fgets( buffer, 80, file ); 
                        str_buffer = buffer;
                        if( feof( file ) ){
                                printf( " \"texCoord\" section not found.");
                                noTexture = true;
                                break;//exit(0);
                        }
                }while( ( str_buffer.find( "texCoord", 0 ) == string::npos ) );
                if( !noTexture ){
                        fpos_t filePos;
                        do {
                                fgetpos ( file , &filePos );
                                fgets( buffer, 80, file ); 
                                str_buffer = buffer;
                                printf( "str_buffer=%s\n", str_buffer.c_str() );
                                if( feof( file ) ){
                                        printf( "Texture coordinates not found.");
                                        noTexture = true;
                                        break;//exit(0);
                                }
                        }while( ( str_buffer.find( "point [", 0 ) == string::npos ) );
        
                        if( ! noTexture ){
                                printf( "Start reading texture coordinates.\n" );
                                int texInd = 0;
                                fsetpos ( file , &filePos );
                                fscanf( file, "         point [ %f %f,", &t1, &t2 );
                                vertices[texInd++].setTexCoord( COME_Point2D(t1, t2) );
                                while( fscanf( file, "          %f %f,", &t1, &t2 ) == 2 ){
                                        // add tex coordinates to vertices here
                                        vertices[texInd++].setTexCoord( COME_Point2D(t1, t2) );
                                }
                                printf( "Finished reading texture coordinates.\n" );
                        }
                }

                fclose( file );

                printf( "Start calculating normals.\n" );
                updateFacesNormalsGlobalPosition();
                updateVerticesNormalsGlobalPosition();
                printf( "Finished calculating normals.\n" );

                
                printf( "File %s loaded and scaled by %f.\n", fileName.c_str(), scaleFactor );

                //Initialize deformation saving
                /*string deformFileName = fileName;
                deformFileName.replace( deformFileName.size()-3, 3, ".pre" );
                fclose( fopen( deformFileName.c_str(), "wb" ) );
                setDescription( deformFileName );
                */

        } else {
                printf( "Could not open file %s.\n", fileName.c_str() );
        }
        return false;

}

void
COME_Mesh :: resetMesh(){

        faces.clear();
        vertices.clear();
}

void
COME_Mesh :: update(){

        for( int i = 0; i < vertices. size(); i++ ){

                COME_Vector3D displacement = faces[ vertices[i].getOwnerFace() ].getNearestMolecule()->getLastDisplacement();
                vertices[i] += displacement;
                vertices[i].setCollisionDisp( COME_Vector3D() );
        }
}

void
COME_Mesh :: estabilishLinkVerticesFaces(){

        for( int i = 0; i < faces.size(); i++ ){

                vertices[ faces[i].getIndexVertex(0) ].setOwnerFace( i );
                vertices[ faces[i].getIndexVertex(1) ].setOwnerFace( i );
                vertices[ faces[i].getIndexVertex(2) ].setOwnerFace( i );
        }
}

int
COME_Mesh :: simplify( double minDistance ){


        //
        for( int iFaces = 0; iFaces < faces.size(); /*iFaces++*/ ){

                if( COME_Vector3D( faces[iFaces].getVertex(0) - faces[iFaces].getVertex(1) ).vpModule() < minDistance ){
                        
                        edgeCollapse( faces[iFaces].getIndexVertex(0), faces[iFaces].getIndexVertex(1) );
                        //iFaces = 0;
                } else if( COME_Vector3D( faces[iFaces].getVertex(1) - faces[iFaces].getVertex(2) ).vpModule() < minDistance ){
                        
                        edgeCollapse( faces[iFaces].getIndexVertex(1), faces[iFaces].getIndexVertex(2) );
                        //iFaces = 0;
                } else if( COME_Vector3D( faces[iFaces].getVertex(2) - faces[iFaces].getVertex(0) ).vpModule() < minDistance ){
                        
                        edgeCollapse( faces[iFaces].getIndexVertex(2), faces[iFaces].getIndexVertex(0) );
                        //iFaces = 0;
                } else {
                        iFaces ++;
                }

        }

        return faces.size();
}

void
COME_Mesh :: edgeCollapse( int v1, int v2 ){


        vector<COME_Face>::iterator iterFaces = faces.begin();
        for( int iFaces = 0; iFaces < faces.size(); iFaces++, iterFaces++ ){

                if( faces[iFaces].containsVertexIndex( v1 ) && faces[iFaces].containsVertexIndex( v2 ) ){

                        // remove face
                        faces.erase( iterFaces);//, iterFaces );

                } else if( faces[iFaces].containsVertexIndex( v1 ) ){

                        // change vertex of the face
                        faces[iFaces].replaceIndex( v1, v2 );
                }
        }
        // remove vertice v1
        vector<COME_Vertex3D>::iterator iterVertices = vertices.begin();
        for( int iVertices = 0; iVertices < vertices.size(); iVertices++, iterVertices++ ){
                if( iVertices == v1 ){
                        vertices.erase( iterVertices );//, iterVertices );
                        break;
                }
        }
}

void
COME_Mesh :: transform( COME_Matrix M ){

        for( int i = 0; i < vertices. size(); i++ ){

                vertices[i] = M * vertices[i];
        }
        updateSkin();
}


void
COME_Mesh :: scale( double factor ){

        for( int i = 0; i < vertices. size(); i++ ){

                vertices[i] = vertices[i] * factor;
        }
        updateSkin();

}

void
COME_Mesh :: scaleOnNormals( double factor ){

        for( int i = 0; i < vertices. size(); i++ ){

                COME_Vector3D displacement = vertices[i].getNormal() * -factor;
                vertices[i] += displacement;
        }
        updateSkin();

}

void
COME_Mesh :: scale( double factorx, double factory, double factorz ){

        // Find central point
        double accX = 0.0, accY = 0.0, accZ = 0.0;
        int n;
        for( n = 0; n < vertices.size(); n++ ){
                accX+=vertices[n].getX();
                accY+=vertices[n].getY();
                accZ+=vertices[n].getZ();
        }
        accX/=(double)n; accY/=(double)n; accZ/=(double)n;
        
        // Translate to origin
        translate( -accX, -accY, -accZ );
        
        //Apply scaling
        for( int i = 0; i < vertices. size(); i++ ){

                vertices[i].setXYZ( vertices[i].getX() * factorx, vertices[i].getY() * factory, vertices[i].getZ() * factorz );
        }
        
        // Translate back to position
        translate( accX, accY, accZ );

        updateSkin();
}

void
COME_Mesh :: translate( double dx, double dy, double dz ){

        for( int i = 0; i < vertices. size(); i++ ){
                vertices[i].setXYZ( vertices[i].getX() + dx, vertices[i].getY() + dy, vertices[i].getZ() + dz );
        }
        
        updateSkin(); 
}

void
COME_Mesh :: rotate( double rx, double ry, double rz ){

        // Find central point
        double accX = 0.0, accY = 0.0, accZ = 0.0;
        int n;
        for( n = 0; n < vertices.size(); n++ ){
                accX+=vertices[n].getX();
                accY+=vertices[n].getY();
                accZ+=vertices[n].getZ();
        }
        accX/=(double)n; accY/=(double)n; accZ/=(double)n;
        
        // Translate to origin
        translate( -accX, -accY, -accZ );
        
        // Apply rotations
        for( int i = 0; i < vertices.size(); i++ ){

                vertices[i].setXYZ(
                        vertices[i].getX(),
                        vertices[i].getY() * cos( rx ) + vertices[i].getZ() * sin( rx ),
                        vertices[i].getZ() * cos( rx ) - vertices[i].getY() * sin( rx )
                );
                vertices[i].setXYZ(
                        vertices[i].getX() * cos( ry ) - vertices[i].getZ() * sin( ry ),
                        vertices[i].getY(),
                        vertices[i].getX() * sin( ry ) + vertices[i].getZ() * cos( ry )
                );
                vertices[i].setXYZ(
                        vertices[i].getX() * cos( rz ) + vertices[i].getY() * sin( rz ),
                        vertices[i].getY() * cos( rz ) - vertices[i].getX() * sin( rz ),
                        vertices[i].getZ()
                );
        }
        
        // Translate back to position
        translate( accX, accY, accZ );
        
        updateSkin();
}

void
COME_Mesh :: anchorToMolecules( COME_MoleculesTissue* substract, int anchorNumber ){

        printf( "GO ANCHOR? Molec=%d Anchor=%d \n", substract->getShape()->size(), anchorNumber );
        if( substract->getShape()->size() >= anchorNumber ){
                printf( "Anchoring... " );
                // For every vertex, find the "anchorNumber" nearest molecules
                for( int i = 0; i < vertices.size(); i++ ){     
                        list<COME_Molecule*>::iterator iter;
                        /*
                        for (iter = substract->getShape()->begin(); iter != substract->getShape()->end(); iter++ ){                     
                                if( (*iter)->getConnectionList()->size() >= 3 ){
                                        
                                        
                                        if( vertices[i].anchors.size() < anchorNumber ){
                                                vertices[i].addAnchor( (*iter) );
                                        } else {
                                                //changeAnchorIfNear((*iter),anchorNumber, i);

                                                for( int contaMenor = 0; contaMenor < vertices[i].anchors.size(); contaMenor++ ){

                                                        if(     vertices[i].anchors[contaMenor]->getPosition().vpDistance( vertices[i] ) > (*iter)->getPosition().vpDistance( vertices[i] ) ){
                                                                vertices[i].anchors[contaMenor] = (*iter);
                                                                break;
                                                        }
                                                }
                                        }

                                } else {
                                        printf( "An anchor has been found which does not have at least 3 connections. Ignoring that molecule as an anchor.\n" );
                                        //return;
                                }
                        }*/
                        COME_Molecule* anchor1, *anchor2, *anchor3, *anchor4;
                        //iter = substract->getShape()->begin();
                        //anchor1 = (*iter); iter++;
                        //anchor2 = (*iter); iter++;
                        //anchor3 = (*iter); iter++;//iter++;
                        //anchor4 = (*iter);
                        //anchor1 = anchor2 = anchor3 = anchor4 = substract->getShape()->back();
                        
                        double minDist = INFINITE;
                        for (iter = substract->getShape()->begin(); iter != substract->getShape()->end(); iter++ ){                     
                                if( (*iter)->getConnectionList()->size() >= 3 ){

                                        if( minDist > (*iter)->getPosition().vpDistance( vertices[i] ) ){
                                                minDist = (*iter)->getPosition().vpDistance( vertices[i] );
                                                anchor1 = (*iter);
                                        }
                                } else {
                                        printf( "An anchor has been found which does not have at least 3 connections. Ignoring that molecule as an anchor.\n" );
                                }
                        }
                        minDist = INFINITE;
                        for (iter = substract->getShape()->begin(); iter != substract->getShape()->end(); iter++ ){                     
                                if( (*iter) != anchor1 )
                                if( (*iter)->getConnectionList()->size() >= 3 ){

                                        if( minDist > (*iter)->getPosition().vpDistance( vertices[i] ) ){
                                                minDist = (*iter)->getPosition().vpDistance( vertices[i] );
                                                anchor2 = (*iter);
                                        }
                                } else {
                                        printf( "An anchor has been found which does not have at least 3 connections. Ignoring that molecule as an anchor.\n" );
                                }
                        }
                        minDist = INFINITE;
                        for (iter = substract->getShape()->begin(); iter != substract->getShape()->end(); iter++ ){                     
                                if( ( (*iter) != anchor1 ) && ( (*iter) != anchor2 ) )
                                if( (*iter)->getConnectionList()->size() >= 3 ){

                                        if( minDist > (*iter)->getPosition().vpDistance( vertices[i] ) ){
                                                minDist = (*iter)->getPosition().vpDistance( vertices[i] );
                                                anchor3 = (*iter);
                                        }
                                } else {
                                        printf( "An anchor has been found which does not have at least 3 connections. Ignoring that molecule as an anchor.\n" );
                                }
                        }
                        minDist = INFINITE;
                        for (iter = substract->getShape()->begin(); iter != substract->getShape()->end(); iter++ ){                     
                                if( ( (*iter) != anchor1 ) && ( (*iter) != anchor2 ) && ( (*iter) != anchor3 ) )
                                if( (*iter)->getConnectionList()->size() >= 3 ){

                                        if( minDist > (*iter)->getPosition().vpDistance( vertices[i] ) ){
                                                minDist = (*iter)->getPosition().vpDistance( vertices[i] );
                                                anchor4 = (*iter);
                                        }
                                } else {
                                        printf( "An anchor has been found which does not have at least 3 connections. Ignoring that molecule as an anchor.\n" );
                                }
                        }

                        vertices[i].addAnchor( anchor1 );
                        vertices[i].addAnchor( anchor2 );
                        vertices[i].addAnchor( anchor3 );
                        vertices[i].addAnchor( anchor4 );
                        //printf( "anchor of %d ar at %f %f %f \n %f %f %f - %f %f %f \n", i, vertices[i].x, vertices[i].y, vertices[i].z, anchor1->getPosition().x, anchor1->getPosition().y, anchor1->getPosition().z, anchor4->getPosition().x, anchor4->getPosition().y, anchor4->getPosition().z );
/*
                        verticesGlobal[i].addAnchor( anchor1 );
                        verticesGlobal[i].addAnchor( anchor2 );
                        verticesGlobal[i].addAnchor( anchor3 );
                        verticesGlobal[i].addAnchor( anchor4 );
*/                      
                }
        
                initClosestNeighbour();
                printf( "done.\n" );
        }
        
        // Set buois
        for( int iv = 0; iv < vertices.size(); iv++ ){
                for( int a = 0; a < vertices[iv].getAnchors().size(); a++ ){
                        vertices[iv].anchors[a]->addBuoy( &(verticesGlobal[iv]), iv );
                }

        }
        /*
        // Testar aqui se as bóias foram setadas para as moleculas do substrato
        list<COME_Molecule*>::iterator iter;
        for( iter = substract->getShape()->begin(); iter != substract->getShape()->end(); iter++ ){
                
                vector<COME_Vertex3D*> *boias = (*iter)->getBuois();
                for( int b = 0; b < boias->size(); b++ ){
                        printf( "%s: %f %f %f\n", substract->getParent()->getDescription().c_str(), (*boias)[b]->getX(), (*boias)[b]->getY(), (*boias)[b]->getZ() );
                }
                printf( "=======next molec=====\n" );
                 
        }
        printf( "==============end organ============\n" );
        */
}

//used by anchorToMolecules
void  
COME_Mesh :: changeAnchorIfNear( COME_Molecule* molecule, int anchorNumber, int vert ){

                int maxDistantAnchor = findDistantAnchor(vert,anchorNumber);                    
                double actualDistance = molecule->getPosition().vpDistance(vertices[vert]);
                double maxDistance =  vertices[vert].anchors[maxDistantAnchor]->getPosition().vpDistance(vertices[vert]) ;
                
                //printf( "%f %f \n", actualDistance, maxDistance );

                if( actualDistance < maxDistance){
                        vertices[vert].anchors[maxDistantAnchor] = molecule;
                }
                                        
}

//returns the index of the most distant anchor to a given vertex
int 
COME_Mesh :: findDistantAnchor(int indVertex, int anchorNumber){
        
        double maxDistance=0;
        double actualDistance=0;
        int result = 0;

        for( int j = 0; j < anchorNumber; j++ ){                
                actualDistance = vertices[indVertex].anchors[j]->getPosition().vpDistance( vertices[indVertex] );
                        if (actualDistance > maxDistance){
                                maxDistance = actualDistance;
                                result = j;
                        }               
         }
        return result;
}
//Initialises (finds and stores) the closest Anchor-molecule
void 
COME_Mesh :: initClosestNeighbour(){
        
        for (int f= 0 ; f < faces.size(); f++){
                int closestVertex = 0;
                int closestMolecule = 0;
                double actualDistance = 0;              
                vector <COME_Vertex3D> faceVertices = faces[f].getVertices();
                double closestDistance = faceVertices[closestVertex].anchors[closestMolecule]->getPosition().vpDistance(faceVertices[closestVertex]);
                for (int i= 0 ; i < faceVertices.size(); i++){

                        //printf( "face %d vert %d is at %f %f %f \n", f, i, faceVertices[i].x, faceVertices[i].y, faceVertices[i].z );
                        //vector <COME_Molecule*> molecules = faceVertices[i].anchors;

                        for (int j =0 ; j < faceVertices[i].anchors.size(); j++){                               
                                actualDistance = faceVertices[i].anchors[j]->getPosition().vpDistance(faceVertices[i]);
                                if (actualDistance < closestDistance){
                                        closestDistance = actualDistance;
                                        closestVertex = i;
                                        closestMolecule = j;
                                }
                        }

                }
                faces[f].setNearestMolecule(faceVertices[closestVertex].anchors[closestMolecule]);
        }
}
//calculates the center of gravity of the mesh
COME_Vertex3D 
COME_Mesh:: calcGravityPoint(vector<COME_Vertex3D> vertices){

        int n=vertices.size();
        COME_Vertex3D gravityCenter;
        
        for(int i=0;i<n;i++){           
                gravityCenter.x+=vertices[i].x;
                gravityCenter.y+=vertices[i].y;
                gravityCenter.z+=vertices[i].z;                 
        }
        gravityCenter.x /= n;
        gravityCenter.y /= n;
        gravityCenter.z /= n;
        return gravityCenter;
}

//calculates the area of a Face
double 
COME_Mesh::calcAreaOfFace(vector<COME_Vertex3D> verticesFace){

        double a,b,c,S;                         
        a=sqrt(pow((verticesFace[1].x-verticesFace[2].x),2)+pow((verticesFace[1].y-verticesFace[2].y),2)+pow((verticesFace[1].z-verticesFace[2].z),2));
        b=sqrt(pow((verticesFace[0].x-verticesFace[2].x),2)+pow((verticesFace[0].y-verticesFace[2].y),2)+pow((verticesFace[0].z-verticesFace[2].z),2));
        c=sqrt(pow((verticesFace[1].x-verticesFace[0].x),2)+pow((verticesFace[1].y-verticesFace[0].y),2)+pow((verticesFace[1].z-verticesFace[0].z),2));
        S=(a+b+c)/2.0;
        return sqrt(S*(S-a)*(S-b)*(S-c));
}

//Self made absolute 
double myAbs (double x) {
  return x > 0 ? x : -x;
}

//calaculates the distance between the face and the vertex given in parameter
//it is assumed that the normals are up to date
double                          
COME_Mesh :: distanceVertexFace( COME_Vertex3D vertex, vector<COME_Vertex3D> verticesFace){

        COME_Vertex3D A = verticesFace[0];
        COME_Vector3D v1;v1.setVector3D( verticesFace[0].getPoint3D() - verticesFace[1].getPoint3D());
        COME_Vector3D v2;v2.setVector3D( verticesFace[1].getPoint3D() - verticesFace[2].getPoint3D() );
        COME_Vector3D v3 = v1 * v2;
        v3.vpNormalize();
        COME_Vector3D normal = COME_Vector3D( v3 );
        return myAbs(normal.x*(vertex.x-A.x)+normal.y*(vertex.y-A.y)+normal.z*(vertex.z-A.z))/normal.vpModule() ;       
}

//calculates the volume of the hole mesh in local frame
double 
COME_Mesh :: calcVolume(){

        double volume=0.0;
        vector<COME_Vertex3D> verticesGlobal = getVerticesGlobal();
        COME_Vertex3D G = calcGravityPoint(verticesGlobal);
        vector<COME_Vertex3D> verticesFace;
        for( int i = 0; i < faces.size(); i++ ){
                COME_Vertex3D one = verticesGlobal[faces[i].getIndexVertex(0)];
                COME_Vertex3D two = verticesGlobal[faces[i].getIndexVertex(1)];
                COME_Vertex3D three = verticesGlobal[faces[i].getIndexVertex(2)];               
                verticesFace.push_back(one);
                verticesFace.push_back(two);
                verticesFace.push_back(three);
                volume+=calcAreaOfFace(verticesFace)*distanceVertexFace(G, verticesFace)/3;
                verticesFace.clear();
        }
        return volume;
}

void
COME_Mesh::initVerticesNeighbours(){
                
        printf( "verticesGlobal: %d ; faces: %d\n", verticesGlobal.size(), faces.size() );
        for(int i = 0; i < verticesGlobal.size(); i++ ){
                
                for( int j = 0; j < faces.size(); j++ ){

                        if( ( faces[j].getIndexVertex(0) == i ) ||
                                ( faces[j].getIndexVertex(1) == i ) ||
                                ( faces[j].getIndexVertex(2) == i ) ){
                                verticesGlobal[i].addNeighbour(j, this );
                        }
                }               
        }               
}



void
COME_Mesh::initFacesNeighbours(){
                        
        for( int a = 0; a < faces.size(); a++ ){                                
                //The indexes of the current face's vertices
                vector <int> actualVertices = faces[a].getIndexVertices();
                
                for ( int b = 0; b < actualVertices.size() ; b++){
                        
                        for( int c = 0; c < faces.size(); c++ ){

                                if(a != c){
                                        vector<int> verticesOfTempFace = faces[c].getIndexVertices();
                                        for (int d = 0; d < verticesOfTempFace.size(); d++){
                        
                                                if (actualVertices[b] == verticesOfTempFace[d]){ 
                                                        faces[a].addNeighbour(c);
                                                        //printf( "FN added: %d\n", c );
                                                }
                                        }
                                }
                        }
                }
        }
}


void
COME_Mesh::reverse(char* data,int size) {

  int i;
  char temp;
  for (i=0;i<size/2;i++)
  {
    temp=data[i];
    data[i]=data[size-i-1];
    data[size-i-1]=temp;
  }
}

// quick and dirty bitmap loader...for 24 bit bitmaps with 1 plane only.  
// See http://www.dcs.ed.ac.uk/~mxr/gfx/2d/BMP.txt for more info.
int
COME_Mesh::loadImage( string filename ) {

    FILE *file;
    unsigned long size;                 // size of the image in bytes.
    unsigned long i;                    // standard counter.
    unsigned short int planes;          // number of planes in image (must be 1) 
    unsigned short int bpp;             // number of bits per pixel (must be 24)
    char temp;                          // temporary color storage for bgr-rgb conversion.
    char k[4];
    
    // allocate space for texture
    texture =  (Image *) malloc(sizeof(Image));
    if (texture == NULL) {
        printf("Error allocating space for texture image");
        exit(0);
    }
    
    // make sure the file is there.
    if ((file = fopen(filename.c_str(), "rb"))==NULL)
    {
        printf("File Not Found : %s\n",filename.c_str());
        return 0;
    }
    
    // seek through the bmp header, up to the width/height:
    fseek(file, 18, SEEK_CUR);

    // read the width
    if ((i = fread(k, 4, 1, file)) != 1) {
        printf("Error reading width from %s.\n", filename.c_str());
        return 0;
    }

    //reverse(k,4);
    texture->sizeX=*(int*)k;
    

   
    
    printf("Width of %s: %lu\n", filename.c_str(), texture->sizeX);
    
    // read the height 
    if ((i = fread(k, 4, 1, file)) != 1) {
        printf("Error reading height from %s.\n", filename.c_str());
        return 0;
    }

    //reverse(k,4);
    texture->sizeY=*(int*)k;

    printf("Height of %s: %lu\n", filename.c_str(), texture->sizeY);
    
    // calculate the size (assuming 24 bits or 3 bytes per pixel).
    size = texture->sizeX * texture->sizeY * 3;

    // read the planes
    if ((fread(k, 2, 1, file)) != 1) {
        printf("Error reading planes from %s.\n", filename.c_str());
        return 0;
    }
   
    reverse(k,2);
    planes=*(int*)k;
    planes=1; // :)


    if (planes != 1) {
        printf("Planes from %s is not 1: %u\n", filename.c_str(), planes);
        return 0;
    }

    // read the bpp
    if ((i = fread(&bpp, 2, 1, file)) != 1) {
        printf("Error reading bpp from %s.\n", filename.c_str());
        return 0;
    }
    bpp=24;
    if (bpp != 24) {
        printf("Bpp from %s is not 24: %u\n", filename.c_str(), bpp);
        return 0;
    }
        
    // seek past the rest of the bitmap header.
    fseek(file, 24, SEEK_CUR);

    // read the data. 
    texture->data = (char *) malloc(size);
    if (texture->data == NULL) {
        printf("Error allocating memory for color-corrected image data");
        return 0;       
    }

    if ((i = fread(texture->data, size, 1, file)) != 1) {
        printf("Error reading image data from %s.\n", filename.c_str());
        return 0;
    }

    for (i=0;i<size;i+=3) { // reverse all of the colors. (bgr -> rgb)
        temp = texture->data[i];
        texture->data[i] = texture->data[i+2];
        texture->data[i+2] = temp;
    }
    
    // we're done.
    textureID = textureCount++;
    return 1;
}

Image *
COME_Mesh::getTexture(){

        return texture;
}

unsigned int*
COME_Mesh::getTextureID(){

        return &textureID;
}

---