1 files changed, 162 insertions, 137 deletions

diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.h b/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.h
index 86579e71ac..eb05b9f010 100644
--- a/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.h
+++ b/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.h
@@ -31,143 +31,168 @@
 class btDeformableBackwardEulerObjective
 {
 public:
-    typedef btAlignedObjectArray<btVector3> TVStack;
-    btScalar m_dt;
-    btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
-    btAlignedObjectArray<btSoftBody *>& m_softBodies;
-    Preconditioner* m_preconditioner;
-    btDeformableContactProjection m_projection;
-    const TVStack& m_backupVelocity;
-    btAlignedObjectArray<btSoftBody::Node* > m_nodes;
-    bool m_implicit;
-    MassPreconditioner* m_massPreconditioner;
-    KKTPreconditioner* m_KKTPreconditioner;
-
-    btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v);
-    
-    virtual ~btDeformableBackwardEulerObjective();
-    
-    void initialize(){}
-    
-    // compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual
-    void computeResidual(btScalar dt, TVStack& residual);
-    
-    // add explicit force to the velocity
-    void applyExplicitForce(TVStack& force);
-    
-    // apply force to velocity and optionally reset the force to zero
-    void applyForce(TVStack& force, bool setZero);
-    
-    // compute the norm of the residual
-    btScalar computeNorm(const TVStack& residual) const;
-    
-    // compute one step of the solve (there is only one solve if the system is linear)
-    void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt);
-    
-    // perform A*x = b
-    void multiply(const TVStack& x, TVStack& b) const;
-    
-    // set initial guess for CG solve
-    void initialGuess(TVStack& dv, const TVStack& residual);
-    
-    // reset data structure and reset dt
-    void reinitialize(bool nodeUpdated, btScalar dt);
-    
-    void setDt(btScalar dt);
-    
-    // add friction force to residual
-    void applyDynamicFriction(TVStack& r);
-    
-    // add dv to velocity
-    void updateVelocity(const TVStack& dv);
-    
-    //set constraints as projections
-    void setConstraints(const btContactSolverInfo& infoGlobal);
-    
-    // update the projections and project the residual
-    void project(TVStack& r)
-    {
-        BT_PROFILE("project");
-        m_projection.project(r);
-    }
-    
-    // perform precondition M^(-1) x = b
-    void precondition(const TVStack& x, TVStack& b)
-    {
-        m_preconditioner->operator()(x,b);
-    }
-
-    // reindex all the vertices 
-    virtual void updateId()
-    {
-        size_t node_id = 0;
-        size_t face_id = 0;
-        m_nodes.clear();
-        for (int i = 0; i < m_softBodies.size(); ++i)
-        {
-            btSoftBody* psb = m_softBodies[i];
-            for (int j = 0; j < psb->m_nodes.size(); ++j)
-            {
-                psb->m_nodes[j].index = node_id;
-                m_nodes.push_back(&psb->m_nodes[j]);
-                ++node_id;
-            }
-            for (int j = 0; j < psb->m_faces.size(); ++j)
-            {
-                psb->m_faces[j].m_index = face_id;
-                ++face_id;
-            }
-        }
-    }
-    
-    const btAlignedObjectArray<btSoftBody::Node*>* getIndices() const
-    {
-        return &m_nodes;
-    }
-    
-    void setImplicit(bool implicit)
-    {
-        m_implicit = implicit;
-    }
-
-    // Calculate the total potential energy in the system
-    btScalar totalEnergy(btScalar dt);
-    
-    void addLagrangeMultiplier(const TVStack& vec, TVStack& extended_vec)
-    {
-        extended_vec.resize(vec.size() + m_projection.m_lagrangeMultipliers.size());
-        for (int i = 0; i < vec.size(); ++i)
-        {
-            extended_vec[i] = vec[i];
-        }
-        int offset = vec.size();
-        for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i)
-        {
-            extended_vec[offset + i].setZero();
-        }
-    }
-    
-    void addLagrangeMultiplierRHS(const TVStack& residual, const TVStack& m_dv, TVStack& extended_residual)
-    {
-        extended_residual.resize(residual.size() + m_projection.m_lagrangeMultipliers.size());
-        for (int i = 0; i < residual.size(); ++i)
-        {
-            extended_residual[i] = residual[i];
-        }
-        int offset = residual.size();
-        for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i)
-        {
-            const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[i];
-            extended_residual[offset + i].setZero();
-            for (int d = 0; d < lm.m_num_constraints; ++d)
-            {
-                for (int n = 0; n < lm.m_num_nodes; ++n)
-                {
-                    extended_residual[offset + i][d] += lm.m_weights[n] * m_dv[lm.m_indices[n]].dot(lm.m_dirs[d]);
-                }
-            }
-        }
-    }
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	btScalar m_dt;
+	btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
+	btAlignedObjectArray<btSoftBody*>& m_softBodies;
+	Preconditioner* m_preconditioner;
+	btDeformableContactProjection m_projection;
+	const TVStack& m_backupVelocity;
+	btAlignedObjectArray<btSoftBody::Node*> m_nodes;
+	bool m_implicit;
+	MassPreconditioner* m_massPreconditioner;
+	KKTPreconditioner* m_KKTPreconditioner;
+
+	btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backup_v);
+
+	virtual ~btDeformableBackwardEulerObjective();
+
+	void initialize() {}
+
+	// compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual
+	void computeResidual(btScalar dt, TVStack& residual);
+
+	// add explicit force to the velocity
+	void applyExplicitForce(TVStack& force);
+
+	// apply force to velocity and optionally reset the force to zero
+	void applyForce(TVStack& force, bool setZero);
+
+	// compute the norm of the residual
+	btScalar computeNorm(const TVStack& residual) const;
+
+	// compute one step of the solve (there is only one solve if the system is linear)
+	void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt);
+
+	// perform A*x = b
+	void multiply(const TVStack& x, TVStack& b) const;
+
+	// set initial guess for CG solve
+	void initialGuess(TVStack& dv, const TVStack& residual);
+
+	// reset data structure and reset dt
+	void reinitialize(bool nodeUpdated, btScalar dt);
+
+	void setDt(btScalar dt);
+
+	// add friction force to residual
+	void applyDynamicFriction(TVStack& r);
+
+	// add dv to velocity
+	void updateVelocity(const TVStack& dv);
+
+	//set constraints as projections
+	void setConstraints(const btContactSolverInfo& infoGlobal);
+
+	// update the projections and project the residual
+	void project(TVStack& r)
+	{
+		BT_PROFILE("project");
+		m_projection.project(r);
+	}
+
+	// perform precondition M^(-1) x = b
+	void precondition(const TVStack& x, TVStack& b)
+	{
+		m_preconditioner->operator()(x, b);
+	}
+
+	// reindex all the vertices
+	virtual void updateId()
+	{
+		size_t node_id = 0;
+		size_t face_id = 0;
+		m_nodes.clear();
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				psb->m_nodes[j].index = node_id;
+				m_nodes.push_back(&psb->m_nodes[j]);
+				++node_id;
+			}
+			for (int j = 0; j < psb->m_faces.size(); ++j)
+			{
+				psb->m_faces[j].m_index = face_id;
+				++face_id;
+			}
+		}
+	}
+
+	const btAlignedObjectArray<btSoftBody::Node*>* getIndices() const
+	{
+		return &m_nodes;
+	}
+
+	void setImplicit(bool implicit)
+	{
+		m_implicit = implicit;
+	}
+
+	// Calculate the total potential energy in the system
+	btScalar totalEnergy(btScalar dt);
+
+	void addLagrangeMultiplier(const TVStack& vec, TVStack& extended_vec)
+	{
+		extended_vec.resize(vec.size() + m_projection.m_lagrangeMultipliers.size());
+		for (int i = 0; i < vec.size(); ++i)
+		{
+			extended_vec[i] = vec[i];
+		}
+		int offset = vec.size();
+		for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i)
+		{
+			extended_vec[offset + i].setZero();
+		}
+	}
+
+	void addLagrangeMultiplierRHS(const TVStack& residual, const TVStack& m_dv, TVStack& extended_residual)
+	{
+		extended_residual.resize(residual.size() + m_projection.m_lagrangeMultipliers.size());
+		for (int i = 0; i < residual.size(); ++i)
+		{
+			extended_residual[i] = residual[i];
+		}
+		int offset = residual.size();
+		for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i)
+		{
+			const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[i];
+			extended_residual[offset + i].setZero();
+			for (int d = 0; d < lm.m_num_constraints; ++d)
+			{
+				for (int n = 0; n < lm.m_num_nodes; ++n)
+				{
+					extended_residual[offset + i][d] += lm.m_weights[n] * m_dv[lm.m_indices[n]].dot(lm.m_dirs[d]);
+				}
+			}
+		}
+	}
+
+	void calculateContactForce(const TVStack& dv, const TVStack& rhs, TVStack& f)
+	{
+		size_t counter = 0;
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				const btSoftBody::Node& node = psb->m_nodes[j];
+				f[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : dv[counter] / node.m_im;
+				++counter;
+			}
+		}
+		for (int i = 0; i < m_lf.size(); ++i)
+		{
+			// add damping matrix
+			m_lf[i]->addScaledDampingForceDifferential(-m_dt, dv, f);
+		}
+		counter = 0;
+		for (; counter < f.size(); ++counter)
+		{
+			f[counter] = rhs[counter] - f[counter];
+		}
+	}
 };
 
 #endif /* btBackwardEulerObjective_h */