Ignore:
File:
1 edited

Legend:

Unmodified
Added
Removed

  • src/vector.cpp

    r9c20aa redb93c  
    55 */
    66
     7
    78#include "molecules.hpp"
    89
    … …  
    2829double Vector::DistanceSquared(const Vector *y) const
    2930{
    30         double res = 0.;
    31         for (int i=NDIM;i--;)
    32                 res += (x[i]-y->x[i])*(x[i]-y->x[i]);
    33         return (res);
     31  double res = 0.;
     32  for (int i=NDIM;i--;)
     33    res += (x[i]-y->x[i])*(x[i]-y->x[i]);
     34  return (res);
    3435};
    3536
    … …  
    4041double Vector::Distance(const Vector *y) const
    4142{
    42         double res = 0.;
    43         for (int i=NDIM;i--;)
    44                 res += (x[i]-y->x[i])*(x[i]-y->x[i]);
    45         return (sqrt(res));
     43  double res = 0.;
     44  for (int i=NDIM;i--;)
     45    res += (x[i]-y->x[i])*(x[i]-y->x[i]);
     46  return (sqrt(res));
    4647};
    4748
    … …  
    5354double Vector::PeriodicDistance(const Vector *y, const double *cell_size) const
    5455{
    55         double res = Distance(y), tmp, matrix[NDIM*NDIM];
    56         Vector Shiftedy, TranslationVector;
    57         int N[NDIM];
    58         matrix[0] = cell_size[0];
    59         matrix[1] = cell_size[1];
    60         matrix[2] = cell_size[3];
    61         matrix[3] = cell_size[1];
    62         matrix[4] = cell_size[2];
    63         matrix[5] = cell_size[4];
    64         matrix[6] = cell_size[3];
    65         matrix[7] = cell_size[4];
    66         matrix[8] = cell_size[5];
    67         // in order to check the periodic distance, translate one of the vectors into each of the 27 neighbouring cells
    68         for (N[0]=-1;N[0]<=1;N[0]++)
    69                 for (N[1]=-1;N[1]<=1;N[1]++)
    70                         for (N[2]=-1;N[2]<=1;N[2]++) {
    71                                 // create the translation vector
    72                                 TranslationVector.Zero();
    73                                 for (int i=NDIM;i--;)
    74                                         TranslationVector.x[i] = (double)N[i];
    75                                 TranslationVector.MatrixMultiplication(matrix);
    76                                 // add onto the original vector to compare with
    77                                 Shiftedy.CopyVector(y);
    78                                 Shiftedy.AddVector(&TranslationVector);
    79                                 // get distance and compare with minimum so far
    80                                 tmp = Distance(&Shiftedy);
    81                                 if (tmp < res) res = tmp;
    82                         }
    83         return (res);
     56  double res = Distance(y), tmp, matrix[NDIM*NDIM];
     57  Vector Shiftedy, TranslationVector;
     58  int N[NDIM];
     59  matrix[0] = cell_size[0];
     60  matrix[1] = cell_size[1];
     61  matrix[2] = cell_size[3];
     62  matrix[3] = cell_size[1];
     63  matrix[4] = cell_size[2];
     64  matrix[5] = cell_size[4];
     65  matrix[6] = cell_size[3];
     66  matrix[7] = cell_size[4];
     67  matrix[8] = cell_size[5];
     68  // in order to check the periodic distance, translate one of the vectors into each of the 27 neighbouring cells
     69  for (N[0]=-1;N[0]<=1;N[0]++)
     70    for (N[1]=-1;N[1]<=1;N[1]++)
     71      for (N[2]=-1;N[2]<=1;N[2]++) {
     72        // create the translation vector
     73        TranslationVector.Zero();
     74        for (int i=NDIM;i--;)
     75          TranslationVector.x[i] = (double)N[i];
     76        TranslationVector.MatrixMultiplication(matrix);
     77        // add onto the original vector to compare with
     78        Shiftedy.CopyVector(y);
     79        Shiftedy.AddVector(&TranslationVector);
     80        // get distance and compare with minimum so far
     81        tmp = Distance(&Shiftedy);
     82        if (tmp < res) res = tmp;
     83      }
     84  return (res);
    8485};
    8586
    … …  
    9192double Vector::PeriodicDistanceSquared(const Vector *y, const double *cell_size) const
    9293{
    93         double res = DistanceSquared(y), tmp, matrix[NDIM*NDIM];
    94         Vector Shiftedy, TranslationVector;
    95         int N[NDIM];
    96         matrix[0] = cell_size[0];
    97         matrix[1] = cell_size[1];
    98         matrix[2] = cell_size[3];
    99         matrix[3] = cell_size[1];
    100         matrix[4] = cell_size[2];
    101         matrix[5] = cell_size[4];
    102         matrix[6] = cell_size[3];
    103         matrix[7] = cell_size[4];
    104         matrix[8] = cell_size[5];
    105         // in order to check the periodic distance, translate one of the vectors into each of the 27 neighbouring cells
    106         for (N[0]=-1;N[0]<=1;N[0]++)
    107                 for (N[1]=-1;N[1]<=1;N[1]++)
    108                         for (N[2]=-1;N[2]<=1;N[2]++) {
    109                                 // create the translation vector
    110                                 TranslationVector.Zero();
    111                                 for (int i=NDIM;i--;)
    112                                         TranslationVector.x[i] = (double)N[i];
    113                                 TranslationVector.MatrixMultiplication(matrix);
    114                                 // add onto the original vector to compare with
    115                                 Shiftedy.CopyVector(y);
    116                                 Shiftedy.AddVector(&TranslationVector);
    117                                 // get distance and compare with minimum so far
    118                                 tmp = DistanceSquared(&Shiftedy);
    119                                 if (tmp < res) res = tmp;
    120                         }
    121         return (res);
     94  double res = DistanceSquared(y), tmp, matrix[NDIM*NDIM];
     95  Vector Shiftedy, TranslationVector;
     96  int N[NDIM];
     97  matrix[0] = cell_size[0];
     98  matrix[1] = cell_size[1];
     99  matrix[2] = cell_size[3];
     100  matrix[3] = cell_size[1];
     101  matrix[4] = cell_size[2];
     102  matrix[5] = cell_size[4];
     103  matrix[6] = cell_size[3];
     104  matrix[7] = cell_size[4];
     105  matrix[8] = cell_size[5];
     106  // in order to check the periodic distance, translate one of the vectors into each of the 27 neighbouring cells
     107  for (N[0]=-1;N[0]<=1;N[0]++)
     108    for (N[1]=-1;N[1]<=1;N[1]++)
     109      for (N[2]=-1;N[2]<=1;N[2]++) {
     110        // create the translation vector
     111        TranslationVector.Zero();
     112        for (int i=NDIM;i--;)
     113          TranslationVector.x[i] = (double)N[i];
     114        TranslationVector.MatrixMultiplication(matrix);
     115        // add onto the original vector to compare with
     116        Shiftedy.CopyVector(y);
     117        Shiftedy.AddVector(&TranslationVector);
     118        // get distance and compare with minimum so far
     119        tmp = DistanceSquared(&Shiftedy);
     120        if (tmp < res) res = tmp;
     121      }
     122  return (res);
    122123};
    123124
    … …  
    128129void Vector::KeepPeriodic(ofstream *out, double *matrix)
    129130{
    130 //      int N[NDIM];
    131 //      bool flag = false;
    132         //vector Shifted, TranslationVector;
    133         Vector TestVector;
    134 //      *out << Verbose(1) << "Begin of KeepPeriodic." << endl;
    135 //      *out << Verbose(2) << "Vector is: ";
    136 //      Output(out);
    137 //      *out << endl;
    138         TestVector.CopyVector(this);
    139         TestVector.InverseMatrixMultiplication(matrix);
    140         for(int i=NDIM;i--;) { // correct periodically
    141                 if (TestVector.x[i] < 0) {      // get every coefficient into the interval [0,1)
    142                         TestVector.x[i] += ceil(TestVector.x[i]);
    143                 } else {
    144                         TestVector.x[i] -= floor(TestVector.x[i]);
    145                 }
    146         }
    147         TestVector.MatrixMultiplication(matrix);
    148         CopyVector(&TestVector);
    149 //      *out << Verbose(2) << "New corrected vector is: ";
    150 //      Output(out);
    151 //      *out << endl;
    152 //      *out << Verbose(1) << "End of KeepPeriodic." << endl;
     131//  int N[NDIM];
     132//  bool flag = false;
     133  //vector Shifted, TranslationVector;
     134  Vector TestVector;
     135//  *out << Verbose(1) << "Begin of KeepPeriodic." << endl;
     136//  *out << Verbose(2) << "Vector is: ";
     137//  Output(out);
     138//  *out << endl;
     139  TestVector.CopyVector(this);
     140  TestVector.InverseMatrixMultiplication(matrix);
     141  for(int i=NDIM;i--;) { // correct periodically
     142    if (TestVector.x[i] < 0) {  // get every coefficient into the interval [0,1)
     143      TestVector.x[i] += ceil(TestVector.x[i]);
     144    } else {
     145      TestVector.x[i] -= floor(TestVector.x[i]);
     146    }
     147  }
     148  TestVector.MatrixMultiplication(matrix);
     149  CopyVector(&TestVector);
     150//  *out << Verbose(2) << "New corrected vector is: ";
     151//  Output(out);
     152//  *out << endl;
     153//  *out << Verbose(1) << "End of KeepPeriodic." << endl;
    153154};
    154155
    … …  
    159160double Vector::ScalarProduct(const Vector *y) const
    160161{
    161         double res = 0.;
    162         for (int i=NDIM;i--;)
    163                 res += x[i]*y->x[i];
    164         return (res);
     162  double res = 0.;
     163  for (int i=NDIM;i--;)
     164    res += x[i]*y->x[i];
     165  return (res);
    165166};
    166167
    167168
    168169/** Calculates VectorProduct between this and another vector.
    169  *      -# returns the Product in place of vector from which it was initiated
    170  *      -# ATTENTION: Only three dim.
    171  *      \param *y array to vector with which to calculate crossproduct
    172  *      \return \f$ x \times y \f&
     170 *  -# returns the Product in place of vector from which it was initiated
     171 *  -# ATTENTION: Only three dim.
     172 *  \param *y array to vector with which to calculate crossproduct
     173 *  \return \f$ x \times y \f&
    173174 */
    174175void Vector::VectorProduct(const Vector *y)
    175176{
    176         Vector tmp;
    177         tmp.x[0] = x[1]* (y->x[2]) - x[2]* (y->x[1]);
    178         tmp.x[1] = x[2]* (y->x[0]) - x[0]* (y->x[2]);
    179         tmp.x[2] = x[0]* (y->x[1]) - x[1]* (y->x[0]);
    180         this->CopyVector(&tmp);
     177  Vector tmp;
     178  tmp.x[0] = x[1]* (y->x[2]) - x[2]* (y->x[1]);
     179  tmp.x[1] = x[2]* (y->x[0]) - x[0]* (y->x[2]);
     180  tmp.x[2] = x[0]* (y->x[1]) - x[1]* (y->x[0]);
     181  this->CopyVector(&tmp);
    181182
    182183};
    … …  
    189190void Vector::ProjectOntoPlane(const Vector *y)
    190191{
    191         Vector tmp;
    192         tmp.CopyVector(y);
    193         tmp.Normalize();
    194         tmp.Scale(ScalarProduct(&tmp));
    195         this->SubtractVector(&tmp);
     192  Vector tmp;
     193  tmp.CopyVector(y);
     194  tmp.Normalize();
     195  tmp.Scale(ScalarProduct(&tmp));
     196  this->SubtractVector(&tmp);
     197};
     198
     199/** Calculates the intersection point between a line defined by \a *LineVector and \a *LineVector2 and a plane defined by \a *Normal and \a *PlaneOffset.
     200 * According to [Bronstein] the vectorial plane equation is:
     201 *   -# \f$\stackrel{r}{\rightarrow} \cdot \stackrel{N}{\rightarrow} + D = 0\f$,
     202 * where \f$\stackrel{r}{\rightarrow}\f$ is the vector to be testet, \f$\stackrel{N}{\rightarrow}\f$ is the plane's normal vector and
     203 * \f$D = - \stackrel{a}{\rightarrow} \stackrel{N}{\rightarrow}\f$, the offset with respect to origin, if \f$\stackrel{a}{\rightarrow}\f$,
     204 * is an offset vector onto the plane. The line is parametrized by \f$\stackrel{x}{\rightarrow} + k \stackrel{t}{\rightarrow}\f$, where
     205 * \f$\stackrel{x}{\rightarrow}\f$ is the offset and \f$\stackrel{t}{\rightarrow}\f$ the directional vector (NOTE: No need to normalize
     206 * the latter). Inserting the parametrized form into the plane equation and solving for \f$k\f$, which we insert then into the parametrization
     207 * of the line yields the intersection point on the plane.
     208 * \param *out output stream for debugging
     209 * \param *PlaneNormal Plane's normal vector
     210 * \param *PlaneOffset Plane's offset vector
     211 * \param *LineVector first vector of line
     212 * \param *LineVector2 second vector of line
     213 * \return true -  \a this contains intersection point on return, false - line is parallel to plane
     214 */
     215bool Vector::GetIntersectionWithPlane(ofstream *out, Vector *PlaneNormal, Vector *PlaneOffset, Vector *LineVector, Vector *LineVector2)
     216{
     217  double factor;
     218  Vector Direction;
     219
     220  // find intersection of a line defined by Offset and Direction with a  plane defined by triangle
     221  Direction.CopyVector(LineVector2);
     222  Direction.SubtractVector(LineVector);
     223  if (Direction.ScalarProduct(PlaneNormal) < MYEPSILON)
     224    return false;
     225  factor = LineVector->ScalarProduct(PlaneNormal)*(-PlaneOffset->ScalarProduct(PlaneNormal))/(Direction.ScalarProduct(PlaneNormal));
     226  Direction.Scale(factor);
     227  CopyVector(LineVector);
     228  SubtractVector(&Direction);
     229
     230  // test whether resulting vector really is on plane
     231  Direction.CopyVector(this);
     232  Direction.SubtractVector(PlaneOffset);
     233  if (Direction.ScalarProduct(PlaneNormal) > MYEPSILON)
     234    return true;
     235  else
     236    return false;
     237};
     238
     239/** Calculates the intersection of the two lines that are both on the same plane.
     240 * Note that we do not check whether they are on the same plane.
     241 * \param *out output stream for debugging
     242 * \param *Line1a first vector of first line
     243 * \param *Line1b second vector of first line
     244 * \param *Line2a first vector of second line
     245 * \param *Line2b second vector of second line
     246 * \return true - \a this will contain the intersection on return, false - lines are parallel
     247 */
     248bool Vector::GetIntersectionOfTwoLinesOnPlane(ofstream *out, Vector *Line1a, Vector *Line1b, Vector *Line2a, Vector *Line2b)
     249{
     250  double k1,a1,h1,b1,k2,a2,h2,b2;
     251  // equation for line 1
     252  if (fabs(Line1a->x[0] - Line2a->x[0]) < MYEPSILON) {
     253    k1 = 0;
     254    h1 = 0;
     255  } else {
     256    k1 = (Line1a->x[1] - Line2a->x[1])/(Line1a->x[0] - Line2a->x[0]);
     257    h1 = (Line1a->x[2] - Line2a->x[2])/(Line1a->x[0] - Line2a->x[0]);
     258  }
     259  a1 = 0.5*((Line1a->x[1] + Line2a->x[1]) - k1*(Line1a->x[0] + Line2a->x[0]));
     260  b1 = 0.5*((Line1a->x[2] + Line2a->x[2]) - h1*(Line1a->x[0] + Line2a->x[0]));
     261
     262  // equation for line 2
     263  if (fabs(Line2a->x[0] - Line2a->x[0]) < MYEPSILON) {
     264    k1 = 0;
     265    h1 = 0;
     266  } else {
     267    k1 = (Line2a->x[1] - Line2a->x[1])/(Line2a->x[0] - Line2a->x[0]);
     268    h1 = (Line2a->x[2] - Line2a->x[2])/(Line2a->x[0] - Line2a->x[0]);
     269  }
     270  a1 = 0.5*((Line2a->x[1] + Line2a->x[1]) - k1*(Line2a->x[0] + Line2a->x[0]));
     271  b1 = 0.5*((Line2a->x[2] + Line2a->x[2]) - h1*(Line2a->x[0] + Line2a->x[0]));
     272
     273  // calculate cross point
     274  if (fabs((a1-a2)*(h1-h2) - (b1-b2)*(k1-k2)) < MYEPSILON) {
     275    x[0] = (a2-a1)/(k1-k2);
     276    x[1] = (k1*a2-k2*a1)/(k1-k2);
     277    x[2] = (h1*b2-h2*b1)/(h1-h2);
     278    *out << Verbose(4) << "Lines do intersect at " << this << "." << endl;
     279    return true;
     280  } else {
     281    *out << Verbose(4) << "Lines do not intersect." << endl;
     282    return false;
     283  }
    196284};
    197285
    … …  
    202290double Vector::Projection(const Vector *y) const
    203291{
    204         return (ScalarProduct(y));
     292  return (ScalarProduct(y));
    205293};
    206294
    … …  
    210298double Vector::Norm() const
    211299{
    212         double res = 0.;
    213         for (int i=NDIM;i--;)
    214                 res += this->x[i]*this->x[i];
    215         return (sqrt(res));
     300  double res = 0.;
     301  for (int i=NDIM;i--;)
     302    res += this->x[i]*this->x[i];
     303  return (sqrt(res));
     304};
     305
     306/** Calculates squared norm of this vector.
     307 * \return \f$|x|^2\f$
     308 */
     309double Vector::NormSquared() const
     310{
     311  return (ScalarProduct(this));
    216312};
    217313
    … …  
    220316void Vector::Normalize()
    221317{
    222         double res = 0.;
    223         for (int i=NDIM;i--;)
    224                 res += this->x[i]*this->x[i];
    225         if (fabs(res) > MYEPSILON)
    226                 res = 1./sqrt(res);
    227         Scale(&res);
     318  double res = 0.;
     319  for (int i=NDIM;i--;)
     320    res += this->x[i]*this->x[i];
     321  if (fabs(res) > MYEPSILON)
     322    res = 1./sqrt(res);
     323  Scale(&res);
    228324};
    229325
    … …  
    232328void Vector::Zero()
    233329{
    234         for (int i=NDIM;i--;)
    235                 this->x[i] = 0.;
     330  for (int i=NDIM;i--;)
     331    this->x[i] = 0.;
    236332};
    237333
    … …  
    240336void Vector::One(double one)
    241337{
    242         for (int i=NDIM;i--;)
    243                 this->x[i] = one;
     338  for (int i=NDIM;i--;)
     339    this->x[i] = one;
    244340};
    245341
    … …  
    248344void Vector::Init(double x1, double x2, double x3)
    249345{
    250         x[0] = x1;
    251         x[1] = x2;
    252         x[2] = x3;
    253 };
    254 
    255 /** Checks whether vector has all components zero.
    256  * @return true - vector is zero, false - vector is not
    257  */
    258 bool Vector::IsNull()
    259 {
    260   return (fabs(x[0]+x[1]+x[2]) < MYEPSILON);
     346  x[0] = x1;
     347  x[1] = x2;
     348  x[2] = x3;
    261349};
    262350
    … …  
    267355double Vector::Angle(const Vector *y) const
    268356{
    269   double angle = this->ScalarProduct(y)/Norm()/y->Norm();
     357  double norm1 = Norm(), norm2 = y->Norm();
     358  double angle = 1;
     359  if ((fabs(norm1) > MYEPSILON) && (fabs(norm2) > MYEPSILON))
     360    angle = this->ScalarProduct(y)/norm1/norm2;
    270361  // -1-MYEPSILON occured due to numerical imprecision, catch ...
    271362  //cout << Verbose(2) << "INFO: acos(-1) = " << acos(-1) << ", acos(-1+MYEPSILON) = " << acos(-1+MYEPSILON) << ", acos(-1-MYEPSILON) = " << acos(-1-MYEPSILON) << "." << endl;
    … …  
    274365  if (angle > 1)
    275366    angle = 1;
    276         return acos(angle);
     367  return acos(angle);
    277368};
    278369
    … …  
    283374void Vector::RotateVector(const Vector *axis, const double alpha)
    284375{
    285         Vector a,y;
    286         // normalise this vector with respect to axis
    287         a.CopyVector(this);
    288         a.Scale(Projection(axis));
    289         SubtractVector(&a);
    290         // construct normal vector
    291         y.MakeNormalVector(axis,this);
    292         y.Scale(Norm());
    293         // scale normal vector by sine and this vector by cosine
    294         y.Scale(sin(alpha));
    295         Scale(cos(alpha));
    296         // add scaled normal vector onto this vector
    297         AddVector(&y);
    298         // add part in axis direction
    299         AddVector(&a);
     376  Vector a,y;
     377  // normalise this vector with respect to axis
     378  a.CopyVector(this);
     379  a.Scale(Projection(axis));
     380  SubtractVector(&a);
     381  // construct normal vector
     382  y.MakeNormalVector(axis,this);
     383  y.Scale(Norm());
     384  // scale normal vector by sine and this vector by cosine
     385  y.Scale(sin(alpha));
     386  Scale(cos(alpha));
     387  // add scaled normal vector onto this vector
     388  AddVector(&y);
     389  // add part in axis direction
     390  AddVector(&a);
    300391};
    301392
    … …  
    307398Vector& operator+=(Vector& a, const Vector& b)
    308399{
    309         a.AddVector(&b);
    310         return a;
     400  a.AddVector(&b);
     401  return a;
    311402};
    312403/** factor each component of \a a times a double \a m.
    … …  
    317408Vector& operator*=(Vector& a, const double m)
    318409{
    319         a.Scale(m);
    320         return a;
    321 };
    322 
    323 /** Sums two vectors \a and \b component-wise.
     410  a.Scale(m);
     411  return a;
     412};
     413
     414/** Sums two vectors \a  and \b component-wise.
    324415 * \param a first vector
    325416 * \param b second vector
    … …  
    328419Vector& operator+(const Vector& a, const Vector& b)
    329420{
    330         Vector *x = new Vector;
    331         x->CopyVector(&a);
    332         x->AddVector(&b);
    333         return *x;
     421  Vector *x = new Vector;
     422  x->CopyVector(&a);
     423  x->AddVector(&b);
     424  return *x;
    334425};
    335426
    … …  
    341432Vector& operator*(const Vector& a, const double m)
    342433{
    343         Vector *x = new Vector;
    344         x->CopyVector(&a);
    345         x->Scale(m);
    346         return *x;
     434  Vector *x = new Vector;
     435  x->CopyVector(&a);
     436  x->Scale(m);
     437  return *x;
    347438};
    348439
    … …  
    353444bool Vector::Output(ofstream *out) const
    354445{
    355         if (out != NULL) {
    356                 *out << "(";
    357                 for (int i=0;i<NDIM;i++) {
    358                         *out << x[i];
    359                         if (i != 2)
    360                                 *out << ",";
    361                 }
    362                 *out << ")";
    363                 return true;
    364         } else
    365                 return false;
    366 };
    367 
    368 ostream& operator<<(ostream& ost, const Vector& m)
    369 {
    370         ost << "(";
    371         for (int i=0;i<NDIM;i++) {
    372                 ost << m.x[i];
    373                 if (i != 2)
    374                         ost << ",";
    375         }
    376         ost << ")";
    377         return ost;
     446  if (out != NULL) {
     447    *out << "(";
     448    for (int i=0;i<NDIM;i++) {
     449      *out << x[i];
     450      if (i != 2)
     451        *out << ",";
     452    }
     453    *out << ")";
     454    return true;
     455  } else
     456    return false;
     457};
     458
     459ostream& operator<<(ostream& ost,Vector& m)
     460{
     461  ost << "(";
     462  for (int i=0;i<NDIM;i++) {
     463    ost << m.x[i];
     464    if (i != 2)
     465      ost << ",";
     466  }
     467  ost << ")";
     468  return ost;
    378469};
    379470
    … …  
    383474void Vector::Scale(double **factor)
    384475{
    385         for (int i=NDIM;i--;)
    386                 x[i] *= (*factor)[i];
     476  for (int i=NDIM;i--;)
     477    x[i] *= (*factor)[i];
    387478};
    388479
    389480void Vector::Scale(double *factor)
    390481{
    391         for (int i=NDIM;i--;)
    392                 x[i] *= *factor;
     482  for (int i=NDIM;i--;)
     483    x[i] *= *factor;
    393484};
    394485
    395486void Vector::Scale(double factor)
    396487{
    397         for (int i=NDIM;i--;)
    398                 x[i] *= factor;
     488  for (int i=NDIM;i--;)
     489    x[i] *= factor;
    399490};
    400491
    … …  
    404495void Vector::Translate(const Vector *trans)
    405496{
    406         for (int i=NDIM;i--;)
    407                 x[i] += trans->x[i];
     497  for (int i=NDIM;i--;)
     498    x[i] += trans->x[i];
    408499};
    409500
    … …  
    413504void Vector::MatrixMultiplication(double *M)
    414505{
    415         Vector C;
    416         // do the matrix multiplication
    417         C.x[0] = M[0]*x[0]+M[3]*x[1]+M[6]*x[2];
    418         C.x[1] = M[1]*x[0]+M[4]*x[1]+M[7]*x[2];
    419         C.x[2] = M[2]*x[0]+M[5]*x[1]+M[8]*x[2];
    420         // transfer the result into this
    421         for (int i=NDIM;i--;)
    422                 x[i] = C.x[i];
    423 };
    424 
    425 /** Do a matrix multiplication with \a *matrix' inverse.
     506  Vector C;
     507  // do the matrix multiplication
     508  C.x[0] = M[0]*x[0]+M[3]*x[1]+M[6]*x[2];
     509  C.x[1] = M[1]*x[0]+M[4]*x[1]+M[7]*x[2];
     510  C.x[2] = M[2]*x[0]+M[5]*x[1]+M[8]*x[2];
     511  // transfer the result into this
     512  for (int i=NDIM;i--;)
     513    x[i] = C.x[i];
     514};
     515
     516/** Calculate the inverse of a 3x3 matrix.
    426517 * \param *matrix NDIM_NDIM array
    427518 */
     519double * Vector::InverseMatrix(double *A)
     520{
     521  double *B = (double *) Malloc(sizeof(double)*NDIM*NDIM, "Vector::InverseMatrix: *B");
     522  double detA = RDET3(A);
     523  double detAReci;
     524
     525  for (int i=0;i<NDIM*NDIM;++i)
     526    B[i] = 0.;
     527  // calculate the inverse B
     528  if (fabs(detA) > MYEPSILON) {;  // RDET3(A) yields precisely zero if A irregular
     529    detAReci = 1./detA;
     530    B[0] =  detAReci*RDET2(A[4],A[5],A[7],A[8]);    // A_11
     531    B[1] = -detAReci*RDET2(A[1],A[2],A[7],A[8]);    // A_12
     532    B[2] =  detAReci*RDET2(A[1],A[2],A[4],A[5]);    // A_13
     533    B[3] = -detAReci*RDET2(A[3],A[5],A[6],A[8]);    // A_21
     534    B[4] =  detAReci*RDET2(A[0],A[2],A[6],A[8]);    // A_22
     535    B[5] = -detAReci*RDET2(A[0],A[2],A[3],A[5]);    // A_23
     536    B[6] =  detAReci*RDET2(A[3],A[4],A[6],A[7]);    // A_31
     537    B[7] = -detAReci*RDET2(A[0],A[1],A[6],A[7]);    // A_32
     538    B[8] =  detAReci*RDET2(A[0],A[1],A[3],A[4]);    // A_33
     539  }
     540  return B;
     541};
     542
     543/** Do a matrix multiplication with the \a *A' inverse.
     544 * \param *matrix NDIM_NDIM array
     545 */
    428546void Vector::InverseMatrixMultiplication(double *A)
    429547{
    430         Vector C;
    431         double B[NDIM*NDIM];
    432         double detA = RDET3(A);
    433         double detAReci;
    434 
    435         // calculate the inverse B
    436         if (fabs(detA) > MYEPSILON) {;  // RDET3(A) yields precisely zero if A irregular
    437                 detAReci = 1./detA;
    438                 B[0] =  detAReci*RDET2(A[4],A[5],A[7],A[8]);            // A_11
    439                 B[1] = -detAReci*RDET2(A[1],A[2],A[7],A[8]);            // A_12
    440                 B[2] =  detAReci*RDET2(A[1],A[2],A[4],A[5]);            // A_13
    441                 B[3] = -detAReci*RDET2(A[3],A[5],A[6],A[8]);            // A_21
    442                 B[4] =  detAReci*RDET2(A[0],A[2],A[6],A[8]);            // A_22
    443                 B[5] = -detAReci*RDET2(A[0],A[2],A[3],A[5]);            // A_23
    444                 B[6] =  detAReci*RDET2(A[3],A[4],A[6],A[7]);            // A_31
    445                 B[7] = -detAReci*RDET2(A[0],A[1],A[6],A[7]);            // A_32
    446                 B[8] =  detAReci*RDET2(A[0],A[1],A[3],A[4]);            // A_33
    447 
    448                 // do the matrix multiplication
    449                 C.x[0] = B[0]*x[0]+B[3]*x[1]+B[6]*x[2];
    450                 C.x[1] = B[1]*x[0]+B[4]*x[1]+B[7]*x[2];
    451                 C.x[2] = B[2]*x[0]+B[5]*x[1]+B[8]*x[2];
    452                 // transfer the result into this
    453                 for (int i=NDIM;i--;)
    454                         x[i] = C.x[i];
    455         } else {
    456                 cerr << "ERROR: inverse of matrix does not exists!" << endl;
    457         }
     548  Vector C;
     549  double B[NDIM*NDIM];
     550  double detA = RDET3(A);
     551  double detAReci;
     552
     553  // calculate the inverse B
     554  if (fabs(detA) > MYEPSILON) {;  // RDET3(A) yields precisely zero if A irregular
     555    detAReci = 1./detA;
     556    B[0] =  detAReci*RDET2(A[4],A[5],A[7],A[8]);    // A_11
     557    B[1] = -detAReci*RDET2(A[1],A[2],A[7],A[8]);    // A_12
     558    B[2] =  detAReci*RDET2(A[1],A[2],A[4],A[5]);    // A_13
     559    B[3] = -detAReci*RDET2(A[3],A[5],A[6],A[8]);    // A_21
     560    B[4] =  detAReci*RDET2(A[0],A[2],A[6],A[8]);    // A_22
     561    B[5] = -detAReci*RDET2(A[0],A[2],A[3],A[5]);    // A_23
     562    B[6] =  detAReci*RDET2(A[3],A[4],A[6],A[7]);    // A_31
     563    B[7] = -detAReci*RDET2(A[0],A[1],A[6],A[7]);    // A_32
     564    B[8] =  detAReci*RDET2(A[0],A[1],A[3],A[4]);    // A_33
     565
     566    // do the matrix multiplication
     567    C.x[0] = B[0]*x[0]+B[3]*x[1]+B[6]*x[2];
     568    C.x[1] = B[1]*x[0]+B[4]*x[1]+B[7]*x[2];
     569    C.x[2] = B[2]*x[0]+B[5]*x[1]+B[8]*x[2];
     570    // transfer the result into this
     571    for (int i=NDIM;i--;)
     572      x[i] = C.x[i];
     573  } else {
     574    cerr << "ERROR: inverse of matrix does not exists: det A = " << detA << "." << endl;
     575  }
    458576};
    459577
    … …  
    468586void Vector::LinearCombinationOfVectors(const Vector *x1, const Vector *x2, const Vector *x3, double *factors)
    469587{
    470         for(int i=NDIM;i--;)
    471                 x[i] = factors[0]*x1->x[i] + factors[1]*x2->x[i] + factors[2]*x3->x[i];
     588  for(int i=NDIM;i--;)
     589    x[i] = factors[0]*x1->x[i] + factors[1]*x2->x[i] + factors[2]*x3->x[i];
    472590};
    473591
    … …  
    477595void Vector::Mirror(const Vector *n)
    478596{
    479         double projection;
    480         projection = ScalarProduct(n)/n->ScalarProduct(n);              // remove constancy from n (keep as logical one)
    481         // withdraw projected vector twice from original one
    482         cout << Verbose(1) << "Vector: ";
    483         Output((ofstream *)&cout);
    484         cout << "\t";
    485         for (int i=NDIM;i--;)
    486                 x[i] -= 2.*projection*n->x[i];
    487         cout << "Projected vector: ";
    488         Output((ofstream *)&cout);
    489         cout << endl;
     597  double projection;
     598  projection = ScalarProduct(n)/n->ScalarProduct(n);    // remove constancy from n (keep as logical one)
     599  // withdraw projected vector twice from original one
     600  cout << Verbose(1) << "Vector: ";
     601  Output((ofstream *)&cout);
     602  cout << "\t";
     603  for (int i=NDIM;i--;)
     604    x[i] -= 2.*projection*n->x[i];
     605  cout << "Projected vector: ";
     606  Output((ofstream *)&cout);
     607  cout << endl;
    490608};
    491609
    … …  
    499617bool Vector::MakeNormalVector(const Vector *y1, const Vector *y2, const Vector *y3)
    500618{
    501         Vector x1, x2;
    502 
    503         x1.CopyVector(y1);
    504         x1.SubtractVector(y2);
    505         x2.CopyVector(y3);
    506         x2.SubtractVector(y2);
    507         if ((fabs(x1.Norm()) < MYEPSILON) || (fabs(x2.Norm()) < MYEPSILON) || (fabs(x1.Angle(&x2)) < MYEPSILON)) {
    508                 cout << Verbose(4) << "Given vectors are linear dependent." << endl;
    509                 return false;
    510         }
    511 //      cout << Verbose(4) << "relative, first plane coordinates:";
    512 //      x1.Output((ofstream *)&cout);
    513 //      cout << endl;
    514 //      cout << Verbose(4) << "second plane coordinates:";
    515 //      x2.Output((ofstream *)&cout);
    516 //      cout << endl;
    517 
    518         this->x[0] = (x1.x[1]*x2.x[2] - x1.x[2]*x2.x[1]);
    519         this->x[1] = (x1.x[2]*x2.x[0] - x1.x[0]*x2.x[2]);
    520         this->x[2] = (x1.x[0]*x2.x[1] - x1.x[1]*x2.x[0]);
    521         Normalize();
    522 
    523         return true;
     619  Vector x1, x2;
     620
     621  x1.CopyVector(y1);
     622  x1.SubtractVector(y2);
     623  x2.CopyVector(y3);
     624  x2.SubtractVector(y2);
     625  if ((fabs(x1.Norm()) < MYEPSILON) || (fabs(x2.Norm()) < MYEPSILON) || (fabs(x1.Angle(&x2)) < MYEPSILON)) {
     626    cout << Verbose(4) << "Given vectors are linear dependent." << endl;
     627    return false;
     628  }
     629//  cout << Verbose(4) << "relative, first plane coordinates:";
     630//  x1.Output((ofstream *)&cout);
     631//  cout << endl;
     632//  cout << Verbose(4) << "second plane coordinates:";
     633//  x2.Output((ofstream *)&cout);
     634//  cout << endl;
     635
     636  this->x[0] = (x1.x[1]*x2.x[2] - x1.x[2]*x2.x[1]);
     637  this->x[1] = (x1.x[2]*x2.x[0] - x1.x[0]*x2.x[2]);
     638  this->x[2] = (x1.x[0]*x2.x[1] - x1.x[1]*x2.x[0]);
     639  Normalize();
     640
     641  return true;
    524642};
    525643
    … …  
    535653bool Vector::MakeNormalVector(const Vector *y1, const Vector *y2)
    536654{
    537         Vector x1,x2;
    538         x1.CopyVector(y1);
    539         x2.CopyVector(y2);
    540         Zero();
    541         if ((fabs(x1.Norm()) < MYEPSILON) || (fabs(x2.Norm()) < MYEPSILON) || (fabs(x1.Angle(&x2)) < MYEPSILON)) {
    542                 cout << Verbose(4) << "Given vectors are linear dependent." << endl;
    543                 return false;
    544         }
    545 //      cout << Verbose(4) << "relative, first plane coordinates:";
    546 //      x1.Output((ofstream *)&cout);
    547 //      cout << endl;
    548 //      cout << Verbose(4) << "second plane coordinates:";
    549 //      x2.Output((ofstream *)&cout);
    550 //      cout << endl;
    551 
    552         this->x[0] = (x1.x[1]*x2.x[2] - x1.x[2]*x2.x[1]);
    553         this->x[1] = (x1.x[2]*x2.x[0] - x1.x[0]*x2.x[2]);
    554         this->x[2] = (x1.x[0]*x2.x[1] - x1.x[1]*x2.x[0]);
    555         Normalize();
    556 
    557         return true;
     655  Vector x1,x2;
     656  x1.CopyVector(y1);
     657  x2.CopyVector(y2);
     658  Zero();
     659  if ((fabs(x1.Norm()) < MYEPSILON) || (fabs(x2.Norm()) < MYEPSILON) || (fabs(x1.Angle(&x2)) < MYEPSILON)) {
     660    cout << Verbose(4) << "Given vectors are linear dependent." << endl;
     661    return false;
     662  }
     663//  cout << Verbose(4) << "relative, first plane coordinates:";
     664//  x1.Output((ofstream *)&cout);
     665//  cout << endl;
     666//  cout << Verbose(4) << "second plane coordinates:";
     667//  x2.Output((ofstream *)&cout);
     668//  cout << endl;
     669
     670  this->x[0] = (x1.x[1]*x2.x[2] - x1.x[2]*x2.x[1]);
     671  this->x[1] = (x1.x[2]*x2.x[0] - x1.x[0]*x2.x[2]);
     672  this->x[2] = (x1.x[0]*x2.x[1] - x1.x[1]*x2.x[0]);
     673  Normalize();
     674
     675  return true;
    558676};
    559677
    … …  
    565683bool Vector::MakeNormalVector(const Vector *y1)
    566684{
    567         bool result = false;
    568         Vector x1;
    569         x1.CopyVector(y1);
    570         x1.Scale(x1.Projection(this));
    571         SubtractVector(&x1);
    572         for (int i=NDIM;i--;)
    573                 result = result || (fabs(x[i]) > MYEPSILON);
    574 
    575         return result;
     685  bool result = false;
     686  Vector x1;
     687  x1.CopyVector(y1);
     688  x1.Scale(x1.Projection(this));
     689  SubtractVector(&x1);
     690  for (int i=NDIM;i--;)
     691    result = result || (fabs(x[i]) > MYEPSILON);
     692
     693  return result;
    576694};
    577695
    … …  
    584702bool Vector::GetOneNormalVector(const Vector *GivenVector)
    585703{
    586         int Components[NDIM]; // contains indices of non-zero components
    587         int Last = 0;    // count the number of non-zero entries in vector
    588         int j;  // loop variables
    589         double norm;
    590 
    591         cout << Verbose(4);
    592         GivenVector->Output((ofstream *)&cout);
    593         cout << endl;
    594         for (j=NDIM;j--;)
    595                 Components[j] = -1;
    596         // find two components != 0
    597         for (j=0;j<NDIM;j++)
    598                 if (fabs(GivenVector->x[j]) > MYEPSILON)
    599                         Components[Last++] = j;
    600         cout << Verbose(4) << Last << " Components != 0: (" << Components[0] << "," << Components[1] << "," << Components[2] << ")" << endl;
    601 
    602         switch(Last) {
    603                 case 3: // threecomponent system
    604                 case 2: // two component system
    605                         norm = sqrt(1./(GivenVector->x[Components[1]]*GivenVector->x[Components[1]]) + 1./(GivenVector->x[Components[0]]*GivenVector->x[Components[0]]));
    606                         x[Components[2]] = 0.;
    607                         // in skp both remaining parts shall become zero but with opposite sign and third is zero
    608                         x[Components[1]] = -1./GivenVector->x[Components[1]] / norm;
    609                         x[Components[0]] = 1./GivenVector->x[Components[0]] / norm;
    610                         return true;
    611                         break;
    612                 case 1: // one component system
    613                         // set sole non-zero component to 0, and one of the other zero component pendants to 1
    614                         x[(Components[0]+2)%NDIM] = 0.;
    615                         x[(Components[0]+1)%NDIM] = 1.;
    616                         x[Components[0]] = 0.;
    617                         return true;
    618                         break;
    619                 default:
    620                         return false;
    621         }
     704  int Components[NDIM]; // contains indices of non-zero components
     705  int Last = 0;  // count the number of non-zero entries in vector
     706  int j;  // loop variables
     707  double norm;
     708
     709  cout << Verbose(4);
     710  GivenVector->Output((ofstream *)&cout);
     711  cout << endl;
     712  for (j=NDIM;j--;)
     713    Components[j] = -1;
     714  // find two components != 0
     715  for (j=0;j<NDIM;j++)
     716    if (fabs(GivenVector->x[j]) > MYEPSILON)
     717      Components[Last++] = j;
     718  cout << Verbose(4) << Last << " Components != 0: (" << Components[0] << "," << Components[1] << "," << Components[2] << ")" << endl;
     719
     720  switch(Last) {
     721    case 3:  // threecomponent system
     722    case 2:  // two component system
     723      norm = sqrt(1./(GivenVector->x[Components[1]]*GivenVector->x[Components[1]]) + 1./(GivenVector->x[Components[0]]*GivenVector->x[Components[0]]));
     724      x[Components[2]] = 0.;
     725      // in skp both remaining parts shall become zero but with opposite sign and third is zero
     726      x[Components[1]] = -1./GivenVector->x[Components[1]] / norm;
     727      x[Components[0]] = 1./GivenVector->x[Components[0]] / norm;
     728      return true;
     729      break;
     730    case 1: // one component system
     731      // set sole non-zero component to 0, and one of the other zero component pendants to 1
     732      x[(Components[0]+2)%NDIM] = 0.;
     733      x[(Components[0]+1)%NDIM] = 1.;
     734      x[Components[0]] = 0.;
     735      return true;
     736      break;
     737    default:
     738      return false;
     739  }
    622740};
    623741
    … …  
    630748double Vector::CutsPlaneAt(Vector *A, Vector *B, Vector *C)
    631749{
    632 //      cout << Verbose(3) << "For comparison: ";
    633 //      cout << "A " << A->Projection(this) << "\t";
    634 //      cout << "B " << B->Projection(this) << "\t";
    635 //      cout << "C " << C->Projection(this) << "\t";
    636 //      cout << endl;
    637         return A->Projection(this);
     750//  cout << Verbose(3) << "For comparison: ";
     751//  cout << "A " << A->Projection(this) << "\t";
     752//  cout << "B " << B->Projection(this) << "\t";
     753//  cout << "C " << C->Projection(this) << "\t";
     754//  cout << endl;
     755  return A->Projection(this);
    638756};
    639757
    … …  
    645763bool Vector::LSQdistance(Vector **vectors, int num)
    646764{
    647         int j;
    648 
    649         for (j=0;j<num;j++) {
    650                 cout << Verbose(1) << j << "th atom's vector: ";
    651                 (vectors[j])->Output((ofstream *)&cout);
    652                 cout << endl;
    653         }
    654 
    655         int np = 3;
    656         struct LSQ_params par;
    657 
    658         const gsl_multimin_fminimizer_type *T =
    659                 gsl_multimin_fminimizer_nmsimplex;
    660         gsl_multimin_fminimizer *s = NULL;
    661         gsl_vector *ss, *y;
    662         gsl_multimin_function minex_func;
    663 
    664         size_t iter = 0, i;
    665         int status;
    666         double size;
    667 
    668         /* Initial vertex size vector */
    669         ss = gsl_vector_alloc (np);
    670         y = gsl_vector_alloc (np);
    671 
    672         /* Set all step sizes to 1 */
    673         gsl_vector_set_all (ss, 1.0);
    674 
    675         /* Starting point */
    676         par.vectors = vectors;
    677         par.num = num;
    678 
    679         for (i=NDIM;i--;)
    680                 gsl_vector_set(y, i, (vectors[0]->x[i] - vectors[1]->x[i])/2.);
    681 
    682         /* Initialize method and iterate */
    683         minex_func.f = &LSQ;
    684         minex_func.n = np;
    685         minex_func.params = (void *)&par;
    686 
    687         s = gsl_multimin_fminimizer_alloc (T, np);
    688         gsl_multimin_fminimizer_set (s, &minex_func, y, ss);
    689 
    690         do
    691                 {
    692                         iter++;
    693                         status = gsl_multimin_fminimizer_iterate(s);
    694 
    695                         if (status)
    696                                 break;
    697 
    698                         size = gsl_multimin_fminimizer_size (s);
    699                         status = gsl_multimin_test_size (size, 1e-2);
    700 
    701                         if (status == GSL_SUCCESS)
    702                                 {
    703                                         printf ("converged to minimum at\n");
    704                                 }
    705 
    706                         printf ("%5d ", (int)iter);
    707                         for (i = 0; i < (size_t)np; i++)
    708                                 {
    709                                         printf ("%10.3e ", gsl_vector_get (s->x, i));
    710                                 }
    711                         printf ("f() = %7.3f size = %.3f\n", s->fval, size);
    712                 }
    713         while (status == GSL_CONTINUE && iter < 100);
    714 
    715         for (i=(size_t)np;i--;)
    716                 this->x[i] = gsl_vector_get(s->x, i);
    717         gsl_vector_free(y);
    718         gsl_vector_free(ss);
    719         gsl_multimin_fminimizer_free (s);
    720 
    721         return true;
     765  int j;
     766
     767  for (j=0;j<num;j++) {
     768    cout << Verbose(1) << j << "th atom's vector: ";
     769    (vectors[j])->Output((ofstream *)&cout);
     770    cout << endl;
     771  }
     772
     773  int np = 3;
     774  struct LSQ_params par;
     775
     776  const gsl_multimin_fminimizer_type *T =
     777    gsl_multimin_fminimizer_nmsimplex;
     778  gsl_multimin_fminimizer *s = NULL;
     779  gsl_vector *ss, *y;
     780  gsl_multimin_function minex_func;
     781
     782  size_t iter = 0, i;
     783  int status;
     784  double size;
     785
     786  /* Initial vertex size vector */
     787  ss = gsl_vector_alloc (np);
     788  y = gsl_vector_alloc (np);
     789
     790  /* Set all step sizes to 1 */
     791  gsl_vector_set_all (ss, 1.0);
     792
     793  /* Starting point */
     794  par.vectors = vectors;
     795  par.num = num;
     796
     797  for (i=NDIM;i--;)
     798    gsl_vector_set(y, i, (vectors[0]->x[i] - vectors[1]->x[i])/2.);
     799
     800  /* Initialize method and iterate */
     801  minex_func.f = &LSQ;
     802  minex_func.n = np;
     803  minex_func.params = (void *)&par;
     804
     805  s = gsl_multimin_fminimizer_alloc (T, np);
     806  gsl_multimin_fminimizer_set (s, &minex_func, y, ss);
     807
     808  do
     809    {
     810      iter++;
     811      status = gsl_multimin_fminimizer_iterate(s);
     812
     813      if (status)
     814        break;
     815
     816      size = gsl_multimin_fminimizer_size (s);
     817      status = gsl_multimin_test_size (size, 1e-2);
     818
     819      if (status == GSL_SUCCESS)
     820        {
     821          printf ("converged to minimum at\n");
     822        }
     823
     824      printf ("%5d ", (int)iter);
     825      for (i = 0; i < (size_t)np; i++)
     826        {
     827          printf ("%10.3e ", gsl_vector_get (s->x, i));
     828        }
     829      printf ("f() = %7.3f size = %.3f\n", s->fval, size);
     830    }
     831  while (status == GSL_CONTINUE && iter < 100);
     832
     833  for (i=(size_t)np;i--;)
     834    this->x[i] = gsl_vector_get(s->x, i);
     835  gsl_vector_free(y);
     836  gsl_vector_free(ss);
     837  gsl_multimin_fminimizer_free (s);
     838
     839  return true;
    722840};
    723841
    … …  
    727845void Vector::AddVector(const Vector *y)
    728846{
    729         for (int i=NDIM;i--;)
    730                 this->x[i] += y->x[i];
     847  for (int i=NDIM;i--;)
     848    this->x[i] += y->x[i];
    731849}
    732850
    … …  
    736854void Vector::SubtractVector(const Vector *y)
    737855{
    738         for (int i=NDIM;i--;)
    739                 this->x[i] -= y->x[i];
     856  for (int i=NDIM;i--;)
     857    this->x[i] -= y->x[i];
    740858}
    741859
    … …  
    745863void Vector::CopyVector(const Vector *y)
    746864{
    747         for (int i=NDIM;i--;)
    748                 this->x[i] = y->x[i];
     865  for (int i=NDIM;i--;)
     866    this->x[i] = y->x[i];
    749867}
    750868
    … …  
    756874void Vector::AskPosition(double *cell_size, bool check)
    757875{
    758         char coords[3] = {'x','y','z'};
    759         int j = -1;
    760         for (int i=0;i<3;i++) {
    761                 j += i+1;
    762                 do {
    763                         cout << Verbose(0) << coords[i] << "[0.." << cell_size[j] << "]: ";
    764                         cin >> x[i];
    765                 } while (((x[i] < 0) || (x[i] >= cell_size[j])) && (check));
    766         }
     876  char coords[3] = {'x','y','z'};
     877  int j = -1;
     878  for (int i=0;i<3;i++) {
     879    j += i+1;
     880    do {
     881      cout << Verbose(0) << coords[i] << "[0.." << cell_size[j] << "]: ";
     882      cin >> x[i];
     883    } while (((x[i] < 0) || (x[i] >= cell_size[j])) && (check));
     884  }
    767885};
    768886
    … …  
    784902bool Vector::SolveSystem(Vector *x1, Vector *x2, Vector *y, double alpha, double beta, double c)
    785903{
    786         double D1,D2,D3,E1,E2,F1,F2,F3,p,q=0., A, B1, B2, C;
    787         double ang; // angle on testing
    788         double sign[3];
    789         int i,j,k;
    790         A = cos(alpha) * x1->Norm() * c;
    791         B1 = cos(beta + M_PI/2.) * y->Norm() * c;
    792         B2 = cos(beta) * x2->Norm() * c;
    793         C = c * c;
    794         cout << Verbose(2) << "A " << A << "\tB " << B1 << "\tC " << C << endl;
    795         int flag = 0;
    796         if (fabs(x1->x[0]) < MYEPSILON) { // check for zero components for the later flipping and back-flipping
    797                 if (fabs(x1->x[1]) > MYEPSILON) {
    798                         flag = 1;
    799                 } else if (fabs(x1->x[2]) > MYEPSILON) {
    800                         flag = 2;
    801                 } else {
    802                         return false;
    803                 }
    804         }
    805         switch (flag) {
    806                 default:
    807                 case 0:
    808                         break;
    809                 case 2:
    810                         flip(&x1->x[0],&x1->x[1]);
    811                         flip(&x2->x[0],&x2->x[1]);
    812                         flip(&y->x[0],&y->x[1]);
    813                         //flip(&x[0],&x[1]);
    814                         flip(&x1->x[1],&x1->x[2]);
    815                         flip(&x2->x[1],&x2->x[2]);
    816                         flip(&y->x[1],&y->x[2]);
    817                         //flip(&x[1],&x[2]);
    818                 case 1:
    819                         flip(&x1->x[0],&x1->x[1]);
    820                         flip(&x2->x[0],&x2->x[1]);
    821                         flip(&y->x[0],&y->x[1]);
    822                         //flip(&x[0],&x[1]);
    823                         flip(&x1->x[1],&x1->x[2]);
    824                         flip(&x2->x[1],&x2->x[2]);
    825                         flip(&y->x[1],&y->x[2]);
    826                         //flip(&x[1],&x[2]);
    827                         break;
    828         }
    829         // now comes the case system
    830         D1 = -y->x[0]/x1->x[0]*x1->x[1]+y->x[1];
    831         D2 = -y->x[0]/x1->x[0]*x1->x[2]+y->x[2];
    832         D3 = y->x[0]/x1->x[0]*A-B1;
    833         cout << Verbose(2) << "D1 " << D1 << "\tD2 " << D2 << "\tD3 " << D3 << "\n";
    834         if (fabs(D1) < MYEPSILON) {
    835                 cout << Verbose(2) << "D1 == 0!\n";
    836                 if (fabs(D2) > MYEPSILON) {
    837                         cout << Verbose(3) << "D2 != 0!\n";
    838                         x[2] = -D3/D2;
    839                         E1 = A/x1->x[0] + x1->x[2]/x1->x[0]*D3/D2;
    840                         E2 = -x1->x[1]/x1->x[0];
    841                         cout << Verbose(3) << "E1 " << E1 << "\tE2 " << E2 << "\n";
    842                         F1 = E1*E1 + 1.;
    843                         F2 = -E1*E2;
    844                         F3 = E1*E1 + D3*D3/(D2*D2) - C;
    845                         cout << Verbose(3) << "F1 " << F1 << "\tF2 " << F2 << "\tF3 " << F3 << "\n";
    846                         if (fabs(F1) < MYEPSILON) {
    847                                 cout << Verbose(4) << "F1 == 0!\n";
    848                                 cout << Verbose(4) << "Gleichungssystem linear\n";
    849                                 x[1] = F3/(2.*F2);
    850                         } else {
    851                                 p = F2/F1;
    852                                 q = p*p - F3/F1;
    853                                 cout << Verbose(4) << "p " << p << "\tq " << q << endl;
    854                                 if (q < 0) {
    855                                         cout << Verbose(4) << "q < 0" << endl;
    856                                         return false;
    857                                 }
    858                                 x[1] = p + sqrt(q);
    859                         }
    860                         x[0] =  A/x1->x[0] - x1->x[1]/x1->x[0]*x[1] + x1->x[2]/x1->x[0]*x[2];
    861                 } else {
    862                         cout << Verbose(2) << "Gleichungssystem unterbestimmt\n";
    863                         return false;
    864                 }
    865         } else {
    866                 E1 = A/x1->x[0]+x1->x[1]/x1->x[0]*D3/D1;
    867                 E2 = x1->x[1]/x1->x[0]*D2/D1 - x1->x[2];
    868                 cout << Verbose(2) << "E1 " << E1 << "\tE2 " << E2 << "\n";
    869                 F1 = E2*E2 + D2*D2/(D1*D1) + 1.;
    870                 F2 = -(E1*E2 + D2*D3/(D1*D1));
    871                 F3 = E1*E1 + D3*D3/(D1*D1) - C;
    872                 cout << Verbose(2) << "F1 " << F1 << "\tF2 " << F2 << "\tF3 " << F3 << "\n";
    873                 if (fabs(F1) < MYEPSILON) {
    874                         cout << Verbose(3) << "F1 == 0!\n";
    875                         cout << Verbose(3) << "Gleichungssystem linear\n";
    876                         x[2] = F3/(2.*F2);
    877                 } else {
    878                         p = F2/F1;
    879                         q = p*p - F3/F1;
    880                         cout << Verbose(3) << "p " << p << "\tq " << q << endl;
    881                         if (q < 0) {
    882                                 cout << Verbose(3) << "q < 0" << endl;
    883                                 return false;
    884                         }
    885                         x[2] = p + sqrt(q);
    886                 }
    887                 x[1] = (-D2 * x[2] - D3)/D1;
    888                 x[0] = A/x1->x[0] - x1->x[1]/x1->x[0]*x[1] + x1->x[2]/x1->x[0]*x[2];
    889         }
    890         switch (flag) { // back-flipping
    891                 default:
    892                 case 0:
    893                         break;
    894                 case 2:
    895                         flip(&x1->x[0],&x1->x[1]);
    896                         flip(&x2->x[0],&x2->x[1]);
    897                         flip(&y->x[0],&y->x[1]);
    898                         flip(&x[0],&x[1]);
    899                         flip(&x1->x[1],&x1->x[2]);
    900                         flip(&x2->x[1],&x2->x[2]);
    901                         flip(&y->x[1],&y->x[2]);
    902                         flip(&x[1],&x[2]);
    903                 case 1:
    904                         flip(&x1->x[0],&x1->x[1]);
    905                         flip(&x2->x[0],&x2->x[1]);
    906                         flip(&y->x[0],&y->x[1]);
    907                         //flip(&x[0],&x[1]);
    908                         flip(&x1->x[1],&x1->x[2]);
    909                         flip(&x2->x[1],&x2->x[2]);
    910                         flip(&y->x[1],&y->x[2]);
    911                         flip(&x[1],&x[2]);
    912                         break;
    913         }
    914         // one z component is only determined by its radius (without sign)
    915         // thus check eight possible sign flips and determine by checking angle with second vector
    916         for (i=0;i<8;i++) {
    917                 // set sign vector accordingly
    918                 for (j=2;j>=0;j--) {
    919                         k = (i & pot(2,j)) << j;
    920                         cout << Verbose(2) << "k " << k << "\tpot(2,j) " << pot(2,j) << endl;
    921                         sign[j] = (k == 0) ? 1. : -1.;
    922                 }
    923                 cout << Verbose(2) << i << ": sign matrix is " << sign[0] << "\t" << sign[1] << "\t" << sign[2] << "\n";
    924                 // apply sign matrix
    925                 for (j=NDIM;j--;)
    926                         x[j] *= sign[j];
    927                 // calculate angle and check
    928                 ang = x2->Angle (this);
    929                 cout << Verbose(1) << i << "th angle " << ang << "\tbeta " << cos(beta) << " :\t";
    930                 if (fabs(ang - cos(beta)) < MYEPSILON) {
    931                         break;
    932                 }
    933                 // unapply sign matrix (is its own inverse)
    934                 for (j=NDIM;j--;)
    935                         x[j] *= sign[j];
    936         }
    937         return true;
    938 };
     904  double D1,D2,D3,E1,E2,F1,F2,F3,p,q=0., A, B1, B2, C;
     905  double ang; // angle on testing
     906  double sign[3];
     907  int i,j,k;
     908  A = cos(alpha) * x1->Norm() * c;
     909  B1 = cos(beta + M_PI/2.) * y->Norm() * c;
     910  B2 = cos(beta) * x2->Norm() * c;
     911  C = c * c;
     912  cout << Verbose(2) << "A " << A << "\tB " << B1 << "\tC " << C << endl;
     913  int flag = 0;
     914  if (fabs(x1->x[0]) < MYEPSILON) { // check for zero components for the later flipping and back-flipping
     915    if (fabs(x1->x[1]) > MYEPSILON) {
     916      flag = 1;
     917    } else if (fabs(x1->x[2]) > MYEPSILON) {
     918      flag = 2;
     919    } else {
     920      return false;
     921    }
     922  }
     923  switch (flag) {
     924    default:
     925    case 0:
     926      break;
     927    case 2:
     928      flip(&x1->x[0],&x1->x[1]);
     929      flip(&x2->x[0],&x2->x[1]);
     930      flip(&y->x[0],&y->x[1]);
     931      //flip(&x[0],&x[1]);
     932      flip(&x1->x[1],&x1->x[2]);
     933      flip(&x2->x[1],&x2->x[2]);
     934      flip(&y->x[1],&y->x[2]);
     935      //flip(&x[1],&x[2]);
     936    case 1:
     937      flip(&x1->x[0],&x1->x[1]);
     938      flip(&x2->x[0],&x2->x[1]);
     939      flip(&y->x[0],&y->x[1]);
     940      //flip(&x[0],&x[1]);
     941      flip(&x1->x[1],&x1->x[2]);
     942      flip(&x2->x[1],&x2->x[2]);
     943      flip(&y->x[1],&y->x[2]);
     944      //flip(&x[1],&x[2]);
     945      break;
     946  }
     947  // now comes the case system
     948  D1 = -y->x[0]/x1->x[0]*x1->x[1]+y->x[1];
     949  D2 = -y->x[0]/x1->x[0]*x1->x[2]+y->x[2];
     950  D3 = y->x[0]/x1->x[0]*A-B1;
     951  cout << Verbose(2) << "D1 " << D1 << "\tD2 " << D2 << "\tD3 " << D3 << "\n";
     952  if (fabs(D1) < MYEPSILON) {
     953    cout << Verbose(2) << "D1 == 0!\n";
     954    if (fabs(D2) > MYEPSILON) {
     955      cout << Verbose(3) << "D2 != 0!\n";
     956      x[2] = -D3/D2;
     957      E1 = A/x1->x[0] + x1->x[2]/x1->x[0]*D3/D2;
     958      E2 = -x1->x[1]/x1->x[0];
     959      cout << Verbose(3) << "E1 " << E1 << "\tE2 " << E2 << "\n";
     960      F1 = E1*E1 + 1.;
     961      F2 = -E1*E2;
     962      F3 = E1*E1 + D3*D3/(D2*D2) - C;
     963      cout << Verbose(3) << "F1 " << F1 << "\tF2 " << F2 << "\tF3 " << F3 << "\n";
     964      if (fabs(F1) < MYEPSILON) {
     965        cout << Verbose(4) << "F1 == 0!\n";
     966        cout << Verbose(4) << "Gleichungssystem linear\n";
     967        x[1] = F3/(2.*F2);
     968      } else {
     969        p = F2/F1;
     970        q = p*p - F3/F1;
     971        cout << Verbose(4) << "p " << p << "\tq " << q << endl;
     972        if (q < 0) {
     973          cout << Verbose(4) << "q < 0" << endl;
     974          return false;
     975        }
     976        x[1] = p + sqrt(q);
     977      }
     978      x[0] =  A/x1->x[0] - x1->x[1]/x1->x[0]*x[1] + x1->x[2]/x1->x[0]*x[2];
     979    } else {
     980      cout << Verbose(2) << "Gleichungssystem unterbestimmt\n";
     981      return false;
     982    }
     983  } else {
     984    E1 = A/x1->x[0]+x1->x[1]/x1->x[0]*D3/D1;
     985    E2 = x1->x[1]/x1->x[0]*D2/D1 - x1->x[2];
     986    cout << Verbose(2) << "E1 " << E1 << "\tE2 " << E2 << "\n";
     987    F1 = E2*E2 + D2*D2/(D1*D1) + 1.;
     988    F2 = -(E1*E2 + D2*D3/(D1*D1));
     989    F3 = E1*E1 + D3*D3/(D1*D1) - C;
     990    cout << Verbose(2) << "F1 " << F1 << "\tF2 " << F2 << "\tF3 " << F3 << "\n";
     991    if (fabs(F1) < MYEPSILON) {
     992      cout << Verbose(3) << "F1 == 0!\n";
     993      cout << Verbose(3) << "Gleichungssystem linear\n";
     994      x[2] = F3/(2.*F2);
     995    } else {
     996      p = F2/F1;
     997      q = p*p - F3/F1;
     998      cout << Verbose(3) << "p " << p << "\tq " << q << endl;
     999      if (q < 0) {
     1000        cout << Verbose(3) << "q < 0" << endl;
     1001        return false;
     1002      }
     1003      x[2] = p + sqrt(q);
     1004    }
     1005    x[1] = (-D2 * x[2] - D3)/D1;
     1006    x[0] = A/x1->x[0] - x1->x[1]/x1->x[0]*x[1] + x1->x[2]/x1->x[0]*x[2];
     1007  }
     1008  switch (flag) { // back-flipping
     1009    default:
     1010    case 0:
     1011      break;
     1012    case 2:
     1013      flip(&x1->x[0],&x1->x[1]);
     1014      flip(&x2->x[0],&x2->x[1]);
     1015      flip(&y->x[0],&y->x[1]);
     1016      flip(&x[0],&x[1]);
     1017      flip(&x1->x[1],&x1->x[2]);
     1018      flip(&x2->x[1],&x2->x[2]);
     1019      flip(&y->x[1],&y->x[2]);
     1020      flip(&x[1],&x[2]);
     1021    case 1:
     1022      flip(&x1->x[0],&x1->x[1]);
     1023      flip(&x2->x[0],&x2->x[1]);
     1024      flip(&y->x[0],&y->x[1]);
     1025      //flip(&x[0],&x[1]);
     1026      flip(&x1->x[1],&x1->x[2]);
     1027      flip(&x2->x[1],&x2->x[2]);
     1028      flip(&y->x[1],&y->x[2]);
     1029      flip(&x[1],&x[2]);
     1030      break;
     1031  }
     1032  // one z component is only determined by its radius (without sign)
     1033  // thus check eight possible sign flips and determine by checking angle with second vector
     1034  for (i=0;i<8;i++) {
     1035    // set sign vector accordingly
     1036    for (j=2;j>=0;j--) {
     1037      k = (i & pot(2,j)) << j;
     1038      cout << Verbose(2) << "k " << k << "\tpot(2,j) " << pot(2,j) << endl;
     1039      sign[j] = (k == 0) ? 1. : -1.;
     1040    }
     1041    cout << Verbose(2) << i << ": sign matrix is " << sign[0] << "\t" << sign[1] << "\t" << sign[2] << "\n";
     1042    // apply sign matrix
     1043    for (j=NDIM;j--;)
     1044      x[j] *= sign[j];
     1045    // calculate angle and check
     1046    ang = x2->Angle (this);
     1047    cout << Verbose(1) << i << "th angle " << ang << "\tbeta " << cos(beta) << " :\t";
     1048    if (fabs(ang - cos(beta)) < MYEPSILON) {
     1049      break;
     1050    }
     1051    // unapply sign matrix (is its own inverse)
     1052    for (j=NDIM;j--;)
     1053      x[j] *= sign[j];
     1054  }
     1055  return true;
     1056};
Note: See TracChangeset for help on using the changeset viewer.