TensorForceField.cpp

Go to the documentation of this file.
00001 /******************************************************************************
00002 *       SOFA, Simulation Open-Framework Architecture, version 1.0 beta 3      *
00003 *                (c) 2006-2008 MGH, INRIA, USTL, UJF, CNRS                    *
00004 *                                                                             *
00005 * This library is free software; you can redistribute it and/or modify it     *
00006 * under the terms of the GNU Lesser General Public License as published by    *
00007 * the Free Software Foundation; either version 2.1 of the License, or (at     *
00008 * your option) any later version.                                             *
00009 *                                                                             *
00010 * This library is distributed in the hope that it will be useful, but WITHOUT *
00011 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
00012 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
00013 * for more details.                                                           *
00014 *                                                                             *
00015 * You should have received a copy of the GNU Lesser General Public License    *
00016 * along with this library; if not, write to the Free Software Foundation,     *
00017 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *
00018 *******************************************************************************
00019 *                               SOFA :: Modules                               *
00020 *                                                                             *
00021 * Authors: The SOFA Team and external contributors (see Authors.txt)          *
00022 *                                                                             *
00023 * Contact information: contact@sofa-framework.org                             *
00024 ******************************************************************************/
00025 #include <sofa/component/forcefield/TensorForceField.h>
00026 #include <sofa/core/ObjectFactory.h>
00027 #include <sofa/helper/system/gl.h>
00028 #include <fstream> // for reading the file
00029 #include <iostream> //for debugging
00030 #include <sofa/defaulttype/Vec3Types.h>
00031 
00032 using std::cerr;
00033 using std::endl;
00034 
00035 namespace sofa
00036 {
00037 
00038 namespace component
00039 {
00040 
00041 namespace forcefield
00042 {
00043 
00044 //dunnno how it was defined, but it works anyway...
00045 static unsigned int vertexEdge[4][4]={{0,0,1,2,},{0,0,3,4},{1,3,0,5},{2,4,5,0}};
00046 
00047 
00048 
00049 template <class DataTypes>
00050 TensorForceField<DataTypes>::TensorForceField(const char* filename) {
00051   load(filename);
00052   initialize();
00053 }
00054 
00055 
00056 
00057 template <class DataTypes>
00058 TensorForceField<DataTypes>::TensorForceField(
00059   component::MechanicalObject<DataTypes>* object, const char* filename
00060 )
00061   : object_(object), alpha_(0.0), lambda_(2.80e5), mu_(3.1e4) {
00062     load(filename);
00063     initialize();
00064 }
00065 
00066 
00067 
00068 template<class DataTypes>
00069 void
00070 TensorForceField<DataTypes>::load(const char *filename) {
00071   // opening the wrapping file
00072   std::ifstream in(filename);
00073 
00074   // read the young Modulues E and  the Poisson coefficient nu and compute the
00075   // lambda and mu lame coefficients from them
00076   // TODO : change this to E and nu.
00077   Real E, nu;
00078   in >> E >> nu;
00079   lambda_ = E * nu / ((1 + nu) * (1 - 2 * nu));
00080   mu_ = E / (2 * (1 + nu));
00081 
00082   // Read the damping factor alpha
00083   in >> alpha_;
00084             
00085   // read the name of the file containing the forcefield geometry
00086   std::string filename2;
00087   in >> filename2;
00088   // we have all we need, so close the wrapping file
00089   in.close();
00090   std::ifstream input(filename2.c_str());
00091 
00092   // read nb vertices and tetrahedra
00093   unsigned int nbVertices,nbTetrahedra;
00094     unsigned int dummyUInt;
00095     input >> nbTetrahedra >> nbVertices;
00096   // skips next 15 integers
00097     for (int i = 0; i < 15; ++i)
00098         input >> dummyUInt;
00099     
00100     // read tetrahedra
00101     unsigned int **vertexTetrahedronTable = new unsigned int * [nbTetrahedra];
00102     for (unsigned int i = 0; i < nbTetrahedra; ++i)
00103         vertexTetrahedronTable[i] = new unsigned int [4];
00104 
00105   for (unsigned int i = 0; i < nbTetrahedra; ++i) {
00106         for (int j = 0; j < 4; ++j) {
00107             input >> vertexTetrahedronTable[i][j];
00108       // the file indices are 1-based, so we convert them to a 0-base
00109             --vertexTetrahedronTable[i][j];
00110         }
00111     }
00112     
00113     // read vertices
00114     Real px, py, pz;
00115     for (unsigned int i = 0; i < nbVertices; ++i) {
00116         input >> px >> py >> pz;
00117         vertex_.push_back( Coord(px, py, pz) );
00118     }
00119 
00120   // we have read all we needed, so close the geometry file
00121   input.close();
00122   
00123   // defining tetrahedra
00124     for (unsigned int i = 0; i < nbTetrahedra; ++i) {
00125         Tetrahedron tetra;
00126         
00127     tetra.vertex[0] = vertexTetrahedronTable[i][0];
00128         tetra.vertex[1] = vertexTetrahedronTable[i][1];
00129         tetra.vertex[2] = vertexTetrahedronTable[i][2];
00130         tetra.vertex[3] = vertexTetrahedronTable[i][3];
00131 
00132     // defining the edges of the current tetrahedron        
00133         for (int i = 1; i < 4; ++i) {
00134             int v0 = tetra.vertex[i];
00135             for (int j = 0; j < i; ++j) {
00136                 int v1 = tetra.vertex[j];
00137                 int e = getEdge(v0, v1);
00138                 tetra.edge[ vertexEdge[i][j] ] = e;
00139             }
00140         }
00141         
00142         //defining the triangles of the current tetrahedron
00143         for (int i = 0; i < 4; ++i) {
00144             int v0 = tetra.vertex[(i+1)%4];
00145             int v1 = tetra.vertex[(i+2)%4];
00146             int v2 = tetra.vertex[(i+3)%4];
00147             int tr = getTriangle(v0,v1,v2);
00148             tetra.triangle[i] = tr;
00149         }
00150         
00151         tetra.index = tetrahedron_.size();
00152         tetrahedron_.push_back(tetra);
00153     
00154     }
00155 
00156   // freeing allocated ressources
00157   for (unsigned int i = 0; i < nbTetrahedra; ++i)
00158     delete vertexTetrahedronTable[i];
00159   delete vertexTetrahedronTable;
00160   
00161     return;           
00162 }
00163 
00164 
00165 
00166 template<class DataTypes>
00167 void
00168 TensorForceField<DataTypes>::initialize() {
00169     
00170   // setting all tensors to null
00171     
00172   for (unsigned int i = 0; i < vertex_.size(); ++i) {
00173         vertexTensor_.push_back(VertexTensor());
00174         vertexTensor_[i].resetToNull();
00175     }
00176 
00177     for (unsigned int i = 0; i < edge_.size(); ++i) {
00178         edgeTensor_.push_back(EdgeTensor());
00179         edgeTensor_[i].resetToNull();
00180     }
00181     
00182     // computing rest volume and triangles shapeVectors for all tetrahedrons
00183     for (unsigned int i = 0; i < tetrahedron_.size(); ++i) {
00184         Real a[3] = {vertex_[ tetrahedron_[i].vertex[0] ][0],
00185                      vertex_[ tetrahedron_[i].vertex[0] ][1],
00186                      vertex_[ tetrahedron_[i].vertex[0] ][2]};
00187         Real b[3] = {vertex_[ tetrahedron_[i].vertex[1] ][0],
00188                      vertex_[ tetrahedron_[i].vertex[1] ][1],
00189                      vertex_[ tetrahedron_[i].vertex[1] ][2]};
00190         Real c[3] = {vertex_[ tetrahedron_[i].vertex[2] ][0],
00191                      vertex_[ tetrahedron_[i].vertex[2] ][1],
00192                      vertex_[ tetrahedron_[i].vertex[2] ][2]};
00193         Real d[3] = {vertex_[ tetrahedron_[i].vertex[3] ][0],
00194                      vertex_[ tetrahedron_[i].vertex[3] ][1],
00195                      vertex_[ tetrahedron_[i].vertex[3] ][2]};
00196     
00197         Real ab[3] = {b[0] - a[0], b[1] - a[1], b[2] - a[2]};
00198         Real ac[3] = {c[0] - a[0], c[1] - a[1], c[2] - a[2]};
00199         Real ca[3] = {a[0] - c[0], a[1] - c[1], a[2] - c[2]};
00200         Real ad[3] = {d[0] - a[0], d[1] - a[1], d[2] - a[2]};
00201         Real bc[3] = {c[0] - b[0], c[1] - b[1], c[2] - b[2]};
00202         Real cb[3] = {b[0] - c[0], b[1] - c[1], b[2] - c[2]};
00203         Real bd[3] = {d[0] - b[0], d[1] - b[1], d[2] - b[2]};
00204         
00205     // restVolume = dot(cross(ab, ac), ad) / 6.0
00206     tetrahedron_[i].restVolume = ((ab[1] * ac[2] - ab[2] * ac[1]) * ad[0] +
00207                                   (ab[2] * ac[0] - ab[0] * ac[2]) * ad[1] +
00208                                   (ab[0] * ac[1] - ab[1] * ac[0]) * ad[2]) /
00209                                  6.0f;
00210 
00211     // tetrahedron_[i].triangleShapeVector[0] = cross (bc, bd)
00212     tetrahedron_[i].triangleShapeVector[0][0] = bc[1] * bd[2] - bc[2] * bd[1];
00213     tetrahedron_[i].triangleShapeVector[0][1] = bc[2] * bd[0] - bc[0] * bd[2];
00214     tetrahedron_[i].triangleShapeVector[0][2] = bc[0] * bd[1] - bc[1] * bd[0];
00215 
00216     // tetrahedron_[i].triangleShapeVector[0] = cross (ad, ac)
00217     tetrahedron_[i].triangleShapeVector[1][0] = ad[1] * ac[2] - ad[2] * ac[1];
00218     tetrahedron_[i].triangleShapeVector[1][1] = ad[2] * ac[0] - ad[0] * ac[2];
00219     tetrahedron_[i].triangleShapeVector[1][2] = ad[0] * ac[1] - ad[1] * ac[0];
00220 
00221     // tetrahedron_[i].triangleShapeVector[0] = cross (ab, ad)
00222     tetrahedron_[i].triangleShapeVector[2][0] = ab[1] * ad[2] - ab[2] * ad[1];
00223     tetrahedron_[i].triangleShapeVector[2][1] = ab[2] * ad[0] - ab[0] * ad[2];
00224     tetrahedron_[i].triangleShapeVector[2][2] = ab[0] * ad[1] - ab[1] * ad[0];
00225 
00226     // tetrahedron_[i].triangleShapeVector[0] = cross (cb, ca)
00227     tetrahedron_[i].triangleShapeVector[0][0] = cb[1] * ca[2] - cb[2] * ca[1];
00228     tetrahedron_[i].triangleShapeVector[0][1] = cb[2] * ca[0] - cb[0] * ca[2];
00229     tetrahedron_[i].triangleShapeVector[0][2] = cb[0] * ca[1] - cb[1] * ca[0];
00230 
00231         
00232     }
00233   
00234     // initializing elastic tensors;
00235     for (unsigned int i = 0; i < tetrahedron_.size(); ++i)
00236         addElasticTensors( tetrahedron_[i] );
00237         
00238 }
00239 
00240 
00241 
00242 template<class DataTypes>
00243 void 
00244 TensorForceField<DataTypes>::addForce () {
00245     // getting the containing mechanical object's data
00246     VecDeriv& f = *object_->getF();
00247     const VecCoord& p = *object_->getX();
00248     const VecDeriv& v = *object_->getV();
00249     
00250     f.resize( p.size() ); // ??really needed??
00251 
00252   // computing difference between current position and rest position
00253   sofa::helper::vector< Coord > pos;
00254     for (unsigned int i = 0; i < p.size(); ++i ) {
00255     Coord dif = p[i] - vertex_[i];
00256         pos.push_back(dif);
00257     }
00258     
00259     
00260   // la force au point i est egale a la matrice de raideur (tenseur) au
00261   // point i * le deplacement du point i par rapport a sa position de repos plus
00262   // la somme des matrices de raideur sur chacune des arretes partant du point i
00263   // * le deplacement du point a l'autre bout de l'arrete.
00264 
00265   // Adding vertices' contribution.
00266   // TODO Check for sign correctness
00267     for (unsigned int i = 0; i < vertex_.size(); ++i) {
00268     f[i][0] -= vertexTensor_[i].tensor[0][0] * (pos[i][0] + alpha_ * v[i][0]) +
00269                vertexTensor_[i].tensor[0][1] * (pos[i][1] + alpha_ * v[i][1]) +
00270                vertexTensor_[i].tensor[0][2] * (pos[i][2] + alpha_ * v[i][2]);
00271                
00272     f[i][1] -= vertexTensor_[i].tensor[1][0] * (pos[i][0] + alpha_ * v[i][0]) +
00273                vertexTensor_[i].tensor[1][1] * (pos[i][1] + alpha_ * v[i][1]) +
00274                vertexTensor_[i].tensor[1][2] * (pos[i][2] + alpha_ * v[i][2]);
00275                
00276     f[i][2] -= vertexTensor_[i].tensor[2][0] * (pos[i][0] + alpha_ * v[i][0]) +
00277                vertexTensor_[i].tensor[2][1] * (pos[i][1] + alpha_ * v[i][1]) +
00278                vertexTensor_[i].tensor[2][2] * (pos[i][2] + alpha_ * v[i][2]);
00279     }
00280     
00281     // Adding edges' contribution.
00282     // TODO Check for sign correctness
00283   // TODO Check for transposition correctness
00284     for (unsigned int i = 0; i < edge_.size(); ++i) {
00285     int v0 = edge_[i].vertex[0];
00286     int v1 = edge_[i].vertex[1];
00287     
00288     f[v0][0] -= edgeTensor_[i].tensor[0][0] * (pos[v1][0] + alpha_ * v[v1][0]) +
00289                 edgeTensor_[i].tensor[1][0] * (pos[v1][1] + alpha_ * v[v1][1]) +
00290                 edgeTensor_[i].tensor[2][0] * (pos[v1][2] + alpha_ * v[v1][2]);
00291 
00292     f[v0][1] -= edgeTensor_[i].tensor[0][1] * (pos[v1][0] + alpha_ * v[v1][0]) +
00293                 edgeTensor_[i].tensor[1][1] * (pos[v1][1] + alpha_ * v[v1][1]) +
00294                 edgeTensor_[i].tensor[2][1] * (pos[v1][2] + alpha_ * v[v1][2]);
00295 
00296     f[v0][2] -= edgeTensor_[i].tensor[0][2] * (pos[v1][0] + alpha_ * v[v1][0]) +
00297                 edgeTensor_[i].tensor[1][2] * (pos[v1][1] + alpha_ * v[v1][1]) +
00298                 edgeTensor_[i].tensor[2][2] * (pos[v1][2] + alpha_ * v[v1][2]);
00299 
00300     f[v1][0] -= edgeTensor_[i].tensor[0][0] * (pos[v0][0] + alpha_ * v[v0][0]) +
00301                 edgeTensor_[i].tensor[0][1] * (pos[v0][1] + alpha_ * v[v0][1]) +
00302                 edgeTensor_[i].tensor[0][2] * (pos[v0][2] + alpha_ * v[v0][2]);
00303                 
00304     f[v1][1] -= edgeTensor_[i].tensor[1][0] * (pos[v0][0] + alpha_ * v[v0][0]) +
00305                 edgeTensor_[i].tensor[1][1] * (pos[v0][1] + alpha_ * v[v0][1]) +
00306                 edgeTensor_[i].tensor[1][2] * (pos[v0][2] + alpha_ * v[v0][2]);
00307 
00308     f[v1][2] -= edgeTensor_[i].tensor[2][0] * (pos[v0][0] + alpha_ * v[v0][0]) +
00309                 edgeTensor_[i].tensor[2][1] * (pos[v0][1] + alpha_ * v[v0][1]) +
00310                 edgeTensor_[i].tensor[2][2] * (pos[v0][2] + alpha_ * v[v0][2]);
00311 
00312   }
00313 
00314 }
00315 
00316 
00317 
00318 template<class DataTypes>
00319 void
00320 TensorForceField<DataTypes>::addDForce() {
00321   // getting the containing mechanical object's data
00322   VecDeriv& f = *object_->getF();
00323   // use Dx instead of X
00324   const VecCoord& p = *object_->getDx();
00325   const VecDeriv& v = *object_->getV();
00326   
00327   f.resize( p.size() ); // ??really needed??
00328 
00329   // computing difference between current position and rest position
00330   /*sofa::helper::vector< Coord > pos;
00331   for (unsigned int i = 0; i < p.size(); ++i ) {
00332     Coord dif = p[i] - vertex_[i];
00333     pos.push_back(dif);
00334   }*/
00335   
00336   
00337   // la force au point i est egale a la matrice de raideur (tenseur) au
00338   // point i * le deplacement du point i par rapport a sa position de repos plus
00339   // la somme des matrices de raideur sur chacune des arretes partant du point i
00340   // * le deplacement du point a l'autre bout de l'arrete.
00341 
00342   // Adding vertices' contribution.
00343   // TODO Check for sign correctness
00344   for (unsigned int i = 0; i < vertex_.size(); ++i) {
00345     f[i][0] -= vertexTensor_[i].tensor[0][0] * (p[i][0] + alpha_ * v[i][0]) +
00346                vertexTensor_[i].tensor[0][1] * (p[i][1] + alpha_ * v[i][1]) +
00347                vertexTensor_[i].tensor[0][2] * (p[i][2] + alpha_ * v[i][2]);
00348                
00349     f[i][1] -= vertexTensor_[i].tensor[1][0] * (p[i][0] + alpha_ * v[i][0]) +
00350                vertexTensor_[i].tensor[1][1] * (p[i][1] + alpha_ * v[i][1]) +
00351                vertexTensor_[i].tensor[1][2] * (p[i][2] + alpha_ * v[i][2]);
00352                
00353     f[i][2] -= vertexTensor_[i].tensor[2][0] * (p[i][0] + alpha_ * v[i][0]) +
00354                vertexTensor_[i].tensor[2][1] * (p[i][1] + alpha_ * v[i][1]) +
00355                vertexTensor_[i].tensor[2][2] * (p[i][2] + alpha_ * v[i][2]);
00356   }
00357   
00358   // Adding edges' contribution.
00359   // TODO Check for sign correctness
00360   // TODO Check for transposition correctness
00361   for (unsigned int i = 0; i < edge_.size(); ++i) {
00362     int v0 = edge_[i].vertex[0];
00363     int v1 = edge_[i].vertex[1];
00364     
00365     f[v0][0] -= edgeTensor_[i].tensor[0][0] * (p[v1][0] + alpha_ * v[v1][0]) +
00366                 edgeTensor_[i].tensor[1][0] * (p[v1][1] + alpha_ * v[v1][1]) +
00367                 edgeTensor_[i].tensor[2][0] * (p[v1][2] + alpha_ * v[v1][2]);
00368 
00369     f[v0][1] -= edgeTensor_[i].tensor[0][1] * (p[v1][0] + alpha_ * v[v1][0]) +
00370                 edgeTensor_[i].tensor[1][1] * (p[v1][1] + alpha_ * v[v1][1]) +
00371                 edgeTensor_[i].tensor[2][1] * (p[v1][2] + alpha_ * v[v1][2]);
00372 
00373     f[v0][2] -= edgeTensor_[i].tensor[0][2] * (p[v1][0] + alpha_ * v[v1][0]) +
00374                 edgeTensor_[i].tensor[1][2] * (p[v1][1] + alpha_ * v[v1][1]) +
00375                 edgeTensor_[i].tensor[2][2] * (p[v1][2] + alpha_ * v[v1][2]);
00376 
00377     f[v1][0] -= edgeTensor_[i].tensor[0][0] * (p[v0][0] + alpha_ * v[v0][0]) +
00378                 edgeTensor_[i].tensor[0][1] * (p[v0][1] + alpha_ * v[v0][1]) +
00379                 edgeTensor_[i].tensor[0][2] * (p[v0][2] + alpha_ * v[v0][2]);
00380                 
00381     f[v1][1] -= edgeTensor_[i].tensor[1][0] * (p[v0][0] + alpha_ * v[v0][0]) +
00382                 edgeTensor_[i].tensor[1][1] * (p[v0][1] + alpha_ * v[v0][1]) +
00383                 edgeTensor_[i].tensor[1][2] * (p[v0][2] + alpha_ * v[v0][2]);
00384 
00385     f[v1][2] -= edgeTensor_[i].tensor[2][0] * (p[v0][0] + alpha_ * v[v0][0]) +
00386                 edgeTensor_[i].tensor[2][1] * (p[v0][1] + alpha_ * v[v0][1]) +
00387                 edgeTensor_[i].tensor[2][2] * (p[v0][2] + alpha_ * v[v0][2]);
00388 
00389   }
00390 
00391 }
00392 
00393 template <class DataTypes> 
00394         double TensorForceField<DataTypes>::getPotentialEnergy()
00395 {
00396     cerr<<"TensorForceField::getPotentialEnergy-not-implemented !!!"<<endl;
00397     return 0;
00398 }
00399 
00400 
00401 template<class DataTypes>
00402 void TensorForceField<DataTypes>::draw() {
00403     if (!getContext()->getShowForceFields()) return;
00404     const VecCoord& p1 = *object_->getX();
00405     glDisable(GL_LIGHTING);
00406     glColor4f(1,1,1,1);
00407     glBegin(GL_LINES);
00408     for (unsigned int i = 0; i < edge_.size(); ++i) {
00409     glVertex3d(p1[edge_[i].vertex[0]][0],
00410                p1[edge_[i].vertex[0]][1],
00411                p1[edge_[i].vertex[0]][2]);
00412  
00413     glVertex3d(p1[edge_[i].vertex[1]][0],
00414                p1[edge_[i].vertex[1]][1],
00415                p1[edge_[i].vertex[1]][2]);
00416     }
00417     glEnd();
00418 }
00419 
00420 
00421 template<class DataTypes>
00422 void TensorForceField<DataTypes>::initTextures() {
00423 
00424 }
00425 
00426 
00427 template<class DataTypes>
00428 void TensorForceField<DataTypes>::update() {
00429 
00430 }
00431 
00432 
00433 // search for the edge connecting the given vertices, create it if not found
00434 template<class DataTypes>
00435 int 
00436 TensorForceField<DataTypes>::getEdge(const int v0, const int v1) {
00437   for (unsigned int i = 0; i < edge_.size(); ++i) {
00438     if (
00439       (edge_[i].vertex[0] == v0 && edge_[i].vertex[1] == v1) ||
00440       (edge_[i].vertex[0] == v1 && edge_[i].vertex[1] == v0)
00441     )
00442       return i;
00443   }
00444   
00445   // edge wasn't found, we have to create it
00446   Edge e;
00447   
00448   e.index = edge_.size();
00449   
00450   e.vertex[0] = v0;
00451   e.vertex[1] = v1;
00452   
00453   edge_.push_back(e);
00454   
00455   return e.index;
00456 }
00457 
00458 
00459 
00460 // search for the triangle connecting the given vertices, create it if not found
00461 template<class DataTypes>
00462 int 
00463 TensorForceField<DataTypes>::getTriangle(const int v0, const int v1,
00464                                              const int v2) {
00465   for (unsigned int i = 0; i < triangle_.size(); ++i) {
00466     int tv0 = triangle_[i].vertex[0];
00467     int tv1 = triangle_[i].vertex[1];
00468     int tv2 = triangle_[i].vertex[2];
00469 
00470     if (
00471       (tv0 == v0 && tv1 == v1 && tv2 == v2) ||
00472       (tv0 == v0 && tv1 == v2 && tv2 == v1) ||
00473       (tv0 == v1 && tv1 == v0 && tv2 == v2) ||
00474       (tv0 == v1 && tv1 == v2 && tv2 == v0) ||
00475       (tv0 == v2 && tv1 == v0 && tv2 == v1) ||
00476       (tv0 == v2 && tv1 == v1 && tv2 == v0)
00477     )
00478     return i;
00479   }
00480   
00481   // triangle wasn't found, we have to create it
00482   Triangle t;
00483   
00484   t.index = triangle_.size();
00485   
00486   t.vertex[0] = v0;
00487   t.vertex[1] = v1;
00488   t.vertex[2] = v2;
00489   
00490   triangle_.push_back(t);
00491   
00492   return t.index;
00493   
00494 }
00495 
00496 
00497 
00498 // add the elastic tensors for the given tetrahedron
00499 template<class DataTypes>
00500 void TensorForceField<DataTypes>::addElasticTensors(Tetrahedron& tetra) {
00501   Real si[3];
00502   Real sj[3];
00503   
00504   Real t[3][3];
00505   Real id[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
00506   
00507   for (int i = 0; i < 4; ++i) {
00508     for (int j = i; j < 4; ++j) {
00509       // si is the shape vector of triangle[i]
00510       si[0] = tetra.triangleShapeVector[i][0];
00511       si[1] = tetra.triangleShapeVector[i][1];
00512       si[2] = tetra.triangleShapeVector[i][2];
00513       // sj is the shape vector of triangle[j]
00514       sj[0] = tetra.triangleShapeVector[j][0];
00515       sj[1] = tetra.triangleShapeVector[j][1];
00516       sj[2] = tetra.triangleShapeVector[j][2];
00517 
00518       Real dot = (si[0] * sj[0]) + (si[1] * sj[1]) + (si[2] * sj[2]);
00519       
00520       // mij is the tensor product of si sj
00521       Real mij[3][3]       = {{si[0] * sj[0], si[0] * sj[1], si[0] * sj[2]},
00522                                 {si[1] * sj[0], si[1] * sj[1], si[1] * sj[2]},
00523                                 {si[2] * sj[0], si[2] * sj[1], si[2] * sj[2]}};
00524                               
00525       Real mijTransp[3][3] = {{si[0] * sj[0], si[1] * sj[0], si[2] * sj[0]},
00526                                 {si[0] * sj[1], si[1] * sj[1], si[2] * sj[1]},
00527                                 {si[0] * sj[2], si[1] * sj[2], si[2] * sj[2]}};
00528       
00529       // t is the edge or vertex tensor
00530       for (int k = 0; k < 3; ++k) {
00531         for (int l = 0; l < 3; ++l) {
00532           t[k][l] = lambda_ * mij[k][l] +
00533                     mu_ * ( mijTransp[k][l] + dot * id[k][l] );
00534           // divide by (6.0 * volume)^2 to get the real shape vectors in the
00535           // products ( (x) -> ^2) and multiplied by volume from the volumetric
00536           // integration
00537           t[k][l] /= 36.0f * tetra.restVolume;
00538         }
00539       }
00540       
00541       // add t to the tensor stored in edges or vertices
00542       if (i==j) {
00543         // t is the tensor for the vertex
00544         for (int k = 0; k < 3; ++k) {
00545           for (int l = 0; l < 3; ++l) {
00546             vertexTensor_[tetra.vertex[i]].tensor[k][l] += t[k][l];
00547           }
00548         }
00549       }
00550       else {
00551         // t (or its transposed, depending on the tetrahedron orientation) is
00552         // the tensor for the edge.
00553         if ( edge_[ tetra.edge[ vertexEdge[i][j] ] ].vertex[0] ==
00554              tetra.vertex[i] ) {
00555           for (int k = 0; k < 3; ++k) {
00556             for (int l = 0; l < 3; ++l) {
00557               edgeTensor_[tetra.edge[vertexEdge[i][j]]].tensor[k][l]+= t[k][l];
00558             }
00559           }
00560         }
00561         else {
00562           for (int k = 0; k < 3; ++k) {
00563             for (int l = 0; l < 3; ++l) {
00564               edgeTensor_[tetra.edge[vertexEdge[i][j]]].tensor[k][l]+= t[l][k];
00565             }
00566           }
00567         }
00568       }
00569     }
00570   }
00571 }
00572 
00573 
00574 
00575 //--- the following seems to be needed for factory registering
00576 
00577 
00578 SOFA_DECL_CLASS(TensorForceField)
00579 
00580 using namespace sofa::defaulttype;
00581 
00582 
00583 template<class DataTypes>
00584 void create(TensorForceField<DataTypes>*& obj,
00585             simulation::tree::xml::ObjectDescription* arg) {
00586     simulation::tree::xml::createWithParentAndFilename<
00587     TensorForceField<DataTypes>, component::MechanicalObject<DataTypes>
00588   > (obj, arg);
00589 }
00590 
00591 #ifndef SOFA_FLOAT
00592 Creator<simulation::tree::xml::ObjectFactory, TensorForceField<Vec3dTypes> >
00593   TensorForceFieldVec3dClass("TensorForceField", true);
00594 template class TensorForceField<Vec3dTypes>;
00595 #endif
00596 #ifndef SOFA_DOUBLE
00597 Creator<simulation::tree::xml::ObjectFactory, TensorForceField<Vec3fTypes> >
00598   TensorForceFieldVec3fClass("TensorForceField", true);
00599 template class TensorForceField<Vec3fTypes>; // doesn't work for now
00600 #endif
00601 
00602 } // namespace  forcefield
00603 
00604 } // namespace  component
00605 
00606 } // namespace sofa
00607 

Generated on Sun Nov 23 03:11:51 2008 for Sofa by  doxygen 1.5.7.1