Biomechanical Joint Model  Author: Anderson Maciel

---

comemoleculescartilage.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.             *
 **************************************************************************/

//
//  PROJECT.....: CO-ME
//  RESPONSIBLE.: 
//
//  AUTHOR......: Anderson Maciel
//  DATE........: August/05/2002
//  DESCRIPTION.: Class declaration.
//


#include <bio/comemoleculescartilage.h>
#include <bio/comematerial.h>
#include <math.h>
#include <time.h>
#include <algebra/comemesh.h>
#include <algebra/comematrix.h>
#include <libgeom/geom.h>

//#include <LinAlg/linalg.h>
//using namespace LinAlg;
//#include <MarchingCubes/marching_cubes.h>
//#include <MarchingCubes/blob.h>
using namespace geom;

COME_MoleculesCartilage::COME_MoleculesCartilage(){

        tissue = new COME_MoleculesTissue();
        tissue->setParent(this);
        surface = NULL;
        hashTable = NULL;
        globalHooke = 0.0;
        collisionRadius = 1.0;
}

COME_MoleculesCartilage::~COME_MoleculesCartilage(){

        
}

void
COME_MoleculesCartilage::setTissue( COME_MoleculesTissue *tissueN ){

        tissue = tissueN;
        tissue->setParent(this);
}

void
COME_MoleculesCartilage::setDeformationFile( string defFile ){

        deformationFile = defFile;

}

string
COME_MoleculesCartilage::getDeformationFile(){

        return deformationFile;
}

COME_MoleculesTissue*
COME_MoleculesCartilage::getTissue(){

        return tissue;
}

ARRAY2D*
COME_MoleculesCartilage::getMatrixJinv(){

        return matrixJinv;
}

//Recalculates the Mesh based on the new positions of anchoring the molecules
void 
COME_MoleculesCartilage:: updateSkin(){
        if( surface )
                surface->updateSkin();
}

void
COME_MoleculesCartilage::updateSurface( bool regenerate ){

        // Parameter of the potential filed
        //     small values (<1.0) -> implicit surface is big and mesh doesn't follow the form of the spheres
        //     big values   (>1.0) -> surface follows the form of the spheres
        
        // Mesh resouluton[0.1 high resolution mesh generated, 2.0 very corse mesh generated]
        
        if( regenerate == IMPLICIT ){
                COME_Point3D mins, maxs;
                getEnvelop( mins, maxs );
                double volume = ( maxs.getX() - mins.getX() ) * ( maxs.getY() - mins.getY() ) * ( maxs.getZ() - mins.getZ() );
                double radiusAvg = tissue->getRadiusAverage();

                updateSurface( 1.0, 1/(radiusAvg/2), sqrt( volume )/20.0 + 0.005 );
                surface->estabilishLinkVerticesFaces();

        } else {
                surface->update();
        }
  
}


void
COME_MoleculesCartilage::updateSurface( double skinSphere, double stiff, double resolution ){

/*  ///commented in 01 september 2004 because of link errors
        printf( "update IMPLICIT CARTILAGE Is IN \n" );
        // Metaballs represented as the spheres with Raquel's density function
        MetaballsSphere *metaSphere = new MetaballsSphere( skinSphere );
        
        vector<Primitive*>                      primitives;
        VEC                                                     C(3);           // sphere center

        list<COME_Molecule*>::iterator iter;    
        for( iter = tissue->getShape()->begin(); iter != tissue->getShape()->end(); iter++ ){
                //read spheres
                COME_Point3D point      = (*iter)->getPosition();

                C[0] = point.getX();
                C[1] = point.getY();
                C[2] = point.getZ();
                
                //add sphere  
                metaSphere->addSphere( Sphere( C, (*iter)->getRadius(), stiff ) );
        }
  
        //add to primitives
        primitives.push_back( metaSphere );

        //create surface
        marchingCubes surfaceTriangles( primitives, resolution );

        // copy vertices and triangle indices into the Class Mesh       
        
        int iPos;

        vector<VEC>     vertexList      = surfaceTriangles.getVertices();
        vector<VEC> normalList  = surfaceTriangles.getNormals();

        // copy all vertices and the normals of the triangulated surface 
        surface->resetMesh();
        for ( iPos = 0; iPos < vertexList.size(); iPos++ ){
                
                COME_Vector3D normal = COME_Vector3D( normalList[ iPos ][ 0 ], normalList[ iPos ][ 1 ], normalList[ iPos ][ 2 ] );
                surface->addVertex( COME_Vertex3D( vertexList[ iPos ][ 0 ], vertexList[ iPos ][ 1 ], vertexList[ iPos ][ 2 ], normal ) );
        }

        //copy all indices of vertices 
        vector<Triangle>        trianglesList   = surfaceTriangles.getTriangles();
        for (vector<Triangle>::const_iterator tri=trianglesList.begin(); tri!=trianglesList.end(); tri++){     
                
                vector<int>                     indicesTriangles;

                indicesTriangles.push_back( tri->indices[0] );
                indicesTriangles.push_back( tri->indices[1] );
                indicesTriangles.push_back( tri->indices[2] );

                surface->addFaceAsIndices( indicesTriangles );
        }
*/
}

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

        tissue->getEnvelop( mins, maxs );
}

