Ctrl+K

Program listing for file numerics/src/FrictionContact/grfc3d_ipm.h

Program listing for file numerics/src/FrictionContact/grfc3d_ipm.h#

 1#include "JordanAlgebra.h"
 2
 3
 4static double getStepLength(const double *const x, const double *const dx,
 5                            const unsigned int vecSize, const unsigned int varsCount,
 6                            const double gamma) {
 7  int dimension = (int)(vecSize / varsCount);
 8  unsigned int pos;
 9  float_type aL, bL, cL, dL, alphaL, nxb;
10  double min_alpha;
11
12  min_alpha = 1e20;
13
14  for (unsigned int i = 0; i < varsCount; ++i) {
15    pos = i * dimension;
16    aL = dnrm2l(dimension - 1, dx + pos + 1);
17    aL = (dx[pos] - aL) * (dx[pos] + aL);
18    bL = x[pos] * dx[pos];
19    for (int k = 1; k < dimension; bL -= x[pos + k] * dx[pos + k], k++)
20      ;
21    nxb = dnrm2l(dimension - 1, x + pos + 1);
22
23    cL = x[pos] - nxb;
24    if (cL <= 0.)
25      cL = DBL_EPSILON * (x[pos] + nxb);
26    else
27      cL = (x[pos] - nxb) *
28           (x[pos] + nxb);
29    dL = bL * bL - aL * cL;
30    if (aL < 0 || (bL < 0 && dL > 0))
31      if (bL > 0)
32        alphaL = -(bL + sqrtl(dL)) / aL;
33      else
34        alphaL = cL / (-bL + sqrtl(dL));
35    else if ((fabsl(aL) == 0.0) && (bL < 0))
36      alphaL = -cL / bL / 2;
37    else
38      alphaL = DBL_MAX;
39    min_alpha = ((alphaL < min_alpha) ? alphaL : min_alpha);
40  }
41  min_alpha = gamma * min_alpha;
42  min_alpha = ((min_alpha < 1.0) ? min_alpha : 1.0);
43  return min_alpha;
44}
45
46
47static double relGap(NumericsMatrix *M, const double *f, const double *w,
48                     const double *globalVelocity, const double *reaction,
49                     const unsigned int nd, const unsigned int m, const double gapVal) {
50  double *Mv = (double *)calloc(m, sizeof(double));
51  double vMv, pval, dval;
52
53  NM_gemv(0.5, M, globalVelocity, 0.0, Mv);
54  vMv = cblas_ddot(m, globalVelocity, 1, Mv, 1);
55  free(Mv);
56  pval = vMv - cblas_ddot(m, f, 1, globalVelocity, 1);
57  dval = -vMv - cblas_ddot(nd, w, 1, reaction, 1);
58  return gapVal / (1 + fabs(pval) + fabs(dval));
59}
60
61
62static double complemResidualNorm(const double *const velocity, const double *const reaction,
63                                  const unsigned int vecSize, const unsigned int varsCount) {
64  double *resid = (double *)calloc(vecSize, sizeof(double));
65  JA_prod(velocity, reaction, vecSize, varsCount, resid);
66  double norm2 = cblas_dnrm2(vecSize, resid, 1);
67  free(resid);
68  return norm2;
69}
70
71NumericsMatrix *compute_JQinv2Jt(const double *u1, const double *r1, const double *u2,
72                                 const double *r2, const size_t vecSize,
73                                 const size_t varsCount);
74
75CS_INT cs_dupl_zeros(cs *A);