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" 23 #include "geomVertexRewriter.h" 24 #include "geomVertexReader.h" 34 BulletSoftBodyNode(btSoftBody *body,
const char *name) :
BulletBodyNode(name) {
37 _sync = TransformState::make_identity();
38 _sync_disable =
false;
42 _soft->setUserPointer(
this);
45 btCollisionShape *shape_ptr = _soft->getCollisionShape();
47 nassertv(shape_ptr != NULL);
48 nassertv(shape_ptr->getShapeType() == SOFTBODY_SHAPE_PROXYTYPE);
63 btCollisionObject *BulletSoftBodyNode::
96 int BulletSoftBodyNode::
97 get_num_materials()
const {
99 return _soft->m_materials.size();
108 get_material(
int idx)
const {
112 btSoftBody::Material *material = _soft->m_materials[idx];
124 btSoftBody::Material *material = _soft->appendMaterial();
135 int BulletSoftBodyNode::
136 get_num_nodes()
const {
138 return _soft->m_nodes.size();
147 get_node(
int idx)
const {
158 void BulletSoftBodyNode::
162 _soft->generateBendingConstraints(distance, &(material->get_material()));
165 _soft->generateBendingConstraints(distance);
174 void BulletSoftBodyNode::
175 randomize_constraints() {
177 _soft->randomizeConstraints();
185 void BulletSoftBodyNode::
186 transform_changed() {
188 if (_sync_disable)
return;
191 CPT(TransformState) ts = np.get_net_transform();
199 btTransform trans = TransformState_to_btTrans(ts);
202 btVector3 pos = LVecBase3_to_btVector3(this->get_aabb().get_approx_center());
203 btVector3 origin = _soft->m_initialWorldTransform.getOrigin();
204 btVector3 offset = pos - origin;
207 trans.setOrigin(trans.getOrigin() - offset);
210 _soft->transform(_soft->m_initialWorldTransform.inverse());
211 _soft->transform(trans);
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());
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());
221 _soft->scale(current_scale);
222 _soft->scale(new_scale);
234 void BulletSoftBodyNode::
245 void BulletSoftBodyNode::
250 btTransform trans = btTransform::getIdentity();
251 get_node_transform(trans,
this);
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;
265 if (flips.get_data1i() > 0) n *= -1;
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());
273 btSoftBody::tNodeArray &nodes(_soft->m_nodes);
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));
282 btSoftBody::tNodeArray &nodes(_soft->m_nodes);
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());
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));
298 LVecBase3 pos = this->get_aabb().get_approx_center();
299 CPT(TransformState) ts = TransformState::make_pos(pos);
303 ts = ts->set_scale(scale);
306 _sync_disable =
true;
308 _sync_disable =
false;
313 this->r_mark_geom_bounds_stale(current_thread);
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;
357 void BulletSoftBodyNode::
358 link_geom(
Geom *geom) {
360 nassertv(geom->get_vertex_data()->has_column(InternalName::get_vertex()));
361 nassertv(geom->get_vertex_data()->has_column(InternalName::get_normal()));
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);
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);
396 void BulletSoftBodyNode::
407 void BulletSoftBodyNode::
420 void BulletSoftBodyNode::
431 void BulletSoftBodyNode::
444 void BulletSoftBodyNode::
461 _soft->getAabb(pMin, pMax);
464 btVector3_to_LPoint3(pMin),
465 btVector3_to_LPoint3(pMax)
474 void BulletSoftBodyNode::
475 set_volume_mass(PN_stdfloat mass) {
477 _soft->setVolumeMass(mass);
485 void BulletSoftBodyNode::
486 set_total_mass(PN_stdfloat mass,
bool fromfaces) {
488 _soft->setTotalMass(mass, fromfaces);
496 void BulletSoftBodyNode::
497 set_volume_density(PN_stdfloat density) {
499 _soft->setVolumeDensity(density);
507 void BulletSoftBodyNode::
508 set_total_density(PN_stdfloat density) {
510 _soft->setTotalDensity(density);
518 void BulletSoftBodyNode::
519 set_mass(
int node, PN_stdfloat mass) {
521 _soft->setMass(node, mass);
529 PN_stdfloat BulletSoftBodyNode::
530 get_mass(
int node)
const {
532 return _soft->getMass(node);
540 PN_stdfloat BulletSoftBodyNode::
541 get_total_mass()
const {
543 return _soft->getTotalMass();
551 PN_stdfloat BulletSoftBodyNode::
554 return _soft->getVolume();
562 void BulletSoftBodyNode::
565 nassertv(!force.
is_nan());
566 _soft->addForce(LVecBase3_to_btVector3(force));
574 void BulletSoftBodyNode::
575 add_force(
const LVector3 &force,
int node) {
577 nassertv(!force.
is_nan());
578 _soft->addForce(LVecBase3_to_btVector3(force), node);
586 void BulletSoftBodyNode::
587 set_velocity(
const LVector3 &velocity) {
589 nassertv(!velocity.
is_nan());
590 _soft->setVelocity(LVecBase3_to_btVector3(velocity));
598 void BulletSoftBodyNode::
599 add_velocity(
const LVector3 &velocity) {
601 nassertv(!velocity.
is_nan());
602 _soft->addVelocity(LVecBase3_to_btVector3(velocity));
610 void BulletSoftBodyNode::
611 add_velocity(
const LVector3 &velocity,
int node) {
613 nassertv(!velocity.
is_nan());
614 _soft->addVelocity(LVecBase3_to_btVector3(velocity), node);
622 void BulletSoftBodyNode::
623 generate_clusters(
int k,
int maxiterations) {
625 _soft->generateClusters(k, maxiterations);
633 void BulletSoftBodyNode::
636 _soft->releaseClusters();
644 void BulletSoftBodyNode::
645 release_cluster(
int index) {
647 _soft->releaseCluster(index);
655 int BulletSoftBodyNode::
656 get_num_clusters()
const {
658 return _soft->clusterCount();
667 cluster_com(
int cluster)
const {
669 return btVector3_to_LVecBase3(_soft->clusterCom(cluster));
677 void BulletSoftBodyNode::
678 set_pose(
bool bvolume,
bool bframe) {
680 _soft->setPose(bvolume, bframe);
688 void BulletSoftBodyNode::
691 nassertv(node < _soft->m_nodes.size())
696 btRigidBody *ptr = (btRigidBody *)body->get_object();
697 _soft->appendAnchor(node, ptr, disable);
705 void BulletSoftBodyNode::
708 nassertv(node < _soft->m_nodes.size())
710 nassertv(!pivot.
is_nan());
714 btRigidBody *ptr = (btRigidBody *)body->get_object();
715 _soft->appendAnchor(node, ptr, LVecBase3_to_btVector3(pivot), disable);
723 BulletSoftBodyNodeElement::
724 BulletSoftBodyNodeElement(btSoftBody::Node &node) : _node(node) {
736 int BulletSoftBodyNode::
737 get_point_index(
LVecBase3 p, PTA_LVecBase3 points) {
739 PN_stdfloat eps = 1.0e-6f;
741 for (PTA_LVecBase3::size_type i=0; i<points.size(); i++) {
742 if (points[i].almost_equal(p, eps)) {
756 int BulletSoftBodyNode::
757 next_line(
const char* buffer) {
759 int num_bytes_read = 0;
761 while (*buffer !=
'\n') {
766 if (buffer[0] == 0x0a) {
771 return num_bytes_read;
782 btSoftBody *body = btSoftBodyHelpers::CreateRope(
784 LVecBase3_to_btVector3(from),
785 LVecBase3_to_btVector3(to),
802 btSoftBody *body = btSoftBodyHelpers::CreatePatch(
804 LVecBase3_to_btVector3(corner00),
805 LVecBase3_to_btVector3(corner10),
806 LVecBase3_to_btVector3(corner01),
807 LVecBase3_to_btVector3(corner11),
826 btSoftBody *body = btSoftBodyHelpers::CreateEllipsoid(
828 LVecBase3_to_btVector3(center),
829 LVecBase3_to_btVector3(radius),
843 make_tri_mesh(
BulletSoftBodyWorldInfo &info, PTA_LVecBase3 points, PTA_int indices,
bool randomizeConstraints) {
846 PTA_LVecBase3 mapped_points;
847 PTA_int mapped_indices;
851 for (PTA_LVecBase3::size_type i=0; i<points.size(); i++) {
853 int j = get_point_index(p, mapped_points);
855 mapping[i] = mapped_points.size();
856 mapped_points.push_back(p);
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);
869 points = mapped_points;
870 indices = mapped_indices;
873 int num_vertices = points.size();
874 int num_triangles = indices.size() / 3;
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();
883 int *triangles =
new int[num_triangles * 3];
884 for (
int i=0; i < num_triangles * 3; i++) {
885 triangles[i] = indices[i];
889 btSoftBody *body = btSoftBodyHelpers::CreateFromTriMesh(
894 randomizeConstraints);
896 nassertr(body, NULL);
915 PTA_LVecBase3 points;
920 nassertr(vdata->has_column(InternalName::get_vertex()), NULL);
924 while (!vreader.is_at_end()) {
930 for (
int i=0; i<geom->get_num_primitives(); i++) {
933 prim = prim->decompose();
935 for (
int j=0; j<prim->get_num_primitives(); j++) {
937 int s = prim->get_primitive_start(j);
938 int e = prim->get_primitive_end(j);
940 for (
int k=s; k<e; k++) {
941 indices.push_back(prim->get_vertex(k));
947 return make_tri_mesh(info, points, indices, randomizeConstraints);
959 btAlignedObjectArray<btVector3> pos;
960 pos.resize(points.size());
961 for (PTA_LVecBase3::size_type i=0; i<points.size(); i++) {
963 pos[i] = LVecBase3_to_btVector3(point);
967 btSoftBody* body =
new btSoftBody(&info.get_info(), pos.size(), &pos[0], 0);
970 for (PTA_int::size_type i=0; i<indices.size() / 4; i++) {
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];
978 body->appendTetra(ni[0],ni[1],ni[2],ni[3]);
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);
1004 nassertr(node && node[0], NULL);
1007 btAlignedObjectArray<btVector3> pos;
1014 sscanf(node,
"%d %d %d %d", &npos, &ndims, &nattrb, &hasbounds);
1015 node += next_line(node);
1019 for (
int i=0; i<pos.size(); ++i) {
1023 sscanf(node,
"%d %f %f %f", &index, &x, &y, &z);
1024 node += next_line(node);
1026 pos[index].setX(btScalar(x));
1027 pos[index].setY(btScalar(y));
1028 pos[index].setZ(btScalar(z));
1032 btSoftBody *body =
new btSoftBody(&info.get_info(), npos, &pos[0], 0);
1035 if (face && face[0]) {
1039 sscanf(face,
"%d %d", &nface, &hasbounds);
1040 face += next_line(face);
1042 for (
int i=0; i<nface; ++i) {
1046 sscanf(face,
"%d %d %d %d", &index, &ni[0], &ni[1], &ni[2]);
1047 face += next_line(face);
1049 body->appendFace(ni[0], ni[1], ni[2]);
1054 if (ele && ele[0]) {
1059 sscanf(ele,
"%d %d %d", &ntetra, &ncorner, &neattrb);
1060 ele += next_line(ele);
1062 for (
int i=0; i<ntetra; ++i) {
1066 sscanf(ele,
"%d %d %d %d %d", &index, &ni[0], &ni[1], &ni[2], &ni[3]);
1067 ele += next_line(ele);
1069 body->appendTetra(ni[0], ni[1], ni[2], ni[3]);
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);
1091 void BulletSoftBodyNode::
1092 append_linear_joint(
BulletBodyNode *body,
int cluster, PN_stdfloat erp, PN_stdfloat cfm, PN_stdfloat split) {
1096 btCollisionObject *ptr = body->get_object();
1098 btSoftBody::LJoint::Specs ls;
1102 ls.position = _soft->clusterCom(cluster);
1104 _soft->appendLinearJoint(ls, ptr);
1112 void BulletSoftBodyNode::
1113 append_linear_joint(
BulletBodyNode *body,
const LPoint3 &pos, PN_stdfloat erp, PN_stdfloat cfm, PN_stdfloat split) {
1117 btCollisionObject *ptr = body->get_object();
1119 btSoftBody::LJoint::Specs ls;
1123 ls.position = LVecBase3_to_btVector3(pos);
1125 _soft->appendLinearJoint(ls, ptr);
1133 void BulletSoftBodyNode::
1138 btCollisionObject *ptr = body->get_object();
1140 btSoftBody::AJoint::Specs as;
1144 as.axis = LVecBase3_to_btVector3(axis);
1145 as.icontrol = control ? control : btSoftBody::AJoint::IControl::Default();
1147 _soft->appendAngularJoint(as, ptr);
1155 void BulletSoftBodyNode::
1156 set_wind_velocity(
const LVector3 &velocity) {
1158 nassertv(!velocity.
is_nan());
1159 _soft->setWindVelocity(LVecBase3_to_btVector3(velocity));
1168 get_wind_velocity()
const {
1170 return btVector3_to_LVector3(_soft->getWindVelocity());
A basic node of the scene graph or data graph.
An axis-aligned bounding box; that is, a minimum and maximum coordinate triple.
This is the base class for all three-component vectors and points.
bool almost_equal(const LMatrix4f &other, float threshold) const
Returns true if two matrices are memberwise equal within a specified tolerance.
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...
This is a three-component vector distance (as opposed to a three-component point, which represents a ...
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 ...
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.
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.
void set_transform(const TransformState *transform, Thread *current_thread=Thread::get_current_thread())
Changes the complete transform object on this node.
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...
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.
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.
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.
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.
TypeHandle is the identifier used to differentiate C++ class types.
NodePath is the fundamental system for disambiguating instances, and also provides a higher-level int...
This object provides the functionality of both a GeomVertexReader and a GeomVertexWriter, combined together into one convenient package.