Panda3D
|
00001 // Filename: bulletSoftBodyNode.cxx 00002 // Created by: enn0x (27Dec10) 00003 // 00004 //////////////////////////////////////////////////////////////////// 00005 // 00006 // PANDA 3D SOFTWARE 00007 // Copyright (c) Carnegie Mellon University. All rights reserved. 00008 // 00009 // All use of this software is subject to the terms of the revised BSD 00010 // license. You should have received a copy of this license along 00011 // with this source code in a file named "LICENSE." 00012 // 00013 //////////////////////////////////////////////////////////////////// 00014 00015 #include "bulletSoftBodyNode.h" 00016 #include "bulletSoftBodyConfig.h" 00017 #include "bulletSoftBodyMaterial.h" 00018 #include "bulletSoftBodyShape.h" 00019 #include "bulletSoftBodyWorldInfo.h" 00020 #include "bulletHelper.h" 00021 00022 #include "geomVertexRewriter.h" 00023 #include "geomVertexReader.h" 00024 00025 TypeHandle BulletSoftBodyNode::_type_handle; 00026 00027 //////////////////////////////////////////////////////////////////// 00028 // Function: BulletSoftBodyNode::Constructor 00029 // Access: Public 00030 // Description: 00031 //////////////////////////////////////////////////////////////////// 00032 BulletSoftBodyNode:: 00033 BulletSoftBodyNode(btSoftBody *body, const char *name) : BulletBodyNode(name) { 00034 00035 // Synchronised transform 00036 _sync = TransformState::make_identity(); 00037 _sync_disable = false; 00038 00039 // Softbody 00040 _soft = body; 00041 _soft->setUserPointer(this); 00042 00043 // Shape 00044 btCollisionShape *shape_ptr = _soft->getCollisionShape(); 00045 00046 nassertv(shape_ptr != NULL); 00047 nassertv(shape_ptr->getShapeType() == SOFTBODY_SHAPE_PROXYTYPE); 00048 00049 _shapes.push_back(new BulletSoftBodyShape((btSoftBodyCollisionShape *)shape_ptr)); 00050 00051 // Rendering 00052 _geom = NULL; 00053 _curve = NULL; 00054 _surface = NULL; 00055 } 00056 00057 //////////////////////////////////////////////////////////////////// 00058 // Function: BulletSoftBodyNode::get_object 00059 // Access: Public 00060 // Description: 00061 //////////////////////////////////////////////////////////////////// 00062 btCollisionObject *BulletSoftBodyNode:: 00063 get_object() const { 00064 00065 return _soft; 00066 } 00067 00068 //////////////////////////////////////////////////////////////////// 00069 // Function: BulletSoftBodyNode::get_cfg 00070 // Access: Published 00071 // Description: 00072 //////////////////////////////////////////////////////////////////// 00073 BulletSoftBodyConfig BulletSoftBodyNode:: 00074 get_cfg() { 00075 00076 return BulletSoftBodyConfig(_soft->m_cfg); 00077 } 00078 00079 //////////////////////////////////////////////////////////////////// 00080 // Function: BulletSoftBodyNode::get_world_info 00081 // Access: Published 00082 // Description: 00083 //////////////////////////////////////////////////////////////////// 00084 BulletSoftBodyWorldInfo BulletSoftBodyNode:: 00085 get_world_info() { 00086 00087 return BulletSoftBodyWorldInfo(*(_soft->m_worldInfo)); 00088 } 00089 00090 //////////////////////////////////////////////////////////////////// 00091 // Function: BulletSoftBodyNode::get_num_materials 00092 // Access: Published 00093 // Description: 00094 //////////////////////////////////////////////////////////////////// 00095 int BulletSoftBodyNode:: 00096 get_num_materials() const { 00097 00098 return _soft->m_materials.size(); 00099 } 00100 00101 //////////////////////////////////////////////////////////////////// 00102 // Function: BulletSoftBodyNode::get_material 00103 // Access: Published 00104 // Description: 00105 //////////////////////////////////////////////////////////////////// 00106 BulletSoftBodyMaterial BulletSoftBodyNode:: 00107 get_material(int idx) const { 00108 00109 nassertr(idx >= 0 && idx < get_num_materials(), BulletSoftBodyMaterial::empty()); 00110 00111 btSoftBody::Material *material = _soft->m_materials[idx]; 00112 return BulletSoftBodyMaterial(*material); 00113 } 00114 00115 //////////////////////////////////////////////////////////////////// 00116 // Function: BulletSoftBodyNode::append_material 00117 // Access: Published 00118 // Description: 00119 //////////////////////////////////////////////////////////////////// 00120 BulletSoftBodyMaterial BulletSoftBodyNode:: 00121 append_material() { 00122 00123 btSoftBody::Material *material = _soft->appendMaterial(); 00124 nassertr(material, BulletSoftBodyMaterial::empty()); 00125 00126 return BulletSoftBodyMaterial(*material); 00127 } 00128 00129 //////////////////////////////////////////////////////////////////// 00130 // Function: BulletSoftBodyNode::get_num_nodes 00131 // Access: Published 00132 // Description: 00133 //////////////////////////////////////////////////////////////////// 00134 int BulletSoftBodyNode:: 00135 get_num_nodes() const { 00136 00137 return _soft->m_nodes.size(); 00138 } 00139 00140 //////////////////////////////////////////////////////////////////// 00141 // Function: BulletSoftBodyNode::get_node 00142 // Access: Published 00143 // Description: 00144 //////////////////////////////////////////////////////////////////// 00145 BulletSoftBodyNodeElement BulletSoftBodyNode:: 00146 get_node(int idx) const { 00147 00148 nassertr(idx >=0 && idx < get_num_nodes(), BulletSoftBodyNodeElement::empty()); 00149 return BulletSoftBodyNodeElement(_soft->m_nodes[idx]); 00150 } 00151 00152 //////////////////////////////////////////////////////////////////// 00153 // Function: BulletSoftBodyNode::generate_bending_constraints 00154 // Access: Published 00155 // Description: 00156 //////////////////////////////////////////////////////////////////// 00157 void BulletSoftBodyNode:: 00158 generate_bending_constraints(int distance, BulletSoftBodyMaterial *material) { 00159 00160 if (material) { 00161 _soft->generateBendingConstraints(distance, &(material->get_material())); 00162 } 00163 else { 00164 _soft->generateBendingConstraints(distance); 00165 } 00166 } 00167 00168 //////////////////////////////////////////////////////////////////// 00169 // Function: BulletSoftBodyNode::randomize_constraints 00170 // Access: Published 00171 // Description: 00172 //////////////////////////////////////////////////////////////////// 00173 void BulletSoftBodyNode:: 00174 randomize_constraints() { 00175 00176 _soft->randomizeConstraints(); 00177 } 00178 00179 //////////////////////////////////////////////////////////////////// 00180 // Function: BulletSoftBodyNode::transform_changed 00181 // Access: Protected 00182 // Description: 00183 //////////////////////////////////////////////////////////////////// 00184 void BulletSoftBodyNode:: 00185 transform_changed() { 00186 00187 if (_sync_disable) return; 00188 00189 NodePath np = NodePath::any_path((PandaNode *)this); 00190 CPT(TransformState) ts = np.get_net_transform(); 00191 00192 LMatrix4 m_sync = _sync->get_mat(); 00193 LMatrix4 m_ts = ts->get_mat(); 00194 00195 if (!m_sync.almost_equal(m_ts)) { 00196 _sync = ts; 00197 00198 btTransform trans = TransformState_to_btTrans(ts); 00199 00200 trans *= _soft->m_initialWorldTransform.inverse(); 00201 _soft->transform(trans); 00202 00203 if (ts->has_scale()) { 00204 LVecBase3 scale = ts->get_scale(); 00205 if (!scale.almost_equal(LVecBase3(1.0f, 1.0f, 1.0f))) { 00206 for (int i=0; i<get_num_shapes(); i++) { 00207 PT(BulletShape) shape = _shapes[i]; 00208 shape->set_local_scale(scale); 00209 } 00210 } 00211 } 00212 } 00213 } 00214 00215 //////////////////////////////////////////////////////////////////// 00216 // Function: BulletSoftBodyNode::sync_p2b 00217 // Access: Public 00218 // Description: 00219 //////////////////////////////////////////////////////////////////// 00220 void BulletSoftBodyNode:: 00221 sync_p2b() { 00222 00223 transform_changed(); 00224 } 00225 00226 //////////////////////////////////////////////////////////////////// 00227 // Function: BulletSoftBodyNode::sync_b2p 00228 // Access: Public 00229 // Description: 00230 //////////////////////////////////////////////////////////////////// 00231 void BulletSoftBodyNode:: 00232 sync_b2p() { 00233 00234 // Render softbody 00235 if (_geom) { 00236 btTransform trans = btTransform::getIdentity(); 00237 get_node_transform(trans, this); 00238 00239 PT(GeomVertexData) vdata = _geom->modify_vertex_data(); 00240 00241 GeomVertexRewriter vertices(vdata, InternalName::get_vertex()); 00242 GeomVertexRewriter normals(vdata, InternalName::get_normal()); 00243 GeomVertexReader indices(vdata, BulletHelper::get_sb_index()); 00244 GeomVertexReader flips(vdata, BulletHelper::get_sb_flip()); 00245 00246 while (!vertices.is_at_end()) { 00247 btSoftBody::Node node = _soft->m_nodes[indices.get_data1i()]; 00248 btVector3 v = trans.invXform(node.m_x); 00249 btVector3 n = node.m_n; 00250 00251 if (flips.get_data1i() > 0) n *= -1; 00252 00253 vertices.set_data3((PN_stdfloat)v.getX(), (PN_stdfloat)v.getY(), (PN_stdfloat)v.getZ()); 00254 normals.set_data3((PN_stdfloat)n.getX(), (PN_stdfloat)n.getY(), (PN_stdfloat)n.getZ()); 00255 } 00256 } 00257 00258 if (_curve) { 00259 btSoftBody::tNodeArray &nodes(_soft->m_nodes); 00260 00261 for (int i=0; i < nodes.size(); i++) { 00262 btVector3 pos = nodes[i].m_x; 00263 _curve->set_vertex(i, btVector3_to_LPoint3(pos)); 00264 } 00265 } 00266 00267 if (_surface) { 00268 btSoftBody::tNodeArray &nodes(_soft->m_nodes); 00269 00270 int num_u = _surface->get_num_u_vertices(); 00271 int num_v = _surface->get_num_v_vertices(); 00272 nassertv(num_u * num_v == nodes.size()); 00273 00274 for (int u=0; u < num_u; u++) { 00275 for (int v=0; v < num_v; v++) { 00276 btVector3 pos = nodes[u * num_u + v].m_x; 00277 _surface->set_vertex(u, v, btVector3_to_LPoint3(pos)); 00278 } 00279 } 00280 } 00281 00282 // It is ok to pass the address of a temporary object here, because 00283 // set_bounds does not store the pointer - it makes a copy using 00284 // volume->make_copy(). 00285 BoundingBox bb = this->get_aabb(); 00286 LVecBase3 pos = bb.get_approx_center(); 00287 00288 NodePath np = NodePath::any_path((PandaNode *)this); 00289 LVecBase3 scale = np.get_net_transform()->get_scale(); 00290 00291 CPT(TransformState) ts = TransformState::make_pos(pos); 00292 ts = ts->set_scale(scale); 00293 00294 _sync = ts; 00295 _sync_disable = true; 00296 np.set_transform(NodePath(), ts); 00297 _sync_disable = false; 00298 00299 Thread *current_thread = Thread::get_current_thread(); 00300 this->r_mark_geom_bounds_stale(current_thread); 00301 } 00302 00303 //////////////////////////////////////////////////////////////////// 00304 // Function: BulletSoftBodyNode::get_closest_node_index 00305 // Access: Published 00306 // Description: Returns the index of the node which is closest 00307 // to the given point. The distance between each node 00308 // and the given point is computed in world space 00309 // if local=false, and in local space if local=true. 00310 //////////////////////////////////////////////////////////////////// 00311 int BulletSoftBodyNode:: 00312 get_closest_node_index(LVecBase3 point, bool local) { 00313 00314 btScalar max_dist_sqr = 1e30; 00315 btVector3 point_x = LVecBase3_to_btVector3(point); 00316 00317 btTransform trans = btTransform::getIdentity(); 00318 if (local == true) { 00319 get_node_transform(trans, this); 00320 } 00321 00322 btSoftBody::tNodeArray &nodes(_soft->m_nodes); 00323 int node_idx = 0; 00324 00325 for (int i=0; i<nodes.size(); ++i) { 00326 00327 btVector3 node_x = nodes[i].m_x; 00328 btScalar dist_sqr = (trans.invXform(node_x) - point_x).length2(); 00329 00330 if (dist_sqr < max_dist_sqr) { 00331 max_dist_sqr = dist_sqr; 00332 node_idx = i; 00333 } 00334 } 00335 00336 return node_idx; 00337 } 00338 00339 //////////////////////////////////////////////////////////////////// 00340 // Function: BulletSoftBodyNode::link_geom 00341 // Access: Published 00342 // Description: 00343 //////////////////////////////////////////////////////////////////// 00344 void BulletSoftBodyNode:: 00345 link_geom(Geom *geom) { 00346 00347 nassertv(geom->get_vertex_data()->has_column(InternalName::get_vertex())); 00348 nassertv(geom->get_vertex_data()->has_column(InternalName::get_normal())); 00349 00350 sync_p2b(); 00351 00352 _geom = geom; 00353 00354 PT(GeomVertexData) vdata = _geom->modify_vertex_data(); 00355 00356 if (!vdata->has_column(BulletHelper::get_sb_index())) { 00357 CPT(GeomVertexFormat) format = vdata->get_format(); 00358 format = BulletHelper::add_sb_index_column(format); 00359 vdata->set_format(format); 00360 } 00361 00362 if (!vdata->has_column(BulletHelper::get_sb_flip())) { 00363 CPT(GeomVertexFormat) format = vdata->get_format(); 00364 format = BulletHelper::add_sb_flip_column(format); 00365 vdata->set_format(format); 00366 } 00367 00368 GeomVertexReader vertices(vdata, InternalName::get_vertex()); 00369 GeomVertexRewriter indices(vdata, BulletHelper::get_sb_index()); 00370 00371 while (!vertices.is_at_end()) { 00372 LVecBase3 point = vertices.get_data3(); 00373 int node_idx = get_closest_node_index(point, true); 00374 indices.set_data1i(node_idx); 00375 } 00376 } 00377 00378 //////////////////////////////////////////////////////////////////// 00379 // Function: BulletSoftBodyNode::unlink_geom 00380 // Access: Published 00381 // Description: 00382 //////////////////////////////////////////////////////////////////// 00383 void BulletSoftBodyNode:: 00384 unlink_geom() { 00385 00386 _geom = NULL; 00387 } 00388 00389 //////////////////////////////////////////////////////////////////// 00390 // Function: BulletSoftBodyNode::link_curve 00391 // Access: Published 00392 // Description: 00393 //////////////////////////////////////////////////////////////////// 00394 void BulletSoftBodyNode:: 00395 link_curve(NurbsCurveEvaluator *curve) { 00396 00397 nassertv(curve->get_num_vertices() == _soft->m_nodes.size()); 00398 00399 _curve = curve; 00400 } 00401 00402 //////////////////////////////////////////////////////////////////// 00403 // Function: BulletSoftBodyNode::unlink_curve 00404 // Access: Published 00405 // Description: 00406 //////////////////////////////////////////////////////////////////// 00407 void BulletSoftBodyNode:: 00408 unlink_curve() { 00409 00410 _curve = NULL; 00411 } 00412 00413 //////////////////////////////////////////////////////////////////// 00414 // Function: BulletSoftBodyNode::link_surface 00415 // Access: Published 00416 // Description: 00417 //////////////////////////////////////////////////////////////////// 00418 void BulletSoftBodyNode:: 00419 link_surface(NurbsSurfaceEvaluator *surface) { 00420 00421 nassertv(surface->get_num_u_vertices() * surface->get_num_v_vertices() == _soft->m_nodes.size()); 00422 00423 _surface = surface; 00424 } 00425 00426 //////////////////////////////////////////////////////////////////// 00427 // Function: BulletSoftBodyNode::unlink_surface 00428 // Access: Published 00429 // Description: 00430 //////////////////////////////////////////////////////////////////// 00431 void BulletSoftBodyNode:: 00432 unlink_surface() { 00433 00434 _surface = NULL; 00435 } 00436 00437 //////////////////////////////////////////////////////////////////// 00438 // Function: BulletSoftBodyNode::get_aabb 00439 // Access: Published 00440 // Description: 00441 //////////////////////////////////////////////////////////////////// 00442 BoundingBox BulletSoftBodyNode:: 00443 get_aabb() const { 00444 00445 btVector3 pMin; 00446 btVector3 pMax; 00447 00448 _soft->getAabb(pMin, pMax); 00449 00450 return BoundingBox( 00451 btVector3_to_LPoint3(pMin), 00452 btVector3_to_LPoint3(pMax) 00453 ); 00454 } 00455 00456 //////////////////////////////////////////////////////////////////// 00457 // Function: BulletSoftBodyNode::set_volume_mass 00458 // Access: Published 00459 // Description: 00460 //////////////////////////////////////////////////////////////////// 00461 void BulletSoftBodyNode:: 00462 set_volume_mass(PN_stdfloat mass) { 00463 00464 _soft->setVolumeMass(mass); 00465 } 00466 00467 //////////////////////////////////////////////////////////////////// 00468 // Function: BulletSoftBodyNode::set_total_mass 00469 // Access: Published 00470 // Description: 00471 //////////////////////////////////////////////////////////////////// 00472 void BulletSoftBodyNode:: 00473 set_total_mass(PN_stdfloat mass, bool fromfaces) { 00474 00475 _soft->setTotalMass(mass, fromfaces); 00476 } 00477 00478 //////////////////////////////////////////////////////////////////// 00479 // Function: BulletSoftBodyNode::set_volume_density 00480 // Access: Published 00481 // Description: 00482 //////////////////////////////////////////////////////////////////// 00483 void BulletSoftBodyNode:: 00484 set_volume_density(PN_stdfloat density) { 00485 00486 _soft->setVolumeDensity(density); 00487 } 00488 00489 //////////////////////////////////////////////////////////////////// 00490 // Function: BulletSoftBodyNode::set_total_density 00491 // Access: Published 00492 // Description: 00493 //////////////////////////////////////////////////////////////////// 00494 void BulletSoftBodyNode:: 00495 set_total_density(PN_stdfloat density) { 00496 00497 _soft->setTotalDensity(density); 00498 } 00499 00500 //////////////////////////////////////////////////////////////////// 00501 // Function: BulletSoftBodyNode::set_mass 00502 // Access: Published 00503 // Description: 00504 //////////////////////////////////////////////////////////////////// 00505 void BulletSoftBodyNode:: 00506 set_mass(int node, PN_stdfloat mass) { 00507 00508 _soft->setMass(node, mass); 00509 } 00510 00511 //////////////////////////////////////////////////////////////////// 00512 // Function: BulletSoftBodyNode::get_mass 00513 // Access: Published 00514 // Description: 00515 //////////////////////////////////////////////////////////////////// 00516 PN_stdfloat BulletSoftBodyNode:: 00517 get_mass(int node) const { 00518 00519 return _soft->getMass(node); 00520 } 00521 00522 //////////////////////////////////////////////////////////////////// 00523 // Function: BulletSoftBodyNode::get_total_mass 00524 // Access: Published 00525 // Description: 00526 //////////////////////////////////////////////////////////////////// 00527 PN_stdfloat BulletSoftBodyNode:: 00528 get_total_mass() const { 00529 00530 return _soft->getTotalMass(); 00531 } 00532 00533 //////////////////////////////////////////////////////////////////// 00534 // Function: BulletSoftBodyNode::get_volume 00535 // Access: Published 00536 // Description: 00537 //////////////////////////////////////////////////////////////////// 00538 PN_stdfloat BulletSoftBodyNode:: 00539 get_volume() const { 00540 00541 return _soft->getVolume(); 00542 } 00543 00544 //////////////////////////////////////////////////////////////////// 00545 // Function: BulletSoftBodyNode::add_force 00546 // Access: Published 00547 // Description: 00548 //////////////////////////////////////////////////////////////////// 00549 void BulletSoftBodyNode:: 00550 add_force(const LVector3 &force) { 00551 00552 nassertv(!force.is_nan()); 00553 _soft->addForce(LVecBase3_to_btVector3(force)); 00554 } 00555 00556 //////////////////////////////////////////////////////////////////// 00557 // Function: BulletSoftBodyNode::add_force 00558 // Access: Published 00559 // Description: 00560 //////////////////////////////////////////////////////////////////// 00561 void BulletSoftBodyNode:: 00562 add_force(const LVector3 &force, int node) { 00563 00564 nassertv(!force.is_nan()); 00565 _soft->addForce(LVecBase3_to_btVector3(force), node); 00566 } 00567 00568 //////////////////////////////////////////////////////////////////// 00569 // Function: BulletSoftBodyNode::set_velocity 00570 // Access: Published 00571 // Description: 00572 //////////////////////////////////////////////////////////////////// 00573 void BulletSoftBodyNode:: 00574 set_velocity(const LVector3 &velocity) { 00575 00576 nassertv(!velocity.is_nan()); 00577 _soft->setVelocity(LVecBase3_to_btVector3(velocity)); 00578 } 00579 00580 //////////////////////////////////////////////////////////////////// 00581 // Function: BulletSoftBodyNode::add_velocity 00582 // Access: Published 00583 // Description: 00584 //////////////////////////////////////////////////////////////////// 00585 void BulletSoftBodyNode:: 00586 add_velocity(const LVector3 &velocity) { 00587 00588 nassertv(!velocity.is_nan()); 00589 _soft->addVelocity(LVecBase3_to_btVector3(velocity)); 00590 } 00591 00592 //////////////////////////////////////////////////////////////////// 00593 // Function: BulletSoftBodyNode::add_velocity 00594 // Access: Published 00595 // Description: 00596 //////////////////////////////////////////////////////////////////// 00597 void BulletSoftBodyNode:: 00598 add_velocity(const LVector3 &velocity, int node) { 00599 00600 nassertv(!velocity.is_nan()); 00601 _soft->addVelocity(LVecBase3_to_btVector3(velocity), node); 00602 } 00603 00604 //////////////////////////////////////////////////////////////////// 00605 // Function: BulletSoftBodyNode::generate_clusters 00606 // Access: Published 00607 // Description: 00608 //////////////////////////////////////////////////////////////////// 00609 void BulletSoftBodyNode:: 00610 generate_clusters(int k, int maxiterations) { 00611 00612 _soft->generateClusters(k, maxiterations); 00613 } 00614 00615 //////////////////////////////////////////////////////////////////// 00616 // Function: BulletSoftBodyNode::release_clusters 00617 // Access: Published 00618 // Description: 00619 //////////////////////////////////////////////////////////////////// 00620 void BulletSoftBodyNode:: 00621 release_clusters() { 00622 00623 _soft->releaseClusters(); 00624 } 00625 00626 //////////////////////////////////////////////////////////////////// 00627 // Function: BulletSoftBodyNode::release_cluster 00628 // Access: Published 00629 // Description: 00630 //////////////////////////////////////////////////////////////////// 00631 void BulletSoftBodyNode:: 00632 release_cluster(int index) { 00633 00634 _soft->releaseCluster(index); 00635 } 00636 00637 //////////////////////////////////////////////////////////////////// 00638 // Function: BulletSoftBodyNode::get_num_clusters 00639 // Access: Published 00640 // Description: 00641 //////////////////////////////////////////////////////////////////// 00642 int BulletSoftBodyNode:: 00643 get_num_clusters() const { 00644 00645 return _soft->clusterCount(); 00646 } 00647 00648 //////////////////////////////////////////////////////////////////// 00649 // Function: BulletSoftBodyNode::cluster_com 00650 // Access: Published 00651 // Description: 00652 //////////////////////////////////////////////////////////////////// 00653 LVecBase3 BulletSoftBodyNode:: 00654 cluster_com(int cluster) const { 00655 00656 return btVector3_to_LVecBase3(_soft->clusterCom(cluster)); 00657 } 00658 00659 //////////////////////////////////////////////////////////////////// 00660 // Function: BulletSoftBodyNode::set_pose 00661 // Access: Published 00662 // Description: 00663 //////////////////////////////////////////////////////////////////// 00664 void BulletSoftBodyNode:: 00665 set_pose(bool bvolume, bool bframe) { 00666 00667 _soft->setPose(bvolume, bframe); 00668 } 00669 00670 //////////////////////////////////////////////////////////////////// 00671 // Function: BulletSoftBodyNode::append_anchor 00672 // Access: Published 00673 // Description: 00674 //////////////////////////////////////////////////////////////////// 00675 void BulletSoftBodyNode:: 00676 append_anchor(int node, BulletRigidBodyNode *body, bool disable) { 00677 00678 nassertv(node < _soft->m_nodes.size()) 00679 nassertv(body); 00680 00681 body->sync_p2b(); 00682 00683 btRigidBody *ptr =(btRigidBody *)body->get_object(); 00684 _soft->appendAnchor(node, ptr, disable); 00685 } 00686 00687 //////////////////////////////////////////////////////////////////// 00688 // Function: BulletSoftBodyNode::append_anchor 00689 // Access: Published 00690 // Description: 00691 //////////////////////////////////////////////////////////////////// 00692 void BulletSoftBodyNode:: 00693 append_anchor(int node, BulletRigidBodyNode *body, const LVector3 &pivot, bool disable) { 00694 00695 nassertv(node < _soft->m_nodes.size()) 00696 nassertv(body); 00697 nassertv(!pivot.is_nan()); 00698 00699 body->sync_p2b(); 00700 00701 btRigidBody *ptr =(btRigidBody *)body->get_object(); 00702 _soft->appendAnchor(node, ptr, LVecBase3_to_btVector3(pivot), disable); 00703 } 00704 00705 //////////////////////////////////////////////////////////////////// 00706 // Function: BulletSoftBodyNodeElement::Constructor 00707 // Access: Public 00708 // Description: 00709 //////////////////////////////////////////////////////////////////// 00710 BulletSoftBodyNodeElement:: 00711 BulletSoftBodyNodeElement(btSoftBody::Node &node) : _node(node) { 00712 00713 } 00714 00715 //////////////////////////////////////////////////////////////////// 00716 // Function: BulletSoftBodyNode::get_point_index 00717 // Access: Private 00718 // Description: Returns the index of the first point within an 00719 // array of points which has about the same 00720 // coordinates as the given point. If no points 00721 // is found -1 is returned. 00722 //////////////////////////////////////////////////////////////////// 00723 int BulletSoftBodyNode:: 00724 get_point_index(LVecBase3 p, PTA_LVecBase3 points) { 00725 00726 PN_stdfloat eps = 1.0e-6f; // TODO make this a config option 00727 00728 for (PTA_LVecBase3::size_type i=0; i<points.size(); i++) { 00729 if (points[i].almost_equal(p, eps)) { 00730 return i; // Found 00731 } 00732 } 00733 00734 return -1; // Not found 00735 } 00736 00737 //////////////////////////////////////////////////////////////////// 00738 // Function: BulletSoftBodyNode::next_line 00739 // Access: Published 00740 // Description: Read on until the next linebreak is detected, or 00741 // the end of file has been reached. 00742 //////////////////////////////////////////////////////////////////// 00743 int BulletSoftBodyNode:: 00744 next_line(const char* buffer) { 00745 00746 int num_bytes_read = 0; 00747 00748 while (*buffer != '\n') { 00749 buffer++; 00750 num_bytes_read++; 00751 } 00752 00753 if (buffer[0] == 0x0a) { 00754 buffer++; 00755 num_bytes_read++; 00756 } 00757 00758 return num_bytes_read; 00759 } 00760 00761 //////////////////////////////////////////////////////////////////// 00762 // Function: BulletSoftBodyNode::make_rope 00763 // Access: Published 00764 // Description: 00765 //////////////////////////////////////////////////////////////////// 00766 PT(BulletSoftBodyNode) BulletSoftBodyNode:: 00767 make_rope(BulletSoftBodyWorldInfo &info, const LPoint3 &from, const LPoint3 &to, int res, int fixeds) { 00768 00769 btSoftBody *body = btSoftBodyHelpers::CreateRope( 00770 info.get_info(), 00771 LVecBase3_to_btVector3(from), 00772 LVecBase3_to_btVector3(to), 00773 res, 00774 fixeds); 00775 00776 PT(BulletSoftBodyNode) node = new BulletSoftBodyNode(body); 00777 00778 return node; 00779 } 00780 00781 //////////////////////////////////////////////////////////////////// 00782 // Function: BulletSoftBodyNode::make_patch 00783 // Access: Published 00784 // Description: 00785 //////////////////////////////////////////////////////////////////// 00786 PT(BulletSoftBodyNode) BulletSoftBodyNode:: 00787 make_patch(BulletSoftBodyWorldInfo &info, const LPoint3 &corner00, const LPoint3 &corner10, const LPoint3 &corner01, const LPoint3 &corner11, int resx, int resy, int fixeds, bool gendiags) { 00788 00789 btSoftBody *body = btSoftBodyHelpers::CreatePatch( 00790 info.get_info(), 00791 LVecBase3_to_btVector3(corner00), 00792 LVecBase3_to_btVector3(corner10), 00793 LVecBase3_to_btVector3(corner01), 00794 LVecBase3_to_btVector3(corner11), 00795 resx, 00796 resy, 00797 fixeds, 00798 gendiags); 00799 00800 PT(BulletSoftBodyNode) node = new BulletSoftBodyNode(body); 00801 00802 return node; 00803 } 00804 00805 //////////////////////////////////////////////////////////////////// 00806 // Function: BulletSoftBodyNode::make_ellipsoid 00807 // Access: Published 00808 // Description: 00809 //////////////////////////////////////////////////////////////////// 00810 PT(BulletSoftBodyNode) BulletSoftBodyNode:: 00811 make_ellipsoid(BulletSoftBodyWorldInfo &info, const LPoint3 ¢er, const LVecBase3 &radius, int res) { 00812 00813 btSoftBody *body = btSoftBodyHelpers::CreateEllipsoid( 00814 info.get_info(), 00815 LVecBase3_to_btVector3(center), 00816 LVecBase3_to_btVector3(radius), 00817 res); 00818 00819 PT(BulletSoftBodyNode) node = new BulletSoftBodyNode(body); 00820 00821 return node; 00822 } 00823 00824 //////////////////////////////////////////////////////////////////// 00825 // Function: BulletSoftBodyNode::make_tri_mesh 00826 // Access: Published 00827 // Description: 00828 //////////////////////////////////////////////////////////////////// 00829 PT(BulletSoftBodyNode) BulletSoftBodyNode:: 00830 make_tri_mesh(BulletSoftBodyWorldInfo &info, PTA_LVecBase3 points, PTA_int indices, bool randomizeConstraints) { 00831 00832 // Eliminate duplicate vertices 00833 PTA_LVecBase3 mapped_points; 00834 PTA_int mapped_indices; 00835 00836 pmap<int, int> mapping; 00837 00838 for (PTA_LVecBase3::size_type i=0; i<points.size(); i++) { 00839 LVecBase3 p = points[i]; 00840 int j = get_point_index(p, mapped_points); 00841 if (j < 0) { 00842 mapping[i] = mapped_points.size(); 00843 mapped_points.push_back(p); 00844 } 00845 else { 00846 mapping[i] = j; 00847 } 00848 } 00849 00850 for (PTA_int::size_type i=0; i<indices.size(); i++) { 00851 int idx = indices[i]; 00852 int mapped_idx = mapping[idx]; 00853 mapped_indices.push_back(mapped_idx); 00854 } 00855 00856 points = mapped_points; 00857 indices = mapped_indices; 00858 00859 // Convert arrays 00860 int num_vertices = points.size(); 00861 int num_triangles = indices.size() / 3; 00862 00863 btScalar *vertices = new btScalar[num_vertices * 3]; 00864 for (int i=0; i < num_vertices; i++) { 00865 vertices[3*i] = points[i].get_x(); 00866 vertices[3*i+1] = points[i].get_y(); 00867 vertices[3*i+2] = points[i].get_z(); 00868 } 00869 00870 int *triangles = new int[num_triangles * 3]; 00871 for (int i=0; i < num_triangles * 3; i++) { 00872 triangles[i] = indices[i]; 00873 } 00874 00875 // Create body 00876 btSoftBody *body = btSoftBodyHelpers::CreateFromTriMesh( 00877 info.get_info(), 00878 vertices, 00879 triangles, 00880 num_triangles, 00881 randomizeConstraints); 00882 00883 nassertr(body, NULL); 00884 00885 delete[] vertices; 00886 delete[] triangles; 00887 00888 PT(BulletSoftBodyNode) node = new BulletSoftBodyNode(body); 00889 00890 return node; 00891 } 00892 00893 //////////////////////////////////////////////////////////////////// 00894 // Function: BulletSoftBodyNode::make_tri_mesh 00895 // Access: Published 00896 // Description: 00897 //////////////////////////////////////////////////////////////////// 00898 PT(BulletSoftBodyNode) BulletSoftBodyNode:: 00899 make_tri_mesh(BulletSoftBodyWorldInfo &info, const Geom *geom, bool randomizeConstraints) { 00900 00901 // Read vertex data 00902 PTA_LVecBase3 points; 00903 PTA_int indices; 00904 00905 CPT(GeomVertexData) vdata = geom->get_vertex_data(); 00906 00907 nassertr(vdata->has_column(InternalName::get_vertex()), NULL); 00908 00909 GeomVertexReader vreader(vdata, InternalName::get_vertex()); 00910 00911 while (!vreader.is_at_end()) { 00912 LVecBase3 v = vreader.get_data3(); 00913 points.push_back(v); 00914 } 00915 00916 // Read indices 00917 for (int i=0; i<geom->get_num_primitives(); i++) { 00918 00919 CPT(GeomPrimitive) prim = geom->get_primitive(i); 00920 prim = prim->decompose(); 00921 00922 for (int j=0; j<prim->get_num_primitives(); j++) { 00923 00924 int s = prim->get_primitive_start(j); 00925 int e = prim->get_primitive_end(j); 00926 00927 for (int k=s; k<e; k++) { 00928 indices.push_back(prim->get_vertex(k)); 00929 } 00930 } 00931 } 00932 00933 // Create body 00934 return make_tri_mesh(info, points, indices, randomizeConstraints); 00935 } 00936 00937 //////////////////////////////////////////////////////////////////// 00938 // Function: BulletSoftBodyNode::make_tet_mesh 00939 // Access: Published 00940 // Description: 00941 //////////////////////////////////////////////////////////////////// 00942 PT(BulletSoftBodyNode) BulletSoftBodyNode:: 00943 make_tet_mesh(BulletSoftBodyWorldInfo &info, PTA_LVecBase3 points, PTA_int indices, bool tetralinks) { 00944 00945 // Points 00946 btAlignedObjectArray<btVector3> pos; 00947 pos.resize(points.size()); 00948 for (PTA_LVecBase3::size_type i=0; i<points.size(); i++) { 00949 LVecBase3 point = points[i]; 00950 pos[i] = LVecBase3_to_btVector3(point); 00951 } 00952 00953 // Body 00954 btSoftBody* body = new btSoftBody(&info.get_info(), pos.size(), &pos[0], 0); 00955 00956 // Indices 00957 for (PTA_int::size_type i=0; i<indices.size() / 4; i++) { 00958 int ni[4]; 00959 00960 ni[0] = indices[4*i]; 00961 ni[1] = indices[4*i+1]; 00962 ni[2] = indices[4*i+2]; 00963 ni[3] = indices[4*i+3]; 00964 00965 body->appendTetra(ni[0],ni[1],ni[2],ni[3]); 00966 00967 if (tetralinks) { 00968 body->appendLink(ni[0], ni[1], 0, true); 00969 body->appendLink(ni[1], ni[2], 0, true); 00970 body->appendLink(ni[2], ni[0], 0, true); 00971 body->appendLink(ni[0], ni[3], 0, true); 00972 body->appendLink(ni[1], ni[3], 0, true); 00973 body->appendLink(ni[2], ni[3], 0, true); 00974 } 00975 } 00976 00977 // Node 00978 PT(BulletSoftBodyNode) node = new BulletSoftBodyNode(body); 00979 00980 return node; 00981 } 00982 00983 //////////////////////////////////////////////////////////////////// 00984 // Function: BulletSoftBodyNode::make_tet_mesh 00985 // Access: Published 00986 // Description: 00987 //////////////////////////////////////////////////////////////////// 00988 PT(BulletSoftBodyNode) BulletSoftBodyNode:: 00989 make_tet_mesh(BulletSoftBodyWorldInfo &info, const char *ele, const char *face, const char *node) { 00990 00991 nassertr(node && node[0], NULL); 00992 00993 // Nodes 00994 btAlignedObjectArray<btVector3> pos; 00995 00996 int npos = 0; 00997 int ndims = 0; // not used 00998 int nattrb = 0; // not used 00999 int hasbounds = 0; // not used 01000 01001 sscanf(node, "%d %d %d %d", &npos, &ndims, &nattrb, &hasbounds); 01002 node += next_line(node); 01003 01004 pos.resize(npos); 01005 01006 for (int i=0; i<pos.size(); ++i) { 01007 int index = 0; 01008 PN_stdfloat x, y, z; 01009 01010 sscanf(node, "%d %f %f %f", &index, &x, &y, &z); 01011 node += next_line(node); 01012 01013 pos[index].setX(btScalar(x)); 01014 pos[index].setY(btScalar(y)); 01015 pos[index].setZ(btScalar(z)); 01016 } 01017 01018 // Body 01019 btSoftBody *body = new btSoftBody(&info.get_info(), npos, &pos[0], 0); 01020 01021 // Faces 01022 if (face && face[0]) { 01023 int nface = 0; 01024 int hasbounds = 0; // not used 01025 01026 sscanf(face, "%d %d", &nface, &hasbounds); 01027 face += next_line(face); 01028 01029 for (int i=0; i<nface; ++i) { 01030 int index = 0; 01031 int ni[3]; 01032 01033 sscanf(face, "%d %d %d %d", &index, &ni[0], &ni[1], &ni[2]); 01034 face += next_line(face); 01035 01036 body->appendFace(ni[0], ni[1], ni[2]); 01037 } 01038 } 01039 01040 // Links 01041 if (ele && ele[0]) { 01042 int ntetra = 0; 01043 int ncorner = 0; 01044 int neattrb = 0; 01045 01046 sscanf(ele, "%d %d %d", &ntetra, &ncorner, &neattrb); 01047 ele += next_line(ele); 01048 01049 for (int i=0; i<ntetra; ++i) { 01050 int index = 0; 01051 int ni[4]; 01052 01053 sscanf(ele, "%d %d %d %d %d", &index, &ni[0], &ni[1], &ni[2], &ni[3]); 01054 ele += next_line(ele); 01055 01056 body->appendTetra(ni[0], ni[1], ni[2], ni[3]); 01057 01058 body->appendLink(ni[0], ni[1], 0, true); 01059 body->appendLink(ni[1], ni[2], 0, true); 01060 body->appendLink(ni[2], ni[0], 0, true); 01061 body->appendLink(ni[0], ni[3], 0, true); 01062 body->appendLink(ni[1], ni[3], 0, true); 01063 body->appendLink(ni[2], ni[3], 0, true); 01064 } 01065 } 01066 01067 // Node 01068 PT(BulletSoftBodyNode) sbnode = new BulletSoftBodyNode(body); 01069 01070 return sbnode; 01071 } 01072