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,
117  const int start,
118  const int end,
119  const int column_1,
120  const int column_2) const
121  {
122  switch (type)
123  {
124  case COPY:
125  return;
126  case SWAP:
127  M.col(column_1).segment(start, end-start).swap(
128  M.col(column_2).segment(start, end-start) );
129  return;
130  case NONTRIVIAL:
131  M.middleRows(start, end - start).transpose().applyOnTheLeft(
132  column_1, column_2,
133  Eigen::JacobiRotation<t_Scalar> (cos, sin));
134  return;
135  }
136  }
137 
138 
139  template<class t_MatrixType>
140  void applyRowWise( t_MatrixType &M,
141  const int start,
142  const int end,
143  const int row_1,
144  const int row_2) const
145  {
146  switch (type)
147  {
148  case COPY:
149  return;
150  case SWAP:
151  M.row(row_1).segment(start, end-start).swap(
152  M.row(row_2).segment(start, end-start) );
153  return;
154  case NONTRIVIAL:
155  M.middleCols(start, end - start).applyOnTheLeft(
156  row_1, row_2,
157  Eigen::JacobiRotation<t_Scalar> (cos, sin));
158  return;
159  }
160  }
161 
162  private:
164  t_Scalar cos;
165  t_Scalar sin;
166 
167 
168  private:
169  inline void swap(t_Scalar & a, t_Scalar & b) const
170  {
171  std::swap(a,b);
172  }
173 
174  inline void applyNonTrivial(t_Scalar & a, t_Scalar & b) const
175  {
176  t_Scalar t1 = a;
177  t_Scalar t2 = b;
178  a = t1 * cos + t2 * sin;
179  b = - sin * t1 + cos * t2;
180  }
181  };
182 }
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:140
void swap(t_Scalar &a, t_Scalar &b) const
Definition: givens.h:169
void applyNonTrivial(t_Scalar &a, t_Scalar &b) const
Definition: givens.h:174
Type computeAndApply(t_Scalar &a, t_Scalar &b, const t_Scalar eps)
Definition: givens.h:50