qpmad
Eigen-based C++ QP solver.
Loading...
Searching...
No Matches
givens.h
Go to the documentation of this file.
1/**
2 @file
3 @author Alexander Sherikov
4
5 @copyright 2017 Alexander Sherikov. Licensed under the Apache License,
6 Version 2.0. (see LICENSE or http://www.apache.org/licenses/LICENSE-2.0)
7
8 @brief
9*/
10
11
12#pragma once
13
14namespace qpmad
15{
16 /**
17 * @brief
18 *
19 * Represents Givens rotation
20 *
21 * [ cos, sin ]
22 * [ ]
23 * [ -sin, cos ]
24 *
25 * for a given vector (a, b) is defined with
26 *
27 * [ cos, sin ] [ a ] [ d ]
28 * [ ] * [ ] = [ ]
29 * [ -sin, cos ] [ b ] [ 0 ]
30 *
31 * sin^2 + cos^2 = 1
32 *
33 * Special cases:
34 * COPY b == 0: cos = 1, sin = 0
35 * SWAP (b != 0) && (a == 0): cos = 0, sin = 1
36 */
37 template <typename t_Scalar>
39 {
40 public:
41 enum Type
42 {
44 COPY = 1,
45 SWAP = 2
46 };
47
48
49 public:
50 Type computeAndApply(t_Scalar &a, t_Scalar &b, const t_Scalar eps)
51 {
52 t_Scalar abs_b = std::fabs(b);
53
54 if (abs_b > eps)
55 {
56 t_Scalar abs_a = std::fabs(a);
57
58 if (abs_a > eps)
59 {
60 t_Scalar t;
61 if (abs_a > abs_b)
62 {
63 t = (abs_b / abs_a);
64 t = abs_a * std::sqrt(1.0 + t * t);
65 }
66 else
67 {
68 t = (abs_a / abs_b);
69 t = abs_b * std::sqrt(1.0 + t * t);
70 }
71 t = copysign(t, a);
72
73 cos = a / t;
74 sin = b / t;
75
76 a = t;
77 b = 0.0;
78
80 }
81 else
82 {
83 // cos = 0.0;
84 // sin = 1.0;
85 swap(a, b);
86 type = SWAP;
87 }
88 }
89 else
90 {
91 // cos = 1.0;
92 // sin = 0.0;
93 type = COPY;
94 }
95
96 return (type);
97 }
98
99
100 void apply(t_Scalar &a, t_Scalar &b) const
101 {
102 switch (type)
103 {
104 case COPY:
105 return;
106 case SWAP:
107 swap(a, b);
108 return;
109 case NONTRIVIAL:
110 applyNonTrivial(a, b);
111 return;
112 }
113 }
114
115 template <class t_MatrixType>
116 void applyColumnWise(t_MatrixType &M, const int start, const int end, const int column_1, const int column_2)
117 const
118 {
119 switch (type)
120 {
121 case COPY:
122 return;
123 case SWAP:
124 M.col(column_1).segment(start, end - start).swap(M.col(column_2).segment(start, end - start));
125 return;
126 case NONTRIVIAL:
127 M.middleRows(start, end - start)
128 .transpose()
129 .applyOnTheLeft(column_1, column_2, Eigen::JacobiRotation<t_Scalar>(cos, sin));
130 return;
131 }
132 }
133
134
135 template <class t_MatrixType>
136 void applyRowWise(t_MatrixType &M, const int start, const int end, const int row_1, const int row_2) const
137 {
138 switch (type)
139 {
140 case COPY:
141 return;
142 case SWAP:
143 M.row(row_1).segment(start, end - start).swap(M.row(row_2).segment(start, end - start));
144 return;
145 case NONTRIVIAL:
146 M.middleCols(start, end - start)
147 .applyOnTheLeft(row_1, row_2, Eigen::JacobiRotation<t_Scalar>(cos, sin));
148 return;
149 }
150 }
151
152 private:
154 t_Scalar cos;
155 t_Scalar sin;
156
157
158 private:
159 inline void swap(t_Scalar &a, t_Scalar &b) const
160 {
161 std::swap(a, b);
162 }
163
164 inline void applyNonTrivial(t_Scalar &a, t_Scalar &b) const
165 {
166 t_Scalar t1 = a;
167 t_Scalar t2 = b;
168 a = t1 * cos + t2 * sin;
169 b = -sin * t1 + cos * t2;
170 }
171 };
172} // namespace qpmad
Represents Givens rotation.
Definition: givens.h:39
void applyColumnWise(t_MatrixType &M, const int start, const int end, const int column_1, const int column_2) const
Definition: givens.h:116
void applyRowWise(t_MatrixType &M, const int start, const int end, const int row_1, const int row_2) const
Definition: givens.h:136
void applyNonTrivial(t_Scalar &a, t_Scalar &b) const
Definition: givens.h:164
Type computeAndApply(t_Scalar &a, t_Scalar &b, const t_Scalar eps)
Definition: givens.h:50
void swap(t_Scalar &a, t_Scalar &b) const
Definition: givens.h:159
void apply(t_Scalar &a, t_Scalar &b) const
Definition: givens.h:100