GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/seek/descend.c Lines: 0 443 0.0 %
Date: 2017-05-26 Branches: 0 156 0.0 %

Line Branch Exec Source
1
/*
2
  Teem: Tools to process and visualize scientific data and images             .
3
  Copyright (C) 2011, 2010, 2009, 2008  Thomas Schultz
4
5
  This library is free software; you can redistribute it and/or
6
  modify it under the terms of the GNU Lesser General Public License
7
  (LGPL) as published by the Free Software Foundation; either
8
  version 2.1 of the License, or (at your option) any later version.
9
  The terms of redistributing and/or modifying this software also
10
  include exceptions to the LGPL that facilitate static linking.
11
12
  This library is distributed in the hope that it will be useful,
13
  but WITHOUT ANY WARRANTY; without even the implied warranty of
14
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
15
  Lesser General Public License for more details.
16
17
  You should have received a copy of the GNU Lesser General Public License
18
  along with this library; if not, write to Free Software Foundation, Inc.,
19
  51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
20
*/
21
22
/* This file collects functions that implement various gradient
23
 * descents required in the context of extracting crease surfaces */
24
25
#include "seek.h"
26
#include "privateSeek.h"
27
28
/* Tries to find a degenerate point on a bilinearly interpolated face
29
 * of symmetric second-order tensors. Uses the discriminant constraint
30
 * functions from Zheng/Parlett/Pang, TVCG 2005, and the
31
 * Newton-Raphson method with Armijo stepsize control.
32
 *
33
 * coord are coordinates relative to the given face and will be
34
 *      updated in each iteration.
35
 * botleft - topright are 9-vectors, representing the symmetric
36
 *      matrices at the corners of the face (input only)
37
 * maxiter is the maximum number of iterations allowed
38
 * eps is the accuracy up to which the discriminant should be zero
39
 *     The discriminant scales with the Frobenius norm to the sixth power,
40
 *     so the exact constraint is disc/|T|^6<eps
41
 * type can be 'l', 'p', or something else (=both)
42
 *
43
 * Returns 0 if the point was found up to the given accuracy
44
 * Returns 1 if we left the face
45
 * Returns 2 if we hit maxiter
46
 * Returns 3 if we could not invert a matrix to find next gradient dir
47
 * Returns 4 if we found a point, but it does not have the desired type
48
 * Returns 5 if Armijo rule failed to find a valid stepsize
49
 * Returns 6 if we hit a zero tensor (|T|<1e-300)
50
 */
51
int seekDescendToDeg(double *coord, double *botleft, double *botright,
52
                     double *topleft, double *topright,
53
                     int maxiter, double eps, char type)
