OOPSE/libmdtools/DirectionalAtom.cpp

#include <math.h>

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


void DirectionalAtom::zeroForces() {
  if( hasCoords ){
    frc[offsetX] = 0.0; 
    frc[offsetY] = 0.0; 
    frc[offsetZ] = 0.0;
    
    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;

  *mu = myMu;

}

double DirectionalAtom::getMu( void ) {

  if( hasCoords ){
    return *mu;
  }
  else{
    return myMu;
  }
}

void DirectionalAtom::setMu( double the_mu ) { 

  if( hasCoords ){
    *mu = the_mu;
    myMu = the_mu;
  }
  else{
    myMu = the_mu;
  }
}

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] = sux;
  the_u[1] = suy;
  the_u[2] = suz;

  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::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::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] * sux) + (Amat[Ayx] * suy) + (Amat[Azx] * suz);
    ul[offsetY] = (Amat[Axy] * sux) + (Amat[Ayy] * suy) + (Amat[Azy] * suz);
    ul[offsetZ] = (Amat[Axz] * sux) + (Amat[Ayz] * suy) + (Amat[Azz] * suz);
  }
  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 pi;
  double cphi,ctheta,cpsi;
  double sphi,stheta,spsi;
  double b[3];
  int flip[3];

  // set the tolerance for Euler angles and rotation elements
  
  eps = 1.0e-8;

  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::max(double x, double  y) {  
  return (x > y) ? x : y;
}

