OOPSE/libmdtools/DirectionalAtom.cpp

#include <math.h>

#include "Atom.hpp"
#include "DirectionalAtom.hpp"
#include "simError.h"
#include "MatVec3.h"

void DirectionalAtom::zeroForces() {
  if( hasCoords ){

    Atom::zeroForces();
    
    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,
                     &quat);
  }
  else{
    sprintf( painCave.errMsg,
             "Attempted to set Atom %d  coordinates with an unallocated "
             "SimState object.\n", index );
    painCave.isFatal = 1;
    simError();
  }

  hasCoords = true;

}

void DirectionalAtom::setA( double the_A[3][3] ){

  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] ){  
  
  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];
  Izx = the_I[2][0]; Izy = the_I[2][1]; Izz = the_I[2][2];
}

void DirectionalAtom::setQ( double the_q[4] ){

  double q0Sqr, q1Sqr, q2Sqr, q3Sqr;

  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();
  }

}

void DirectionalAtom::getA( double the_A[3][3] ){
  
  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();
  }

}

void DirectionalAtom::printAmatIndex( void ){

  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();
  }
}


void DirectionalAtom::getU( double the_u[3] ){
  
  the_u[0] = sU[2][0];
  the_u[1] = sU[2][1];
  the_u[2] = sU[2][2];
  
  this->body2Lab( the_u );
}

