blob: 855cb7a1b4673cbac7038af210349f4ec41311cc (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
|
// TODO : move this to GivensQR once there's such a thing in Eigen
template <typename Scalar>
void ei_r1mpyq(int m, int n, Scalar *a, const std::vector<PlanarRotation<Scalar> > &v_givens, const std::vector<PlanarRotation<Scalar> > &w_givens)
{
/* apply the first set of givens rotations to a. */
for (int j = n-2; j>=0; --j)
for (int i = 0; i<m; ++i) {
Scalar temp = v_givens[j].c() * a[i+m*j] - v_givens[j].s() * a[i+m*(n-1)];
a[i+m*(n-1)] = v_givens[j].s() * a[i+m*j] + v_givens[j].c() * a[i+m*(n-1)];
a[i+m*j] = temp;
}
/* apply the second set of givens rotations to a. */
for (int j = 0; j<n-1; ++j)
for (int i = 0; i<m; ++i) {
Scalar temp = w_givens[j].c() * a[i+m*j] + w_givens[j].s() * a[i+m*(n-1)];
a[i+m*(n-1)] = -w_givens[j].s() * a[i+m*j] + w_givens[j].c() * a[i+m*(n-1)];
a[i+m*j] = temp;
}
}
|