« back to all changes in this revision
Viewing changes to QBestFit.c
- browse files at revision 222
- compare with another revision
- download diff
- download tarball
- view history from revision 222
- Committer: asaladin
- Date: 2008-02-27 15:32:47 UTC
- Revision ID: svn-v4:d4b151a5-ce23-0410-94d7-eeae68ba5c7e:ptools/trunk:328
added missing file to compile trunk/
- files added:
- QBestFit.c
added
removed
QBestFit.c
1
/*
2
* This program is for 3D superposition of sets of coordinates.
3
* It is based on the quaternions, as proposed by:
4
* Zuker & Somorjai, Bulletin of Mathematical Biology,
5
* vol. 51, No 1, p 55-78, 1989.
6
*
7
* The search for the largest eigen value is based on a QR, and not
8
* on the Jacobian based solvation approach used by most programmers.
9
*
10
* In our experience, the routine is very robust.
11
*
12
* If you use this program standalone, the call should be on the form:
13
* QBestFit <-bfxyz, -bfpdb> < crdfile.xyz > fitcrd.xyz
14
*
15
* Input:
16
* first line: N, the number of coordinates
17
* following 2 x N lines: N coordinates (x y z, e.g. "0.134 2.345 12.3245", i.e. no commas)
18
* the first N xyz lines will be the template, the last N xyz lines will
19
* undergo the best fit transformation.
20
*
21
* Output:
22
* first line: original rmsd, before fit.
23
* second line : bestfit rmsd.
24
* nextt 4 lines: the 4x4 transformation matrix
25
* N following lines: the N coordinates transformed.
26
*
27
* If you want it directly, the main function is:
28
*
29
* zuker_superpose(DtPoint3 *c1, DtPoint3 *c2, int len, DtMatrix4x4 M)
30
*
31
* This program can be freely used, modified and distributed, given that this
32
* header is maintained.
33
*
34
* If using this routine for publication, please cite F. Guyon and P. Tuffery.
35
*/
36
37
38
#include <string.h>
39
40
#include <stdlib.h>
41
#include <stdio.h>
42
#include <math.h>
43
44
#define DtFloat double /* Le type flottant. */
45
typedef DtFloat DtPoint3[3];
46
typedef DtFloat DtPoint4[4];
47
typedef DtFloat DtMatrix3x3[3][3];
48
typedef DtFloat DtMatrix4x4[4][4];
49
50
51
#define boucle(i,min,max) for((i)=(min);(i)<(max);(i)++)
52
#define max(a,b) (a<b?b:a)
53
#define min(a,b) (a<b?a:b)
54
#define sign(a,b) ((b)>=0.0 ? fabs(a) : -fabs(a))
55
#define EPS 1.e-10
56
57
58
double **alloc_mat(int n, int m) {
59
int i;
60
double **mat = (double **)calloc(m,sizeof(double *));
61
for (i=0; i<n; i++)
62
mat[i]=(double *)calloc(n, sizeof(double));
63
return mat;
64
}
65
66
void free_mat(double **mat, int n){
67
int i;
68
for (i=0; i<n; i++)
69
free(mat[i]);
70
free(mat);
71
}
72
73
void print_matrice(double **a, int n, int m, char *name){
74
int i,j;
75
76
for (i=0; i<n; i++) {
77
for (j=0; j<m; j++)
78
printf ("%s[%d,%d]=%lf ", name, i, j, a[i][j]);
79
printf ("\n");
80
}
81
}
82
83
void print_vector(double *a, int n, char *name){
84
int i;
85
86
for (i=0; i<n; i++)
87
printf ("%s[%d]=%lf\n ", name, i, a[i]);
88
}
89
90
91
double *random_vect(int n) {
92
int i;
93
double *v=(double *) calloc(n, sizeof(double));
94
for (i=0; i<n; i++)
95
v[i]= (double)rand()/(RAND_MAX+1.0);
96
return(v);
97
}
98
99
static double inner(double *x, double *y, int n) {
100
int i;
101
double sum;
102
103
for (sum=0, i=0; i<n; sum+=x[i]*y[i],i++);
104
return sum;
105
106
}
107
108
static double *product(double **A, double *x, int n) {
109
int i, j;
110
double sum;
111
double *y=(double *)calloc(n, sizeof(double));
112
113
for (i=0; i<n; i++) {
114
sum=0;
115
for (j=0; j<n; j++)
116
sum+=A[i][j]*x[j];
117
y[i]=sum;
118
}
119
return y;
120
}
121
122
static int lu_c (double a[4][4], int n)
123
{
124
int i,j,k,err;
125
double pivot,coef,s;
126
127
err=1;
128
k=0;
129
while (err==1 && k<n) {
130
pivot=a[k][k];
131
if(fabs(pivot)>=EPS) {
132
for(i=k+1;i<n;i++) {
133
coef=a[i][k]/pivot;
134
for(j=k;j<n;j++)
135
a[i][j] -= coef*a[k][j];
136
a[i][k]=coef;
137
}
138
}
139
else err=0;
140
k++;
141
}
142
if(a[n-1][n-1]==0) err=0;
143
return err;
144
}
145
146
147
static void resol_lu(double a[4][4], double *b, int n)
148
{
149
int i,j;
150
double sum;
151
double y[n];
152
y[0]=b[0];
153
for(i=1;i<n;i++) {
154
sum=b[i];
155
for(j=0;j<i;j++)
156
sum-=a[i][j]*y[j];
157
y[i]=sum;
158
}
159
b[n-1]=y[n-1]/a[n-1][n-1];
160
for(i=n-1;i>=0;i--) {
161
sum=y[i];
162
for(j=i+1;j<n;j++)
163
sum-=a[i][j]*b[j];
164
b[i]=sum/a[i][i];
165
}
166
}
167
168
int power(double *a[], int n, int maxiter, double eps, double *v, double *w) {
169
int niter,i;
170
double *y;
171
double sum, l, normy, d;
172
y=random_vect(n);
173
niter=0;
174
do {
175
normy=sqrt(inner(y,y,n));
176
for (i=0; i<n; i++) w[i]=y[i]/normy;
177
y=product(a, w, n);
178
l=inner(w,y,n);
179
niter++;
180
for (sum=0,i=0; i<n; i++) {
181
d=y[i]-l*w[i];
182
sum+=d*d;
183
}
184
d=sqrt(sum);
185
} while (d>eps*fabs(l) && niter<maxiter);
186
free(y);
187
*v=l;
188
return niter;
189
}
190
191
double best_shift(double *a[], int n) {
192
double m, M, s;
193
double t, sum;
194
int i, j;
195
t=a[0][0];
196
for (i=1; i<n; i++) t=max(t, a[i][i]);
197
M=t;
198
t=a[0][0];
199
for (i=0; i<n; i++) {
200
for (sum=0,j=0; j<n; j++)
201
if (j!=i) sum+=fabs(a[i][j]);
202
t=min(t, a[i][i]-sum);
203
}
204
m=t;
205
s=-0.5*(M+m);
206
for (i=0; i<n; i++)
207
a[i][i]=a[i][i]+s;
208
return s;
209
}
210
211
double lmax_estim (double a[4][4], int n) {
212
double t, sum;
213
int i, j;
214
t=a[0][0];
215
for (i=0; i<n; i++) {
216
for (sum=0,j=0; j<n; j++)
217
if (j!=i) sum+=fabs(a[i][j]);
218
t=max(t, a[i][i]+sum);
219
}
220
return t;
221
}
222
223
224
int shift_power(double *a, int n, int maxiter, double eps, double *v, double *w) {
225
double **tmp;
226
double sh;
227
int niter;
228
int i,j;
229
230
/* fprintf(stderr,"ESSAI avec shift_power.\n"); */
231
232
tmp=alloc_mat(n, n);
233
/* copyMat(a, tmp, n, n); */
234
for (i=0; i<n; i++) {
235
for (j=0; j<n; j++) {
236
tmp[i][j] = a[i*n+j];
237
}
238
}
239
sh=best_shift(tmp, n);
240
241
niter=power(tmp, n, maxiter, eps, v, w);
242
*v=*v-sh;
243
free_mat(tmp, n);
244
return niter;
245
}
246
247
int inverse_power(double a[4][4], int n, int maxiter, double eps, double *v, double *w) {
248
int niter,i;
249
double *y;
250
double r, sum, l, normy, d;
251
y=random_vect(n);
252
niter=0;
253
254
r=lmax_estim(a, n);
255
for (i=0; i<n; i++) a[i][i]=a[i][i]-r;
256
if (lu_c(a, n)==0) {
257
/* fprintf(stderr,"ATTENTION ! cas singulier de inverse_power.\n"); */
258
free(y);
259
//exit(0);
260
return 0;
261
}
262
263
do {
264
normy=sqrt(inner(y,y,n));
265
for (i=0; i<n; i++) {
266
w[i]=y[i]/normy;
267
y[i]=w[i];
268
}
269
resol_lu(a, y, n);
270
l=inner(w,y,n);
271
niter++;
272
for (sum=0,i=0; i<n; i++) {
273
d=y[i]-l*w[i];
274
sum+=d*d;
275
}
276
d=sqrt(sum);
277
} while (d>eps*fabs(l) && niter<maxiter);
278
free(y);
279
*v=r+1.0/l;
280
return niter;
281
}
282
283
int largestEV4(double R[4][4], double v[4], double *vp)
284
{
285
double iw[4];
286
double dx,dy,dz, dt;
287
// int aCycle;
288
double M2[4][4];
289
int rs;
290
291
memcpy(M2,R,sizeof(double)*16);
292
293
rs = inverse_power(R, 4, 10000, 1.e-8, vp, v);
294
295
if (!rs)
296
return shift_power(&M2[0][0], 4, 10000, 1.e-8, vp, v);
297
298
return rs;
299
300
}
301
302
303
/* ==================================================================
304
* Calcule la matrice d'inertie des points
305
*
306
* id : indices des atomes dans At
307
* from,tto: indices extremes
308
* P : le barycentre
309
* ==================================================================
310
*/
311
DtMatrix3x3 *XYCov(DtMatrix3x3 *pM,
312
DtPoint3 *X,
313
DtPoint3 *Y,
314
DtPoint3 Xmean,
315
DtPoint3 Ymean,
316
int aSze)
317
{
318
int i,j;
319
double Xx,Xy,Xz;
320
double Yx,Yy,Yz;
321
double daSze;
322
323
/* fprintf(stdout,"inertia, len %d\n",aSze); */
324
325
/* X average*/
326
Xmean[0] = Xmean[1] = Xmean[2] = 0.;
327
for (i=0;i<aSze;i++) {
328
Xmean[0] += X[i][0];
329
Xmean[1] += X[i][1];
330
Xmean[2] += X[i][2];
331
}
332
if (aSze) {
333
daSze = (double) aSze;
334
Xmean[0] /= daSze;
335
Xmean[1] /= daSze;
336
Xmean[2] /= daSze;
337
}
338
339
/* Y average*/
340
Ymean[0] = Ymean[1] = Ymean[2] = 0.;
341
for (i=0;i<aSze;i++) {
342
Ymean[0] += Y[i][0];
343
Ymean[1] += Y[i][1];
344
Ymean[2] += Y[i][2];
345
}
346
if (aSze) {
347
daSze = (double) aSze;
348
Ymean[0] /= daSze;
349
Ymean[1] /= daSze;
350
Ymean[2] /= daSze;
351
}
352
353
/* Covariance matrix */
354
if (pM == NULL) {
355
pM = (DtMatrix3x3 *) calloc(1,sizeof(DtMatrix3x3));
356
} else {
357
memset((void *) pM, 0, sizeof(DtMatrix3x3));
358
}
359
360
for (i=0;i<aSze;i++) {
361
Xx = (double) X[i][0] - Xmean[0];
362
Xy = (double) X[i][1] - Xmean[1];
363
Xz = (double) X[i][2] - Xmean[2];
364
Yx = (double) Y[i][0] - Ymean[0];
365
Yy = (double) Y[i][1] - Ymean[1];
366
Yz = (double) Y[i][2] - Ymean[2];
367
368
(*pM)[0][0] += Xx*Yx;
369
(*pM)[0][1] += Xx*Yy;
370
(*pM)[0][2] += Xx*Yz;
371
372
(*pM)[1][0] += Xy*Yx;
373
(*pM)[1][1] += Xy*Yy;
374
(*pM)[1][2] += Xy*Yz;
375
376
(*pM)[2][0] += Xz*Yx;
377
(*pM)[2][1] += Xz*Yy;
378
(*pM)[2][2] += Xz*Yz;
379
}
380
381
return pM;
382
}
383
384
/* ------------------------------------------------------------------
385
Matrice 4x4 Translation
386
---------- PREMULTIPLIE -> Y = XM (X vecteur ligne) -----------
387
------------------------------------------------------------------ */
388
void MkTrnsIIMat4x4(DtMatrix4x4 m, DtPoint3 tr)
389
{
390
m[0][0] = 1.; m[0][1] = 0.; m[0][2] = 0.; m[0][3] = 0.;
391
m[1][0] = 0.; m[1][1] = 1.; m[1][2] = 0.; m[1][3] = 0.;
392
m[2][0] = 0.; m[2][1] = 0.; m[2][2] = 1.; m[2][3] = 0.;
393
m[3][0] = tr[0]; m[3][1] = tr[1]; m[3][2] = tr[2]; m[3][3] = 1.;
394
}
395
396
/* Matrices 4 x 4 (a x b dans c) ------------------------------------ */
397
void mulMat4x4(DtMatrix4x4 a,DtMatrix4x4 b,DtMatrix4x4 c)
398
{
399
int i,j,k;
400
boucle(i,0,4) {
401
boucle(j,0,4) {
402
c[i][j] = 0.;
403
boucle(k,0,4) c[i][j] += a[i][k]*b[k][j];
404
}
405
}
406
}
407
408
409
/* ===============================================================
410
* ---------- Transformation d'une coordonnee par rmat. ----------
411
* =============================================================== */
412
413
void crdRotate(DtPoint3 p,DtMatrix4x4 rmat)
414
{
415
double x,y,z;
416
417
x = p[0] * rmat[0][0] + p[1] * rmat[1][0] + p[2] * rmat[2][0] + rmat[3][0];
418
y = p[0] * rmat[0][1] + p[1] * rmat[1][1] + p[2] * rmat[2][1] + rmat[3][1];
419
z = p[0] * rmat[0][2] + p[1] * rmat[1][2] + p[2] * rmat[2][2] + rmat[3][2];
420
p[0] = x;
421
p[1] = y;
422
p[2] = z;
423
}
424
425
#define SQUARED_DISTANCE(K,R) ((K)[0] - (R)[0]) * ((K)[0] - (R)[0]) + ((K)[1] - (R)[1]) * ((K)[1] - (R)[1]) + ((K)[2] - (R)[2]) * ((K)[2] - (R)[2])
426
427
DtFloat squared_distance(DtPoint3 R,DtPoint3 K)
428
{
429
return (DtFloat) ((K[0] - R[0]) * (K[0] - R[0]) +
430
(K[1] - R[1]) * (K[1] - R[1]) +
431
(K[2] - R[2]) * (K[2] - R[2]));
432
}
433
434
435
/*
436
* Best fit matrix as proposed by:
437
* Zuker & Somorjai, Bulletin of Mathematical Biology,
438
* vol. 51, No 1, p 55-78, 1989.
439
*
440
* c1, c2 two sets of coordinates to superpose.
441
* len : the number of coordiantes of c1, c2.
442
*
443
* M, the 4x4 transforamtion matrix to pass from c2 to c2 superposed onto c1.
444
*
445
* Input:
446
* c1, c2, and len must be given.
447
* M should be passed, but is meaningless.
448
*
449
* Output:
450
* c2 is superposed on c1.
451
* M, a transformation matrix ready for use.
452
*
453
*/
454
double zuker_superpose(DtPoint3 *c1, DtPoint3 *c2, int len, DtMatrix4x4 M)
455
{
456
DtMatrix3x3 C;
457
DtMatrix3x3 *pC;
458
DtMatrix4x4 RM;
459
DtMatrix4x4 TM;
460
DtMatrix4x4 TMP;
461
DtMatrix4x4 TX;
462
DtMatrix4x4 TY;
463
DtMatrix4x4 P;
464
DtMatrix4x4 Pd;
465
DtPoint4 lambdas;
466
DtPoint4 V;
467
DtPoint3 bc1, bc2;
468
DtPoint3 trx, try;
469
470
double eval;
471
double squared_rms = 0.;
472
473
int nCycles;
474
int aDot;
475
476
477
/* Compute transformation matrix as proposed by zuker */
478
pC = &C;
479
pC = XYCov(pC, (DtPoint3 *) c1, (DtPoint3 *) c2, bc1, bc2, len);
480
481
P[0][0] = -C[0][0]+C[1][1]-C[2][2];
482
P[0][1] = P[1][0] = -C[0][1]-C[1][0];
483
P[0][2] = P[2][0] = -C[1][2]-C[2][1];
484
P[0][3] = P[3][0] = C[0][2]-C[2][0];
485
486
P[1][1] = C[0][0]-C[1][1]-C[2][2];
487
P[1][2] = P[2][1] = C[0][2]+C[2][0];
488
P[1][3] = P[3][1] = C[1][2]-C[2][1];
489
490
P[2][2] = -C[0][0]-C[1][1]+C[2][2];
491
P[2][3] = P[3][2] = C[0][1]-C[1][0];
492
493
P[3][3] = C[0][0]+C[1][1]+C[2][2];
494
495
#if 0
496
printMat4x4("zuker P", P);
497
#endif
498
499
nCycles = largestEV4(P, V, &eval);
500
501
RM[0][0] = -V[0]*V[0]+V[1]*V[1]-V[2]*V[2]+V[3]*V[3];
502
RM[1][0] = 2*(V[2]*V[3]-V[0]*V[1]);
503
RM[2][0] = 2*(V[1]*V[2]+V[0]*V[3]);
504
RM[3][0] = 0.;
505
506
RM[0][1] = -2*(V[0]*V[1]+V[2]*V[3]);
507
RM[1][1] = V[0]*V[0]-V[1]*V[1]-V[2]*V[2]+V[3]*V[3];
508
RM[2][1] = 2*(V[1]*V[3]-V[0]*V[2]);
509
RM[3][1] = 0.;
510
511
RM[0][2] = 2*(V[1]*V[2]-V[0]*V[3]);
512
RM[1][2] = -2*(V[0]*V[2]+V[1]*V[3]);
513
RM[2][2] = -V[0]*V[0]-V[1]*V[1]+V[2]*V[2]+V[3]*V[3];
514
RM[3][2] = 0.;
515
516
RM[0][3] = 0.;
517
RM[1][3] = 0.;
518
RM[2][3] = 0.;
519
RM[3][3] = 1.;
520
521
/* printMat4x4("zuker RM", RM); */
522
523
/* Solution eprouvee ! */
524
try[0] = - bc2[0]; try[1] = - bc2[1]; try[2] = - bc2[2];
525
MkTrnsIIMat4x4(TY, try);
526
MkTrnsIIMat4x4(TX, bc1);
527
mulMat4x4(TY,RM,TMP);
528
mulMat4x4(TMP, TX, M);
529
530
/* Now superpose the coordinates */
531
for (aDot=0;aDot<len;aDot++) {
532
crdRotate(c2[aDot],M);
533
}
534
535
/* Compute squared RMSd */
536
for (aDot=0;aDot<len;aDot++) {
537
squared_rms += squared_distance(c1[aDot],c2[aDot]);
538
}
539
540
return squared_rms / (double) len;
541
}
542
543
Loggerhead is a web-based interface for Breezy
Version: 2.0.1