add support for reduced vectors and modified gram schmidt

src/BulletSoftBody/btDeformableContactProjection.cpp

@@ -109,7 +109,7 @@ btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverI
 }
 
 void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal)
-{
+{  
 	BT_PROFILE("setConstraints");
 	for (int i = 0; i < m_softBodies.size(); ++i)
 	{

src/BulletSoftBody/btDeformableContactProjection.h

@@ -21,6 +21,8 @@
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 #include "btDeformableContactConstraint.h"
 #include "LinearMath/btHashMap.h"
+#include "LinearMath/btReducedVector.h"
+#include "LinearMath/btModifiedGramSchmidt.h"
 #include <vector>
 class btDeformableContactProjection
 {

src/BulletSoftBody/btSoftBody.cpp

@@ -3051,9 +3051,6 @@ void btSoftBody::updateArea(bool averageArea)
 	{
 		Face& f = m_faces[i];
 		f.m_ra = AreaOf(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x);
-        f.m_rl[0] = (f.m_n[0]->m_x - f.m_n[1]->m_x).safeNorm();
-        f.m_rl[1] = (f.m_n[0]->m_x - f.m_n[2]->m_x).safeNorm();
-        f.m_rl[2] = (f.m_n[1]->m_x - f.m_n[2]->m_x).safeNorm();
 	}
 
 	/* Node area		*/

src/LinearMath/CMakeLists.txt

@@ -10,6 +10,7 @@ SET(LinearMath_SRCS
 	btGeometryUtil.cpp
 	btPolarDecomposition.cpp
 	btQuickprof.cpp
+	btReducedVector.cpp
 	btSerializer.cpp
 	btSerializer64.cpp
 	btThreads.cpp
@@ -40,6 +41,7 @@ SET(LinearMath_HDRS
 	btQuadWord.h
 	btQuaternion.h
 	btQuickprof.h
+	btReducedVector.h
 	btRandom.h
 	btScalar.h
 	btSerializer.h

src/LinearMath/btModifiedGramSchmidt.h

@@ -0,0 +1,71 @@
+//
+//  btModifiedGramSchmidt.h
+//  LinearMath
+//
+//  Created by Xuchen Han on 4/4/20.
+//
+
+#ifndef btModifiedGramSchmidt_h
+#define btModifiedGramSchmidt_h
+
+#include "btReducedVector.h"
+#include "btAlignedObjectArray.h"
+
+template<class TV>
+class btModifiedGramSchmidt
+{
+public:
+    btAlignedObjectArray<TV> m_in;
+    btAlignedObjectArray<TV> m_out;
+    
+    btModifiedGramSchmidt(const btAlignedObjectArray<TV>& vecs): m_in(vecs)
+    {
+        m_out.resize(0);
+    }
+    
+    void solve()
+    {
+        m_out.resize(m_in.size());
+        for (int i = 0; i < m_in.size(); ++i)
+        {
+            TV v(m_in[i]);
+            v.print();
+            for (int j = 0; j < i; ++j)
+            {
+                v = v - v.proj(m_out[j]);
+                v.print();
+            }
+            v.normalize();
+            v.print();
+            m_out[i] = v;
+            printf("===========\n");
+        }
+    }
+    
+    void test()
+    {
+        btScalar eps = SIMD_EPSILON;
+        for (int i = 0; i < m_out.size(); ++i)
+        {
+            for (int j = 0; j < m_out.size(); ++j)
+            {
+                if (i == j)
+                {
+                    if (std::abs(1-m_out[i].dot(m_out[j])) > eps && std::abs(m_out[i].dot(m_out[j])) > eps)
+                    {
+                        printf("vec[%d] is not unit, norm squared = %f\n", i,m_out[i].dot(m_out[j]));
+                    }
+                }
+                else
+                {
+                    if (std::abs(m_out[i].dot(m_out[j])) > eps)
+                    {
+                        printf("vec[%d] and vec[%d] is not orthogonal, dot product = %f\n", i, j, m_out[i].dot(m_out[j]));
+                    }
+                }
+            }
+        }
+    }
+};
+template class btModifiedGramSchmidt<btReducedVector>;
+#endif /* btModifiedGramSchmidt_h */

src/LinearMath/btReducedVector.cpp

@@ -0,0 +1,162 @@
+//
+//  btReducedVector.cpp
+//  LinearMath
+//
+//  Created by Xuchen Han on 4/4/20.
+//
+#include <stdio.h>
+#include "btReducedVector.h"
+
+// returns the projection of this onto other
+btReducedVector btReducedVector::proj(const btReducedVector& other) const
+{
+    btReducedVector ret(m_sz);
+    btScalar other_length2 = other.length2();
+    if (other_length2 == 0)
+    {
+        return ret;
+    }
+    return other*(this->dot(other) / other_length2);
+}
+
+void btReducedVector::normalize()
+{
+    if (this->length2() < SIMD_EPSILON)
+        return;
+    *this /= btSqrt(this->length2());
+}
+
+bool btReducedVector::testAdd() const
+{
+    int sz = 5;
+    btAlignedObjectArray<int> id1;
+    id1.push_back(1);
+    id1.push_back(3);
+    btAlignedObjectArray<btVector3> v1;
+    v1.push_back(btVector3(1,0,1));
+    v1.push_back(btVector3(3,1,5));
+    btAlignedObjectArray<int> id2;
+    id2.push_back(2);
+    id2.push_back(3);
+    id2.push_back(5);
+    btAlignedObjectArray<btVector3> v2;
+    v2.push_back(btVector3(2,3,1));
+    v2.push_back(btVector3(3,4,9));
+    v2.push_back(btVector3(0,4,0));
+    btAlignedObjectArray<int> id3;
+    id3.push_back(1);
+    id3.push_back(2);
+    id3.push_back(3);
+    id3.push_back(5);
+    btAlignedObjectArray<btVector3> v3;
+    v3.push_back(btVector3(1,0,1));
+    v3.push_back(btVector3(2,3,1));
+    v3.push_back(btVector3(6,5,14));
+    v3.push_back(btVector3(0,4,0));
+    btReducedVector rv1(sz, id1, v1);
+    btReducedVector rv2(sz, id2, v2);
+    btReducedVector ans(sz, id3, v3);
+    bool ret = ((ans == rv1+rv2) && (ans == rv2+rv1));
+    if (!ret)
+        printf("btReducedVector testAdd failed\n");
+    return ret;
+}
+
+bool btReducedVector::testMinus() const
+{
+    int sz = 5;
+    btAlignedObjectArray<int> id1;
+    id1.push_back(1);
+    id1.push_back(3);
+    btAlignedObjectArray<btVector3> v1;
+    v1.push_back(btVector3(1,0,1));
+    v1.push_back(btVector3(3,1,5));
+    btAlignedObjectArray<int> id2;
+    id2.push_back(2);
+    id2.push_back(3);
+    id2.push_back(5);
+    btAlignedObjectArray<btVector3> v2;
+    v2.push_back(btVector3(2,3,1));
+    v2.push_back(btVector3(3,4,9));
+    v2.push_back(btVector3(0,4,0));
+    btAlignedObjectArray<int> id3;
+    id3.push_back(1);
+    id3.push_back(2);
+    id3.push_back(3);
+    id3.push_back(5);
+    btAlignedObjectArray<btVector3> v3;
+    v3.push_back(btVector3(-1,-0,-1));
+    v3.push_back(btVector3(2,3,1));
+    v3.push_back(btVector3(0,3,4));
+    v3.push_back(btVector3(0,4,0));
+    btReducedVector rv1(sz, id1, v1);
+    btReducedVector rv2(sz, id2, v2);
+    btReducedVector ans(sz, id3, v3);
+    bool ret = (ans == rv2-rv1);
+    if (!ret)
+        printf("btReducedVector testMinus failed\n");
+    return ret;
+}
+
+bool btReducedVector::testDot() const
+{
+    int sz = 5;
+    btAlignedObjectArray<int> id1;
+    id1.push_back(1);
+    id1.push_back(3);
+    btAlignedObjectArray<btVector3> v1;
+    v1.push_back(btVector3(1,0,1));
+    v1.push_back(btVector3(3,1,5));
+    btAlignedObjectArray<int> id2;
+    id2.push_back(2);
+    id2.push_back(3);
+    id2.push_back(5);
+    btAlignedObjectArray<btVector3> v2;
+    v2.push_back(btVector3(2,3,1));
+    v2.push_back(btVector3(3,4,9));
+    v2.push_back(btVector3(0,4,0));
+    btReducedVector rv1(sz, id1, v1);
+    btReducedVector rv2(sz, id2, v2);
+    btScalar ans = 58;
+    bool ret = (ans == rv2.dot(rv1) && ans == rv1.dot(rv2));
+    if (!ret)
+        printf("btReducedVector testDot failed\n");
+    return ret;
+}
+
+bool btReducedVector::testMultiply() const
+{
+    int sz = 5;
+    btAlignedObjectArray<int> id1;
+    id1.push_back(1);
+    id1.push_back(3);
+    btAlignedObjectArray<btVector3> v1;
+    v1.push_back(btVector3(1,0,1));
+    v1.push_back(btVector3(3,1,5));
+    btScalar s = 2;
+    btReducedVector rv1(sz, id1, v1);
+    btAlignedObjectArray<int> id2;
+    id2.push_back(1);
+    id2.push_back(3);
+    btAlignedObjectArray<btVector3> v2;
+    v2.push_back(btVector3(2,0,2));
+    v2.push_back(btVector3(6,2,10));
+    btReducedVector ans(sz, id2, v2);
+    bool ret = (ans == rv1*s);
+    if (!ret)
+        printf("btReducedVector testMultiply failed\n");
+    return ret;
+}
+
+void btReducedVector::test() const
+{
+    bool ans = testAdd() && testMinus() && testDot() && testMultiply();
+    if (ans)
+    {
+        printf("All tests passed\n");
+    }
+    else
+    {
+        printf("Tests failed\n");
+    }
+}

src/LinearMath/btReducedVector.h

@@ -0,0 +1,267 @@
+//
+//  btReducedVectors.h
+//  BulletLinearMath
+//
+//  Created by Xuchen Han on 4/4/20.
+//
+#ifndef btReducedVectors_h
+#define btReducedVectors_h
+#include "btVector3.h"
+#include "btMatrix3x3.h"
+#include "btAlignedObjectArray.h"
+#include <stdio.h>
+// A helper vector type used for CG projections
+class btReducedVector
+{
+public:
+    btAlignedObjectArray<int> m_indices;
+    btAlignedObjectArray<btVector3> m_vecs;
+    int m_sz; // all m_indices value < m_sz
+public:
+	btReducedVector():m_sz(0)
+	{
+		m_indices.resize(0);
+		m_vecs.resize(0);
+	}
+	
+    btReducedVector(int sz): m_sz(sz)
+    {
+        m_indices.resize(0);
+        m_vecs.resize(0);
+    }
+    
+    btReducedVector(int sz, const btAlignedObjectArray<int>& indices, const btAlignedObjectArray<btVector3>& vecs): m_sz(sz), m_indices(indices), m_vecs(vecs)
+    {
+    }
+    
+    void simplify()
+    {
+        btAlignedObjectArray<int> old_indices(m_indices);
+        btAlignedObjectArray<btVector3> old_vecs(m_vecs);
+        m_indices.resize(0);
+        m_vecs.resize(0);
+        for (int i = 0; i < old_indices.size(); ++i)
+        {
+            if (old_vecs[i].length2() > SIMD_EPSILON)
+            {
+                m_indices.push_back(old_indices[i]);
+                m_vecs.push_back(old_vecs[i]);
+            }
+        }
+    }
+    
+    btReducedVector operator+(const btReducedVector& other)
+    {
+		btReducedVector ret(m_sz);
+		int i=0, j=0;
+		while (i < m_indices.size() && j < other.m_indices.size())
+		{
+			if (m_indices[i] < other.m_indices[j])
+			{
+				ret.m_indices.push_back(m_indices[i]);
+				ret.m_vecs.push_back(m_vecs[i]);
+				++i;
+			}
+			else if (m_indices[i] > other.m_indices[j])
+			{
+				ret.m_indices.push_back(other.m_indices[j]);
+				ret.m_vecs.push_back(other.m_vecs[j]);
+				++j;
+			}
+			else
+			{
+				ret.m_indices.push_back(other.m_indices[j]);
+				ret.m_vecs.push_back(m_vecs[i] + other.m_vecs[j]);
+				++i; ++j;
+			}
+		}
+		while (i < m_indices.size())
+		{
+			ret.m_indices.push_back(m_indices[i]);
+			ret.m_vecs.push_back(m_vecs[i]);
+			++i;
+		}
+		while (j < other.m_indices.size())
+		{
+			ret.m_indices.push_back(other.m_indices[j]);
+			ret.m_vecs.push_back(other.m_vecs[j]);
+			++j;
+		}
+        ret.simplify();
+        return ret;
+    }
+
+    btReducedVector operator-()
+    {
+        btReducedVector ret(m_sz);
+        for (int i = 0; i < m_indices.size(); ++i)
+        {
+            ret.m_indices.push_back(m_indices[i]);
+            ret.m_vecs.push_back(-m_vecs[i]);
+        }
+        ret.simplify();
+        return ret;
+    }
+    
+    btReducedVector operator-(const btReducedVector& other)
+    {
+		btReducedVector ret(m_sz);
+		int i=0, j=0;
+		while (i < m_indices.size() && j < other.m_indices.size())
+		{
+			if (m_indices[i] < other.m_indices[j])
+			{
+				ret.m_indices.push_back(m_indices[i]);
+				ret.m_vecs.push_back(m_vecs[i]);
+				++i;
+			}
+			else if (m_indices[i] > other.m_indices[j])
+			{
+				ret.m_indices.push_back(other.m_indices[j]);
+				ret.m_vecs.push_back(-other.m_vecs[j]);
+				++j;
+			}
+			else
+			{
+				ret.m_indices.push_back(other.m_indices[j]);
+				ret.m_vecs.push_back(m_vecs[i] - other.m_vecs[j]);
+				++i; ++j;
+			}
+		}
+		while (i < m_indices.size())
+		{
+			ret.m_indices.push_back(m_indices[i]);
+			ret.m_vecs.push_back(m_vecs[i]);
+			++i;
+		}
+		while (j < other.m_indices.size())
+		{
+			ret.m_indices.push_back(other.m_indices[j]);
+			ret.m_vecs.push_back(-other.m_vecs[j]);
+			++j;
+		}
+        ret.simplify();
+		return ret;
+    }
+    
+    bool operator==(const btReducedVector& other) const
+    {
+        if (m_sz != other.m_sz)
+            return false;
+        if (m_indices.size() != other.m_indices.size())
+            return false;
+        for (int i = 0; i < m_indices.size(); ++i)
+        {
+            if (m_indices[i] != other.m_indices[i] || m_vecs[i] != other.m_vecs[i])
+            {
+                return false;
+            }
+        }
+        return true;
+    }
+    
+    bool operator!=(const btReducedVector& other) const
+    {
+        return !(*this == other);
+    }
+	
+	btReducedVector& operator=(const btReducedVector& other)
+	{
+		if (this == &other)
+		{
+			return *this;
+		}
+		m_indices.copyFromArray(other.m_indices);
+		m_vecs.copyFromArray(other.m_vecs);
+		return *this;
+	}
+    
+    btScalar dot(const btReducedVector& other) const
+    {
+        btScalar ret = 0;
+        int j = 0;
+        for (int i = 0; i < m_indices.size(); ++i)
+        {
+            while (j < other.m_indices.size() && other.m_indices[j] < m_indices[i])
+            {
+                ++j;
+            }
+            if (j < other.m_indices.size() && other.m_indices[j] == m_indices[i])
+            {
+                ret += m_vecs[i].dot(other.m_vecs[j]);
+            }
+        }
+        return ret;
+    }
+    
+    btScalar length2() const
+    {
+        return this->dot(*this);
+    }
+	
+	void normalize();
+    
+    // returns the projection of this onto other
+    btReducedVector proj(const btReducedVector& other) const;
+    
+    bool testAdd() const;
+    
+    bool testMinus() const;
+    
+    bool testDot() const;
+    
+    bool testMultiply() const;
+    
+    void test() const;
+    
+    void print() const
+    {
+        for (int i = 0; i < m_indices.size(); ++i)
+        {
+            printf("%d: (%f, %f, %f)/", m_indices[i], m_vecs[i][0],m_vecs[i][1],m_vecs[i][2]);
+        }
+        printf("\n");
+    }
+};
+
+SIMD_FORCE_INLINE btReducedVector operator*(const btReducedVector& v, btScalar s)
+{
+    btReducedVector ret(v.m_sz);
+    for (int i = 0; i < v.m_indices.size(); ++i)
+    {
+        ret.m_indices.push_back(v.m_indices[i]);
+        ret.m_vecs.push_back(s*v.m_vecs[i]);
+    }
+    ret.simplify();
+    return ret;
+}
+
+SIMD_FORCE_INLINE btReducedVector operator*(btScalar s, const btReducedVector& v)
+{
+    return v*s;
+}
+
+SIMD_FORCE_INLINE btReducedVector operator/(const btReducedVector& v, btScalar s)
+{
+	return v * (1.0/s);
+}
+
+SIMD_FORCE_INLINE btReducedVector& operator/=(btReducedVector& v, btScalar s)
+{
+	v = v/s;
+	return v;
+}
+
+SIMD_FORCE_INLINE btReducedVector& operator+=(btReducedVector& v1, const btReducedVector& v2)
+{
+	v1 = v1+v2;
+	return v1;
+}
+
+SIMD_FORCE_INLINE btReducedVector& operator-=(btReducedVector& v1, const btReducedVector& v2)
+{
+	v1 = v1-v2;
+	return v1;
+}
+
+#endif /* btReducedVectors_h */