bool
COME_MoleculesCartilage::update( double timestep, double simClock ){

        if( COME::flagNumIntegration == RUNGE_KUTTA4 ){
                if( tissue->update( timestep, simClock, deformationFile ) ){
                        updateSkin(); 
                        return true;
                }
        } else {
                if( COME::flagNumIntegration == EULER )
                if( tissue->updateEuler( timestep, simClock, deformationFile ) ){
                        updateSkin(); 
                        return true;
                }
        }
        return false;
}


void
COME_MoleculesCartilage::initializeSkinning(){

        if( this->getSurface() == NULL ){
                surface = new COME_Mesh();
                surface->setParent( this );
                updateSurface( IMPLICIT );
                printf( "update IMPLICIT CARTILAGE done \n" );
                return;
        }
                
        this->getSurface()->anchorToMolecules( this->getTissue(), 4 );
        
        vector<COME_Vertex3D>* localVertices = surface->getVerticesPt();
        for( int i = 0; i < localVertices->size(); i++ ){

                vector<COME_Molecule*> localAnchors = (*localVertices)[i].getAnchors();
                for( int j = 0; j < localAnchors.size(); j++ ){

                        localAnchors[j]->initializeOrthogonalNeighbors();
                
                        //(*localVertices)[i].addLocalPosition( (*localVertices)[i] - localAnchors[j]->getPosition() );
                        COME_Matrix MX_INVERSE = localAnchors[j]->getLocalFrame()->getInverse();
                        (*localVertices)[i].addLocalPosition( MX_INVERSE * (*localVertices)[i] );

                }
        }
        cout << "initialising Faces Neighbours...";
        this->getPointerToSurface()->initFacesNeighbours();
        cout << "done \ninitialising Vertices Neighbours...";
        this->getPointerToSurface()->initVerticesNeighbours();
        cout << "done\n";
        
        //printf( "initializing jacobian... " );
        //initializeJacobian();
        //printf( "done.\n" );
}

