Debian Codegraph @

3rdparty/mshmet/src/sourceslib/redsim.c

raw
1 #include "mshmet.h"
2
3 #define tuMv(u,m,v) (\
4 (u[0]*m[0]+u[1]*m[1])*v[0] + \
5 (u[0]*m[1]+u[1]*m[2])*v[1])
6
7
8 /* invert 3x3 symmetric matrix */
9 static int invmat(double m[6],double mi[6]) {
10 double aa,bb,cc,det,vmin,vmax,maxx;
11 int k;
12
13 /* check diagonal matrices */
14 vmax = fabs(m[1]);
15 maxx = fabs(m[2]);
16 if( maxx > vmax ) vmax = maxx;
17 maxx = fabs(m[4]);
18 if( maxx > vmax ) vmax = maxx;
19 if ( vmax < EPS ) {
20 mi[0] = 1.0/m[0];
21 mi[3] = 1.0/m[3];
22 mi[5] = 1.0/m[5];
23 mi[1] = mi[2] = mi[4] = 0.0;
24 return(1);
25 }
26
27 /* check ill-conditionned matrix */
28 vmin = vmax = fabs(m[0]);
29 for (k=1; k<6; k++) {
30 maxx = fabs(m[k]);
31 if ( maxx < vmin ) vmin = maxx;
32 else if ( maxx > vmax ) vmax = maxx;
33 }
34 if ( vmax == 0.0 ) return(0);
35
36 /* compute sub-dets */
37 aa = m[3]*m[5] - m[4]*m[4];
38 bb = m[4]*m[2] - m[1]*m[5];
39 cc = m[1]*m[4] - m[2]*m[3];
40 det = m[0]*aa + m[1]*bb + m[2]*cc;
41 if ( fabs(det) < EPS1 ) return(0);
42 det = 1.0 / det;
43
44 mi[0] = aa*det;
45 mi[1] = bb*det;
46 mi[2] = cc*det;
47 mi[3] = (m[0]*m[5] - m[2]*m[2])*det;
48 mi[4] = (m[1]*m[2] - m[0]*m[4])*det;
49 mi[5] = (m[0]*m[3] - m[1]*m[1])*det;
50
51 return(1);
52 }
53
54
55 /* invert 3x3 non-symmetric matrix */
56 int invmatg(double m[9],double mi[9]) {
57 double aa,bb,cc,det,vmin,vmax,maxx;
58 int k;
59
60 /* check ill-conditionned matrix */
61 vmin = vmax = fabs(m[0]);
62 for (k=1; k<9; k++) {
63 maxx = fabs(m[k]);
64 if ( maxx < vmin ) vmin = maxx;
65 else if ( maxx > vmax ) vmax = maxx;
66 }
67 if ( vmax == 0.0 ) return(0);
68
69 /* compute sub-dets */
70 aa = m[4]*m[8] - m[5]*m[7];
71 bb = m[5]*m[6] - m[3]*m[8];
72 cc = m[3]*m[7] - m[4]*m[6];
73 det = m[0]*aa + m[1]*bb + m[2]*cc;
74 if ( fabs(det) < EPS1 ) return(0);
75 det = 1.0f / det;
76
77 mi[0] = aa*det;
78 mi[3] = bb*det;
79 mi[6] = cc*det;
80 mi[1] = (m[2]*m[7] - m[1]*m[8])*det;
81 mi[4] = (m[0]*m[8] - m[2]*m[6])*det;
82 mi[7] = (m[1]*m[6] - m[0]*m[7])*det;
83 mi[2] = (m[1]*m[5] - m[2]*m[4])*det;
84 mi[5] = (m[2]*m[3] - m[0]*m[5])*det;
85 mi[8] = (m[0]*m[4] - m[1]*m[3])*det;
86
87 return(1);
88 }
89
90
91 /* simultaneous reduction mr = m1 cap m2 */
92 int redsim_3d(double *m1,double *m2,double *mr) {
93 double dd,lambda[3],v[3][3],hh[3];
94 double maxd1,maxd2,ex,ey,ez,m1i[6],n[9],p[9],pi[9];
95 int i,j,k,order;
96
97 maxd1 = fabs(m1[1]);
98 maxd1 = MS_MAX(maxd1,fabs(m1[2]));
99 maxd1 = MS_MAX(maxd1,fabs(m1[4]));
100
101 maxd2 = fabs(m2[1]);
102 maxd2 = MS_MAX(maxd2,fabs(m2[2]));
103 maxd2 = MS_MAX(maxd2,fabs(m2[4]));
104
105 /* check diag matrices */
106 if ( maxd1 < EPS1 && maxd2 < EPS1 ) {
107 mr[0] = MS_MAX(m1[0],m2[0]);
108 mr[3] = MS_MAX(m1[3],m2[3]);
109 mr[5] = MS_MAX(m1[5],m2[5]);
110 mr[1] = mr[2] = mr[4] = 0.0;
111 return(1);
112 }
113 if ( !invmat(m1,m1i) ) return(0);
114
115 /* n = (m1)^-1*m2 */
116 n[0] = m1i[0]*m2[0] + m1i[1]*m2[1] + m1i[2]*m2[2];
117 n[1] = m1i[0]*m2[1] + m1i[1]*m2[3] + m1i[2]*m2[4];
118 n[2] = m1i[0]*m2[2] + m1i[1]*m2[4] + m1i[2]*m2[5];
119 n[3] = m1i[1]*m2[0] + m1i[3]*m2[1] + m1i[4]*m2[2];
120 n[4] = m1i[1]*m2[1] + m1i[3]*m2[3] + m1i[4]*m2[4];
121 n[5] = m1i[1]*m2[2] + m1i[3]*m2[4] + m1i[4]*m2[5];
122 n[6] = m1i[2]*m2[0] + m1i[4]*m2[1] + m1i[5]*m2[2];
123 n[7] = m1i[2]*m2[1] + m1i[4]*m2[3] + m1i[5]*m2[4];
124 n[8] = m1i[2]*m2[2] + m1i[4]*m2[4] + m1i[5]*m2[5];
125
126 /* eigenvectors */
127 order = eigenv(0,n,lambda,v);
128
129 if ( order == 0 )
130 return(0);
131 else if ( order == 3 ) {
132 mr[0] = mr[3] = mr[5] = lambda[0];
133 mr[1] = mr[2] = mr[4] = 0.0;
134 return(1);
135 }
136 else {
137 /* matrix of passage */
138 for (i=0,k=0; i<3; i++)
139 for (j=0; j<3; j++)
140 p[k++] = v[j][i];
141 if ( !invmatg(p,pi) ) return(0);
142
143 for (i=0; i<3; i++) {
144 ex = v[i][0];
145 ey = v[i][1];
146 ez = v[i][2];
147 maxd1 = ex*(m1[0]*ex+m1[1]*ey+m1[2]*ez)
148 + ey*(m1[1]*ex+m1[3]*ey+m1[4]*ez)
149 + ez*(m1[2]*ex+m1[4]*ey+m1[5]*ez);
150 maxd2 = ex*(m2[0]*ex+m2[1]*ey+m2[2]*ez)
151 + ey*(m2[1]*ex+m2[3]*ey+m2[4]*ez)
152 + ez*(m2[2]*ex+m2[4]*ey+m2[5]*ez);
153 hh[i] = MS_MAX(maxd1,maxd2);
154 }
155
156 /* compose matrix tP^-1*lambda*P^-1 */
157 mr[0] = pi[0]*hh[0]*pi[0] + pi[3]*hh[1]*pi[3] + pi[6]*hh[2]*pi[6];
158 mr[1] = pi[0]*hh[0]*pi[1] + pi[3]*hh[1]*pi[4] + pi[6]*hh[2]*pi[7];
159 mr[2] = pi[0]*hh[0]*pi[2] + pi[3]*hh[1]*pi[5] + pi[6]*hh[2]*pi[8];
160 mr[3] = pi[1]*hh[0]*pi[1] + pi[4]*hh[1]*pi[4] + pi[7]*hh[2]*pi[7];
161 mr[4] = pi[1]*hh[0]*pi[2] + pi[4]*hh[1]*pi[5] + pi[7]*hh[2]*pi[8];
162 mr[5] = pi[2]*hh[0]*pi[2] + pi[5]*hh[1]*pi[5] + pi[8]*hh[2]*pi[8];
163
164 /* for safety... */
165 maxd1 = fabs(mr[0]);
166 for (i=1; i<6; i++)
167 maxd1 = MS_MAX(maxd1,fabs(mr[i]));
168 if ( maxd1 < EPS1 ) {
169 fprintf(stderr," %%%% Null matrix.\n");
170 exit(1);
171 }
172 dd = 1.0 / maxd1;
173 for (i=0; i<6; i++) {
174 mr[i] *= dd;
175 /*if ( fabs(m[i]) < EPS ) m[i] = 0.0;*/
176 }
177
178 maxd2 = fabs(mr[1]);
179 maxd2 = MS_MAX(maxd2,fabs(mr[2]));
180 maxd2 = MS_MAX(maxd2,fabs(mr[4]));
181
182 if ( maxd2 < EPS ) {
183 mr[1] = mr[2] = mr[4] = 0.0;
184 mr[0] = fabs(mr[0]) * maxd1;
185 mr[3] = fabs(mr[3]) * maxd1;
186 mr[5] = fabs(mr[5]) * maxd1;
187 }
188 else {
189 order = eigenv(1,mr,lambda,v);
190 for (i=0; i<6; i++) mr[i] *= maxd1;
191
192 if ( lambda[0] < -EPS1 || lambda[1] < -EPS1 || lambda[2] < -EPS1 ) {
193 fprintf(stderr," %%%% Metric inconsistency\n");
194 fprintf(stderr," %.6f %.6f %.6f\n%.6f %.6f\n%.6f\n",
195 mr[0],mr[1],mr[2],mr[3],mr[4],mr[5]);
196 fprintf(stderr," Lambda %f %f %f\n",lambda[0],lambda[1],lambda[2]);
197 return(0);
198 }
199 }
200 }
201 return(1);
202 }
203
204
205 /* find non orthogonal basis (v1,v2) where m1,m2 are diag */
206 int redsim_2d(double *m1,double *m2,double *mr) {
207 double n[4],v1[2],v2[2],lambda[2],vnorm,dd,det1,inv1,tracn;
208 double lambda1[2],lambda2[2],ip[4],tip[4],disc,det,vp1,vp2;
209
210 /* eigenvect of n = m1^(-1) m2 */
211 det1 = m1[0]*m1[2]-m1[1]*m1[1];
212 if ( fabs(det1) < EPS1 ) {
213 return(0);
214 }
215 inv1 = 1.0 / det1;
216
217 /* verif idem vp tq : M1v = vp*M2v <=> (M1-vp*M2)v = 0 */
218 n[0] = (m1[2]*m2[0] - m1[1]*m2[1]) * inv1;
219 n[1] = (m1[2]*m2[1] - m1[1]*m2[2]) * inv1;
220 n[2] = (m1[0]*m2[1] - m1[1]*m2[0]) * inv1;
221 n[3] = (m1[0]*m2[2] - m1[1]*m2[1]) * inv1;
222
223 /* delta >=0 as N diag in IR */
224 dd = n[0] - n[3];
225 disc = sqrt(fabs(dd*dd + 4.0*n[1]*n[2]));
226 tracn = n[0] + n[3];
227
228 lambda[0] = 0.5 * (tracn + disc);
229 if ( fabs(lambda[0]) < EPS1 ) {
230 mr[0] = m1[0];
231 mr[1] = m1[1];
232 mr[2] = m1[2];
233 return(1);
234 }
235
236 /* check for ellipse included in other */
237 if ( disc / lambda[0] < EPS ) {
238 /* delta<<Tr => delta~0 : eigenvalue double, P not invertible */
239 if ( lambda[0] <= 1.0 ) {
240 mr[0] = m1[0];
241 mr[1] = m1[1];
242 mr[2] = m1[2];
243 }
244 else {
245 mr[0] = m2[0];
246 mr[1] = m2[1];
247 mr[2] = m2[2];
248 }
249 return(1);
250 }
251 else {
252 lambda[1] = 0.5 * (tracn - disc);
253
254 v1[0] = -n[1];
255 v1[1] = n[0] - lambda[0];
256 vnorm = sqrt(v1[0]*v1[0] + v1[1]*v1[1]);
257 if ( vnorm < EPS1 ) {
258 v1[0] = lambda[0] - n[3];
259 v1[1] = n[2];
260 vnorm = sqrt(v1[0]*v1[0] + v1[1]*v1[1]);
261 }
262 vnorm = 1.0 / vnorm;
263 v1[0] *= vnorm;
264 v1[1] *= vnorm;
265
266 v2[0] = -n[1];
267 v2[1] = n[0] - lambda[1];
268 vnorm = sqrt(v2[0]*v2[0] + v2[1]*v2[1]);
269 if ( vnorm < EPS1 ) {
270 v2[0] = lambda[1] - n[3];
271 v2[1] = n[2];
272 vnorm = sqrt(v2[0]*v2[0] + v2[1]*v2[1]);
273 }
274 vnorm = 1.0 / vnorm;
275 v2[0] *= vnorm;
276 v2[1] *= vnorm;
277
278 lambda1[0] = tuMv(v1,m1,v1);
279 lambda2[0] = tuMv(v1,m2,v1);
280 lambda1[1] = tuMv(v2,m1,v2);
281 lambda2[1] = tuMv(v2,m2,v2);
282
283 /* intersect (m1,m2)= tP^(-1)&*L&*P^(-1) */
284 det = v1[0]*v2[1] - v2[0]*v1[1];
285 if ( fabs(det) < EPS1 ) return(0);
286 det = 1.0 / det;
287
288 ip[0] = v2[1]*det;
289 ip[1] = -v2[0]*det;
290 ip[2] = -v1[1]*det;
291 ip[3] = v1[0]*det;
292
293 /* tP^(-1)&*L */
294 vp1 = MS_MAX(lambda1[0],lambda2[0]);
295 vp2 = MS_MAX(lambda1[1],lambda2[1]);
296
297 tip[0] = ip[0] * vp1;
298 tip[1] = ip[2] * vp2;
299 tip[2] = ip[1] * vp1;
300 tip[3] = ip[3] * vp2;
301
302 mr[0] = tip[0]*ip[0] + tip[1]*ip[2];
303 mr[1] = tip[0]*ip[1] + tip[1]*ip[3];
304 mr[2] = tip[2]*ip[1] + tip[3]*ip[3];
305 }
306
307 return(1);
308 }