Geomi
GalerkinStepInternals.hpp
1 #ifndef DEF_VARIATIONAL_GALERKINSTEPINTERNALS
2 #define DEF_VARIATIONAL_GALERKINSTEPINTERNALS
3 
4 #include <vector>
5 
6 namespace Variational {
7 
114 template <typename T_M,
115  typename T_Q,
116  typename T_TQ,
117  int T_N_STEPS>
118 class GalerkinStepInternals:
119  public Abstract::StepInternals<T_M,T_Q,T_TQ>,
120  public ::Abstract::NOXStep<T_Q,T_N_STEPS>
121 {
122 protected:
125  std::vector<T_Q> m_v_cur_q;
128  std::vector<T_Q> m_v_prev_q;
129  LagrangeInterpolation<T_Q> m_interp;
130  int m_quad_deg;
131 
132 public:
133  GalerkinStepInternals<T_M,T_Q,T_TQ,T_N_STEPS>
134  (Abstract::Problem<T_M,T_Q>& problem, int quad_deg)
135  : Abstract::StepInternals<T_M,T_Q,T_TQ>(problem)
136  {
137  m_interp = LagrangeInterpolation<T_Q>();
138  m_quad_deg = quad_deg;
139  m_v_cur_q = std::vector<T_Q>(T_N_STEPS+1);
140  m_v_prev_q = std::vector<T_Q>(T_N_STEPS+1);
141  }
142 
143  void
144  setData (T_M h, T_Q q0, T_Q q1)
145  {
146  m_v_prev_q[0] = q0;
147  m_v_prev_q[T_N_STEPS] = q1;
148  this->m_h = h;
149  }
150 
151  const NOXVector<T_Q::DOF*T_N_STEPS>
152  getInitialGuess ()
153  {
154  NOXVector<T_Q::DOF*T_N_STEPS> ret;
155  T_Q q0 = m_v_prev_q[0];
156  T_Q q1 = m_v_prev_q[T_N_STEPS];
157 
158  for (int i=1; i<=T_N_STEPS; i++) {
159  ret.segment((i-1)*T_Q::DOF,T_Q::DOF) =
160  NOXVector<T_Q::DOF>(q1+(float(i)/(T_N_STEPS*this->m_h))*(q1-q0));
161  }
162  return ret;
163  }
164 
165  const NOXVector<T_Q::DOF*(T_N_STEPS-1)>
166  initGetInitialGuess ()
167  {
168  NOXVector<T_Q::DOF*(T_N_STEPS-1)> ret;
169  T_Q q0 = m_v_prev_q[0];
170  T_Q q1 = m_v_prev_q[T_N_STEPS];
171 
172  for (int i=1; i<T_N_STEPS; i++) {
173  ret.segment((i-1)*T_Q::DOF,T_Q::DOF) =
174  NOXVector<T_Q::DOF>(q0+(float(i)/(T_N_STEPS*this->m_h))*(q1-q0));
175  }
176  return ret;
177  }
178 
179  void
180  updatePosition (const NOXVector<T_Q::DOF*T_N_STEPS>& q)
181  {
182  m_v_prev_q[0] = m_v_prev_q[T_N_STEPS];
183 
184  for (int i=1; i<=T_N_STEPS; i++) {
185  m_v_prev_q[i] = T_Q(q.segment((i-1)*T_Q::DOF,T_Q::DOF));
186  }
187 
188  m_v_cur_q[0] = m_v_prev_q[T_N_STEPS];
189  }
190 
191  void
192  updateInitialPosition (const NOXVector<T_Q::DOF*(T_N_STEPS-1)>& q)
193  {
194  for (int i=1; i<T_N_STEPS; i++) {
195  m_v_prev_q[i]= T_Q(q.segment((i-1)*T_Q::DOF,T_Q::DOF));
196  }
197 
198  m_v_cur_q[0] = m_v_prev_q[T_N_STEPS];
199  }
200 
201  bool
202  computeF ( NOXVector<T_Q::DOF*T_N_STEPS>& f,
203  const NOXVector<T_Q::DOF*T_N_STEPS>& q)
204  {
205  int nu; // index polynome
206  int k; // index date
207 
208 
209  // this one is not supposed to have changed, but just in case...
210  m_v_cur_q[0] = m_v_prev_q[T_N_STEPS];
211  for (nu=0; nu<T_N_STEPS; nu++) {
212  m_v_cur_q[nu+1] = T_Q(q.segment(nu*T_Q::DOF,T_Q::DOF));
213  }
214 
215  int r = m_quad_deg;
216  std::vector<double> w = GaussLegendre::weights(r);
217  std::vector<double> c = GaussLegendre::dates(r);
218 
219 
220  std::vector<std::vector<double>> vv_lag =
221  this->m_interp.polynomials(T_N_STEPS,c);
222  std::vector<std::vector<double>> vv_lag_der =
223  this->m_interp.polynomials_derivatives(T_N_STEPS,c);
224 
225 
226  std::vector<T_Q> v_pos_interp =
227  m_interp.pos_interp(T_N_STEPS,c,m_v_cur_q);
228  std::vector<T_Q> v_vel_interp =
229  m_interp.vel_interp(T_N_STEPS,c,m_v_cur_q,this->m_h);
230  std::vector<T_Q> v_prev_pos_interp =
231  m_interp.pos_interp(T_N_STEPS,c,m_v_prev_q);
232  std::vector<T_Q> v_prev_vel_interp =
233  m_interp.vel_interp(T_N_STEPS,c,m_v_prev_q,this->m_h);
234 
235  std::vector<NOXVector<T_Q::DOF>> v_cur_dLdq;
236  std::vector<NOXVector<T_Q::DOF>> v_cur_dLdv;
237  std::vector<NOXVector<T_Q::DOF>> v_prev_dLdq;
238  std::vector<NOXVector<T_Q::DOF>> v_prev_dLdv;
239 
240  for (k=0; k<r; k++) {
241  v_cur_dLdq.push_back(
242  this->m_problem.dLdq(v_pos_interp[k], v_vel_interp[k]));
243  v_cur_dLdv.push_back(
244  this->m_problem.dLdv(v_pos_interp[k], v_vel_interp[k]));
245  v_prev_dLdq.push_back(
246  this->m_problem.dLdq(v_prev_pos_interp[k],v_prev_vel_interp[k]));
247  v_prev_dLdv.push_back(
248  this->m_problem.dLdv(v_prev_pos_interp[k],v_prev_vel_interp[k]));
249  }
250 
251 
252  NOXVector<T_Q::DOF> somme = NOXVector<T_Q::DOF>::Zero();
253  // DEL
254  for (k=0; k<r; k++) {
255  somme +=
256  w[k]*( this->m_h*vv_lag[k][0]*v_cur_dLdq[k]
257  +vv_lag_der[k][0]*v_cur_dLdv[k]
258  +this->m_h*vv_lag[k][T_N_STEPS]*v_prev_dLdq[k]
259  +vv_lag_der[k][T_N_STEPS]*v_prev_dLdv[k]);
260  }
261  f.head(T_Q::DOF) = somme;
262 
263  // Internal equations
264  for (nu=1;nu<T_N_STEPS;nu++) {
265  somme = NOXVector<T_Q::DOF>::Zero();
266  for (k=0;k<r;k++) {
267  somme += w[k]*( this->m_h*vv_lag[k][nu]*v_cur_dLdq[k]
268  +vv_lag_der[k][nu]*v_cur_dLdv[k]);
269  }
270  f.segment(nu*T_Q::DOF,T_Q::DOF) = somme;
271  }
272 
273  return true;
274  }
275 
276  bool
277  computeInitF ( NOXVector<T_Q::DOF*(T_N_STEPS-1)>& f,
278  const NOXVector<T_Q::DOF*(T_N_STEPS-1)>& q)
279  {
280  int nu; // index polynome
281  int k; // index date
282 
283 
284  for (nu=0; nu<T_N_STEPS-1; nu++) {
285  m_v_prev_q[nu+1] = T_Q(q.segment(nu*T_Q::DOF,T_Q::DOF));
286  }
287 
288  int r = m_quad_deg;
289  std::vector<double> w = GaussLegendre::weights(r);
290  std::vector<double> c = GaussLegendre::dates(r);
291 
292  std::vector<std::vector<double>> vv_lag =
293  this->m_interp.polynomials(T_N_STEPS,c);
294  std::vector<std::vector<double>> vv_lag_der =
295  this->m_interp.polynomials_derivatives(T_N_STEPS,c);
296 
297 
298  std::vector<T_Q> v_pos_interp =
299  m_interp.pos_interp(T_N_STEPS,c,m_v_prev_q);
300  std::vector<T_Q> v_vel_interp =
301  m_interp.vel_interp(T_N_STEPS,c,m_v_prev_q,this->m_h);
302 
303 
304  std::vector<NOXVector<T_Q::DOF>> v_prev_dLdq;
305  std::vector<NOXVector<T_Q::DOF>> v_prev_dLdv;
306 
307 
308  for (k=0; k<r; k++) {
309  v_prev_dLdq.push_back(
310  this->m_problem.dLdq(v_pos_interp[k],v_vel_interp[k]));
311  v_prev_dLdv.push_back(
312  this->m_problem.dLdv(v_pos_interp[k],v_vel_interp[k]));
313  }
314 
315  NOXVector<T_Q::DOF> somme;
316  for (nu=1;nu<T_N_STEPS;nu++) {
317  somme = NOXVector<T_Q::DOF>::Zero();
318  for (k=0;k<r;k++) {
319  somme += w[k]*( this->m_h*vv_lag[k][nu]*v_prev_dLdq[k]
320  +vv_lag_der[k][nu]*v_prev_dLdv[k]);
321  }
322  f.segment((nu-1)*T_Q::DOF,T_Q::DOF) = somme;
323  }
324 
325  return true;
326  }
327 
333  bool
334  computeJacobian (
335  Eigen::Matrix<double,T_Q::DOF*T_N_STEPS,T_Q::DOF*T_N_STEPS>& J,
336  const NOXVector<T_Q::DOF*T_N_STEPS>& q)
337  {
338  int nu; // index polynome
339  int k; // index date
340  int i; // jacobian line index
341  int j; // jacobian column index
342 
343 
344  // this one is not supposed to have changed, but just in case...
345  m_v_cur_q[0] = m_v_prev_q[T_N_STEPS];
346  for (nu=0; nu<T_N_STEPS; nu++) {
347  m_v_cur_q[nu+1] = T_Q(q.segment(nu*T_Q::DOF,T_Q::DOF));
348  }
349 
350 
351  int r = m_quad_deg;
352  std::vector<double> w = GaussLegendre::weights(r);
353  std::vector<double> c = GaussLegendre::dates(r);
354 
355 
356  std::vector<std::vector<double>> vv_lag =
357  this->m_interp.polynomials(T_N_STEPS,c);
358  std::vector<std::vector<double>> vv_lag_der =
359  this->m_interp.polynomials_derivatives(T_N_STEPS,c);
360 
361 
362  std::vector<T_Q> v_pos_interp =
363  m_interp.pos_interp(T_N_STEPS,c,m_v_cur_q);
364  std::vector<T_Q> v_vel_interp =
365  m_interp.vel_interp(T_N_STEPS,c,m_v_cur_q,this->m_h);
366  std::vector<T_Q> v_prev_pos_interp =
367  m_interp.pos_interp(T_N_STEPS,c,m_v_prev_q);
368  std::vector<T_Q> v_prev_vel_interp =
369  m_interp.vel_interp(T_N_STEPS,c,m_v_prev_q,this->m_h);
370 
371  std::vector<Eigen::Matrix<double,T_Q::DOF,T_Q::DOF>> v_JqdLdq;
372  std::vector<Eigen::Matrix<double,T_Q::DOF,T_Q::DOF>> v_JqdLdv;
373  std::vector<Eigen::Matrix<double,T_Q::DOF,T_Q::DOF>> v_JvdLdq;
374  std::vector<Eigen::Matrix<double,T_Q::DOF,T_Q::DOF>> v_JvdLdv;
375 
376  for (k=0; k<r; k++) {
377  v_JqdLdq.push_back(
378  this->m_problem.JqdLdq(v_pos_interp[k],v_vel_interp[k]));
379  v_JqdLdv.push_back(
380  this->m_problem.JqdLdv(v_pos_interp[k],v_vel_interp[k]));
381  v_JvdLdq.push_back(
382  this->m_problem.JvdLdq(v_pos_interp[k],v_vel_interp[k]));
383  v_JvdLdv.push_back(
384  this->m_problem.JvdLdv(v_pos_interp[k],v_vel_interp[k]));
385  }
386 
387  Eigen::Matrix<double,T_Q::DOF,T_Q::DOF> somme =
388  Eigen::Matrix<double,T_Q::DOF,T_Q::DOF>::Zero();
389 
390  for (j=1; j<=T_N_STEPS; j++) {
391  for (i=0; i<T_N_STEPS; i++) {
392  somme = Eigen::Matrix<double,T_Q::DOF,T_Q::DOF>::Zero();
393  for (k=0; k<r; k++) {
394  somme +=
395  w[k]*(vv_lag[k][i]*(
396  this->m_h*vv_lag[k][j]*v_JqdLdq[k]
397  +vv_lag_der[k][j]*v_JvdLdq[k])
398  + vv_lag_der[k][i]*(
399  vv_lag[k][j]*v_JqdLdv[k]
400  +(vv_lag_der[k][j]/this->m_h)*v_JvdLdv[k]));
401  }
402  J.block(i*T_Q::DOF,(j-1)*T_Q::DOF,T_Q::DOF,T_Q::DOF) = somme;
403  }
404  }
405 
406  return true;
407  }
408 
409  bool
410  computeInitJacobian (
411  Eigen::Matrix<double,T_Q::DOF*(T_N_STEPS-1),T_Q::DOF*(T_N_STEPS-1)>& J,
412  const NOXVector<T_Q::DOF*(T_N_STEPS-1)>& q)
413  {
414  int nu; // index polynome
415  int k; // index date
416  int i; // jacobian line index
417  int j; // jacobian column index
418 
419 
420  // don't touch q[0] and q[T_N_STEPS]
421  for (nu=0; nu<T_N_STEPS-1; nu++) {
422  m_v_prev_q[nu+1] = T_Q(q.segment(nu*T_Q::DOF,T_Q::DOF));
423  }
424 
425 
426  int r = m_quad_deg;
427  std::vector<double> w = GaussLegendre::weights(r);
428  std::vector<double> c = GaussLegendre::dates(r);
429 
430 
431  std::vector<std::vector<double>> vv_lag =
432  this->m_interp.polynomials(T_N_STEPS,c);
433  std::vector<std::vector<double>> vv_lag_der =
434  this->m_interp.polynomials_derivatives(T_N_STEPS,c);
435 
436 
437  std::vector<T_Q> v_pos_interp =
438  m_interp.pos_interp(T_N_STEPS,c,m_v_prev_q);
439  std::vector<T_Q> v_vel_interp =
440  m_interp.vel_interp(T_N_STEPS,c,m_v_prev_q,this->m_h);
441 
442  std::vector<Eigen::Matrix<double,T_Q::DOF,T_Q::DOF>> v_JqdLdq;
443  std::vector<Eigen::Matrix<double,T_Q::DOF,T_Q::DOF>> v_JqdLdv;
444  std::vector<Eigen::Matrix<double,T_Q::DOF,T_Q::DOF>> v_JvdLdq;
445  std::vector<Eigen::Matrix<double,T_Q::DOF,T_Q::DOF>> v_JvdLdv;
446 
447  for (k=0; k<r; k++) {
448  v_JqdLdq.push_back(
449  this->m_problem.JqdLdq(v_pos_interp[k],v_vel_interp[k]));
450  v_JqdLdv.push_back(
451  this->m_problem.JqdLdv(v_pos_interp[k],v_vel_interp[k]));
452  v_JvdLdq.push_back(
453  this->m_problem.JvdLdq(v_pos_interp[k],v_vel_interp[k]));
454  v_JvdLdv.push_back(
455  this->m_problem.JvdLdv(v_pos_interp[k],v_vel_interp[k]));
456  }
457 
458  Eigen::Matrix<double,T_Q::DOF,T_Q::DOF> somme =
459  Eigen::Matrix<double,T_Q::DOF,T_Q::DOF>::Zero();
460 
461  for (j=1; j<T_N_STEPS; j++) {
462  for (i=1; i<T_N_STEPS; i++) {
463  somme = Eigen::Matrix<double,T_Q::DOF,T_Q::DOF>::Zero();
464  for (k=0; k<r; k++) {
465  somme += w[k]*(vv_lag[k][i]*(
466  this->m_h*vv_lag[k][j]*v_JqdLdq[k]
467  +vv_lag_der[k][j]*v_JvdLdq[k])
468  + vv_lag_der[k][i]*(
469  vv_lag[k][j]*v_JqdLdv[k]
470  +(vv_lag_der[k][j]/this->m_h)*v_JvdLdv[k]));
471  }
472  J.block((i-1)*T_Q::DOF,(j-1)*T_Q::DOF,T_Q::DOF,T_Q::DOF) = somme;
473  }
474  }
475 
476  return true;
477  }
478 };
479 
480 template <typename T_M,
481  typename T_Q,
482  typename T_TQ,
483  int T_N_STEPS>
484 class GalerkinStepInitWrapper:
485  public ::Abstract::NOXStep<T_Q,T_N_STEPS-1>
486 {
487 private:
488  GalerkinStepInternals<T_M,T_Q,T_TQ,T_N_STEPS>* m_internals;
489 
490 public:
491  GalerkinStepInitWrapper<T_M,T_Q,T_TQ,T_N_STEPS>
492  (GalerkinStepInternals<T_M,T_Q,T_TQ,T_N_STEPS>* internals)
493  { m_internals = internals; }
494 
495  const NOXVector<T_Q::DOF*(T_N_STEPS-1)>
496  getInitialGuess ()
497  { return m_internals->initGetInitialGuess(); }
498 
499  void
500  updateInitialPosition (const NOXVector<T_Q::DOF*(T_N_STEPS-1)>& q)
501  { m_internals->updateInitialPosition(q); }
502 
503  bool
504  computeF (
505  NOXVector<T_Q::DOF*(T_N_STEPS-1)>& f,
506  const NOXVector<T_Q::DOF*(T_N_STEPS-1)>& q)
507  { return m_internals->computeInitF(f,q); }
508 
509  bool
510  computeJacobian (
511  Eigen::Matrix<double,T_Q::DOF*(T_N_STEPS-1),T_Q::DOF*(T_N_STEPS-1)>& J,
512  const NOXVector<T_Q::DOF*(T_N_STEPS-1)>& q)
513  { return m_internals->computeInitJacobian(J,q); }
514 };
515 
516 } // namespace Variational
517 
518 #endif
Variational::GalerkinStepInitWrapper
Definition: GalerkinStepInternals.hpp:484
Variational::Abstract::Problem
Definition: Abstract_Problem.hpp:10
Variational::GalerkinStepInternals::m_v_cur_q
std::vector< T_Q > m_v_cur_q
Definition: GalerkinStepInternals.hpp:125
Variational
Definition: Abstract_LieProblem.hpp:6
Variational::GalerkinStepInternals::computeJacobian
bool computeJacobian(Eigen::Matrix< double, T_Q::DOF *T_N_STEPS, T_Q::DOF *T_N_STEPS > &J, const NOXVector< T_Q::DOF *T_N_STEPS > &q)
Definition: GalerkinStepInternals.hpp:334
Variational::Abstract::StepInternals
Definition: Abstract_StepInternals.hpp:13
Variational::GalerkinStepInternals
Definition: GalerkinStepInternals.hpp:118
LagrangeInterpolation
Definition: LagrangeInterpolation.hpp:152
Variational::GalerkinStepInternals::m_v_prev_q
std::vector< T_Q > m_v_prev_q
Definition: GalerkinStepInternals.hpp:128
NOXVector
Definition: NOXVector.hpp:14
Abstract::NOXStep
Definition: Abstract_NOXStep.hpp:12
Generated by   doxygen 1.8.13