--- trunk/OOPSE/libmdtools/Integrator.cpp	2004/04/20 16:56:40	1127
+++ trunk/OOPSE/libmdtools/Integrator.cpp	2004/08/23 15:11:36	1452
@@ -1,7 +1,8 @@
 #include <iostream>
 #include <stdlib.h>
 #include <math.h>
-
+#include "Rattle.hpp"
+#include "Roll.hpp"
 #ifdef IS_MPI
 #include "mpiSimulation.hpp"
 #include <unistd.h>
@@ -33,6 +34,16 @@ template<typename T> Integrator<T>::Integrator(SimInfo
   nAtoms = info->n_atoms;
   integrableObjects = info->integrableObjects;
 
+  consFramework = new RollFramework(info);
+
+  if(consFramework == NULL){
+    sprintf(painCave.errMsg,
+      "Integrator::Intergrator() Error: Memory allocation error for RattleFramework" );
+    painCave.isFatal = 1;
+    simError();
+  }
+  
+/*
   // check for constraints
 
   constrainedA = NULL;
@@ -45,9 +56,13 @@ template<typename T> Integrator<T>::Integrator(SimInfo
   nConstrained = 0;
 
   checkConstraints();
+*/
 }
 
 template<typename T> Integrator<T>::~Integrator(){
+  if (consFramework != NULL)
+    delete consFramework;
+/*
   if (nConstrained){
     delete[] constrainedA;
     delete[] constrainedB;
@@ -56,8 +71,10 @@ template<typename T> Integrator<T>::~Integrator(){
     delete[] moved;
     delete[] oldPos;
   }
+*/
 }
 
+/*
 template<typename T> void Integrator<T>::checkConstraints(void){
   isConstrained = 0;
 
@@ -92,7 +109,7 @@ template<typename T> void Integrator<T>::checkConstrai
       if (constrained){
         dummy_plug = theArray[j]->get_constraint();
         temp_con[nConstrained].set_a(dummy_plug->get_a());
-        temp_con[nConstrained].set_b(dummy_plug->get_b());
+        temp_con[nConstrained].set_b(Dummy_plug->get_b());
         temp_con[nConstrained].set_dsqr(dummy_plug->get_dsqr());
 
         nConstrained++;
@@ -150,8 +167,8 @@ template<typename T> void Integrator<T>::checkConstrai
 
   delete[] temp_con;
 }
+*/
 
