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