void
COME_MoleculesCartilage::discretize( int type, double dist, double radius ){

        // Reset tissue
        if( tissue ){
                delete tissue;
        }
        tissue = new COME_MoleculesTissue();
        tissue->setParent(this);
        
        if( type == BEST ){

                COME_Point3D mins, maxs;
                COME_Material *discrMaterial = new COME_Material( COME_Vector3D( 1.0, 1.0, 1.0 ), 1.0, 0.3, 1000.0, 1.0, 0.0, 0.0  );
                COME_Material *fixedMaterial = new COME_Material( COME_Vector3D( 1.0, 0.4, 0.4 ), 1.0, 0.3, 1000.0, 1.0, 0.0, 0.0  );
                surface->getEnvelop( mins, maxs );
                
                // Marchig grid to create surface balls
                vector<COME_Point3D> *allIntersections = new vector<COME_Point3D>;
                for( double xMgrid = mins.getX(); xMgrid <= maxs.getX(); xMgrid += dist ){
                        for( double yMgrid = mins.getY(); yMgrid <= maxs.getY(); yMgrid += dist ){
                                surface->findIntersections( COME_Point3D( xMgrid, yMgrid, mins.getZ() ), COME_Point3D( xMgrid, yMgrid, maxs.getZ() ), allIntersections );
                        }
                        for( double zMgrid = mins.getZ(); zMgrid <= maxs.getZ(); zMgrid += dist ){
                                surface->findIntersections( COME_Point3D( xMgrid, mins.getY(), zMgrid ), COME_Point3D( xMgrid, maxs.getY(), zMgrid ), allIntersections );
                        }
                }
                for( double zMgrid = mins.getZ(); zMgrid <= maxs.getZ(); zMgrid += dist ){
                        for( double yMgrid = mins.getY(); yMgrid <= maxs.getY(); yMgrid += dist ){
                                surface->findIntersections( COME_Point3D( mins.getX(), yMgrid, zMgrid ), COME_Point3D( maxs.getX(), yMgrid, zMgrid ), allIntersections );
                        }
                }
                printf( "%d intersections found on the surface.\n", allIntersections->size() );
                
                // Add molecules on surface points (they are fixed)
                for( int i = 0; i < allIntersections->size(); i++ ){
                        if( ! tissue->isTooClose( (*allIntersections)[i], dist/10 ) ){
                        
                                COME_Molecule *molecule = new COME_Molecule( (*allIntersections)[i], (dist/2.0)+(dist/5.0), 0.02, fixedMaterial, COME_Vector3D(), (COME_BioStructure *)this );
                                molecule->setFixed( true );
                                tissue->addMolecule( molecule );
                        }
                }
                delete allIntersections;
                printf( "%d molecules added on the surface.\n", tissue->getShape()->size() );
                
                // Marchig cubes to create internal balls
                for( double xMcubes = mins.getX(); xMcubes <= maxs.getX(); xMcubes += dist ){
                        for( double yMcubes = mins.getY(); yMcubes <= maxs.getY(); yMcubes += dist ){
                                for( double zMcubes = mins.getZ(); zMcubes <= maxs.getZ(); zMcubes += dist ){
        
                                        COME_Point3D currentPoint( xMcubes, yMcubes, zMcubes );
                                        if( ( surface->isInside( currentPoint ) ) && ( ! tissue->isTooClose( currentPoint, dist/1.5 ) ) ){
                                                
                                                // Attention: Using always the same material
                                                COME_Molecule *molecule = new COME_Molecule( currentPoint, (dist/2.0)+(dist/10.0), 0.02, discrMaterial, COME_Vector3D(), this );
                                                molecule->setFixed( false );
                                                tissue->addMolecule( molecule );
                                        }
                                }
                        }
                }
                printf( "%d molecules have been created for this object.\n", tissue->getShape()->size() );
                list<COME_Molecule*>::iterator iter, dead;
                for (iter = tissue->getShape()->begin(); iter != tissue->getShape()->end(); iter++){
                        // Reconfigure molecules
                        (*iter)->setRadius(radius);
                        // Reconfigure links for pseudo-simulation
                        list<COME_MoleculeLink*>::iterator iterThis;
                        for (iterThis = (*iter)->getConnectionList()->begin(); !( iterThis == (*iter)->getConnectionList()->end() ); iterThis++){
                                (*iterThis)->setNominalDist(dist);
                        }
                }
        }
}

