33 BulletSoftBodyNode(btSoftBody *body,
const char *name) :
BulletBodyNode(name) {
36 _sync = TransformState::make_identity();
37 _sync_disable =
false;
41 _soft->setUserPointer(
this);
44 btCollisionShape *shape_ptr = _soft->getCollisionShape();
46 nassertv(shape_ptr !=
nullptr);
47 nassertv(shape_ptr->getShapeType() == SOFTBODY_SHAPE_PROXYTYPE);
60 btCollisionObject *BulletSoftBodyNode::
89 int BulletSoftBodyNode::
90 get_num_materials()
const {
93 return _soft->m_materials.size();
100 get_material(
int idx)
const {
105 btSoftBody::Material *material = _soft->m_materials[idx];
116 btSoftBody::Material *material = _soft->appendMaterial();
125 int BulletSoftBodyNode::
126 get_num_nodes()
const {
129 return _soft->m_nodes.size();
136 get_node(
int idx)
const {
146 void BulletSoftBodyNode::
151 _soft->generateBendingConstraints(distance, &(material->get_material()));
154 _soft->generateBendingConstraints(distance);
161 void BulletSoftBodyNode::
162 randomize_constraints() {
165 _soft->randomizeConstraints();
171 void BulletSoftBodyNode::
172 transform_changed() {
173 if (_sync_disable)
return;
180 LMatrix4 m_sync = _sync->
get_mat();
181 LMatrix4 m_ts = ts->get_mat();
183 if (!m_sync.almost_equal(m_ts)) {
186 btTransform trans = TransformState_to_btTrans(ts);
189 btVector3 pos = LVecBase3_to_btVector3(this->do_get_aabb().get_approx_center());
190 btVector3 origin = _soft->m_initialWorldTransform.getOrigin();
191 btVector3 offset = pos - origin;
194 trans.setOrigin(trans.getOrigin() - offset);
197 _soft->transform(_soft->m_initialWorldTransform.inverse());
198 _soft->transform(trans);
200 if (ts->has_scale()) {
201 btVector3 current_scale = LVecBase3_to_btVector3(_sync->get_scale());
202 btVector3 new_scale = LVecBase3_to_btVector3(ts->get_scale());
204 current_scale.setX(1.0 / current_scale.getX());
205 current_scale.setY(1.0 / current_scale.getY());
206 current_scale.setZ(1.0 / current_scale.getZ());
208 _soft->scale(current_scale);
209 _soft->scale(new_scale);
212 _sync = std::move(ts);
219 void BulletSoftBodyNode::
233 btTransform trans = btTransform::getIdentity();
234 get_node_transform(trans,
this);
244 btSoftBody::Node node = _soft->m_nodes[indices.
get_data1i()];
245 btVector3 v = trans.invXform(node.m_x);
246 btVector3 n = node.m_n;
250 vertices.
set_data3((PN_stdfloat)v.getX(), (PN_stdfloat)v.getY(), (PN_stdfloat)v.getZ());
251 normals.
set_data3((PN_stdfloat)n.getX(), (PN_stdfloat)n.getY(), (PN_stdfloat)n.getZ());
256 btSoftBody::tNodeArray &nodes(_soft->m_nodes);
258 for (
int i=0; i < nodes.size(); i++) {
259 btVector3 pos = nodes[i].m_x;
260 _curve->set_vertex(i, btVector3_to_LPoint3(pos));
265 btSoftBody::tNodeArray &nodes(_soft->m_nodes);
267 int num_u = _surface->get_num_u_vertices();
268 int num_v = _surface->get_num_v_vertices();
269 nassertv(num_u * num_v == nodes.size());
271 for (
int u=0; u < num_u; u++) {
272 for (
int v=0; v < num_v; v++) {
273 btVector3 pos = nodes[u * num_u + v].m_x;
274 _surface->set_vertex(u, v, btVector3_to_LPoint3(pos));
281 btVector3 pMin, pMax;
282 _soft->getAabb(pMin, pMax);
283 LPoint3 pos = (btVector3_to_LPoint3(pMin) + btVector3_to_LPoint3(pMax)) * 0.5;
286 ts = TransformState::make_pos(pos);
288 ts = TransformState::make_identity();
292 LVecBase3 scale = np.get_net_transform()->
get_scale();
293 ts = ts->set_scale(scale);
296 _sync_disable =
true;
298 _sync_disable =
false;
303 this->r_mark_geom_bounds_stale(current_thread);
315 return do_get_closest_node_index(point, local);
324 int BulletSoftBodyNode::
325 do_get_closest_node_index(LVecBase3 point,
bool local) {
327 btScalar max_dist_sqr = 1e30;
328 btVector3 point_x = LVecBase3_to_btVector3(point);
330 btTransform trans = btTransform::getIdentity();
332 get_node_transform(trans,
this);
335 btSoftBody::tNodeArray &nodes(_soft->m_nodes);
338 for (
int i=0; i<nodes.size(); ++i) {
340 btVector3 node_x = nodes[i].m_x;
341 btScalar dist_sqr = (trans.invXform(node_x) - point_x).length2();
343 if (dist_sqr < max_dist_sqr) {
344 max_dist_sqr = dist_sqr;
355 void BulletSoftBodyNode::
356 link_geom(
Geom *geom) {
359 nassertv(geom->get_vertex_data()->has_column(InternalName::get_vertex()));
360 nassertv(geom->get_vertex_data()->has_column(InternalName::get_normal()));
368 if (!vdata->has_column(BulletHelper::get_sb_index())) {
370 format = BulletHelper::add_sb_index_column(format);
371 vdata->set_format(format);
374 if (!vdata->has_column(BulletHelper::get_sb_flip())) {
376 format = BulletHelper::add_sb_flip_column(format);
377 vdata->set_format(format);
383 while (!vertices.is_at_end()) {
384 LVecBase3 point = vertices.get_data3();
385 int node_idx = do_get_closest_node_index(point,
true);
386 indices.set_data1i(node_idx);
393 void BulletSoftBodyNode::
403 void BulletSoftBodyNode::
415 void BulletSoftBodyNode::
425 void BulletSoftBodyNode::
437 void BulletSoftBodyNode::
451 return do_get_aabb();
458 do_get_aabb()
const {
463 _soft->getAabb(pMin, pMax);
466 btVector3_to_LPoint3(pMin),
467 btVector3_to_LPoint3(pMax)
474 void BulletSoftBodyNode::
475 set_volume_mass(PN_stdfloat mass) {
478 _soft->setVolumeMass(mass);
484 void BulletSoftBodyNode::
485 set_total_mass(PN_stdfloat mass,
bool fromfaces) {
488 _soft->setTotalMass(mass, fromfaces);
494 void BulletSoftBodyNode::
495 set_volume_density(PN_stdfloat density) {
498 _soft->setVolumeDensity(density);
504 void BulletSoftBodyNode::
505 set_total_density(PN_stdfloat density) {
508 _soft->setTotalDensity(density);
514 void BulletSoftBodyNode::
515 set_mass(
int node, PN_stdfloat mass) {
518 _soft->setMass(node, mass);
524 PN_stdfloat BulletSoftBodyNode::
525 get_mass(
int node)
const {
528 return _soft->getMass(node);
534 PN_stdfloat BulletSoftBodyNode::
535 get_total_mass()
const {
538 return _soft->getTotalMass();
544 PN_stdfloat BulletSoftBodyNode::
548 return _soft->getVolume();
554 void BulletSoftBodyNode::
555 add_force(
const LVector3 &force) {
558 nassertv(!force.is_nan());
559 _soft->addForce(LVecBase3_to_btVector3(force));
565 void BulletSoftBodyNode::
566 add_force(
const LVector3 &force,
int node) {
569 nassertv(!force.is_nan());
570 _soft->addForce(LVecBase3_to_btVector3(force), node);
576 void BulletSoftBodyNode::
577 set_velocity(
const LVector3 &velocity) {
580 nassertv(!velocity.is_nan());
581 _soft->setVelocity(LVecBase3_to_btVector3(velocity));
587 void BulletSoftBodyNode::
588 add_velocity(
const LVector3 &velocity) {
591 nassertv(!velocity.is_nan());
592 _soft->addVelocity(LVecBase3_to_btVector3(velocity));
598 void BulletSoftBodyNode::
599 add_velocity(
const LVector3 &velocity,
int node) {
602 nassertv(!velocity.is_nan());
603 _soft->addVelocity(LVecBase3_to_btVector3(velocity), node);
609 void BulletSoftBodyNode::
610 generate_clusters(
int k,
int maxiterations) {
613 _soft->generateClusters(k, maxiterations);
619 void BulletSoftBodyNode::
623 _soft->releaseClusters();
629 void BulletSoftBodyNode::
630 release_cluster(
int index) {
633 _soft->releaseCluster(index);
639 int BulletSoftBodyNode::
640 get_num_clusters()
const {
643 return _soft->clusterCount();
649 LVecBase3 BulletSoftBodyNode::
650 cluster_com(
int cluster)
const {
653 return btVector3_to_LVecBase3(_soft->clusterCom(cluster));
659 void BulletSoftBodyNode::
660 set_pose(
bool bvolume,
bool bframe) {
663 _soft->setPose(bvolume, bframe);
669 void BulletSoftBodyNode::
673 nassertv(node < _soft->m_nodes.size())
678 btRigidBody *ptr = (btRigidBody *)body->get_object();
679 _soft->appendAnchor(node, ptr, disable);
689 nassertv(node < _soft->m_nodes.size())
691 nassertv(!pivot.is_nan());
695 btRigidBody *ptr = (btRigidBody *)body->get_object();
696 _soft->appendAnchor(node, ptr, LVecBase3_to_btVector3(pivot), disable);
712 int BulletSoftBodyNode::
713 get_point_index(LVecBase3 p, PTA_LVecBase3 points) {
716 PN_stdfloat eps = 1.0e-6f;
718 for (PTA_LVecBase3::size_type i=0; i<points.size(); i++) {
719 if (points[i].almost_equal(p, eps)) {
731 int BulletSoftBodyNode::
732 next_line(
const char* buffer) {
734 int num_bytes_read = 0;
736 while (*buffer !=
'\n') {
741 if (buffer[0] == 0x0a) {
746 return num_bytes_read;
755 btSoftBody *body = btSoftBodyHelpers::CreateRope(
757 LVecBase3_to_btVector3(from),
758 LVecBase3_to_btVector3(to),
771 make_patch(
BulletSoftBodyWorldInfo &info,
const LPoint3 &corner00,
const LPoint3 &corner10,
const LPoint3 &corner01,
const LPoint3 &corner11,
int resx,
int resy,
int fixeds,
bool gendiags) {
773 btSoftBody *body = btSoftBodyHelpers::CreatePatch(
775 LVecBase3_to_btVector3(corner00),
776 LVecBase3_to_btVector3(corner10),
777 LVecBase3_to_btVector3(corner01),
778 LVecBase3_to_btVector3(corner11),
795 btSoftBody *body = btSoftBodyHelpers::CreateEllipsoid(
797 LVecBase3_to_btVector3(center),
798 LVecBase3_to_btVector3(radius),
810 make_tri_mesh(
BulletSoftBodyWorldInfo &info, PTA_LVecBase3 points, PTA_int indices,
bool randomizeConstraints) {
813 PTA_LVecBase3 mapped_points;
814 PTA_int mapped_indices;
818 for (PTA_LVecBase3::size_type i=0; i<points.size(); i++) {
819 LVecBase3 p = points[i];
820 int j = get_point_index(p, mapped_points);
822 mapping[i] = mapped_points.size();
823 mapped_points.push_back(p);
830 for (PTA_int::size_type i=0; i<indices.size(); i++) {
831 int idx = indices[i];
832 int mapped_idx = mapping[idx];
833 mapped_indices.push_back(mapped_idx);
836 points = mapped_points;
837 indices = mapped_indices;
840 int num_vertices = points.size();
841 int num_triangles = indices.size() / 3;
843 btScalar *vertices =
new btScalar[num_vertices * 3];
844 for (
int i=0; i < num_vertices; i++) {
845 vertices[3*i] = points[i].get_x();
846 vertices[3*i+1] = points[i].get_y();
847 vertices[3*i+2] = points[i].get_z();
850 int *triangles =
new int[num_triangles * 3];
851 for (
int i=0; i < num_triangles * 3; i++) {
852 triangles[i] = indices[i];
856 btSoftBody *body = btSoftBodyHelpers::CreateFromTriMesh(
861 randomizeConstraints);
863 nassertr(body,
nullptr);
880 PTA_LVecBase3 points;
885 nassertr(vdata->has_column(InternalName::get_vertex()),
nullptr);
889 while (!vreader.is_at_end()) {
890 LVecBase3 v = vreader.get_data3();
895 for (
size_t i = 0; i < geom->get_num_primitives(); ++i) {
898 prim = prim->decompose();
900 for (
int j=0; j<prim->get_num_primitives(); j++) {
902 int s = prim->get_primitive_start(j);
903 int e = prim->get_primitive_end(j);
905 for (
int k=s; k<e; k++) {
906 indices.push_back(prim->get_vertex(k));
912 return make_tri_mesh(info, points, indices, randomizeConstraints);
922 btAlignedObjectArray<btVector3> pos;
923 pos.resize(points.size());
924 for (PTA_LVecBase3::size_type i=0; i<points.size(); i++) {
925 LVecBase3 point = points[i];
926 pos[i] = LVecBase3_to_btVector3(point);
930 btSoftBody* body =
new btSoftBody(&info.get_info(), pos.size(), &pos[0], 0);
933 for (PTA_int::size_type i=0; i<indices.size() / 4; i++) {
936 ni[0] = indices[4*i];
937 ni[1] = indices[4*i+1];
938 ni[2] = indices[4*i+2];
939 ni[3] = indices[4*i+3];
941 body->appendTetra(ni[0],ni[1],ni[2],ni[3]);
944 body->appendLink(ni[0], ni[1], 0,
true);
945 body->appendLink(ni[1], ni[2], 0,
true);
946 body->appendLink(ni[2], ni[0], 0,
true);
947 body->appendLink(ni[0], ni[3], 0,
true);
948 body->appendLink(ni[1], ni[3], 0,
true);
949 body->appendLink(ni[2], ni[3], 0,
true);
965 nassertr(node && node[0],
nullptr);
968 btAlignedObjectArray<btVector3> pos;
975 sscanf(node,
"%d %d %d %d", &npos, &ndims, &nattrb, &hasbounds);
976 node += next_line(node);
980 for (
int i=0; i<pos.size(); ++i) {
984 sscanf(node,
"%d %f %f %f", &index, &x, &y, &z);
985 node += next_line(node);
987 pos[index].setX(btScalar(x));
988 pos[index].setY(btScalar(y));
989 pos[index].setZ(btScalar(z));
993 btSoftBody *body =
new btSoftBody(&info.get_info(), npos, &pos[0], 0);
996 if (face && face[0]) {
1000 sscanf(face,
"%d %d", &nface, &hasbounds);
1001 face += next_line(face);
1003 for (
int i=0; i<nface; ++i) {
1007 sscanf(face,
"%d %d %d %d", &index, &ni[0], &ni[1], &ni[2]);
1008 face += next_line(face);
1010 body->appendFace(ni[0], ni[1], ni[2]);
1015 if (ele && ele[0]) {
1020 sscanf(ele,
"%d %d %d", &ntetra, &ncorner, &neattrb);
1021 ele += next_line(ele);
1023 for (
int i=0; i<ntetra; ++i) {
1027 sscanf(ele,
"%d %d %d %d %d", &index, &ni[0], &ni[1], &ni[2], &ni[3]);
1028 ele += next_line(ele);
1030 body->appendTetra(ni[0], ni[1], ni[2], ni[3]);
1032 body->appendLink(ni[0], ni[1], 0,
true);
1033 body->appendLink(ni[1], ni[2], 0,
true);
1034 body->appendLink(ni[2], ni[0], 0,
true);
1035 body->appendLink(ni[0], ni[3], 0,
true);
1036 body->appendLink(ni[1], ni[3], 0,
true);
1037 body->appendLink(ni[2], ni[3], 0,
true);
1050 void BulletSoftBodyNode::
1051 append_linear_joint(
BulletBodyNode *body,
int cluster, PN_stdfloat erp, PN_stdfloat cfm, PN_stdfloat split) {
1056 btCollisionObject *ptr = body->get_object();
1058 btSoftBody::LJoint::Specs ls;
1062 ls.position = _soft->clusterCom(cluster);
1064 _soft->appendLinearJoint(ls, ptr);
1070 void BulletSoftBodyNode::
1071 append_linear_joint(
BulletBodyNode *body,
const LPoint3 &pos, PN_stdfloat erp, PN_stdfloat cfm, PN_stdfloat split) {
1076 btCollisionObject *ptr = body->get_object();
1078 btSoftBody::LJoint::Specs ls;
1082 ls.position = LVecBase3_to_btVector3(pos);
1084 _soft->appendLinearJoint(ls, ptr);
1090 void BulletSoftBodyNode::
1096 btCollisionObject *ptr = body->get_object();
1098 btSoftBody::AJoint::Specs as;
1102 as.axis = LVecBase3_to_btVector3(axis);
1103 as.icontrol = control ? control : btSoftBody::AJoint::IControl::Default();
1105 _soft->appendAngularJoint(as, ptr);
1111 void BulletSoftBodyNode::
1112 set_wind_velocity(
const LVector3 &velocity) {
1115 nassertv(!velocity.is_nan());
1116 _soft->setWindVelocity(LVecBase3_to_btVector3(velocity));
1122 LVector3 BulletSoftBodyNode::
1123 get_wind_velocity()
const {
1126 return btVector3_to_LVector3(_soft->getWindVelocity());
1132 LPoint3 BulletSoftBodyNodeElement::
1136 return btVector3_to_LPoint3(_node.m_x);
1142 LVector3 BulletSoftBodyNodeElement::
1143 get_normal()
const {
1146 return btVector3_to_LVector3(_node.m_n);
1152 LVector3 BulletSoftBodyNodeElement::
1153 get_velocity()
const {
1156 return btVector3_to_LVector3(_node.m_v);
1162 PN_stdfloat BulletSoftBodyNodeElement::
1163 get_inv_mass()
const {
1166 return (PN_stdfloat)_node.m_im;
1172 PN_stdfloat BulletSoftBodyNodeElement::
1176 return (PN_stdfloat)_node.m_area;
1182 int BulletSoftBodyNodeElement::
1183 is_attached()
const {
1186 return (PN_stdfloat)_node.m_battach;