--- trunk/OOPSE/libmdtools/Integrator.cpp	2003/06/19 22:02:44	559
+++ trunk/OOPSE/libmdtools/Integrator.cpp	2003/07/11 22:34:48	594
@@ -1,5 +1,6 @@
 #include <iostream>
 #include <cstdlib>
+#include <cmath>
 
 #ifdef IS_MPI
 #include "mpiSimulation.hpp"
@@ -10,7 +11,7 @@
 #include "simError.h"
 
 
-Integrator::Integrator( SimInfo* theInfo, ForceFields* the_ff ){
+Integrator::Integrator( SimInfo *theInfo, ForceFields* the_ff ){
   
   info = theInfo;
   myFF = the_ff;
@@ -26,6 +27,8 @@ Integrator::Integrator( SimInfo* theInfo, ForceFields*
 
   nAtoms = info->n_atoms;
 
+  std::cerr << "integ nAtoms = "  << nAtoms << "\n";
+
   // check for constraints
   
   constrainedA    = NULL;
@@ -33,7 +36,7 @@ Integrator::Integrator( SimInfo* theInfo, ForceFields*
   constrainedDsqr = NULL;
   moving          = NULL;
   moved           = NULL;
-  prePos          = NULL;
+  oldPos          = NULL;
   
   nConstrained = 0;
 
@@ -48,7 +51,7 @@ Integrator::~Integrator() {
     delete[] constrainedDsqr;
     delete[] moving;
     delete[] moved;
-    delete[] prePos;
+    delete[] oldPos;
   }
   
 }
@@ -71,9 +74,14 @@ void Integrator::checkConstraints( void ){
     for(int j=0; j<molecules[i].getNBonds(); j++){
       
       constrained = theArray[j]->is_constrained();
+
+      std::cerr << "Is the folowing bond constrained \n";
+      theArray[j]->printMe();
       
       if(constrained){
 	
+	std::cerr << "Yes\n";
+
 	dummy_plug = theArray[j]->get_constraint();
 	temp_con[nConstrained].set_a( dummy_plug->get_a() );
 	temp_con[nConstrained].set_b( dummy_plug->get_b() );
@@ -81,7 +89,8 @@ void Integrator::checkConstraints( void ){
 	
 	nConstrained++;
 	constrained = 0;
-      }
+      } 
+      else std::cerr << "No.\n";
     }
 
     theArray = (SRI**) molecules[i].getMyBends();
@@ -136,6 +145,7 @@ void Integrator::checkConstraints( void ){
       constrainedA[i] = temp_con[i].get_a();
       constrainedB[i] = temp_con[i].get_b();
       constrainedDsqr[i] = temp_con[i].get_dsqr();
+
     }
 
     
@@ -146,7 +156,7 @@ void Integrator::checkConstraints( void ){
     moving = new int[nAtoms];
     moved  = new int[nAtoms];
 
-    prePos = new double[nAtoms*3];
+    oldPos = new double[nAtoms*3];
   }
   
   delete[] temp_con;
@@ -156,24 +166,7 @@ void Integrator::integrate( void ){
 void Integrator::integrate( void ){
 
   int i, j;                         // loop counters
-  double kE = 0.0;                  // the kinetic energy  
-  double rot_kE;
-  double trans_kE;
-  int tl;                        // the time loop conter
-  double dt2;                       // half the dt
 
-  double vx, vy, vz;    // the velocities
-  double vx2, vy2, vz2; // the square of the velocities
-  double rx, ry, rz;    // the postitions
-  
-  double ji[3];   // the body frame angular momentum
-  double jx2, jy2, jz2; // the square of the angular momentums
-  double Tb[3];   // torque in the body frame
-  double angle;   // the angle through which to rotate the rotation matrix
-  double A[3][3]; // the rotation matrix
-  double press[9];
-
-  double dt          = info->dt;
   double runTime     = info->run_time;
   double sampleTime  = info->sampleTime;
   double statusTime  = info->statusTime;
@@ -187,12 +180,16 @@ void Integrator::integrate( void ){
   int calcPot, calcStress;
   int isError;
 
+
+
   tStats   = new Thermo( info );
-  e_out    = new StatWriter( info );
-  dump_out = new DumpWriter( info );
+  statOut  = new StatWriter( info );
+  dumpOut  = new DumpWriter( info );
 
-  Atom** atoms = info->atoms;
+  atoms = info->atoms;
   DirectionalAtom* dAtom;
+
+  dt = info->dt;
   dt2 = 0.5 * dt;
 
   // initialize the forces before the first step
@@ -204,8 +201,8 @@ void Integrator::integrate( void ){
     tStats->velocitize();
   }
   
-  dump_out->writeDump( 0.0 );
-  e_out->writeStat( 0.0 );
+  dumpOut->writeDump( 0.0 );
+  statOut->writeStat( 0.0 );
   
   calcPot     = 0;
   calcStress  = 0;
@@ -223,13 +220,23 @@ void Integrator::integrate( void ){
   MPIcheckPoint();
 #endif // is_mpi
 
+
+  pos  = Atom::getPosArray();
+  vel  = Atom::getVelArray();
+  frc  = Atom::getFrcArray();
+  trq  = Atom::getTrqArray();
+  Amat = Atom::getAmatArray();
+
   while( currTime < runTime ){
 
     if( (currTime+dt) >= currStatus ){
       calcPot = 1;
       calcStress = 1;
     }
-    
+
+    std::cerr << "calcPot = " << calcPot << "; calcStress = "
+	      << calcStress << "\n";
+
     integrateStep( calcPot, calcStress );
       
     currTime += dt;
@@ -242,12 +249,12 @@ void Integrator::integrate( void ){
     }
 
     if( currTime >= currSample ){
-      dump_out->writeDump( currTime );
+      dumpOut->writeDump( currTime );
       currSample += sampleTime;
     }
 
     if( currTime >= currStatus ){ 
-      e_out->writeStat( time * dt ); 
+      statOut->writeStat( currTime ); 
       calcPot = 0; 
       calcStress = 0;
       currStatus += statusTime;
@@ -261,17 +268,19 @@ void Integrator::integrate( void ){
 
   }
 
-  dump_out->writeFinal();
+  dumpOut->writeFinal(currTime);
 
-  delete dump_out;
-  delete e_out;
+  delete dumpOut;
+  delete statOut;
 }
 
 void Integrator::integrateStep( int calcPot, int calcStress ){
 
+
+      
   // Position full step, and velocity half step
 
-  //preMove();
+  preMove();
   moveA();
   if( nConstrained ) constrainA();
 
@@ -294,20 +303,32 @@ void Integrator::moveA( void ){
   DirectionalAtom* dAtom;
   double Tb[3];
   double ji[3];
+  double angle;
+  double A[3][3];
 
+
   for( i=0; i<nAtoms; i++ ){
     atomIndex = i * 3;
     aMatIndex = i * 9;
-    
+
     // velocity half step
     for( j=atomIndex; j<(atomIndex+3); j++ )
       vel[j] += ( dt2 * frc[j] / atoms[i]->getMass() ) * eConvert;
+
+    std::cerr<< "MoveA vel[" << i << "] = "
+	     << vel[atomIndex] << "\t"
+	     << vel[atomIndex+1]<< "\t"
+	     << vel[atomIndex+2]<< "\n";
 
     // position whole step    
-    for( j=atomIndex; j<(atomIndex+3); j++ )
-      pos[j] += dt * vel[j];
+    for( j=atomIndex; j<(atomIndex+3); j++ ) pos[j] += dt * vel[j];
+    
 
-   
+    std::cerr<< "MoveA pos[" << i << "] = " 
+	     << pos[atomIndex] << "\t"
+	     << pos[atomIndex+1]<< "\t"
+	     << pos[atomIndex+2]<< "\n";
+
     if( atoms[i]->isDirectional() ){
 
       dAtom = (DirectionalAtom *)atoms[i];
@@ -329,25 +350,39 @@ void Integrator::moveA( void ){
       // use the angular velocities to propagate the rotation matrix a
       // full time step
       
+	  // get the atom's rotation matrix
+	  
+      A[0][0] = dAtom->getAxx();
+      A[0][1] = dAtom->getAxy();
+      A[0][2] = dAtom->getAxz();
+      
+      A[1][0] = dAtom->getAyx();
+      A[1][1] = dAtom->getAyy();
+      A[1][2] = dAtom->getAyz();
+      
+      A[2][0] = dAtom->getAzx();
+      A[2][1] = dAtom->getAzy();
+      A[2][2] = dAtom->getAzz();
+      
       // rotate about the x-axis      
       angle = dt2 * ji[0] / dAtom->getIxx();
-      this->rotate( 1, 2, angle, ji, &aMat[aMatIndex] ); 
+      this->rotate( 1, 2, angle, ji, A ); 
       
       // rotate about the y-axis
       angle = dt2 * ji[1] / dAtom->getIyy();
-      this->rotate( 2, 0, angle, ji, &aMat[aMatIndex] );
+      this->rotate( 2, 0, angle, ji, A );
       
       // rotate about the z-axis
       angle = dt * ji[2] / dAtom->getIzz();
-      this->rotate( 0, 1, angle, ji, &aMat[aMatIndex] );
+      this->rotate( 0, 1, angle, ji, A );
       
       // rotate about the y-axis
       angle = dt2 * ji[1] / dAtom->getIyy();
-      this->rotate( 2, 0, angle, ji, &aMat[aMatIndex] );
+      this->rotate( 2, 0, angle, ji, A );
       
        // rotate about the x-axis
       angle = dt2 * ji[0] / dAtom->getIxx();
-      this->rotate( 1, 2, angle, ji, &aMat[aMatIndex] );
+      this->rotate( 1, 2, angle, ji, A );
       
       dAtom->setJx( ji[0] );
       dAtom->setJy( ji[1] );
@@ -372,6 +407,12 @@ void Integrator::moveB( void ){
     for( j=atomIndex; j<(atomIndex+3); j++ )
       vel[j] += ( dt2 * frc[j] / atoms[i]->getMass() ) * eConvert;
 
+    std::cerr<< "MoveB vel[" << i << "] = "
+	     << vel[atomIndex] << "\t"
+	     << vel[atomIndex+1]<< "\t"
+	     << vel[atomIndex+2]<< "\n";
+
+
     if( atoms[i]->isDirectional() ){
       
       dAtom = (DirectionalAtom *)atoms[i];
@@ -391,10 +432,6 @@ void Integrator::moveB( void ){
       ji[1] = dAtom->getJy() + ( dt2 * Tb[1] ) * eConvert;
       ji[2] = dAtom->getJz() + ( dt2 * Tb[2] ) * eConvert;
       
-      jx2 = ji[0] * ji[0];
-      jy2 = ji[1] * ji[1];
-      jz2 = ji[2] * ji[2];
-      
       dAtom->setJx( ji[0] );
       dAtom->setJy( ji[1] );
       dAtom->setJz( ji[2] );
@@ -407,22 +444,7 @@ void Integrator::preMove( void ){
   int i;
 
   if( nConstrained ){
-    if( oldAtoms != nAtoms ){
-      
-      // save oldAtoms to check for lode balanceing later on.
-      
-      oldAtoms = nAtoms;
-      
-      delete[] moving;
-      delete[] moved;
-      delete[] oldPos;
-      
-      moving = new int[nAtoms];
-      moved  = new int[nAtoms];
-      
-      oldPos = new double[nAtoms*3];
-    }
-  
+
     for(i=0; i<(nAtoms*3); i++) oldPos[i] = pos[i];
   }
 }  
@@ -431,25 +453,23 @@ void Integrator::constrainA(){
 
   int i,j,k;
   int done;
-  double pxab, pyab, pzab;
-  double rxab, ryab, rzab;
-  int a, b;
+  double pab[3];
+  double rab[3];
+  int a, b, ax, ay, az, bx, by, bz;
   double rma, rmb;
   double dx, dy, dz;
+  double rpab;
   double rabsq, pabsq, rpabsq;
   double diffsq;
   double gab;
   int iteration;
 
-
-  
   for( i=0; i<nAtoms; i++){
     
     moving[i] = 0;
     moved[i]  = 1;
   }
-  
-  
+
   iteration = 0;
   done = 0;
   while( !done && (iteration < maxIteration )){
@@ -459,49 +479,51 @@ void Integrator::constrainA(){
 
       a = constrainedA[i];
       b = constrainedB[i];
-    
+      
+      ax = (a*3) + 0;
+      ay = (a*3) + 1;
+      az = (a*3) + 2;
+
+      bx = (b*3) + 0;
+      by = (b*3) + 1;
+      bz = (b*3) + 2;
+
       if( moved[a] || moved[b] ){
 	
-	pxab = pos[3*a+0] - pos[3*b+0];
-	pyab = pos[3*a+1] - pos[3*b+1];
-	pzab = pos[3*a+2] - pos[3*b+2];
+	pab[0] = pos[ax] - pos[bx];
+	pab[1] = pos[ay] - pos[by];
+	pab[2] = pos[az] - pos[bz];
 
-	//periodic boundary condition
-	pxab = pxab - info->box_x * copysign(1, pxab) 
-	  * int(pxab / info->box_x + 0.5);
-	pyab = pyab - info->box_y * copysign(1, pyab) 
-	  * int(pyab / info->box_y + 0.5);
-	pzab = pzab - info->box_z * copysign(1, pzab) 
-	  * int(pzab / info->box_z + 0.5);
-      
-	pabsq = pxab * pxab + pyab * pyab + pzab * pzab;
-	rabsq = constraintedDsqr[i];
-	diffsq = pabsq - rabsq;
+ 	//periodic boundary condition
 
+	info->wrapVector( pab );
+
+ 	pabsq = pab[0] * pab[0] + pab[1] * pab[1] + pab[2] * pab[2];
+
+	rabsq = constrainedDsqr[i];
+	diffsq = rabsq - pabsq;
+
 	// the original rattle code from alan tidesley
-	if (fabs(diffsq) > tol*rabsq*2) {
-	  rxab = oldPos[3*a+0] - oldPos[3*b+0];
-	  ryab = oldPos[3*a+1] - oldPos[3*b+1];
-	  rzab = oldPos[3*a+2] - oldPos[3*b+2];
- 
-	  rxab = rxab - info->box_x * copysign(1, rxab) 
-	    * int(rxab / info->box_x + 0.5);
- 	  ryab = ryab - info->box_y * copysign(1, ryab) 
-	    * int(ryab / info->box_y + 0.5);
-	  rzab = rzab - info->box_z * copysign(1, rzab) 
-	    * int(rzab / info->box_z + 0.5);
+	if (fabs(diffsq) > (tol*rabsq*2)) {
+	  rab[0] = oldPos[ax] - oldPos[bx];
+	  rab[1] = oldPos[ay] - oldPos[by];
+	  rab[2] = oldPos[az] - oldPos[bz];
 
-	  rpab = rxab * pxab + ryab * pyab + rzab * pzab;
+	  info->wrapVector( rab );
+
+	  rpab = rab[0] * pab[0] + rab[1] * pab[1] + rab[2] * pab[2];
+
 	  rpabsq = rpab * rpab;
 
 
 	  if (rpabsq < (rabsq * -diffsq)){
+
 #ifdef IS_MPI
 	    a = atoms[a]->getGlobalIndex();
 	    b = atoms[b]->getGlobalIndex();
 #endif //is_mpi
 	    sprintf( painCave.errMsg,
-		     "Constraint failure in constrainA at atom %d and %d\n.",
+		     "Constraint failure in constrainA at atom %d and %d.\n",
 		     a, b );
 	    painCave.isFatal = 1;
 	    simError();
@@ -509,31 +531,32 @@ void Integrator::constrainA(){
 
 	  rma = 1.0 / atoms[a]->getMass();
 	  rmb = 1.0 / atoms[b]->getMass();
-	  
+
 	  gab = diffsq / ( 2.0 * ( rma + rmb ) * rpab );
-          dx = rxab * gab;
-          dy = ryab * gab;
-          dz = rzab * gab;
 
-	  pos[3*a+0] += rma * dx;
-	  pos[3*a+1] += rma * dy;
-	  pos[3*a+2] += rma * dz;
+          dx = rab[0] * gab;
+          dy = rab[1] * gab;
+          dz = rab[2] * gab;
 
-	  pos[3*b+0] -= rmb * dx;
-	  pos[3*b+1] -= rmb * dy;
-	  pos[3*b+2] -= rmb * dz;
+	  pos[ax] += rma * dx;
+	  pos[ay] += rma * dy;
+	  pos[az] += rma * dz;
 
+	  pos[bx] -= rmb * dx;
+	  pos[by] -= rmb * dy;
+	  pos[bz] -= rmb * dz;
+
           dx = dx / dt;
           dy = dy / dt;
           dz = dz / dt;
 
-	  vel[3*a+0] += rma * dx;
-	  vel[3*a+1] += rma * dy;
-	  vel[3*a+2] += rma * dz;
+	  vel[ax] += rma * dx;
+	  vel[ay] += rma * dy;
+	  vel[az] += rma * dz;
 
-	  vel[3*b+0] -= rmb * dx;
-	  vel[3*b+1] -= rmb * dy;
-	  vel[3*b+2] -= rmb * dz;
+	  vel[bx] -= rmb * dx;
+	  vel[by] -= rmb * dy;
+	  vel[bz] -= rmb * dz;
 
 	  moving[a] = 1;
 	  moving[b] = 1;
@@ -553,9 +576,9 @@ void Integrator::constrainA(){
 
   if( !done ){
 
-    sprintf( painCae.errMsg,
+    sprintf( painCave.errMsg,
 	     "Constraint failure in constrainA, too many iterations: %d\n",
-	     iterations );
+	     iteration );
     painCave.isFatal = 1;
     simError();
   }
@@ -567,8 +590,8 @@ void Integrator::constrainB( void ){
   int i,j,k;
   int done;
   double vxab, vyab, vzab;
-  double rxab, ryab, rzab;
-  int a, b;
+  double rab[3];
+  int a, b, ax, ay, az, bx, by, bz;
   double rma, rmb;
   double dx, dy, dz;
   double rabsq, pabsq, rvab;
@@ -576,56 +599,62 @@ void Integrator::constrainB( void ){
   double gab;
   int iteration;
 
-  for(i=0; i<nAtom; i++){
+  for(i=0; i<nAtoms; i++){
     moving[i] = 0;
     moved[i] = 1;
   }
 
   done = 0;
+  iteration = 0;
   while( !done && (iteration < maxIteration ) ){
 
+    done = 1;
+
     for(i=0; i<nConstrained; i++){
       
       a = constrainedA[i];
       b = constrainedB[i];
 
+      ax = (a*3) + 0;
+      ay = (a*3) + 1;
+      az = (a*3) + 2;
+
+      bx = (b*3) + 0;
+      by = (b*3) + 1;
+      bz = (b*3) + 2;
+
       if( moved[a] || moved[b] ){
 	
-	vxab = vel[3*a+0] - vel[3*b+0];
-	vyab = vel[3*a+1] - vel[3*b+1];
-	vzab = vel[3*a+2] - vel[3*b+2];
+	vxab = vel[ax] - vel[bx];
+	vyab = vel[ay] - vel[by];
+	vzab = vel[az] - vel[bz];
 
-	rxab = pos[3*a+0] - pos[3*b+0];q
-	ryab = pos[3*a+1] - pos[3*b+1];
-	rzab = pos[3*a+2] - pos[3*b+2];
+	rab[0] = pos[ax] - pos[bx];
+	rab[1] = pos[ay] - pos[by];
+	rab[2] = pos[az] - pos[bz];
 	
-	rxab = rxab - info->box_x * copysign(1, rxab) 
-	  * int(rxab / info->box_x + 0.5);
-	ryab = ryab - info->box_y * copysign(1, ryab) 
-	  * int(ryab / info->box_y + 0.5);
-	rzab = rzab - info->box_z * copysign(1, rzab) 
-	  * int(rzab / info->box_z + 0.5);
-
+	info->wrapVector( rab );
+	
 	rma = 1.0 / atoms[a]->getMass();
 	rmb = 1.0 / atoms[b]->getMass();
 
-	rvab = rxab * vxab + ryab * vyab + rzab * vzab;
+	rvab = rab[0] * vxab + rab[1] * vyab + rab[2] * vzab;
 	  
-	gab = -rvab / ( ( rma + rmb ) * constraintsDsqr[i] );
+	gab = -rvab / ( ( rma + rmb ) * constrainedDsqr[i] );
 
 	if (fabs(gab) > tol) {
 	  
-	  dx = rxab * gab;
-	  dy = ryab * gab;
-	  dz = rzab * gab;
+	  dx = rab[0] * gab;
+	  dy = rab[1] * gab;
+	  dz = rab[2] * gab;
 	  
-	  vel[3*a+0] += rma * dx;
-	  vel[3*a+1] += rma * dy;
-	  vel[3*a+2] += rma * dz;
+	  vel[ax] += rma * dx;
+	  vel[ay] += rma * dy;
+	  vel[az] += rma * dz;
 
-	  vel[3*b+0] -= rmb * dx;
-	  vel[3*b+1] -= rmb * dy;
-	  vel[3*b+2] -= rmb * dz;
+	  vel[bx] -= rmb * dx;
+	  vel[by] -= rmb * dy;
+	  vel[bz] -= rmb * dz;
 	  
 	  moving[a] = 1;
 	  moving[b] = 1;
@@ -645,9 +674,9 @@ void Integrator::constrainB( void ){
   if( !done ){
 
    
-    sprintf( painCae.errMsg,
+    sprintf( painCave.errMsg,
 	     "Constraint failure in constrainB, too many iterations: %d\n",
-	     iterations );
+	     iteration );
     painCave.isFatal = 1;
     simError();
   } 
@@ -728,7 +757,7 @@ void Integrator::rotate( int axes1, int axes2, double 
   //            A[][] = A[][] * transpose(rot[][])
 
 
-  // NOte for as yet unknown reason, we are setting the performing the
+  // NOte for as yet unknown reason, we are performing the
   // calculation as:
   //                transpose(A[][]) = transpose(A[][]) * transpose(rot[][])