1 /* 2 Bullet Continuous Collision Detection and Physics Library 3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ 4 5 This software is provided 'as-is', without any express or implied warranty. 6 In no event will the authors be held liable for any damages arising from the use of this software. 7 Permission is granted to anyone to use this software for any purpose, 8 including commercial applications, and to alter it and redistribute it freely, 9 subject to the following restrictions: 10 11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 13 3. This notice may not be removed or altered from any source distribution. 14 */ 15 ///btSoftBody implementation by Nathanael Presson 16 17 #ifndef _BT_SOFT_BODY_H 18 #define _BT_SOFT_BODY_H 19 20 #include "LinearMath/btAlignedObjectArray.h" 21 #include "LinearMath/btTransform.h" 22 #include "LinearMath/btIDebugDraw.h" 23 #include "LinearMath/btVector3.h" 24 #include "BulletDynamics/Dynamics/btRigidBody.h" 25 26 #include "BulletCollision/CollisionShapes/btConcaveShape.h" 27 #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" 28 #include "btSparseSDF.h" 29 #include "BulletCollision/BroadphaseCollision/btDbvt.h" 30 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" 31 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" 32 //#ifdef BT_USE_DOUBLE_PRECISION 33 //#define btRigidBodyData btRigidBodyDoubleData 34 //#define btRigidBodyDataName "btRigidBodyDoubleData" 35 //#else 36 #define btSoftBodyData btSoftBodyFloatData 37 #define btSoftBodyDataName "btSoftBodyFloatData" 38 static const btScalar OVERLAP_REDUCTION_FACTOR = 0.1; 39 static unsigned long seed = 243703; 40 //#endif //BT_USE_DOUBLE_PRECISION 41 42 class btBroadphaseInterface; 43 class btDispatcher; 44 class btSoftBodySolver; 45 46 /* btSoftBodyWorldInfo */ 47 struct btSoftBodyWorldInfo 48 { 49 btScalar air_density; 50 btScalar water_density; 51 btScalar water_offset; 52 btScalar m_maxDisplacement; 53 btVector3 water_normal; 54 btBroadphaseInterface* m_broadphase; 55 btDispatcher* m_dispatcher; 56 btVector3 m_gravity; 57 btSparseSdf<3> m_sparsesdf; 58 btSoftBodyWorldInfobtSoftBodyWorldInfo59 btSoftBodyWorldInfo() 60 : air_density((btScalar)1.2), 61 water_density(0), 62 water_offset(0), 63 m_maxDisplacement(1000.f), //avoid soft body from 'exploding' so use some upper threshold of maximum motion that a node can travel per frame 64 water_normal(0, 0, 0), 65 m_broadphase(0), 66 m_dispatcher(0), 67 m_gravity(0, -10, 0) 68 { 69 } 70 }; 71 72 ///The btSoftBody is an class to simulate cloth and volumetric soft bodies. 73 ///There is two-way interaction between btSoftBody and btRigidBody/btCollisionObject. 74 class btSoftBody : public btCollisionObject 75 { 76 public: 77 btAlignedObjectArray<const class btCollisionObject*> m_collisionDisabledObjects; 78 79 // The solver object that handles this soft body 80 btSoftBodySolver* m_softBodySolver; 81 82 // 83 // Enumerations 84 // 85 86 ///eAeroModel 87 struct eAeroModel 88 { 89 enum _ 90 { 91 V_Point, ///Vertex normals are oriented toward velocity 92 V_TwoSided, ///Vertex normals are flipped to match velocity 93 V_TwoSidedLiftDrag, ///Vertex normals are flipped to match velocity and lift and drag forces are applied 94 V_OneSided, ///Vertex normals are taken as it is 95 F_TwoSided, ///Face normals are flipped to match velocity 96 F_TwoSidedLiftDrag, ///Face normals are flipped to match velocity and lift and drag forces are applied 97 F_OneSided, ///Face normals are taken as it is 98 END 99 }; 100 }; 101 102 ///eVSolver : velocities solvers 103 struct eVSolver 104 { 105 enum _ 106 { 107 Linear, ///Linear solver 108 END 109 }; 110 }; 111 112 ///ePSolver : positions solvers 113 struct ePSolver 114 { 115 enum _ 116 { 117 Linear, ///Linear solver 118 Anchors, ///Anchor solver 119 RContacts, ///Rigid contacts solver 120 SContacts, ///Soft contacts solver 121 END 122 }; 123 }; 124 125 ///eSolverPresets 126 struct eSolverPresets 127 { 128 enum _ 129 { 130 Positions, 131 Velocities, 132 Default = Positions, 133 END 134 }; 135 }; 136 137 ///eFeature 138 struct eFeature 139 { 140 enum _ 141 { 142 None, 143 Node, 144 Link, 145 Face, 146 Tetra, 147 END 148 }; 149 }; 150 151 typedef btAlignedObjectArray<eVSolver::_> tVSolverArray; 152 typedef btAlignedObjectArray<ePSolver::_> tPSolverArray; 153 154 // 155 // Flags 156 // 157 158 ///fCollision 159 struct fCollision 160 { 161 enum _ 162 { 163 RVSmask = 0x000f, ///Rigid versus soft mask 164 SDF_RS = 0x0001, ///SDF based rigid vs soft 165 CL_RS = 0x0002, ///Cluster vs convex rigid vs soft 166 SDF_RD = 0x0004, ///rigid vs deformable 167 168 SVSmask = 0x00f0, ///Rigid versus soft mask 169 VF_SS = 0x0010, ///Vertex vs face soft vs soft handling 170 CL_SS = 0x0020, ///Cluster vs cluster soft vs soft handling 171 CL_SELF = 0x0040, ///Cluster soft body self collision 172 VF_DD = 0x0080, ///Vertex vs face soft vs soft handling 173 174 RVDFmask = 0x0f00, /// Rigid versus deformable face mask 175 SDF_RDF = 0x0100, /// GJK based Rigid vs. deformable face 176 SDF_MDF = 0x0200, /// GJK based Multibody vs. deformable face 177 SDF_RDN = 0x0400, /// SDF based Rigid vs. deformable node 178 /* presets */ 179 Default = SDF_RS, 180 END 181 }; 182 }; 183 184 ///fMaterial 185 struct fMaterial 186 { 187 enum _ 188 { 189 DebugDraw = 0x0001, /// Enable debug draw 190 /* presets */ 191 Default = DebugDraw, 192 END 193 }; 194 }; 195 196 // 197 // API Types 198 // 199 200 /* sRayCast */ 201 struct sRayCast 202 { 203 btSoftBody* body; /// soft body 204 eFeature::_ feature; /// feature type 205 int index; /// feature index 206 btScalar fraction; /// time of impact fraction (rayorg+(rayto-rayfrom)*fraction) 207 }; 208 209 /* ImplicitFn */ 210 struct ImplicitFn 211 { ~ImplicitFnImplicitFn212 virtual ~ImplicitFn() {} 213 virtual btScalar Eval(const btVector3& x) = 0; 214 }; 215 216 // 217 // Internal types 218 // 219 220 typedef btAlignedObjectArray<btScalar> tScalarArray; 221 typedef btAlignedObjectArray<btVector3> tVector3Array; 222 223 /* sCti is Softbody contact info */ 224 struct sCti 225 { 226 const btCollisionObject* m_colObj; /* Rigid body */ 227 btVector3 m_normal; /* Outward normal */ 228 btScalar m_offset; /* Offset from origin */ 229 btVector3 m_bary; /* Barycentric weights for faces */ 230 }; 231 232 /* sMedium */ 233 struct sMedium 234 { 235 btVector3 m_velocity; /* Velocity */ 236 btScalar m_pressure; /* Pressure */ 237 btScalar m_density; /* Density */ 238 }; 239 240 /* Base type */ 241 struct Element 242 { 243 void* m_tag; // User data ElementElement244 Element() : m_tag(0) {} 245 }; 246 /* Material */ 247 struct Material : Element 248 { 249 btScalar m_kLST; // Linear stiffness coefficient [0,1] 250 btScalar m_kAST; // Area/Angular stiffness coefficient [0,1] 251 btScalar m_kVST; // Volume stiffness coefficient [0,1] 252 int m_flags; // Flags 253 }; 254 255 /* Feature */ 256 struct Feature : Element 257 { 258 Material* m_material; // Material 259 }; 260 /* Node */ 261 struct Node : Feature 262 { 263 btVector3 m_x; // Position 264 btVector3 m_q; // Previous step position/Test position 265 btVector3 m_v; // Velocity 266 btVector3 m_vn; // Previous step velocity 267 btVector3 m_f; // Force accumulator 268 btVector3 m_n; // Normal 269 btScalar m_im; // 1/mass 270 btScalar m_area; // Area 271 btDbvtNode* m_leaf; // Leaf data 272 int m_constrained; // depth of penetration 273 int m_battach : 1; // Attached 274 int index; 275 btVector3 m_splitv; // velocity associated with split impulse 276 btMatrix3x3 m_effectiveMass; // effective mass in contact 277 btMatrix3x3 m_effectiveMass_inv; // inverse of effective mass 278 }; 279 /* Link */ ATTRIBUTE_ALIGNED16(struct)280 ATTRIBUTE_ALIGNED16(struct) 281 Link : Feature 282 { 283 btVector3 m_c3; // gradient 284 Node* m_n[2]; // Node pointers 285 btScalar m_rl; // Rest length 286 int m_bbending : 1; // Bending link 287 btScalar m_c0; // (ima+imb)*kLST 288 btScalar m_c1; // rl^2 289 btScalar m_c2; // |gradient|^2/c0 290 291 BT_DECLARE_ALIGNED_ALLOCATOR(); 292 }; 293 /* Face */ 294 struct Face : Feature 295 { 296 Node* m_n[3]; // Node pointers 297 btVector3 m_normal; // Normal 298 btScalar m_ra; // Rest area 299 btDbvtNode* m_leaf; // Leaf data 300 btVector4 m_pcontact; // barycentric weights of the persistent contact 301 btVector3 m_n0, m_n1, m_vn; 302 int m_index; 303 }; 304 /* Tetra */ 305 struct Tetra : Feature 306 { 307 Node* m_n[4]; // Node pointers 308 btScalar m_rv; // Rest volume 309 btDbvtNode* m_leaf; // Leaf data 310 btVector3 m_c0[4]; // gradients 311 btScalar m_c1; // (4*kVST)/(im0+im1+im2+im3) 312 btScalar m_c2; // m_c1/sum(|g0..3|^2) 313 btMatrix3x3 m_Dm_inverse; // rest Dm^-1 314 btMatrix3x3 m_F; 315 btScalar m_element_measure; 316 btVector4 m_P_inv[3]; // first three columns of P_inv matrix 317 }; 318 319 /* TetraScratch */ 320 struct TetraScratch 321 { 322 btMatrix3x3 m_F; // deformation gradient F 323 btScalar m_trace; // trace of F^T * F 324 btScalar m_J; // det(F) 325 btMatrix3x3 m_cofF; // cofactor of F 326 btMatrix3x3 m_corotation; // corotatio of the tetra 327 }; 328 329 /* RContact */ 330 struct RContact 331 { 332 sCti m_cti; // Contact infos 333 Node* m_node; // Owner node 334 btMatrix3x3 m_c0; // Impulse matrix 335 btVector3 m_c1; // Relative anchor 336 btScalar m_c2; // ima*dt 337 btScalar m_c3; // Friction 338 btScalar m_c4; // Hardness 339 340 // jacobians and unit impulse responses for multibody 341 btMultiBodyJacobianData jacobianData_normal; 342 btMultiBodyJacobianData jacobianData_t1; 343 btMultiBodyJacobianData jacobianData_t2; 344 btVector3 t1; 345 btVector3 t2; 346 }; 347 348 class DeformableRigidContact 349 { 350 public: 351 sCti m_cti; // Contact infos 352 btMatrix3x3 m_c0; // Impulse matrix 353 btVector3 m_c1; // Relative anchor 354 btScalar m_c2; // inverse mass of node/face 355 btScalar m_c3; // Friction 356 btScalar m_c4; // Hardness 357 btMatrix3x3 m_c5; // inverse effective mass 358 359 // jacobians and unit impulse responses for multibody 360 btMultiBodyJacobianData jacobianData_normal; 361 btMultiBodyJacobianData jacobianData_t1; 362 btMultiBodyJacobianData jacobianData_t2; 363 btVector3 t1; 364 btVector3 t2; 365 }; 366 367 class DeformableNodeRigidContact : public DeformableRigidContact 368 { 369 public: 370 Node* m_node; // Owner node 371 }; 372 373 class DeformableNodeRigidAnchor : public DeformableNodeRigidContact 374 { 375 public: 376 btVector3 m_local; // Anchor position in body space 377 }; 378 379 class DeformableFaceRigidContact : public DeformableRigidContact 380 { 381 public: 382 Face* m_face; // Owner face 383 btVector3 m_contactPoint; // Contact point 384 btVector3 m_bary; // Barycentric weights 385 btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v; 386 }; 387 388 struct DeformableFaceNodeContact 389 { 390 Node* m_node; // Node 391 Face* m_face; // Face 392 btVector3 m_bary; // Barycentric weights 393 btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v; 394 btVector3 m_normal; // Normal 395 btScalar m_margin; // Margin 396 btScalar m_friction; // Friction 397 btScalar m_imf; // inverse mass of the face at contact point 398 btScalar m_c0; // scale of the impulse matrix; 399 }; 400 401 /* SContact */ 402 struct SContact 403 { 404 Node* m_node; // Node 405 Face* m_face; // Face 406 btVector3 m_weights; // Weigths 407 btVector3 m_normal; // Normal 408 btScalar m_margin; // Margin 409 btScalar m_friction; // Friction 410 btScalar m_cfm[2]; // Constraint force mixing 411 }; 412 /* Anchor */ 413 struct Anchor 414 { 415 Node* m_node; // Node pointer 416 btVector3 m_local; // Anchor position in body space 417 btRigidBody* m_body; // Body 418 btScalar m_influence; 419 btMatrix3x3 m_c0; // Impulse matrix 420 btVector3 m_c1; // Relative anchor 421 btScalar m_c2; // ima*dt 422 }; 423 /* Note */ 424 struct Note : Element 425 { 426 const char* m_text; // Text 427 btVector3 m_offset; // Offset 428 int m_rank; // Rank 429 Node* m_nodes[4]; // Nodes 430 btScalar m_coords[4]; // Coordinates 431 }; 432 /* Pose */ 433 struct Pose 434 { 435 bool m_bvolume; // Is valid 436 bool m_bframe; // Is frame 437 btScalar m_volume; // Rest volume 438 tVector3Array m_pos; // Reference positions 439 tScalarArray m_wgh; // Weights 440 btVector3 m_com; // COM 441 btMatrix3x3 m_rot; // Rotation 442 btMatrix3x3 m_scl; // Scale 443 btMatrix3x3 m_aqq; // Base scaling 444 }; 445 /* Cluster */ 446 struct Cluster 447 { 448 tScalarArray m_masses; 449 btAlignedObjectArray<Node*> m_nodes; 450 tVector3Array m_framerefs; 451 btTransform m_framexform; 452 btScalar m_idmass; 453 btScalar m_imass; 454 btMatrix3x3 m_locii; 455 btMatrix3x3 m_invwi; 456 btVector3 m_com; 457 btVector3 m_vimpulses[2]; 458 btVector3 m_dimpulses[2]; 459 int m_nvimpulses; 460 int m_ndimpulses; 461 btVector3 m_lv; 462 btVector3 m_av; 463 btDbvtNode* m_leaf; 464 btScalar m_ndamping; /* Node damping */ 465 btScalar m_ldamping; /* Linear damping */ 466 btScalar m_adamping; /* Angular damping */ 467 btScalar m_matching; 468 btScalar m_maxSelfCollisionImpulse; 469 btScalar m_selfCollisionImpulseFactor; 470 bool m_containsAnchor; 471 bool m_collide; 472 int m_clusterIndex; ClusterCluster473 Cluster() : m_leaf(0), m_ndamping(0), m_ldamping(0), m_adamping(0), m_matching(0), m_maxSelfCollisionImpulse(100.f), m_selfCollisionImpulseFactor(0.01f), m_containsAnchor(false) 474 { 475 } 476 }; 477 /* Impulse */ 478 struct Impulse 479 { 480 btVector3 m_velocity; 481 btVector3 m_drift; 482 int m_asVelocity : 1; 483 int m_asDrift : 1; ImpulseImpulse484 Impulse() : m_velocity(0, 0, 0), m_drift(0, 0, 0), m_asVelocity(0), m_asDrift(0) {} 485 Impulse operator-() const 486 { 487 Impulse i = *this; 488 i.m_velocity = -i.m_velocity; 489 i.m_drift = -i.m_drift; 490 return (i); 491 } 492 Impulse operator*(btScalar x) const 493 { 494 Impulse i = *this; 495 i.m_velocity *= x; 496 i.m_drift *= x; 497 return (i); 498 } 499 }; 500 /* Body */ 501 struct Body 502 { 503 Cluster* m_soft; 504 btRigidBody* m_rigid; 505 const btCollisionObject* m_collisionObject; 506 BodyBody507 Body() : m_soft(0), m_rigid(0), m_collisionObject(0) {} BodyBody508 Body(Cluster* p) : m_soft(p), m_rigid(0), m_collisionObject(0) {} BodyBody509 Body(const btCollisionObject* colObj) : m_soft(0), m_collisionObject(colObj) 510 { 511 m_rigid = (btRigidBody*)btRigidBody::upcast(m_collisionObject); 512 } 513 activateBody514 void activate() const 515 { 516 if (m_rigid) 517 m_rigid->activate(); 518 if (m_collisionObject) 519 m_collisionObject->activate(); 520 } invWorldInertiaBody521 const btMatrix3x3& invWorldInertia() const 522 { 523 static const btMatrix3x3 iwi(0, 0, 0, 0, 0, 0, 0, 0, 0); 524 if (m_rigid) return (m_rigid->getInvInertiaTensorWorld()); 525 if (m_soft) return (m_soft->m_invwi); 526 return (iwi); 527 } invMassBody528 btScalar invMass() const 529 { 530 if (m_rigid) return (m_rigid->getInvMass()); 531 if (m_soft) return (m_soft->m_imass); 532 return (0); 533 } xformBody534 const btTransform& xform() const 535 { 536 static const btTransform identity = btTransform::getIdentity(); 537 if (m_collisionObject) return (m_collisionObject->getWorldTransform()); 538 if (m_soft) return (m_soft->m_framexform); 539 return (identity); 540 } linearVelocityBody541 btVector3 linearVelocity() const 542 { 543 if (m_rigid) return (m_rigid->getLinearVelocity()); 544 if (m_soft) return (m_soft->m_lv); 545 return (btVector3(0, 0, 0)); 546 } angularVelocityBody547 btVector3 angularVelocity(const btVector3& rpos) const 548 { 549 if (m_rigid) return (btCross(m_rigid->getAngularVelocity(), rpos)); 550 if (m_soft) return (btCross(m_soft->m_av, rpos)); 551 return (btVector3(0, 0, 0)); 552 } angularVelocityBody553 btVector3 angularVelocity() const 554 { 555 if (m_rigid) return (m_rigid->getAngularVelocity()); 556 if (m_soft) return (m_soft->m_av); 557 return (btVector3(0, 0, 0)); 558 } velocityBody559 btVector3 velocity(const btVector3& rpos) const 560 { 561 return (linearVelocity() + angularVelocity(rpos)); 562 } applyVImpulseBody563 void applyVImpulse(const btVector3& impulse, const btVector3& rpos) const 564 { 565 if (m_rigid) m_rigid->applyImpulse(impulse, rpos); 566 if (m_soft) btSoftBody::clusterVImpulse(m_soft, rpos, impulse); 567 } applyDImpulseBody568 void applyDImpulse(const btVector3& impulse, const btVector3& rpos) const 569 { 570 if (m_rigid) m_rigid->applyImpulse(impulse, rpos); 571 if (m_soft) btSoftBody::clusterDImpulse(m_soft, rpos, impulse); 572 } applyImpulseBody573 void applyImpulse(const Impulse& impulse, const btVector3& rpos) const 574 { 575 if (impulse.m_asVelocity) 576 { 577 // printf("impulse.m_velocity = %f,%f,%f\n",impulse.m_velocity.getX(),impulse.m_velocity.getY(),impulse.m_velocity.getZ()); 578 applyVImpulse(impulse.m_velocity, rpos); 579 } 580 if (impulse.m_asDrift) 581 { 582 // printf("impulse.m_drift = %f,%f,%f\n",impulse.m_drift.getX(),impulse.m_drift.getY(),impulse.m_drift.getZ()); 583 applyDImpulse(impulse.m_drift, rpos); 584 } 585 } applyVAImpulseBody586 void applyVAImpulse(const btVector3& impulse) const 587 { 588 if (m_rigid) m_rigid->applyTorqueImpulse(impulse); 589 if (m_soft) btSoftBody::clusterVAImpulse(m_soft, impulse); 590 } applyDAImpulseBody591 void applyDAImpulse(const btVector3& impulse) const 592 { 593 if (m_rigid) m_rigid->applyTorqueImpulse(impulse); 594 if (m_soft) btSoftBody::clusterDAImpulse(m_soft, impulse); 595 } applyAImpulseBody596 void applyAImpulse(const Impulse& impulse) const 597 { 598 if (impulse.m_asVelocity) applyVAImpulse(impulse.m_velocity); 599 if (impulse.m_asDrift) applyDAImpulse(impulse.m_drift); 600 } applyDCImpulseBody601 void applyDCImpulse(const btVector3& impulse) const 602 { 603 if (m_rigid) m_rigid->applyCentralImpulse(impulse); 604 if (m_soft) btSoftBody::clusterDCImpulse(m_soft, impulse); 605 } 606 }; 607 /* Joint */ 608 struct Joint 609 { 610 struct eType 611 { 612 enum _ 613 { 614 Linear = 0, 615 Angular, 616 Contact 617 }; 618 }; 619 struct Specs 620 { SpecsJoint::Specs621 Specs() : erp(1), cfm(1), split(1) {} 622 btScalar erp; 623 btScalar cfm; 624 btScalar split; 625 }; 626 Body m_bodies[2]; 627 btVector3 m_refs[2]; 628 btScalar m_cfm; 629 btScalar m_erp; 630 btScalar m_split; 631 btVector3 m_drift; 632 btVector3 m_sdrift; 633 btMatrix3x3 m_massmatrix; 634 bool m_delete; ~JointJoint635 virtual ~Joint() {} JointJoint636 Joint() : m_delete(false) {} 637 virtual void Prepare(btScalar dt, int iterations); 638 virtual void Solve(btScalar dt, btScalar sor) = 0; 639 virtual void Terminate(btScalar dt) = 0; 640 virtual eType::_ Type() const = 0; 641 }; 642 /* LJoint */ 643 struct LJoint : Joint 644 { 645 struct Specs : Joint::Specs 646 { 647 btVector3 position; 648 }; 649 btVector3 m_rpos[2]; 650 void Prepare(btScalar dt, int iterations); 651 void Solve(btScalar dt, btScalar sor); 652 void Terminate(btScalar dt); TypeLJoint653 eType::_ Type() const { return (eType::Linear); } 654 }; 655 /* AJoint */ 656 struct AJoint : Joint 657 { 658 struct IControl 659 { ~IControlAJoint::IControl660 virtual ~IControl() {} PrepareAJoint::IControl661 virtual void Prepare(AJoint*) {} SpeedAJoint::IControl662 virtual btScalar Speed(AJoint*, btScalar current) { return (current); } DefaultAJoint::IControl663 static IControl* Default() 664 { 665 static IControl def; 666 return (&def); 667 } 668 }; 669 struct Specs : Joint::Specs 670 { SpecsAJoint::Specs671 Specs() : icontrol(IControl::Default()) {} 672 btVector3 axis; 673 IControl* icontrol; 674 }; 675 btVector3 m_axis[2]; 676 IControl* m_icontrol; 677 void Prepare(btScalar dt, int iterations); 678 void Solve(btScalar dt, btScalar sor); 679 void Terminate(btScalar dt); TypeAJoint680 eType::_ Type() const { return (eType::Angular); } 681 }; 682 /* CJoint */ 683 struct CJoint : Joint 684 { 685 int m_life; 686 int m_maxlife; 687 btVector3 m_rpos[2]; 688 btVector3 m_normal; 689 btScalar m_friction; 690 void Prepare(btScalar dt, int iterations); 691 void Solve(btScalar dt, btScalar sor); 692 void Terminate(btScalar dt); TypeCJoint693 eType::_ Type() const { return (eType::Contact); } 694 }; 695 /* Config */ 696 struct Config 697 { 698 eAeroModel::_ aeromodel; // Aerodynamic model (default: V_Point) 699 btScalar kVCF; // Velocities correction factor (Baumgarte) 700 btScalar kDP; // Damping coefficient [0,1] 701 btScalar kDG; // Drag coefficient [0,+inf] 702 btScalar kLF; // Lift coefficient [0,+inf] 703 btScalar kPR; // Pressure coefficient [-inf,+inf] 704 btScalar kVC; // Volume conversation coefficient [0,+inf] 705 btScalar kDF; // Dynamic friction coefficient [0,1] 706 btScalar kMT; // Pose matching coefficient [0,1] 707 btScalar kCHR; // Rigid contacts hardness [0,1] 708 btScalar kKHR; // Kinetic contacts hardness [0,1] 709 btScalar kSHR; // Soft contacts hardness [0,1] 710 btScalar kAHR; // Anchors hardness [0,1] 711 btScalar kSRHR_CL; // Soft vs rigid hardness [0,1] (cluster only) 712 btScalar kSKHR_CL; // Soft vs kinetic hardness [0,1] (cluster only) 713 btScalar kSSHR_CL; // Soft vs soft hardness [0,1] (cluster only) 714 btScalar kSR_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only) 715 btScalar kSK_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only) 716 btScalar kSS_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only) 717 btScalar maxvolume; // Maximum volume ratio for pose 718 btScalar timescale; // Time scale 719 int viterations; // Velocities solver iterations 720 int piterations; // Positions solver iterations 721 int diterations; // Drift solver iterations 722 int citerations; // Cluster solver iterations 723 int collisions; // Collisions flags 724 tVSolverArray m_vsequence; // Velocity solvers sequence 725 tPSolverArray m_psequence; // Position solvers sequence 726 tPSolverArray m_dsequence; // Drift solvers sequence 727 btScalar drag; // deformable air drag 728 btScalar m_maxStress; // Maximum principle first Piola stress 729 }; 730 /* SolverState */ 731 struct SolverState 732 { 733 //if you add new variables, always initialize them! SolverStateSolverState734 SolverState() 735 : sdt(0), 736 isdt(0), 737 velmrg(0), 738 radmrg(0), 739 updmrg(0) 740 { 741 } 742 btScalar sdt; // dt*timescale 743 btScalar isdt; // 1/sdt 744 btScalar velmrg; // velocity margin 745 btScalar radmrg; // radial margin 746 btScalar updmrg; // Update margin 747 }; 748 /// RayFromToCaster takes a ray from, ray to (instead of direction!) 749 struct RayFromToCaster : btDbvt::ICollide 750 { 751 btVector3 m_rayFrom; 752 btVector3 m_rayTo; 753 btVector3 m_rayNormalizedDirection; 754 btScalar m_mint; 755 Face* m_face; 756 int m_tests; 757 RayFromToCaster(const btVector3& rayFrom, const btVector3& rayTo, btScalar mxt); 758 void Process(const btDbvtNode* leaf); 759 760 static /*inline*/ btScalar rayFromToTriangle(const btVector3& rayFrom, 761 const btVector3& rayTo, 762 const btVector3& rayNormalizedDirection, 763 const btVector3& a, 764 const btVector3& b, 765 const btVector3& c, 766 btScalar maxt = SIMD_INFINITY); 767 }; 768 769 // 770 // Typedefs 771 // 772 773 typedef void (*psolver_t)(btSoftBody*, btScalar, btScalar); 774 typedef void (*vsolver_t)(btSoftBody*, btScalar); 775 typedef btAlignedObjectArray<Cluster*> tClusterArray; 776 typedef btAlignedObjectArray<Note> tNoteArray; 777 typedef btAlignedObjectArray<Node> tNodeArray; 778 typedef btAlignedObjectArray<btDbvtNode*> tLeafArray; 779 typedef btAlignedObjectArray<Link> tLinkArray; 780 typedef btAlignedObjectArray<Face> tFaceArray; 781 typedef btAlignedObjectArray<Tetra> tTetraArray; 782 typedef btAlignedObjectArray<Anchor> tAnchorArray; 783 typedef btAlignedObjectArray<RContact> tRContactArray; 784 typedef btAlignedObjectArray<SContact> tSContactArray; 785 typedef btAlignedObjectArray<Material*> tMaterialArray; 786 typedef btAlignedObjectArray<Joint*> tJointArray; 787 typedef btAlignedObjectArray<btSoftBody*> tSoftBodyArray; 788 789 // 790 // Fields 791 // 792 793 Config m_cfg; // Configuration 794 SolverState m_sst; // Solver state 795 Pose m_pose; // Pose 796 void* m_tag; // User data 797 btSoftBodyWorldInfo* m_worldInfo; // World info 798 tNoteArray m_notes; // Notes 799 tNodeArray m_nodes; // Nodes 800 tNodeArray m_renderNodes; // Nodes 801 tLinkArray m_links; // Links 802 tFaceArray m_faces; // Faces 803 tFaceArray m_renderFaces; // Faces 804 tTetraArray m_tetras; // Tetras 805 btAlignedObjectArray<TetraScratch> m_tetraScratches; 806 btAlignedObjectArray<TetraScratch> m_tetraScratchesTn; 807 tAnchorArray m_anchors; // Anchors 808 btAlignedObjectArray<DeformableNodeRigidAnchor> m_deformableAnchors; 809 tRContactArray m_rcontacts; // Rigid contacts 810 btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts; 811 btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts; 812 btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts; 813 tSContactArray m_scontacts; // Soft contacts 814 tJointArray m_joints; // Joints 815 tMaterialArray m_materials; // Materials 816 btScalar m_timeacc; // Time accumulator 817 btVector3 m_bounds[2]; // Spatial bounds 818 bool m_bUpdateRtCst; // Update runtime constants 819 btDbvt m_ndbvt; // Nodes tree 820 btDbvt m_fdbvt; // Faces tree 821 btDbvntNode* m_fdbvnt; // Faces tree with normals 822 btDbvt m_cdbvt; // Clusters tree 823 tClusterArray m_clusters; // Clusters 824 btScalar m_dampingCoefficient; // Damping Coefficient 825 btScalar m_sleepingThreshold; 826 btScalar m_maxSpeedSquared; 827 btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection 828 btScalar m_repulsionStiffness; 829 btScalar m_gravityFactor; 830 btAlignedObjectArray<btVector3> m_X; // initial positions 831 832 btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights; 833 btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents; 834 btAlignedObjectArray<btScalar> m_z; // vertical distance used in extrapolation 835 bool m_useSelfCollision; 836 bool m_softSoftCollision; 837 838 btAlignedObjectArray<bool> m_clusterConnectivity; //cluster connectivity, for self-collision 839 840 btVector3 m_windVelocity; 841 842 btScalar m_restLengthScale; 843 844 // 845 // Api 846 // 847 848 /* ctor */ 849 btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m); 850 851 /* ctor */ 852 btSoftBody(btSoftBodyWorldInfo* worldInfo); 853 854 void initDefaults(); 855 856 /* dtor */ 857 virtual ~btSoftBody(); 858 /* Check for existing link */ 859 860 btAlignedObjectArray<int> m_userIndexMapping; 861 getWorldInfo()862 btSoftBodyWorldInfo* getWorldInfo() 863 { 864 return m_worldInfo; 865 } 866 setDampingCoefficient(btScalar damping_coeff)867 void setDampingCoefficient(btScalar damping_coeff) 868 { 869 m_dampingCoefficient = damping_coeff; 870 } 871 872 ///@todo: avoid internal softbody shape hack and move collision code to collision library setCollisionShape(btCollisionShape * collisionShape)873 virtual void setCollisionShape(btCollisionShape* collisionShape) 874 { 875 } 876 877 bool checkLink(int node0, 878 int node1) const; 879 bool checkLink(const Node* node0, 880 const Node* node1) const; 881 /* Check for existring face */ 882 bool checkFace(int node0, 883 int node1, 884 int node2) const; 885 /* Append material */ 886 Material* appendMaterial(); 887 /* Append note */ 888 void appendNote(const char* text, 889 const btVector3& o, 890 const btVector4& c = btVector4(1, 0, 0, 0), 891 Node* n0 = 0, 892 Node* n1 = 0, 893 Node* n2 = 0, 894 Node* n3 = 0); 895 void appendNote(const char* text, 896 const btVector3& o, 897 Node* feature); 898 void appendNote(const char* text, 899 const btVector3& o, 900 Link* feature); 901 void appendNote(const char* text, 902 const btVector3& o, 903 Face* feature); 904 /* Append node */ 905 void appendNode(const btVector3& x, btScalar m); 906 /* Append link */ 907 void appendLink(int model = -1, Material* mat = 0); 908 void appendLink(int node0, 909 int node1, 910 Material* mat = 0, 911 bool bcheckexist = false); 912 void appendLink(Node* node0, 913 Node* node1, 914 Material* mat = 0, 915 bool bcheckexist = false); 916 /* Append face */ 917 void appendFace(int model = -1, Material* mat = 0); 918 void appendFace(int node0, 919 int node1, 920 int node2, 921 Material* mat = 0); 922 void appendTetra(int model, Material* mat); 923 // 924 void appendTetra(int node0, 925 int node1, 926 int node2, 927 int node3, 928 Material* mat = 0); 929 930 /* Append anchor */ 931 void appendDeformableAnchor(int node, btRigidBody* body); 932 void appendDeformableAnchor(int node, btMultiBodyLinkCollider* link); 933 void appendAnchor(int node, 934 btRigidBody* body, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1); 935 void appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1); 936 void removeAnchor(int node); 937 /* Append linear joint */ 938 void appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1); 939 void appendLinearJoint(const LJoint::Specs& specs, Body body = Body()); 940 void appendLinearJoint(const LJoint::Specs& specs, btSoftBody* body); 941 /* Append linear joint */ 942 void appendAngularJoint(const AJoint::Specs& specs, Cluster* body0, Body body1); 943 void appendAngularJoint(const AJoint::Specs& specs, Body body = Body()); 944 void appendAngularJoint(const AJoint::Specs& specs, btSoftBody* body); 945 /* Add force (or gravity) to the entire body */ 946 void addForce(const btVector3& force); 947 /* Add force (or gravity) to a node of the body */ 948 void addForce(const btVector3& force, 949 int node); 950 /* Add aero force to a node of the body */ 951 void addAeroForceToNode(const btVector3& windVelocity, int nodeIndex); 952 953 /* Add aero force to a face of the body */ 954 void addAeroForceToFace(const btVector3& windVelocity, int faceIndex); 955 956 /* Add velocity to the entire body */ 957 void addVelocity(const btVector3& velocity); 958 959 /* Set velocity for the entire body */ 960 void setVelocity(const btVector3& velocity); 961 962 /* Add velocity to a node of the body */ 963 void addVelocity(const btVector3& velocity, 964 int node); 965 /* Set mass */ 966 void setMass(int node, 967 btScalar mass); 968 /* Get mass */ 969 btScalar getMass(int node) const; 970 /* Get total mass */ 971 btScalar getTotalMass() const; 972 /* Set total mass (weighted by previous masses) */ 973 void setTotalMass(btScalar mass, 974 bool fromfaces = false); 975 /* Set total density */ 976 void setTotalDensity(btScalar density); 977 /* Set volume mass (using tetrahedrons) */ 978 void setVolumeMass(btScalar mass); 979 /* Set volume density (using tetrahedrons) */ 980 void setVolumeDensity(btScalar density); 981 /* Get the linear velocity of the center of mass */ 982 btVector3 getLinearVelocity(); 983 /* Set the linear velocity of the center of mass */ 984 void setLinearVelocity(const btVector3& linVel); 985 /* Set the angular velocity of the center of mass */ 986 void setAngularVelocity(const btVector3& angVel); 987 /* Get best fit rigid transform */ 988 btTransform getRigidTransform(); 989 /* Transform to given pose */ 990 void transformTo(const btTransform& trs); 991 /* Transform */ 992 void transform(const btTransform& trs); 993 /* Translate */ 994 void translate(const btVector3& trs); 995 /* Rotate */ 996 void rotate(const btQuaternion& rot); 997 /* Scale */ 998 void scale(const btVector3& scl); 999 /* Get link resting lengths scale */ 1000 btScalar getRestLengthScale(); 1001 /* Scale resting length of all springs */ 1002 void setRestLengthScale(btScalar restLength); 1003 /* Set current state as pose */ 1004 void setPose(bool bvolume, 1005 bool bframe); 1006 /* Set current link lengths as resting lengths */ 1007 void resetLinkRestLengths(); 1008 /* Return the volume */ 1009 btScalar getVolume() const; 1010 /* Cluster count */ getCenterOfMass()1011 btVector3 getCenterOfMass() const 1012 { 1013 btVector3 com(0, 0, 0); 1014 for (int i = 0; i < m_nodes.size(); i++) 1015 { 1016 com += (m_nodes[i].m_x * this->getMass(i)); 1017 } 1018 com /= this->getTotalMass(); 1019 return com; 1020 } 1021 int clusterCount() const; 1022 /* Cluster center of mass */ 1023 static btVector3 clusterCom(const Cluster* cluster); 1024 btVector3 clusterCom(int cluster) const; 1025 /* Cluster velocity at rpos */ 1026 static btVector3 clusterVelocity(const Cluster* cluster, const btVector3& rpos); 1027 /* Cluster impulse */ 1028 static void clusterVImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse); 1029 static void clusterDImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse); 1030 static void clusterImpulse(Cluster* cluster, const btVector3& rpos, const Impulse& impulse); 1031 static void clusterVAImpulse(Cluster* cluster, const btVector3& impulse); 1032 static void clusterDAImpulse(Cluster* cluster, const btVector3& impulse); 1033 static void clusterAImpulse(Cluster* cluster, const Impulse& impulse); 1034 static void clusterDCImpulse(Cluster* cluster, const btVector3& impulse); 1035 /* Generate bending constraints based on distance in the adjency graph */ 1036 int generateBendingConstraints(int distance, 1037 Material* mat = 0); 1038 /* Randomize constraints to reduce solver bias */ 1039 void randomizeConstraints(); 1040 /* Release clusters */ 1041 void releaseCluster(int index); 1042 void releaseClusters(); 1043 /* Generate clusters (K-mean) */ 1044 ///generateClusters with k=0 will create a convex cluster for each tetrahedron or triangle 1045 ///otherwise an approximation will be used (better performance) 1046 int generateClusters(int k, int maxiterations = 8192); 1047 /* Refine */ 1048 void refine(ImplicitFn* ifn, btScalar accurary, bool cut); 1049 /* CutLink */ 1050 bool cutLink(int node0, int node1, btScalar position); 1051 bool cutLink(const Node* node0, const Node* node1, btScalar position); 1052 1053 ///Ray casting using rayFrom and rayTo in worldspace, (not direction!) 1054 bool rayTest(const btVector3& rayFrom, 1055 const btVector3& rayTo, 1056 sRayCast& results); 1057 bool rayFaceTest(const btVector3& rayFrom, 1058 const btVector3& rayTo, 1059 sRayCast& results); 1060 int rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo, 1061 btScalar& mint, int& index) const; 1062 /* Solver presets */ 1063 void setSolver(eSolverPresets::_ preset); 1064 /* predictMotion */ 1065 void predictMotion(btScalar dt); 1066 /* solveConstraints */ 1067 void solveConstraints(); 1068 /* staticSolve */ 1069 void staticSolve(int iterations); 1070 /* solveCommonConstraints */ 1071 static void solveCommonConstraints(btSoftBody** bodies, int count, int iterations); 1072 /* solveClusters */ 1073 static void solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies); 1074 /* integrateMotion */ 1075 void integrateMotion(); 1076 /* defaultCollisionHandlers */ 1077 void defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap); 1078 void defaultCollisionHandler(btSoftBody* psb); 1079 void setSelfCollision(bool useSelfCollision); 1080 bool useSelfCollision(); 1081 void updateDeactivation(btScalar timeStep); 1082 void setZeroVelocity(); 1083 bool wantsSleeping(); 1084 1085 // 1086 // Functionality to deal with new accelerated solvers. 1087 // 1088 1089 /** 1090 * Set a wind velocity for interaction with the air. 1091 */ 1092 void setWindVelocity(const btVector3& velocity); 1093 1094 /** 1095 * Return the wind velocity for interaction with the air. 1096 */ 1097 const btVector3& getWindVelocity(); 1098 1099 // 1100 // Set the solver that handles this soft body 1101 // Should not be allowed to get out of sync with reality 1102 // Currently called internally on addition to the world setSoftBodySolver(btSoftBodySolver * softBodySolver)1103 void setSoftBodySolver(btSoftBodySolver* softBodySolver) 1104 { 1105 m_softBodySolver = softBodySolver; 1106 } 1107 1108 // 1109 // Return the solver that handles this soft body 1110 // getSoftBodySolver()1111 btSoftBodySolver* getSoftBodySolver() 1112 { 1113 return m_softBodySolver; 1114 } 1115 1116 // 1117 // Return the solver that handles this soft body 1118 // getSoftBodySolver()1119 btSoftBodySolver* getSoftBodySolver() const 1120 { 1121 return m_softBodySolver; 1122 } 1123 1124 // 1125 // Cast 1126 // 1127 upcast(const btCollisionObject * colObj)1128 static const btSoftBody* upcast(const btCollisionObject* colObj) 1129 { 1130 if (colObj->getInternalType() == CO_SOFT_BODY) 1131 return (const btSoftBody*)colObj; 1132 return 0; 1133 } upcast(btCollisionObject * colObj)1134 static btSoftBody* upcast(btCollisionObject* colObj) 1135 { 1136 if (colObj->getInternalType() == CO_SOFT_BODY) 1137 return (btSoftBody*)colObj; 1138 return 0; 1139 } 1140 1141 // 1142 // ::btCollisionObject 1143 // 1144 getAabb(btVector3 & aabbMin,btVector3 & aabbMax)1145 virtual void getAabb(btVector3& aabbMin, btVector3& aabbMax) const 1146 { 1147 aabbMin = m_bounds[0]; 1148 aabbMax = m_bounds[1]; 1149 } 1150 // 1151 // Private 1152 // 1153 void pointersToIndices(); 1154 void indicesToPointers(const int* map = 0); 1155 1156 int rayTest(const btVector3& rayFrom, const btVector3& rayTo, 1157 btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const; 1158 void initializeFaceTree(); 1159 void rebuildNodeTree(); 1160 btVector3 evaluateCom() const; 1161 bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; 1162 bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, Face& f, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; 1163 bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const; 1164 void updateNormals(); 1165 void updateBounds(); 1166 void updatePose(); 1167 void updateConstants(); 1168 void updateLinkConstants(); 1169 void updateArea(bool averageArea = true); 1170 void initializeClusters(); 1171 void updateClusters(); 1172 void cleanupClusters(); 1173 void prepareClusters(int iterations); 1174 void solveClusters(btScalar sor); 1175 void applyClusters(bool drift); 1176 void dampClusters(); 1177 void setSpringStiffness(btScalar k); 1178 void setGravityFactor(btScalar gravFactor); 1179 void initializeDmInverse(); 1180 void updateDeformation(); 1181 void advanceDeformation(); 1182 void applyForces(); 1183 void setMaxStress(btScalar maxStress); 1184 void interpolateRenderMesh(); 1185 void setCollisionQuadrature(int N); 1186 static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti); 1187 static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti); 1188 static void PSolve_SContacts(btSoftBody* psb, btScalar, btScalar ti); 1189 static void PSolve_Links(btSoftBody* psb, btScalar kst, btScalar ti); 1190 static void VSolve_Links(btSoftBody* psb, btScalar kst); 1191 static psolver_t getSolver(ePSolver::_ solver); 1192 static vsolver_t getSolver(eVSolver::_ solver); 1193 void geometricCollisionHandler(btSoftBody* psb); 1194 #define SAFE_EPSILON SIMD_EPSILON * 100.0 updateNode(btDbvtNode * node,bool use_velocity,bool margin)1195 void updateNode(btDbvtNode* node, bool use_velocity, bool margin) 1196 { 1197 if (node->isleaf()) 1198 { 1199 btSoftBody::Node* n = (btSoftBody::Node*)(node->data); 1200 ATTRIBUTE_ALIGNED16(btDbvtVolume) 1201 vol; 1202 btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision 1203 if (use_velocity) 1204 { 1205 btVector3 points[2] = {n->m_x, n->m_x + m_sst.sdt * n->m_v}; 1206 vol = btDbvtVolume::FromPoints(points, 2); 1207 vol.Expand(btVector3(pad, pad, pad)); 1208 } 1209 else 1210 { 1211 vol = btDbvtVolume::FromCR(n->m_x, pad); 1212 } 1213 node->volume = vol; 1214 return; 1215 } 1216 else 1217 { 1218 updateNode(node->childs[0], use_velocity, margin); 1219 updateNode(node->childs[1], use_velocity, margin); 1220 ATTRIBUTE_ALIGNED16(btDbvtVolume) 1221 vol; 1222 Merge(node->childs[0]->volume, node->childs[1]->volume, vol); 1223 node->volume = vol; 1224 } 1225 } 1226 updateNodeTree(bool use_velocity,bool margin)1227 void updateNodeTree(bool use_velocity, bool margin) 1228 { 1229 if (m_ndbvt.m_root) 1230 updateNode(m_ndbvt.m_root, use_velocity, margin); 1231 } 1232 1233 template <class DBVTNODE> // btDbvtNode or btDbvntNode updateFace(DBVTNODE * node,bool use_velocity,bool margin)1234 void updateFace(DBVTNODE* node, bool use_velocity, bool margin) 1235 { 1236 if (node->isleaf()) 1237 { 1238 btSoftBody::Face* f = (btSoftBody::Face*)(node->data); 1239 btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision 1240 ATTRIBUTE_ALIGNED16(btDbvtVolume) 1241 vol; 1242 if (use_velocity) 1243 { 1244 btVector3 points[6] = {f->m_n[0]->m_x, f->m_n[0]->m_x + m_sst.sdt * f->m_n[0]->m_v, 1245 f->m_n[1]->m_x, f->m_n[1]->m_x + m_sst.sdt * f->m_n[1]->m_v, 1246 f->m_n[2]->m_x, f->m_n[2]->m_x + m_sst.sdt * f->m_n[2]->m_v}; 1247 vol = btDbvtVolume::FromPoints(points, 6); 1248 } 1249 else 1250 { 1251 btVector3 points[3] = {f->m_n[0]->m_x, 1252 f->m_n[1]->m_x, 1253 f->m_n[2]->m_x}; 1254 vol = btDbvtVolume::FromPoints(points, 3); 1255 } 1256 vol.Expand(btVector3(pad, pad, pad)); 1257 node->volume = vol; 1258 return; 1259 } 1260 else 1261 { 1262 updateFace(node->childs[0], use_velocity, margin); 1263 updateFace(node->childs[1], use_velocity, margin); 1264 ATTRIBUTE_ALIGNED16(btDbvtVolume) 1265 vol; 1266 Merge(node->childs[0]->volume, node->childs[1]->volume, vol); 1267 node->volume = vol; 1268 } 1269 } updateFaceTree(bool use_velocity,bool margin)1270 void updateFaceTree(bool use_velocity, bool margin) 1271 { 1272 if (m_fdbvt.m_root) 1273 updateFace(m_fdbvt.m_root, use_velocity, margin); 1274 if (m_fdbvnt) 1275 updateFace(m_fdbvnt, use_velocity, margin); 1276 } 1277 1278 template <typename T> BaryEval(const T & a,const T & b,const T & c,const btVector3 & coord)1279 static inline T BaryEval(const T& a, 1280 const T& b, 1281 const T& c, 1282 const btVector3& coord) 1283 { 1284 return (a * coord.x() + b * coord.y() + c * coord.z()); 1285 } 1286 applyRepulsionForce(btScalar timeStep,bool applySpringForce)1287 void applyRepulsionForce(btScalar timeStep, bool applySpringForce) 1288 { 1289 btAlignedObjectArray<int> indices; 1290 { 1291 // randomize the order of repulsive force 1292 indices.resize(m_faceNodeContacts.size()); 1293 for (int i = 0; i < m_faceNodeContacts.size(); ++i) 1294 indices[i] = i; 1295 #define NEXTRAND (seed = (1664525L * seed + 1013904223L) & 0xffffffff) 1296 int i, ni; 1297 1298 for (i = 0, ni = indices.size(); i < ni; ++i) 1299 { 1300 btSwap(indices[i], indices[NEXTRAND % ni]); 1301 } 1302 } 1303 for (int k = 0; k < m_faceNodeContacts.size(); ++k) 1304 { 1305 int i = indices[k]; 1306 btSoftBody::DeformableFaceNodeContact& c = m_faceNodeContacts[i]; 1307 btSoftBody::Node* node = c.m_node; 1308 btSoftBody::Face* face = c.m_face; 1309 const btVector3& w = c.m_bary; 1310 const btVector3& n = c.m_normal; 1311 btVector3 l = node->m_x - BaryEval(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, w); 1312 btScalar d = c.m_margin - n.dot(l); 1313 d = btMax(btScalar(0), d); 1314 1315 const btVector3& va = node->m_v; 1316 btVector3 vb = BaryEval(face->m_n[0]->m_v, face->m_n[1]->m_v, face->m_n[2]->m_v, w); 1317 btVector3 vr = va - vb; 1318 const btScalar vn = btDot(vr, n); // dn < 0 <==> opposing 1319 if (vn > OVERLAP_REDUCTION_FACTOR * d / timeStep) 1320 continue; 1321 btVector3 vt = vr - vn * n; 1322 btScalar I = 0; 1323 btScalar mass = node->m_im == 0 ? 0 : btScalar(1) / node->m_im; 1324 if (applySpringForce) 1325 I = -btMin(m_repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn)); 1326 if (vn < 0) 1327 I += 0.5 * mass * vn; 1328 int face_penetration = 0, node_penetration = node->m_constrained; 1329 for (int i = 0; i < 3; ++i) 1330 face_penetration |= face->m_n[i]->m_constrained; 1331 btScalar I_tilde = 2.0 * I / (1.0 + w.length2()); 1332 1333 // double the impulse if node or face is constrained. 1334 if (face_penetration > 0 || node_penetration > 0) 1335 { 1336 I_tilde *= 2.0; 1337 } 1338 if (face_penetration <= 0) 1339 { 1340 for (int j = 0; j < 3; ++j) 1341 face->m_n[j]->m_v += w[j] * n * I_tilde * node->m_im; 1342 } 1343 if (node_penetration <= 0) 1344 { 1345 node->m_v -= I_tilde * node->m_im * n; 1346 } 1347 1348 // apply frictional impulse 1349 btScalar vt_norm = vt.safeNorm(); 1350 if (vt_norm > SIMD_EPSILON) 1351 { 1352 btScalar delta_vn = -2 * I * node->m_im; 1353 btScalar mu = c.m_friction; 1354 btScalar vt_new = btMax(btScalar(1) - mu * delta_vn / (vt_norm + SIMD_EPSILON), btScalar(0)) * vt_norm; 1355 I = 0.5 * mass * (vt_norm - vt_new); 1356 vt.safeNormalize(); 1357 I_tilde = 2.0 * I / (1.0 + w.length2()); 1358 // double the impulse if node or face is constrained. 1359 if (face_penetration > 0 || node_penetration > 0) 1360 I_tilde *= 2.0; 1361 if (face_penetration <= 0) 1362 { 1363 for (int j = 0; j < 3; ++j) 1364 face->m_n[j]->m_v += w[j] * vt * I_tilde * (face->m_n[j])->m_im; 1365 } 1366 if (node_penetration <= 0) 1367 { 1368 node->m_v -= I_tilde * node->m_im * vt; 1369 } 1370 } 1371 } 1372 } 1373 virtual int calculateSerializeBufferSize() const; 1374 1375 ///fills the dataBuffer and returns the struct name (and 0 on failure) 1376 virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; 1377 }; 1378 1379 #endif //_BT_SOFT_BODY_H 1380