001/* 002 * BioJava development code 003 * 004 * This code may be freely distributed and modified under the 005 * terms of the GNU Lesser General Public Licence. This should 006 * be distributed with the code. If you do not have a copy, 007 * see: 008 * 009 * http://www.gnu.org/copyleft/lesser.html 010 * 011 * Copyright for this code is held jointly by the individual 012 * authors. These should be listed in @author doc comments. 013 * 014 * For more information on the BioJava project and its aims, 015 * or to join the biojava-l mailing list, visit the home page 016 * at: 017 * 018 * http://www.biojava.org/ 019 * 020 */ 021 022package org.biojava.nbio.structure.symmetry.geometry; 023 024import javax.vecmath.Matrix3d; 025import javax.vecmath.Matrix4d; 026import javax.vecmath.Point3d; 027import javax.vecmath.Vector3d; 028 029 030/** 031 * 032 * @author Peter 033 */ 034public final class SuperPositionQCP { 035 double evecprec = 1d-6; 036 double evalprec = 1d-11; 037 038 private Point3d[] x = null; 039 private Point3d[] y = null; 040 041 private double[] weight = null; 042 043 private Point3d[] xref = null; 044 private Point3d[] yref = null; 045 private Point3d xtrans; 046 private Point3d ytrans; 047 048 private double e0; 049 private Matrix3d rotmat = new Matrix3d(); 050 private Matrix4d transformation = new Matrix4d(); 051 private double rmsd = 0; 052 private double Sxy, Sxz, Syx, Syz, Szx, Szy; 053 private double SxxpSyy, Szz, mxEigenV, SyzmSzy,SxzmSzx, SxymSyx; 054 private double SxxmSyy, SxypSyx, SxzpSzx; 055 private double Syy, Sxx, SyzpSzy; 056 private boolean rmsdCalculated = false; 057 private boolean transformationCalculated = false; 058 private boolean centered = false; 059 060 061 public void set(Point3d[] x, Point3d[] y) { 062 this.x = x; 063 this.y = y; 064 rmsdCalculated = false; 065 transformationCalculated = false; 066 } 067 068 public void set(Point3d[] x, Point3d[] y, double[] weight) { 069 this.x = x; 070 this.y = y; 071 this.weight = weight; 072 rmsdCalculated = false; 073 transformationCalculated = false; 074 } 075 076 public void setCentered(boolean centered) { 077 this.centered = centered; 078 } 079 080 public double getRmsd() { 081 if (! rmsdCalculated) { 082 calcRmsd(x, y); 083 } 084 return rmsd; 085 } 086 087 public Matrix4d getTransformationMatrix() { 088 getRotationMatrix(); 089 if (! centered) { 090 calcTransformation(); 091 } else { 092 transformation.set(rotmat); 093 } 094 return transformation; 095 } 096 097 public Matrix3d getRotationMatrix() { 098 getRmsd(); 099 if (! transformationCalculated) { 100 calcRotationMatrix(); 101 } 102 return rotmat; 103 } 104 105 public Point3d[] getTransformedCoordinates() { 106 SuperPosition.transform(transformation, x); 107 return x; 108 } 109 110 /** 111 * this requires the coordinates to be precentered 112 * @param x 113 * @param y 114 */ 115 private void calcRmsd(Point3d[] x, Point3d[] y) { 116 if (centered) { 117 innerProduct(y, x); 118 } else { 119 // translate to origin 120 xref = SuperPosition.clonePoint3dArray(x); 121 xtrans = SuperPosition.centroid(xref); 122 // System.out.println("x centroid: " + xtrans); 123 xtrans.negate(); 124 SuperPosition.translate(xtrans, xref); 125 126 yref = SuperPosition.clonePoint3dArray(y); 127 ytrans = SuperPosition.centroid(yref); 128 // System.out.println("y centroid: " + ytrans); 129 ytrans.negate(); 130 SuperPosition.translate(ytrans, yref); 131 innerProduct(yref, xref); 132 } 133 calcRmsd(x.length); 134 } 135 136 /* Superposition coords2 onto coords1 -- in other words, coords2 is rotated, coords1 is held fixed */ 137 private void calcTransformation() { 138// transformation.set(rotmat,new Vector3d(0,0,0), 1); 139 transformation.set(rotmat); 140// long t2 = System.nanoTime(); 141// System.out.println("create transformation: " + (t2-t1)); 142// System.out.println("m3d -> m4d"); 143// System.out.println(transformation); 144 145 // combine with x -> origin translation 146 Matrix4d trans = new Matrix4d(); 147 trans.setIdentity(); 148 trans.setTranslation(new Vector3d(xtrans)); 149 transformation.mul(transformation, trans); 150// System.out.println("setting xtrans"); 151// System.out.println(transformation); 152// 153// // combine with origin -> y translation 154 ytrans.negate(); 155 Matrix4d transInverse = new Matrix4d(); 156 transInverse.setIdentity(); 157 transInverse.setTranslation(new Vector3d(ytrans)); 158 transformation.mul(transInverse, transformation); 159// System.out.println("setting ytrans"); 160// System.out.println(transformation); 161 } 162 163 /** 164 * http://theobald.brandeis.edu/qcp/qcprot.c 165 * @param A 166 * @param coords1 167 * @param coords2 168 * @return 169 */ 170 private void innerProduct(Point3d[] coords1, Point3d[] coords2) { 171 double x1, x2, y1, y2, z1, z2; 172 double g1 = 0.0, g2 = 0.0; 173 174 Sxx = 0; 175 Sxy = 0; 176 Sxz = 0; 177 Syx = 0; 178 Syy = 0; 179 Syz = 0; 180 Szx = 0; 181 Szy = 0; 182 Szz = 0; 183 184 if (weight != null) 185 { 186 for (int i = 0; i < coords1.length; i++) 187 { 188 x1 = weight[i] * coords1[i].x; 189 y1 = weight[i] * coords1[i].y; 190 z1 = weight[i] * coords1[i].z; 191 192 g1 += x1 * coords1[i].x + y1 * coords1[i].y + z1 * coords1[i].z; 193 194 x2 = coords2[i].x; 195 y2 = coords2[i].y; 196 z2 = coords2[i].z; 197 198 g2 += weight[i] * (x2 * x2 + y2 * y2 + z2 * z2); 199 200 Sxx += (x1 * x2); 201 Sxy += (x1 * y2); 202 Sxz += (x1 * z2); 203 204 Syx += (y1 * x2); 205 Syy += (y1 * y2); 206 Syz += (y1 * z2); 207 208 Szx += (z1 * x2); 209 Szy += (z1 * y2); 210 Szz += (z1 * z2); 211 } 212 } 213 else 214 { 215 for (int i = 0; i < coords1.length; i++) 216 { 217 g1 += coords1[i].x * coords1[i].x + coords1[i].y * coords1[i].y + coords1[i].z * coords1[i].z; 218 g2 += coords2[i].x * coords2[i].x + coords2[i].y * coords2[i].y + coords2[i].z * coords2[i].z; 219 220 Sxx += coords1[i].x * coords2[i].x; 221 Sxy += coords1[i].x * coords2[i].y; 222 Sxz += coords1[i].x * coords2[i].z; 223 224 Syx += coords1[i].y * coords2[i].x; 225 Syy += coords1[i].y * coords2[i].y; 226 Syz += coords1[i].y * coords2[i].z; 227 228 Szx += coords1[i].z * coords2[i].x; 229 Szy += coords1[i].z * coords2[i].y; 230 Szz += coords1[i].z * coords2[i].z; 231 } 232 } 233 234 e0 = (g1 + g2) * 0.5; 235 } 236 237 private int calcRmsd(int len) 238 { 239 double Sxx2 = Sxx * Sxx; 240 double Syy2 = Syy * Syy; 241 double Szz2 = Szz * Szz; 242 243 double Sxy2 = Sxy * Sxy; 244 double Syz2 = Syz * Syz; 245 double Sxz2 = Sxz * Sxz; 246 247 double Syx2 = Syx * Syx; 248 double Szy2 = Szy * Szy; 249 double Szx2 = Szx * Szx; 250 251 double SyzSzymSyySzz2 = 2.0*(Syz*Szy - Syy*Szz); 252 double Sxx2Syy2Szz2Syz2Szy2 = Syy2 + Szz2 - Sxx2 + Syz2 + Szy2; 253 254 double c2 = -2.0 * (Sxx2 + Syy2 + Szz2 + Sxy2 + Syx2 + Sxz2 + Szx2 + Syz2 + Szy2); 255 double c1 = 8.0 * (Sxx*Syz*Szy + Syy*Szx*Sxz + Szz*Sxy*Syx - Sxx*Syy*Szz - Syz*Szx*Sxy - Szy*Syx*Sxz); 256 257 SxzpSzx = Sxz + Szx; 258 SyzpSzy = Syz + Szy; 259 SxypSyx = Sxy + Syx; 260 SyzmSzy = Syz - Szy; 261 SxzmSzx = Sxz - Szx; 262 SxymSyx = Sxy - Syx; 263 SxxpSyy = Sxx + Syy; 264 SxxmSyy = Sxx - Syy; 265 266 double Sxy2Sxz2Syx2Szx2 = Sxy2 + Sxz2 - Syx2 - Szx2; 267 268 double c0 = Sxy2Sxz2Syx2Szx2 * Sxy2Sxz2Syx2Szx2 269 + (Sxx2Syy2Szz2Syz2Szy2 + SyzSzymSyySzz2) * (Sxx2Syy2Szz2Syz2Szy2 - SyzSzymSyySzz2) 270 + (-(SxzpSzx)*(SyzmSzy)+(SxymSyx)*(SxxmSyy-Szz)) * (-(SxzmSzx)*(SyzpSzy)+(SxymSyx)*(SxxmSyy+Szz)) 271 + (-(SxzpSzx)*(SyzpSzy)-(SxypSyx)*(SxxpSyy-Szz)) * (-(SxzmSzx)*(SyzmSzy)-(SxypSyx)*(SxxpSyy+Szz)) 272 + (+(SxypSyx)*(SyzpSzy)+(SxzpSzx)*(SxxmSyy+Szz)) * (-(SxymSyx)*(SyzmSzy)+(SxzpSzx)*(SxxpSyy+Szz)) 273 + (+(SxypSyx)*(SyzmSzy)+(SxzmSzx)*(SxxmSyy-Szz)) * (-(SxymSyx)*(SyzpSzy)+(SxzmSzx)*(SxxpSyy-Szz)); 274 275 mxEigenV = e0; 276 277 int i; 278 for (i = 0; i < 50; ++i) 279 { 280 double oldg = mxEigenV; 281 double x2 = mxEigenV*mxEigenV; 282 double b = (x2 + c2)*mxEigenV; 283 double a = b + c1; 284 double delta = ((a*mxEigenV + c0)/(2.0*x2*mxEigenV + b + a)); 285 mxEigenV -= delta; 286 287 if (Math.abs(mxEigenV - oldg) < Math.abs(evalprec*mxEigenV)) 288 break; 289 } 290 291 292 if (i == 50) 293 System.err.println("More than %d iterations needed!" + i); 294 295 /* the fabs() is to guard against extremely small, but *negative* numbers due to floating point error */ 296 rmsd = Math.sqrt(Math.abs(2.0 * (e0 - mxEigenV)/len)); 297 298 return 1; 299 } 300 301 private int calcRotationMatrix() { 302 double a11 = SxxpSyy + Szz-mxEigenV; 303 double a12 = SyzmSzy; 304 double a13 = - SxzmSzx; 305 double a14 = SxymSyx; 306 double a21 = SyzmSzy; 307 double a22 = SxxmSyy - Szz-mxEigenV; 308 double a23 = SxypSyx; 309 double a24= SxzpSzx; 310 double a31 = a13; 311 double a32 = a23; 312 double a33 = Syy-Sxx-Szz - mxEigenV; 313 double a34 = SyzpSzy; 314 double a41 = a14; 315 double a42 = a24; 316 double a43 = a34; 317 double a44 = Szz - SxxpSyy - mxEigenV; 318 double a3344_4334 = a33 * a44 - a43 * a34; 319 double a3244_4234 = a32 * a44-a42*a34; 320 double a3243_4233 = a32 * a43 - a42 * a33; 321 double a3143_4133 = a31 * a43-a41*a33; 322 double a3144_4134 = a31 * a44 - a41 * a34; 323 double a3142_4132 = a31 * a42-a41*a32; 324 double q1 = a22*a3344_4334-a23*a3244_4234+a24*a3243_4233; 325 double q2 = -a21*a3344_4334+a23*a3144_4134-a24*a3143_4133; 326 double q3 = a21*a3244_4234-a22*a3144_4134+a24*a3142_4132; 327 double q4 = -a21*a3243_4233+a22*a3143_4133-a23*a3142_4132; 328 329 double qsqr = q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4; 330 331 /* The following code tries to calculate another column in the adjoint matrix when the norm of the 332 * current column is too small. 333 * Usually this commented block will never be activated. To be absolutely safe this should be 334 * uncommented, but it is most likely unnecessary. 335 */ 336 if (qsqr < evecprec) 337 { 338 q1 = a12*a3344_4334 - a13*a3244_4234 + a14*a3243_4233; 339 q2 = -a11*a3344_4334 + a13*a3144_4134 - a14*a3143_4133; 340 q3 = a11*a3244_4234 - a12*a3144_4134 + a14*a3142_4132; 341 q4 = -a11*a3243_4233 + a12*a3143_4133 - a13*a3142_4132; 342 qsqr = q1*q1 + q2 *q2 + q3*q3+q4*q4; 343 344 if (qsqr < evecprec) 345 { 346 double a1324_1423 = a13 * a24 - a14 * a23, a1224_1422 = a12 * a24 - a14 * a22; 347 double a1223_1322 = a12 * a23 - a13 * a22, a1124_1421 = a11 * a24 - a14 * a21; 348 double a1123_1321 = a11 * a23 - a13 * a21, a1122_1221 = a11 * a22 - a12 * a21; 349 350 q1 = a42 * a1324_1423 - a43 * a1224_1422 + a44 * a1223_1322; 351 q2 = -a41 * a1324_1423 + a43 * a1124_1421 - a44 * a1123_1321; 352 q3 = a41 * a1224_1422 - a42 * a1124_1421 + a44 * a1122_1221; 353 q4 = -a41 * a1223_1322 + a42 * a1123_1321 - a43 * a1122_1221; 354 qsqr = q1*q1 + q2 *q2 + q3*q3+q4*q4; 355 356 if (qsqr < evecprec) 357 { 358 q1 = a32 * a1324_1423 - a33 * a1224_1422 + a34 * a1223_1322; 359 q2 = -a31 * a1324_1423 + a33 * a1124_1421 - a34 * a1123_1321; 360 q3 = a31 * a1224_1422 - a32 * a1124_1421 + a34 * a1122_1221; 361 q4 = -a31 * a1223_1322 + a32 * a1123_1321 - a33 * a1122_1221; 362 qsqr = q1*q1 + q2 *q2 + q3*q3 + q4*q4; 363 364 if (qsqr < evecprec) 365 { 366 /* if qsqr is still too small, return the identity matrix. */ 367 rotmat.setIdentity(); 368 369 return 0; 370 } 371 } 372 } 373 } 374 375 double normq = Math.sqrt(qsqr); 376 q1 /= normq; 377 q2 /= normq; 378 q3 /= normq; 379 q4 /= normq; 380 381 System.out.println("q: " + q1 + " " + q2 + " " + q3 + " " + q4); 382 383 double a2 = q1 * q1; 384 double x2 = q2 * q2; 385 double y2 = q3 * q3; 386 double z2 = q4 * q4; 387 388 double xy = q2 * q3; 389 double az = q1 * q4; 390 double zx = q4 * q2; 391 double ay = q1 * q3; 392 double yz = q3 * q4; 393 double ax = q1 * q2; 394 395 rotmat.m00 = a2 + x2 - y2 - z2; 396 rotmat.m01 = 2 * (xy + az); 397 rotmat.m02 = 2 * (zx - ay); 398 399 rotmat.m10 = 2 * (xy - az); 400 rotmat.m11 = a2 - x2 + y2 - z2; 401 rotmat.m12 = 2 * (yz + ax); 402 403 rotmat.m20 = 2 * (zx + ay); 404 rotmat.m21 = 2 * (yz - ax); 405 rotmat.m22 = a2 - x2 - y2 + z2; 406 407 return 1; 408 } 409 410}