b7901c773c
Stop include Bullet headers using `-isystem` for GCC/Clang as it misleads SCons into not properly rebuilding all files when headers change. This means we also need to make sure Bullet builds without warning, and current version fares fairly well, there were just a couple to fix (patch included). Increase minimum version for distro packages to 2.90 (this was never released as the "next" version after 2.89 was 3.05... but that covers it too).
83 lines
2.2 KiB
C++
83 lines
2.2 KiB
C++
//
|
|
// 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 */
|