00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
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
00029
00030
00031
00032 void PhysxSoftBodyNode::
00033 allocate(PhysxSoftBody *softbody) {
00034
00035 _softbody = softbody;
00036
00037
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;
00045
00046
00047
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
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
00069
00070
00071
00072
00073
00074
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
00129
00130
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
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
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
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
00222
00223
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
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
00250
00251
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
00268
00269
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
00315
00316
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
00355
00356
00357
00358 void PhysxSoftBodyNode::
00359 update() {
00360
00361 update_tetra_links();
00362 }
00363
00364
00365
00366
00367
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