54
{
55
  double discr; /* store discriminant value of previous iteration */
56
  double hesstop[9]; /* used to interpolate Hessian */
57
  double hessbot[9];
58
  double hess[9];
59
  double ten[6]; /* makes access more convenient */
60
  double tsqr[6]; /* squared tensor values, are used repeatedly */
61
  double cf[7]; /* constraint function vector */
62
  double norm; /* Frobenius norm, for normalization */
63
  int i, j, iter; /* counting variables for loops */
64
65
  /* check initial point */
66
  ELL_3M_LERP(hessbot,coord[0],botleft,botright);
67
  ELL_3M_LERP(hesstop,coord[0],topleft,topright);
68
  ELL_3M_LERP(hess,coord[1],hessbot,hesstop);
69
70
  /* normalize for scale invariance & to avoid numerical problems */
71
  norm = sqrt(hess[0]*hess[0]+hess[4]*hess[4]+hess[8]*hess[8]+
72
              2*(hess[1]*hess[1]+hess[2]*hess[2]+hess[5]*hess[5]));
73
  if (norm<1e-300) return 6;
74
  ten[0] = hess[0]/norm; ten[1] = hess[1]/norm; ten[2] = hess[2]/norm;
75
  ten[3] = hess[4]/norm; ten[4] = hess[5]/norm; ten[5] = hess[8]/norm;
76
  for (i=0; i<6; i++)
77
    tsqr[i] = ten[i]*ten[i];
78
79
  /* evaluate the constraint function vector */
80
  cf[0] = ten[0]*(tsqr[3]-tsqr[5])+ten[0]*(tsqr[1]-tsqr[2])+
81
    ten[3]*(tsqr[5]-tsqr[0])+ten[3]*(tsqr[4]-tsqr[1])+
82
    ten[5]*(tsqr[0]-tsqr[3])+ten[5]*(tsqr[2]-tsqr[4]); /* fx */
83
  cf[1] = ten[4]*(2*(tsqr[4]-tsqr[0])-(tsqr[2]+tsqr[1])+
84
                  2*(ten[3]*ten[0]+ten[5]*ten[0]-ten[3]*ten[5]))+
85
    ten[1]*ten[2]*(2*ten[0]-ten[5]-ten[3]); /* fy1 */
86
  cf[2] = ten[2]*(2*(tsqr[2]-tsqr[3])-(tsqr[1]+tsqr[4])+
87
                  2*(ten[5]*ten[3]+ten[0]*ten[3]-ten[5]*ten[0]))+
88
    ten[4]*ten[1]*(2*ten[3]-ten[0]-ten[5]); /* fy2 */
89
  cf[3] = ten[1]*(2*(tsqr[1]-tsqr[5])-(tsqr[4]+tsqr[2])+
90
                  2*(ten[0]*ten[5]+ten[3]*ten[5]-ten[0]*ten[3]))+
91
    ten[2]*ten[4]*(2*ten[5]-ten[3]-ten[0]); /* fy3 */
92
  cf[4] = ten[4]*(tsqr[2]-tsqr[1])+ten[1]*ten[2]*(ten[3]-ten[5]); /* fz1 */
93
  cf[5] = ten[2]*(tsqr[1]-tsqr[4])+ten[4]*ten[1]*(ten[5]-ten[0]); /* fz2 */
94
  cf[6] = ten[1]*(tsqr[4]-tsqr[2])+ten[2]*ten[4]*(ten[0]-ten[3]); /* fz3 */
95
96
  discr = cf[0]*cf[0]+cf[1]*cf[1]+cf[2]*cf[2]+cf[3]*cf[3]+
97
    15*(cf[4]*cf[4]+cf[5]*cf[5]+cf[6]*cf[6]);
98
  if (discr<eps) {
99
    if (type!='l' && type!='p') return 0;
100
    else {
101
      /* check if type is correct */
102
      double dev[9], det;
103
      double mean = (ten[0]+ten[3]+ten[5])/3;
104
      dev[0] = ten[0]-mean; dev[1] = ten[1]; dev[2] = ten[2];
105
      dev[3] = ten[1]; dev[4]=ten[3]-mean; dev[5] = ten[4];
106
      dev[6] = ten[2]; dev[7]=ten[4]; dev[8]=ten[5]-mean;
107
      det = ELL_3M_DET(dev);
108
      if ((type=='l' && det>0) || (type=='p' && det<0)) return 0;
109
      else return 4; /* sufficient accuracy, but wrong type  */
110
    }
111
  }
112
113
  for (iter=0; iter<maxiter; iter++) {
114
    /* find derivative of constraint function vector using the chain rule */
115
    double cft[42]; /* derive relative to tensor values, 7x6 matrix */
116
    double tx[12]; /* spatial derivative of tensor values, 6x2 matrix */
117
    double cfx[14]; /* spatial derivative of constraint funct., 7x2 matrix */
118
    double denom[3], det; /* symmetric 2x2 matrix that is to be inverted */
119
    double inv[3]; /* inverse of that matrix */
120
    double nom[2], dx[2]; /* more helpers to compute next step */
121
    /* variables needed for Armijo stepsize rule */
122
    double beta=1, _gamma=0.5, alpha=beta; /* parameters */
123
    int accept=0, safetyct=0, maxct=30; /* counters */
124
    double dxsqr; /* squared length of stepsize */
125
    double hessleft[9], hessright[9]; /* used to compute Hessian derivative */
126
    double hessder[9];
127
    int row, col;
128
129
    cft [0] = tsqr[3]-tsqr[5]+tsqr[1]-tsqr[2]-2*ten[0]*ten[3]+2*ten[0]*ten[5];
130
    /* fx/T00 */
131
    cft [1] = 2*ten[0]*ten[1]-2*ten[3]*ten[1]; /* fx/T01 */
132
    cft [2] = -2*ten[0]*ten[2]+2*ten[5]*ten[2]; /* fx/T02 */
133
    cft [3] = 2*ten[0]*ten[3]+tsqr[5]-tsqr[0]+tsqr[4]-tsqr[1]-2*ten[5]*ten[3];
134
    /* fx/T11 */
135
    cft [4] = 2*ten[3]*ten[4]-2*ten[5]*ten[4]; /* fx/T12 */
136
    cft [5] = -2*ten[0]*ten[5]+2*ten[3]*ten[5]+tsqr[0]-tsqr[3]+tsqr[2]-tsqr[4];
137
    /* fx/T22 */
138
139
    cft [6] = -4*ten[0]*ten[4]+2*ten[4]*ten[3]+2*ten[4]*ten[5]+2*ten[1]*ten[2];
140
    /* fy1/T00 */
141
    cft [7] = -2*ten[4]*ten[1]+ten[2]*(2*ten[0]-ten[5]-ten[3]); /* fy1/T01 */
142
    cft [8] = -2*ten[4]*ten[2]+ten[1]*(2*ten[0]-ten[5]-ten[3]); /* fy1/T02 */
143
    cft [9] = 2*ten[4]*ten[0]-2*ten[4]*ten[5]-ten[1]*ten[2]; /* fy1/T11 */
144
    cft[10] = 6*tsqr[4]-2*tsqr[0]-(tsqr[2]+tsqr[1])+
145
      2*(ten[3]*ten[0]+ten[5]*ten[0]-ten[3]*ten[5]); /* fy1/T12 */
146
    cft[11] = 2*ten[4]*ten[0]-2*ten[4]*ten[3]-ten[1]*ten[2]; /* fy1/T22 */
147
148
    cft[12] = 2*ten[2]*ten[3]-2*ten[2]*ten[5]-ten[4]*ten[1]; /* fy2/T00 */
149
    cft[13] = -2*ten[2]*ten[1]+ten[4]*(2*ten[3]-ten[0]-ten[5]); /* fy2/T01 */
150
    cft[14] = 6*tsqr[2]-2*tsqr[3]-(tsqr[1]+tsqr[4])+
151
      2*(ten[5]*ten[3]+ten[0]*ten[3]-ten[5]*ten[0]); /* fy2/T02 */
152
    cft[15] = -4*ten[2]*ten[3]+2*ten[2]*ten[5]+2*ten[2]*ten[0]+2*ten[4]*ten[1];
153
    /* fy2/T11 */
154
    cft[16] = -2*ten[2]*ten[4]+ten[1]*(2*ten[3]-ten[0]-ten[5]); /* fy2/T12 */
155
    cft[17] = 2*ten[2]*ten[3]-2*ten[2]*ten[0]-ten[4]*ten[1]; /* fy2/T22 */
156
157
    cft[18] = 2*ten[1]*ten[5]-2*ten[1]*ten[3]-ten[2]*ten[4]; /* fy3/T00 */
158
    cft[19] = 6*tsqr[1]-2*tsqr[5]-(tsqr[4]+tsqr[2])+
159
      2*(ten[0]*ten[5]+ten[3]*ten[5]-ten[0]*ten[3]); /* fy3/T01 */
160
    cft[20] = -2*ten[1]*ten[2]+ten[4]*(2*ten[5]-ten[3]-ten[0]); /* fy3/T02 */
161
    cft[21] = 2*ten[1]*ten[5]-2*ten[1]*ten[0]-ten[2]*ten[4]; /* fy3/T11 */
162
    cft[22] = -2*ten[1]*ten[4]+ten[2]*(2*ten[5]-ten[3]-ten[0]); /* fy3/T12 */
163
    cft[23] = -4*ten[1]*ten[5]+2*ten[0]*ten[1]+2*ten[1]*ten[3]+2*ten[2]*ten[4];
164
    /* fy3/T22 */
165
166
    cft[24] = 0; /* fz1/T00 */
167
    cft[25] = -2*ten[4]*ten[1]+ten[2]*(ten[3]-ten[5]); /* fz1/T01 */
168
    cft[26] = 2*ten[4]*ten[2]+ten[1]*(ten[3]-ten[5]); /* fz1/T02 */
169
    cft[27] = ten[1]*ten[2]; /* fz1/T11 */
170
    cft[28] = tsqr[2]-tsqr[1]; /* fz1/T12 */
171
    cft[29] = -ten[1]*ten[2]; /* fz1/T22 */
172
173
    cft[30] = -ten[4]*ten[1]; /* fz2/T00 */
174
    cft[31] = 2*ten[2]*ten[1]+ten[4]*(ten[5]-ten[0]); /* fz2/T01 */
175
    cft[32] = tsqr[1]-tsqr[4]; /* fz2/T02 */
176
    cft[33] = 0; /* fz2/T11 */
177
    cft[34] = -2*ten[2]*ten[4]+ten[1]*(ten[5]-ten[0]); /* fz2/T12 */
178
    cft[35] = ten[4]*ten[1]; /* fz2/T22 */
179
180
    cft[36] = ten[2]*ten[4]; /* fz3/T00 */
181
    cft[37] = tsqr[4]-tsqr[2]; /* fz3/T01 */
182
    cft[38] = -2*ten[1]*ten[2]+ten[4]*(ten[0]-ten[3]); /* fz3/T02 */
183
    cft[39] = -ten[2]*ten[4]; /* fz3/T11 */
184
    cft[40] = 2*ten[1]*ten[4]+ten[2]*(ten[0]-ten[3]); /* fz3/T12 */
185
    cft[41] = 0; /* fz3/T22 */
186
187
    /* approximate Hessian derivative in x dir */
188
    ELL_3M_LERP(hessleft,coord[1],botleft,topleft);
189
    ELL_3M_LERP(hessright,coord[1],botright,topright);
190
    ELL_3M_SUB(hessder,hessright,hessleft);
191
    ELL_3M_SCALE(hessder,1.0/norm,hessder);
192
193
    tx[0] = hessder[0]; /* T00 / x */
194
    tx[2] = hessder[1]; /* T01 / x */
195
    tx[4] = hessder[2]; /* T02 / x */
196
    tx[6] = hessder[4]; /* T11 / x */
197
    tx[8] = hessder[5]; /* T12 / x */
198
    tx[10] = hessder[8]; /* T22 / x */
199
200
    /* approximate Hessian derivative in z dir */
201
    ELL_3M_SUB(hessder,hesstop,hessbot);
202
    ELL_3M_SCALE(hessder,1.0/norm,hessder);
203
204
    tx[1] = hessder[0]; /* T00 / z */
205
    tx[3] = hessder[1]; /* T01 / z */
206
    tx[5] = hessder[2]; /* T02 / z */
207
    tx[7] = hessder[4]; /* T11 / z */
208
    tx[9] = hessder[5]; /* T12 / z */
209
    tx[11] = hessder[8]; /* T22 / z */
210
211
    /* matrix multiply cft*tx */
212
    for (row=0; row<7; row++)
213
      for (col=0; col<2; col++) {
214
        i = row*2+col;
215
        cfx[i] = 0;
216
        for (j=0; j<6; j++) {
217
          cfx[i] += cft[row*6+j]*tx[j*2+col];
218
        }
219
      }
220
221
    for (i=0; i<3; i++)
222
      denom[i] = 0;
223
    for (j=0; j<7; j++) {
224
      denom[0] += cfx[j*2]*cfx[j*2];
225
      denom[1] += cfx[j*2+1]*cfx[j*2];
226
      denom[2] += cfx[j*2+1]*cfx[j*2+1];
227
    }
228
229
    det = denom[0]*denom[2]-denom[1]*denom[1];
230
    if (fabs(det)<DBL_EPSILON)
231
      return 3;
232
233
    inv[0] = denom[2] / det;
234
    inv[1] = -denom[1] / det;
235
    inv[2] = denom[0] / det;
236
237
    /* multiply transpose(cfx)*cf */
238
    nom[0]=0; nom[1]=0;
239
    for (j=0; j<7; j++) {
240
      nom[0] += cfx[j*2]   * cf[j];
241
      nom[1] += cfx[j*2+1] * cf[j];
242
    }
243
244
    /* calculate the coordinate offset dx = inv*nom */
245
    dx[0] = inv[0]*nom[0]+inv[1]*nom[1];
246
    dx[1] = inv[1]*nom[0]+inv[2]*nom[1];
247
248
    /* employ the Armijo stepsize rule for improved convergence */
249
    dxsqr = dx[0]*dx[0]+dx[1]*dx[1];
250
    while (!accept && safetyct++<maxct) {
251
      /* test discriminant at new position */
252
      double newcoord[2];
253
      double newdiscr;
254
      ELL_2V_SET(newcoord, coord[0]-alpha*dx[0], coord[1]-alpha*dx[1]);
255
256
      if (newcoord[0]<0 || newcoord[0]>1 ||
257
          newcoord[1]<0 || newcoord[1]>1) {
258
        if (safetyct==maxct)
259
          return 1; /* we left the cell */
260
        alpha*=_gamma;
261
      }
262
263
      ELL_3M_LERP(hessbot,newcoord[0],botleft,botright);
264
      ELL_3M_LERP(hesstop,newcoord[0],topleft,topright);
265
      ELL_3M_LERP(hess,newcoord[1],hessbot,hesstop);
266
267
      norm = sqrt(hess[0]*hess[0]+hess[4]*hess[4]+hess[8]*hess[8]+
268
                  2*(hess[1]*hess[1]+hess[2]*hess[2]+hess[5]*hess[5]));
269
      if (norm<1e-300) return 6;
270
      /* copy over */
271
      ten[0] = hess[0]/norm; ten[1] = hess[1]/norm; ten[2] = hess[2]/norm;
272
      ten[3] = hess[4]/norm; ten[4] = hess[5]/norm; ten[5] = hess[8]/norm;
273
      for (i=0; i<6; i++)
274
        tsqr[i] = ten[i]*ten[i];
275
276
      /* evaluate the constraint function vector */
277
      cf[0] = ten[0]*(tsqr[3]-tsqr[5])+ten[0]*(tsqr[1]-tsqr[2])+
278
        ten[3]*(tsqr[5]-tsqr[0])+ten[3]*(tsqr[4]-tsqr[1])+
279
        ten[5]*(tsqr[0]-tsqr[3])+ten[5]*(tsqr[2]-tsqr[4]); /* fx */
280
      cf[1] = ten[4]*(2*(tsqr[4]-tsqr[0])-(tsqr[2]+tsqr[1])+
281
                      2*(ten[3]*ten[0]+ten[5]*ten[0]-ten[3]*ten[5]))+
282
        ten[1]*ten[2]*(2*ten[0]-ten[5]-ten[3]); /* fy1 */
283
      cf[2] = ten[2]*(2*(tsqr[2]-tsqr[3])-(tsqr[1]+tsqr[4])+
284
                      2*(ten[5]*ten[3]+ten[0]*ten[3]-ten[5]*ten[0]))+
285
        ten[4]*ten[1]*(2*ten[3]-ten[0]-ten[5]); /* fy2 */
286
      cf[3] = ten[1]*(2*(tsqr[1]-tsqr[5])-(tsqr[4]+tsqr[2])+
287
                      2*(ten[0]*ten[5]+ten[3]*ten[5]-ten[0]*ten[3]))+
288
        ten[2]*ten[4]*(2*ten[5]-ten[3]-ten[0]); /* fy3 */
289
      cf[4] = ten[4]*(tsqr[2]-tsqr[1])+ten[1]*ten[2]*(ten[3]-ten[5]); /* fz1 */
290
      cf[5] = ten[2]*(tsqr[1]-tsqr[4])+ten[4]*ten[1]*(ten[5]-ten[0]); /* fz2 */
291
      cf[6] = ten[1]*(tsqr[4]-tsqr[2])+ten[2]*ten[4]*(ten[0]-ten[3]); /* fz3 */
292
293
      newdiscr = cf[0]*cf[0]+cf[1]*cf[1]+cf[2]*cf[2]+cf[3]*cf[3]+
294
        15*(cf[4]*cf[4]+cf[5]*cf[5]+cf[6]*cf[6]);
295
      if (newdiscr<eps) {
296
        coord[0]=newcoord[0]; coord[1]=newcoord[1]; /* update coord! */
297
        if (type!='l' && type!='p') return 0;
298
        else {
299
          /* check if type is correct */
300
          double dev[9];
301
          double mean = (ten[0]+ten[3]+ten[5])/3;
302
          dev[0] = ten[0]-mean; dev[1] = ten[1]; dev[2] = ten[2];
303
          dev[3] = ten[1]; dev[4]=ten[3]-mean; dev[5] = ten[4];
304
          dev[6] = ten[2]; dev[7]=ten[4]; dev[8]=ten[5]-mean;
305
          det = ELL_3M_DET(dev);
306
          if ((type=='l' && det>0) || (type=='p' && det<0)) return 0;
307
          else return 4; /* sufficient accuracy, but wrong type  */
308
        }
309
      }
310
311
      if (newdiscr<=discr-0.5*alpha*dxsqr) {
312
        accept=1;
313
        discr = newdiscr;
314
      } else {
315
        alpha*=_gamma;
316
      }
317
    }
318
319
    if (!accept)
320
      return 5;
321
    coord[0] -= alpha*dx[0];
322
    coord[1] -= alpha*dx[1];
323
  }
324
  return 2; /* hit maxiter */
325
}
326
327
/* Descends to the degenerate line in a trilinearly interpolated cell
328
 * using the discriminant constraint functions and the Newton-Raphson
329
 * method with Armijo stepsize control.
330
 *
331
 * This function is NOT part of the crease extraction, but has been
332
 * used for debugging it.
333
 *
334
 * coord are coordinates relative to the given cell and will be
335
 *      updated in each iteration.
336
 * Hbfl - Htbr are 9-vectors, representing the symmetric matrices at the
337
 *      corners of the face (input only)
338
 * maxiter is the maximum number of iterations allowed
339
 * eps is the accuracy up to which the discriminant should be zero
340
 *     The discriminant scales with the Frobenius norm to the sixth power,
341
 *     so the exact constraint is disc/|T|^6<eps
342
 * type can be 'l', 'p' or something else (=both)
343
 *
344
 * Returns 0 if the point was found up to the given accuracy
345
 * Returns 1 if we left the cell
346
 * Returns 2 if we hit maxiter
347
 * Returns 3 if we could not invert a matrix to find next gradient dir
348
 * Returns 4 if we found a point, but it does not have the desired type
349
 * Returns 5 if Armijo rule failed to find a valid stepsize
350
 * Returns 6 if we hit a zero tensor (|T|<1e-300)
351
 */
