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