1 files changed, 83 insertions, 0 deletions
diff --git a/thirdparty/bullet/LinearMath/btModifiedGramSchmidt.h b/thirdparty/bullet/LinearMath/btModifiedGramSchmidt.h
new file mode 100644
index 0000000000..33bab8d650
--- /dev/null
+++ b/thirdparty/bullet/LinearMath/btModifiedGramSchmidt.h
@@ -0,0 +1,83 @@
+//
+//  btModifiedGramSchmidt.h
+//  LinearMath
+//
+//  Created by Xuchen Han on 4/4/20.
+//
+
+#ifndef btModifiedGramSchmidt_h
+#define btModifiedGramSchmidt_h
+
+#include "btReducedVector.h"
+#include "btAlignedObjectArray.h"
+#include <iostream>
+#include <cmath>
+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)
+        {
+//            printf("========= starting %d ==========\n", 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();
+            m_out[i] = v;
+//            v.print();
+        }
+    }
+    
+    void test()
+    {
+        std::cout << SIMD_EPSILON << std::endl;
+        printf("=======inputs=========\n");
+        for (int i = 0; i < m_out.size(); ++i)
+        {
+            m_in[i].print();
+        }
+        printf("=======output=========\n");
+        for (int i = 0; i < m_out.size(); ++i)
+        {
+            m_out[i].print();
+        }
+        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.0-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 */