--- trunk/OOPSE/libmdtools/DirectionalAtom.cpp 2003/08/12 19:56:49 689 +++ trunk/OOPSE/libmdtools/DirectionalAtom.cpp 2004/08/23 15:11:36 1452 @@ -1,13 +1,14 @@ -#include +#include #include "Atom.hpp" +#include "DirectionalAtom.hpp" #include "simError.h" +#include "MatVec3.h" void DirectionalAtom::zeroForces() { if( hasCoords ){ - frc[offsetX] = 0.0; - frc[offsetY] = 0.0; - frc[offsetZ] = 0.0; + + Atom::zeroForces(); trq[offsetX] = 0.0; trq[offsetY] = 0.0; @@ -34,44 +35,21 @@ void DirectionalAtom::setCoords(void){ &trq, &Amat, &mu, - &ul ); + &ul, + &quat); } else{ sprintf( painCave.errMsg, "Attempted to set Atom %d coordinates with an unallocated " - "SimState object.\n" ); + "SimState object.\n", index ); painCave.isFatal = 1; simError(); } hasCoords = true; - mu[index] = myMu; - } -double DirectionalAtom::getMu( void ) { - - if( hasCoords ){ - return mu[index]; - } - else{ - return myMu; - } - return 0; -} - -void DirectionalAtom::setMu( double the_mu ) { - - if( hasCoords ){ - mu[index] = the_mu; - myMu = the_mu; - } - else{ - myMu = the_mu; - } -} - void DirectionalAtom::setA( double the_A[3][3] ){ if( hasCoords ){ @@ -91,7 +69,7 @@ void DirectionalAtom::setA( double the_A[3][3] ){ } } -void DirectionalAtom::setI( double the_I[3][3] ){ +void DirectionalAtom::setI( double the_I[3][3] ){ Ixx = the_I[0][0]; Ixy = the_I[0][1]; Ixz = the_I[0][2]; Iyx = the_I[1][0]; Iyy = the_I[1][1]; Iyz = the_I[1][2]; @@ -181,10 +159,10 @@ void DirectionalAtom::getU( double the_u[3] ){ void DirectionalAtom::getU( double the_u[3] ){ - the_u[0] = sux; - the_u[1] = suy; - the_u[2] = suz; - + the_u[0] = sU[2][0]; + the_u[1] = sU[2][1]; + the_u[2] = sU[2][2]; + this->body2Lab( the_u ); } @@ -246,7 +224,57 @@ void DirectionalAtom::getQ( double q[4] ){ } } +void DirectionalAtom::setUnitFrameFromEuler(double phi, + double theta, + double psi) { + double myA[3][3]; + double uFrame[3][3]; + double len; + int i, j; + + myA[0][0] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi)); + myA[0][1] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi)); + myA[0][2] = sin(theta) * sin(psi); + + myA[1][0] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi)); + myA[1][1] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi)); + myA[1][2] = sin(theta) * cos(psi); + + myA[2][0] = sin(phi) * sin(theta); + myA[2][1] = -cos(phi) * sin(theta); + myA[2][2] = cos(theta); + + // Make the unit Frame: + + for (i=0; i < 3; i++) + for (j=0; j < 3; j++) + uFrame[i][j] = 0.0; + + for (i=0; i < 3; i++) + uFrame[i][i] = 1.0; + + // rotate by the given rotation matrix: + + matMul3(myA, uFrame, sU); + + // renormalize column vectors: + + for (i=0; i < 3; i++) { + len = 0.0; + for (j = 0; j < 3; j++) { + len += sU[i][j]*sU[i][j]; + } + len = sqrt(len); + for (j = 0; j < 3; j++) { + sU[i][j] /= len; + } + } + + // sU now contains the coordinates of the 'special' frame; + +} + void DirectionalAtom::setEuler( double phi, double theta, double psi ){ if( hasCoords ){ @@ -299,6 +327,43 @@ void DirectionalAtom::lab2Body( double r[3] ){ } +void DirectionalAtom::rotateBy( double by_A[3][3]) { + + // Check this + + double r00, r01, r02, r10, r11, r12, r20, r21, r22; + + if( hasCoords ){ + + r00 = by_A[0][0]*Amat[Axx] + by_A[0][1]*Amat[Ayx] + by_A[0][2]*Amat[Azx]; + r01 = by_A[0][0]*Amat[Axy] + by_A[0][1]*Amat[Ayy] + by_A[0][2]*Amat[Azy]; + r02 = by_A[0][0]*Amat[Axz] + by_A[0][1]*Amat[Ayz] + by_A[0][2]*Amat[Azz]; + + r10 = by_A[1][0]*Amat[Axx] + by_A[1][1]*Amat[Ayx] + by_A[1][2]*Amat[Azx]; + r11 = by_A[1][0]*Amat[Axy] + by_A[1][1]*Amat[Ayy] + by_A[1][2]*Amat[Azy]; + r12 = by_A[1][0]*Amat[Axz] + by_A[1][1]*Amat[Ayz] + by_A[1][2]*Amat[Azz]; + + r20 = by_A[2][0]*Amat[Axx] + by_A[2][1]*Amat[Ayx] + by_A[2][2]*Amat[Azx]; + r21 = by_A[2][0]*Amat[Axy] + by_A[2][1]*Amat[Ayy] + by_A[2][2]*Amat[Azy]; + r22 = by_A[2][0]*Amat[Axz] + by_A[2][1]*Amat[Ayz] + by_A[2][2]*Amat[Azz]; + + Amat[Axx] = r00; Amat[Axy] = r01; Amat[Axz] = r02; + Amat[Ayx] = r10; Amat[Ayy] = r11; Amat[Ayz] = r12; + Amat[Azx] = r20; Amat[Azy] = r21; Amat[Azz] = r22; + + } + else{ + + sprintf( painCave.errMsg, + "Attempt to rotate frame for atom %d before coords set.\n", + index ); + painCave.isFatal = 1; + simError(); + } + +} + + void DirectionalAtom::body2Lab( double r[3] ){ double rb[3]; // the body frame vector @@ -325,9 +390,12 @@ void DirectionalAtom::updateU( void ){ void DirectionalAtom::updateU( void ){ 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); + ul[offsetX] = (Amat[Axx] * sU[2][0]) + + (Amat[Ayx] * sU[2][1]) + (Amat[Azx] * sU[2][2]); + ul[offsetY] = (Amat[Axy] * sU[2][0]) + + (Amat[Ayy] * sU[2][1]) + (Amat[Azy] * sU[2][2]); + ul[offsetZ] = (Amat[Axz] * sU[2][0]) + + (Amat[Ayz] * sU[2][1]) + (Amat[Azz] * sU[2][2]); } else{ @@ -370,6 +438,23 @@ void DirectionalAtom::getTrq( double theT[3] ){ } } +void DirectionalAtom::setTrq( double theT[3] ){ + + 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(); + } +} + void DirectionalAtom::addTrq( double theT[3] ){ if( hasCoords ){ @@ -401,4 +486,169 @@ void DirectionalAtom::getI( double the_I[3][3] ){ the_I[2][0] = Izx; 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] = cphi; + etheta[1] = sphi; + etheta[2] = 0.0; + + epsi[0] = stheta * cphi; + epsi[1] = stheta * sphi; + epsi[2] = ctheta; + + for (int j = 0 ; j<3; j++) + grad[j] = frc[j]; + + grad[3] = 0; + grad[4] = 0; + grad[5] = 0; + + 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]; + + } + } + +/** + * 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; +*/ +void DirectionalAtom::getEulerAngles(double myEuler[3]) { + + // We use so-called "x-convention", which is the most common definition. + // In this convention, the rotation given by Euler angles (phi, theta, psi), where the first + // rotation is by an angle phi about the z-axis, the second is by an angle + // theta (0 <= theta <= 180)about the x-axis, and thethird is by an angle psi about the + //z-axis (again). + + + double phi,theta,psi,eps; + double ctheta,stheta; + + // set the tolerance for Euler angles and rotation elements + + eps = 1.0e-8; + + theta = acos(min(1.0,max(-1.0,Amat[Azz]))); + ctheta = Amat[Azz]; + stheta = sqrt(1.0 - ctheta * ctheta); + + // when sin(theta) is close to 0, we need to consider singularity + // In this case, we can assign an arbitary value to phi (or psi), and then determine + // the psi (or phi) or vice-versa. We'll assume that phi always gets the rotation, and psi is 0 + // in cases of singularity. + // we use atan2 instead of atan, since atan2 will give us -Pi to Pi. + // Since 0 <= theta <= 180, sin(theta) will be always non-negative. Therefore, it never + // change the sign of both of the parameters passed to atan2. + + if (fabs(stheta) <= eps){ + psi = 0.0; + phi = atan2(-Amat[Ayx], Amat[Axx]); + } + // we only have one unique solution + else{ + phi = atan2(Amat[Azx], -Amat[Azy]); + psi = atan2(Amat[Axz], Amat[Ayz]); + } + + //wrap phi and psi, make sure they are in the range from 0 to 2*Pi + //if (phi < 0) + // phi += M_PI; + + //if (psi < 0) + // psi += M_PI; + + myEuler[0] = phi; + myEuler[1] = theta; + myEuler[2] = psi; + + return; +} + +double DirectionalAtom::getZangle( ){ + + if( hasCoords ){ + return zAngle; + } + else{ + + sprintf( painCave.errMsg, + "Attempt to get zAngle for atom %d before coords set.\n", + index ); + painCave.isFatal = 1; + simError(); + return 0; + } +} + +void DirectionalAtom::setZangle( double zAng ){ + + if( hasCoords ){ + zAngle = zAng; + } + else{ + + sprintf( painCave.errMsg, + "Attempt to set zAngle for atom %d before coords set.\n", + index ); + painCave.isFatal = 1; + simError(); + } +} + +void DirectionalAtom::addZangle( double zAng ){ + + if( hasCoords ){ + zAngle += zAng; + } + else{ + + sprintf( painCave.errMsg, + "Attempt to add zAngle to atom %d before coords set.\n", + index ); + painCave.isFatal = 1; + simError(); + } +} + +double DirectionalAtom::max(double x, double y) { + return (x > y) ? x : y; +} + +double DirectionalAtom::min(double x, double y) { + return (x > y) ? y : x; +}