diff options
author | Sebastian Parborg <darkdefende@gmail.com> | 2020-09-02 21:41:30 +0300 |
---|---|---|
committer | Sebastian Parborg <darkdefende@gmail.com> | 2020-09-02 21:41:30 +0300 |
commit | 4446c3a593c51603e135e38951607b9b668ddec5 (patch) | |
tree | 9552dbd903b4fd05ea740e1bba9b1b87d97414a1 /extern/bullet2/src/LinearMath/btModifiedGramSchmidt.h | |
parent | 6f6f6ee18695dad66ad8aa0eb2bcab72501df597 (diff) |
Sync Bullet to upstream
This syncs Bullet to the latest upstream git version as of writing this.
(commit 47b0259b9700455022b5cf79b651cc1dc71dd59e).
Diffstat (limited to 'extern/bullet2/src/LinearMath/btModifiedGramSchmidt.h')
-rw-r--r-- | extern/bullet2/src/LinearMath/btModifiedGramSchmidt.h | 83 |
1 files changed, 83 insertions, 0 deletions
diff --git a/extern/bullet2/src/LinearMath/btModifiedGramSchmidt.h b/extern/bullet2/src/LinearMath/btModifiedGramSchmidt.h new file mode 100644 index 00000000000..33bab8d650d --- /dev/null +++ b/extern/bullet2/src/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 */ |