COME - Biomodel

00001 /***************************************************************************
00002  *   Copyright (C) 2000 by Anderson Maciel                                 *
00003  *   andi.maciel@gmail.com                                                 *
00004  *                                                                         *
00005  *   This program is free software; you can redistribute it and/or modify  *
00006  *   it under the terms of the GNU General Public License as published by  *
00007  *   the Free Software Foundation; either version 2 of the License, or     *
00008  *   (at your option) any later version.                                   *
00009  *                                                                         *
00010  *   This program is distributed in the hope that it will be useful,       *
00011  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00012  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
00013  *   GNU General Public License for more details.                          *
00014  *                                                                         *
00015  *   You should have received a copy of the GNU General Public License     *
00016  *   along with this program; if not, write to the                         *
00017  *   Free Software Foundation, Inc.,                                       *
00018  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
00019  **************************************************************************/
00020 
00021 #include        <stdio.h>
00022 #include        <math.h>
00023 
00024 #include        <kinematics/comedof.h>
00025 #include        <kinematics/comejoint.h>
00026 #include        <kinematics/comemodifier.h>
00027 
00028 COME_Dof::COME_Dof( void ){
00029 
00030         axis = COME_Vector3D( 0, 0, 1 );
00031         evoluta = new COME_Bezier();
00032         position = COME_Point3D( 0, 0, 0 );
00033         lim = COME_Matrix();
00034         minAngle = 0;
00035         maxAngle = 0;
00036         comfortMinAngle = 0;
00037         comfortMaxAngle = 0;
00038         currentPosition = 0;
00039         restPosition = 0;
00040         rangeModifier = NULL;
00041 }
00042 
00043 COME_Dof::COME_Dof( COME_Vector3D za, COME_Point3D pos, COME_Bezier *evol ){
00044 
00045         axis = za;
00046         evoluta = evol;
00047         position = pos;
00048         lim = COME_Matrix();
00049         minAngle = 0;
00050         maxAngle = 0;
00051         comfortMinAngle = 0;
00052         comfortMaxAngle = 0;
00053         currentPosition = 0;
00054         restPosition = 0;
00055         rangeModifier = NULL;
00056 }
00057 
00058 COME_Dof::COME_Dof( COME_Vector3D za, COME_Point3D pos, COME_Curve *evol, float min, float max, float cur, float rest ){
00059         
00060         axis = za;
00061         evoluta = (COME_Bezier *)evol;
00062         position = pos;
00063         lim = COME_Matrix();
00064         minAngle = min;
00065         maxAngle = max;
00066         comfortMinAngle = 0;
00067         comfortMaxAngle = 0;
00068         currentPosition = cur;
00069         restPosition = rest;
00070         rangeModifier = NULL;
00071 }
00072 
00073 COME_Dof::COME_Dof( COME_Vector3D za, COME_Point3D pos, COME_Bezier *evol, float min, float max, float cur, float rest, COME_Modifier *rm ){
00074         
00075         axis = za;
00076         evoluta = evol;
00077         position = pos;
00078         lim = COME_Matrix();
00079         minAngle = min;
00080         maxAngle = max;
00081         comfortMinAngle = 0;
00082         comfortMaxAngle = 0;
00083         currentPosition = cur;
00084         restPosition = rest;
00085         rangeModifier = rm;
00086 }
00087 
00088 void
00089 COME_Dof::setDescription( char* szDesc ){
00090         description = szDesc;
00091 }
00092 
00093 void
00094 COME_Dof::setDescription( string sDesc ){
00095         description = sDesc;
00096 }
00097 
00098 string
00099 COME_Dof::getDescription(){
00100         return description;
00101 }
00102 
00103 COME_Vector3D
00104 COME_Dof::getAxis( void ){
00105         
00106         return axis;
00107 }
00108 
00109 COME_Point3D
00110 COME_Dof::getOrigin( void ){
00111         
00112         COME_Point3D p;
00113         p = lim * p;
00114         return p;
00115 }
00116 
00117 COME_Point3D
00118 COME_Dof::getPosition( void ){
00119         
00120         return position;
00121 }
00122 
00123 COME_Curve
00124 COME_Dof::getEvoluta( void ){
00125         
00126         return *evoluta;
00127 }
00128 
00129 COME_Matrix
00130 COME_Dof::getLim( void ){
00131         
00132         return lim;
00133 }
00134 
00135 COME_Matrix
00136 COME_Dof::getBim( void ){
00137         
00138         
00140         axis.vpNormalize();
00141         COME_Vector3D vx = COME_Vector3D( 1, 0, 0 );
00142         COME_Vector3D vy = axis.vpCrossProduct(vx);
00143         // Verify linearity
00144         if( vy == COME_Vector3D( 0, 0, 0 ) ){
00145                 vx = COME_Vector3D( 0, 0, 1 );
00146                 vy = axis.vpCrossProduct(vx);
00147         }
00148         vy.vpNormalize();
00149         vx = vy.vpCrossProduct(axis);
00150         vx.vpNormalize();
00151         double                  fm[4][4];
00152 
00153         fm[0][0] = vx.getX();
00154         fm[1][0] = vx.getY();
00155         fm[2][0] = vx.getZ();
00156         fm[3][0] = 0;
00157         
00158         fm[0][1] = vy.getX();
00159         fm[1][1] = vy.getY();
00160         fm[2][1] = vy.getZ();
00161         fm[3][1] = 0;
00162         
00163         fm[0][2] = axis.getX();
00164         fm[1][2] = axis.getY();
00165         fm[2][2] = axis.getZ();
00166         fm[3][2] = 0;
00167         
00168         fm[0][3] = 0;
00169         fm[1][3] = 0;
00170         fm[2][3] = 0;
00171         fm[3][3] = 1;
00172         
00173         COME_Matrix lBim = COME_Matrix( fm );
00174         return lBim;
00175 }
00176 
00177 float
00178 COME_Dof::getMin( void ){
00179         
00180         return minAngle;
00181 }
00182 
00183 float
00184 COME_Dof::getMax( void ){
00185         
00186         return maxAngle;
00187 }
00188 
00189 float
00190 COME_Dof::getComfortMin( void ){
00191         
00192         return comfortMinAngle;
00193 }
00194 
00195 float
00196 COME_Dof::getComfortMax( void ){
00197         
00198         return comfortMaxAngle;
00199 }
00200 
00201 float
00202 COME_Dof::getCurrentMin( void ){
00203         
00204         float minModif = minAngle;
00205         if( rangeModifier ){
00206                 minModif = rangeModifier->getMin();
00207         }
00208         if( minAngle < minModif ){
00209                 return minModif;
00210         }
00211         return minAngle;
00212 }
00213 
00214 float
00215 COME_Dof::getCurrentMax( void ){
00216         
00217         float maxModif = maxAngle;
00218         if( rangeModifier ){
00219                 maxModif = rangeModifier->getMax();
00220         }
00221         if( maxAngle > maxModif ){
00222                 return maxModif;
00223         }
00224         return maxAngle;
00225 }
00226 
00227 float
00228 COME_Dof::getCurrent( void ){
00229         
00230         return currentPosition;
00231 }
00232 
00233 float
00234 COME_Dof::getRest( void ){
00235         
00236         return restPosition;
00237 }
00238 
00239 COME_Joint*
00240 COME_Dof::getOwnerJoint( void ){
00241         
00242         return ownerJoint;
00243 }
00244 
00245 void
00246 COME_Dof::setAxis( COME_Vector3D za ){
00247         
00248         axis = za;
00249 }
00250 
00251 void
00252 COME_Dof::setEvoluta( COME_Bezier *evol ){
00253         
00254         evoluta = evol;
00255 }
00256 
00257 void
00258 COME_Dof::setLim( COME_Matrix m ){
00259         
00260         lim = m;
00261 }
00262 
00263 void
00264 COME_Dof::setMin( float min ){
00265         
00266         minAngle = min;
00267 }
00268 
00269 void
00270 COME_Dof::setMax( float max ){
00271         
00272         maxAngle = max;
00273 }
00274 
00275 void
00276 COME_Dof::setComfortMax( float max ){
00277         
00278         comfortMaxAngle = max;
00279 }
00280 
00281 void
00282 COME_Dof::setComfortMin( float min ){
00283         
00284         comfortMinAngle = min;
00285 }
00286 
00287 void
00288 COME_Dof::vpMoveTo( float pos ){
00289         
00290         // Avoid values out of range
00291         if( pos > 1.0 ) pos = 1.0;
00292         if( pos < 0.0 ) pos = 0.0;
00293         
00294         // Get resources (obtain angles and points by interpolation)
00295         COME_Point3D tgtpos = evoluta->getPointAsPoint( pos ); //target position
00296         float tgtang = getCurrentMin() + ( ( getCurrentMax() - getCurrentMin() ) * pos ); //target angle
00297         
00298         COME_Point3D p = position + COME_Point3D( tgtpos );
00299                 
00300         // Build transformation matrix
00301         double r[4][4];
00302         r[0][0] = cos( tgtang );
00303         r[0][1] = sin( tgtang );
00304         r[1][0] = -sin( tgtang );
00305         r[1][1] = cos( tgtang );
00306         r[3][0] = p.getX();
00307         r[3][1] = p.getY();
00308         r[3][2] = p.getZ();
00309         r[2][2] = r[3][3] = 1;
00310         r[0][2] = r[1][2] = r[2][0] = r[2][1] = r[0][3] = r[1][3] = r[2][3] = 0;
00311         COME_Matrix mt = COME_Matrix( r );
00312         
00313         // update to new position
00314         // lim = bim * T * inv(bim)
00315         //setLim( (getBim() * mt ) * ( getBim().getTransposed() ) );
00316         //setLim( ( getBim().getTransposed() ) * (getBim() * mt ) );
00317         setLim( ( getBim().getTransposed() ) * ( mt * getBim() ) );
00318         currentPosition = pos;
00319 }
00320 
00321 
00322 void
00323 COME_Dof::setOwnerJoint( COME_Joint *ow ){
00324         ownerJoint = ow;
00325 }
00326 
00327 void
00328 COME_Dof::setRest( float rest ){
00329         
00330         restPosition = rest;
00331 }
00332 
00333 void
00334 COME_Dof::vpRest( void ){
00335         
00336         vpMoveTo( restPosition );
00337 }
00338 
00339 void
00340 COME_Dof::setRangeModifier( COME_Modifier *m ){
00341         rangeModifier = m;
00342 }
00343 
00344 COME_Modifier*
00345 COME_Dof::getRangeModifier( void ){
00346         return rangeModifier;
00347 }
00348 
00349 
00350 void
00351 COME_Dof::vpPrintLim( void ){
00352         printf( "%f %f %f %f \n",getLim().getValueAt(0,0), getLim().getValueAt(0,1),
00353                                                                                                 getLim().getValueAt(0,2), getLim().getValueAt(0,3) );
00354   printf( "%f %f %f %f \n",getLim().getValueAt(1,0), getLim().getValueAt(1,1),
00355                                                                                                 getLim().getValueAt(1,2), getLim().getValueAt(1,3) );
00356         printf( "%f %f %f %f \n",getLim().getValueAt(2,0), getLim().getValueAt(2,1),
00357                                                                                                 getLim().getValueAt(2,2), getLim().getValueAt(2,3) );
00358         printf( "%f %f %f %f \n\n",getLim().getValueAt(3,0), getLim().getValueAt(3,1),
00359                                                                                                 getLim().getValueAt(3,2), getLim().getValueAt(3,3) );
00360 }
comedof.cpp
