blob: 7ac3d4fdda72e5f8e4683356d0d2de072004061f (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
|
#pragma once
#ifndef __CVTT_COVARIANCEMATRIX_H__
#define __CVTT_COVARIANCEMATRIX_H__
namespace cvtt
{
namespace Internal
{
template<int TMatrixSize>
class PackedCovarianceMatrix
{
public:
// 0: xx,
// 1: xy, yy
// 3: xz, yz, zz
// 6: xw, yw, zw, ww
// ... etc.
static const int PyramidSize = (TMatrixSize * (TMatrixSize + 1)) / 2;
typedef ParallelMath::Float MFloat;
PackedCovarianceMatrix()
{
for (int i = 0; i < PyramidSize; i++)
m_values[i] = ParallelMath::MakeFloatZero();
}
void Add(const ParallelMath::Float *vec, const ParallelMath::Float &weight)
{
int index = 0;
for (int row = 0; row < TMatrixSize; row++)
{
for (int col = 0; col <= row; col++)
{
m_values[index] = m_values[index] + vec[row] * vec[col] * weight;
index++;
}
}
}
void Product(MFloat *outVec, const MFloat *inVec)
{
for (int row = 0; row < TMatrixSize; row++)
{
MFloat sum = ParallelMath::MakeFloatZero();
int index = (row * (row + 1)) >> 1;
for (int col = 0; col < TMatrixSize; col++)
{
sum = sum + inVec[col] * m_values[index];
if (col >= row)
index += col + 1;
else
index++;
}
outVec[row] = sum;
}
}
private:
ParallelMath::Float m_values[PyramidSize];
};
}
}
#endif
|