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