Bullet Collision Detection & Physics Library
src
LinearMath
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>
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 */
SIMD_EPSILON
#define SIMD_EPSILON
Definition:
btScalar.h:543
btModifiedGramSchmidt::test
void test()
Definition:
btModifiedGramSchmidt.h:46
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition:
btQuaternion.h:888
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition:
btScalar.h:314
btModifiedGramSchmidt::m_out
btAlignedObjectArray< TV > m_out
Definition:
btModifiedGramSchmidt.h:20
btModifiedGramSchmidt::solve
void solve()
Definition:
btModifiedGramSchmidt.h:27
btModifiedGramSchmidt
Definition:
btModifiedGramSchmidt.h:17
btReducedVector.h
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition:
btAlignedObjectArray.h:203
btAlignedObjectArray< TV >
btAlignedObjectArray.h
btModifiedGramSchmidt::btModifiedGramSchmidt
btModifiedGramSchmidt(const btAlignedObjectArray< TV > &vecs)
Definition:
btModifiedGramSchmidt.h:22
btModifiedGramSchmidt::m_in
btAlignedObjectArray< TV > m_in
Definition:
btModifiedGramSchmidt.h:19
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition:
btAlignedObjectArray.h:142
eps
const btScalar eps
Definition:
poly34.cpp:11
Generated by
1.8.20