src/primitives/DirectionalAtom.cpp

#include <math.h>

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