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

   1 /*
   2  Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
   3
   4  Bullet Continuous Collision Detection and Physics Library
   5  Copyright (c) 2019 Google Inc. http://bulletphysics.org
   6  This software is provided 'as-is', without any express or implied warranty.
   7  In no event will the authors be held liable for any damages arising from the use of this software.
   8  Permission is granted to anyone to use this software for any purpose,
   9  including commercial applications, and to alter it and redistribute it freely,
  10  subject to the following restrictions:
  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
  16 #ifndef BT_BACKWARD_EULER_OBJECTIVE_H
  17 #define BT_BACKWARD_EULER_OBJECTIVE_H
  18 //#include "btConjugateGradient.h"
  19 #include "btDeformableLagrangianForce.h"
  20 #include "btDeformableMassSpringForce.h"
  21 #include "btDeformableGravityForce.h"
  22 #include "btDeformableCorotatedForce.h"
  23 #include "btDeformableMousePickingForce.h"
  24 #include "btDeformableLinearElasticityForce.h"
  25 #include "btDeformableNeoHookeanForce.h"
  26 #include "btDeformableContactProjection.h"
  27 #include "btPreconditioner.h"
  28 // #include "btDeformableMultiBodyDynamicsWorld.h"
  29 #include "LinearMath/btQuickprof.h"
  30
  31 class btDeformableBackwardEulerObjective
  32 {
  33 public:
  34         enum _
  35         {
  36                 Mass_preconditioner,
  37                 KKT_preconditioner
  38         };
  39
  40         typedef btAlignedObjectArray<btVector3> TVStack;
  41         btScalar m_dt;
  42         btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
  43         btAlignedObjectArray<btSoftBody*>& m_softBodies;
  44         Preconditioner* m_preconditioner;
  45         btDeformableContactProjection m_projection;
  46         const TVStack& m_backupVelocity;
  47         btAlignedObjectArray<btSoftBody::Node*> m_nodes;
  48         bool m_implicit;
  49         MassPreconditioner* m_massPreconditioner;
  50         KKTPreconditioner* m_KKTPreconditioner;
  51
  52         btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backup_v);
  53
  54         virtual ~btDeformableBackwardEulerObjective();
  55
  56         void initialize() {}
  57
  58         // compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual
  59         void computeResidual(btScalar dt, TVStack& residual);
  60
  61         // add explicit force to the velocity
  62         void applyExplicitForce(TVStack& force);
  63
  64         // apply force to velocity and optionally reset the force to zero
  65         void applyForce(TVStack& force, bool setZero);
  66
  67         // compute the norm of the residual
  68         btScalar computeNorm(const TVStack& residual) const;
  69
  70         // compute one step of the solve (there is only one solve if the system is linear)
  71         void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt);
  72
  73         // perform A*x = b
  74         void multiply(const TVStack& x, TVStack& b) const;
  75
  76         // set initial guess for CG solve
  77         void initialGuess(TVStack& dv, const TVStack& residual);
  78
  79         // reset data structure and reset dt
  80         void reinitialize(bool nodeUpdated, btScalar dt);
  81
  82         void setDt(btScalar dt);
  83
  84         // add friction force to residual
  85         void applyDynamicFriction(TVStack& r);
  86
  87         // add dv to velocity
  88         void updateVelocity(const TVStack& dv);
  89
  90         //set constraints as projections
  91         void setConstraints(const btContactSolverInfo& infoGlobal);
  92
  93         // update the projections and project the residual
  94         void project(TVStack& r)
  95         {
  96                 BT_PROFILE("project");
  97                 m_projection.project(r);
  98         }
  99
 100         // perform precondition M^(-1) x = b
 101         void precondition(const TVStack& x, TVStack& b)
 102         {
 103                 m_preconditioner->operator()(x, b);
 104         }
 105
 106         // reindex all the vertices
 107         virtual void updateId()
 108         {
 109                 size_t node_id = 0;
 110                 size_t face_id = 0;
 111                 m_nodes.clear();
 112                 for (int i = 0; i < m_softBodies.size(); ++i)
 113                 {
 114                         btSoftBody* psb = m_softBodies[i];
 115                         for (int j = 0; j < psb->m_nodes.size(); ++j)
 116                         {
 117                                 psb->m_nodes[j].index = node_id;
 118                                 m_nodes.push_back(&psb->m_nodes[j]);
 119                                 ++node_id;
 120                         }
 121                         for (int j = 0; j < psb->m_faces.size(); ++j)
 122                         {
 123                                 psb->m_faces[j].m_index = face_id;
 124                                 ++face_id;
 125                         }
 126                 }
 127         }
 128
 129         const btAlignedObjectArray<btSoftBody::Node*>* getIndices() const
 130         {
 131                 return &m_nodes;
 132         }
 133
 134         void setImplicit(bool implicit)
 135         {
 136                 m_implicit = implicit;
 137         }
 138
 139         // Calculate the total potential energy in the system
 140         btScalar totalEnergy(btScalar dt);
 141
 142         void addLagrangeMultiplier(const TVStack& vec, TVStack& extended_vec)
 143         {
 144                 extended_vec.resize(vec.size() + m_projection.m_lagrangeMultipliers.size());
 145                 for (int i = 0; i < vec.size(); ++i)
 146                 {
 147                         extended_vec[i] = vec[i];
 148                 }
 149                 int offset = vec.size();
 150                 for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i)
 151                 {
 152                         extended_vec[offset + i].setZero();
 153                 }
 154         }
 155
 156         void addLagrangeMultiplierRHS(const TVStack& residual, const TVStack& m_dv, TVStack& extended_residual)
 157         {
 158                 extended_residual.resize(residual.size() + m_projection.m_lagrangeMultipliers.size());
 159                 for (int i = 0; i < residual.size(); ++i)
 160                 {
 161                         extended_residual[i] = residual[i];
 162                 }
 163                 int offset = residual.size();
 164                 for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i)
 165                 {
 166                         const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[i];
 167                         extended_residual[offset + i].setZero();
 168                         for (int d = 0; d < lm.m_num_constraints; ++d)
 169                         {
 170                                 for (int n = 0; n < lm.m_num_nodes; ++n)
 171                                 {
 172                                         extended_residual[offset + i][d] += lm.m_weights[n] * m_dv[lm.m_indices[n]].dot(lm.m_dirs[d]);
 173                                 }
 174                         }
 175                 }
 176         }
 177
 178         void calculateContactForce(const TVStack& dv, const TVStack& rhs, TVStack& f)
 179         {
 180                 size_t counter = 0;
 181                 for (int i = 0; i < m_softBodies.size(); ++i)
 182                 {
 183                         btSoftBody* psb = m_softBodies[i];
 184                         for (int j = 0; j < psb->m_nodes.size(); ++j)
 185                         {
 186                                 const btSoftBody::Node& node = psb->m_nodes[j];
 187                                 f[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : dv[counter] / node.m_im;
 188                                 ++counter;
 189                         }
 190                 }
 191                 for (int i = 0; i < m_lf.size(); ++i)
 192                 {
 193                         // add damping matrix
 194                         m_lf[i]->addScaledDampingForceDifferential(-m_dt, dv, f);
 195                 }
 196                 counter = 0;
 197                 for (; counter < f.size(); ++counter)
 198                 {
 199                         f[counter] = rhs[counter] - f[counter];
 200                 }
 201         }
 202 };
 203
 204 #endif /* btBackwardEulerObjective_h */