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 |
|
|
} |