dali-physics/third-party/bullet3/src/BulletSoftBody/btSoftBody.h

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
   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
  59         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         {
 212                 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
 244                 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 RenderNode
 262         {
 263                 btVector3 m_x;
 264                 btVector3 m_uv1;
 265                 btVector3 m_normal;
 266         };
 267         struct Node : Feature
 268         {
 269                 btVector3 m_x;       // Position
 270                 btVector3 m_q;       // Previous step position/Test position
 271                 btVector3 m_v;       // Velocity
 272                 btVector3 m_vn;      // Previous step velocity
 273                 btVector3 m_f;       // Force accumulator
 274                 btVector3 m_n;       // Normal
 275                 btScalar m_im;       // 1/mass
 276                 btScalar m_area;     // Area
 277                 btDbvtNode* m_leaf;  // Leaf data
 278                 int m_constrained;   // depth of penetration
 279                 int m_battach : 1;   // Attached
 280                 int index;
 281                 btVector3 m_splitv;               // velocity associated with split impulse
 282                 btMatrix3x3 m_effectiveMass;      // effective mass in contact
 283                 btMatrix3x3 m_effectiveMass_inv;  // inverse of effective mass
 284         };
 285         /* Link                 */
 286         ATTRIBUTE_ALIGNED16(struct)
 287         Link : Feature
 288         {
 289                 btVector3 m_c3;      // gradient
 290                 Node* m_n[2];        // Node pointers
 291                 btScalar m_rl;       // Rest length
 292                 int m_bbending : 1;  // Bending link
 293                 btScalar m_c0;       // (ima+imb)*kLST
 294                 btScalar m_c1;       // rl^2
 295                 btScalar m_c2;       // |gradient|^2/c0
 296
 297                 BT_DECLARE_ALIGNED_ALLOCATOR();
 298         };
 299         struct RenderFace
 300         {
 301                 RenderNode* m_n[3];  // Node pointers
 302         };
 303
 304         /* Face                 */
 305         struct Face : Feature
 306         {
 307                 Node* m_n[3];          // Node pointers
 308                 btVector3 m_normal;    // Normal
 309                 btScalar m_ra;         // Rest area
 310                 btDbvtNode* m_leaf;    // Leaf data
 311                 btVector4 m_pcontact;  // barycentric weights of the persistent contact
 312                 btVector3 m_n0, m_n1, m_vn;
 313                 int m_index;
 314         };
 315         /* Tetra                */
 316         struct Tetra : Feature
 317         {
 318                 Node* m_n[4];              // Node pointers
 319                 btScalar m_rv;             // Rest volume
 320                 btDbvtNode* m_leaf;        // Leaf data
 321                 btVector3 m_c0[4];         // gradients
 322                 btScalar m_c1;             // (4*kVST)/(im0+im1+im2+im3)
 323                 btScalar m_c2;             // m_c1/sum(|g0..3|^2)
 324                 btMatrix3x3 m_Dm_inverse;  // rest Dm^-1
 325                 btMatrix3x3 m_F;
 326                 btScalar m_element_measure;
 327                 btVector4 m_P_inv[3];  // first three columns of P_inv matrix
 328         };
 329
 330         /*  TetraScratch  */
 331         struct TetraScratch
 332         {
 333                 btMatrix3x3 m_F;           // deformation gradient F
 334                 btScalar m_trace;          // trace of F^T * F
 335                 btScalar m_J;              // det(F)
 336                 btMatrix3x3 m_cofF;        // cofactor of F
 337                 btMatrix3x3 m_corotation;  // corotatio of the tetra
 338         };
 339
 340         /* RContact             */
 341         struct RContact
 342         {
 343                 sCti m_cti;        // Contact infos
 344                 Node* m_node;      // Owner node
 345                 btMatrix3x3 m_c0;  // Impulse matrix
 346                 btVector3 m_c1;    // Relative anchor
 347                 btScalar m_c2;     // ima*dt
 348                 btScalar m_c3;     // Friction
 349                 btScalar m_c4;     // Hardness
 350
 351                 // jacobians and unit impulse responses for multibody
 352                 btMultiBodyJacobianData jacobianData_normal;
 353                 btMultiBodyJacobianData jacobianData_t1;
 354                 btMultiBodyJacobianData jacobianData_t2;
 355                 btVector3 t1;
 356                 btVector3 t2;
 357         };
 358
 359         class DeformableRigidContact
 360         {
 361         public:
 362                 sCti m_cti;        // Contact infos
 363                 btMatrix3x3 m_c0;  // Impulse matrix
 364                 btVector3 m_c1;    // Relative anchor
 365                 btScalar m_c2;     // inverse mass of node/face
 366                 btScalar m_c3;     // Friction
 367                 btScalar m_c4;     // Hardness
 368                 btMatrix3x3 m_c5;  // inverse effective mass
 369
 370                 // jacobians and unit impulse responses for multibody
 371                 btMultiBodyJacobianData jacobianData_normal;
 372                 btMultiBodyJacobianData jacobianData_t1;
 373                 btMultiBodyJacobianData jacobianData_t2;
 374                 btVector3 t1;
 375                 btVector3 t2;
 376         };
 377
 378         class DeformableNodeRigidContact : public DeformableRigidContact
 379         {
 380         public:
 381                 Node* m_node;  // Owner node
 382         };
 383
 384         class DeformableNodeRigidAnchor : public DeformableNodeRigidContact
 385         {
 386         public:
 387                 btVector3 m_local;  // Anchor position in body space
 388         };
 389
 390         class DeformableFaceRigidContact : public DeformableRigidContact
 391         {
 392         public:
 393                 Face* m_face;              // Owner face
 394                 btVector3 m_contactPoint;  // Contact point
 395                 btVector3 m_bary;          // Barycentric weights
 396                 btVector3 m_weights;       // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
 397         };
 398
 399         struct DeformableFaceNodeContact
 400         {
 401                 Node* m_node;         // Node
 402                 Face* m_face;         // Face
 403                 btVector3 m_bary;     // Barycentric weights
 404                 btVector3 m_weights;  // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
 405                 btVector3 m_normal;   // Normal
 406                 btScalar m_margin;    // Margin
 407                 btScalar m_friction;  // Friction
 408                 btScalar m_imf;       // inverse mass of the face at contact point
 409                 btScalar m_c0;        // scale of the impulse matrix;
 410                 const btCollisionObject* m_colObj;  // Collision object to collide with.
 411         };
 412
 413         /* SContact             */
 414         struct SContact
 415         {
 416                 Node* m_node;         // Node
 417                 Face* m_face;         // Face
 418                 btVector3 m_weights;  // Weigths
 419                 btVector3 m_normal;   // Normal
 420                 btScalar m_margin;    // Margin
 421                 btScalar m_friction;  // Friction
 422                 btScalar m_cfm[2];    // Constraint force mixing
 423         };
 424         /* Anchor               */
 425         struct Anchor
 426         {
 427                 Node* m_node;         // Node pointer
 428                 btVector3 m_local;    // Anchor position in body space
 429                 btRigidBody* m_body;  // Body
 430                 btScalar m_influence;
 431                 btMatrix3x3 m_c0;  // Impulse matrix
 432                 btVector3 m_c1;    // Relative anchor
 433                 btScalar m_c2;     // ima*dt
 434         };
 435         /* Note                 */
 436         struct Note : Element
 437         {
 438                 const char* m_text;    // Text
 439                 btVector3 m_offset;    // Offset
 440                 int m_rank;            // Rank
 441                 Node* m_nodes[4];      // Nodes
 442                 btScalar m_coords[4];  // Coordinates
 443         };
 444         /* Pose                 */
 445         struct Pose
 446         {
 447                 bool m_bvolume;       // Is valid
 448                 bool m_bframe;        // Is frame
 449                 btScalar m_volume;    // Rest volume
 450                 tVector3Array m_pos;  // Reference positions
 451                 tScalarArray m_wgh;   // Weights
 452                 btVector3 m_com;      // COM
 453                 btMatrix3x3 m_rot;    // Rotation
 454                 btMatrix3x3 m_scl;    // Scale
 455                 btMatrix3x3 m_aqq;    // Base scaling
 456         };
 457         /* Cluster              */
 458         struct Cluster
 459         {
 460                 tScalarArray m_masses;
 461                 btAlignedObjectArray<Node*> m_nodes;
 462                 tVector3Array m_framerefs;
 463                 btTransform m_framexform;
 464                 btScalar m_idmass;
 465                 btScalar m_imass;
 466                 btMatrix3x3 m_locii;
 467                 btMatrix3x3 m_invwi;
 468                 btVector3 m_com;
 469                 btVector3 m_vimpulses[2];
 470                 btVector3 m_dimpulses[2];
 471                 int m_nvimpulses;
 472                 int m_ndimpulses;
 473                 btVector3 m_lv;
 474                 btVector3 m_av;
 475                 btDbvtNode* m_leaf;
 476                 btScalar m_ndamping; /* Node damping            */
 477                 btScalar m_ldamping; /* Linear damping  */
 478                 btScalar m_adamping; /* Angular damping */
 479                 btScalar m_matching;
 480                 btScalar m_maxSelfCollisionImpulse;
 481                 btScalar m_selfCollisionImpulseFactor;
 482                 bool m_containsAnchor;
 483                 bool m_collide;
 484                 int m_clusterIndex;
 485                 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)
 486                 {
 487                 }
 488         };
 489         /* Impulse              */
 490         struct Impulse
 491         {
 492                 btVector3 m_velocity;
 493                 btVector3 m_drift;
 494                 int m_asVelocity : 1;
 495                 int m_asDrift : 1;
 496                 Impulse() : m_velocity(0, 0, 0), m_drift(0, 0, 0), m_asVelocity(0), m_asDrift(0) {}
 497                 Impulse operator-() const
 498                 {
 499                         Impulse i = *this;
 500                         i.m_velocity = -i.m_velocity;
 501                         i.m_drift = -i.m_drift;
 502                         return (i);
 503                 }
 504                 Impulse operator*(btScalar x) const
 505                 {
 506                         Impulse i = *this;
 507                         i.m_velocity *= x;
 508                         i.m_drift *= x;
 509                         return (i);
 510                 }
 511         };
 512         /* Body                 */
 513         struct Body
 514         {
 515                 Cluster* m_soft;
 516                 btRigidBody* m_rigid;
 517                 const btCollisionObject* m_collisionObject;
 518
 519                 Body() : m_soft(0), m_rigid(0), m_collisionObject(0) {}
 520                 Body(Cluster* p) : m_soft(p), m_rigid(0), m_collisionObject(0) {}
 521                 Body(const btCollisionObject* colObj) : m_soft(0), m_collisionObject(colObj)
 522                 {
 523                         m_rigid = (btRigidBody*)btRigidBody::upcast(m_collisionObject);
 524                 }
 525
 526                 void activate() const
 527                 {
 528                         if (m_rigid)
 529                                 m_rigid->activate();
 530                         if (m_collisionObject)
 531                                 m_collisionObject->activate();
 532                 }
 533                 const btMatrix3x3& invWorldInertia() const
 534                 {
 535                         static const btMatrix3x3 iwi(0, 0, 0, 0, 0, 0, 0, 0, 0);
 536                         if (m_rigid) return (m_rigid->getInvInertiaTensorWorld());
 537                         if (m_soft) return (m_soft->m_invwi);
 538                         return (iwi);
 539                 }
 540                 btScalar invMass() const
 541                 {
 542                         if (m_rigid) return (m_rigid->getInvMass());
 543                         if (m_soft) return (m_soft->m_imass);
 544                         return (0);
 545                 }
 546                 const btTransform& xform() const
 547                 {
 548                         static const btTransform identity = btTransform::getIdentity();
 549                         if (m_collisionObject) return (m_collisionObject->getWorldTransform());
 550                         if (m_soft) return (m_soft->m_framexform);
 551                         return (identity);
 552                 }
 553                 btVector3 linearVelocity() const
 554                 {
 555                         if (m_rigid) return (m_rigid->getLinearVelocity());
 556                         if (m_soft) return (m_soft->m_lv);
 557                         return (btVector3(0, 0, 0));
 558                 }
 559                 btVector3 angularVelocity(const btVector3& rpos) const
 560                 {
 561                         if (m_rigid) return (btCross(m_rigid->getAngularVelocity(), rpos));
 562                         if (m_soft) return (btCross(m_soft->m_av, rpos));
 563                         return (btVector3(0, 0, 0));
 564                 }
 565                 btVector3 angularVelocity() const
 566                 {
 567                         if (m_rigid) return (m_rigid->getAngularVelocity());
 568                         if (m_soft) return (m_soft->m_av);
 569                         return (btVector3(0, 0, 0));
 570                 }
 571                 btVector3 velocity(const btVector3& rpos) const
 572                 {
 573                         return (linearVelocity() + angularVelocity(rpos));
 574                 }
 575                 void applyVImpulse(const btVector3& impulse, const btVector3& rpos) const
 576                 {
 577                         if (m_rigid) m_rigid->applyImpulse(impulse, rpos);
 578                         if (m_soft) btSoftBody::clusterVImpulse(m_soft, rpos, impulse);
 579                 }
 580                 void applyDImpulse(const btVector3& impulse, const btVector3& rpos) const
 581                 {
 582                         if (m_rigid) m_rigid->applyImpulse(impulse, rpos);
 583                         if (m_soft) btSoftBody::clusterDImpulse(m_soft, rpos, impulse);
 584                 }
 585                 void applyImpulse(const Impulse& impulse, const btVector3& rpos) const
 586                 {
 587                         if (impulse.m_asVelocity)
 588                         {
 589                                 //                              printf("impulse.m_velocity = %f,%f,%f\n",impulse.m_velocity.getX(),impulse.m_velocity.getY(),impulse.m_velocity.getZ());
 590                                 applyVImpulse(impulse.m_velocity, rpos);
 591                         }
 592                         if (impulse.m_asDrift)
 593                         {
 594                                 //                              printf("impulse.m_drift = %f,%f,%f\n",impulse.m_drift.getX(),impulse.m_drift.getY(),impulse.m_drift.getZ());
 595                                 applyDImpulse(impulse.m_drift, rpos);
 596                         }
 597                 }
 598                 void applyVAImpulse(const btVector3& impulse) const
 599                 {
 600                         if (m_rigid) m_rigid->applyTorqueImpulse(impulse);
 601                         if (m_soft) btSoftBody::clusterVAImpulse(m_soft, impulse);
 602                 }
 603                 void applyDAImpulse(const btVector3& impulse) const
 604                 {
 605                         if (m_rigid) m_rigid->applyTorqueImpulse(impulse);
 606                         if (m_soft) btSoftBody::clusterDAImpulse(m_soft, impulse);
 607                 }
 608                 void applyAImpulse(const Impulse& impulse) const
 609                 {
 610                         if (impulse.m_asVelocity) applyVAImpulse(impulse.m_velocity);
 611                         if (impulse.m_asDrift) applyDAImpulse(impulse.m_drift);
 612                 }
 613                 void applyDCImpulse(const btVector3& impulse) const
 614                 {
 615                         if (m_rigid) m_rigid->applyCentralImpulse(impulse);
 616                         if (m_soft) btSoftBody::clusterDCImpulse(m_soft, impulse);
 617                 }
 618         };
 619         /* Joint                */
 620         struct Joint
 621         {
 622                 struct eType
 623                 {
 624                         enum _
 625                         {
 626                                 Linear = 0,
 627                                 Angular,
 628                                 Contact
 629                         };
 630                 };
 631                 struct Specs
 632                 {
 633                         Specs() : erp(1), cfm(1), split(1) {}
 634                         btScalar erp;
 635                         btScalar cfm;
 636                         btScalar split;
 637                 };
 638                 Body m_bodies[2];
 639                 btVector3 m_refs[2];
 640                 btScalar m_cfm;
 641                 btScalar m_erp;
 642                 btScalar m_split;
 643                 btVector3 m_drift;
 644                 btVector3 m_sdrift;
 645                 btMatrix3x3 m_massmatrix;
 646                 bool m_delete;
 647                 virtual ~Joint() {}
 648                 Joint() : m_delete(false) {}
 649                 virtual void Prepare(btScalar dt, int iterations);
 650                 virtual void Solve(btScalar dt, btScalar sor) = 0;
 651                 virtual void Terminate(btScalar dt) = 0;
 652                 virtual eType::_ Type() const = 0;
 653         };
 654         /* LJoint               */
 655         struct LJoint : Joint
 656         {
 657                 struct Specs : Joint::Specs
 658                 {
 659                         btVector3 position;
 660                 };
 661                 btVector3 m_rpos[2];
 662                 void Prepare(btScalar dt, int iterations);
 663                 void Solve(btScalar dt, btScalar sor);
 664                 void Terminate(btScalar dt);
 665                 eType::_ Type() const { return (eType::Linear); }
 666         };
 667         /* AJoint               */
 668         struct AJoint : Joint
 669         {
 670                 struct IControl
 671                 {
 672                         virtual ~IControl() {}
 673                         virtual void Prepare(AJoint*) {}
 674                         virtual btScalar Speed(AJoint*, btScalar current) { return (current); }
 675                         static IControl* Default()
 676                         {
 677                                 static IControl def;
 678                                 return (&def);
 679                         }
 680                 };
 681                 struct Specs : Joint::Specs
 682                 {
 683                         Specs() : icontrol(IControl::Default()) {}
 684                         btVector3 axis;
 685                         IControl* icontrol;
 686                 };
 687                 btVector3 m_axis[2];
 688                 IControl* m_icontrol;
 689                 void Prepare(btScalar dt, int iterations);
 690                 void Solve(btScalar dt, btScalar sor);
 691                 void Terminate(btScalar dt);
 692                 eType::_ Type() const { return (eType::Angular); }
 693         };
 694         /* CJoint               */
 695         struct CJoint : Joint
 696         {
 697                 int m_life;
 698                 int m_maxlife;
 699                 btVector3 m_rpos[2];
 700                 btVector3 m_normal;
 701                 btScalar m_friction;
 702                 void Prepare(btScalar dt, int iterations);
 703                 void Solve(btScalar dt, btScalar sor);
 704                 void Terminate(btScalar dt);
 705                 eType::_ Type() const { return (eType::Contact); }
 706         };
 707         /* Config               */
 708         struct Config
 709         {
 710                 eAeroModel::_ aeromodel;    // Aerodynamic model (default: V_Point)
 711                 btScalar kVCF;              // Velocities correction factor (Baumgarte)
 712                 btScalar kDP;               // Damping coefficient [0,1]
 713                 btScalar kDG;               // Drag coefficient [0,+inf]
 714                 btScalar kLF;               // Lift coefficient [0,+inf]
 715                 btScalar kPR;               // Pressure coefficient [-inf,+inf]
 716                 btScalar kVC;               // Volume conversation coefficient [0,+inf]
 717                 btScalar kDF;               // Dynamic friction coefficient [0,1]
 718                 btScalar kMT;               // Pose matching coefficient [0,1]
 719                 btScalar kCHR;              // Rigid contacts hardness [0,1]
 720                 btScalar kKHR;              // Kinetic contacts hardness [0,1]
 721                 btScalar kSHR;              // Soft contacts hardness [0,1]
 722                 btScalar kAHR;              // Anchors hardness [0,1]
 723                 btScalar kSRHR_CL;          // Soft vs rigid hardness [0,1] (cluster only)
 724                 btScalar kSKHR_CL;          // Soft vs kinetic hardness [0,1] (cluster only)
 725                 btScalar kSSHR_CL;          // Soft vs soft hardness [0,1] (cluster only)
 726                 btScalar kSR_SPLT_CL;       // Soft vs rigid impulse split [0,1] (cluster only)
 727                 btScalar kSK_SPLT_CL;       // Soft vs rigid impulse split [0,1] (cluster only)
 728                 btScalar kSS_SPLT_CL;       // Soft vs rigid impulse split [0,1] (cluster only)
 729                 btScalar maxvolume;         // Maximum volume ratio for pose
 730                 btScalar timescale;         // Time scale
 731                 int viterations;            // Velocities solver iterations
 732                 int piterations;            // Positions solver iterations
 733                 int diterations;            // Drift solver iterations
 734                 int citerations;            // Cluster solver iterations
 735                 int collisions;             // Collisions flags
 736                 tVSolverArray m_vsequence;  // Velocity solvers sequence
 737                 tPSolverArray m_psequence;  // Position solvers sequence
 738                 tPSolverArray m_dsequence;  // Drift solvers sequence
 739                 btScalar drag;              // deformable air drag
 740                 btScalar m_maxStress;       // Maximum principle first Piola stress
 741         };
 742         /* SolverState  */
 743         struct SolverState
 744         {
 745                 //if you add new variables, always initialize them!
 746                 SolverState()
 747                         : sdt(0),
 748                           isdt(0),
 749                           velmrg(0),
 750                           radmrg(0),
 751                           updmrg(0)
 752                 {
 753                 }
 754                 btScalar sdt;     // dt*timescale
 755                 btScalar isdt;    // 1/sdt
 756                 btScalar velmrg;  // velocity margin
 757                 btScalar radmrg;  // radial margin
 758                 btScalar updmrg;  // Update margin
 759         };
 760         /// RayFromToCaster takes a ray from, ray to (instead of direction!)
 761         struct RayFromToCaster : btDbvt::ICollide
 762         {
 763                 btVector3 m_rayFrom;
 764                 btVector3 m_rayTo;
 765                 btVector3 m_rayNormalizedDirection;
 766                 btScalar m_mint;
 767                 Face* m_face;
 768                 int m_tests;
 769                 RayFromToCaster(const btVector3& rayFrom, const btVector3& rayTo, btScalar mxt);
 770                 void Process(const btDbvtNode* leaf);
 771
 772                 static /*inline*/ btScalar rayFromToTriangle(const btVector3& rayFrom,
 773                                                                                                          const btVector3& rayTo,
 774                                                                                                          const btVector3& rayNormalizedDirection,
 775                                                                                                          const btVector3& a,
 776                                                                                                          const btVector3& b,
 777                                                                                                          const btVector3& c,
 778                                                                                                          btScalar maxt = SIMD_INFINITY);
 779         };
 780
 781         //
 782         // Typedefs
 783         //
 784
 785         typedef void (*psolver_t)(btSoftBody*, btScalar, btScalar);
 786         typedef void (*vsolver_t)(btSoftBody*, btScalar);
 787         typedef btAlignedObjectArray<Cluster*> tClusterArray;
 788         typedef btAlignedObjectArray<Note> tNoteArray;
 789         typedef btAlignedObjectArray<Node> tNodeArray;
 790         typedef btAlignedObjectArray<RenderNode> tRenderNodeArray;
 791         typedef btAlignedObjectArray<btDbvtNode*> tLeafArray;
 792         typedef btAlignedObjectArray<Link> tLinkArray;
 793         typedef btAlignedObjectArray<Face> tFaceArray;
 794         typedef btAlignedObjectArray<RenderFace> tRenderFaceArray;
 795         typedef btAlignedObjectArray<Tetra> tTetraArray;
 796         typedef btAlignedObjectArray<Anchor> tAnchorArray;
 797         typedef btAlignedObjectArray<RContact> tRContactArray;
 798         typedef btAlignedObjectArray<SContact> tSContactArray;
 799         typedef btAlignedObjectArray<Material*> tMaterialArray;
 800         typedef btAlignedObjectArray<Joint*> tJointArray;
 801         typedef btAlignedObjectArray<btSoftBody*> tSoftBodyArray;
 802         typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > tDenseMatrix;
 803
 804         //
 805         // Fields
 806         //
 807
 808         Config m_cfg;                      // Configuration
 809         SolverState m_sst;                 // Solver state
 810         Pose m_pose;                       // Pose
 811         void* m_tag;                       // User data
 812         btSoftBodyWorldInfo* m_worldInfo;  // World info
 813         tNoteArray m_notes;                // Notes
 814         tNodeArray m_nodes;                // Nodes
 815         tRenderNodeArray m_renderNodes;    // Render Nodes
 816         tLinkArray m_links;                // Links
 817         tFaceArray m_faces;                // Faces
 818         tRenderFaceArray m_renderFaces;    // Faces
 819         tTetraArray m_tetras;              // Tetras
 820         btAlignedObjectArray<TetraScratch> m_tetraScratches;
 821         btAlignedObjectArray<TetraScratch> m_tetraScratchesTn;
 822         tAnchorArray m_anchors;  // Anchors
 823         btAlignedObjectArray<DeformableNodeRigidAnchor> m_deformableAnchors;
 824         tRContactArray m_rcontacts;  // Rigid contacts
 825         btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts;
 826         btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts;
 827         btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts;
 828         btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContactsCCD;
 829         tSContactArray m_scontacts;     // Soft contacts
 830         tJointArray m_joints;           // Joints
 831         tMaterialArray m_materials;     // Materials
 832         btScalar m_timeacc;             // Time accumulator
 833         btVector3 m_bounds[2];          // Spatial bounds
 834         bool m_bUpdateRtCst;            // Update runtime constants
 835         btDbvt m_ndbvt;                 // Nodes tree
 836         btDbvt m_fdbvt;                 // Faces tree
 837         btDbvntNode* m_fdbvnt;          // Faces tree with normals
 838         btDbvt m_cdbvt;                 // Clusters tree
 839         tClusterArray m_clusters;       // Clusters
 840         btScalar m_dampingCoefficient;  // Damping Coefficient
 841         btScalar m_sleepingThreshold;
 842         btScalar m_maxSpeedSquared;
 843         btAlignedObjectArray<btVector3> m_quads;  // quadrature points for collision detection
 844         btScalar m_repulsionStiffness;
 845         btScalar m_gravityFactor;
 846         bool m_cacheBarycenter;
 847         btAlignedObjectArray<btVector3> m_X;  // initial positions
 848
 849         btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights;
 850         btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents;
 851         btAlignedObjectArray<btScalar> m_z;  // vertical distance used in extrapolation
 852         bool m_useSelfCollision;
 853         bool m_softSoftCollision;
 854
 855         btAlignedObjectArray<bool> m_clusterConnectivity;  //cluster connectivity, for self-collision
 856
 857         btVector3 m_windVelocity;
 858
 859         btScalar m_restLengthScale;
 860
 861         bool m_reducedModel;    // Reduced deformable model flag
 862
 863         //
 864         // Api
 865         //
 866
 867         /* ctor                                                                                                                                 */
 868         btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m);
 869
 870         /* ctor                                                                                                                                 */
 871         btSoftBody(btSoftBodyWorldInfo* worldInfo);
 872
 873         void initDefaults();
 874
 875         /* dtor                                                                                                                                 */
 876         virtual ~btSoftBody();
 877         /* Check for existing link                                                                                              */
 878
 879         btAlignedObjectArray<int> m_userIndexMapping;
 880
 881         btSoftBodyWorldInfo* getWorldInfo()
 882         {
 883                 return m_worldInfo;
 884         }
 885
 886         void setDampingCoefficient(btScalar damping_coeff)
 887         {
 888                 m_dampingCoefficient = damping_coeff;
 889         }
 890
 891         ///@todo: avoid internal softbody shape hack and move collision code to collision library
 892         virtual void setCollisionShape(btCollisionShape* collisionShape)
 893         {
 894         }
 895
 896         bool checkLink(int node0,
 897                                    int node1) const;
 898         bool checkLink(const Node* node0,
 899                                    const Node* node1) const;
 900         /* Check for existring face                                                                                             */
 901         bool checkFace(int node0,
 902                                    int node1,
 903                                    int node2) const;
 904         /* Append material                                                                                                              */
 905         Material* appendMaterial();
 906         /* Append note                                                                                                                  */
 907         void appendNote(const char* text,
 908                                         const btVector3& o,
 909                                         const btVector4& c = btVector4(1, 0, 0, 0),
 910                                         Node* n0 = 0,
 911                                         Node* n1 = 0,
 912                                         Node* n2 = 0,
 913                                         Node* n3 = 0);
 914         void appendNote(const char* text,
 915                                         const btVector3& o,
 916                                         Node* feature);
 917         void appendNote(const char* text,
 918                                         const btVector3& o,
 919                                         Link* feature);
 920         void appendNote(const char* text,
 921                                         const btVector3& o,
 922                                         Face* feature);
 923         /* Append node                                                                                                                  */
 924         void appendNode(const btVector3& x, btScalar m);
 925         /* Append link                                                                                                                  */
 926         void appendLink(int model = -1, Material* mat = 0);
 927         void appendLink(int node0,
 928                                         int node1,
 929                                         Material* mat = 0,
 930                                         bool bcheckexist = false);
 931         void appendLink(Node* node0,
 932                                         Node* node1,
 933                                         Material* mat = 0,
 934                                         bool bcheckexist = false);
 935         /* Append face                                                                                                                  */
 936         void appendFace(int model = -1, Material* mat = 0);
 937         void appendFace(int node0,
 938                                         int node1,
 939                                         int node2,
 940                                         Material* mat = 0);
 941         void appendTetra(int model, Material* mat);
 942         //
 943         void appendTetra(int node0,
 944                                          int node1,
 945                                          int node2,
 946                                          int node3,
 947                                          Material* mat = 0);
 948
 949         /* Append anchor                                                                                                                */
 950         void appendDeformableAnchor(int node, btRigidBody* body);
 951         void appendDeformableAnchor(int node, btMultiBodyLinkCollider* link);
 952         void appendAnchor(int node,
 953                                           btRigidBody* body, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1);
 954         void appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1);
 955         void removeAnchor(int node);
 956         /* Append linear joint                                                                                                  */
 957         void appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1);
 958         void appendLinearJoint(const LJoint::Specs& specs, Body body = Body());
 959         void appendLinearJoint(const LJoint::Specs& specs, btSoftBody* body);
 960         /* Append linear joint                                                                                                  */
 961         void appendAngularJoint(const AJoint::Specs& specs, Cluster* body0, Body body1);
 962         void appendAngularJoint(const AJoint::Specs& specs, Body body = Body());
 963         void appendAngularJoint(const AJoint::Specs& specs, btSoftBody* body);
 964         /* Add force (or gravity) to the entire body                                                    */
 965         void addForce(const btVector3& force);
 966         /* Add force (or gravity) to a node of the body                                                 */
 967         void addForce(const btVector3& force,
 968                                   int node);
 969         /* Add aero force to a node of the body */
 970         void addAeroForceToNode(const btVector3& windVelocity, int nodeIndex);
 971
 972         /* Add aero force to a face of the body */
 973         void addAeroForceToFace(const btVector3& windVelocity, int faceIndex);
 974
 975         /* Add velocity to the entire body                                                                              */
 976         void addVelocity(const btVector3& velocity);
 977
 978         /* Set velocity for the entire body                                                                             */
 979         void setVelocity(const btVector3& velocity);
 980
 981         /* Add velocity to a node of the body                                                                   */
 982         void addVelocity(const btVector3& velocity,
 983                                          int node);
 984         /* Set mass                                                                                                                             */
 985         void setMass(int node,
 986                                  btScalar mass);
 987         /* Get mass                                                                                                                             */
 988         btScalar getMass(int node) const;
 989         /* Get total mass                                                                                                               */
 990         btScalar getTotalMass() const;
 991         /* Set total mass (weighted by previous masses)                                                 */
 992         void setTotalMass(btScalar mass,
 993                                           bool fromfaces = false);
 994         /* Set total density                                                                                                    */
 995         void setTotalDensity(btScalar density);
 996         /* Set volume mass (using tetrahedrons)                                                                 */
 997         void setVolumeMass(btScalar mass);
 998         /* Set volume density (using tetrahedrons)                                                              */
 999         void setVolumeDensity(btScalar density);
