--- trunk/OOPSE/libmdtools/Integrator.cpp	2003/06/20 20:29:36	561
+++ trunk/OOPSE/libmdtools/Integrator.cpp	2003/07/14 21:28:54	597
@@ -27,6 +27,8 @@ Integrator::Integrator( SimInfo *theInfo, ForceFields*
 
   nAtoms = info->n_atoms;
 
+  std::cerr << "integ nAtoms = "  << nAtoms << "\n";
+
   // check for constraints
   
   constrainedA    = NULL;
@@ -72,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() );
@@ -82,7 +89,8 @@ void Integrator::checkConstraints( void ){
 	
 	nConstrained++;
 	constrained = 0;
-      }
+      } 
+      else std::cerr << "No.\n";
     }
 
     theArray = (SRI**) molecules[i].getMyBends();
@@ -137,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();
+
     }
 
     
@@ -215,8 +224,6 @@ void Integrator::integrate( void ){
   pos  = Atom::getPosArray();
   vel  = Atom::getVelArray();
   frc  = Atom::getFrcArray();
-  trq  = Atom::getTrqArray();
-  Amat = Atom::getAmatArray();
 
   while( currTime < runTime ){
 
@@ -225,6 +232,8 @@ void Integrator::integrate( void ){
       calcStress = 1;
     }
 
+    std::cerr << currTime << "\n";
+
     integrateStep( calcPot, calcStress );
       
     currTime += dt;
@@ -256,7 +265,7 @@ void Integrator::integrate( void ){
 
   }
 
-  dumpOut->writeFinal();
+  dumpOut->writeFinal(currTime);
 
   delete dumpOut;
   delete statOut;
@@ -270,7 +279,7 @@ void Integrator::integrateStep( int calcPot, int calcS
 
   preMove();
   moveA();
-  if( nConstrained ) constrainA();
+  //if( nConstrained ) constrainA();
 
   // calc forces
 
@@ -292,20 +301,22 @@ void Integrator::moveA( void ){
   double Tb[3];
   double ji[3];
   double angle;
+  double A[3][3], At[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;
 
+
     // 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];
+    
 
-   
     if( atoms[i]->isDirectional() ){
 
       dAtom = (DirectionalAtom *)atoms[i];
@@ -315,9 +326,9 @@ void Integrator::moveA( void ){
       Tb[0] = dAtom->getTx();
       Tb[1] = dAtom->getTy();
       Tb[2] = dAtom->getTz();
-      
+
       dAtom->lab2Body( Tb );
-      
+
       // get the angular momentum, and propagate a half step
       
       ji[0] = dAtom->getJx() + ( dt2 * Tb[0] ) * eConvert;
@@ -330,7 +341,7 @@ void Integrator::moveA( void ){
       // rotate about the x-axis      
       angle = dt2 * ji[0] / dAtom->getIxx();
       this->rotate( 1, 2, angle, ji, &Amat[aMatIndex] ); 
-      
+
       // rotate about the y-axis
       angle = dt2 * ji[1] / dAtom->getIyy();
       this->rotate( 2, 0, angle, ji, &Amat[aMatIndex] );
@@ -350,6 +361,15 @@ void Integrator::moveA( void ){
       dAtom->setJx( ji[0] );
       dAtom->setJy( ji[1] );
       dAtom->setJz( ji[2] );
+
+      std::cerr << "Amat[" << i << "]\n";
+      info->printMat9( &Amat[aMatIndex] );
+	  
+      std::cerr << "ji[" << i << "]\t"
+		<< ji[0] << "\t"
+		<< ji[1] << "\t"
+		<< ji[2] << "\n";
+	  
     }
     
   }
@@ -358,18 +378,20 @@ void Integrator::moveB( void ){
 
 void Integrator::moveB( void ){
   int i,j,k;
-  int atomIndex;
+  int atomIndex, aMatIndex;
   DirectionalAtom* dAtom;
   double Tb[3];
   double ji[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;
 
+ 
     if( atoms[i]->isDirectional() ){
       
       dAtom = (DirectionalAtom *)atoms[i];
@@ -380,6 +402,11 @@ void Integrator::moveB( void ){
       Tb[1] = dAtom->getTy();
       Tb[2] = dAtom->getTz();
       
+      std::cerr << "TrqB[" << i << "]\t"
+		<< Tb[0] << "\t"
+		<< Tb[1] << "\t"
+		<< Tb[2] << "\n";
+
       dAtom->lab2Body( Tb );
       
       // get the angular momentum, and complete the angular momentum
@@ -392,6 +419,15 @@ void Integrator::moveB( void ){
       dAtom->setJx( ji[0] );
       dAtom->setJy( ji[1] );
       dAtom->setJz( ji[2] );
+
+
+      std::cerr << "Amat[" << i << "]\n";
+      info->printMat9( &Amat[aMatIndex] );
+	  
+      std::cerr << "ji[" << i << "]\t"
+		<< ji[0] << "\t"
+		<< ji[1] << "\t"
+		<< ji[2] << "\n";
     }
   }
 
@@ -402,22 +438,6 @@ void Integrator::preMove( void ){
 
   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];
   }
 }  
@@ -426,9 +446,9 @@ 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;
@@ -437,15 +457,12 @@ void Integrator::constrainA(){
   double gab;
   int iteration;
 
-
-  
   for( i=0; i<nAtoms; i++){
     
     moving[i] = 0;
     moved[i]  = 1;
   }
-  
-  
+
   iteration = 0;
   done = 0;
   while( !done && (iteration < maxIteration )){
@@ -455,49 +472,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( fabs(pxab) / info->box_x + 0.5);
-	pyab = pyab - info->box_y * copysign(1, pyab) 
-	  * int( fabs(pyab) / info->box_y + 0.5);
-	pzab = pzab - info->box_z * copysign(1, pzab) 
-	  * int( fabs(pzab) / info->box_z + 0.5);
-      
-	pabsq = pxab * pxab + pyab * pyab + pzab * pzab;
+ 	//periodic boundary condition
+
+	info->wrapVector( pab );
+
+ 	pabsq = pab[0] * pab[0] + pab[1] * pab[1] + pab[2] * pab[2];
+
 	rabsq = constrainedDsqr[i];
-	diffsq = pabsq - rabsq;
+	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( fabs(rxab) / info->box_x + 0.5);
- 	  ryab = ryab - info->box_y * copysign(1, ryab) 
-	    * int( fabs(ryab) / info->box_y + 0.5);
-	  rzab = rzab - info->box_z * copysign(1, rzab) 
-	    * int( fabs(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();
@@ -505,31 +524,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;
@@ -563,8 +583,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;
@@ -581,48 +601,53 @@ void Integrator::constrainB( void ){
   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];
-	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( fabs(rxab) / info->box_x + 0.5);
-	ryab = ryab - info->box_y * copysign(1, ryab) 
-	  * int( fabs(ryab) / info->box_y + 0.5);
-	rzab = rzab - info->box_z * copysign(1, rzab) 
-	  * int( fabs(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 ) * 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;
@@ -670,11 +695,12 @@ void Integrator::rotate( int axes1, int axes2, double 
   double tempA[3][3];
   double tempJ[3];
 
+
   // initialize the tempA
 
   for(i=0; i<3; i++){
     for(j=0; j<3; j++){
-      tempA[j][i] = A[3*i + j];
+      tempA[j][i] = A[3*i+j];
     }
   }
 
@@ -731,9 +757,9 @@ void Integrator::rotate( int axes1, int axes2, double 
 
   for(i=0; i<3; i++){
     for(j=0; j<3; j++){
-      A[3*j + i] = 0.0;
+      A[3*j+i] = 0.0;
       for(k=0; k<3; k++){
-	A[3*j + i] += tempA[i][k] * rot[j][k];
+	A[3*j+i] += tempA[i][k] * rot[j][k];
       }
     }
   }