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 #if BT_BULLET_VERSION >= 290
191 btVector3 origin = _soft->getWorldTransform().getOrigin();
193 btVector3 origin = _soft->m_initialWorldTransform.getOrigin();
195 btVector3 offset = pos - origin;
198 trans.setOrigin(trans.getOrigin() - offset);
201 #if BT_BULLET_VERSION >= 290
202 _soft->transform(_soft->getWorldTransform().inverse());
204 _soft->transform(_soft->m_initialWorldTransform.inverse());
206 _soft->transform(trans);
208 if (ts->has_scale()) {
209 btVector3 current_scale = LVecBase3_to_btVector3(_sync->get_scale());
210 btVector3 new_scale = LVecBase3_to_btVector3(ts->get_scale());
212 current_scale.setX(1.0 / current_scale.getX());
213 current_scale.setY(1.0 / current_scale.getY());
214 current_scale.setZ(1.0 / current_scale.getZ());
216 _soft->scale(current_scale);
217 _soft->scale(new_scale);
220 _sync = std::move(ts);
227 void BulletSoftBodyNode::
241 btTransform trans = btTransform::getIdentity();
242 get_node_transform(trans,
this);
252 btSoftBody::Node node = _soft->m_nodes[indices.
get_data1i()];
253 btVector3 v = trans.invXform(node.m_x);
254 btVector3 n = node.m_n;
258 vertices.
set_data3((PN_stdfloat)v.getX(), (PN_stdfloat)v.getY(), (PN_stdfloat)v.getZ());
259 normals.
set_data3((PN_stdfloat)n.getX(), (PN_stdfloat)n.getY(), (PN_stdfloat)n.getZ());
264 btSoftBody::tNodeArray &nodes(_soft->m_nodes);
266 for (
int i=0; i < nodes.size(); i++) {
267 btVector3 pos = nodes[i].m_x;
268 _curve->set_vertex(i, btVector3_to_LPoint3(pos));
273 btSoftBody::tNodeArray &nodes(_soft->m_nodes);
275 int num_u = _surface->get_num_u_vertices();
276 int num_v = _surface->get_num_v_vertices();
277 nassertv(num_u * num_v == nodes.size());
279 for (
int u=0; u < num_u; u++) {
280 for (
int v=0; v < num_v; v++) {
281 btVector3 pos = nodes[u * num_u + v].m_x;
282 _surface->set_vertex(u, v, btVector3_to_LPoint3(pos));
289 btVector3 pMin, pMax;
290 _soft->getAabb(pMin, pMax);
291 LPoint3 pos = (btVector3_to_LPoint3(pMin) + btVector3_to_LPoint3(pMax)) * 0.5;
294 ts = TransformState::make_pos(pos);
296 ts = TransformState::make_identity();
300 LVecBase3 scale = np.get_net_transform()->
get_scale();
301 ts = ts->set_scale(scale);
304 _sync_disable =
true;
306 _sync_disable =
false;
311 this->r_mark_geom_bounds_stale(current_thread);
323 return do_get_closest_node_index(point, local);
332 int BulletSoftBodyNode::
333 do_get_closest_node_index(LVecBase3 point,
bool local) {
335 btScalar max_dist_sqr = 1e30;
336 btVector3 point_x = LVecBase3_to_btVector3(point);
338 btTransform trans = btTransform::getIdentity();
340 get_node_transform(trans,
this);
343 btSoftBody::tNodeArray &nodes(_soft->m_nodes);
346 for (
int i=0; i<nodes.size(); ++i) {
348 btVector3 node_x = nodes[i].m_x;
349 btScalar dist_sqr = (trans.invXform(node_x) - point_x).length2();
351 if (dist_sqr < max_dist_sqr) {
352 max_dist_sqr = dist_sqr;
363 void BulletSoftBodyNode::
364 link_geom(
Geom *geom) {
367 nassertv(geom->get_vertex_data()->has_column(InternalName::get_vertex()));
368 nassertv(geom->get_vertex_data()->has_column(InternalName::get_normal()));
376 if (!vdata->has_column(BulletHelper::get_sb_index())) {
378 format = BulletHelper::add_sb_index_column(format);
379 vdata->set_format(format);
382 if (!vdata->has_column(BulletHelper::get_sb_flip())) {
384 format = BulletHelper::add_sb_flip_column(format);
385 vdata->set_format(format);
391 while (!vertices.is_at_end()) {
392 LVecBase3 point = vertices.get_data3();
393 int node_idx = do_get_closest_node_index(point,
true);
394 indices.set_data1i(node_idx);
401 void BulletSoftBodyNode::
411 void BulletSoftBodyNode::
423 void BulletSoftBodyNode::
433 void BulletSoftBodyNode::
445 void BulletSoftBodyNode::
459 return do_get_aabb();
466 do_get_aabb()
const {
471 _soft->getAabb(pMin, pMax);
474 btVector3_to_LPoint3(pMin),
475 btVector3_to_LPoint3(pMax)
482 void BulletSoftBodyNode::
483 set_volume_mass(PN_stdfloat mass) {
486 _soft->setVolumeMass(mass);
492 void BulletSoftBodyNode::
493 set_total_mass(PN_stdfloat mass,
bool fromfaces) {
496 _soft->setTotalMass(mass, fromfaces);
502 void BulletSoftBodyNode::
503 set_volume_density(PN_stdfloat density) {
506 _soft->setVolumeDensity(density);
512 void BulletSoftBodyNode::
513 set_total_density(PN_stdfloat density) {
516 _soft->setTotalDensity(density);
522 void BulletSoftBodyNode::
523 set_mass(
int node, PN_stdfloat mass) {
526 _soft->setMass(node, mass);
532 PN_stdfloat BulletSoftBodyNode::
533 get_mass(
int node)
const {
536 return _soft->getMass(node);
542 PN_stdfloat BulletSoftBodyNode::
543 get_total_mass()
const {
546 return _soft->getTotalMass();
552 PN_stdfloat BulletSoftBodyNode::
556 return _soft->getVolume();
562 void BulletSoftBodyNode::
563 add_force(
const LVector3 &force) {
566 nassertv(!force.is_nan());
567 _soft->addForce(LVecBase3_to_btVector3(force));
573 void BulletSoftBodyNode::
574 add_force(
const LVector3 &force,
int node) {
577 nassertv(!force.is_nan());
578 _soft->addForce(LVecBase3_to_btVector3(force), node);
584 void BulletSoftBodyNode::
585 set_velocity(
const LVector3 &velocity) {
588 nassertv(!velocity.is_nan());
589 _soft->setVelocity(LVecBase3_to_btVector3(velocity));
595 void BulletSoftBodyNode::
596 add_velocity(
const LVector3 &velocity) {
599 nassertv(!velocity.is_nan());
600 _soft->addVelocity(LVecBase3_to_btVector3(velocity));
606 void BulletSoftBodyNode::
607 add_velocity(
const LVector3 &velocity,
int node) {
610 nassertv(!velocity.is_nan());
611 _soft->addVelocity(LVecBase3_to_btVector3(velocity), node);
617 void BulletSoftBodyNode::
618 generate_clusters(
int k,
int maxiterations) {
621 _soft->generateClusters(k, maxiterations);
627 void BulletSoftBodyNode::
631 _soft->releaseClusters();
637 void BulletSoftBodyNode::
638 release_cluster(
int index) {
641 _soft->releaseCluster(index);
647 int BulletSoftBodyNode::
648 get_num_clusters()
const {
651 return _soft->clusterCount();
657 LVecBase3 BulletSoftBodyNode::
658 cluster_com(
int cluster)
const {
661 return btVector3_to_LVecBase3(_soft->clusterCom(cluster));
667 void BulletSoftBodyNode::
668 set_pose(
bool bvolume,
bool bframe) {
671 _soft->setPose(bvolume, bframe);
677 void BulletSoftBodyNode::
681 nassertv(node < _soft->m_nodes.size())
686 btRigidBody *ptr = (btRigidBody *)body->get_object();
687 _soft->appendAnchor(node, ptr, disable);
697 nassertv(node < _soft->m_nodes.size())
699 nassertv(!pivot.is_nan());
703 btRigidBody *ptr = (btRigidBody *)body->get_object();
704 _soft->appendAnchor(node, ptr, LVecBase3_to_btVector3(pivot), disable);
720 int BulletSoftBodyNode::
721 get_point_index(LVecBase3 p, PTA_LVecBase3 points) {
724 PN_stdfloat eps = 1.0e-6f;
726 for (PTA_LVecBase3::size_type i=0; i<points.size(); i++) {
727 if (points[i].almost_equal(p, eps)) {
739 int BulletSoftBodyNode::
740 next_line(
const char* buffer) {
742 int num_bytes_read = 0;
744 while (*buffer !=
'\n') {
749 if (buffer[0] == 0x0a) {
754 return num_bytes_read;
763 btSoftBody *body = btSoftBodyHelpers::CreateRope(
765 LVecBase3_to_btVector3(from),
766 LVecBase3_to_btVector3(to),
779 make_patch(
BulletSoftBodyWorldInfo &info,
const LPoint3 &corner00,
const LPoint3 &corner10,
const LPoint3 &corner01,
const LPoint3 &corner11,
int resx,
int resy,
int fixeds,
bool gendiags) {
781 btSoftBody *body = btSoftBodyHelpers::CreatePatch(
783 LVecBase3_to_btVector3(corner00),
784 LVecBase3_to_btVector3(corner10),
785 LVecBase3_to_btVector3(corner01),
786 LVecBase3_to_btVector3(corner11),
803 btSoftBody *body = btSoftBodyHelpers::CreateEllipsoid(
805 LVecBase3_to_btVector3(center),
806 LVecBase3_to_btVector3(radius),
818 make_tri_mesh(
BulletSoftBodyWorldInfo &info, PTA_LVecBase3 points, PTA_int indices,
bool randomizeConstraints) {
821 PTA_LVecBase3 mapped_points;
822 PTA_int mapped_indices;
826 for (PTA_LVecBase3::size_type i=0; i<points.size(); i++) {
827 LVecBase3 p = points[i];
828 int j = get_point_index(p, mapped_points);
830 mapping[i] = mapped_points.size();
831 mapped_points.push_back(p);
838 for (PTA_int::size_type i=0; i<indices.size(); i++) {
839 int idx = indices[i];
840 int mapped_idx = mapping[idx];
841 mapped_indices.push_back(mapped_idx);
844 points = mapped_points;
845 indices = mapped_indices;
848 int num_vertices = points.size();
849 int num_triangles = indices.size() / 3;
851 btScalar *vertices =
new btScalar[num_vertices * 3];
852 for (
int i=0; i < num_vertices; i++) {
853 vertices[3*i] = points[i].get_x();
854 vertices[3*i+1] = points[i].get_y();
855 vertices[3*i+2] = points[i].get_z();
858 int *triangles =
new int[num_triangles * 3];
859 for (
int i=0; i < num_triangles * 3; i++) {
860 triangles[i] = indices[i];
864 btSoftBody *body = btSoftBodyHelpers::CreateFromTriMesh(
869 randomizeConstraints);
871 nassertr(body,
nullptr);
888 PTA_LVecBase3 points;
893 nassertr(vdata->has_column(InternalName::get_vertex()),
nullptr);
897 while (!vreader.is_at_end()) {
898 LVecBase3 v = vreader.get_data3();
903 for (
size_t i = 0; i < geom->get_num_primitives(); ++i) {
906 prim = prim->decompose();
908 for (
int j=0; j<prim->get_num_primitives(); j++) {
910 int s = prim->get_primitive_start(j);
911 int e = prim->get_primitive_end(j);
913 for (
int k=s; k<e; k++) {
914 indices.push_back(prim->get_vertex(k));
920 return make_tri_mesh(info, points, indices, randomizeConstraints);
930 btAlignedObjectArray<btVector3> pos;
931 pos.resize(points.size());
932 for (PTA_LVecBase3::size_type i=0; i<points.size(); i++) {
933 LVecBase3 point = points[i];
934 pos[i] = LVecBase3_to_btVector3(point);
938 btSoftBody* body =
new btSoftBody(&info.get_info(), pos.size(), &pos[0], 0);
941 for (PTA_int::size_type i=0; i<indices.size() / 4; i++) {
944 ni[0] = indices[4*i];
945 ni[1] = indices[4*i+1];
946 ni[2] = indices[4*i+2];
947 ni[3] = indices[4*i+3];
949 body->appendTetra(ni[0],ni[1],ni[2],ni[3]);
952 body->appendLink(ni[0], ni[1], 0,
true);
953 body->appendLink(ni[1], ni[2], 0,
true);
954 body->appendLink(ni[2], ni[0], 0,
true);
955 body->appendLink(ni[0], ni[3], 0,
true);
956 body->appendLink(ni[1], ni[3], 0,
true);
957 body->appendLink(ni[2], ni[3], 0,
true);
973 nassertr(node && node[0],
nullptr);
976 btAlignedObjectArray<btVector3> pos;
983 sscanf(node,
"%d %d %d %d", &npos, &ndims, &nattrb, &hasbounds);
984 node += next_line(node);
988 for (
int i=0; i<pos.size(); ++i) {
992 sscanf(node,
"%d %f %f %f", &index, &x, &y, &z);
993 node += next_line(node);
995 pos[index].setX(btScalar(x));
996 pos[index].setY(btScalar(y));
997 pos[index].setZ(btScalar(z));
1001 btSoftBody *body =
new btSoftBody(&info.get_info(), npos, &pos[0], 0);
1004 if (face && face[0]) {
1008 sscanf(face,
"%d %d", &nface, &hasbounds);
1009 face += next_line(face);
1011 for (
int i=0; i<nface; ++i) {
1015 sscanf(face,
"%d %d %d %d", &index, &ni[0], &ni[1], &ni[2]);
1016 face += next_line(face);
1018 body->appendFace(ni[0], ni[1], ni[2]);
1023 if (ele && ele[0]) {
1028 sscanf(ele,
"%d %d %d", &ntetra, &ncorner, &neattrb);
1029 ele += next_line(ele);
1031 for (
int i=0; i<ntetra; ++i) {
1035 sscanf(ele,
"%d %d %d %d %d", &index, &ni[0], &ni[1], &ni[2], &ni[3]);
1036 ele += next_line(ele);
1038 body->appendTetra(ni[0], ni[1], ni[2], ni[3]);
1040 body->appendLink(ni[0], ni[1], 0,
true);
1041 body->appendLink(ni[1], ni[2], 0,
true);
1042 body->appendLink(ni[2], ni[0], 0,
true);
1043 body->appendLink(ni[0], ni[3], 0,
true);
1044 body->appendLink(ni[1], ni[3], 0,
true);
1045 body->appendLink(ni[2], ni[3], 0,
true);
1058 void BulletSoftBodyNode::
1059 append_linear_joint(
BulletBodyNode *body,
int cluster, PN_stdfloat erp, PN_stdfloat cfm, PN_stdfloat split) {
1064 btCollisionObject *ptr = body->get_object();
1066 btSoftBody::LJoint::Specs ls;
1070 ls.position = _soft->clusterCom(cluster);
1072 _soft->appendLinearJoint(ls, ptr);
1078 void BulletSoftBodyNode::
1079 append_linear_joint(
BulletBodyNode *body,
const LPoint3 &pos, PN_stdfloat erp, PN_stdfloat cfm, PN_stdfloat split) {
1084 btCollisionObject *ptr = body->get_object();
1086 btSoftBody::LJoint::Specs ls;
1090 ls.position = LVecBase3_to_btVector3(pos);
1092 _soft->appendLinearJoint(ls, ptr);
1098 void BulletSoftBodyNode::
1104 btCollisionObject *ptr = body->get_object();
1106 btSoftBody::AJoint::Specs as;
1110 as.axis = LVecBase3_to_btVector3(axis);
1111 as.icontrol = control ? control : btSoftBody::AJoint::IControl::Default();
1113 _soft->appendAngularJoint(as, ptr);
1119 void BulletSoftBodyNode::
1120 set_wind_velocity(
const LVector3 &velocity) {
1123 nassertv(!velocity.is_nan());
1124 _soft->setWindVelocity(LVecBase3_to_btVector3(velocity));
1130 LVector3 BulletSoftBodyNode::
1131 get_wind_velocity()
const {
1134 return btVector3_to_LVector3(_soft->getWindVelocity());
1140 LPoint3 BulletSoftBodyNodeElement::
1144 return btVector3_to_LPoint3(_node.m_x);
1150 LVector3 BulletSoftBodyNodeElement::
1151 get_normal()
const {
1154 return btVector3_to_LVector3(_node.m_n);
1160 LVector3 BulletSoftBodyNodeElement::
1161 get_velocity()
const {
1164 return btVector3_to_LVector3(_node.m_v);
1170 PN_stdfloat BulletSoftBodyNodeElement::
1171 get_inv_mass()
const {
1174 return (PN_stdfloat)_node.m_im;
1180 PN_stdfloat BulletSoftBodyNodeElement::
1184 return (PN_stdfloat)_node.m_area;
1190 int BulletSoftBodyNodeElement::
1191 is_attached()
const {
1194 return (PN_stdfloat)_node.m_battach;