dali-physics/third-party/bullet3/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h

   1 #ifndef BT_REDUCED_SOFT_BODY_H
   2 #define BT_REDUCED_SOFT_BODY_H
   3
   4 #include "../btSoftBody.h"
   5 #include "LinearMath/btAlignedObjectArray.h"
   6 #include "LinearMath/btVector3.h"
   7 #include "LinearMath/btMatrix3x3.h"
   8 #include "LinearMath/btTransform.h"
   9
  10 // Reduced deformable body is a simplified deformable object embedded in a rigid frame.
  11 class btReducedDeformableBody : public btSoftBody
  12 {
  13  public:
  14   //
  15   //  Typedefs
  16   //
  17   typedef btAlignedObjectArray<btVector3> TVStack;
  18   // typedef btAlignedObjectArray<btMatrix3x3> tBlockDiagMatrix;
  19   typedef btAlignedObjectArray<btScalar> tDenseArray;
  20   typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > tDenseMatrix;
  21
  22  private:
  23   // flag to turn off the reduced modes
  24   bool m_rigidOnly;
  25
  26   // Flags for transform. Once transform is applied, users cannot scale the mesh or change its total mass.
  27   bool m_transform_lock;
  28
  29   // scaling factors
  30   btScalar m_rhoScale;         // mass density scale
  31   btScalar m_ksScale;          // stiffness scale
  32
  33   // projection matrix
  34   tDenseMatrix m_projPA;        // Eqn. 4.11 from Rahul Sheth's thesis
  35   tDenseMatrix m_projCq;
  36   tDenseArray m_STP;
  37   tDenseArray m_MrInvSTP;
  38
  39   TVStack m_localMomentArm; // Sq + x0
  40
  41   btVector3 m_internalDeltaLinearVelocity;
  42   btVector3 m_internalDeltaAngularVelocity;
  43   tDenseArray m_internalDeltaReducedVelocity;
  44
  45   btVector3 m_linearVelocityFromReduced;  // contribution to the linear velocity from reduced velocity
  46   btVector3 m_angularVelocityFromReduced; // contribution to the angular velocity from reduced velocity
  47   btVector3 m_internalDeltaAngularVelocityFromReduced;
  48
  49  protected:
  50   // rigid frame
  51   btScalar m_mass;          // total mass of the rigid frame
  52   btScalar m_inverseMass;   // inverse of the total mass of the rigid frame
  53   btVector3 m_linearVelocity;
  54   btVector3 m_angularVelocity;
  55   btScalar m_linearDamping;    // linear damping coefficient
  56   btScalar m_angularDamping;    // angular damping coefficient
  57   btVector3 m_linearFactor;
  58   btVector3 m_angularFactor;
  59   // btVector3 m_invInertiaLocal;
  60   btMatrix3x3 m_invInertiaLocal;
  61   btTransform m_rigidTransformWorld;
  62   btMatrix3x3 m_invInertiaTensorWorldInitial;
  63   btMatrix3x3 m_invInertiaTensorWorld;
  64   btMatrix3x3 m_interpolateInvInertiaTensorWorld;
  65   btVector3 m_initialCoM;  // initial center of mass (original of the m_rigidTransformWorld)
  66
  67   // damping
  68   btScalar m_dampingAlpha;
  69   btScalar m_dampingBeta;
  70
  71  public:
  72   //
  73   //  Fields
  74   //
  75
  76   // reduced space
  77   int m_nReduced;
  78   int m_nFull;
  79   tDenseMatrix m_modes;                                                                                                         // modes of the reduced deformable model. Each inner array is a mode, outer array size = n_modes
  80   tDenseArray m_reducedDofs;                               // Reduced degree of freedom
  81   tDenseArray m_reducedDofsBuffer;     // Reduced degree of freedom at t^n
  82   tDenseArray m_reducedVelocity;                   // Reduced velocity array
  83   tDenseArray m_reducedVelocityBuffer; // Reduced velocity array at t^n
  84   tDenseArray m_reducedForceExternal;          // reduced external force
  85   tDenseArray m_reducedForceElastic;           // reduced internal elastic force
  86   tDenseArray m_reducedForceDamping;           // reduced internal damping force
  87   tDenseArray m_eigenvalues;            // eigenvalues of the reduce deformable model
  88   tDenseArray m_Kr;     // reduced stiffness matrix
  89
  90   // full space
  91   TVStack m_x0;                                                                  // Rest position
  92   tDenseArray m_nodalMass;           // Mass on each node
  93   btAlignedObjectArray<int> m_fixedNodes; // index of the fixed nodes
  94   int m_nodeIndexOffset;             // offset of the node index needed for contact solver when there are multiple reduced deformable body in the world.
  95
  96   // contacts
  97   btAlignedObjectArray<int> m_contactNodesList;
  98
  99   //
 100   // Api
 101   //
 102   btReducedDeformableBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m);
 103
 104   ~btReducedDeformableBody() {}
 105
 106   //
 107   // initializing helpers
 108   //
 109   void internalInitialization();
 110
 111   void setReducedModes(int num_modes, int full_size);
 112
 113   void setMassProps(const tDenseArray& mass_array);
 114
 115   void setInertiaProps();
 116
 117   void setRigidVelocity(const btVector3& v);
 118
 119   void setRigidAngularVelocity(const btVector3& omega);
 120
 121   void setStiffnessScale(const btScalar ks);
 122
 123   void setMassScale(const btScalar rho);
 124
 125   void setFixedNodes(const int n_node);
 126
 127   void setDamping(const btScalar alpha, const btScalar beta);
 128
 129   void disableReducedModes(const bool rigid_only);
 130
 131   virtual void setTotalMass(btScalar mass, bool fromfaces = false);
 132
 133   //
 134   // various internal updates
 135   //
 136   virtual void transformTo(const btTransform& trs);
 137   virtual void transform(const btTransform& trs);
 138   // caution:
 139   // need to use scale before using transform, because the scale is performed in the local frame
 140   // (i.e., may have some rotation already, but the m_rigidTransformWorld doesn't have this info)
 141   virtual void scale(const btVector3& scl);
 142
 143  private:
 144   void updateRestNodalPositions();
 145
 146   void updateInitialInertiaTensor(const btMatrix3x3& rotation);
 147
 148   void updateLocalInertiaTensorFromNodes();
 149
 150   void updateInertiaTensor();
 151
 152   void updateModesByRotation(const btMatrix3x3& rotation);
 153
 154  public:
 155   void updateLocalMomentArm();
 156
 157   void predictIntegratedTransform(btScalar dt, btTransform& predictedTransform);
 158
 159   // update the external force projection matrix
 160   void updateExternalForceProjectMatrix(bool initialized);
 161
 162   void endOfTimeStepZeroing();
 163
 164   void applyInternalVelocityChanges();
 165
 166   //
 167   // position and velocity update related
 168   //
 169
 170   // compute reduced degree of freedoms
 171   void updateReducedDofs(btScalar solverdt);
 172
 173   // compute reduced velocity update (for explicit time stepping)
 174   void updateReducedVelocity(btScalar solverdt);
 175
 176   // map to full degree of freedoms
 177   void mapToFullPosition(const btTransform& ref_trans);
 178
 179   // compute full space velocity from the reduced velocity
 180   void mapToFullVelocity(const btTransform& ref_trans);
 181
 182   // compute total angular momentum
 183   const btVector3 computeTotalAngularMomentum() const;
 184
 185   // get a single node's full space velocity from the reduced velocity
 186   const btVector3 computeNodeFullVelocity(const btTransform& ref_trans, int n_node) const;
 187
 188   // get a single node's all delta velocity
 189   const btVector3 internalComputeNodeDeltaVelocity(const btTransform& ref_trans, int n_node) const;
 190
 191   //
 192   // rigid motion related
 193   //
 194   void applyDamping(btScalar timeStep);
 195
 196   void applyCentralImpulse(const btVector3& impulse);
 197
 198   void applyTorqueImpulse(const btVector3& torque);
 199
 200   void proceedToTransform(btScalar dt, bool end_of_time_step);
 201
 202   //
 203   // force related
 204   //
 205
 206   // apply impulse to the rigid frame
 207   void internalApplyRigidImpulse(const btVector3& impulse, const btVector3& rel_pos);
 208
 209   // apply impulse to nodes in the full space
 210   void internalApplyFullSpaceImpulse(const btVector3& impulse, const btVector3& rel_pos, int n_node, btScalar dt);
 211
 212   // apply nodal external force in the full space
 213   void applyFullSpaceNodalForce(const btVector3& f_ext, int n_node);
 214
 215   // apply gravity to the rigid frame
 216   void applyRigidGravity(const btVector3& gravity, btScalar dt);
 217
 218   // apply reduced elastic force
 219   void applyReducedElasticForce(const tDenseArray& reduce_dofs);
 220
 221   // apply reduced damping force
 222   void applyReducedDampingForce(const tDenseArray& reduce_vel);
 223
 224   // calculate the impulse factor
 225   virtual btMatrix3x3 getImpulseFactor(int n_node);
 226
 227   // get relative position from a node to the CoM of the rigid frame
 228   btVector3 getRelativePos(int n_node);
 229
 230   //
 231   // accessors
 232   //
 233   bool isReducedModesOFF() const;
 234   btScalar getTotalMass() const;
 235   btTransform& getRigidTransform();
 236   const btVector3& getLinearVelocity() const;
 237         const btVector3& getAngularVelocity() const;
 238
 239   #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
 240   void clampVelocity(btVector3& v) const {
 241       v.setX(
 242           fmax(-BT_CLAMP_VELOCITY_TO,
 243                 fmin(BT_CLAMP_VELOCITY_TO, v.getX()))
 244       );
 245       v.setY(
 246           fmax(-BT_CLAMP_VELOCITY_TO,
 247                 fmin(BT_CLAMP_VELOCITY_TO, v.getY()))
 248       );
 249       v.setZ(
 250           fmax(-BT_CLAMP_VELOCITY_TO,
 251                 fmin(BT_CLAMP_VELOCITY_TO, v.getZ()))
 252       );
 253   }
 254   #endif
 255 };
 256
 257 #endif // BT_REDUCED_SOFT_BODY_H