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

  int n_linear_coords, i, j;
  
  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];
  
  n_linear_coords = 0;

  for (i = 0; i<3; i++) {
    if (fabs(the_I[i][i]) < momIntTol) {
      is_linear = true;
      n_linear_coords++;
      linear_axis = i;
    }
  }
  
  if (n_linear_coords > 1) {
    sprintf( painCave.errMsg,
             "DirectionalAtom error.\n"
             "\tOOPSE was told to set more than one axis in this\n"
             "\tDirectionalAtom to a vanishing moment of inertia.\n"
             "\tThis should not be a DirectionalAtom.  Use an Atom.\n"
             );
      painCave.isFatal = 1;
      simError();
  }


}

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