void DirectionalAtom::getQ( double q[4] ){
  
  double t, s;
  double ad1, ad2, ad3;

  if( hasCoords ){
    
    t = Amat[Axx] + Amat[Ayy] + Amat[Azz] + 1.0;
    if( t > 0.0 ){
      
      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 ){
        
        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::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 ){
    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();
  }
}


void DirectionalAtom::lab2Body( double r[3] ){

  double rl[3]; // the lab frame vector 
  
  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();
  }

}

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 
  
  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 ){

  if( hasCoords ){
    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{
    
    sprintf( painCave.errMsg,
             "Attempt to updateU for atom %d before coords set.\n",
             index );
    painCave.isFatal = 1;
    simError();
  }
}

void DirectionalAtom::getJ( double theJ[3] ){
  
  theJ[0] = jx;
  theJ[1] = jy;
  theJ[2] = jz;
}

void DirectionalAtom::setJ( double theJ[3] ){
  
  jx = theJ[0];
  jy = theJ[1];
  jz = theJ[2];
}

void DirectionalAtom::getTrq( double theT[3] ){
  
  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::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 ){
    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::getI( double the_I[3][3] ){
  
  the_I[0][0] = Ixx;
  the_I[0][1] = Ixy;
  the_I[0][2] = Ixz;

  the_I[1][0] = Iyx;
  the_I[1][1] = Iyy;
  the_I[1][2] = Iyz;

  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;
}
Revision:	1452
Committed:	Mon Aug 23 15:11:36 2004 UTC (20 years ago) by tim
File size:	15238 byte(s)
Log Message:	* empty log message *
#	User	Rev	Content
1	gezelter	829	#include <math.h>
2	mmeineke	377
3			#include "Atom.hpp"
4	gezelter	1097	#include "DirectionalAtom.hpp"
5	mmeineke	670	#include "simError.h"
6	gezelter	1097	#include "MatVec3.h"
7	mmeineke	377
8	mmeineke	670	void DirectionalAtom::zeroForces() {
9			if( hasCoords ){
10	gezelter	1097
11			Atom::zeroForces();
12	mmeineke	670
13			trq[offsetX] = 0.0;
14			trq[offsetY] = 0.0;
15			trq[offsetZ] = 0.0;
16			}
17			else{
18
19			sprintf( painCave.errMsg,
20			"Attempt to zero frc and trq for atom %d before coords set.\n",
21			index );
22			painCave.isFatal = 1;
23			simError();
24			}
25			}
26	mmeineke	377
27	tim	689	void DirectionalAtom::setCoords(void){
28	mmeineke	414
29	tim	689	if( myConfig->isAllocated() ){
30
31			myConfig->getAtomPointers( index,
32			&pos,
33			&vel,
34			&frc,
35			&trq,
36			&Amat,
37			&mu,
38	tim	1452	&ul,
39			&quat);
40	mmeineke	670	}
41			else{
42			sprintf( painCave.errMsg,
43	tim	689	"Attempted to set Atom %d coordinates with an unallocated "
44	mmeineke	787	"SimState object.\n", index );
45	mmeineke	670	painCave.isFatal = 1;
46			simError();
47			}
48	tim	689
49			hasCoords = true;
50
51			}
52
53	mmeineke	377	void DirectionalAtom::setA( double the_A[3][3] ){
54
55	mmeineke	670	if( hasCoords ){
56			Amat[Axx] = the_A[0][0]; Amat[Axy] = the_A[0][1]; Amat[Axz] = the_A[0][2];
57			Amat[Ayx] = the_A[1][0]; Amat[Ayy] = the_A[1][1]; Amat[Ayz] = the_A[1][2];
58			Amat[Azx] = the_A[2][0]; Amat[Azy] = the_A[2][1]; Amat[Azz] = the_A[2][2];
59
60			this->updateU();
61			}
62			else{
63
64			sprintf( painCave.errMsg,
65			"Attempt to set Amat for atom %d before coords set.\n",
66			index );
67			painCave.isFatal = 1;
68			simError();
69			}
70	mmeineke	377	}
71
72	gezelter	1097	void DirectionalAtom::setI( double the_I[3][3] ){
73	mmeineke	377
74			Ixx = the_I[0][0]; Ixy = the_I[0][1]; Ixz = the_I[0][2];
75			Iyx = the_I[1][0]; Iyy = the_I[1][1]; Iyz = the_I[1][2];
76			Izx = the_I[2][0]; Izy = the_I[2][1]; Izz = the_I[2][2];
77			}
78
79			void DirectionalAtom::setQ( double the_q[4] ){
80
81			double q0Sqr, q1Sqr, q2Sqr, q3Sqr;
82
83	mmeineke	670	if( hasCoords ){
84			q0Sqr = the_q[0] * the_q[0];
85			q1Sqr = the_q[1] * the_q[1];
86			q2Sqr = the_q[2] * the_q[2];
87			q3Sqr = the_q[3] * the_q[3];
88
89
90			Amat[Axx] = q0Sqr + q1Sqr - q2Sqr - q3Sqr;
91			Amat[Axy] = 2.0 * ( the_q[1] * the_q[2] + the_q[0] * the_q[3] );
92			Amat[Axz] = 2.0 * ( the_q[1] * the_q[3] - the_q[0] * the_q[2] );
93
94			Amat[Ayx] = 2.0 * ( the_q[1] * the_q[2] - the_q[0] * the_q[3] );
95			Amat[Ayy] = q0Sqr - q1Sqr + q2Sqr - q3Sqr;
96			Amat[Ayz] = 2.0 * ( the_q[2] * the_q[3] + the_q[0] * the_q[1] );
97
98			Amat[Azx] = 2.0 * ( the_q[1] * the_q[3] + the_q[0] * the_q[2] );
99			Amat[Azy] = 2.0 * ( the_q[2] * the_q[3] - the_q[0] * the_q[1] );
100			Amat[Azz] = q0Sqr - q1Sqr -q2Sqr +q3Sqr;
101
102			this->updateU();
103			}
104			else{
105
106			sprintf( painCave.errMsg,
107			"Attempt to set Q for atom %d before coords set.\n",
108			index );
109			painCave.isFatal = 1;
110			simError();
111			}
112	mmeineke	377
113			}
114
115			void DirectionalAtom::getA( double the_A[3][3] ){
116
117	mmeineke	670	if( hasCoords ){
118			the_A[0][0] = Amat[Axx];
119			the_A[0][1] = Amat[Axy];
120			the_A[0][2] = Amat[Axz];
121
122			the_A[1][0] = Amat[Ayx];
123			the_A[1][1] = Amat[Ayy];
124			the_A[1][2] = Amat[Ayz];
125
126			the_A[2][0] = Amat[Azx];
127			the_A[2][1] = Amat[Azy];
128			the_A[2][2] = Amat[Azz];
129			}
130			else{
131
132			sprintf( painCave.errMsg,
133			"Attempt to get Amat for atom %d before coords set.\n",
134			index );
135			painCave.isFatal = 1;
136			simError();
137			}
138	mmeineke	377
139			}
140
141	mmeineke	597	void DirectionalAtom::printAmatIndex( void ){
142	mmeineke	377
143	mmeineke	670	if( hasCoords ){
144			std::cerr << "Atom[" << index << "] index =>\n"
145			<< "[ " << Axx << ", " << Axy << ", " << Axz << " ]\n"
146			<< "[ " << Ayx << ", " << Ayy << ", " << Ayz << " ]\n"
147			<< "[ " << Azx << ", " << Azy << ", " << Azz << " ]\n";
148			}
149			else{
150
151			sprintf( painCave.errMsg,
152			"Attempt to print Amat indices for atom %d before coords set.\n",
153			index );
154			painCave.isFatal = 1;
155			simError();
156			}
157	mmeineke	597	}
158
159
160	mmeineke	377	void DirectionalAtom::getU( double the_u[3] ){
161
162	gezelter	1097	the_u[0] = sU[2][0];
163			the_u[1] = sU[2][1];
164			the_u[2] = sU[2][2];
165
166	mmeineke	377	this->body2Lab( the_u );
167			}
168
169			void DirectionalAtom::getQ( double q[4] ){
170
171			double t, s;
172			double ad1, ad2, ad3;
173
174	mmeineke	670	if( hasCoords ){
175	mmeineke	377
176	mmeineke	670	t = Amat[Axx] + Amat[Ayy] + Amat[Azz] + 1.0;
177			if( t > 0.0 ){
178	mmeineke	377
179	mmeineke	670	s = 0.5 / sqrt( t );
180			q[0] = 0.25 / s;
181			q[1] = (Amat[Ayz] - Amat[Azy]) * s;
182			q[2] = (Amat[Azx] - Amat[Axz]) * s;
183			q[3] = (Amat[Axy] - Amat[Ayx]) * s;
184	mmeineke	377	}
185			else{
186
187	mmeineke	670	ad1 = fabs( Amat[Axx] );
188			ad2 = fabs( Amat[Ayy] );
189			ad3 = fabs( Amat[Azz] );
190
191			if( ad1 >= ad2 && ad1 >= ad3 ){
192
193			s = 2.0 * sqrt( 1.0 + Amat[Axx] - Amat[Ayy] - Amat[Azz] );
194			q[0] = (Amat[Ayz] + Amat[Azy]) / s;
195			q[1] = 0.5 / s;
196			q[2] = (Amat[Axy] + Amat[Ayx]) / s;
197			q[3] = (Amat[Axz] + Amat[Azx]) / s;
198			}
199			else if( ad2 >= ad1 && ad2 >= ad3 ){
200
201			s = sqrt( 1.0 + Amat[Ayy] - Amat[Axx] - Amat[Azz] ) * 2.0;
202			q[0] = (Amat[Axz] + Amat[Azx]) / s;
203			q[1] = (Amat[Axy] + Amat[Ayx]) / s;
204			q[2] = 0.5 / s;
205			q[3] = (Amat[Ayz] + Amat[Azy]) / s;
206			}
207			else{
208
209			s = sqrt( 1.0 + Amat[Azz] - Amat[Axx] - Amat[Ayy] ) * 2.0;
210			q[0] = (Amat[Axy] + Amat[Ayx]) / s;
211			q[1] = (Amat[Axz] + Amat[Azx]) / s;
212			q[2] = (Amat[Ayz] + Amat[Azy]) / s;
213			q[3] = 0.5 / s;
214			}
215	mmeineke	377	}
216			}
217	mmeineke	670	else{
218
219			sprintf( painCave.errMsg,
220			"Attempt to get Q for atom %d before coords set.\n",
221			index );
222			painCave.isFatal = 1;
223			simError();
224			}
225	mmeineke	377	}
226
227	gezelter	1097	void DirectionalAtom::setUnitFrameFromEuler(double phi,
228			double theta,
229			double psi) {
230	mmeineke	377
231	gezelter	1097	double myA[3][3];
232			double uFrame[3][3];
233			double len;
234			int i, j;
235
236			myA[0][0] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi));
237			myA[0][1] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi));
238			myA[0][2] = sin(theta) * sin(psi);
239
240			myA[1][0] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi));
241			myA[1][1] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi));
242			myA[1][2] = sin(theta) * cos(psi);
243
244			myA[2][0] = sin(phi) * sin(theta);
245			myA[2][1] = -cos(phi) * sin(theta);
246			myA[2][2] = cos(theta);
247
248			// Make the unit Frame:
249
250			for (i=0; i < 3; i++)
251			for (j=0; j < 3; j++)
252			uFrame[i][j] = 0.0;
253
254			for (i=0; i < 3; i++)
255			uFrame[i][i] = 1.0;
256
257			// rotate by the given rotation matrix:
258
259			matMul3(myA, uFrame, sU);
260
261			// renormalize column vectors:
262
263			for (i=0; i < 3; i++) {
264			len = 0.0;
265			for (j = 0; j < 3; j++) {
266			len += sU[i][j]*sU[i][j];
267			}
268			len = sqrt(len);
269			for (j = 0; j < 3; j++) {
270			sU[i][j] /= len;
271			}
272			}
273
274			// sU now contains the coordinates of the 'special' frame;
275
276			}
277
278	mmeineke	377	void DirectionalAtom::setEuler( double phi, double theta, double psi ){
279
280	mmeineke	670	if( hasCoords ){
281			Amat[Axx] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi));
282			Amat[Axy] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi));
283			Amat[Axz] = sin(theta) * sin(psi);
284
285			Amat[Ayx] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi));
286			Amat[Ayy] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi));
287			Amat[Ayz] = sin(theta) * cos(psi);
288
289			Amat[Azx] = sin(phi) * sin(theta);
290			Amat[Azy] = -cos(phi) * sin(theta);
291			Amat[Azz] = cos(theta);
292
293			this->updateU();
294			}
295			else{
296
297			sprintf( painCave.errMsg,
298			"Attempt to set Euler angles for atom %d before coords set.\n",
299			index );
300			painCave.isFatal = 1;
301			simError();
302			}
303	mmeineke	377	}
304
305
306			void DirectionalAtom::lab2Body( double r[3] ){
307
308			double rl[3]; // the lab frame vector
309
310	mmeineke	670	if( hasCoords ){
311			rl[0] = r[0];
312			rl[1] = r[1];
313			rl[2] = r[2];
314
315			r[0] = (Amat[Axx] * rl[0]) + (Amat[Axy] * rl[1]) + (Amat[Axz] * rl[2]);
316			r[1] = (Amat[Ayx] * rl[0]) + (Amat[Ayy] * rl[1]) + (Amat[Ayz] * rl[2]);
317			r[2] = (Amat[Azx] * rl[0]) + (Amat[Azy] * rl[1]) + (Amat[Azz] * rl[2]);
318			}
319			else{
320
321			sprintf( painCave.errMsg,
322			"Attempt to convert lab2body for atom %d before coords set.\n",
323			index );
324			painCave.isFatal = 1;
325			simError();
326			}
327	mmeineke	377
328			}
329
330	gezelter	1097	void DirectionalAtom::rotateBy( double by_A[3][3]) {
331
332			// Check this
333
334			double r00, r01, r02, r10, r11, r12, r20, r21, r22;
335
336			if( hasCoords ){
337
338			r00 = by_A[0][0]Amat[Axx] + by_A[0][1]Amat[Ayx] + by_A[0][2]*Amat[Azx];
339			r01 = by_A[0][0]Amat[Axy] + by_A[0][1]Amat[Ayy] + by_A[0][2]*Amat[Azy];
340			r02 = by_A[0][0]Amat[Axz] + by_A[0][1]Amat[Ayz] + by_A[0][2]*Amat[Azz];
341
342			r10 = by_A[1][0]Amat[Axx] + by_A[1][1]Amat[Ayx] + by_A[1][2]*Amat[Azx];
343			r11 = by_A[1][0]Amat[Axy] + by_A[1][1]Amat[Ayy] + by_A[1][2]*Amat[Azy];
344			r12 = by_A[1][0]Amat[Axz] + by_A[1][1]Amat[Ayz] + by_A[1][2]*Amat[Azz];
345
346			r20 = by_A[2][0]Amat[Axx] + by_A[2][1]Amat[Ayx] + by_A[2][2]*Amat[Azx];
347			r21 = by_A[2][0]Amat[Axy] + by_A[2][1]Amat[Ayy] + by_A[2][2]*Amat[Azy];
348			r22 = by_A[2][0]Amat[Axz] + by_A[2][1]Amat[Ayz] + by_A[2][2]*Amat[Azz];
349
350			Amat[Axx] = r00; Amat[Axy] = r01; Amat[Axz] = r02;
351			Amat[Ayx] = r10; Amat[Ayy] = r11; Amat[Ayz] = r12;
352			Amat[Azx] = r20; Amat[Azy] = r21; Amat[Azz] = r22;
353
354			}
355			else{
356
357			sprintf( painCave.errMsg,
358			"Attempt to rotate frame for atom %d before coords set.\n",
359			index );
360			painCave.isFatal = 1;
361			simError();
362			}
363
364			}
365
366
367	mmeineke	377	void DirectionalAtom::body2Lab( double r[3] ){
368
369			double rb[3]; // the body frame vector
370
371	mmeineke	670	if( hasCoords ){
372			rb[0] = r[0];
373			rb[1] = r[1];
374			rb[2] = r[2];
375
376			r[0] = (Amat[Axx] * rb[0]) + (Amat[Ayx] * rb[1]) + (Amat[Azx] * rb[2]);
377			r[1] = (Amat[Axy] * rb[0]) + (Amat[Ayy] * rb[1]) + (Amat[Azy] * rb[2]);
378			r[2] = (Amat[Axz] * rb[0]) + (Amat[Ayz] * rb[1]) + (Amat[Azz] * rb[2]);
379			}
380			else{
381
382			sprintf( painCave.errMsg,
383			"Attempt to convert body2lab for atom %d before coords set.\n",
384			index );
385			painCave.isFatal = 1;
386			simError();
387			}
388	mmeineke	377	}
389
390			void DirectionalAtom::updateU( void ){
391
392	mmeineke	670	if( hasCoords ){
393	gezelter	1097	ul[offsetX] = (Amat[Axx] * sU[2][0]) +
394			(Amat[Ayx] * sU[2][1]) + (Amat[Azx] * sU[2][2]);
395			ul[offsetY] = (Amat[Axy] * sU[2][0]) +
396			(Amat[Ayy] * sU[2][1]) + (Amat[Azy] * sU[2][2]);
397			ul[offsetZ] = (Amat[Axz] * sU[2][0]) +
398			(Amat[Ayz] * sU[2][1]) + (Amat[Azz] * sU[2][2]);
399	mmeineke	670	}
400			else{
401
402			sprintf( painCave.errMsg,
403			"Attempt to updateU for atom %d before coords set.\n",
404			index );
405			painCave.isFatal = 1;
406			simError();
407			}
408	mmeineke	377	}
409
410	mmeineke	599	void DirectionalAtom::getJ( double theJ[3] ){
411
412			theJ[0] = jx;
413			theJ[1] = jy;
414			theJ[2] = jz;
415			}
416
417			void DirectionalAtom::setJ( double theJ[3] ){
418
419			jx = theJ[0];
420			jy = theJ[1];
421			jz = theJ[2];
422			}
423
424			void DirectionalAtom::getTrq( double theT[3] ){
425
426	mmeineke	670	if( hasCoords ){
427			theT[0] = trq[offsetX];
428			theT[1] = trq[offsetY];
429			theT[2] = trq[offsetZ];
430			}
431			else{
432
433			sprintf( painCave.errMsg,
434			"Attempt to get Trq for atom %d before coords set.\n",
435			index );
436			painCave.isFatal = 1;
437			simError();
438			}
439	mmeineke	599	}
440
441	tim	1452	void DirectionalAtom::setTrq( double theT[3] ){
442
443			if( hasCoords ){
444			trq[offsetX] = theT[0];
445			trq[offsetY] = theT[1];
446			trq[offsetZ] = theT[2];
447			}
448			else{
449
450			sprintf( painCave.errMsg,
451			"Attempt to add Trq for atom %d before coords set.\n",
452			index );
453			painCave.isFatal = 1;
454			simError();
455			}
456			}
457
458	mmeineke	599	void DirectionalAtom::addTrq( double theT[3] ){
459
460	mmeineke	670	if( hasCoords ){
461			trq[offsetX] += theT[0];
462			trq[offsetY] += theT[1];
463			trq[offsetZ] += theT[2];
464			}
465			else{
466
467			sprintf( painCave.errMsg,
468			"Attempt to add Trq for atom %d before coords set.\n",
469			index );
470			painCave.isFatal = 1;
471			simError();
472			}
473	mmeineke	599	}
474
475
476			void DirectionalAtom::getI( double the_I[3][3] ){
477
478			the_I[0][0] = Ixx;
479			the_I[0][1] = Ixy;
480			the_I[0][2] = Ixz;
481
482			the_I[1][0] = Iyx;
483			the_I[1][1] = Iyy;
484			the_I[1][2] = Iyz;
485
486			the_I[2][0] = Izx;
487			the_I[2][1] = Izy;
488			the_I[2][2] = Izz;
489			}
490	gezelter	878
491			void DirectionalAtom::getGrad( double grad[6] ) {
492
493			double myEuler[3];
494			double phi, theta, psi;
495			double cphi, sphi, ctheta, stheta;
496			double ephi[3];
497			double etheta[3];
498			double epsi[3];
499
500			this->getEulerAngles(myEuler);
501
502			phi = myEuler[0];
503			theta = myEuler[1];
504			psi = myEuler[2];
505
506			cphi = cos(phi);
507			sphi = sin(phi);
508			ctheta = cos(theta);
509			stheta = sin(theta);
510
511			// get unit vectors along the phi, theta and psi rotation axes
512
513			ephi[0] = 0.0;
514			ephi[1] = 0.0;
515			ephi[2] = 1.0;
516	tim	1046
517			etheta[0] = cphi;
518			etheta[1] = sphi;
519	gezelter	878	etheta[2] = 0.0;
520
521	tim	1046	epsi[0] = stheta * cphi;
522			epsi[1] = stheta * sphi;
523			epsi[2] = ctheta;
524
525	gezelter	878	for (int j = 0 ; j<3; j++)
526			grad[j] = frc[j];
527
528	tim	1046	grad[3] = 0;
529			grad[4] = 0;
530			grad[5] = 0;
531
532	gezelter	878	for (int j = 0; j < 3; j++ ) {
533
534			grad[3] += trq[j]*ephi[j];
535			grad[4] += trq[j]*etheta[j];
536			grad[5] += trq[j]*epsi[j];
537
538			}
539
540			}
541
542	tim	1046	/**
543			* getEulerAngles computes a set of Euler angle values consistent
544			* with an input rotation matrix. They are returned in the following
545			* order:
546			* myEuler[0] = phi;
547			* myEuler[1] = theta;
548			* myEuler[2] = psi;
549			*/
550	gezelter	878	void DirectionalAtom::getEulerAngles(double myEuler[3]) {
551
552	tim	1046	// We use so-called "x-convention", which is the most common definition.
553			// In this convention, the rotation given by Euler angles (phi, theta, psi), where the first
554			// rotation is by an angle phi about the z-axis, the second is by an angle
555			// theta (0 <= theta <= 180)about the x-axis, and thethird is by an angle psi about the
556			//z-axis (again).
557	gezelter	878
558	tim	1046
559	gezelter	878	double phi,theta,psi,eps;
560	gezelter	1252	double ctheta,stheta;
561	gezelter	878
562			// set the tolerance for Euler angles and rotation elements
563
564			eps = 1.0e-8;
565
566	tim	1046	theta = acos(min(1.0,max(-1.0,Amat[Azz])));
567			ctheta = Amat[Azz];
568			stheta = sqrt(1.0 - ctheta * ctheta);
569
570			// when sin(theta) is close to 0, we need to consider singularity
571			// In this case, we can assign an arbitary value to phi (or psi), and then determine
572			// the psi (or phi) or vice-versa. We'll assume that phi always gets the rotation, and psi is 0
573			// in cases of singularity.
574			// we use atan2 instead of atan, since atan2 will give us -Pi to Pi.
575			// Since 0 <= theta <= 180, sin(theta) will be always non-negative. Therefore, it never
576			// change the sign of both of the parameters passed to atan2.
577	gezelter	878
578	tim	1046	if (fabs(stheta) <= eps){
579			psi = 0.0;
580			phi = atan2(-Amat[Ayx], Amat[Axx]);
581	gezelter	878	}
582	tim	1046	// we only have one unique solution
583			else{
584			phi = atan2(Amat[Azx], -Amat[Azy]);
585			psi = atan2(Amat[Axz], Amat[Ayz]);
586			}
587	gezelter	878
588	tim	1046	//wrap phi and psi, make sure they are in the range from 0 to 2*Pi
589			//if (phi < 0)
590			// phi += M_PI;
591	gezelter	878
592	tim	1046	//if (psi < 0)
593			// psi += M_PI;
594	gezelter	878
595			myEuler[0] = phi;
596			myEuler[1] = theta;
597			myEuler[2] = psi;
598	tim	1046
599	gezelter	878	return;
600			}
601
602	chrisfen	1187	double DirectionalAtom::getZangle( ){
603
604			if( hasCoords ){
605			return zAngle;
606			}
607			else{
608
609			sprintf( painCave.errMsg,
610			"Attempt to get zAngle for atom %d before coords set.\n",
611			index );
612			painCave.isFatal = 1;
613			simError();
614			return 0;
615			}
616			}
617
618			void DirectionalAtom::setZangle( double zAng ){
619
620			if( hasCoords ){
621			zAngle = zAng;
622			}
623			else{
624
625			sprintf( painCave.errMsg,
626			"Attempt to set zAngle for atom %d before coords set.\n",
627			index );
628			painCave.isFatal = 1;
629			simError();
630			}
631			}
632
633			void DirectionalAtom::addZangle( double zAng ){
634
635			if( hasCoords ){
636			zAngle += zAng;
637			}
638			else{
639
640			sprintf( painCave.errMsg,
641			"Attempt to add zAngle to atom %d before coords set.\n",
642			index );
643			painCave.isFatal = 1;
644			simError();
645			}
646			}
647
648	gezelter	878	double DirectionalAtom::max(double x, double y) {
649			return (x > y) ? x : y;
650			}
651
652			double DirectionalAtom::min(double x, double y) {
653			return (x > y) ? y : x;
654			}