double DirectionalAtom::min(double x, double  y) {  
  return (x > y) ? y : x;
}
Revision:	1046
Committed:	Tue Feb 10 21:33:44 2004 UTC (21 years, 8 months ago) by tim
File size:	11891 byte(s)
Log Message:	single version of energy minimization is working.
#	User	Rev	Content
1	gezelter	829	#include <math.h>
2	mmeineke	377
3			#include "Atom.hpp"
4	mmeineke	670	#include "simError.h"
5	mmeineke	377
6	gezelter	878
7
8	mmeineke	670	void DirectionalAtom::zeroForces() {
9			if( hasCoords ){
10			frc[offsetX] = 0.0;
11			frc[offsetY] = 0.0;
12			frc[offsetZ] = 0.0;
13
14			trq[offsetX] = 0.0;
15			trq[offsetY] = 0.0;
16			trq[offsetZ] = 0.0;
17			}
18			else{
19
20			sprintf( painCave.errMsg,
21			"Attempt to zero frc and trq for atom %d before coords set.\n",
22			index );
23			painCave.isFatal = 1;
24			simError();
25			}
26			}
27	mmeineke	377
28	tim	689	void DirectionalAtom::setCoords(void){
29	mmeineke	414
30	tim	689	if( myConfig->isAllocated() ){
31
32			myConfig->getAtomPointers( index,
33			&pos,
34			&vel,
35			&frc,
36			&trq,
37			&Amat,
38			&mu,
39			&ul );
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	mmeineke	690	*mu = myMu;
52	tim	689
53			}
54
55			double DirectionalAtom::getMu( void ) {
56
57			if( hasCoords ){
58	mmeineke	690	return *mu;
59	tim	689	}
60			else{
61			return myMu;
62			}
63	mmeineke	670	}
64
65			void DirectionalAtom::setMu( double the_mu ) {
66
67			if( hasCoords ){
68	mmeineke	690	*mu = the_mu;
69	tim	689	myMu = the_mu;
70	mmeineke	670	}
71			else{
72	tim	689	myMu = the_mu;
73	mmeineke	670	}
74			}
75
76	mmeineke	377	void DirectionalAtom::setA( double the_A[3][3] ){
77
78	mmeineke	670	if( hasCoords ){
79			Amat[Axx] = the_A[0][0]; Amat[Axy] = the_A[0][1]; Amat[Axz] = the_A[0][2];
80			Amat[Ayx] = the_A[1][0]; Amat[Ayy] = the_A[1][1]; Amat[Ayz] = the_A[1][2];
81			Amat[Azx] = the_A[2][0]; Amat[Azy] = the_A[2][1]; Amat[Azz] = the_A[2][2];
82
83			this->updateU();
84			}
85			else{
86
87			sprintf( painCave.errMsg,
88			"Attempt to set Amat for atom %d before coords set.\n",
89			index );
90			painCave.isFatal = 1;
91			simError();
92			}
93	mmeineke	377	}
94
95			void DirectionalAtom::setI( double the_I[3][3] ){
96
97			Ixx = the_I[0][0]; Ixy = the_I[0][1]; Ixz = the_I[0][2];
98			Iyx = the_I[1][0]; Iyy = the_I[1][1]; Iyz = the_I[1][2];
99			Izx = the_I[2][0]; Izy = the_I[2][1]; Izz = the_I[2][2];
100			}
101
102			void DirectionalAtom::setQ( double the_q[4] ){
103
104			double q0Sqr, q1Sqr, q2Sqr, q3Sqr;
105
106	mmeineke	670	if( hasCoords ){
107			q0Sqr = the_q[0] * the_q[0];
108			q1Sqr = the_q[1] * the_q[1];
109			q2Sqr = the_q[2] * the_q[2];
110			q3Sqr = the_q[3] * the_q[3];
111
112
113			Amat[Axx] = q0Sqr + q1Sqr - q2Sqr - q3Sqr;
114			Amat[Axy] = 2.0 * ( the_q[1] * the_q[2] + the_q[0] * the_q[3] );
115			Amat[Axz] = 2.0 * ( the_q[1] * the_q[3] - the_q[0] * the_q[2] );
116
117			Amat[Ayx] = 2.0 * ( the_q[1] * the_q[2] - the_q[0] * the_q[3] );
118			Amat[Ayy] = q0Sqr - q1Sqr + q2Sqr - q3Sqr;
119			Amat[Ayz] = 2.0 * ( the_q[2] * the_q[3] + the_q[0] * the_q[1] );
120
121			Amat[Azx] = 2.0 * ( the_q[1] * the_q[3] + the_q[0] * the_q[2] );
122			Amat[Azy] = 2.0 * ( the_q[2] * the_q[3] - the_q[0] * the_q[1] );
123			Amat[Azz] = q0Sqr - q1Sqr -q2Sqr +q3Sqr;
124
125			this->updateU();
126			}
127			else{
128
129			sprintf( painCave.errMsg,
130			"Attempt to set Q for atom %d before coords set.\n",
131			index );
132			painCave.isFatal = 1;
133			simError();
134			}
135	mmeineke	377
136			}
137
138			void DirectionalAtom::getA( double the_A[3][3] ){
139
140	mmeineke	670	if( hasCoords ){
141			the_A[0][0] = Amat[Axx];
142			the_A[0][1] = Amat[Axy];
143			the_A[0][2] = Amat[Axz];
144
145			the_A[1][0] = Amat[Ayx];
146			the_A[1][1] = Amat[Ayy];
147			the_A[1][2] = Amat[Ayz];
148
149			the_A[2][0] = Amat[Azx];
150			the_A[2][1] = Amat[Azy];
151			the_A[2][2] = Amat[Azz];
152			}
153			else{
154
155			sprintf( painCave.errMsg,
156			"Attempt to get Amat for atom %d before coords set.\n",
157			index );
158			painCave.isFatal = 1;
159			simError();
160			}
161	mmeineke	377
162			}
163
164	mmeineke	597	void DirectionalAtom::printAmatIndex( void ){
165	mmeineke	377
166	mmeineke	670	if( hasCoords ){
167			std::cerr << "Atom[" << index << "] index =>\n"
168			<< "[ " << Axx << ", " << Axy << ", " << Axz << " ]\n"
169			<< "[ " << Ayx << ", " << Ayy << ", " << Ayz << " ]\n"
170			<< "[ " << Azx << ", " << Azy << ", " << Azz << " ]\n";
171			}
172			else{
173
174			sprintf( painCave.errMsg,
175			"Attempt to print Amat indices for atom %d before coords set.\n",
176			index );
177			painCave.isFatal = 1;
178			simError();
179			}
180	mmeineke	597	}
181
182
183	mmeineke	377	void DirectionalAtom::getU( double the_u[3] ){
184
185			the_u[0] = sux;
186			the_u[1] = suy;
187			the_u[2] = suz;
188
189			this->body2Lab( the_u );
190			}
191
192			void DirectionalAtom::getQ( double q[4] ){
193
194			double t, s;
195			double ad1, ad2, ad3;
196
197	mmeineke	670	if( hasCoords ){
198	mmeineke	377
199	mmeineke	670	t = Amat[Axx] + Amat[Ayy] + Amat[Azz] + 1.0;
200			if( t > 0.0 ){
201	mmeineke	377
202	mmeineke	670	s = 0.5 / sqrt( t );
203			q[0] = 0.25 / s;
204			q[1] = (Amat[Ayz] - Amat[Azy]) * s;
205			q[2] = (Amat[Azx] - Amat[Axz]) * s;
206			q[3] = (Amat[Axy] - Amat[Ayx]) * s;
207	mmeineke	377	}
208			else{
209
210	mmeineke	670	ad1 = fabs( Amat[Axx] );
211			ad2 = fabs( Amat[Ayy] );
212			ad3 = fabs( Amat[Azz] );
213
214			if( ad1 >= ad2 && ad1 >= ad3 ){
215
216			s = 2.0 * sqrt( 1.0 + Amat[Axx] - Amat[Ayy] - Amat[Azz] );
217			q[0] = (Amat[Ayz] + Amat[Azy]) / s;
218			q[1] = 0.5 / s;
219			q[2] = (Amat[Axy] + Amat[Ayx]) / s;
220			q[3] = (Amat[Axz] + Amat[Azx]) / s;
221			}
222			else if( ad2 >= ad1 && ad2 >= ad3 ){
223
224			s = sqrt( 1.0 + Amat[Ayy] - Amat[Axx] - Amat[Azz] ) * 2.0;
225			q[0] = (Amat[Axz] + Amat[Azx]) / s;
226			q[1] = (Amat[Axy] + Amat[Ayx]) / s;
227			q[2] = 0.5 / s;
228			q[3] = (Amat[Ayz] + Amat[Azy]) / s;
229			}
230			else{
231
232			s = sqrt( 1.0 + Amat[Azz] - Amat[Axx] - Amat[Ayy] ) * 2.0;
233			q[0] = (Amat[Axy] + Amat[Ayx]) / s;
234			q[1] = (Amat[Axz] + Amat[Azx]) / s;
235			q[2] = (Amat[Ayz] + Amat[Azy]) / s;
236			q[3] = 0.5 / s;
237			}
238	mmeineke	377	}
239			}
240	mmeineke	670	else{
241
242			sprintf( painCave.errMsg,
243			"Attempt to get Q for atom %d before coords set.\n",
244			index );
245			painCave.isFatal = 1;
246			simError();
247			}
248	mmeineke	377	}
249
250
251			void DirectionalAtom::setEuler( double phi, double theta, double psi ){
252
253	mmeineke	670	if( hasCoords ){
254			Amat[Axx] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi));
255			Amat[Axy] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi));
256			Amat[Axz] = sin(theta) * sin(psi);
257
258			Amat[Ayx] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi));
259			Amat[Ayy] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi));
260			Amat[Ayz] = sin(theta) * cos(psi);
261
262			Amat[Azx] = sin(phi) * sin(theta);
263			Amat[Azy] = -cos(phi) * sin(theta);
264			Amat[Azz] = cos(theta);
265
266			this->updateU();
267			}
268			else{
269
270			sprintf( painCave.errMsg,
271			"Attempt to set Euler angles for atom %d before coords set.\n",
272			index );
273			painCave.isFatal = 1;
274			simError();
275			}
276	mmeineke	377	}
277
278
279			void DirectionalAtom::lab2Body( double r[3] ){
280
281			double rl[3]; // the lab frame vector
282
283	mmeineke	670	if( hasCoords ){
284			rl[0] = r[0];
285			rl[1] = r[1];
286			rl[2] = r[2];
287
288			r[0] = (Amat[Axx] * rl[0]) + (Amat[Axy] * rl[1]) + (Amat[Axz] * rl[2]);
289			r[1] = (Amat[Ayx] * rl[0]) + (Amat[Ayy] * rl[1]) + (Amat[Ayz] * rl[2]);
290			r[2] = (Amat[Azx] * rl[0]) + (Amat[Azy] * rl[1]) + (Amat[Azz] * rl[2]);
291			}
292			else{
293
294			sprintf( painCave.errMsg,
295			"Attempt to convert lab2body for atom %d before coords set.\n",
296			index );
297			painCave.isFatal = 1;
298			simError();
299			}
300	mmeineke	377
301			}
302
303			void DirectionalAtom::body2Lab( double r[3] ){
304
305			double rb[3]; // the body frame vector
306
307	mmeineke	670	if( hasCoords ){
308			rb[0] = r[0];
309			rb[1] = r[1];
310			rb[2] = r[2];
311
312			r[0] = (Amat[Axx] * rb[0]) + (Amat[Ayx] * rb[1]) + (Amat[Azx] * rb[2]);
313			r[1] = (Amat[Axy] * rb[0]) + (Amat[Ayy] * rb[1]) + (Amat[Azy] * rb[2]);
314			r[2] = (Amat[Axz] * rb[0]) + (Amat[Ayz] * rb[1]) + (Amat[Azz] * rb[2]);
315			}
316			else{
317
318			sprintf( painCave.errMsg,
319			"Attempt to convert body2lab for atom %d before coords set.\n",
320			index );
321			painCave.isFatal = 1;
322			simError();
323			}
324	mmeineke	377	}
325
326			void DirectionalAtom::updateU( void ){
327
328	mmeineke	670	if( hasCoords ){
329			ul[offsetX] = (Amat[Axx] * sux) + (Amat[Ayx] * suy) + (Amat[Azx] * suz);
330			ul[offsetY] = (Amat[Axy] * sux) + (Amat[Ayy] * suy) + (Amat[Azy] * suz);
331			ul[offsetZ] = (Amat[Axz] * sux) + (Amat[Ayz] * suy) + (Amat[Azz] * suz);
332			}
333			else{
334
335			sprintf( painCave.errMsg,
336			"Attempt to updateU for atom %d before coords set.\n",
337			index );
338			painCave.isFatal = 1;
339			simError();
340			}
341	mmeineke	377	}
342
343	mmeineke	599	void DirectionalAtom::getJ( double theJ[3] ){
344
345			theJ[0] = jx;
346			theJ[1] = jy;
347			theJ[2] = jz;
348			}
349
350			void DirectionalAtom::setJ( double theJ[3] ){
351
352			jx = theJ[0];
353			jy = theJ[1];
354			jz = theJ[2];
355			}
356
357			void DirectionalAtom::getTrq( double theT[3] ){
358
359	mmeineke	670	if( hasCoords ){
360			theT[0] = trq[offsetX];
361			theT[1] = trq[offsetY];
362			theT[2] = trq[offsetZ];
363			}
364			else{
365
366			sprintf( painCave.errMsg,
367			"Attempt to get Trq for atom %d before coords set.\n",
368			index );
369			painCave.isFatal = 1;
370			simError();
371			}
372	mmeineke	599	}
373
374			void DirectionalAtom::addTrq( double theT[3] ){
375
376	mmeineke	670	if( hasCoords ){
377			trq[offsetX] += theT[0];
378			trq[offsetY] += theT[1];
379			trq[offsetZ] += theT[2];
380			}
381			else{
382
383			sprintf( painCave.errMsg,
384			"Attempt to add Trq for atom %d before coords set.\n",
385			index );
386			painCave.isFatal = 1;
387			simError();
388			}
389	mmeineke	599	}
390
391
392			void DirectionalAtom::getI( double the_I[3][3] ){
393
394			the_I[0][0] = Ixx;
395			the_I[0][1] = Ixy;
396			the_I[0][2] = Ixz;
397
398			the_I[1][0] = Iyx;
399			the_I[1][1] = Iyy;
400			the_I[1][2] = Iyz;
401
402			the_I[2][0] = Izx;
403			the_I[2][1] = Izy;
404			the_I[2][2] = Izz;
405			}
406	gezelter	878
407			void DirectionalAtom::getGrad( double grad[6] ) {
408
409			double myEuler[3];
410			double phi, theta, psi;
411			double cphi, sphi, ctheta, stheta;
412			double ephi[3];
413			double etheta[3];
414			double epsi[3];
415
416			this->getEulerAngles(myEuler);
417
418			phi = myEuler[0];
419			theta = myEuler[1];
420			psi = myEuler[2];
421
422			cphi = cos(phi);
423			sphi = sin(phi);
424			ctheta = cos(theta);
425			stheta = sin(theta);
426
427			// get unit vectors along the phi, theta and psi rotation axes
428
429			ephi[0] = 0.0;
430			ephi[1] = 0.0;
431			ephi[2] = 1.0;
432	tim	1046
433			etheta[0] = cphi;
434			etheta[1] = sphi;
435	gezelter	878	etheta[2] = 0.0;
436
437	tim	1046	epsi[0] = stheta * cphi;
438			epsi[1] = stheta * sphi;
439			epsi[2] = ctheta;
440
441	gezelter	878	for (int j = 0 ; j<3; j++)
442			grad[j] = frc[j];
443
444	tim	1046	grad[3] = 0;
445			grad[4] = 0;
446			grad[5] = 0;
447
448	gezelter	878	for (int j = 0; j < 3; j++ ) {
449
450			grad[3] += trq[j]*ephi[j];
451			grad[4] += trq[j]*etheta[j];
452			grad[5] += trq[j]*epsi[j];
453
454			}
455
456			}
457
458	tim	1046	/**
459			* getEulerAngles computes a set of Euler angle values consistent
460			* with an input rotation matrix. They are returned in the following
461			* order:
462			* myEuler[0] = phi;
463			* myEuler[1] = theta;
464			* myEuler[2] = psi;
465			*/
466	gezelter	878	void DirectionalAtom::getEulerAngles(double myEuler[3]) {
467
468	tim	1046	// We use so-called "x-convention", which is the most common definition.
469			// In this convention, the rotation given by Euler angles (phi, theta, psi), where the first
470			// rotation is by an angle phi about the z-axis, the second is by an angle
471			// theta (0 <= theta <= 180)about the x-axis, and thethird is by an angle psi about the
472			//z-axis (again).
473	gezelter	878
474	tim	1046
475	gezelter	878	double phi,theta,psi,eps;
476			double pi;
477			double cphi,ctheta,cpsi;
478			double sphi,stheta,spsi;
479			double b[3];
480			int flip[3];
481
482			// set the tolerance for Euler angles and rotation elements
483
484			eps = 1.0e-8;
485
486	tim	1046	theta = acos(min(1.0,max(-1.0,Amat[Azz])));
487			ctheta = Amat[Azz];
488			stheta = sqrt(1.0 - ctheta * ctheta);
489
490			// when sin(theta) is close to 0, we need to consider singularity
491			// In this case, we can assign an arbitary value to phi (or psi), and then determine
492			// the psi (or phi) or vice-versa. We'll assume that phi always gets the rotation, and psi is 0
493			// in cases of singularity.
494			// we use atan2 instead of atan, since atan2 will give us -Pi to Pi.
495			// Since 0 <= theta <= 180, sin(theta) will be always non-negative. Therefore, it never
496			// change the sign of both of the parameters passed to atan2.
497	gezelter	878
498	tim	1046	if (fabs(stheta) <= eps){
499			psi = 0.0;
500			phi = atan2(-Amat[Ayx], Amat[Axx]);
501	gezelter	878	}
502	tim	1046	// we only have one unique solution
503			else{
504			phi = atan2(Amat[Azx], -Amat[Azy]);
505			psi = atan2(Amat[Axz], Amat[Ayz]);
506			}
507	gezelter	878
508	tim	1046	//wrap phi and psi, make sure they are in the range from 0 to 2*Pi
509			//if (phi < 0)
510			// phi += M_PI;
511	gezelter	878
512	tim	1046	//if (psi < 0)
513			// psi += M_PI;
514	gezelter	878
515			myEuler[0] = phi;
516			myEuler[1] = theta;
517			myEuler[2] = psi;
518	tim	1046
519	gezelter	878	return;
520			}
521
522			double DirectionalAtom::max(double x, double y) {
523			return (x > y) ? x : y;
524			}
525
526			double DirectionalAtom::min(double x, double y) {
527			return (x > y) ? y : x;
528			}