00001
00007
00008 #include "Settle.h"
00009 #include <string.h>
00010 #include <math.h>
00011
00012
00013 #if defined(__SSE2__) && ! defined(NAMD_DISABLE_SSE)
00014 #include <emmintrin.h>
00015 #if defined(__INTEL_COMPILER)
00016 #define __align(X) __declspec(align(X) )
00017 #elif defined(__PGI)
00018 #define __align(X) __attribute__((aligned(X) ))
00019 #define MISSING_mm_cvtsd_f64
00020 #elif defined(__GNUC__)
00021 #define __align(X) __attribute__((aligned(X) ))
00022 #if (__GNUC__ < 4)
00023 #define MISSING_mm_cvtsd_f64
00024 #endif
00025 #else
00026 #define __align(X) __declspec(align(X) )
00027 #endif
00028 #endif
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 void settle1init(BigReal pmO, BigReal pmH, BigReal hhdist, BigReal ohdist,
00041 BigReal &mOrmT, BigReal &mHrmT, BigReal &ra,
00042 BigReal &rb, BigReal &rc, BigReal &rra) {
00043
00044 BigReal rmT = 1.0 / (pmO+pmH+pmH);
00045 mOrmT = pmO * rmT;
00046 mHrmT = pmH * rmT;
00047 BigReal t1 = 0.5*pmO/pmH;
00048 rc = 0.5*hhdist;
00049 ra = sqrt(ohdist*ohdist-rc*rc)/(1.0+t1);
00050 rb = t1*ra;
00051 rra = 1.0 / ra;
00052 }
00053
00054
00055 int settle1(const Vector *ref, Vector *pos, Vector *vel, BigReal invdt,
00056 BigReal mOrmT, BigReal mHrmT, BigReal ra,
00057 BigReal rb, BigReal rc, BigReal rra) {
00058 #if defined(__SSE2__) && ! defined(NAMD_DISABLE_SSE)
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070 Vector b0, c0;
00071
00072 __m128d REF0xy = _mm_loadu_pd((double *) &ref[0].x);
00073 __m128d REF1xy = _mm_loadu_pd((double *) &ref[1].x);
00074
00075 __m128d B0xy = _mm_sub_pd(REF1xy, REF0xy);
00076 _mm_storeu_pd((double *) &b0.x, B0xy);
00077 b0.z = ref[1].z - ref[0].z;
00078
00079 __m128d REF2xy = _mm_loadu_pd((double *) &ref[2].x);
00080
00081 __m128d C0xy = _mm_sub_pd(REF2xy, REF0xy);
00082 _mm_storeu_pd((double *) &c0.x, C0xy);
00083 c0.z = ref[2].z - ref[0].z;
00084
00085
00086
00087 __align(16) Vector a1;
00088 __align(16) Vector b1;
00089 __align(16) Vector c1;
00090 __align(16) Vector d0;
00091
00092 __m128d POS1xy = _mm_loadu_pd((double *) &pos[1].x);
00093 __m128d POS2xy = _mm_loadu_pd((double *) &pos[2].x);
00094 __m128d PMHrmTxy = _mm_mul_pd(_mm_add_pd(POS1xy, POS2xy), _mm_set1_pd(mHrmT));
00095
00096 __m128d POS0xy = _mm_loadu_pd((double *) &pos[0].x);
00097 __m128d PMOrmTxy = _mm_mul_pd(POS0xy, _mm_set1_pd(mOrmT));
00098 __m128d D0xy = _mm_add_pd(PMOrmTxy, PMHrmTxy);
00099
00100 d0.z = pos[0].z * mOrmT + ((pos[1].z + pos[2].z) * mHrmT);
00101 a1.z = pos[0].z - d0.z;
00102 b1.z = pos[1].z - d0.z;
00103 c1.z = pos[2].z - d0.z;
00104
00105 __m128d A1xy = _mm_sub_pd(POS0xy, D0xy);
00106 _mm_store_pd((double *) &a1.x, A1xy);
00107
00108 __m128d B1xy = _mm_sub_pd(POS1xy, D0xy);
00109 _mm_store_pd((double *) &b1.x, B1xy);
00110
00111 __m128d C1xy = _mm_sub_pd(POS2xy, D0xy);
00112 _mm_store_pd((double *) &c1.x, C1xy);
00113
00114 _mm_store_pd((double *) &d0.x, D0xy);
00115
00116
00117
00118 Vector n0 = cross(b0, c0);
00119 Vector n1 = cross(a1, n0);
00120 Vector n2 = cross(n0, n1);
00121 #else
00122
00123 Vector b0 = ref[1]-ref[0];
00124 Vector c0 = ref[2]-ref[0];
00125
00126
00127 Vector d0 = pos[0]*mOrmT + ((pos[1] + pos[2])*mHrmT);
00128
00129 Vector a1 = pos[0] - d0;
00130 Vector b1 = pos[1] - d0;
00131 Vector c1 = pos[2] - d0;
00132
00133
00134
00135 Vector n0 = cross(b0, c0);
00136 Vector n1 = cross(a1, n0);
00137 Vector n2 = cross(n0, n1);
00138 #endif
00139
00140 #if defined(__SSE2__) && ! defined(NAMD_DISABLE_SSE) && ! defined(MISSING_mm_cvtsd_f64)
00141 __m128d l1 = _mm_set_pd(n0.x, n0.y);
00142 l1 = _mm_mul_pd(l1, l1);
00143
00144 double l1xy0 = _mm_cvtsd_f64(_mm_add_sd(l1, _mm_shuffle_pd(l1, l1, 1)));
00145
00146 __m128d l3 = _mm_set_pd(n1.y, n1.z);
00147 l3 = _mm_mul_pd(l3, l3);
00148
00149 double l3yz1 = _mm_cvtsd_f64(_mm_add_sd(l3, _mm_shuffle_pd(l3, l3, 1)));
00150
00151 __m128d l2 = _mm_set_pd(n1.x, n0.z);
00152
00153 __m128d ts01 = _mm_add_pd(_mm_set_pd(l3yz1, l1xy0), _mm_mul_pd(l2, l2));
00154
00155 __m128d l4 = _mm_set_pd(n2.x, n2.y);
00156 l4 = _mm_mul_pd(l4, l4);
00157
00158 double l4xy2 = _mm_cvtsd_f64(_mm_add_sd(l4, _mm_shuffle_pd(l4, l4, 1)));
00159 double ts2 = l4xy2 + (n2.z * n2.z);
00160
00161 double invlens[4];
00162
00163
00164 static const __m128d fvecd1p0 = _mm_set1_pd(1.0);
00165
00166
00167 __m128d invlen12 = _mm_div_pd(fvecd1p0, _mm_sqrt_pd(ts01));
00168
00169
00170 _mm_storeu_pd(invlens, invlen12);
00171
00172 n0 = n0 * invlens[0];
00173
00174
00175
00176
00177 BigReal A1Z = n0 * a1;
00178 BigReal sinphi = A1Z * rra;
00179 BigReal tmp = 1.0-sinphi*sinphi;
00180
00181 __m128d n2cosphi = _mm_sqrt_pd(_mm_set_pd(tmp, ts2));
00182
00183 _mm_storeu_pd(invlens+2, n2cosphi);
00184
00185 n1 = n1 * invlens[1];
00186 n2 = n2 * (1.0 / invlens[2]);
00187 BigReal cosphi = invlens[3];
00188
00189 b0 = Vector(n1*b0, n2*b0, n0*b0);
00190 c0 = Vector(n1*c0, n2*c0, n0*c0);
00191
00192 b1 = Vector(n1*b1, n2*b1, n0*b1);
00193 c1 = Vector(n1*c1, n2*c1, n0*c1);
00194
00195
00196 BigReal sinpsi = (b1.z - c1.z)/(2.0*rc*cosphi);
00197 tmp = 1.0-sinpsi*sinpsi;
00198 BigReal cospsi = sqrt(tmp);
00199 #else
00200 n0 = n0.unit();
00201 n1 = n1.unit();
00202 n2 = n2.unit();
00203
00204 b0 = Vector(n1*b0, n2*b0, n0*b0);
00205 c0 = Vector(n1*c0, n2*c0, n0*c0);
00206
00207 BigReal A1Z = n0 * a1;
00208 b1 = Vector(n1*b1, n2*b1, n0*b1);
00209 c1 = Vector(n1*c1, n2*c1, n0*c1);
00210
00211
00212 BigReal sinphi = A1Z * rra;
00213 BigReal tmp = 1.0-sinphi*sinphi;
00214 BigReal cosphi = sqrt(tmp);
00215 BigReal sinpsi = (b1.z - c1.z)/(2.0*rc*cosphi);
00216 tmp = 1.0-sinpsi*sinpsi;
00217 BigReal cospsi = sqrt(tmp);
00218 #endif
00219
00220 BigReal rbphi = -rb*cosphi;
00221 BigReal tmp1 = rc*sinpsi*sinphi;
00222 BigReal tmp2 = rc*sinpsi*cosphi;
00223
00224 Vector a2(0, ra*cosphi, ra*sinphi);
00225 Vector b2(-rc*cospsi, rbphi - tmp1, -rb*sinphi + tmp2);
00226 Vector c2( rc*cosphi, rbphi + tmp1, -rb*sinphi - tmp2);
00227
00228
00229
00230 BigReal alpha = b2.x*(b0.x - c0.x) + b0.y*b2.y + c0.y*c2.y;
00231 BigReal beta = b2.x*(c0.y - b0.y) + b0.x*b2.y + c0.x*c2.y;
00232 BigReal gama = b0.x*b1.y - b1.x*b0.y + c0.x*c1.y - c1.x*c0.y;
00233
00234 BigReal a2b2 = alpha*alpha + beta*beta;
00235 BigReal sintheta = (alpha*gama - beta*sqrt(a2b2 - gama*gama))/a2b2;
00236 BigReal costheta = sqrt(1.0 - sintheta*sintheta);
00237
00238 #if 0
00239 Vector a3( -a2.y*sintheta,
00240 a2.y*costheta,
00241 a2.z);
00242 Vector b3(b2.x*costheta - b2.y*sintheta,
00243 b2.x*sintheta + b2.y*costheta,
00244 b2.z);
00245 Vector c3(c2.x*costheta - c2.y*sintheta,
00246 c2.x*sintheta + c2.y*costheta,
00247 c2.z);
00248
00249 #else
00250 Vector a3( -a2.y*sintheta,
00251 a2.y*costheta,
00252 A1Z);
00253 Vector b3(b2.x*costheta - b2.y*sintheta,
00254 b2.x*sintheta + b2.y*costheta,
00255 b1.z);
00256 Vector c3(-b2.x*costheta - c2.y*sintheta,
00257 -b2.x*sintheta + c2.y*costheta,
00258 c1.z);
00259
00260 #endif
00261
00262
00263 Vector m1(n1.x, n2.x, n0.x);
00264 Vector m2(n1.y, n2.y, n0.y);
00265 Vector m0(n1.z, n2.z, n0.z);
00266
00267 pos[0] = Vector(a3*m1, a3*m2, a3*m0) + d0;
00268 pos[1] = Vector(b3*m1, b3*m2, b3*m0) + d0;
00269 pos[2] = Vector(c3*m1, c3*m2, c3*m0) + d0;
00270
00271
00272 if (invdt != 0) {
00273 vel[0] = (pos[0]-ref[0])*invdt;
00274 vel[1] = (pos[1]-ref[1])*invdt;
00275 vel[2] = (pos[2]-ref[2])*invdt;
00276 }
00277
00278 return 0;
00279 }
00280
00281
00282
00283
00284 template <int veclen>
00285 void settle1_SIMD(const Vector *ref, Vector *pos,
00286 BigReal mOrmT, BigReal mHrmT, BigReal ra,
00287 BigReal rb, BigReal rc, BigReal rra) {
00288
00289 BigReal ref0xt[veclen];
00290 BigReal ref0yt[veclen];
00291 BigReal ref0zt[veclen];
00292 BigReal ref1xt[veclen];
00293 BigReal ref1yt[veclen];
00294 BigReal ref1zt[veclen];
00295 BigReal ref2xt[veclen];
00296 BigReal ref2yt[veclen];
00297 BigReal ref2zt[veclen];
00298
00299 BigReal pos0xt[veclen];
00300 BigReal pos0yt[veclen];
00301 BigReal pos0zt[veclen];
00302 BigReal pos1xt[veclen];
00303 BigReal pos1yt[veclen];
00304 BigReal pos1zt[veclen];
00305 BigReal pos2xt[veclen];
00306 BigReal pos2yt[veclen];
00307 BigReal pos2zt[veclen];
00308
00309 for (int i=0;i < veclen;i++) {
00310 ref0xt[i] = ref[i*3+0].x;
00311 ref0yt[i] = ref[i*3+0].y;
00312 ref0zt[i] = ref[i*3+0].z;
00313 ref1xt[i] = ref[i*3+1].x;
00314 ref1yt[i] = ref[i*3+1].y;
00315 ref1zt[i] = ref[i*3+1].z;
00316 ref2xt[i] = ref[i*3+2].x;
00317 ref2yt[i] = ref[i*3+2].y;
00318 ref2zt[i] = ref[i*3+2].z;
00319
00320 pos0xt[i] = pos[i*3+0].x;
00321 pos0yt[i] = pos[i*3+0].y;
00322 pos0zt[i] = pos[i*3+0].z;
00323 pos1xt[i] = pos[i*3+1].x;
00324 pos1yt[i] = pos[i*3+1].y;
00325 pos1zt[i] = pos[i*3+1].z;
00326 pos2xt[i] = pos[i*3+2].x;
00327 pos2yt[i] = pos[i*3+2].y;
00328 pos2zt[i] = pos[i*3+2].z;
00329 }
00330
00331 #pragma omp simd
00332 for (int i=0;i < veclen;i++) {
00333
00334 BigReal ref0x = ref0xt[i];
00335 BigReal ref0y = ref0yt[i];
00336 BigReal ref0z = ref0zt[i];
00337 BigReal ref1x = ref1xt[i];
00338 BigReal ref1y = ref1yt[i];
00339 BigReal ref1z = ref1zt[i];
00340 BigReal ref2x = ref2xt[i];
00341 BigReal ref2y = ref2yt[i];
00342 BigReal ref2z = ref2zt[i];
00343
00344 BigReal pos0x = pos0xt[i];
00345 BigReal pos0y = pos0yt[i];
00346 BigReal pos0z = pos0zt[i];
00347 BigReal pos1x = pos1xt[i];
00348 BigReal pos1y = pos1yt[i];
00349 BigReal pos1z = pos1zt[i];
00350 BigReal pos2x = pos2xt[i];
00351 BigReal pos2y = pos2yt[i];
00352 BigReal pos2z = pos2zt[i];
00353
00354
00355 BigReal b0x = ref1x - ref0x;
00356 BigReal b0y = ref1y - ref0y;
00357 BigReal b0z = ref1z - ref0z;
00358
00359 BigReal c0x = ref2x - ref0x;
00360 BigReal c0y = ref2y - ref0y;
00361 BigReal c0z = ref2z - ref0z;
00362
00363
00364 BigReal d0x = pos0x*mOrmT + ((pos1x + pos2x)*mHrmT);
00365 BigReal d0y = pos0y*mOrmT + ((pos1y + pos2y)*mHrmT);
00366 BigReal d0z = pos0z*mOrmT + ((pos1z + pos2z)*mHrmT);
00367
00368 BigReal a1x = pos0x - d0x;
00369 BigReal a1y = pos0y - d0y;
00370 BigReal a1z = pos0z - d0z;
00371
00372 BigReal b1x = pos1x - d0x;
00373 BigReal b1y = pos1y - d0y;
00374 BigReal b1z = pos1z - d0z;
00375
00376 BigReal c1x = pos2x - d0x;
00377 BigReal c1y = pos2y - d0y;
00378 BigReal c1z = pos2z - d0z;
00379
00380
00381
00382
00383 BigReal n0x = b0y*c0z-c0y*b0z;
00384 BigReal n0y = c0x*b0z-b0x*c0z;
00385 BigReal n0z = b0x*c0y-c0x*b0y;
00386
00387
00388 BigReal n1x = a1y*n0z-n0y*a1z;
00389 BigReal n1y = n0x*a1z-a1x*n0z;
00390 BigReal n1z = a1x*n0y-n0x*a1y;
00391
00392
00393 BigReal n2x = n0y*n1z-n1y*n0z;
00394 BigReal n2y = n1x*n0z-n0x*n1z;
00395 BigReal n2z = n0x*n1y-n1x*n0y;
00396
00397
00398 BigReal n0inv = 1.0/sqrt(n0x*n0x + n0y*n0y + n0z*n0z);
00399 n0x *= n0inv;
00400 n0y *= n0inv;
00401 n0z *= n0inv;
00402
00403 BigReal n1inv = 1.0/sqrt(n1x*n1x + n1y*n1y + n1z*n1z);
00404 n1x *= n1inv;
00405 n1y *= n1inv;
00406 n1z *= n1inv;
00407
00408 BigReal n2inv = 1.0/sqrt(n2x*n2x + n2y*n2y + n2z*n2z);
00409 n2x *= n2inv;
00410 n2y *= n2inv;
00411 n2z *= n2inv;
00412
00413
00414 BigReal n1b0 = n1x*b0x + n1y*b0y + n1z*b0z;
00415 BigReal n2b0 = n2x*b0x + n2y*b0y + n2z*b0z;
00416
00417
00418 BigReal n1c0 = n1x*c0x + n1y*c0y + n1z*c0z;
00419 BigReal n2c0 = n2x*c0x + n2y*c0y + n2z*c0z;
00420
00421 BigReal A1Z = n0x*a1x + n0y*a1y + n0z*a1z;
00422
00423
00424 BigReal n1b1 = n1x*b1x + n1y*b1y + n1z*b1z;
00425 BigReal n2b1 = n2x*b1x + n2y*b1y + n2z*b1z;
00426 BigReal n0b1 = n0x*b1x + n0y*b1y + n0z*b1z;
00427
00428
00429 BigReal n1c1 = n1x*c1x + n1y*c1y + n1z*c1z;
00430 BigReal n2c1 = n2x*c1x + n2y*c1y + n2z*c1z;
00431 BigReal n0c1 = n0x*c1x + n0y*c1y + n0z*c1z;
00432
00433
00434 BigReal sinphi = A1Z * rra;
00435 BigReal tmp = 1.0-sinphi*sinphi;
00436 BigReal cosphi = sqrt(tmp);
00437 BigReal sinpsi = (n0b1 - n0c1)/(2.0*rc*cosphi);
00438 tmp = 1.0-sinpsi*sinpsi;
00439 BigReal cospsi = sqrt(tmp);
00440
00441 BigReal rbphi = -rb*cosphi;
00442 BigReal tmp1 = rc*sinpsi*sinphi;
00443 BigReal tmp2 = rc*sinpsi*cosphi;
00444
00445
00446 BigReal a2y = ra*cosphi;
00447
00448
00449 BigReal b2x = -rc*cospsi;
00450 BigReal b2y = rbphi - tmp1;
00451
00452
00453 BigReal c2y = rbphi + tmp1;
00454
00455
00456
00457 BigReal alpha = b2x*(n1b0 - n1c0) + n2b0*b2y + n2c0*c2y;
00458 BigReal beta = b2x*(n2c0 - n2b0) + n1b0*b2y + n1c0*c2y;
00459 BigReal gama = n1b0*n2b1 - n1b1*n2b0 + n1c0*n2c1 - n1c1*n2c0;
00460
00461 BigReal a2b2 = alpha*alpha + beta*beta;
00462 BigReal sintheta = (alpha*gama - beta*sqrt(a2b2 - gama*gama))/a2b2;
00463 BigReal costheta = sqrt(1.0 - sintheta*sintheta);
00464
00465
00466
00467
00468 BigReal a3x = -a2y*sintheta;
00469 BigReal a3y = a2y*costheta;
00470 BigReal a3z = A1Z;
00471
00472
00473
00474
00475 BigReal b3x = b2x*costheta - b2y*sintheta;
00476 BigReal b3y = b2x*sintheta + b2y*costheta;
00477 BigReal b3z = n0b1;
00478
00479
00480
00481
00482 BigReal c3x = -b2x*costheta - c2y*sintheta;
00483 BigReal c3y = -b2x*sintheta + c2y*costheta;
00484 BigReal c3z = n0c1;
00485
00486
00487
00488 BigReal m1x = n1x;
00489 BigReal m1y = n2x;
00490 BigReal m1z = n0x;
00491
00492
00493 BigReal m2x = n1y;
00494 BigReal m2y = n2y;
00495 BigReal m2z = n0y;
00496
00497
00498 BigReal m0x = n1z;
00499 BigReal m0y = n2z;
00500 BigReal m0z = n0z;
00501
00502
00503 pos0x = a3x*m1x + a3y*m1y + a3z*m1z + d0x;
00504 pos0y = a3x*m2x + a3y*m2y + a3z*m2z + d0y;
00505 pos0z = a3x*m0x + a3y*m0y + a3z*m0z + d0z;
00506
00507
00508 pos1x = b3x*m1x + b3y*m1y + b3z*m1z + d0x;
00509 pos1y = b3x*m2x + b3y*m2y + b3z*m2z + d0y;
00510 pos1z = b3x*m0x + b3y*m0y + b3z*m0z + d0z;
00511
00512
00513 pos2x = c3x*m1x + c3y*m1y + c3z*m1z + d0x;
00514 pos2y = c3x*m2x + c3y*m2y + c3z*m2z + d0y;
00515 pos2z = c3x*m0x + c3y*m0y + c3z*m0z + d0z;
00516
00517 pos0xt[i] = pos0x;
00518 pos0yt[i] = pos0y;
00519 pos0zt[i] = pos0z;
00520 pos1xt[i] = pos1x;
00521 pos1yt[i] = pos1y;
00522 pos1zt[i] = pos1z;
00523 pos2xt[i] = pos2x;
00524 pos2yt[i] = pos2y;
00525 pos2zt[i] = pos2z;
00526 }
00527
00528 for (int i=0;i < veclen;i++) {
00529 pos[i*3+0].x = pos0xt[i];
00530 pos[i*3+0].y = pos0yt[i];
00531 pos[i*3+0].z = pos0zt[i];
00532 pos[i*3+1].x = pos1xt[i];
00533 pos[i*3+1].y = pos1yt[i];
00534 pos[i*3+1].z = pos1zt[i];
00535 pos[i*3+2].x = pos2xt[i];
00536 pos[i*3+2].y = pos2yt[i];
00537 pos[i*3+2].z = pos2zt[i];
00538 }
00539
00540 }
00541
00542
00543
00544
00545 template <int veclen>
00546 void rattlePair(const RattleParam* rattleParam,
00547 const BigReal *refx, const BigReal *refy, const BigReal *refz,
00548 BigReal *posx, BigReal *posy, BigReal *posz, bool& consFailure) {
00549
00550 int a = rattleParam[0].ia;
00551 int b = rattleParam[0].ib;
00552 BigReal pabx = posx[a] - posx[b];
00553 BigReal paby = posy[a] - posy[b];
00554 BigReal pabz = posz[a] - posz[b];
00555 BigReal pabsq = pabx*pabx + paby*paby + pabz*pabz;
00556 BigReal rabsq = rattleParam[0].dsq;
00557 BigReal diffsq = rabsq - pabsq;
00558 BigReal rabx = refx[a] - refx[b];
00559 BigReal raby = refy[a] - refy[b];
00560 BigReal rabz = refz[a] - refz[b];
00561
00562 BigReal refsq = rabx*rabx + raby*raby + rabz*rabz;
00563
00564 BigReal rpab = rabx*pabx + raby*paby + rabz*pabz;
00565
00566 BigReal rma = rattleParam[0].rma;
00567 BigReal rmb = rattleParam[0].rmb;
00568
00569 BigReal gab;
00570 BigReal sqrtarg = rpab*rpab + refsq*diffsq;
00571 if ( sqrtarg < 0. ) {
00572 consFailure = true;
00573 gab = 0.;
00574 } else {
00575 consFailure = false;
00576 gab = (-rpab + sqrt(sqrtarg))/(refsq*(rma + rmb));
00577 }
00578
00579 BigReal dpx = rabx * gab;
00580 BigReal dpy = raby * gab;
00581 BigReal dpz = rabz * gab;
00582 posx[a] += rma * dpx;
00583 posy[a] += rma * dpy;
00584 posz[a] += rma * dpz;
00585 posx[b] -= rmb * dpx;
00586 posy[b] -= rmb * dpy;
00587 posz[b] -= rmb * dpz;
00588
00589 }
00590
00591 void rattleN(const int icnt, const RattleParam* rattleParam,
00592 const BigReal *refx, const BigReal *refy, const BigReal *refz,
00593 BigReal *posx, BigReal *posy, BigReal *posz,
00594 const BigReal tol2, const int maxiter,
00595 bool& done, bool& consFailure) {
00596
00597 for (int iter = 0; iter < maxiter; ++iter ) {
00598 done = true;
00599 consFailure = false;
00600 for (int i = 0; i < icnt; ++i ) {
00601 int a = rattleParam[i].ia;
00602 int b = rattleParam[i].ib;
00603 BigReal pabx = posx[a] - posx[b];
00604 BigReal paby = posy[a] - posy[b];
00605 BigReal pabz = posz[a] - posz[b];
00606 BigReal pabsq = pabx*pabx + paby*paby + pabz*pabz;
00607 BigReal rabsq = rattleParam[i].dsq;
00608 BigReal diffsq = rabsq - pabsq;
00609 if ( fabs(diffsq) > (rabsq * tol2) ) {
00610 BigReal rabx = refx[a] - refx[b];
00611 BigReal raby = refy[a] - refy[b];
00612 BigReal rabz = refz[a] - refz[b];
00613 BigReal rpab = rabx*pabx + raby*paby + rabz*pabz;
00614 if ( rpab < ( rabsq * 1.0e-6 ) ) {
00615 done = false;
00616 consFailure = true;
00617 continue;
00618 }
00619 BigReal rma = rattleParam[i].rma;
00620 BigReal rmb = rattleParam[i].rmb;
00621 BigReal gab = diffsq / ( 2.0 * ( rma + rmb ) * rpab );
00622 BigReal dpx = rabx * gab;
00623 BigReal dpy = raby * gab;
00624 BigReal dpz = rabz * gab;
00625 posx[a] += rma * dpx;
00626 posy[a] += rma * dpy;
00627 posz[a] += rma * dpz;
00628 posx[b] -= rmb * dpx;
00629 posy[b] -= rmb * dpy;
00630 posz[b] -= rmb * dpz;
00631 done = false;
00632 }
00633 }
00634 if ( done ) break;
00635 }
00636
00637 }
00638
00639
00640
00641
00642 template void rattlePair<1>(const RattleParam* rattleParam,
00643 const BigReal *refx, const BigReal *refy, const BigReal *refz,
00644 BigReal *posx, BigReal *posy, BigReal *posz, bool& consFailure);
00645 template void settle1_SIMD<2>(const Vector *ref, Vector *pos,
00646 BigReal mOrmT, BigReal mHrmT, BigReal ra,
00647 BigReal rb, BigReal rc, BigReal rra);
00648 template void settle1_SIMD<1>(const Vector *ref, Vector *pos,
00649 BigReal mOrmT, BigReal mHrmT, BigReal ra,
00650 BigReal rb, BigReal rc, BigReal rra);
00651
00652 static int settlev(const Vector *pos, BigReal ma, BigReal mb, Vector *vel,
00653 BigReal dt, Tensor *virial) {
00654
00655 Vector rAB = pos[1]-pos[0];
00656 Vector rBC = pos[2]-pos[1];
00657 Vector rCA = pos[0]-pos[2];
00658
00659 Vector AB = rAB.unit();
00660 Vector BC = rBC.unit();
00661 Vector CA = rCA.unit();
00662
00663 BigReal cosA = -AB * CA;
00664 BigReal cosB = -BC * AB;
00665 BigReal cosC = -CA * BC;
00666
00667 BigReal vab = (vel[1]-vel[0])*AB;
00668 BigReal vbc = (vel[2]-vel[1])*BC;
00669 BigReal vca = (vel[0]-vel[2])*CA;
00670
00671 BigReal mab = ma+mb;
00672
00673 BigReal d = (2*mab*mab + 2*ma*mb*cosA*cosB*cosC - 2*mb*mb*cosA*cosA
00674 - ma*mab*(cosB*cosB + cosC*cosC))*0.5/mb;
00675
00676 BigReal tab = (vab*(2*mab - ma*cosC*cosC) +
00677 vbc*(mb*cosC*cosA - mab*cosB) +
00678 vca*(ma*cosB*cosC - 2*mb*cosA))*ma/d;
00679
00680 BigReal tbc = (vbc*(mab*mab - mb*mb*cosA*cosA) +
00681 vca*ma*(mb*cosA*cosB - mab*cosC) +
00682 vab*ma*(mb*cosC*cosA - mab*cosB))/d;
00683
00684 BigReal tca = (vca*(2*mab - ma*cosB*cosB) +
00685 vab*(ma*cosB*cosC - 2*mb*cosA) +
00686 vbc*(mb*cosA*cosB - mab*cosC))*ma/d;
00687
00688 Vector ga = tab*AB - tca*CA;
00689 Vector gb = tbc*BC - tab*AB;
00690 Vector gc = tca*CA - tbc*BC;
00691 #if 0
00692 if (virial) {
00693 *virial += 0.5*outer(tab, rAB)/dt;
00694 *virial += 0.5*outer(tbc, rBC)/dt;
00695 *virial += 0.5*outer(tca, rCA)/dt;
00696 }
00697 #endif
00698 vel[0] += (0.5/ma)*ga;
00699 vel[1] += (0.5/mb)*gb;
00700 vel[2] += (0.5/mb)*gc;
00701
00702 return 0;
00703 }
00704
00705 int settle2(BigReal mO, BigReal mH, const Vector *pos,
00706 Vector *vel, BigReal dt, Tensor *virial) {
00707
00708 settlev(pos, mO, mH, vel, dt, virial);
00709 return 0;
00710 }
00711