void
COME_MoleculesCartilage::respondCollision(){

//printf( "Entrou cartilage::respondcollision \n" );

        list<COME_Molecule*>::iterator iter;
        for( iter = tissue->getShape()->begin(); iter != tissue->getShape()->end(); iter++ ){
        
                COME_Point3D reseting = (*iter)->getPosition();
                //(*iter)->setINITPosition( reseting );
                if( !(*iter)->isFixed() ){      
                        double sumDists = 0.0;
                        COME_Point3D newPos = (*iter)->getPosition();
                        vector<COME_Vertex3D*> *vertices = (*iter)->getBuois();
                        int i;
                        int count = 1;
                        for( i = 0; i < vertices->size(); i++ ){
                                sumDists += pow( (*vertices)[i]->vpDistance( (*iter)->getPosition() ), 2.0 );
                        }
                        
                        count = i-1;
                        
                        for( i = 0; i < vertices->size(); i++ ){
                                double w = ( 1.0 - ( pow( (*vertices)[i]->vpDistance( (*iter)->getPosition() ), 2.0 ) / sumDists ) ) / (count*2.0);
                                newPos =  newPos + ( (*vertices)[i]->getCollisionDispAvg() * w );
                                
                                //printf("TOTAL: %f AVG: %f\n",(*vertices)[i]->getCollisionDisp().vpModule(), (*vertices)[i]->getCollisionDispAvg().vpModule() );
                        }
                        
                        if( (*iter)->getPosition().vpDistance( newPos ) != 0.0 ){
                                COME_Vector3D zero;
                                (*iter)->setVelocity( zero );
                                (*iter)->setAcceleration( zero );
                                (*iter)->setPosition( newPos );
                        }
                        //(*iter)->setINITPosition( newPos ); //TEMP for testing RESPONSE
                }
        }
}

void
COME_MoleculesCartilage::initializeJacobian(){

        int lines = surface->getVerticesPt()->size();
        int columns = tissue->getShape()->size();
        
        // initialize matrix J
        ARRAY2D *matrixJ = array2d_creation (lines*3,  columns*3);
        matrixJinv = array2d_creation (lines*3,  columns*3);
        array2d_nul( matrixJ );
        //matrixJ = (double*) malloc((unsigned) columns*3*lines*3*sizeof(double)); 
        //for( int initialize=0; initialize < (columns*3*lines*3) ; initialize++ ) matrixJ[initialize] = 0.0;
        
        // Add molecules x vertices relations to the J matrix
        list<COME_Molecule*>::iterator iter;
        iter = tissue->getShape()->begin();
        for( int mi = 0; iter != tissue->getShape()->end(); mi++, iter++ ){
                vector<COME_Vertex3D*>* buois = (*iter)->getBuois();
                vector<int>* bInd = (*iter)->getBuoisIndices();
                
                double sumDists = 0.0;
                for( int i = 0; i < buois->size(); i++ ){
                        sumDists += pow( (*buois)[i]->vpDistance( (*iter)->getGlobalPosition() ), 2.0 );
                }
                for( int bi = 0; bi < buois->size(); bi++ ){
                        
                        double wj = ( 1.0 - ( pow( (*buois)[bi]->vpDistance( (*iter)->getGlobalPosition() ), 2.0 ) / sumDists ) );
                        AIJ(matrixJ, ( (*bInd)[bi] * 3+0 ), ( mi*3+0 ) ) = wj;
                        AIJ(matrixJ, ( (*bInd)[bi] * 3+1 ), ( mi*3+1 ) ) = wj;
                        AIJ(matrixJ, ( (*bInd)[bi] * 3+2 ), ( mi*3+2 ) ) = wj;
                        
                        //matrixJ[ ( (*bInd)[bi] * 3+0 )*(columns*3) + ( mi*3+0 ) ] = wj;
                        //matrixJ[ ( (*bInd)[bi] * 3+1 )*(columns*3) + ( mi*3+1 ) ] = wj;
                        //matrixJ[ ( (*bInd)[bi] * 3+2 )*(columns*3) + ( mi*3+2 ) ] = wj;
                }
        }
        
        FILE    *file = fopen( "/home/amaciel/Desktop/matrixJ.txt", "wt" );
        array2d_write ( matrixJ, file);
        fclose( file );
        
        // Inverse the matrix using libgeom functions
        time_t start,end;
        double dif;
        time (&start);

        array2d_damped_least_square_inverse ( matrixJinv, matrixJ, 0.0 );
        
        time (&end);
        dif = difftime (end,start);
        printf ("It has taken %.2lf seconds to inverse the matrix of %d lines and %d columns.\n", dif, lines*3, columns*3 );
        
        file = fopen( "/home/amaciel/Desktop/matrixJinv.txt", "wt" );
        array2d_write ( matrixJinv, file);
        fclose( file );
}

---