Panda3D
physxSoftBodyNode.cxx
Go to the documentation of this file.
1 /**
2  * PANDA 3D SOFTWARE
3  * Copyright (c) Carnegie Mellon University. All rights reserved.
4  *
5  * All use of this software is subject to the terms of the revised BSD
6  * license. You should have received a copy of this license along
7  * with this source code in a file named "LICENSE."
8  *
9  * @file physxSoftBodyNode.cxx
10  * @author enn0x
11  * @date 2010-09-13
12  */
13 
14 #include "physxSoftBodyNode.h"
15 #include "physxSoftBody.h"
16 #include "physxFileStream.h"
17 #include "physxManager.h"
18 #include "physxMeshHash.h"
19 
20 #include "geomVertexFormat.h"
21 #include "geomVertexWriter.h"
22 #include "geomVertexRewriter.h"
23 
24 TypeHandle PhysxSoftBodyNode::_type_handle;
25 
26 /**
27  *
28  */
29 void PhysxSoftBodyNode::
30 allocate(PhysxSoftBody *softbody) {
31 
32  _softbody = softbody;
33 
34  // Retrieve number of vertices and triangles the hard way
35  NxSoftBodyMeshDesc meshDesc;
36  _softbody->ptr()->getSoftBodyMesh()->saveToDesc(meshDesc);
37 
38  NxU32 numVertices = meshDesc.numVertices;
39  NxU32 numTetrahedra = meshDesc.numTetrahedra;
40 
41  float factor = 1.0f; // TODO max(1.0f, factor);
42 
43  // Reserve more memory for vertices than the initial mesh takes because
44  // tearing creates new vertices
45  NxU32 maxVertices = factor * numVertices;
46  _mesh.verticesPosBegin = (NxVec3 *)malloc(sizeof(NxVec3) * maxVertices);
47  _mesh.verticesPosByteStride = sizeof(NxVec3);
48  _mesh.maxVertices = maxVertices;
49  _mesh.numVerticesPtr = (NxU32 *)malloc(sizeof(NxU32));
50 
51  // The number of tetrahedra is constant, even if the softbody is torn
52  NxU32 maxIndices = 4 * numTetrahedra;
53  _mesh.indicesBegin = (NxU32 *)malloc(sizeof(NxU32) * maxIndices);
54  _mesh.indicesByteStride = sizeof(NxU32);
55  _mesh.maxIndices = maxIndices;
56  _mesh.numIndicesPtr = (NxU32 *)malloc(sizeof(NxU32));
57 
58  *(_mesh.numVerticesPtr) = 0;
59  *(_mesh.numIndicesPtr) = 0;
60 
61  _softbody->ptr()->setMeshData(_mesh);
62 }
63 
64 /**
65  * Reads the vertices and indices from an existing Geom and makes a decomposed
66  * copy of the data. Then computes links between the owning soft body
67  * tetrahedron mesh in order to render an updated geometry every simulation
68  * frame.
69  */
71 set_from_geom(const Geom *geom) {
72 
73  _prim->clear_vertices();
74 
75  GeomVertexWriter vwriter = GeomVertexWriter(_vdata, InternalName::get_vertex());
76  GeomVertexWriter nwriter = GeomVertexWriter(_vdata, InternalName::get_normal());
77  GeomVertexWriter twriter = GeomVertexWriter(_vdata, InternalName::get_texcoord());
78 
79  CPT(GeomVertexData) vdata = geom->get_vertex_data();
80  GeomVertexReader vreader = GeomVertexReader(vdata, InternalName::get_vertex());
81  GeomVertexReader nreader = GeomVertexReader(vdata, InternalName::get_normal());
82  GeomVertexReader treader = GeomVertexReader(vdata, InternalName::get_texcoord());
83 
84  while (!vreader.is_at_end()) {
85  LVecBase3f v = vreader.get_data3f();
86  vwriter.add_data3f(v.get_x(), v.get_y(), v.get_z());
87  }
88 
89  while (!nreader.is_at_end()) {
90  LVecBase3f n = nreader.get_data3f();
91  nwriter.add_data3f(n.get_x(), n.get_y(), n.get_z());
92  }
93 
94  while (!treader.is_at_end()) {
95  LVecBase2f t = treader.get_data2f();
96  twriter.add_data2f(t.get_x(), t.get_y());
97  }
98 
99  for (int i=0; i<geom->get_num_primitives(); i++) {
100 
101  CPT(GeomPrimitive) prim = geom->get_primitive(i);
102  prim = prim->decompose();
103 
104  for (int j=0; j<prim->get_num_primitives(); j++) {
105 
106  int s = prim->get_primitive_start(j);
107  int e = prim->get_primitive_end(j);
108 
109  for (int l=s; l<e; l++) {
110  _prim->add_vertex(prim->get_vertex(l));
111  }
112  }
113  }
114 
115  _prim->close_primitive();
116 
117  update_bounds();
118  build_tetra_links();
119 }
120 
121 /**
122  *
123  */
124 void PhysxSoftBodyNode::
125 build_tetra_links() {
126 
127  NxSoftBodyMeshDesc meshDesc;
128  _softbody->ptr()->getSoftBodyMesh()->saveToDesc(meshDesc);
129  const NxVec3 *vertices = (const NxVec3 *) meshDesc.vertices;
130  const NxU32 *indices = (const NxU32 *) meshDesc.tetrahedra;
131  const NxU32 numTets = meshDesc.numTetrahedra;
132 
133  _tetraLinks.clear();
134 
135  PhysxMeshHash* hash = new PhysxMeshHash();
136  hash->set_grid_spacing(_bounds.min.distance(_bounds.max) * 0.1f);
137 
138  for (NxU32 i=0; i<numTets; i++) {
139  const NxU32 *ix = &indices[4*i];
140  NxBounds3 tetraBounds;
141  tetraBounds.setEmpty();
142  tetraBounds.include(vertices[*ix++]);
143  tetraBounds.include(vertices[*ix++]);
144  tetraBounds.include(vertices[*ix++]);
145  tetraBounds.include(vertices[*ix++]);
146  hash->add(tetraBounds, i);
147  }
148 
149  GeomVertexReader vreader = GeomVertexReader(_vdata, InternalName::get_vertex());
150 
151  while (!vreader.is_at_end()) {
152 
153  // Prepare datastructure for drained tetras
154  _drainedTriVertices.push_back(false);
155 
156  TetraLink tmpLink;
157 
158  LVecBase3f v = vreader.get_data3f();
159  NxVec3 triVert = PhysxManager::vec3_to_nxVec3(v);
160  pvector<int> itemIndices;
161  hash->query_unique(triVert, itemIndices);
162 
163  NxReal minDist = 0.0f;
164  NxVec3 b;
165  int num, isize;
166  num = isize = itemIndices.size();
167  if (num == 0) {
168  num = numTets;
169  }
170 
171  for (int i=0; i<num; i++) {
172  int j = i;
173  if (isize > 0) {
174  j = itemIndices[i];
175  }
176 
177  const NxU32 *ix = &indices[j*4];
178  const NxVec3 &p0 = vertices[*ix++];
179  const NxVec3 &p1 = vertices[*ix++];
180  const NxVec3 &p2 = vertices[*ix++];
181  const NxVec3 &p3 = vertices[*ix++];
182 
183  NxVec3 b = compute_bary_coords(triVert, p0, p1, p2, p3);
184 
185  // Is the vertex inside the tetrahedron? If yes we take it
186  if (b.x >= 0.0f && b.y >= 0.0f && b.z >= 0.0f && (b.x + b.y + b.z) <= 1.0f) {
187  tmpLink.barycentricCoords = b;
188  tmpLink.tetraNr = j;
189  break;
190  }
191 
192  // Otherwise, if we are not in any tetrahedron we take the closest one
193  NxReal dist = 0.0f;
194  if (b.x + b.y + b.z > 1.0f) dist = b.x + b.y + b.z - 1.0f;
195  if (b.x < 0.0f) dist = (-b.x < dist) ? dist : -b.x;
196  if (b.y < 0.0f) dist = (-b.y < dist) ? dist : -b.y;
197  if (b.z < 0.0f) dist = (-b.z < dist) ? dist : -b.z;
198 
199  if (i == 0 || dist < minDist) {
200  minDist = dist;
201  tmpLink.barycentricCoords = b;
202  tmpLink.tetraNr = j;
203  }
204  }
205 
206  _tetraLinks.push_back(tmpLink);
207  }
208 
209  delete hash;
210 }
211 
212 /**
213  *
214  */
215 void PhysxSoftBodyNode::
216 remove_tris_related_to_vertex(const int vertexIndex) {
217 
218  GeomVertexRewriter vrewriter = GeomVertexRewriter(_vdata, InternalName::get_vertex());
219  LVecBase3f v;
220 
221  for (int j=0; j<_prim->get_num_primitives(); j++) {
222 
223  int s = _prim->get_primitive_start(j);
224  int idx0 = _prim->get_vertex(s);
225  int idx1 = _prim->get_vertex(s+1);
226  int idx2 = _prim->get_vertex(s+2);
227 
228  if (vertexIndex == idx0 || vertexIndex == idx1 || vertexIndex == idx2) {
229 
230  // Make this triangle degenerated
231  vrewriter.set_row_unsafe(idx0); v = vrewriter.get_data3f();
232  vrewriter.set_row_unsafe(idx1); vrewriter.set_data3f(v.get_x(), v.get_y(), v.get_z());
233  vrewriter.set_row_unsafe(idx2); vrewriter.set_data3f(v.get_x(), v.get_y(), v.get_z());
234  }
235  }
236 }
237 
238 /**
239  *
240  */
241 void PhysxSoftBodyNode::
242 update_bounds() {
243 
244  _bounds.setEmpty();
245 
246  GeomVertexReader vreader = GeomVertexReader(_vdata, InternalName::get_vertex());
247 
248  while (!vreader.is_at_end()) {
249  LVecBase3f v = vreader.get_data3f();
250  _bounds.include(PhysxManager::vec3_to_nxVec3(v));
251  }
252 }
253 
254 /**
255  * _bounds.include(mVertices[i]);
256  */
257 void PhysxSoftBodyNode::
258 update_normals() {
259 
260  _normals.resize(_vdata->get_num_rows());
261 
262  int i;
263  for (i=0; i<(int)_normals.size(); i++) {
264  _normals[i] = LVector3f::zero();
265  }
266 
267  LVecBase3f n, v0, v1, v2;
268  GeomVertexReader vreader = GeomVertexReader(_vdata, InternalName::get_vertex());
269 
270  for (int j=0; j<_prim->get_num_primitives(); j++) {
271 
272  int s = _prim->get_primitive_start(j);
273  int idx0 = _prim->get_vertex(s);
274  int idx1 = _prim->get_vertex(s+1);
275  int idx2 = _prim->get_vertex(s+2);
276 
277  vreader.set_row_unsafe(idx0); v0 = vreader.get_data3f();
278  vreader.set_row_unsafe(idx1); v1 = vreader.get_data3f();
279  vreader.set_row_unsafe(idx2); v2 = vreader.get_data3f();
280 
281  n = (v1 - v0).cross(v2 - v0);
282 
283  _normals[idx0] += n;
284  _normals[idx1] += n;
285  _normals[idx2] += n;
286  }
287 
288  for (i=0; i<(int)_normals.size(); i++) {
289  _normals[i].normalize();
290  }
291 
292  GeomVertexWriter nwriter = GeomVertexWriter(_vdata, InternalName::get_normal());
293  for (i=0; i<(int)_normals.size(); i++) {
294  n = _normals[i];
295  nwriter.add_data3f(n.get_x(), n.get_y(), n.get_z());
296  }
297 }
298 
299 /**
300  *
301  */
302 NxVec3 PhysxSoftBodyNode::
303 compute_bary_coords(NxVec3 vertex, NxVec3 p0, NxVec3 p1, NxVec3 p2, NxVec3 p3) const {
304 
305  NxVec3 baryCoords;
306 
307  NxVec3 q = vertex - p3;
308  NxVec3 q0 = p0 - p3;
309  NxVec3 q1 = p1 - p3;
310  NxVec3 q2 = p2 - p3;
311 
312  NxMat33 m;
313  m.setColumn(0, q0);
314  m.setColumn(1, q1);
315  m.setColumn(2, q2);
316 
317  NxReal det = m.determinant();
318 
319  m.setColumn(0, q);
320  baryCoords.x = m.determinant();
321 
322  m.setColumn(0, q0);
323  m.setColumn(1, q);
324  baryCoords.y = m.determinant();
325 
326  m.setColumn(1, q1);
327  m.setColumn(2,q);
328  baryCoords.z = m.determinant();
329 
330  if (det != 0.0f) {
331  baryCoords /= det;
332  }
333 
334  return baryCoords;
335 }
336 
337 /**
338  *
339  */
340 void PhysxSoftBodyNode::
341 update() {
342 
343  update_tetra_links();
344 }
345 
346 /**
347  *
348  */
349 bool PhysxSoftBodyNode::
350 update_tetra_links() {
351 
352  if (_tetraLinks.size() != _vdata->get_num_rows())
353  {
354  return false;
355  }
356 
357  NxU32 numVertices = *_mesh.numVerticesPtr;
358  NxU32 numTetrahedra = *_mesh.numIndicesPtr / 4;
359  const NxVec3 *vertices = (NxVec3*)_mesh.verticesPosBegin;
360  NxU32* indices = (NxU32*)_mesh.indicesBegin;
361 
362  GeomVertexRewriter vwriter = GeomVertexRewriter(_vdata, InternalName::get_vertex());
363 
364  int i = 0;
365  while (!vwriter.is_at_end()) {
366 
367  TetraLink &link = _tetraLinks[i];
368 
369  if (!_drainedTriVertices[i]) {
370  const NxU32 *ix = &indices[4*link.tetraNr];
371 
372  if (*ix == *(ix + 1)) {
373  remove_tris_related_to_vertex(i);
374  _drainedTriVertices[i] = true;
375  continue;
376  }
377 
378  const NxVec3 &p0 = vertices[*ix++];
379  const NxVec3 &p1 = vertices[*ix++];
380  const NxVec3 &p2 = vertices[*ix++];
381  const NxVec3 &p3 = vertices[*ix++];
382 
383  NxVec3 &b = link.barycentricCoords;
384  NxVec3 tmp = p0 * b.x + p1 * b.y + p2 * b.z + p3 * (1.0f - b.x - b.y - b.z);
385  vwriter.set_data3f(tmp.x, tmp.y, tmp.z);
386  }
387  i++;
388  }
389 
390  update_normals();
391 
392  return true;
393 }
This is an abstract base class for a family of classes that represent the fundamental geometry primit...
Definition: geomPrimitive.h:56
This defines the actual numeric vertex data stored in a Geom, in the structure defined by a particula...
This object provides a high-level interface for quickly reading a sequence of numeric values from a v...
bool is_at_end() const
Returns true if the reader is currently at the end of the list of vertices, false otherwise.
const LVecBase3f & get_data3f()
Returns the data associated with the read row, expressed as a 3-component value, and advances the rea...
const LVecBase2f & get_data2f()
Returns the data associated with the read row, expressed as a 2-component value, and advances the rea...
void set_row_unsafe(int row)
Sets the start row to the indicated value, without internal checks.
This object provides the functionality of both a GeomVertexReader and a GeomVertexWriter,...
void set_row_unsafe(int row)
Sets the start row to the indicated value, without internal checks.
bool is_at_end() const
Returns true if the reader or writer is currently at the end of the list of vertices,...
This object provides a high-level interface for quickly writing a sequence of numeric values from a v...
void set_data3f(float x, float y, float z)
Sets the write row to a particular 3-component value, and advances the write row.
void add_data2f(float x, float y)
Sets the write row to a particular 2-component value, and advances the write row.
void add_data3f(float x, float y, float z)
Sets the write row to a particular 3-component value, and advances the write row.
A container for geometry primitives.
Definition: geom.h:54
static NxVec3 vec3_to_nxVec3(const LVector3f &v)
Converts from LVector3f to NxVec3.
Definition: physxManager.I:27
Utility class used in building links between a tetrahedron mesh (soft body) and a triangle mesh used ...
Definition: physxMeshHash.h:25
void set_from_geom(const Geom *geom)
Reads the vertices and indices from an existing Geom and makes a decomposed copy of the data.
TypeHandle is the identifier used to differentiate C++ class types.
Definition: typeHandle.h:81
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.