1 files changed, 28 insertions, 18 deletions

diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.h b/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.h
index 3c4490765e..8d7e94d4fb 100644
--- a/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.h
+++ b/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.h
@@ -21,30 +21,37 @@
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 #include "btDeformableContactConstraint.h"
 #include "LinearMath/btHashMap.h"
+#include "LinearMath/btReducedVector.h"
+#include "LinearMath/btModifiedGramSchmidt.h"
 #include <vector>
+
+struct LagrangeMultiplier
+{
+    int m_num_constraints;        // Number of constraints
+    int m_num_nodes;              // Number of nodes in these constraints
+    btScalar m_weights[3];        // weights of the nodes involved, same size as m_num_nodes
+    btVector3 m_dirs[3];          // Constraint directions, same size of m_num_constraints;
+    int m_indices[3];             // indices of the nodes involved, same size as m_num_nodes;
+};
+
+
 class btDeformableContactProjection
 {
 public:
     typedef btAlignedObjectArray<btVector3> TVStack;
     btAlignedObjectArray<btSoftBody *>& m_softBodies;
-    
-//    // map from node index to static constraint
-//    btHashMap<btHashInt, btDeformableStaticConstraint> m_staticConstraints;
-//    // map from node index to node rigid constraint
-//    btHashMap<btHashInt, btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
-//    // map from node index to face rigid constraint
-//    btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> > m_faceRigidConstraints;
-//    // map from node index to deformable constraint
-//    btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> > m_deformableConstraints;
-//    // map from node index to node anchor constraint
-//    btHashMap<btHashInt, btDeformableNodeAnchorConstraint> m_nodeAnchorConstraints;
 	
     // all constraints involving face
     btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;
-    
+#ifndef USE_MGS
     // map from node index to projection directions
     btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict;
-	
+#else
+    btAlignedObjectArray<btReducedVector> m_projections;
+#endif
+    
+    btAlignedObjectArray<LagrangeMultiplier> m_lagrangeMultipliers;
+    
 	// map from node index to static constraint
 	btAlignedObjectArray<btAlignedObjectArray<btDeformableStaticConstraint> > m_staticConstraints;
 	// map from node index to node rigid constraint
@@ -56,6 +63,8 @@ public:
 	// map from node index to node anchor constraint
 	btAlignedObjectArray<btAlignedObjectArray<btDeformableNodeAnchorConstraint> > m_nodeAnchorConstraints;
     
+    bool m_useStrainLimiting;
+    
     btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies)
     : m_softBodies(softBodies)
     {
@@ -72,13 +81,10 @@ public:
     virtual void applyDynamicFriction(TVStack& f);
     
     // update and solve the constraints
-    virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies);
-    
-    // solve the position error using split impulse
-    virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
+    virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal);
     
     // Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict.
-    virtual void setConstraints();
+    virtual void setConstraints(const btContactSolverInfo& infoGlobal);
     
     // Set up projections for each vertex by adding the projection direction to
     virtual void setProjection();
@@ -86,5 +92,9 @@ public:
     virtual void reinitialize(bool nodeUpdated);
     
     virtual void splitImpulseSetup(const btContactSolverInfo& infoGlobal);
+    
+    virtual void setLagrangeMultiplier();
+    
+    void checkConstraints(const TVStack& x);
 };
 #endif /* btDeformableContactProjection_h */