-
 template<typename T> void Integrator<T>::integrate(void){
 
   double runTime = info->run_time;
@@ -160,7 +177,7 @@ template<typename T> void Integrator<T>::integrate(voi
   double thermalTime = info->thermalTime;
   double resetTime = info->resetTime;
 
-
+  double difference;
   double currSample;
   double currThermal;
   double currStatus;
@@ -182,17 +199,22 @@ template<typename T> void Integrator<T>::integrate(voi
   // remove center of mass drift velocity (in case we passed in a configuration
   // that was drifting
   tStats->removeCOMdrift();
+  //tStats->removeAngularMomentum();
+  
+  // initialize the retraints if necessary
+  if (info->useSolidThermInt && !info->useLiquidThermInt) {
+    myFF->initRestraints();
+  }
 
   // initialize the forces before the first step
 
   calcForce(1, 1);
-  
-  if (nConstrained){
-    preMove();
-    constrainA();
-    calcForce(1, 1);
-    constrainB();
-  }
+
+  //execute constraint algorithm to make sure at the very beginning the system is constrained  
+  //consFramework->doPreConstraint();
+  //consFramework->doConstrainA();
+  //calcForce(1, 1);
+  //consFramework->doConstrainB();
   
   if (info->setTemp){
     thermalize();
@@ -215,7 +237,8 @@ template<typename T> void Integrator<T>::integrate(voi
 #endif // is_mpi
 
   while (info->getTime() < runTime && !stopIntegrator()){
-    if ((info->getTime() + dt) >= currStatus){
+    difference = info->getTime() + dt - currStatus;
+    if (difference > 0 || fabs(difference) < 1e-4 ){
       calcPot = 1;
       calcStress = 1;
     }
@@ -270,6 +293,11 @@ template<typename T> void Integrator<T>::integrate(voi
 #endif // is_mpi
   }
 
+  // dump out a file containing the omega values for the final configuration
+  if (info->useSolidThermInt && !info->useLiquidThermInt)
+    myFF->dumpzAngle();
+  
+
   delete dumpOut;
   delete statOut;
 }
@@ -282,7 +310,8 @@ template<typename T> void Integrator<T>::integrateStep
   startProfile(pro3);
 #endif //profile
 
-  preMove();
+  //save old state (position, velocity etc)
+  consFramework->doPreConstraint();
 
 #ifdef PROFILE
   endProfile(pro3);
@@ -304,9 +333,7 @@ template<typename T> void Integrator<T>::integrateStep
   MPIcheckPoint();
 #endif // is_mpi
 
-
   // calc forces
-
   calcForce(calcPot, calcStress);
 
 #ifdef IS_MPI
@@ -320,6 +347,8 @@ template<typename T> void Integrator<T>::integrateStep
   startProfile( pro6 );
 #endif //profile
 
+  consFramework->doPreConstraint();
+
   // finish the velocity  half step
 
   moveB();
@@ -341,6 +370,7 @@ template<typename T> void Integrator<T>::moveA(void){
   double Tb[3], ji[3];
   double vel[3], pos[3], frc[3];
   double mass;
+  double omega;
  
   for (i = 0; i < integrableObjects.size() ; i++){
     integrableObjects[i]->getVel(vel);
@@ -376,12 +406,11 @@ template<typename T> void Integrator<T>::moveA(void){
       this->rotationPropagation( integrableObjects[i], ji );
 
       integrableObjects[i]->setJ(ji);
+
     }
   }
 
-  if (nConstrained){
-    constrainA();
-  }
+  consFramework->doConstrainA();
 }
 
 
@@ -420,13 +449,13 @@ template<typename T> void Integrator<T>::moveB(void){
 
       integrableObjects[i]->setJ(ji);
     }
+
   }
 
-  if (nConstrained){
-    constrainB();
-  }
+  consFramework->doConstrainB();
 }
 
+/*
 template<typename T> void Integrator<T>::preMove(void){
   int i, j;
   double pos[3];
@@ -685,7 +714,7 @@ template<typename T> void Integrator<T>::constrainB(vo
     simError();
   }
 }
-
+*/
 template<typename T> void Integrator<T>::rotationPropagation
 ( StuntDouble* sd, double ji[3] ){
 
@@ -724,6 +753,7 @@ template<typename T> void Integrator<T>::rotationPropa
     
     // rotate about the z-axis
     angle = dt * ji[2] / I[2][2];
+    sd->addZangle(angle);
     this->rotate( 0, 1, angle, ji, A);
     
     // rotate about the y-axis
@@ -838,3 +868,82 @@ template<typename T> string Integrator<T>::getAddition
   //return a pointer point to local variable which might cause problem
   return string();
 }
+
+
+template<typename T>  void Integrator<T>::printQuaternion(StuntDouble* sd){
+  Mat4x4d S;
+  double I[3][3];
+  Vector4d j4;
+  Vector3d j;
+  Vector3d tempJ;
+  Vector4d qdot;
+  Vector4d omega4;
+  Mat4x4d I4;
+  Quaternion q;
+  double I0;
+  Vector4d p_qua;
+  
+  if (sd->isDirectional()){
+    sd->getQ(q.vec);
+    sd->getI(I);
+    sd->getJ(j.vec);
+
+    //omega4[0] = 0.0;
+    //omega4[1] = j[0]/I[0][0];
+    //omega4[2] = j[1]/I[1][1];
+    //omega4[3] = j[2]/I[2][2];
+
+    //S = getS(q);
+    //qdot = 0.5 * S * omega4;
+
+    //I0 = (qdot[1] * q[1] * I[0][0] + qdot[2] * q[2] * I[1][1] + qdot[3] * q[3] * I[2][2])/(qdot[1] * q[1]+ qdot[2] * q[2] + qdot[3] * q[3]);
+
+    //I4.element[0][0] = I0;
+    //I4.element[1][1] = I[0][0];
+    //I4.element[2][2] = I[1][1];
+    //I4.element[3][3] = I[2][2];
+
+    S = getS(q);
+    j4[0] = 0.0;
+    j4[1] = j[0];
+    j4[2] = j[1];
+    j4[3] = j[2];
+    
+    p_qua = 2 * S * j4;
+
+    j4 = 0.5 * S.transpose() * p_qua;
+    //cout << "q0^2 + q1^2 + q2^2 + q3^2 = " << q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3] << endl; 
+    //cout << "q0*q0dot + q1*q1dot + q2 *q2dot + q3*q3dot = " <<q[0]*qdot[0] + q[1]*qdot[1] + q[2]*qdot[2] + q[3]*qdot[3] << endl;
+    //cout << "q1*q1dot* Ixx + q2*q2dot* Iyy + q3 *q3dot* Izz = " << qdot[1] * q[1] * I[0][0] + qdot[2] * q[2] * I[1][1] + qdot[3] * q[3] * I[2][2] << endl;
+    //cout << "q1*q1dot + q2 *q2dot + q3*q3dot = "  << qdot[1] * q[1]+ qdot[2] * q[2] + qdot[3] * q[3] << endl;
+    //cout << "I0 = " << I0 << endl;
+    cout << "p_qua[0] = " << p_qua[0] << endl;
+  }    
+}
+
+template<typename T> Mat4x4d Integrator<T>::getS(const Quaternion& q){
+  Mat4x4d result;
+
+  result.element[0][0] = q.x;
+  result.element[0][1] = -q.y;
+  result.element[0][2] = -q.z;
+  result.element[0][3] = -q.w;
+
+  result.element[1][0] = q.y;
+  result.element[1][1] = q.x;
+  result.element[1][2] = -q.w;
+  result.element[1][3] = q.z;
+
+  result.element[2][0] = q.z;
+  result.element[2][1] = q.w;
+  result.element[2][2] = q.x;
+  result.element[2][3] = -q.y;
+
+  result.element[3][0] = q.w;
+  result.element[3][1] = -q.z;
+  result.element[3][2] = q.y;
+  result.element[3][3] = q.x;
+
+  return result;  
+}
+