00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #include <sofa/component/forcefield/TensorForceField.h>
00026 #include <sofa/core/ObjectFactory.h>
00027 #include <sofa/helper/system/gl.h>
00028 #include <fstream>
00029 #include <iostream>
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
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
00072 std::ifstream in(filename);
00073
00074
00075
00076
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
00083 in >> alpha_;
00084
00085
00086 std::string filename2;
00087 in >> filename2;
00088
00089 in.close();
00090 std::ifstream input(filename2.c_str());
00091
00092
00093 unsigned int nbVertices,nbTetrahedra;
00094 unsigned int dummyUInt;
00095 input >> nbTetrahedra >> nbVertices;
00096
00097 for (int i = 0; i < 15; ++i)
00098 input >> dummyUInt;
00099
00100
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
00109 --vertexTetrahedronTable[i][j];
00110 }
00111 }
00112
00113
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
00121 input.close();
00122
00123
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
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
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
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
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
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
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
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
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
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
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
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
00246 VecDeriv& f = *object_->getF();
00247 const VecCoord& p = *object_->getX();
00248 const VecDeriv& v = *object_->getV();
00249
00250 f.resize( p.size() );
00251
00252
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
00261
00262
00263
00264
00265
00266
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
00282
00283
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
00322 VecDeriv& f = *object_->getF();
00323
00324 const VecCoord& p = *object_->getDx();
00325 const VecDeriv& v = *object_->getV();
00326
00327 f.resize( p.size() );
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
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
00359
00360
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
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
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
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
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
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
00510 si[0] = tetra.triangleShapeVector[i][0];
00511 si[1] = tetra.triangleShapeVector[i][1];
00512 si[2] = tetra.triangleShapeVector[i][2];
00513
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
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
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
00535
00536
00537 t[k][l] /= 36.0f * tetra.restVolume;
00538 }
00539 }
00540
00541
00542 if (i==j) {
00543
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
00552
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
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>;
00600 #endif
00601
00602 }
00603
00604 }
00605
00606 }
00607