Panda3D
bulletSoftBodyNode.cxx
1 // Filename: bulletSoftBodyNode.cxx
2 // Created by: enn0x (27Dec10)
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 "bulletSoftBodyNode.h"
16 #include "bulletSoftBodyConfig.h"
17 #include "bulletSoftBodyControl.h"
18 #include "bulletSoftBodyMaterial.h"
19 #include "bulletSoftBodyShape.h"
20 #include "bulletSoftBodyWorldInfo.h"
21 #include "bulletHelper.h"
22 
23 #include "geomVertexRewriter.h"
24 #include "geomVertexReader.h"
25 
26 TypeHandle BulletSoftBodyNode::_type_handle;
27 
28 ////////////////////////////////////////////////////////////////////
29 // Function: BulletSoftBodyNode::Constructor
30 // Access: Public
31 // Description:
32 ////////////////////////////////////////////////////////////////////
33 BulletSoftBodyNode::
34 BulletSoftBodyNode(btSoftBody *body, const char *name) : BulletBodyNode(name) {
35 
36  // Synchronised transform
37  _sync = TransformState::make_identity();
38  _sync_disable = false;
39 
40  // Softbody
41  _soft = body;
42  _soft->setUserPointer(this);
43 
44  // Shape
45  btCollisionShape *shape_ptr = _soft->getCollisionShape();
46 
47  nassertv(shape_ptr != NULL);
48  nassertv(shape_ptr->getShapeType() == SOFTBODY_SHAPE_PROXYTYPE);
49 
50  _shapes.push_back(new BulletSoftBodyShape((btSoftBodyCollisionShape *)shape_ptr));
51 
52  // Rendering
53  _geom = NULL;
54  _curve = NULL;
55  _surface = NULL;
56 }
57 
58 ////////////////////////////////////////////////////////////////////
59 // Function: BulletSoftBodyNode::get_object
60 // Access: Public
61 // Description:
62 ////////////////////////////////////////////////////////////////////
63 btCollisionObject *BulletSoftBodyNode::
64 get_object() const {
65 
66  return _soft;
67 }
68 
69 ////////////////////////////////////////////////////////////////////
70 // Function: BulletSoftBodyNode::get_cfg
71 // Access: Published
72 // Description:
73 ////////////////////////////////////////////////////////////////////
74 BulletSoftBodyConfig BulletSoftBodyNode::
75 get_cfg() {
76 
77  return BulletSoftBodyConfig(_soft->m_cfg);
78 }
79 
80 ////////////////////////////////////////////////////////////////////
81 // Function: BulletSoftBodyNode::get_world_info
82 // Access: Published
83 // Description:
84 ////////////////////////////////////////////////////////////////////
85 BulletSoftBodyWorldInfo BulletSoftBodyNode::
86 get_world_info() {
87 
88  return BulletSoftBodyWorldInfo(*(_soft->m_worldInfo));
89 }
90 
91 ////////////////////////////////////////////////////////////////////
92 // Function: BulletSoftBodyNode::get_num_materials
93 // Access: Published
94 // Description:
95 ////////////////////////////////////////////////////////////////////
96 int BulletSoftBodyNode::
97 get_num_materials() const {
98 
99  return _soft->m_materials.size();
100 }
101 
102 ////////////////////////////////////////////////////////////////////
103 // Function: BulletSoftBodyNode::get_material
104 // Access: Published
105 // Description:
106 ////////////////////////////////////////////////////////////////////
107 BulletSoftBodyMaterial BulletSoftBodyNode::
108 get_material(int idx) const {
109 
110  nassertr(idx >= 0 && idx < get_num_materials(), BulletSoftBodyMaterial::empty());
111 
112  btSoftBody::Material *material = _soft->m_materials[idx];
113  return BulletSoftBodyMaterial(*material);
114 }
115 
116 ////////////////////////////////////////////////////////////////////
117 // Function: BulletSoftBodyNode::append_material
118 // Access: Published
119 // Description:
120 ////////////////////////////////////////////////////////////////////
121 BulletSoftBodyMaterial BulletSoftBodyNode::
122 append_material() {
123 
124  btSoftBody::Material *material = _soft->appendMaterial();
125  nassertr(material, BulletSoftBodyMaterial::empty());
126 
127  return BulletSoftBodyMaterial(*material);
128 }
129 
130 ////////////////////////////////////////////////////////////////////
131 // Function: BulletSoftBodyNode::get_num_nodes
132 // Access: Published
133 // Description:
134 ////////////////////////////////////////////////////////////////////
135 int BulletSoftBodyNode::
136 get_num_nodes() const {
137 
138  return _soft->m_nodes.size();
139 }
140 
141 ////////////////////////////////////////////////////////////////////
142 // Function: BulletSoftBodyNode::get_node
143 // Access: Published
144 // Description:
145 ////////////////////////////////////////////////////////////////////
146 BulletSoftBodyNodeElement BulletSoftBodyNode::
147 get_node(int idx) const {
148 
149  nassertr(idx >=0 && idx < get_num_nodes(), BulletSoftBodyNodeElement::empty());
150  return BulletSoftBodyNodeElement(_soft->m_nodes[idx]);
151 }
152 
153 ////////////////////////////////////////////////////////////////////
154 // Function: BulletSoftBodyNode::generate_bending_constraints
155 // Access: Published
156 // Description:
157 ////////////////////////////////////////////////////////////////////
158 void BulletSoftBodyNode::
159 generate_bending_constraints(int distance, BulletSoftBodyMaterial *material) {
160 
161  if (material) {
162  _soft->generateBendingConstraints(distance, &(material->get_material()));
163  }
164  else {
165  _soft->generateBendingConstraints(distance);
166  }
167 }
168 
169 ////////////////////////////////////////////////////////////////////
170 // Function: BulletSoftBodyNode::randomize_constraints
171 // Access: Published
172 // Description:
173 ////////////////////////////////////////////////////////////////////
174 void BulletSoftBodyNode::
175 randomize_constraints() {
176 
177  _soft->randomizeConstraints();
178 }
179 
180 ////////////////////////////////////////////////////////////////////
181 // Function: BulletSoftBodyNode::transform_changed
182 // Access: Protected
183 // Description:
184 ////////////////////////////////////////////////////////////////////
185 void BulletSoftBodyNode::
186 transform_changed() {
187 
188  if (_sync_disable) return;
189 
190  NodePath np = NodePath::any_path((PandaNode *)this);
191  CPT(TransformState) ts = np.get_net_transform();
192 
193  LMatrix4 m_sync = _sync->get_mat();
194  LMatrix4 m_ts = ts->get_mat();
195 
196  if (!m_sync.almost_equal(m_ts)) {
197 
198  // New transform for the center
199  btTransform trans = TransformState_to_btTrans(ts);
200 
201  // Offset between current approx center and current initial transform
202  btVector3 pos = LVecBase3_to_btVector3(this->get_aabb().get_approx_center());
203  btVector3 origin = _soft->m_initialWorldTransform.getOrigin();
204  btVector3 offset = pos - origin;
205 
206  // Subtract offset to get new transform for the body
207  trans.setOrigin(trans.getOrigin() - offset);
208 
209  // Now apply the new transform
210  _soft->transform(_soft->m_initialWorldTransform.inverse());
211  _soft->transform(trans);
212 
213  if (ts->has_scale()) {
214  btVector3 current_scale = LVecBase3_to_btVector3(_sync->get_scale());
215  btVector3 new_scale = LVecBase3_to_btVector3(ts->get_scale());
216 
217  current_scale.setX(1.0 / current_scale.getX());
218  current_scale.setY(1.0 / current_scale.getY());
219  current_scale.setZ(1.0 / current_scale.getZ());
220 
221  _soft->scale(current_scale);
222  _soft->scale(new_scale);
223  }
224 
225  _sync = ts;
226  }
227 }
228 
229 ////////////////////////////////////////////////////////////////////
230 // Function: BulletSoftBodyNode::sync_p2b
231 // Access: Public
232 // Description:
233 ////////////////////////////////////////////////////////////////////
234 void BulletSoftBodyNode::
235 sync_p2b() {
236 
237  //transform_changed(); Disabled for now...
238 }
239 
240 ////////////////////////////////////////////////////////////////////
241 // Function: BulletSoftBodyNode::sync_b2p
242 // Access: Public
243 // Description:
244 ////////////////////////////////////////////////////////////////////
245 void BulletSoftBodyNode::
246 sync_b2p() {
247 
248  // Render softbody
249  if (_geom) {
250  btTransform trans = btTransform::getIdentity();
251  get_node_transform(trans, this);
252 
253  PT(GeomVertexData) vdata = _geom->modify_vertex_data();
254 
255  GeomVertexRewriter vertices(vdata, InternalName::get_vertex());
256  GeomVertexRewriter normals(vdata, InternalName::get_normal());
257  GeomVertexReader indices(vdata, BulletHelper::get_sb_index());
258  GeomVertexReader flips(vdata, BulletHelper::get_sb_flip());
259 
260  while (!vertices.is_at_end()) {
261  btSoftBody::Node node = _soft->m_nodes[indices.get_data1i()];
262  btVector3 v = trans.invXform(node.m_x);
263  btVector3 n = node.m_n;
264 
265  if (flips.get_data1i() > 0) n *= -1;
266 
267  vertices.set_data3((PN_stdfloat)v.getX(), (PN_stdfloat)v.getY(), (PN_stdfloat)v.getZ());
268  normals.set_data3((PN_stdfloat)n.getX(), (PN_stdfloat)n.getY(), (PN_stdfloat)n.getZ());
269  }
270  }
271 
272  if (_curve) {
273  btSoftBody::tNodeArray &nodes(_soft->m_nodes);
274 
275  for (int i=0; i < nodes.size(); i++) {
276  btVector3 pos = nodes[i].m_x;
277  _curve->set_vertex(i, btVector3_to_LPoint3(pos));
278  }
279  }
280 
281  if (_surface) {
282  btSoftBody::tNodeArray &nodes(_soft->m_nodes);
283 
284  int num_u = _surface->get_num_u_vertices();
285  int num_v = _surface->get_num_v_vertices();
286  nassertv(num_u * num_v == nodes.size());
287 
288  for (int u=0; u < num_u; u++) {
289  for (int v=0; v < num_v; v++) {
290  btVector3 pos = nodes[u * num_u + v].m_x;
291  _surface->set_vertex(u, v, btVector3_to_LPoint3(pos));
292  }
293  }
294  }
295 
296  // Update the synchronized transform with the current
297  // approximate center of the soft body
298  LVecBase3 pos = this->get_aabb().get_approx_center();
299  CPT(TransformState) ts = TransformState::make_pos(pos);
300 
301  NodePath np = NodePath::any_path((PandaNode *)this);
302  LVecBase3 scale = np.get_net_transform()->get_scale();
303  ts = ts->set_scale(scale);
304 
305  _sync = ts;
306  _sync_disable = true;
307  np.set_transform(NodePath(), ts);
308  _sync_disable = false;
309 /*
310 */
311 
312  Thread *current_thread = Thread::get_current_thread();
313  this->r_mark_geom_bounds_stale(current_thread);
314 }
315 
316 ////////////////////////////////////////////////////////////////////
317 // Function: BulletSoftBodyNode::get_closest_node_index
318 // Access: Published
319 // Description: Returns the index of the node which is closest
320 // to the given point. The distance between each node
321 // and the given point is computed in world space
322 // if local=false, and in local space if local=true.
323 ////////////////////////////////////////////////////////////////////
325 get_closest_node_index(LVecBase3 point, bool local) {
326 
327  btScalar max_dist_sqr = 1e30;
328  btVector3 point_x = LVecBase3_to_btVector3(point);
329 
330  btTransform trans = btTransform::getIdentity();
331  if (local == true) {
332  get_node_transform(trans, this);
333  }
334 
335  btSoftBody::tNodeArray &nodes(_soft->m_nodes);
336  int node_idx = 0;
337 
338  for (int i=0; i<nodes.size(); ++i) {
339 
340  btVector3 node_x = nodes[i].m_x;
341  btScalar dist_sqr = (trans.invXform(node_x) - point_x).length2();
342 
343  if (dist_sqr < max_dist_sqr) {
344  max_dist_sqr = dist_sqr;
345  node_idx = i;
346  }
347  }
348 
349  return node_idx;
350 }
351 
352 ////////////////////////////////////////////////////////////////////
353 // Function: BulletSoftBodyNode::link_geom
354 // Access: Published
355 // Description:
356 ////////////////////////////////////////////////////////////////////
357 void BulletSoftBodyNode::
358 link_geom(Geom *geom) {
359 
360  nassertv(geom->get_vertex_data()->has_column(InternalName::get_vertex()));
361  nassertv(geom->get_vertex_data()->has_column(InternalName::get_normal()));
362 
363  sync_p2b();
364 
365  _geom = geom;
366 
367  PT(GeomVertexData) vdata = _geom->modify_vertex_data();
368 
369  if (!vdata->has_column(BulletHelper::get_sb_index())) {
370  CPT(GeomVertexFormat) format = vdata->get_format();
371  format = BulletHelper::add_sb_index_column(format);
372  vdata->set_format(format);
373  }
374 
375  if (!vdata->has_column(BulletHelper::get_sb_flip())) {
376  CPT(GeomVertexFormat) format = vdata->get_format();
377  format = BulletHelper::add_sb_flip_column(format);
378  vdata->set_format(format);
379  }
380 
381  GeomVertexReader vertices(vdata, InternalName::get_vertex());
382  GeomVertexRewriter indices(vdata, BulletHelper::get_sb_index());
383 
384  while (!vertices.is_at_end()) {
385  LVecBase3 point = vertices.get_data3();
386  int node_idx = get_closest_node_index(point, true);
387  indices.set_data1i(node_idx);
388  }
389 }
390 
391 ////////////////////////////////////////////////////////////////////
392 // Function: BulletSoftBodyNode::unlink_geom
393 // Access: Published
394 // Description:
395 ////////////////////////////////////////////////////////////////////
396 void BulletSoftBodyNode::
397 unlink_geom() {
398 
399  _geom = NULL;
400 }
401 
402 ////////////////////////////////////////////////////////////////////
403 // Function: BulletSoftBodyNode::link_curve
404 // Access: Published
405 // Description:
406 ////////////////////////////////////////////////////////////////////
407 void BulletSoftBodyNode::
408 link_curve(NurbsCurveEvaluator *curve) {
409 
410  nassertv(curve->get_num_vertices() == _soft->m_nodes.size());
411 
412  _curve = curve;
413 }
414 
415 ////////////////////////////////////////////////////////////////////
416 // Function: BulletSoftBodyNode::unlink_curve
417 // Access: Published
418 // Description:
419 ////////////////////////////////////////////////////////////////////
420 void BulletSoftBodyNode::
421 unlink_curve() {
422 
423  _curve = NULL;
424 }
425 
426 ////////////////////////////////////////////////////////////////////
427 // Function: BulletSoftBodyNode::link_surface
428 // Access: Published
429 // Description:
430 ////////////////////////////////////////////////////////////////////
431 void BulletSoftBodyNode::
432 link_surface(NurbsSurfaceEvaluator *surface) {
433 
434  nassertv(surface->get_num_u_vertices() * surface->get_num_v_vertices() == _soft->m_nodes.size());
435 
436  _surface = surface;
437 }
438 
439 ////////////////////////////////////////////////////////////////////
440 // Function: BulletSoftBodyNode::unlink_surface
441 // Access: Published
442 // Description:
443 ////////////////////////////////////////////////////////////////////
444 void BulletSoftBodyNode::
445 unlink_surface() {
446 
447  _surface = NULL;
448 }
449 
450 ////////////////////////////////////////////////////////////////////
451 // Function: BulletSoftBodyNode::get_aabb
452 // Access: Published
453 // Description:
454 ////////////////////////////////////////////////////////////////////
455 BoundingBox BulletSoftBodyNode::
456 get_aabb() const {
457 
458  btVector3 pMin;
459  btVector3 pMax;
460 
461  _soft->getAabb(pMin, pMax);
462 
463  return BoundingBox(
464  btVector3_to_LPoint3(pMin),
465  btVector3_to_LPoint3(pMax)
466  );
467 }
468 
469 ////////////////////////////////////////////////////////////////////
470 // Function: BulletSoftBodyNode::set_volume_mass
471 // Access: Published
472 // Description:
473 ////////////////////////////////////////////////////////////////////
474 void BulletSoftBodyNode::
475 set_volume_mass(PN_stdfloat mass) {
476 
477  _soft->setVolumeMass(mass);
478 }
479 
480 ////////////////////////////////////////////////////////////////////
481 // Function: BulletSoftBodyNode::set_total_mass
482 // Access: Published
483 // Description:
484 ////////////////////////////////////////////////////////////////////
485 void BulletSoftBodyNode::
486 set_total_mass(PN_stdfloat mass, bool fromfaces) {
487 
488  _soft->setTotalMass(mass, fromfaces);
489 }
490 
491 ////////////////////////////////////////////////////////////////////
492 // Function: BulletSoftBodyNode::set_volume_density
493 // Access: Published
494 // Description:
495 ////////////////////////////////////////////////////////////////////
496 void BulletSoftBodyNode::
497 set_volume_density(PN_stdfloat density) {
498 
499  _soft->setVolumeDensity(density);
500 }
501 
502 ////////////////////////////////////////////////////////////////////
503 // Function: BulletSoftBodyNode::set_total_density
504 // Access: Published
505 // Description:
506 ////////////////////////////////////////////////////////////////////
507 void BulletSoftBodyNode::
508 set_total_density(PN_stdfloat density) {
509 
510  _soft->setTotalDensity(density);
511 }
512 
513 ////////////////////////////////////////////////////////////////////
514 // Function: BulletSoftBodyNode::set_mass
515 // Access: Published
516 // Description:
517 ////////////////////////////////////////////////////////////////////
518 void BulletSoftBodyNode::
519 set_mass(int node, PN_stdfloat mass) {
520 
521  _soft->setMass(node, mass);
522 }
523 
524 ////////////////////////////////////////////////////////////////////
525 // Function: BulletSoftBodyNode::get_mass
526 // Access: Published
527 // Description:
528 ////////////////////////////////////////////////////////////////////
529 PN_stdfloat BulletSoftBodyNode::
530 get_mass(int node) const {
531 
532  return _soft->getMass(node);
533 }
534 
535 ////////////////////////////////////////////////////////////////////
536 // Function: BulletSoftBodyNode::get_total_mass
537 // Access: Published
538 // Description:
539 ////////////////////////////////////////////////////////////////////
540 PN_stdfloat BulletSoftBodyNode::
541 get_total_mass() const {
542 
543  return _soft->getTotalMass();
544 }
545 
546 ////////////////////////////////////////////////////////////////////
547 // Function: BulletSoftBodyNode::get_volume
548 // Access: Published
549 // Description:
550 ////////////////////////////////////////////////////////////////////
551 PN_stdfloat BulletSoftBodyNode::
552 get_volume() const {
553 
554  return _soft->getVolume();
555 }
556 
557 ////////////////////////////////////////////////////////////////////
558 // Function: BulletSoftBodyNode::add_force
559 // Access: Published
560 // Description:
561 ////////////////////////////////////////////////////////////////////
562 void BulletSoftBodyNode::
563 add_force(const LVector3 &force) {
564 
565  nassertv(!force.is_nan());
566  _soft->addForce(LVecBase3_to_btVector3(force));
567 }
568 
569 ////////////////////////////////////////////////////////////////////
570 // Function: BulletSoftBodyNode::add_force
571 // Access: Published
572 // Description:
573 ////////////////////////////////////////////////////////////////////
574 void BulletSoftBodyNode::
575 add_force(const LVector3 &force, int node) {
576 
577  nassertv(!force.is_nan());
578  _soft->addForce(LVecBase3_to_btVector3(force), node);
579 }
580 
581 ////////////////////////////////////////////////////////////////////
582 // Function: BulletSoftBodyNode::set_velocity
583 // Access: Published
584 // Description:
585 ////////////////////////////////////////////////////////////////////
586 void BulletSoftBodyNode::
587 set_velocity(const LVector3 &velocity) {
588 
589  nassertv(!velocity.is_nan());
590  _soft->setVelocity(LVecBase3_to_btVector3(velocity));
591 }
592 
593 ////////////////////////////////////////////////////////////////////
594 // Function: BulletSoftBodyNode::add_velocity
595 // Access: Published
596 // Description:
597 ////////////////////////////////////////////////////////////////////
598 void BulletSoftBodyNode::
599 add_velocity(const LVector3 &velocity) {
600 
601  nassertv(!velocity.is_nan());
602  _soft->addVelocity(LVecBase3_to_btVector3(velocity));
603 }
604 
605 ////////////////////////////////////////////////////////////////////
606 // Function: BulletSoftBodyNode::add_velocity
607 // Access: Published
608 // Description:
609 ////////////////////////////////////////////////////////////////////
610 void BulletSoftBodyNode::
611 add_velocity(const LVector3 &velocity, int node) {
612 
613  nassertv(!velocity.is_nan());
614  _soft->addVelocity(LVecBase3_to_btVector3(velocity), node);
615 }
616 
617 ////////////////////////////////////////////////////////////////////
618 // Function: BulletSoftBodyNode::generate_clusters
619 // Access: Published
620 // Description:
621 ////////////////////////////////////////////////////////////////////
622 void BulletSoftBodyNode::
623 generate_clusters(int k, int maxiterations) {
624 
625  _soft->generateClusters(k, maxiterations);
626 }
627 
628 ////////////////////////////////////////////////////////////////////
629 // Function: BulletSoftBodyNode::release_clusters
630 // Access: Published
631 // Description:
632 ////////////////////////////////////////////////////////////////////
633 void BulletSoftBodyNode::
634 release_clusters() {
635 
636  _soft->releaseClusters();
637 }
638 
639 ////////////////////////////////////////////////////////////////////
640 // Function: BulletSoftBodyNode::release_cluster
641 // Access: Published
642 // Description:
643 ////////////////////////////////////////////////////////////////////
644 void BulletSoftBodyNode::
645 release_cluster(int index) {
646 
647  _soft->releaseCluster(index);
648 }
649 
650 ////////////////////////////////////////////////////////////////////
651 // Function: BulletSoftBodyNode::get_num_clusters
652 // Access: Published
653 // Description:
654 ////////////////////////////////////////////////////////////////////
655 int BulletSoftBodyNode::
656 get_num_clusters() const {
657 
658  return _soft->clusterCount();
659 }
660 
661 ////////////////////////////////////////////////////////////////////
662 // Function: BulletSoftBodyNode::cluster_com
663 // Access: Published
664 // Description:
665 ////////////////////////////////////////////////////////////////////
666 LVecBase3 BulletSoftBodyNode::
667 cluster_com(int cluster) const {
668 
669  return btVector3_to_LVecBase3(_soft->clusterCom(cluster));
670 }
671 
672 ////////////////////////////////////////////////////////////////////
673 // Function: BulletSoftBodyNode::set_pose
674 // Access: Published
675 // Description:
676 ////////////////////////////////////////////////////////////////////
677 void BulletSoftBodyNode::
678 set_pose(bool bvolume, bool bframe) {
679 
680  _soft->setPose(bvolume, bframe);
681 }
682 
683 ////////////////////////////////////////////////////////////////////
684 // Function: BulletSoftBodyNode::append_anchor
685 // Access: Published
686 // Description:
687 ////////////////////////////////////////////////////////////////////
688 void BulletSoftBodyNode::
689 append_anchor(int node, BulletRigidBodyNode *body, bool disable) {
690 
691  nassertv(node < _soft->m_nodes.size())
692  nassertv(body);
693 
694  body->sync_p2b();
695 
696  btRigidBody *ptr = (btRigidBody *)body->get_object();
697  _soft->appendAnchor(node, ptr, disable);
698 }
699 
700 ////////////////////////////////////////////////////////////////////
701 // Function: BulletSoftBodyNode::append_anchor
702 // Access: Published
703 // Description:
704 ////////////////////////////////////////////////////////////////////
705 void BulletSoftBodyNode::
706 append_anchor(int node, BulletRigidBodyNode *body, const LVector3 &pivot, bool disable) {
707 
708  nassertv(node < _soft->m_nodes.size())
709  nassertv(body);
710  nassertv(!pivot.is_nan());
711 
712  body->sync_p2b();
713 
714  btRigidBody *ptr = (btRigidBody *)body->get_object();
715  _soft->appendAnchor(node, ptr, LVecBase3_to_btVector3(pivot), disable);
716 }
717 
718 ////////////////////////////////////////////////////////////////////
719 // Function: BulletSoftBodyNodeElement::Constructor
720 // Access: Public
721 // Description:
722 ////////////////////////////////////////////////////////////////////
723 BulletSoftBodyNodeElement::
724 BulletSoftBodyNodeElement(btSoftBody::Node &node) : _node(node) {
725 
726 }
727 
728 ////////////////////////////////////////////////////////////////////
729 // Function: BulletSoftBodyNode::get_point_index
730 // Access: Private
731 // Description: Returns the index of the first point within an
732 // array of points which has about the same
733 // coordinates as the given point. If no points
734 // is found -1 is returned.
735 ////////////////////////////////////////////////////////////////////
736 int BulletSoftBodyNode::
737 get_point_index(LVecBase3 p, PTA_LVecBase3 points) {
738 
739  PN_stdfloat eps = 1.0e-6f; // TODO make this a config option
740 
741  for (PTA_LVecBase3::size_type i=0; i<points.size(); i++) {
742  if (points[i].almost_equal(p, eps)) {
743  return i; // Found
744  }
745  }
746 
747  return -1; // Not found
748 }
749 
750 ////////////////////////////////////////////////////////////////////
751 // Function: BulletSoftBodyNode::next_line
752 // Access: Published
753 // Description: Read on until the next linebreak is detected, or
754 // the end of file has been reached.
755 ////////////////////////////////////////////////////////////////////
756 int BulletSoftBodyNode::
757 next_line(const char* buffer) {
758 
759  int num_bytes_read = 0;
760 
761  while (*buffer != '\n') {
762  buffer++;
763  num_bytes_read++;
764  }
765 
766  if (buffer[0] == 0x0a) {
767  buffer++;
768  num_bytes_read++;
769  }
770 
771  return num_bytes_read;
772 }
773 
774 ////////////////////////////////////////////////////////////////////
775 // Function: BulletSoftBodyNode::make_rope
776 // Access: Published
777 // Description:
778 ////////////////////////////////////////////////////////////////////
779 PT(BulletSoftBodyNode) BulletSoftBodyNode::
780 make_rope(BulletSoftBodyWorldInfo &info, const LPoint3 &from, const LPoint3 &to, int res, int fixeds) {
781 
782  btSoftBody *body = btSoftBodyHelpers::CreateRope(
783  info.get_info(),
784  LVecBase3_to_btVector3(from),
785  LVecBase3_to_btVector3(to),
786  res,
787  fixeds);
788 
789  PT(BulletSoftBodyNode) node = new BulletSoftBodyNode(body);
790 
791  return node;
792 }
793 
794 ////////////////////////////////////////////////////////////////////
795 // Function: BulletSoftBodyNode::make_patch
796 // Access: Published
797 // Description:
798 ////////////////////////////////////////////////////////////////////
799 PT(BulletSoftBodyNode) BulletSoftBodyNode::
800 make_patch(BulletSoftBodyWorldInfo &info, const LPoint3 &corner00, const LPoint3 &corner10, const LPoint3 &corner01, const LPoint3 &corner11, int resx, int resy, int fixeds, bool gendiags) {
801 
802  btSoftBody *body = btSoftBodyHelpers::CreatePatch(
803  info.get_info(),
804  LVecBase3_to_btVector3(corner00),
805  LVecBase3_to_btVector3(corner10),
806  LVecBase3_to_btVector3(corner01),
807  LVecBase3_to_btVector3(corner11),
808  resx,
809  resy,
810  fixeds,
811  gendiags);
812 
813  PT(BulletSoftBodyNode) node = new BulletSoftBodyNode(body);
814 
815  return node;
816 }
817 
818 ////////////////////////////////////////////////////////////////////
819 // Function: BulletSoftBodyNode::make_ellipsoid
820 // Access: Published
821 // Description:
822 ////////////////////////////////////////////////////////////////////
823 PT(BulletSoftBodyNode) BulletSoftBodyNode::
824 make_ellipsoid(BulletSoftBodyWorldInfo &info, const LPoint3 &center, const LVecBase3 &radius, int res) {
825 
826  btSoftBody *body = btSoftBodyHelpers::CreateEllipsoid(
827  info.get_info(),
828  LVecBase3_to_btVector3(center),
829  LVecBase3_to_btVector3(radius),
830  res);
831 
832  PT(BulletSoftBodyNode) node = new BulletSoftBodyNode(body);
833 
834  return node;
835 }
836 
837 ////////////////////////////////////////////////////////////////////
838 // Function: BulletSoftBodyNode::make_tri_mesh
839 // Access: Published
840 // Description:
841 ////////////////////////////////////////////////////////////////////
842 PT(BulletSoftBodyNode) BulletSoftBodyNode::
843 make_tri_mesh(BulletSoftBodyWorldInfo &info, PTA_LVecBase3 points, PTA_int indices, bool randomizeConstraints) {
844 
845  // Eliminate duplicate vertices
846  PTA_LVecBase3 mapped_points;
847  PTA_int mapped_indices;
848 
849  pmap<int, int> mapping;
850 
851  for (PTA_LVecBase3::size_type i=0; i<points.size(); i++) {
852  LVecBase3 p = points[i];
853  int j = get_point_index(p, mapped_points);
854  if (j < 0) {
855  mapping[i] = mapped_points.size();
856  mapped_points.push_back(p);
857  }
858  else {
859  mapping[i] = j;
860  }
861  }
862 
863  for (PTA_int::size_type i=0; i<indices.size(); i++) {
864  int idx = indices[i];
865  int mapped_idx = mapping[idx];
866  mapped_indices.push_back(mapped_idx);
867  }
868 
869  points = mapped_points;
870  indices = mapped_indices;
871 
872  // Convert arrays
873  int num_vertices = points.size();
874  int num_triangles = indices.size() / 3;
875 
876  btScalar *vertices = new btScalar[num_vertices * 3];
877  for (int i=0; i < num_vertices; i++) {
878  vertices[3*i] = points[i].get_x();
879  vertices[3*i+1] = points[i].get_y();
880  vertices[3*i+2] = points[i].get_z();
881  }
882 
883  int *triangles = new int[num_triangles * 3];
884  for (int i=0; i < num_triangles * 3; i++) {
885  triangles[i] = indices[i];
886  }
887 
888  // Create body
889  btSoftBody *body = btSoftBodyHelpers::CreateFromTriMesh(
890  info.get_info(),
891  vertices,
892  triangles,
893  num_triangles,
894  randomizeConstraints);
895 
896  nassertr(body, NULL);
897 
898  delete[] vertices;
899  delete[] triangles;
900 
901  PT(BulletSoftBodyNode) node = new BulletSoftBodyNode(body);
902 
903  return node;
904 }
905 
906 ////////////////////////////////////////////////////////////////////
907 // Function: BulletSoftBodyNode::make_tri_mesh
908 // Access: Published
909 // Description:
910 ////////////////////////////////////////////////////////////////////
911 PT(BulletSoftBodyNode) BulletSoftBodyNode::
912 make_tri_mesh(BulletSoftBodyWorldInfo &info, const Geom *geom, bool randomizeConstraints) {
913 
914  // Read vertex data
915  PTA_LVecBase3 points;
916  PTA_int indices;
917 
918  CPT(GeomVertexData) vdata = geom->get_vertex_data();
919 
920  nassertr(vdata->has_column(InternalName::get_vertex()), NULL);
921 
922  GeomVertexReader vreader(vdata, InternalName::get_vertex());
923 
924  while (!vreader.is_at_end()) {
925  LVecBase3 v = vreader.get_data3();
926  points.push_back(v);
927  }
928 
929  // Read indices
930  for (int i=0; i<geom->get_num_primitives(); i++) {
931 
932  CPT(GeomPrimitive) prim = geom->get_primitive(i);
933  prim = prim->decompose();
934 
935  for (int j=0; j<prim->get_num_primitives(); j++) {
936 
937  int s = prim->get_primitive_start(j);
938  int e = prim->get_primitive_end(j);
939 
940  for (int k=s; k<e; k++) {
941  indices.push_back(prim->get_vertex(k));
942  }
943  }
944  }
945 
946  // Create body
947  return make_tri_mesh(info, points, indices, randomizeConstraints);
948 }
949 
950 ////////////////////////////////////////////////////////////////////
951 // Function: BulletSoftBodyNode::make_tet_mesh
952 // Access: Published
953 // Description:
954 ////////////////////////////////////////////////////////////////////
955 PT(BulletSoftBodyNode) BulletSoftBodyNode::
956 make_tet_mesh(BulletSoftBodyWorldInfo &info, PTA_LVecBase3 points, PTA_int indices, bool tetralinks) {
957 
958  // Points
959  btAlignedObjectArray<btVector3> pos;
960  pos.resize(points.size());
961  for (PTA_LVecBase3::size_type i=0; i<points.size(); i++) {
962  LVecBase3 point = points[i];
963  pos[i] = LVecBase3_to_btVector3(point);
964  }
965 
966  // Body
967  btSoftBody* body = new btSoftBody(&info.get_info(), pos.size(), &pos[0], 0);
968 
969  // Indices
970  for (PTA_int::size_type i=0; i<indices.size() / 4; i++) {
971  int ni[4];
972 
973  ni[0] = indices[4*i];
974  ni[1] = indices[4*i+1];
975  ni[2] = indices[4*i+2];
976  ni[3] = indices[4*i+3];
977 
978  body->appendTetra(ni[0],ni[1],ni[2],ni[3]);
979 
980  if (tetralinks) {
981  body->appendLink(ni[0], ni[1], 0, true);
982  body->appendLink(ni[1], ni[2], 0, true);
983  body->appendLink(ni[2], ni[0], 0, true);
984  body->appendLink(ni[0], ni[3], 0, true);
985  body->appendLink(ni[1], ni[3], 0, true);
986  body->appendLink(ni[2], ni[3], 0, true);
987  }
988  }
989 
990  // Node
991  PT(BulletSoftBodyNode) node = new BulletSoftBodyNode(body);
992 
993  return node;
994 }
995 
996 ////////////////////////////////////////////////////////////////////
997 // Function: BulletSoftBodyNode::make_tet_mesh
998 // Access: Published
999 // Description:
1000 ////////////////////////////////////////////////////////////////////
1001 PT(BulletSoftBodyNode) BulletSoftBodyNode::
1002 make_tet_mesh(BulletSoftBodyWorldInfo &info, const char *ele, const char *face, const char *node) {
1003 
1004  nassertr(node && node[0], NULL);
1005 
1006  // Nodes
1007  btAlignedObjectArray<btVector3> pos;
1008 
1009  int npos = 0;
1010  int ndims = 0; // not used
1011  int nattrb = 0; // not used
1012  int hasbounds = 0; // not used
1013 
1014  sscanf(node, "%d %d %d %d", &npos, &ndims, &nattrb, &hasbounds);
1015  node += next_line(node);
1016 
1017  pos.resize(npos);
1018 
1019  for (int i=0; i<pos.size(); ++i) {
1020  int index = 0;
1021  float x, y, z;
1022 
1023  sscanf(node, "%d %f %f %f", &index, &x, &y, &z);
1024  node += next_line(node);
1025 
1026  pos[index].setX(btScalar(x));
1027  pos[index].setY(btScalar(y));
1028  pos[index].setZ(btScalar(z));
1029  }
1030 
1031  // Body
1032  btSoftBody *body = new btSoftBody(&info.get_info(), npos, &pos[0], 0);
1033 
1034  // Faces
1035  if (face && face[0]) {
1036  int nface = 0;
1037  int hasbounds = 0; // not used
1038 
1039  sscanf(face, "%d %d", &nface, &hasbounds);
1040  face += next_line(face);
1041 
1042  for (int i=0; i<nface; ++i) {
1043  int index = 0;
1044  int ni[3];
1045 
1046  sscanf(face, "%d %d %d %d", &index, &ni[0], &ni[1], &ni[2]);
1047  face += next_line(face);
1048 
1049  body->appendFace(ni[0], ni[1], ni[2]);
1050  }
1051  }
1052 
1053  // Links
1054  if (ele && ele[0]) {
1055  int ntetra = 0;
1056  int ncorner = 0;
1057  int neattrb = 0;
1058 
1059  sscanf(ele, "%d %d %d", &ntetra, &ncorner, &neattrb);
1060  ele += next_line(ele);
1061 
1062  for (int i=0; i<ntetra; ++i) {
1063  int index = 0;
1064  int ni[4];
1065 
1066  sscanf(ele, "%d %d %d %d %d", &index, &ni[0], &ni[1], &ni[2], &ni[3]);
1067  ele += next_line(ele);
1068 
1069  body->appendTetra(ni[0], ni[1], ni[2], ni[3]);
1070 
1071  body->appendLink(ni[0], ni[1], 0, true);
1072  body->appendLink(ni[1], ni[2], 0, true);
1073  body->appendLink(ni[2], ni[0], 0, true);
1074  body->appendLink(ni[0], ni[3], 0, true);
1075  body->appendLink(ni[1], ni[3], 0, true);
1076  body->appendLink(ni[2], ni[3], 0, true);
1077  }
1078  }
1079 
1080  // Node
1081  PT(BulletSoftBodyNode) sbnode = new BulletSoftBodyNode(body);
1082 
1083  return sbnode;
1084 }
1085 
1086 ////////////////////////////////////////////////////////////////////
1087 // Function: BulletSoftBodyNode::append_linear_joint
1088 // Access: Published
1089 // Description:
1090 ////////////////////////////////////////////////////////////////////
1091 void BulletSoftBodyNode::
1092 append_linear_joint(BulletBodyNode *body, int cluster, PN_stdfloat erp, PN_stdfloat cfm, PN_stdfloat split) {
1093 
1094  nassertv(body);
1095 
1096  btCollisionObject *ptr = body->get_object();
1097 
1098  btSoftBody::LJoint::Specs ls;
1099  ls.erp = erp;
1100  ls.cfm = cfm;
1101  ls.split = split;
1102  ls.position = _soft->clusterCom(cluster);
1103 
1104  _soft->appendLinearJoint(ls, ptr);
1105 }
1106 
1107 ////////////////////////////////////////////////////////////////////
1108 // Function: BulletSoftBodyNode::append_linear_joint
1109 // Access: Published
1110 // Description:
1111 ////////////////////////////////////////////////////////////////////
1112 void BulletSoftBodyNode::
1113 append_linear_joint(BulletBodyNode *body, const LPoint3 &pos, PN_stdfloat erp, PN_stdfloat cfm, PN_stdfloat split) {
1114 
1115  nassertv(body);
1116 
1117  btCollisionObject *ptr = body->get_object();
1118 
1119  btSoftBody::LJoint::Specs ls;
1120  ls.erp = erp;
1121  ls.cfm = cfm;
1122  ls.split = split;
1123  ls.position = LVecBase3_to_btVector3(pos);
1124 
1125  _soft->appendLinearJoint(ls, ptr);
1126 }
1127 
1128 ////////////////////////////////////////////////////////////////////
1129 // Function: BulletSoftBodyNode::append_angular_joint
1130 // Access: Published
1131 // Description:
1132 ////////////////////////////////////////////////////////////////////
1133 void BulletSoftBodyNode::
1134 append_angular_joint(BulletBodyNode *body, const LVector3 &axis, PN_stdfloat erp, PN_stdfloat cfm, PN_stdfloat split, BulletSoftBodyControl *control) {
1135 
1136  nassertv(body);
1137 
1138  btCollisionObject *ptr = body->get_object();
1139 
1140  btSoftBody::AJoint::Specs as;
1141  as.erp = erp;
1142  as.cfm = cfm;
1143  as.split = split;
1144  as.axis = LVecBase3_to_btVector3(axis);
1145  as.icontrol = control ? control : btSoftBody::AJoint::IControl::Default();
1146 
1147  _soft->appendAngularJoint(as, ptr);
1148 }
1149 
1150 ////////////////////////////////////////////////////////////////////
1151 // Function: BulletSoftBodyNode::set_wind_velocity
1152 // Access: Published
1153 // Description:
1154 ////////////////////////////////////////////////////////////////////
1155 void BulletSoftBodyNode::
1156 set_wind_velocity(const LVector3 &velocity) {
1157 
1158  nassertv(!velocity.is_nan());
1159  _soft->setWindVelocity(LVecBase3_to_btVector3(velocity));
1160 }
1161 
1162 ////////////////////////////////////////////////////////////////////
1163 // Function: BulletSoftBodyNode::get_wind_velocity
1164 // Access: Published
1165 // Description:
1166 ////////////////////////////////////////////////////////////////////
1167 LVector3 BulletSoftBodyNode::
1168 get_wind_velocity() const {
1169 
1170  return btVector3_to_LVector3(_soft->getWindVelocity());
1171 }
1172 
A basic node of the scene graph or data graph.
Definition: pandaNode.h:72
An axis-aligned bounding box; that is, a minimum and maximum coordinate triple.
Definition: boundingBox.h:31
This is the base class for all three-component vectors and points.
Definition: lvecBase3.h:105
bool almost_equal(const LMatrix4f &other, float threshold) const
Returns true if two matrices are memberwise equal within a specified tolerance.
Definition: lmatrix.cxx:879
This class is an abstraction for evaluating NURBS curves.
This is an abstract base class for a family of classes that represent the fundamental geometry primit...
Definition: geomPrimitive.h:63
This is a three-component vector distance (as opposed to a three-component point, which represents a ...
Definition: lvector3.h:100
void set_data1i(int data)
Sets the write row to a particular 1-component value, and advances the write row. ...
This is a three-component point in space (as opposed to a three-component vector, which represents a ...
Definition: lpoint3.h:99
This class is an abstraction for evaluating NURBS surfaces.
bool is_nan() const
Returns true if any component of the vector is not-a-number, false otherwise.
Definition: lvecBase3.h:464
int get_num_v_vertices() const
Returns the number of control vertices in the V direction on the surface.
static Thread * get_current_thread()
Returns a pointer to the currently-executing Thread object.
Definition: thread.I:145
void set_transform(const TransformState *transform, Thread *current_thread=Thread::get_current_thread())
Changes the complete transform object on this node.
Definition: nodePath.I:718
int get_num_u_vertices() const
Returns the number of control vertices in the U direction on the surface.
static NodePath any_path(PandaNode *node, Thread *current_thread=Thread::get_current_thread())
Returns a new NodePath that represents any arbitrary path from the root to the indicated node...
Definition: nodePath.I:77
int get_closest_node_index(LVecBase3 point, bool local)
Returns the index of the node which is closest to the given point.
static BulletSoftBodyNodeElement empty()
Named constructor intended to be used for asserts with have to return a concrete value.
This is a 4-by-4 transform matrix.
Definition: lmatrix.h:451
static BulletSoftBodyMaterial empty()
Named constructor intended to be used for asserts with have to return a concrete value.
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
const LVecBase3 & get_data3()
Returns the data associated with the read row, expressed as a 3-component value, and advances the rea...
int get_num_vertices() const
Returns the number of control vertices in the curve.
A thread; that is, a lightweight process.
Definition: thread.h:51
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...
LVecBase3 get_scale() const
Retrieves the scale component of the transform.
Definition: nodePath.cxx:1331
TypeHandle is the identifier used to differentiate C++ class types.
Definition: typeHandle.h:85
NodePath is the fundamental system for disambiguating instances, and also provides a higher-level int...
Definition: nodePath.h:165
This object provides the functionality of both a GeomVertexReader and a GeomVertexWriter, combined together into one convenient package.