Panda3D
|
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