--- trunk/OOPSE/libmdtools/DirectionalAtom.cpp 2003/07/14 21:48:43 599 +++ trunk/OOPSE/libmdtools/DirectionalAtom.cpp 2003/12/12 15:42:13 878 @@ -1,16 +1,95 @@ -#include +#include #include "Atom.hpp" +#include "simError.h" +void DirectionalAtom::zeroForces() { + if( hasCoords ){ + frc[offsetX] = 0.0; + frc[offsetY] = 0.0; + frc[offsetZ] = 0.0; + + trq[offsetX] = 0.0; + trq[offsetY] = 0.0; + trq[offsetZ] = 0.0; + } + else{ + + sprintf( painCave.errMsg, + "Attempt to zero frc and trq for atom %d before coords set.\n", + index ); + painCave.isFatal = 1; + simError(); + } +} + +void DirectionalAtom::setCoords(void){ + + if( myConfig->isAllocated() ){ + + myConfig->getAtomPointers( index, + &pos, + &vel, + &frc, + &trq, + &Amat, + &mu, + &ul ); + } + else{ + sprintf( painCave.errMsg, + "Attempted to set Atom %d coordinates with an unallocated " + "SimState object.\n", index ); + painCave.isFatal = 1; + simError(); + } + + hasCoords = true; + + *mu = myMu; + +} + +double DirectionalAtom::getMu( void ) { + + if( hasCoords ){ + return *mu; + } + else{ + return myMu; + } +} + +void DirectionalAtom::setMu( double the_mu ) { + + if( hasCoords ){ + *mu = the_mu; + myMu = the_mu; + } + else{ + myMu = the_mu; + } +} + void DirectionalAtom::setA( double the_A[3][3] ){ - - Amat[Axx] = the_A[0][0]; Amat[Axy] = the_A[0][1]; Amat[Axz] = the_A[0][2]; - Amat[Ayx] = the_A[1][0]; Amat[Ayy] = the_A[1][1]; Amat[Ayz] = the_A[1][2]; - Amat[Azx] = the_A[2][0]; Amat[Azy] = the_A[2][1]; Amat[Azz] = the_A[2][2]; - this->updateU(); + if( hasCoords ){ + Amat[Axx] = the_A[0][0]; Amat[Axy] = the_A[0][1]; Amat[Axz] = the_A[0][2]; + Amat[Ayx] = the_A[1][0]; Amat[Ayy] = the_A[1][1]; Amat[Ayz] = the_A[1][2]; + Amat[Azx] = the_A[2][0]; Amat[Azy] = the_A[2][1]; Amat[Azz] = the_A[2][2]; + + this->updateU(); + } + else{ + + sprintf( painCave.errMsg, + "Attempt to set Amat for atom %d before coords set.\n", + index ); + painCave.isFatal = 1; + simError(); + } } void DirectionalAtom::setI( double the_I[3][3] ){ @@ -24,48 +103,80 @@ void DirectionalAtom::setQ( double the_q[4] ){ double q0Sqr, q1Sqr, q2Sqr, q3Sqr; - q0Sqr = the_q[0] * the_q[0]; - q1Sqr = the_q[1] * the_q[1]; - q2Sqr = the_q[2] * the_q[2]; - q3Sqr = the_q[3] * the_q[3]; - + if( hasCoords ){ + q0Sqr = the_q[0] * the_q[0]; + q1Sqr = the_q[1] * the_q[1]; + q2Sqr = the_q[2] * the_q[2]; + q3Sqr = the_q[3] * the_q[3]; + + + Amat[Axx] = q0Sqr + q1Sqr - q2Sqr - q3Sqr; + Amat[Axy] = 2.0 * ( the_q[1] * the_q[2] + the_q[0] * the_q[3] ); + Amat[Axz] = 2.0 * ( the_q[1] * the_q[3] - the_q[0] * the_q[2] ); + + Amat[Ayx] = 2.0 * ( the_q[1] * the_q[2] - the_q[0] * the_q[3] ); + Amat[Ayy] = q0Sqr - q1Sqr + q2Sqr - q3Sqr; + Amat[Ayz] = 2.0 * ( the_q[2] * the_q[3] + the_q[0] * the_q[1] ); + + Amat[Azx] = 2.0 * ( the_q[1] * the_q[3] + the_q[0] * the_q[2] ); + Amat[Azy] = 2.0 * ( the_q[2] * the_q[3] - the_q[0] * the_q[1] ); + Amat[Azz] = q0Sqr - q1Sqr -q2Sqr +q3Sqr; + + this->updateU(); + } + else{ + + sprintf( painCave.errMsg, + "Attempt to set Q for atom %d before coords set.\n", + index ); + painCave.isFatal = 1; + simError(); + } - Amat[Axx] = q0Sqr + q1Sqr - q2Sqr - q3Sqr; - Amat[Axy] = 2.0 * ( the_q[1] * the_q[2] + the_q[0] * the_q[3] ); - Amat[Axz] = 2.0 * ( the_q[1] * the_q[3] - the_q[0] * the_q[2] ); - - Amat[Ayx] = 2.0 * ( the_q[1] * the_q[2] - the_q[0] * the_q[3] ); - Amat[Ayy] = q0Sqr - q1Sqr + q2Sqr - q3Sqr; - Amat[Ayz] = 2.0 * ( the_q[2] * the_q[3] + the_q[0] * the_q[1] ); - - Amat[Azx] = 2.0 * ( the_q[1] * the_q[3] + the_q[0] * the_q[2] ); - Amat[Azy] = 2.0 * ( the_q[2] * the_q[3] - the_q[0] * the_q[1] ); - Amat[Azz] = q0Sqr - q1Sqr -q2Sqr +q3Sqr; - - this->updateU(); } void DirectionalAtom::getA( double the_A[3][3] ){ - the_A[0][0] = Amat[Axx]; - the_A[0][1] = Amat[Axy]; - the_A[0][2] = Amat[Axz]; + if( hasCoords ){ + the_A[0][0] = Amat[Axx]; + the_A[0][1] = Amat[Axy]; + the_A[0][2] = Amat[Axz]; + + the_A[1][0] = Amat[Ayx]; + the_A[1][1] = Amat[Ayy]; + the_A[1][2] = Amat[Ayz]; + + the_A[2][0] = Amat[Azx]; + the_A[2][1] = Amat[Azy]; + the_A[2][2] = Amat[Azz]; + } + else{ + + sprintf( painCave.errMsg, + "Attempt to get Amat for atom %d before coords set.\n", + index ); + painCave.isFatal = 1; + simError(); + } - the_A[1][0] = Amat[Ayx]; - the_A[1][1] = Amat[Ayy]; - the_A[1][2] = Amat[Ayz]; - - the_A[2][0] = Amat[Azx]; - the_A[2][1] = Amat[Azy]; - the_A[2][2] = Amat[Azz]; } void DirectionalAtom::printAmatIndex( void ){ - std::cerr << "Atom[" << index << "] index =>\n" - << "[ " << Axx << ", " << Axy << ", " << Axz << " ]\n" - << "[ " << Ayx << ", " << Ayy << ", " << Ayz << " ]\n" - << "[ " << Azx << ", " << Azy << ", " << Azz << " ]\n"; + if( hasCoords ){ + std::cerr << "Atom[" << index << "] index =>\n" + << "[ " << Axx << ", " << Axy << ", " << Axz << " ]\n" + << "[ " << Ayx << ", " << Ayy << ", " << Ayz << " ]\n" + << "[ " << Azx << ", " << Azy << ", " << Azz << " ]\n"; + } + else{ + + sprintf( painCave.errMsg, + "Attempt to print Amat indices for atom %d before coords set.\n", + index ); + painCave.isFatal = 1; + simError(); + } } @@ -83,64 +194,85 @@ void DirectionalAtom::getQ( double q[4] ){ double t, s; double ad1, ad2, ad3; - t = Amat[Axx] + Amat[Ayy] + Amat[Azz] + 1.0; - if( t > 0.0 ){ + if( hasCoords ){ - s = 0.5 / sqrt( t ); - q[0] = 0.25 / s; - q[1] = (Amat[Ayz] - Amat[Azy]) * s; - q[2] = (Amat[Azx] - Amat[Axz]) * s; - q[3] = (Amat[Axy] - Amat[Ayx]) * s; - } - else{ - - ad1 = fabs( Amat[Axx] ); - ad2 = fabs( Amat[Ayy] ); - ad3 = fabs( Amat[Azz] ); - - if( ad1 >= ad2 && ad1 >= ad3 ){ + t = Amat[Axx] + Amat[Ayy] + Amat[Azz] + 1.0; + if( t > 0.0 ){ - s = 2.0 * sqrt( 1.0 + Amat[Axx] - Amat[Ayy] - Amat[Azz] ); - q[0] = (Amat[Ayz] + Amat[Azy]) / s; - q[1] = 0.5 / s; - q[2] = (Amat[Axy] + Amat[Ayx]) / s; - q[3] = (Amat[Axz] + Amat[Azx]) / s; + s = 0.5 / sqrt( t ); + q[0] = 0.25 / s; + q[1] = (Amat[Ayz] - Amat[Azy]) * s; + q[2] = (Amat[Azx] - Amat[Axz]) * s; + q[3] = (Amat[Axy] - Amat[Ayx]) * s; } - else if( ad2 >= ad1 && ad2 >= ad3 ){ - - s = sqrt( 1.0 + Amat[Ayy] - Amat[Axx] - Amat[Azz] ) * 2.0; - q[0] = (Amat[Axz] + Amat[Azx]) / s; - q[1] = (Amat[Axy] + Amat[Ayx]) / s; - q[2] = 0.5 / s; - q[3] = (Amat[Ayz] + Amat[Azy]) / s; - } else{ - s = sqrt( 1.0 + Amat[Azz] - Amat[Axx] - Amat[Ayy] ) * 2.0; - q[0] = (Amat[Axy] + Amat[Ayx]) / s; - q[1] = (Amat[Axz] + Amat[Azx]) / s; - q[2] = (Amat[Ayz] + Amat[Azy]) / s; - q[3] = 0.5 / s; + ad1 = fabs( Amat[Axx] ); + ad2 = fabs( Amat[Ayy] ); + ad3 = fabs( Amat[Azz] ); + + if( ad1 >= ad2 && ad1 >= ad3 ){ + + s = 2.0 * sqrt( 1.0 + Amat[Axx] - Amat[Ayy] - Amat[Azz] ); + q[0] = (Amat[Ayz] + Amat[Azy]) / s; + q[1] = 0.5 / s; + q[2] = (Amat[Axy] + Amat[Ayx]) / s; + q[3] = (Amat[Axz] + Amat[Azx]) / s; + } + else if( ad2 >= ad1 && ad2 >= ad3 ){ + + s = sqrt( 1.0 + Amat[Ayy] - Amat[Axx] - Amat[Azz] ) * 2.0; + q[0] = (Amat[Axz] + Amat[Azx]) / s; + q[1] = (Amat[Axy] + Amat[Ayx]) / s; + q[2] = 0.5 / s; + q[3] = (Amat[Ayz] + Amat[Azy]) / s; + } + else{ + + s = sqrt( 1.0 + Amat[Azz] - Amat[Axx] - Amat[Ayy] ) * 2.0; + q[0] = (Amat[Axy] + Amat[Ayx]) / s; + q[1] = (Amat[Axz] + Amat[Azx]) / s; + q[2] = (Amat[Ayz] + Amat[Azy]) / s; + q[3] = 0.5 / s; + } } } + else{ + + sprintf( painCave.errMsg, + "Attempt to get Q for atom %d before coords set.\n", + index ); + painCave.isFatal = 1; + simError(); + } } void DirectionalAtom::setEuler( double phi, double theta, double psi ){ - Amat[Axx] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi)); - Amat[Axy] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi)); - Amat[Axz] = sin(theta) * sin(psi); - - Amat[Ayx] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi)); - Amat[Ayy] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi)); - Amat[Ayz] = sin(theta) * cos(psi); - - Amat[Azx] = sin(phi) * sin(theta); - Amat[Azy] = -cos(phi) * sin(theta); - Amat[Azz] = cos(theta); - - this->updateU(); + if( hasCoords ){ + Amat[Axx] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi)); + Amat[Axy] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi)); + Amat[Axz] = sin(theta) * sin(psi); + + Amat[Ayx] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi)); + Amat[Ayy] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi)); + Amat[Ayz] = sin(theta) * cos(psi); + + Amat[Azx] = sin(phi) * sin(theta); + Amat[Azy] = -cos(phi) * sin(theta); + Amat[Azz] = cos(theta); + + this->updateU(); + } + else{ + + sprintf( painCave.errMsg, + "Attempt to set Euler angles for atom %d before coords set.\n", + index ); + painCave.isFatal = 1; + simError(); + } } @@ -148,33 +280,64 @@ void DirectionalAtom::lab2Body( double r[3] ){ double rl[3]; // the lab frame vector - rl[0] = r[0]; - rl[1] = r[1]; - rl[2] = r[2]; + if( hasCoords ){ + rl[0] = r[0]; + rl[1] = r[1]; + rl[2] = r[2]; + + r[0] = (Amat[Axx] * rl[0]) + (Amat[Axy] * rl[1]) + (Amat[Axz] * rl[2]); + r[1] = (Amat[Ayx] * rl[0]) + (Amat[Ayy] * rl[1]) + (Amat[Ayz] * rl[2]); + r[2] = (Amat[Azx] * rl[0]) + (Amat[Azy] * rl[1]) + (Amat[Azz] * rl[2]); + } + else{ + + sprintf( painCave.errMsg, + "Attempt to convert lab2body for atom %d before coords set.\n", + index ); + painCave.isFatal = 1; + simError(); + } - r[0] = (Amat[Axx] * rl[0]) + (Amat[Axy] * rl[1]) + (Amat[Axz] * rl[2]); - r[1] = (Amat[Ayx] * rl[0]) + (Amat[Ayy] * rl[1]) + (Amat[Ayz] * rl[2]); - r[2] = (Amat[Azx] * rl[0]) + (Amat[Azy] * rl[1]) + (Amat[Azz] * rl[2]); } void DirectionalAtom::body2Lab( double r[3] ){ double rb[3]; // the body frame vector - rb[0] = r[0]; - rb[1] = r[1]; - rb[2] = r[2]; - - r[0] = (Amat[Axx] * rb[0]) + (Amat[Ayx] * rb[1]) + (Amat[Azx] * rb[2]); - r[1] = (Amat[Axy] * rb[0]) + (Amat[Ayy] * rb[1]) + (Amat[Azy] * rb[2]); - r[2] = (Amat[Axz] * rb[0]) + (Amat[Ayz] * rb[1]) + (Amat[Azz] * rb[2]); + if( hasCoords ){ + rb[0] = r[0]; + rb[1] = r[1]; + rb[2] = r[2]; + + r[0] = (Amat[Axx] * rb[0]) + (Amat[Ayx] * rb[1]) + (Amat[Azx] * rb[2]); + r[1] = (Amat[Axy] * rb[0]) + (Amat[Ayy] * rb[1]) + (Amat[Azy] * rb[2]); + r[2] = (Amat[Axz] * rb[0]) + (Amat[Ayz] * rb[1]) + (Amat[Azz] * rb[2]); + } + else{ + + sprintf( painCave.errMsg, + "Attempt to convert body2lab for atom %d before coords set.\n", + index ); + painCave.isFatal = 1; + simError(); + } } void DirectionalAtom::updateU( void ){ - ul[offsetX] = (Amat[Axx] * sux) + (Amat[Ayx] * suy) + (Amat[Azx] * suz); - ul[offsetY] = (Amat[Axy] * sux) + (Amat[Ayy] * suy) + (Amat[Azy] * suz); - ul[offsetZ] = (Amat[Axz] * sux) + (Amat[Ayz] * suy) + (Amat[Azz] * suz); + if( hasCoords ){ + ul[offsetX] = (Amat[Axx] * sux) + (Amat[Ayx] * suy) + (Amat[Azx] * suz); + ul[offsetY] = (Amat[Axy] * sux) + (Amat[Ayy] * suy) + (Amat[Azy] * suz); + ul[offsetZ] = (Amat[Axz] * sux) + (Amat[Ayz] * suy) + (Amat[Azz] * suz); + } + else{ + + sprintf( painCave.errMsg, + "Attempt to updateU for atom %d before coords set.\n", + index ); + painCave.isFatal = 1; + simError(); + } } void DirectionalAtom::getJ( double theJ[3] ){ @@ -193,16 +356,36 @@ void DirectionalAtom::getTrq( double theT[3] ){ void DirectionalAtom::getTrq( double theT[3] ){ - theT[0] = trq[offsetX]; - theT[1] = trq[offsetY]; - theT[2] = trq[offsetZ]; + if( hasCoords ){ + theT[0] = trq[offsetX]; + theT[1] = trq[offsetY]; + theT[2] = trq[offsetZ]; + } + else{ + + sprintf( painCave.errMsg, + "Attempt to get Trq for atom %d before coords set.\n", + index ); + painCave.isFatal = 1; + simError(); + } } void DirectionalAtom::addTrq( double theT[3] ){ - trq[offsetX] += theT[0]; - trq[offsetY] += theT[1]; - trq[offsetZ] += theT[2]; + if( hasCoords ){ + trq[offsetX] += theT[0]; + trq[offsetY] += theT[1]; + trq[offsetZ] += theT[2]; + } + else{ + + sprintf( painCave.errMsg, + "Attempt to add Trq for atom %d before coords set.\n", + index ); + painCave.isFatal = 1; + simError(); + } } @@ -220,3 +403,153 @@ void DirectionalAtom::getI( double the_I[3][3] ){ the_I[2][1] = Izy; the_I[2][2] = Izz; } + +void DirectionalAtom::getGrad( double grad[6] ) { + + double myEuler[3]; + double phi, theta, psi; + double cphi, sphi, ctheta, stheta; + double ephi[3]; + double etheta[3]; + double epsi[3]; + + this->getEulerAngles(myEuler); + + phi = myEuler[0]; + theta = myEuler[1]; + psi = myEuler[2]; + + cphi = cos(phi); + sphi = sin(phi); + ctheta = cos(theta); + stheta = sin(theta); + + // get unit vectors along the phi, theta and psi rotation axes + + ephi[0] = 0.0; + ephi[1] = 0.0; + ephi[2] = 1.0; + etheta[0] = -sphi; + etheta[1] = cphi; + etheta[2] = 0.0; + epsi[0] = ctheta * cphi; + epsi[1] = ctheta * sphi; + epsi[2] = -stheta; + + for (int j = 0 ; j<3; j++) + grad[j] = frc[j]; + + for (int j = 0; j < 3; j++ ) { + + grad[3] += trq[j]*ephi[j]; + grad[4] += trq[j]*etheta[j]; + grad[5] += trq[j]*epsi[j]; + + } + +} + + +void DirectionalAtom::getEulerAngles(double myEuler[3]) { + + // getEulerAngles computes a set of Euler angle values consistent + // with an input rotation matrix. They are returned in the following + // order: + // myEuler[0] = phi; + // myEuler[1] = theta; + // myEuler[2] = psi; + + double phi,theta,psi,eps; + double pi; + double cphi,ctheta,cpsi; + double sphi,stheta,spsi; + double b[3]; + int flip[3]; + + // set the tolerance for Euler angles and rotation elements + + eps = 1.0e-8; + + // get a trial value of theta from a single rotation element + + theta = asin(min(1.0,max(-1.0,-Amat[Axz]))); + ctheta = cos(theta); + stheta = -Amat[Axz]; + + // set the phi/psi difference when theta is either 90 or -90 + + if (fabs(ctheta) <= eps) { + phi = 0.0; + if (fabs(Amat[Azx]) < eps) { + psi = asin(min(1.0,max(-1.0,-Amat[Ayx]/Amat[Axz]))); + } else { + if (fabs(Amat[Ayx]) < eps) { + psi = acos(min(1.0,max(-1.0,-Amat[Azx]/Amat[Axz]))); + } else { + psi = atan(Amat[Ayx]/Amat[Azx]); + } + } + } + + // set the phi and psi values for all other theta values + + else { + if (fabs(Amat[Axx]) < eps) { + phi = asin(min(1.0,max(-1.0,Amat[Axy]/ctheta))); + } else { + if (fabs(Amat[Axy]) < eps) { + phi = acos(min(1.0,max(-1.0,Amat[Axx]/ctheta))); + } else { + phi = atan(Amat[Axy]/Amat[Axx]); + } + } + if (fabs(Amat[Azz]) < eps) { + psi = asin(min(1.0,max(-1.0,Amat[Ayz]/ctheta))); + } else { + if (fabs(Amat[Ayz]) < eps) { + psi = acos(min(1.0,max(-1.0,Amat[Azz]/ctheta))); + } + psi = atan(Amat[Ayz]/Amat[Azz]); + } + } + + // find sine and cosine of the trial phi and psi values + + cphi = cos(phi); + sphi = sin(phi); + cpsi = cos(psi); + spsi = sin(psi); + + // reconstruct the diagonal of the rotation matrix + + b[0] = ctheta * cphi; + b[1] = spsi*stheta*sphi + cpsi*cphi; + b[2] = ctheta * cpsi; + + // compare the correct matrix diagonal to rebuilt diagonal + + for (int i = 0; i < 3; i++) { + flip[i] = 0; + if (fabs(Amat[3*i + i] - b[i]) > eps) flip[i] = 1; + } + + // alter Euler angles to get correct rotation matrix values + + if (flip[0] && flip[1]) phi = phi - copysign(M_PI,phi); + if (flip[0] && flip[2]) theta = -theta + copysign(M_PI, theta); + if (flip[1] && flip[2]) psi = psi - copysign(M_PI, psi); + + myEuler[0] = phi; + myEuler[1] = theta; + myEuler[2] = psi; + + return; +} + +double DirectionalAtom::max(double x, double y) { + return (x > y) ? x : y; +} + +double DirectionalAtom::min(double x, double y) { + return (x > y) ? y : x; +}