2 // btModifiedGramSchmidt.h
5 // Created by Xuchen Han on 4/4/20.
8 #ifndef btModifiedGramSchmidt_h
9 #define btModifiedGramSchmidt_h
11 #include "btReducedVector.h"
12 #include "btAlignedObjectArray.h"
16 class btModifiedGramSchmidt
19 btAlignedObjectArray<TV> m_in;
20 btAlignedObjectArray<TV> m_out;
22 btModifiedGramSchmidt(const btAlignedObjectArray<TV>& vecs): m_in(vecs)
29 m_out.resize(m_in.size());
30 for (int i = 0; i < m_in.size(); ++i)
32 // printf("========= starting %d ==========\n", i);
35 for (int j = 0; j < i; ++j)
37 v = v - v.proj(m_out[j]);
48 std::cout << SIMD_EPSILON << std::endl;
49 printf("=======inputs=========\n");
50 for (int i = 0; i < m_out.size(); ++i)
54 printf("=======output=========\n");
55 for (int i = 0; i < m_out.size(); ++i)
59 btScalar eps = SIMD_EPSILON;
60 for (int i = 0; i < m_out.size(); ++i)
62 for (int j = 0; j < m_out.size(); ++j)
66 if (std::abs(1.0-m_out[i].dot(m_out[j])) > eps)// && std::abs(m_out[i].dot(m_out[j])) > eps)
68 printf("vec[%d] is not unit, norm squared = %f\n", i,m_out[i].dot(m_out[j]));
73 if (std::abs(m_out[i].dot(m_out[j])) > eps)
75 printf("vec[%d] and vec[%d] is not orthogonal, dot product = %f\n", i, j, m_out[i].dot(m_out[j]));
82 template class btModifiedGramSchmidt<btReducedVector>;
83 #endif /* btModifiedGramSchmidt_h */