352
int seekDescendToDegCell(double *coord, double *Hbfl, double *Hbfr,
353
                         double *Hbbl, double *Hbbr,
354
                         double *Htfl, double *Htfr, double *Htbl, double *Htbr,
355
                         int maxiter, double eps, char type)
356
{
357
  double discr=0; /* store discriminant value for previous point */
358
359
  double Hfrontleft[9]={0,0,0,0,0,0,0,0,0}, Hbackleft[9]={0,0,0,0,0,0,0,0,0};
360
  double Hfrontright[9]={0,0,0,0,0,0,0,0,0}, Hbackright[9]={0,0,0,0,0,0,0,0,0};
361
  double Hleft[9]={0,0,0,0,0,0,0,0,0}, Hright[9]={0,0,0,0,0,0,0,0,0};
362
  double H[9]={0,0,0,0,0,0,0,0,0}; /* init takes care of compiler warnings */
363
  double optgrad[3]={0.0,0.0,0.0}; /* gradient for descent */
364
365
  int iter=0;
366
  do {
367
    /* on the first run, initialize discr; later, employ the Armijo
368
     * stepsize rule to guarantee convergence */
369
    double beta=1.0;
370
    double _gamma=0.5;
371
    double alpha=beta;
372
    int accept=0;
373
    double optgradsqr = ELL_3V_DOT(optgrad,optgrad);
374
    unsigned int safetyct=0;
375
    const unsigned int maxct=30;
376
    double tsqr[6]={0,0,0,0,0,0}, /* only initialize to silence warning */
377
      ten[6]={0,0,0,0,0,0}, norm=0;
378
    double cf[7]={0,0,0,0,0,0,0}, /* only initialize to silence warning */
379
      cft[42]; /* derive relative to tensor values, 7x6 matrix */
380
    double cfx[21]; /* spatial derivative of constraint functions, 7x3 matrix */
381
    double tx[18]; /* spatial derivative of tensor values, 6x3 matrix */
382
    double Hder[9], Hfront[9], Hback[9]; /* used to approximate Hessian der. */
383
    double Htopleft[9], Htopright[9], Hbotleft[9], Hbotright[9],
384
      Htop[9], Hbot[9];
385
    double denom[9]; /* 3x3 matrix that is to be inverted */
386
    double inv[9]; /* inverse of that matrix */
387
    double nom[3]={0,0,0};
388
389
    int i, j, row, col; /* counters, used later on */
390
391
    while (!accept && safetyct++<maxct) {
392
      /* compute distance at new position */
393
      double newcoord[3];
394
      double newdiscr;
395
      ELL_3V_SET(newcoord, coord[0]-alpha*optgrad[0],
396
                 coord[1]-alpha*optgrad[1], coord[2]-alpha*optgrad[2]);
397
398
      if (newcoord[0]<0 || newcoord[0]>1 ||
399
          newcoord[1]<0 || newcoord[1]>1 ||
400
          newcoord[2]<0 || newcoord[2]>1) {
401
        if (safetyct==maxct) {
402
          ELL_3V_COPY(coord,newcoord); /* such that caller knows which
403
                                          dir was the culprit */
404
          return 1; /* we left the cell */
405
        }
406
        alpha*=_gamma;
407
        continue;
408
      }
409
410
      ELL_3M_LERP(Hfrontleft, newcoord[2], Hbfl, Htfl);
411
      ELL_3M_LERP(Hbackleft, newcoord[2], Hbbl, Htbl);
412
      ELL_3M_LERP(Hleft, newcoord[1], Hfrontleft, Hbackleft);
413
414
      ELL_3M_LERP(Hfrontright, newcoord[2], Hbfr, Htfr);
415
      ELL_3M_LERP(Hbackright, newcoord[2], Hbbr, Htbr);
416
      ELL_3M_LERP(Hright, newcoord[1], Hfrontright, Hbackright);
417
418
      ELL_3M_LERP(H, newcoord[0], Hleft, Hright);
419
      norm = sqrt(H[0]*H[0]+H[4]*H[4]+H[8]*H[8]+
420
                  2*(H[1]*H[1]+H[2]*H[2]+H[5]*H[5]));
421
      if (norm<1e-300) return 6;
422
      ten[0]=H[0]/norm; ten[1]=H[1]/norm; ten[2]=H[2]/norm;
423
      ten[3]=H[4]/norm; ten[4]=H[5]/norm; ten[5]=H[7]/norm;
424
425
      for (i=0; i<6; i++)
426
        tsqr[i] = ten[i]*ten[i];
427
428
      /* evaluate the constraint function vector */
429
      cf[0] = ten[0]*(tsqr[3]-tsqr[5])+ten[0]*(tsqr[1]-tsqr[2])+
430
        ten[3]*(tsqr[5]-tsqr[0])+
431
        ten[3]*(tsqr[4]-tsqr[1])+ten[5]*(tsqr[0]-tsqr[3])+
432
        ten[5]*(tsqr[2]-tsqr[4]); /* fx */
433
      cf[1] = ten[4]*(2*(tsqr[4]-tsqr[0])-(tsqr[2]+tsqr[1])+
434
                      2*(ten[3]*ten[0]+ten[5]*ten[0]-ten[3]*ten[5]))+
435
        ten[1]*ten[2]*(2*ten[0]-ten[5]-ten[3]); /* fy1 */
436
      cf[2] = ten[2]*(2*(tsqr[2]-tsqr[3])-(tsqr[1]+tsqr[4])+
437
                      2*(ten[5]*ten[3]+ten[0]*ten[3]-ten[5]*ten[0]))+
438
        ten[4]*ten[1]*(2*ten[3]-ten[0]-ten[5]); /* fy2 */
439
      cf[3] = ten[1]*(2*(tsqr[1]-tsqr[5])-(tsqr[4]+tsqr[2])+
440
                      2*(ten[0]*ten[5]+ten[3]*ten[5]-ten[0]*ten[3]))+
441
        ten[2]*ten[4]*(2*ten[5]-ten[3]-ten[0]); /* fy3 */
442
      cf[4] = ten[4]*(tsqr[2]-tsqr[1])+ten[1]*ten[2]*(ten[3]-ten[5]); /* fz1 */
443
      cf[5] = ten[2]*(tsqr[1]-tsqr[4])+ten[4]*ten[1]*(ten[5]-ten[0]); /* fz2 */
444
      cf[6] = ten[1]*(tsqr[4]-tsqr[2])+ten[2]*ten[4]*(ten[0]-ten[3]); /* fz3 */
445
446
      newdiscr = cf[0]*cf[0]+cf[1]*cf[1]+cf[2]*cf[2]+cf[3]*cf[3]+
447
        15*(cf[4]*cf[4]+cf[5]*cf[5]+cf[6]*cf[6]);
448
449
      if (newdiscr<eps) {
450
        ELL_3V_COPY(coord, newcoord); /* update coord for output */
451
        if (type!='l' && type!='p') return 0;
452
        else {
453
          /* check if type is correct */
454
          double dev[9], det;
455
          double mean = (ten[0]+ten[3]+ten[5])/3;
456
          dev[0] = ten[0]-mean; dev[1] = ten[1]; dev[2] = ten[2];
457
          dev[3] = ten[1]; dev[4]=ten[3]-mean; dev[5] = ten[4];
458
          dev[6] = ten[2]; dev[7]=ten[4]; dev[8]=ten[5]-mean;
459
          det = ELL_3M_DET(dev);
460
          if ((type=='l' && det>0) || (type=='p' && det<0)) return 0;
461
          else return 4; /* sufficient accuracy, but wrong type  */
462
        }
463
      }
464
465
      if (iter==0 || newdiscr<=discr-0.5*alpha*optgradsqr) {
466
        accept=1;
467
        discr = newdiscr;
468
      } else {
469
        alpha*=_gamma;
470
      }
471
    }
472
473
    if (!accept)
474
      return 5; /* could not find a valid stepsize */
475
    coord[0] -= alpha*optgrad[0];
476
    coord[1] -= alpha*optgrad[1];
477
    coord[2] -= alpha*optgrad[2];
478
479
    if (iter==maxiter-1)
480
      break;
481
482
    /* find derivative of constraint function vector using the chain rule */
483
484
    cft [0] = tsqr[3]-tsqr[5]+tsqr[1]-tsqr[2]-
485
      2*ten[0]*ten[3]+2*ten[0]*ten[5]; /* fx/T00 */
486
    cft [1] = 2*ten[0]*ten[1]-2*ten[3]*ten[1]; /* fx/T01 */
487
    cft [2] = -2*ten[0]*ten[2]+2*ten[5]*ten[2]; /* fx/T02 */
488
    cft [3] = 2*ten[0]*ten[3]+tsqr[5]-tsqr[0]+tsqr[4]-tsqr[1]-
489
      2*ten[5]*ten[3]; /* fx/T11 */
490
    cft [4] = 2*ten[3]*ten[4]-2*ten[5]*ten[4]; /* fx/T12 */
491
    cft [5] = -2*ten[0]*ten[5]+2*ten[3]*ten[5]+tsqr[0]-tsqr[3]+
492
      tsqr[2]-tsqr[4]; /* fx/T22 */
493
494
    cft [6] = -4*ten[0]*ten[4]+2*ten[4]*ten[3]+2*ten[4]*ten[5]+
495
      2*ten[1]*ten[2]; /* fy1/T00 */
496
    cft [7] = -2*ten[4]*ten[1]+ten[2]*(2*ten[0]-ten[5]-ten[3]); /* fy1/T01 */
497
    cft [8] = -2*ten[4]*ten[2]+ten[1]*(2*ten[0]-ten[5]-ten[3]); /* fy1/T02 */
498
    cft [9] = 2*ten[4]*ten[0]-2*ten[4]*ten[5]-ten[1]*ten[2]; /* fy1/T11 */
499
    cft[10] = 6*tsqr[4]-2*tsqr[0]-(tsqr[2]+tsqr[1])+
500
      2*(ten[3]*ten[0]+ten[5]*ten[0]-ten[3]*ten[5]); /* fy1/T12 */
501
    cft[11] = 2*ten[4]*ten[0]-2*ten[4]*ten[3]-ten[1]*ten[2]; /* fy1/T22 */
502
503
    cft[12] = 2*ten[2]*ten[3]-2*ten[2]*ten[5]-ten[4]*ten[1]; /* fy2/T00 */
504
    cft[13] = -2*ten[2]*ten[1]+ten[4]*(2*ten[3]-ten[0]-ten[5]); /* fy2/T01 */
505
    cft[14] = 6*tsqr[2]-2*tsqr[3]-(tsqr[1]+tsqr[4])+
506
      2*(ten[5]*ten[3]+ten[0]*ten[3]-ten[5]*ten[0]); /* fy2/T02 */
507
    cft[15] = -4*ten[2]*ten[3]+2*ten[2]*ten[5]+2*ten[2]*ten[0]+
508
      2*ten[4]*ten[1]; /* fy2/T11 */
509
    cft[16] = -2*ten[2]*ten[4]+ten[1]*(2*ten[3]-ten[0]-ten[5]); /* fy2/T12 */
510
    cft[17] = 2*ten[2]*ten[3]-2*ten[2]*ten[0]-ten[4]*ten[1]; /* fy2/T22 */
511
512
    cft[18] = 2*ten[1]*ten[5]-2*ten[1]*ten[3]-ten[2]*ten[4]; /* fy3/T00 */
513
    cft[19] = 6*tsqr[1]-2*tsqr[5]-(tsqr[4]+tsqr[2])+
514
      2*(ten[0]*ten[5]+ten[3]*ten[5]-ten[0]*ten[3]); /* fy3/T01 */
515
    cft[20] = -2*ten[1]*ten[2]+ten[4]*(2*ten[5]-ten[3]-ten[0]); /* fy3/T02 */
516
    cft[21] = 2*ten[1]*ten[5]-2*ten[1]*ten[0]-ten[2]*ten[4]; /* fy3/T11 */
517
    cft[22] = -2*ten[1]*ten[4]+ten[2]*(2*ten[5]-ten[3]-ten[0]); /* fy3/T12 */
518
    cft[23] = -4*ten[1]*ten[5]+2*ten[0]*ten[1]+2*ten[1]*ten[3]+
519
      2*ten[2]*ten[4]; /* fy3/T22 */
520
521
    cft[24] = 0; /* fz1/T00 */
522
    cft[25] = -2*ten[4]*ten[1]+ten[2]*(ten[3]-ten[5]); /* fz1/T01 */
523
    cft[26] = 2*ten[4]*ten[2]+ten[1]*(ten[3]-ten[5]); /* fz1/T02 */
524
    cft[27] = ten[1]*ten[2]; /* fz1/T11 */
525
    cft[28] = tsqr[2]-tsqr[1]; /* fz1/T12 */
526
    cft[29] = -ten[1]*ten[2]; /* fz1/T22 */
527
528
    cft[30] = -ten[4]*ten[1]; /* fz2/T00 */
529
    cft[31] = 2*ten[2]*ten[1]+ten[4]*(ten[5]-ten[0]); /* fz2/T01 */
530
    cft[32] = tsqr[1]-tsqr[4]; /* fz2/T02 */
531
    cft[33] = 0; /* fz2/T11 */
532
    cft[34] = -2*ten[2]*ten[4]+ten[1]*(ten[5]-ten[0]); /* fz2/T12 */
533
    cft[35] = ten[4]*ten[1]; /* fz2/T22 */
534
535
    cft[36] = ten[2]*ten[4]; /* fz3/T00 */
536
    cft[37] = tsqr[4]-tsqr[2]; /* fz3/T01 */
537
    cft[38] = -2*ten[1]*ten[2]+ten[4]*(ten[0]-ten[3]); /* fz3/T02 */
538
    cft[39] = -ten[2]*ten[4]; /* fz3/T11 */
539
    cft[40] = 2*ten[1]*ten[4]+ten[2]*(ten[0]-ten[3]); /* fz3/T12 */
540
    cft[41] = 0; /* fz3/T22 */
541
542
    /* approximate Hessian derivative in x dir */
543
    ELL_3M_SUB(Hder, Hright, Hleft);
544
    ELL_3M_SCALE(Hder,1.0/norm,Hder);
545
546
    tx[0] = Hder[0]; /* T00 / x */
547
    tx[3] = Hder[1]; /* T01 / x */
548
    tx[6] = Hder[2]; /* T02 / x */
549
    tx[9] = Hder[4]; /* T11 / x */
550
    tx[12] = Hder[5]; /* T12 / x */
551
    tx[15] = Hder[8]; /* T22 / x */
552
553
    ELL_3M_LERP(Hfront, coord[0], Hfrontleft, Hfrontright);
554
    ELL_3M_LERP(Hback, coord[0], Hbackleft, Hbackright);
555
    ELL_3M_SUB(Hder, Hback, Hfront); /* y dir */
556
    ELL_3M_SCALE(Hder,1.0/norm,Hder);
557
558
    tx[1] = Hder[0]; /* T00 / y */
559
    tx[4] = Hder[1]; /* T01 / y */
560
    tx[7] = Hder[2]; /* T02 / y */
561
    tx[10] = Hder[4]; /* T11 / y */
562
    tx[13] = Hder[5]; /* T12 / y */
563
    tx[16] = Hder[8]; /* T22 / y */
564
565
    /* approximate Hessian derivative in z dir */
566
    ELL_3M_LERP(Htopleft, coord[1], Htfl, Htbl);
567
    ELL_3M_LERP(Htopright, coord[1], Htfr, Htbr);
568
    ELL_3M_LERP(Hbotleft, coord[1], Hbfl, Hbbl);
569
    ELL_3M_LERP(Hbotright, coord[1], Hbfr, Hbbr);
570
    ELL_3M_LERP(Htop, coord[0], Htopleft, Htopright);
571
    ELL_3M_LERP(Hbot, coord[0], Hbotleft, Hbotright);
572
    ELL_3M_SUB(Hder, Htop, Hbot); /* z dir */
573
    ELL_3M_SCALE(Hder,1.0/norm,Hder);
574
575
    tx[2] = Hder[0]; /* T00 / z */
576
    tx[5] = Hder[1]; /* T01 / z */
577
    tx[8] = Hder[2]; /* T02 / z */
578
    tx[11] = Hder[4]; /* T11 / z */
579
    tx[14] = Hder[5]; /* T12 / z */
580
    tx[17] = Hder[8]; /* T22 / z */
581
582
    /* matrix multiply cft*tx */
583
    for (row=0; row<7; row++)
584
      for (col=0; col<3; col++) {
585
        i = row*3+col;
586
        cfx[i] = 0;
587
        for (j=0; j<6; j++) {
588
          cfx[i] += cft[row*6+j]*tx[j*3+col];
589
        }
590
      }
591
592
    for (row=0; row<3; row++)
593
      for (col=0; col<3; col++) {
594
        i = row*3+col;
595
        denom[i] = 0;
596
        for (j=0; j<7; j++) {
597
          denom[i] += cfx[j*3+row]*cfx[j*3+col];
598
        }
599
      }
600
    ell_3m_inv_d(inv, denom);
601
602
    /* multiply transpose(cfx)*cf */
603
    for (j=0; j<7; j++) {
604
      nom[0] += cfx[j*3]  * cf[j];
605
      nom[1] += cfx[j*3+1]* cf[j];
606
      nom[2] += cfx[j*3+2]* cf[j];
607
    }
608
609
    /* compute new optgrad = inv*nom */
610
    ELL_3MV_MUL(optgrad,inv,nom);
611
  } while (iter++<maxiter);
612
613
  return 2; /* hit maxiter */
614
}
615
616
/* Gradient descent to a point on a crease surface
617
 *
618
 * NOT used as part of the crease extraction, only for debugging
619
 *
620
 * coord are coordinates relative to the given cell and will be
621
 *      updated in each iteration.
622
 * Hbfl - Htbr are 9-vectors, representing the Hessian matrices at the
623
 *      corners of the face (input only)
624
 * gbfl - gbbr are 3-vectors, representing the gradient directions at the
625
 *      corners of the face (input only)
626
 * maxiter is the maximum number of iterations allowed
627
 * eps is the accuracy up to which |h| (|Tg-g|, cf. paper) must be zero
628
 * ridge is non-zero if we are looking for a ridge (zero for valley)
629
 *
630
 * Returns 0 if the point was found up to the given accuracy
631
 * Returns 1 if we left the cell
632
 * Returns 2 if we hit maxiter
633
 * Returns 3 if Armijo rule failed to find a valid stepsize
634
 */
