Biomechanical Joint Model  Author: Anderson Maciel

---

comejoint.cpp

Go to the documentation of this file.

/***************************************************************************
 *   Copyright (C) 2000 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        <stdio.h>

#include        <algebra/comematrix.h>
#include        <kinematics/comejoint.h>
#include        <kinematics/comedof.h>

COME_Joint::COME_Joint(){

        //position = COME_Point3D();
        vpRefreshInvLim();
        //shapeList = NULL;
        parentJoint = NULL;
        //childList = NULL;
        numDofs = 0;
        dofList = new COME_Dof*[MAXDOF];
                                for( int i = 0; i < MAXDOF; dofList[i++] = NULL );
}

//COME_Joint::~COME_Joint(){

//}

void
COME_Joint::setDescription( char* szDesc ){
        description = szDesc;
}

void
COME_Joint::setDescription( string sDesc ){
        description = sDesc;
}

string
COME_Joint::getDescription(){
        return description;
}

unsigned short int
COME_Joint::getNumDofs(){
        return numDofs;
}

void
COME_Joint::setChanged( bool yesno ){
        changed = yesno;
}

bool
COME_Joint::vpIsChanged( void ){
        return changed;
}
                

COME_Vector3D
COME_Joint::getVectorX(){

        COME_Vector3D *v = new COME_Vector3D( lim.getValueAt(0,0),
                                                                                                                                        lim.getValueAt(1,0),
                                                                                                                                        lim.getValueAt(2,0) );
        return *v;
}

COME_Vector3D
COME_Joint::getVectorY(){

        COME_Vector3D *v = new COME_Vector3D( lim.getValueAt(0,1),
                                                                                                                                        lim.getValueAt(1,1),
                                                                                                                                        lim.getValueAt(2,1) );
        return *v;
}

COME_Vector3D
COME_Joint::getVectorZ(){

        COME_Vector3D *v = new COME_Vector3D( lim.getValueAt(0,2),
                                                                                                                                        lim.getValueAt(1,2),
                                                                                                                                        lim.getValueAt(2,2) );
        return *v;
}

COME_Joint*
COME_Joint::getParent(){

        return parentJoint;
}

list<COME_Joint*>
COME_Joint::getChildList(){

        return childList;
}

list<COME_Joint*>*
COME_Joint::getChildListPt(){

        return &childList;
}

list<COME_BioStructure*>
COME_Joint::getShapeList(){

        return shapeList;
}

list<COME_BioStructure*>*
COME_Joint::getShapeListPt(){

        return &shapeList;
}

COME_Dof**
COME_Joint::getDofList(){

        return dofList;
}

COME_Matrix
COME_Joint::getLim(){

        return lim;
}

COME_Matrix
COME_Joint::getInvLim(){

        return invLim;
}

void
COME_Joint::setNumDofs( unsigned short int n ){
        numDofs = n;
}


void
COME_Joint::setParent( COME_Joint* j ){

        parentJoint = j;
}

void
COME_Joint::setChildList( list<COME_Joint*> j ){

        childList = j;
}

void
COME_Joint::setLim( COME_Matrix m ){

        lim = m;
}

void
COME_Joint::setInvLim( COME_Matrix m ){

        invLim = m;
}


void
COME_Joint::vpRefreshInvLim( void ){


        setInvLim( getLim().getInverse() );
}

void
COME_Joint::vpAddShape( COME_BioStructure *shape ){

        shapeList.push_back( shape );
}

void
COME_Joint::vpAddChild( COME_Joint *child ){

        //childList.push_back( child );
        childList.push_front( child );
}

void
COME_Joint::vpAddDof( COME_Dof *dof ){

        if( !dofList ) dofList = new COME_Dof*[MAXDOF];
        int ind = -1;
        while( dofList[++ind] );
        if( ind <= MAXDOF ){
                dofList[ind] = dof;
                numDofs++;
        }
}

void
COME_Joint::vpMakeLim( void ){
        
        COME_Matrix newLim;
                
        for( int        curDof = 0; curDof < getNumDofs(); curDof++ ){
                newLim = newLim * dofList[curDof]->getLim();// * newLim;
        }
                
        setLim( newLim );
        vpRefreshInvLim();
        
        // Set joint as changed
        setChanged( true );
}

void
COME_Joint::vpMakeGims( COME_Matrix composite ){
        
        COME_Matrix     newMatrix = getLim() * composite;
        
        list<COME_BioStructure*>::iterator shapeIter;
        for( shapeIter = shapeList.begin(); shapeIter != shapeList.end(); shapeIter++ ){
                COME_Matrix localLIM = (*shapeIter)->getLIM();
                (*shapeIter)->setGIM( localLIM * newMatrix );
        }
        
        list<COME_Joint*>::iterator jointIter;
        for( jointIter = childList.begin(); jointIter != childList.end(); jointIter++ ){
                (*jointIter)->vpMakeGims( newMatrix );
        }
}

void
COME_Joint::vpPrintLim( void ){
        printf( "%f %f %f %f \n",getLim().getValueAt(0,0), getLim().getValueAt(0,1),
                                                                                                getLim().getValueAt(0,2), getLim().getValueAt(0,3) );
  printf( "%f %f %f %f \n",getLim().getValueAt(1,0), getLim().getValueAt(1,1),
                                                                                                getLim().getValueAt(1,2), getLim().getValueAt(1,3) );
        printf( "%f %f %f %f \n",getLim().getValueAt(2,0), getLim().getValueAt(2,1),
                                                                                                getLim().getValueAt(2,2), getLim().getValueAt(2,3) );
        printf( "%f %f %f %f \n\n",getLim().getValueAt(3,0), getLim().getValueAt(3,1),
                                                                                                getLim().getValueAt(3,2), getLim().getValueAt(3,3) );
}

---