Panda3D
 All Classes Functions Variables Enumerations
physxSoftBodyNode.cxx
00001 // Filename: physxSoftBodyNode.cxx
00002 // Created by:  enn0x (13Sep10)
00003 //
00004 ////////////////////////////////////////////////////////////////////
00005 //
00006 // PANDA 3D SOFTWARE
00007 // Copyright (c) Carnegie Mellon University.  All rights reserved.
00008 //
00009 // All use of this software is subject to the terms of the revised BSD
00010 // license.  You should have received a copy of this license along
00011 // with this source code in a file named "LICENSE."
00012 //
00013 ////////////////////////////////////////////////////////////////////
00014 
00015 #include "physxSoftBodyNode.h"
00016 #include "physxSoftBody.h"
00017 #include "physxFileStream.h"
00018 #include "physxManager.h"
00019 #include "physxMeshHash.h"
00020 
00021 #include "geomVertexFormat.h"
00022 #include "geomVertexWriter.h"
00023 #include "geomVertexRewriter.h"
00024 
00025 TypeHandle PhysxSoftBodyNode::_type_handle;
00026 
00027 ////////////////////////////////////////////////////////////////////
00028 //     Function: PhysxSoftBodyNode::allocate
00029 //       Access: Public
00030 //  Description: 
00031 ////////////////////////////////////////////////////////////////////
00032 void PhysxSoftBodyNode::
00033 allocate(PhysxSoftBody *softbody) {
00034 
00035   _softbody = softbody;
00036 
00037   // Retrieve number of vertices and triangles the hard way
00038   NxSoftBodyMeshDesc meshDesc;
00039   _softbody->ptr()->getSoftBodyMesh()->saveToDesc(meshDesc);
00040 
00041   NxU32 numVertices = meshDesc.numVertices;
00042   NxU32 numTetrahedra = meshDesc.numTetrahedra;
00043 
00044   float factor = 1.0f; // TODO max(1.0f, factor);
00045 
00046   // Reserve more memory for vertices than the initial mesh takes because
00047   // tearing creates new vertices
00048   NxU32 maxVertices = factor * numVertices;
00049   _mesh.verticesPosBegin = (NxVec3 *)malloc(sizeof(NxVec3) * maxVertices);
00050   _mesh.verticesPosByteStride = sizeof(NxVec3);
00051   _mesh.maxVertices = maxVertices;
00052   _mesh.numVerticesPtr = (NxU32 *)malloc(sizeof(NxU32));
00053 
00054   // The number of tetrahedra is constant, even if the softbody is torn
00055   NxU32 maxIndices = 4 * numTetrahedra;
00056   _mesh.indicesBegin = (NxU32 *)malloc(sizeof(NxU32) * maxIndices);
00057   _mesh.indicesByteStride = sizeof(NxU32);
00058   _mesh.maxIndices = maxIndices;
00059   _mesh.numIndicesPtr = (NxU32 *)malloc(sizeof(NxU32));
00060 
00061   *(_mesh.numVerticesPtr) = 0;
00062   *(_mesh.numIndicesPtr) = 0;
00063 
00064   _softbody->ptr()->setMeshData(_mesh);
00065 }
00066 
00067 ////////////////////////////////////////////////////////////////////
00068 //     Function: PhysxSoftBodyNode::set_from_geom
00069 //       Access: Published
00070 //  Description: Reads the vertices and indices from an existing
00071 //               Geom and makes a decomposed copy of the data.
00072 //               Then computes links between the owning soft body
00073 //               tetrahedron mesh in order to render an updated
00074 //               geometry every simulation frame.
00075 ////////////////////////////////////////////////////////////////////
00076 void PhysxSoftBodyNode::
00077 set_from_geom(const Geom *geom) {
00078 
00079   _prim->clear_vertices();
00080 
00081   GeomVertexWriter vwriter = GeomVertexWriter(_vdata, InternalName::get_vertex());
00082   GeomVertexWriter nwriter = GeomVertexWriter(_vdata, InternalName::get_normal());
00083   GeomVertexWriter twriter = GeomVertexWriter(_vdata, InternalName::get_texcoord());
00084 
00085   CPT(GeomVertexData) vdata = geom->get_vertex_data();
00086   GeomVertexReader vreader = GeomVertexReader(vdata, InternalName::get_vertex());
00087   GeomVertexReader nreader = GeomVertexReader(vdata, InternalName::get_normal());
00088   GeomVertexReader treader = GeomVertexReader(vdata, InternalName::get_texcoord());
00089 
00090   while (!vreader.is_at_end()) {
00091     LVecBase3f v = vreader.get_data3f();
00092     vwriter.add_data3f(v.get_x(), v.get_y(), v.get_z());
00093   }
00094 
00095   while (!nreader.is_at_end()) {
00096     LVecBase3f n = nreader.get_data3f();
00097     nwriter.add_data3f(n.get_x(), n.get_y(), n.get_z());
00098   }
00099 
00100   while (!treader.is_at_end()) {
00101     LVecBase2f t = treader.get_data2f();
00102     twriter.add_data2f(t.get_x(), t.get_y());
00103   }
00104 
00105   for (int i=0; i<geom->get_num_primitives(); i++) {
00106 
00107     CPT(GeomPrimitive) prim = geom->get_primitive(i);
00108     prim = prim->decompose();
00109 
00110     for (int j=0; j<prim->get_num_primitives(); j++) {
00111 
00112       int s = prim->get_primitive_start(j);
00113       int e = prim->get_primitive_end(j);
00114 
00115       for (int l=s; l<e; l++) {
00116         _prim->add_vertex(prim->get_vertex(l));
00117       }
00118     }
00119   }
00120 
00121   _prim->close_primitive();
00122 
00123   update_bounds();
00124   build_tetra_links();
00125 }
00126 
00127 ////////////////////////////////////////////////////////////////////
00128 //     Function: PhysxSoftBodyNode::build_tetra_links
00129 //       Access: Public
00130 //  Description:
00131 ////////////////////////////////////////////////////////////////////
00132 void PhysxSoftBodyNode::
00133 build_tetra_links() {
00134 
00135   NxSoftBodyMeshDesc meshDesc;
00136   _softbody->ptr()->getSoftBodyMesh()->saveToDesc(meshDesc);
00137   const NxVec3 *vertices = (const NxVec3 *) meshDesc.vertices;
00138   const NxU32 *indices = (const NxU32 *) meshDesc.tetrahedra;
00139   const NxU32 numTets = meshDesc.numTetrahedra;
00140 
00141   _tetraLinks.clear();
00142 
00143   PhysxMeshHash* hash = new PhysxMeshHash();
00144   hash->set_grid_spacing(_bounds.min.distance(_bounds.max) * 0.1f);
00145 
00146   for (NxU32 i=0; i<numTets; i++) {
00147     const NxU32 *ix = &indices[4*i];
00148     NxBounds3 tetraBounds;
00149     tetraBounds.setEmpty();
00150     tetraBounds.include(vertices[*ix++]);
00151     tetraBounds.include(vertices[*ix++]);
00152     tetraBounds.include(vertices[*ix++]);
00153     tetraBounds.include(vertices[*ix++]);
00154     hash->add(tetraBounds, i);
00155   }
00156 
00157   GeomVertexReader vreader = GeomVertexReader(_vdata, InternalName::get_vertex());
00158 
00159   while (!vreader.is_at_end()) {
00160 
00161     // Prepare datastructure for drained tetras
00162     _drainedTriVertices.push_back(false);
00163 
00164     TetraLink tmpLink;
00165 
00166     LVecBase3f v = vreader.get_data3f();
00167     NxVec3 triVert = PhysxManager::vec3_to_nxVec3(v);
00168     pvector<int> itemIndices;
00169     hash->query_unique(triVert, itemIndices);
00170 
00171     NxReal minDist = 0.0f;
00172     NxVec3 b;
00173     int num, isize;
00174     num = isize = itemIndices.size();
00175     if (num == 0) {
00176         num = numTets;
00177     }
00178 
00179     for (int i=0; i<num; i++) {
00180       int j = i;
00181       if (isize > 0) {
00182           j = itemIndices[i];
00183       }
00184 
00185       const NxU32 *ix = &indices[j*4];
00186       const NxVec3 &p0 = vertices[*ix++];
00187       const NxVec3 &p1 = vertices[*ix++];
00188       const NxVec3 &p2 = vertices[*ix++];
00189       const NxVec3 &p3 = vertices[*ix++];
00190 
00191       NxVec3 b = compute_bary_coords(triVert, p0, p1, p2, p3);
00192 
00193       // Is the vertex inside the tetrahedron? If yes we take it
00194       if (b.x >= 0.0f && b.y >= 0.0f && b.z >= 0.0f && (b.x + b.y + b.z) <= 1.0f) {
00195         tmpLink.barycentricCoords = b;
00196         tmpLink.tetraNr = j;
00197         break;
00198       }
00199 
00200       // Otherwise, if we are not in any tetrahedron we take the closest one
00201       NxReal dist = 0.0f;
00202       if (b.x + b.y + b.z > 1.0f) dist = b.x + b.y + b.z - 1.0f;
00203       if (b.x < 0.0f) dist = (-b.x < dist) ? dist : -b.x;
00204       if (b.y < 0.0f) dist = (-b.y < dist) ? dist : -b.y;
00205       if (b.z < 0.0f) dist = (-b.z < dist) ? dist : -b.z;
00206 
00207       if (i == 0 || dist < minDist) {
00208         minDist = dist;
00209         tmpLink.barycentricCoords = b;
00210         tmpLink.tetraNr = j;
00211       }
00212     }
00213 
00214     _tetraLinks.push_back(tmpLink);
00215   }
00216 
00217   delete hash;
00218 }
00219 
00220 ////////////////////////////////////////////////////////////////////
00221 //     Function: PhysxSoftBodyNode::remove_tris_related_to_vertex
00222 //       Access: Public
00223 //  Description:
00224 ////////////////////////////////////////////////////////////////////
00225 void PhysxSoftBodyNode::
00226 remove_tris_related_to_vertex(const int vertexIndex) {
00227 
00228   GeomVertexRewriter vrewriter = GeomVertexRewriter(_vdata, InternalName::get_vertex());
00229   LVecBase3f v;
00230 
00231   for (int j=0; j<_prim->get_num_primitives(); j++) {
00232 
00233     int s = _prim->get_primitive_start(j);
00234     int idx0 = _prim->get_vertex(s);
00235     int idx1 = _prim->get_vertex(s+1);
00236     int idx2 = _prim->get_vertex(s+2);
00237 
00238     if (vertexIndex == idx0 || vertexIndex == idx1 || vertexIndex == idx2) {
00239 
00240       // Make this triangle degenerated
00241       vrewriter.set_row_unsafe(idx0); v = vrewriter.get_data3f();
00242       vrewriter.set_row_unsafe(idx1); vrewriter.set_data3f(v.get_x(), v.get_y(), v.get_z());
00243       vrewriter.set_row_unsafe(idx2); vrewriter.set_data3f(v.get_x(), v.get_y(), v.get_z());
00244     }
00245   }
00246 }
00247 
00248 ////////////////////////////////////////////////////////////////////
00249 //     Function: PhysxSoftBodyNode::update_bounds
00250 //       Access: Public
00251 //  Description:
00252 ////////////////////////////////////////////////////////////////////
00253 void PhysxSoftBodyNode::
00254 update_bounds() {
00255 
00256   _bounds.setEmpty();
00257 
00258   GeomVertexReader vreader = GeomVertexReader(_vdata, InternalName::get_vertex());
00259 
00260   while (!vreader.is_at_end()) {
00261     LVecBase3f v = vreader.get_data3f();
00262     _bounds.include(PhysxManager::vec3_to_nxVec3(v));
00263   }
00264 }
00265 
00266 ////////////////////////////////////////////////////////////////////
00267 //     Function: PhysxSoftBodyNode::update_normals
00268 //       Access: Public
00269 //  Description:_bounds.include(mVertices[i]);
00270 ////////////////////////////////////////////////////////////////////
00271 void PhysxSoftBodyNode::
00272 update_normals() {
00273 
00274   _normals.resize(_vdata->get_num_rows());
00275 
00276   int i;
00277   for (i=0; i<(int)_normals.size(); i++) { 
00278     _normals[i] = LVector3f::zero();
00279   }
00280 
00281   LVecBase3f n, v0, v1, v2;
00282   GeomVertexReader vreader = GeomVertexReader(_vdata, InternalName::get_vertex());
00283 
00284   for (int j=0; j<_prim->get_num_primitives(); j++) {
00285 
00286     int s = _prim->get_primitive_start(j);
00287     int idx0 = _prim->get_vertex(s);
00288     int idx1 = _prim->get_vertex(s+1);
00289     int idx2 = _prim->get_vertex(s+2);
00290 
00291     vreader.set_row_unsafe(idx0); v0 = vreader.get_data3f();
00292     vreader.set_row_unsafe(idx1); v1 = vreader.get_data3f();
00293     vreader.set_row_unsafe(idx2); v2 = vreader.get_data3f();
00294 
00295     n = (v1 - v0).cross(v2 - v0);
00296 
00297     _normals[idx0] += n;
00298     _normals[idx1] += n;
00299     _normals[idx2] += n;
00300   }
00301 
00302   for (i=0; i<(int)_normals.size(); i++) { 
00303     _normals[i].normalize();
00304   }
00305 
00306   GeomVertexWriter nwriter = GeomVertexWriter(_vdata, InternalName::get_normal());
00307   for (i=0; i<(int)_normals.size(); i++) { 
00308     n = _normals[i];
00309     nwriter.add_data3f(n.get_x(), n.get_y(), n.get_z());
00310   }
00311 }
00312 
00313 ////////////////////////////////////////////////////////////////////
00314 //     Function: PhysxSoftBodyNode::compute_bary_coords
00315 //       Access: Public
00316 //  Description:
00317 ////////////////////////////////////////////////////////////////////
00318 NxVec3 PhysxSoftBodyNode::
00319 compute_bary_coords(NxVec3 vertex, NxVec3 p0, NxVec3 p1, NxVec3 p2, NxVec3 p3) const {
00320 
00321   NxVec3 baryCoords;
00322 
00323   NxVec3 q  = vertex - p3;
00324   NxVec3 q0 = p0 - p3;
00325   NxVec3 q1 = p1 - p3;
00326   NxVec3 q2 = p2 - p3;
00327 
00328   NxMat33 m;
00329   m.setColumn(0, q0);
00330   m.setColumn(1, q1);
00331   m.setColumn(2, q2);
00332 
00333   NxReal det = m.determinant();
00334 
00335   m.setColumn(0, q);
00336   baryCoords.x = m.determinant();
00337 
00338   m.setColumn(0, q0);
00339   m.setColumn(1, q);
00340   baryCoords.y = m.determinant();
00341 
00342   m.setColumn(1, q1);
00343   m.setColumn(2,q);
00344   baryCoords.z = m.determinant();
00345 
00346   if (det != 0.0f) {
00347     baryCoords /= det;
00348   }
00349 
00350   return baryCoords;
00351 }
00352 
00353 ////////////////////////////////////////////////////////////////////
00354 //     Function: PhysxSoftBodyNode::update
00355 //       Access: Public
00356 //  Description: 
00357 ////////////////////////////////////////////////////////////////////
00358 void PhysxSoftBodyNode::
00359 update() {
00360 
00361   update_tetra_links();
00362 }
00363 
00364 ////////////////////////////////////////////////////////////////////
00365 //     Function: PhysxSoftBodyNode::update_tetra_links
00366 //       Access: Public
00367 //  Description:
00368 ////////////////////////////////////////////////////////////////////
00369 bool PhysxSoftBodyNode::
00370 update_tetra_links() {
00371 
00372   if (_tetraLinks.size() != _vdata->get_num_rows())
00373   {
00374       return false;
00375   }
00376 
00377   NxU32 numVertices = *_mesh.numVerticesPtr;
00378   NxU32 numTetrahedra = *_mesh.numIndicesPtr / 4;
00379   const NxVec3 *vertices = (NxVec3*)_mesh.verticesPosBegin;
00380   NxU32* indices = (NxU32*)_mesh.indicesBegin;
00381 
00382   GeomVertexRewriter vwriter = GeomVertexRewriter(_vdata, InternalName::get_vertex());
00383 
00384   int i = 0;
00385   while (!vwriter.is_at_end()) {
00386 
00387     TetraLink &link = _tetraLinks[i];
00388 
00389     if (!_drainedTriVertices[i]) {
00390       const NxU32 *ix = &indices[4*link.tetraNr];
00391 
00392       if (*ix == *(ix + 1)) {
00393         remove_tris_related_to_vertex(i);
00394         _drainedTriVertices[i] = true;
00395         continue;
00396       }
00397 
00398       const NxVec3 &p0 = vertices[*ix++];
00399       const NxVec3 &p1 = vertices[*ix++];
00400       const NxVec3 &p2 = vertices[*ix++];
00401       const NxVec3 &p3 = vertices[*ix++];
00402 
00403       NxVec3 &b = link.barycentricCoords;
00404       NxVec3 tmp = p0 * b.x + p1 * b.y + p2 * b.z + p3 * (1.0f - b.x - b.y - b.z);
00405       vwriter.set_data3f(tmp.x, tmp.y, tmp.z);
00406     }
00407     i++;
00408   }
00409 
00410   update_normals();
00411 
00412   return true;
00413 }
00414 
 All Classes Functions Variables Enumerations