--- trunk/OOPSE/libmdtools/DirectionalAtom.cpp	2003/10/28 16:03:37	829
+++ trunk/OOPSE/libmdtools/DirectionalAtom.cpp	2003/12/12 15:42:13	878
@@ -2,6 +2,8 @@
 
 #include "Atom.hpp"
 #include "simError.h"
+
+
 
 void DirectionalAtom::zeroForces() {
   if( hasCoords ){
@@ -400,4 +402,154 @@ 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] = -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;
+}