qpmad
Eigen-based C++ QP solver.
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 
14 namespace 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 
79  type = NONTRIVIAL;
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:38
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 apply(t_Scalar &a, t_Scalar &b) const
Definition: givens.h:100
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 swap(t_Scalar &a, t_Scalar &b) const
Definition: givens.h:159
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