dali-physics/third-party/bullet3/src/LinearMath/btModifiedGramSchmidt.h

   1 //
   2 //  btModifiedGramSchmidt.h
   3 //  LinearMath
   4 //
   5 //  Created by Xuchen Han on 4/4/20.
   6 //
   7
   8 #ifndef btModifiedGramSchmidt_h
   9 #define btModifiedGramSchmidt_h
  10
  11 #include "btReducedVector.h"
  12 #include "btAlignedObjectArray.h"
  13 #include <iostream>
  14 #include <cmath>
  15 template<class TV>
  16 class btModifiedGramSchmidt
  17 {
  18 public:
  19     btAlignedObjectArray<TV> m_in;
  20     btAlignedObjectArray<TV> m_out;
  21
  22     btModifiedGramSchmidt(const btAlignedObjectArray<TV>& vecs): m_in(vecs)
  23     {
  24         m_out.resize(0);
  25     }
  26
  27     void solve()
  28     {
  29         m_out.resize(m_in.size());
  30         for (int i = 0; i < m_in.size(); ++i)
  31         {
  32 //            printf("========= starting %d ==========\n", i);
  33             TV v(m_in[i]);
  34 //            v.print();
  35             for (int j = 0; j < i; ++j)
  36             {
  37                 v = v - v.proj(m_out[j]);
  38 //                v.print();
  39             }
  40             v.normalize();
  41             m_out[i] = v;
  42 //            v.print();
  43         }
  44     }
  45
  46     void test()
  47     {
  48         std::cout << SIMD_EPSILON << std::endl;
  49         printf("=======inputs=========\n");
  50         for (int i = 0; i < m_out.size(); ++i)
  51         {
  52             m_in[i].print();
  53         }
  54         printf("=======output=========\n");
  55         for (int i = 0; i < m_out.size(); ++i)
  56         {
  57             m_out[i].print();
  58         }
  59         btScalar eps = SIMD_EPSILON;
  60         for (int i = 0; i < m_out.size(); ++i)
  61         {
  62             for (int j = 0; j < m_out.size(); ++j)
  63             {
  64                 if (i == j)
  65                 {
  66                     if (std::abs(1.0-m_out[i].dot(m_out[j])) > eps)// && std::abs(m_out[i].dot(m_out[j])) > eps)
  67                     {
  68                         printf("vec[%d] is not unit, norm squared = %f\n", i,m_out[i].dot(m_out[j]));
  69                     }
  70                 }
  71                 else
  72                 {
  73                     if (std::abs(m_out[i].dot(m_out[j])) > eps)
  74                     {
  75                         printf("vec[%d] and vec[%d] is not orthogonal, dot product = %f\n", i, j, m_out[i].dot(m_out[j]));
  76                     }
  77                 }
  78             }
  79         }
  80     }
  81 };
  82 template class btModifiedGramSchmidt<btReducedVector>;
  83 #endif /* btModifiedGramSchmidt_h */