Bullet Collision Detection & Physics Library: btModifiedGramSchmidt.h Source File

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

btModifiedGramSchmidt.h
Go to the documentation of this file.
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>
15template<class TV>
16 class btModifiedGramSchmidt
17{
18public:
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};
82template class btModifiedGramSchmidt<btReducedVector>;
83#endif /* btModifiedGramSchmidt_h */
btAlignedObjectArray.h
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
btReducedVector.h
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btAlignedObjectArray
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
Definition: btAlignedObjectArray.h:46
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:142
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btModifiedGramSchmidt
Definition: btModifiedGramSchmidt.h:17
btModifiedGramSchmidt::m_in
btAlignedObjectArray< TV > m_in
Definition: btModifiedGramSchmidt.h:19
btModifiedGramSchmidt::btModifiedGramSchmidt
btModifiedGramSchmidt(const btAlignedObjectArray< TV > &vecs)
Definition: btModifiedGramSchmidt.h:22
btModifiedGramSchmidt::test
void test()
Definition: btModifiedGramSchmidt.h:46
btModifiedGramSchmidt::m_out
btAlignedObjectArray< TV > m_out
Definition: btModifiedGramSchmidt.h:20
btModifiedGramSchmidt::solve
void solve()
Definition: btModifiedGramSchmidt.h:27
eps
const btScalar eps
Definition: poly34.cpp:11