635
int seekDescendToRidge(double *coord,
636
                       double *Hbfl, double *gbfl, double *Hbfr, double *gbfr,
637
                       double *Hbbl, double *gbbl, double *Hbbr, double *gbbr,
638
                       double *Htfl, double *gtfl, double *Htfr, double *gtfr,
639
                       double *Htbl, double *gtbl, double *Htbr, double *gtbr,
640
                       int maxiter, double eps, char ridge,
641
                       const double evalDiffThresh) {
642
  double dist=0; /* store distance value of previous iteration */
643
644
  double Hfrontleft[9], Hbackleft[9];
645
  double Hfrontright[9], Hbackright[9];
646
  double Hleft[9], Hright[9];
647
  double H[9], evals[3], evecs[9], T[9];
648
649
  double gfrontleft[3], gbackleft[3];
650
  double gfrontright[3], gbackright[3];
651
  double gleft[3], gright[3];
652
  double g[3];
653
654
  double optgrad[3]={0.0,0.0,0.0}; /* gradient for descent */
655
656
  int iter=0;
657
  do {
658
    double Tg[3];
659
660
    /* on the first run, initialize dist; later, employ the Armijo
661
     * stepsize rule to guarantee convergence */
662
    double beta=0.1;
663
    double _gamma=0.5;
664
    double alpha=beta;
665
    int accept=0;
666
    double optgradsqr = ELL_3V_DOT(optgrad,optgrad);
667
    int safetyct=0;
668
    int maxct=30; /* avoid infinite loops when finding stepsize */
669
    /* variables used to compute the next step */
670
    double Hder[9], gder[3], Tder[9];
671
    double Tpg[3], Tgp[3];
672
    double Hfront[9], Hback[9], gfront[3], gback[3];
673
    double Htopleft[9], Htopright[9], Hbotleft[9], Hbotright[9],
674
      gtopleft[3], gtopright[3], gbotleft[3], gbotright[3],
675
      Htop[9], Hbot[9], gtop[3], gbot[3];
676
    while (!accept && safetyct++<maxct) {
677
      /* compute distance at new position */
678
      double newcoord[3];
679
      double diff[3], newdist;
680
      ELL_3V_SET(newcoord, coord[0]-alpha*optgrad[0],
681
                 coord[1]-alpha*optgrad[1], coord[2]-alpha*optgrad[2]);
682
683
      if (newcoord[0]<0 || newcoord[0]>1 ||
684
          newcoord[1]<0 || newcoord[1]>1 ||
685
          newcoord[2]<0 || newcoord[2]>1) {
686
        if (safetyct==maxct)
687
          return 1; /* we left the cell */
688
        alpha*=_gamma;
689
      }
690
691
      ELL_3M_LERP(Hfrontleft, newcoord[2], Hbfl, Htfl);
692
      ELL_3M_LERP(Hbackleft, newcoord[2], Hbbl, Htbl);
693
      ELL_3M_LERP(Hleft, newcoord[1], Hfrontleft, Hbackleft);
694
695
      ELL_3M_LERP(Hfrontright, newcoord[2], Hbfr, Htfr);
696
      ELL_3M_LERP(Hbackright, newcoord[2], Hbbr, Htbr);
697
      ELL_3M_LERP(Hright, newcoord[1], Hfrontright, Hbackright);
698
699
      ELL_3M_LERP(H, newcoord[0], Hleft, Hright);
700
      ell_3m_eigensolve_d(evals, evecs, H, AIR_TRUE);
701
702
      _seekHess2T(T,evals,evecs,evalDiffThresh,ridge);
703
704
      ELL_3V_LERP(gfrontleft, newcoord[2], gbfl, gtfl);
705
      ELL_3V_LERP(gbackleft, newcoord[2], gbbl, gtbl);
706
      ELL_3V_LERP(gleft, newcoord[1], gfrontleft, gbackleft);
707
708
      ELL_3V_LERP(gfrontright, newcoord[2], gbfr, gtfr);
709
      ELL_3V_LERP(gbackright, newcoord[2], gbbr, gtbr);
710
      ELL_3V_LERP(gright, newcoord[1], gfrontright, gbackright);
711
712
      ELL_3V_LERP(g, newcoord[0], gleft, gright);
713
714
      ell_3mv_mul_d(Tg, T, g);
715
      ELL_3V_SUB(diff,Tg,g);
716
      newdist = ELL_3V_DOT(diff,diff);
717
718
      if (newdist<eps) {
719
        ELL_3V_COPY(coord, newcoord); /* update for output */
720
        return 0; /* we are on the surface */
721
      }
722
723
      if (iter==0 || newdist<=dist-0.5*alpha*optgradsqr) {
724
        accept=1;
725
        dist = newdist;
726
      } else {
727
        alpha*=_gamma;
728
      }
729
    }
730
731
    if (!accept)
732
      return 3; /* could not find a valid stepsize */
733
    coord[0] -= alpha*optgrad[0];
734
    coord[1] -= alpha*optgrad[1];
735
    coord[2] -= alpha*optgrad[2];
736
737
    if (iter==maxiter-1)
738
      break;
739
740
    /* compute a new optgrad from derivatives of T and g */
741
    ELL_3V_SUB(gder, gright, gleft); /* x dir */
742
    ELL_3M_SUB(Hder, Hright, Hleft);
743
    _seekHessder2Tder(Tder, Hder, evals, evecs, evalDiffThresh, ridge);
744
745
    ell_3mv_mul_d(Tpg,Tder,g);
746
    ell_3mv_mul_d(Tgp,T,gder);
747
    optgrad[0]=ELL_3V_DOT(Tpg,Tg)+ELL_3V_DOT(Tgp,Tg)-
748
      ELL_3V_DOT(Tpg,g)-ELL_3V_DOT(Tgp,g)-
749
      ELL_3V_DOT(Tg,gder)+ELL_3V_DOT(g,gder);
750
751
    ELL_3M_LERP(Hfront, coord[0], Hfrontleft, Hfrontright);
752
    ELL_3M_LERP(Hback, coord[0], Hbackleft, Hbackright);
753
    ELL_3M_SUB(Hder, Hback, Hfront); /* y dir */
754
    _seekHessder2Tder(Tder, Hder, evals, evecs, evalDiffThresh, ridge);
755
756
    ELL_3V_LERP(gfront, coord[0], gfrontleft, gfrontright);
757
    ELL_3V_LERP(gback, coord[0], gbackleft, gbackright);
758
    ELL_3V_SUB(gder, gback, gfront);
759
760
    ell_3mv_mul_d(Tpg,Tder,g);
761
    ell_3mv_mul_d(Tgp,T,gder);
762
    optgrad[1]=ELL_3V_DOT(Tpg,Tg)+ELL_3V_DOT(Tgp,Tg)-
763
      ELL_3V_DOT(Tpg,g)-ELL_3V_DOT(Tgp,g)-
764
      ELL_3V_DOT(Tg,gder)+ELL_3V_DOT(g,gder);
765
766
    ELL_3M_LERP(Htopleft, coord[1], Htfl, Htbl);
767
    ELL_3M_LERP(Htopright, coord[1], Htfr, Htbr);
768
    ELL_3M_LERP(Hbotleft, coord[1], Hbfl, Hbbl);
769
    ELL_3M_LERP(Hbotright, coord[1], Hbfr, Hbbr);
770
    ELL_3M_LERP(Htop, coord[0], Htopleft, Htopright);
771
    ELL_3M_LERP(Hbot, coord[0], Hbotleft, Hbotright);
772
    ELL_3M_SUB(Hder, Htop, Hbot); /* z dir */
773
    _seekHessder2Tder(Tder, Hder, evals, evecs, evalDiffThresh, ridge);
774
775
    ELL_3V_LERP(gtopleft, coord[1], gtfl, gtbl);
776
    ELL_3V_LERP(gtopright, coord[1], gtfr, gtbr);
777
    ELL_3V_LERP(gbotleft, coord[1], gbfl, gbbl);
778
    ELL_3V_LERP(gbotright, coord[1], gbfr, gbbr);
779
    ELL_3V_LERP(gtop, coord[0], gtopleft, gtopright);
780
    ELL_3V_LERP(gbot, coord[0], gbotleft, gbotright);
781
    ELL_3V_SUB(gder, gtop, gbot);
782
783
    ell_3mv_mul_d(Tpg,Tder,g);
784
    ell_3mv_mul_d(Tgp,T,gder);
785
    optgrad[2]=ELL_3V_DOT(Tpg,Tg)+ELL_3V_DOT(Tgp,Tg)-
786
      ELL_3V_DOT(Tpg,g)-ELL_3V_DOT(Tgp,g)-
787
      ELL_3V_DOT(Tg,gder)+ELL_3V_DOT(g,gder);
788
  } while (iter++<maxiter);
789
790
  return 2; /* hit maxiter */
791
}