1000         /* Get the linear velocity of the center of mass                        */
1001         btVector3 getLinearVelocity();
1002         /* Set the linear velocity of the center of mass                        */
1003         void setLinearVelocity(const btVector3& linVel);
1004         /* Set the angular velocity of the center of mass                       */
1005         void setAngularVelocity(const btVector3& angVel);
1006         /* Get best fit rigid transform                                         */
1007         btTransform getRigidTransform();
1008         /* Transform to given pose                                              */
1009         virtual void transformTo(const btTransform& trs);
1010         /* Transform                                                                                                                    */
1011         virtual void transform(const btTransform& trs);
1012         /* Translate                                                                                                                    */
1013         virtual void translate(const btVector3& trs);
1014         /* Rotate                                                                                                                       */
1015         virtual void rotate(const btQuaternion& rot);
1016         /* Scale                                                                                                                                */
1017         virtual void scale(const btVector3& scl);
1018         /* Get link resting lengths scale                                                                               */
1019         btScalar getRestLengthScale();
1020         /* Scale resting length of all springs                                                                  */
1021         void setRestLengthScale(btScalar restLength);
1022         /* Set current state as pose                                                                                    */
1023         void setPose(bool bvolume,
1024                                  bool bframe);
1025         /* Set current link lengths as resting lengths                                                  */
1026         void resetLinkRestLengths();
1027         /* Return the volume                                                                                                    */
1028         btScalar getVolume() const;
1029         /* Cluster count                                                                                                                */
1030         btVector3 getCenterOfMass() const
1031         {
1032                 btVector3 com(0, 0, 0);
1033                 for (int i = 0; i < m_nodes.size(); i++)
1034                 {
1035                         com += (m_nodes[i].m_x * this->getMass(i));
1036                 }
1037                 com /= this->getTotalMass();
1038                 return com;
1039         }
1040         int clusterCount() const;
1041         /* Cluster center of mass                                                                                               */
1042         static btVector3 clusterCom(const Cluster* cluster);
1043         btVector3 clusterCom(int cluster) const;
1044         /* Cluster velocity at rpos                                                                                             */
1045         static btVector3 clusterVelocity(const Cluster* cluster, const btVector3& rpos);
1046         /* Cluster impulse                                                                                                              */
1047         static void clusterVImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse);
1048         static void clusterDImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse);
1049         static void clusterImpulse(Cluster* cluster, const btVector3& rpos, const Impulse& impulse);
1050         static void clusterVAImpulse(Cluster* cluster, const btVector3& impulse);
1051         static void clusterDAImpulse(Cluster* cluster, const btVector3& impulse);
1052         static void clusterAImpulse(Cluster* cluster, const Impulse& impulse);
1053         static void clusterDCImpulse(Cluster* cluster, const btVector3& impulse);
1054         /* Generate bending constraints based on distance in the adjency graph  */
1055         int generateBendingConstraints(int distance,
1056                                                                    Material* mat = 0);
1057         /* Randomize constraints to reduce solver bias                                                  */
1058         void randomizeConstraints();
1059
1060         void updateState(const btAlignedObjectArray<btVector3>& qs, const btAlignedObjectArray<btVector3>& vs);
1061
1062         /* Release clusters                                                                                                             */
1063         void releaseCluster(int index);
1064         void releaseClusters();
1065         /* Generate clusters (K-mean)                                                                                   */
1066         ///generateClusters with k=0 will create a convex cluster for each tetrahedron or triangle
1067         ///otherwise an approximation will be used (better performance)
1068         int generateClusters(int k, int maxiterations = 8192);
1069         /* Refine                                                                                                                               */
1070         void refine(ImplicitFn* ifn, btScalar accurary, bool cut);
1071         /* CutLink                                                                                                                              */
1072         bool cutLink(int node0, int node1, btScalar position);
1073         bool cutLink(const Node* node0, const Node* node1, btScalar position);
1074
1075         ///Ray casting using rayFrom and rayTo in worldspace, (not direction!)
1076         bool rayTest(const btVector3& rayFrom,
1077                                  const btVector3& rayTo,
1078                                  sRayCast& results);
1079         bool rayFaceTest(const btVector3& rayFrom,
1080                                          const btVector3& rayTo,
1081                                          sRayCast& results);
1082         int rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo,
1083                                         btScalar& mint, int& index) const;
1084         /* Solver presets                                                                                                               */
1085         void setSolver(eSolverPresets::_ preset);
1086         /* predictMotion                                                                                                                */
1087         void predictMotion(btScalar dt);
1088         /* solveConstraints                                                                                                             */
1089         void solveConstraints();
1090         /* staticSolve                                                                                                                  */
1091         void staticSolve(int iterations);
1092         /* solveCommonConstraints                                                                                               */
1093         static void solveCommonConstraints(btSoftBody** bodies, int count, int iterations);
1094         /* solveClusters                                                                                                                */
1095         static void solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies);
1096         /* integrateMotion                                                                                                              */
1097         void integrateMotion();
1098         /* defaultCollisionHandlers                                                                                             */
1099         void defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap);
1100         void defaultCollisionHandler(btSoftBody* psb);
1101         void setSelfCollision(bool useSelfCollision);
1102         bool useSelfCollision();
1103         void updateDeactivation(btScalar timeStep);
1104         void setZeroVelocity();
1105         bool wantsSleeping();
1106
1107         virtual btMatrix3x3 getImpulseFactor(int n_node)
1108         {
1109                 btMatrix3x3 tmp;
1110                 tmp.setIdentity();
1111                 return tmp;
1112         }
1113
1114         //
1115         // Functionality to deal with new accelerated solvers.
1116         //
1117
1118         /**
1119          * Set a wind velocity for interaction with the air.
1120          */
1121         void setWindVelocity(const btVector3& velocity);
1122
1123         /**
1124          * Return the wind velocity for interaction with the air.
1125          */
1126         const btVector3& getWindVelocity();
1127
1128         //
1129         // Set the solver that handles this soft body
1130         // Should not be allowed to get out of sync with reality
1131         // Currently called internally on addition to the world
1132         void setSoftBodySolver(btSoftBodySolver* softBodySolver)
1133         {
1134                 m_softBodySolver = softBodySolver;
1135         }
1136
1137         //
1138         // Return the solver that handles this soft body
1139         //
1140         btSoftBodySolver* getSoftBodySolver()
1141         {
1142                 return m_softBodySolver;
1143         }
1144
1145         //
1146         // Return the solver that handles this soft body
1147         //
1148         btSoftBodySolver* getSoftBodySolver() const
1149         {
1150                 return m_softBodySolver;
1151         }
1152
1153         //
1154         // Cast
1155         //
1156
1157         static const btSoftBody* upcast(const btCollisionObject* colObj)
1158         {
1159                 if (colObj->getInternalType() == CO_SOFT_BODY)
1160                         return (const btSoftBody*)colObj;
1161                 return 0;
1162         }
1163         static btSoftBody* upcast(btCollisionObject* colObj)
1164         {
1165                 if (colObj->getInternalType() == CO_SOFT_BODY)
1166                         return (btSoftBody*)colObj;
1167                 return 0;
1168         }
1169
1170         //
1171         // ::btCollisionObject
1172         //
1173
1174         virtual void getAabb(btVector3& aabbMin, btVector3& aabbMax) const
1175         {
1176                 aabbMin = m_bounds[0];
1177                 aabbMax = m_bounds[1];
1178         }
1179         //
1180         // Private
1181         //
1182         void pointersToIndices();
1183         void indicesToPointers(const int* map = 0);
1184
1185         int rayTest(const btVector3& rayFrom, const btVector3& rayTo,
1186                                 btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const;
1187         void initializeFaceTree();
1188         void rebuildNodeTree();
1189         btVector3 evaluateCom() const;
1190         bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
1191         bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, Face& f, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
1192         bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const;
1193         void updateNormals();
1194         void updateBounds();
1195         void updatePose();
1196         void updateConstants();
1197         void updateLinkConstants();
1198         void updateArea(bool averageArea = true);
1199         void initializeClusters();
1200         void updateClusters();
1201         void cleanupClusters();
1202         void prepareClusters(int iterations);
1203         void solveClusters(btScalar sor);
1204         void applyClusters(bool drift);
1205         void dampClusters();
1206         void setSpringStiffness(btScalar k);
1207         void setGravityFactor(btScalar gravFactor);
1208         void setCacheBarycenter(bool cacheBarycenter);
1209         void initializeDmInverse();
1210         void updateDeformation();
1211         void advanceDeformation();
1212         void applyForces();
1213         void setMaxStress(btScalar maxStress);
1214         void interpolateRenderMesh();
1215         void setCollisionQuadrature(int N);
1216         static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti);
1217         static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti);
1218         static void PSolve_SContacts(btSoftBody* psb, btScalar, btScalar ti);
1219         static void PSolve_Links(btSoftBody* psb, btScalar kst, btScalar ti);
1220         static void VSolve_Links(btSoftBody* psb, btScalar kst);
1221         static psolver_t getSolver(ePSolver::_ solver);
1222         static vsolver_t getSolver(eVSolver::_ solver);
1223         void geometricCollisionHandler(btSoftBody* psb);
1224 #define SAFE_EPSILON SIMD_EPSILON * 100.0
1225         void updateNode(btDbvtNode* node, bool use_velocity, bool margin)
1226         {
1227                 if (node->isleaf())
1228                 {
1229                         btSoftBody::Node* n = (btSoftBody::Node*)(node->data);
1230                         ATTRIBUTE_ALIGNED16(btDbvtVolume)
1231                         vol;
1232                         btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON;  // use user defined margin or margin for floating point precision
1233                         if (use_velocity)
1234                         {
1235                                 btVector3 points[2] = {n->m_x, n->m_x + m_sst.sdt * n->m_v};
1236                                 vol = btDbvtVolume::FromPoints(points, 2);
1237                                 vol.Expand(btVector3(pad, pad, pad));
1238                         }
1239                         else
1240                         {
1241                                 vol = btDbvtVolume::FromCR(n->m_x, pad);
1242                         }
1243                         node->volume = vol;
1244                         return;
1245                 }
1246                 else
1247                 {
1248                         updateNode(node->childs[0], use_velocity, margin);
1249                         updateNode(node->childs[1], use_velocity, margin);
1250                         ATTRIBUTE_ALIGNED16(btDbvtVolume)
1251                         vol;
1252                         Merge(node->childs[0]->volume, node->childs[1]->volume, vol);
1253                         node->volume = vol;
1254                 }
1255         }
1256
1257         void updateNodeTree(bool use_velocity, bool margin)
1258         {
1259                 if (m_ndbvt.m_root)
1260                         updateNode(m_ndbvt.m_root, use_velocity, margin);
1261         }
1262
1263         template <class DBVTNODE>  // btDbvtNode or btDbvntNode
1264         void updateFace(DBVTNODE* node, bool use_velocity, bool margin)
1265         {
1266                 if (node->isleaf())
1267                 {
1268                         btSoftBody::Face* f = (btSoftBody::Face*)(node->data);
1269                         btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON;  // use user defined margin or margin for floating point precision
1270                         ATTRIBUTE_ALIGNED16(btDbvtVolume)
1271                         vol;
1272                         if (use_velocity)
1273                         {
1274                                 btVector3 points[6] = {f->m_n[0]->m_x, f->m_n[0]->m_x + m_sst.sdt * f->m_n[0]->m_v,
1275                                                                            f->m_n[1]->m_x, f->m_n[1]->m_x + m_sst.sdt * f->m_n[1]->m_v,
1276                                                                            f->m_n[2]->m_x, f->m_n[2]->m_x + m_sst.sdt * f->m_n[2]->m_v};
1277                                 vol = btDbvtVolume::FromPoints(points, 6);
1278                         }
1279                         else
1280                         {
1281                                 btVector3 points[3] = {f->m_n[0]->m_x,
1282                                                                            f->m_n[1]->m_x,
1283                                                                            f->m_n[2]->m_x};
1284                                 vol = btDbvtVolume::FromPoints(points, 3);
1285                         }
1286                         vol.Expand(btVector3(pad, pad, pad));
1287                         node->volume = vol;
1288                         return;
1289                 }
1290                 else
1291                 {
1292                         updateFace(node->childs[0], use_velocity, margin);
1293                         updateFace(node->childs[1], use_velocity, margin);
1294                         ATTRIBUTE_ALIGNED16(btDbvtVolume)
1295                         vol;
1296                         Merge(node->childs[0]->volume, node->childs[1]->volume, vol);
1297                         node->volume = vol;
1298                 }
1299         }
1300         void updateFaceTree(bool use_velocity, bool margin)
1301         {
1302                 if (m_fdbvt.m_root)
1303                         updateFace(m_fdbvt.m_root, use_velocity, margin);
1304                 if (m_fdbvnt)
1305                         updateFace(m_fdbvnt, use_velocity, margin);
1306         }
1307
1308         template <typename T>
1309         static inline T BaryEval(const T& a,
1310                                                          const T& b,
1311                                                          const T& c,
1312                                                          const btVector3& coord)
1313         {
1314                 return (a * coord.x() + b * coord.y() + c * coord.z());
1315         }
1316
1317         void applyRepulsionForce(btScalar timeStep, bool applySpringForce)
1318         {
1319                 btAlignedObjectArray<int> indices;
1320                 {
1321                         // randomize the order of repulsive force
1322                         indices.resize(m_faceNodeContacts.size());
1323                         for (int i = 0; i < m_faceNodeContacts.size(); ++i)
1324                                 indices[i] = i;
1325 #define NEXTRAND (seed = (1664525L * seed + 1013904223L) & 0xffffffff)
1326                         int i, ni;
1327
1328                         for (i = 0, ni = indices.size(); i < ni; ++i)
1329                         {
1330                                 btSwap(indices[i], indices[NEXTRAND % ni]);
1331                         }
1332                 }
1333                 for (int k = 0; k < m_faceNodeContacts.size(); ++k)
1334                 {
1335                         int idx = indices[k];
1336                         btSoftBody::DeformableFaceNodeContact& c = m_faceNodeContacts[idx];
1337                         btSoftBody::Node* node = c.m_node;
1338                         btSoftBody::Face* face = c.m_face;
1339                         const btVector3& w = c.m_bary;
1340                         const btVector3& n = c.m_normal;
1341                         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);
1342                         btScalar d = c.m_margin - n.dot(l);
1343                         d = btMax(btScalar(0), d);
1344
1345                         const btVector3& va = node->m_v;
1346                         btVector3 vb = BaryEval(face->m_n[0]->m_v, face->m_n[1]->m_v, face->m_n[2]->m_v, w);
1347                         btVector3 vr = va - vb;
1348                         const btScalar vn = btDot(vr, n);  // dn < 0 <==> opposing
1349                         if (vn > OVERLAP_REDUCTION_FACTOR * d / timeStep)
1350                                 continue;
1351                         btVector3 vt = vr - vn * n;
1352                         btScalar I = 0;
1353                         btScalar mass = node->m_im == 0 ? 0 : btScalar(1) / node->m_im;
1354                         if (applySpringForce)
1355                                 I = -btMin(m_repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn));
1356                         if (vn < 0)
1357                                 I += 0.5 * mass * vn;
1358                         int face_penetration = 0, node_penetration = node->m_constrained;
1359                         for (int i = 0; i < 3; ++i)
1360                                 face_penetration |= face->m_n[i]->m_constrained;
1361                         btScalar I_tilde = 2.0 * I / (1.0 + w.length2());
1362
1363                         //             double the impulse if node or face is constrained.
1364                         if (face_penetration > 0 || node_penetration > 0)
1365                         {
1366                                 I_tilde *= 2.0;
1367                         }
1368                         if (face_penetration <= 0)
1369                         {
1370                                 for (int j = 0; j < 3; ++j)
1371                                         face->m_n[j]->m_v += w[j] * n * I_tilde * node->m_im;
1372                         }
1373                         if (node_penetration <= 0)
1374                         {
1375                                 node->m_v -= I_tilde * node->m_im * n;
1376                         }
1377
1378                         // apply frictional impulse
1379                         btScalar vt_norm = vt.safeNorm();
1380                         if (vt_norm > SIMD_EPSILON)
1381                         {
1382                                 btScalar delta_vn = -2 * I * node->m_im;
1383                                 btScalar mu = c.m_friction;
1384                                 btScalar vt_new = btMax(btScalar(1) - mu * delta_vn / (vt_norm + SIMD_EPSILON), btScalar(0)) * vt_norm;
1385                                 I = 0.5 * mass * (vt_norm - vt_new);
1386                                 vt.safeNormalize();
1387                                 I_tilde = 2.0 * I / (1.0 + w.length2());
1388                                 //                 double the impulse if node or face is constrained.
1389                                 if (face_penetration > 0 || node_penetration > 0)
1390                                         I_tilde *= 2.0;
1391                                 if (face_penetration <= 0)
1392                                 {
1393                                         for (int j = 0; j < 3; ++j)
1394                                                 face->m_n[j]->m_v += w[j] * vt * I_tilde * (face->m_n[j])->m_im;
1395                                 }
1396                                 if (node_penetration <= 0)
1397                                 {
1398                                         node->m_v -= I_tilde * node->m_im * vt;
1399                                 }
1400                         }
1401                 }
1402         }
1403         virtual int calculateSerializeBufferSize() const;
1404
1405         ///fills the dataBuffer and returns the struct name (and 0 on failure)
1406         virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
1407 };
1408
1409 #endif  //_BT_SOFT_BODY_H