Build full A matrix and b vector for a MLCP solver interface, to explore Lemke, Dantzig, Newton and other MLCP solvers. The A matrix contains sparsity information.

Added a PGS solver that uses the sparsity of the A matrix, just for testing (the equivalent sequential impulse solver is much faster, not having to allocate the big matrices)

src/LinearMath/btScalar.h

@@ -611,6 +611,17 @@ SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src)
 	return d;
 }
 
+template<typename T>
+SIMD_FORCE_INLINE void btSetZero(T* a, int n)
+{
+  T* acurr = a;
+  size_t ncurr = n;
+  while (ncurr > 0) 
+  {
+    *(acurr++) = 0;
+    --ncurr;
+  }
+}
 // returns normalized value in range [-SIMD_PI, SIMD_PI]
 SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians) 
 {
@@ -629,6 +640,8 @@ SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians)
 	}
 }
 
+
+
 ///rudimentary class to provide type info
 struct btTypedObject
 {