Ignore:
File:
1 edited

Legend:

Unmodified
Added
Removed

  • src/vector.cpp

    r9c20aa r2319ed  
    2828double Vector::DistanceSquared(const Vector *y) const
    2929{
    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);
     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);
    3434};
    3535
    … …  
    4040double Vector::Distance(const Vector *y) const
    4141{
    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));
     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));
    4646};
    4747
    … …  
    5353double Vector::PeriodicDistance(const Vector *y, const double *cell_size) const
    5454{
    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);
     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);
    8484};
    8585
    … …  
    9191double Vector::PeriodicDistanceSquared(const Vector *y, const double *cell_size) const
    9292{
    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);
     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);
    122122};
    123123
    … …  
    128128void Vector::KeepPeriodic(ofstream *out, double *matrix)
    129129{
    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;
     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;
    153153};
    154154
    … …  
    159159double Vector::ScalarProduct(const Vector *y) const
    160160{
    161         double res = 0.;
    162         for (int i=NDIM;i--;)
    163                 res += x[i]*y->x[i];
    164         return (res);
     161  double res = 0.;
     162  for (int i=NDIM;i--;)
     163    res += x[i]*y->x[i];
     164  return (res);
    165165};
    166166
    167167
    168168/** 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&
     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&
    173173 */
    174174void Vector::VectorProduct(const Vector *y)
    175175{
    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);
     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);
    181181
    182182};
    … …  
    189189void Vector::ProjectOntoPlane(const Vector *y)
    190190{
    191         Vector tmp;
    192         tmp.CopyVector(y);
    193         tmp.Normalize();
    194         tmp.Scale(ScalarProduct(&tmp));
    195         this->SubtractVector(&tmp);
     191  Vector tmp;
     192  tmp.CopyVector(y);
     193  tmp.Normalize();
     194  tmp.Scale(ScalarProduct(&tmp));
     195  this->SubtractVector(&tmp);
     196};
     197
     198/** Calculates the intersection point between a line defined by \a *LineVector and \a *LineVector2 and a plane defined by \a *Normal and \a *PlaneOffset.
     199 * According to [Bronstein] the vectorial plane equation is:
     200 *   -# \f$\stackrel{r}{\rightarrow} \cdot \stackrel{N}{\rightarrow} + D = 0\f$,
     201 * where \f$\stackrel{r}{\rightarrow}\f$ is the vector to be testet, \f$\stackrel{N}{\rightarrow}\f$ is the plane's normal vector and
     202 * \f$D = - \stackrel{a}{\rightarrow} \stackrel{N}{\rightarrow}\f$, the offset with respect to origin, if \f$\stackrel{a}{\rightarrow}\f$,
     203 * is an offset vector onto the plane. The line is parametrized by \f$\stackrel{x}{\rightarrow} + k \stackrel{t}{\rightarrow}\f$, where
     204 * \f$\stackrel{x}{\rightarrow}\f$ is the offset and \f$\stackrel{t}{\rightarrow}\f$ the directional vector (NOTE: No need to normalize
     205 * the latter). Inserting the parametrized form into the plane equation and solving for \f$k\f$, which we insert then into the parametrization
     206 * of the line yields the intersection point on the plane.
     207 * \param *out output stream for debugging
     208 * \param *PlaneNormal Plane's normal vector
     209 * \param *PlaneOffset Plane's offset vector
     210 * \param *LineVector first vector of line
     211 * \param *LineVector2 second vector of line
     212 * \return true -  \a this contains intersection point on return, false - line is parallel to plane
     213 */
     214bool Vector::GetIntersectionWithPlane(ofstream *out, Vector *PlaneNormal, Vector *PlaneOffset, Vector *LineVector, Vector *LineVector2)
     215{
     216  double factor;
     217  Vector Direction;
     218
     219  // find intersection of a line defined by Offset and Direction with a  plane defined by triangle
     220  Direction.CopyVector(LineVector2);
     221  Direction.SubtractVector(LineVector);
     222  if (Direction.ScalarProduct(PlaneNormal) < MYEPSILON)
     223    return false;
     224  factor = LineVector->ScalarProduct(PlaneNormal)*(-PlaneOffset->ScalarProduct(PlaneNormal))/(Direction.ScalarProduct(PlaneNormal));
     225  Direction.Scale(factor);
     226  CopyVector(LineVector);
     227  SubtractVector(&Direction);
     228
     229  // test whether resulting vector really is on plane
     230  Direction.CopyVector(this);
     231  Direction.SubtractVector(PlaneOffset);
     232  if (Direction.ScalarProduct(PlaneNormal) > MYEPSILON)
     233    return true;
     234  else
     235    return false;
     236};
     237
     238/** Calculates the intersection of the two lines that are both on the same plane.
     239 * Note that we do not check whether they are on the same plane.
     240 * \param *out output stream for debugging
     241 * \param *Line1a first vector of first line
     242 * \param *Line1b second vector of first line
     243 * \param *Line2a first vector of second line
     244 * \param *Line2b second vector of second line
     245 * \return true - \a this will contain the intersection on return, false - lines are parallel
     246 */
     247bool Vector::GetIntersectionOfTwoLinesOnPlane(ofstream *out, Vector *Line1a, Vector *Line1b, Vector *Line2a, Vector *Line2b)
     248{
     249  double k1,a1,h1,b1,k2,a2,h2,b2;
     250  // equation for line 1
     251  if (fabs(Line1a->x[0] - Line2a->x[0]) < MYEPSILON) {
     252    k1 = 0;
     253    h1 = 0;
     254  } else {
     255    k1 = (Line1a->x[1] - Line2a->x[1])/(Line1a->x[0] - Line2a->x[0]);
     256    h1 = (Line1a->x[2] - Line2a->x[2])/(Line1a->x[0] - Line2a->x[0]);
     257  }
     258  a1 = 0.5*((Line1a->x[1] + Line2a->x[1]) - k1*(Line1a->x[0] + Line2a->x[0]));
     259  b1 = 0.5*((Line1a->x[2] + Line2a->x[2]) - h1*(Line1a->x[0] + Line2a->x[0]));
     260
     261  // equation for line 2
     262  if (fabs(Line2a->x[0] - Line2a->x[0]) < MYEPSILON) {
     263    k1 = 0;
     264    h1 = 0;
     265  } else {
     266    k1 = (Line2a->x[1] - Line2a->x[1])/(Line2a->x[0] - Line2a->x[0]);
     267    h1 = (Line2a->x[2] - Line2a->x[2])/(Line2a->x[0] - Line2a->x[0]);
     268  }
     269  a1 = 0.5*((Line2a->x[1] + Line2a->x[1]) - k1*(Line2a->x[0] + Line2a->x[0]));
     270  b1 = 0.5*((Line2a->x[2] + Line2a->x[2]) - h1*(Line2a->x[0] + Line2a->x[0]));
     271
     272  // calculate cross point
     273  if (fabs((a1-a2)*(h1-h2) - (b1-b2)*(k1-k2)) < MYEPSILON) {
     274    x[0] = (a2-a1)/(k1-k2);
     275    x[1] = (k1*a2-k2*a1)/(k1-k2);
     276    x[2] = (h1*b2-h2*b1)/(h1-h2);
     277    *out << Verbose(4) << "Lines do intersect at " << this << "." << endl;
     278    return true;
     279  } else {
     280    *out << Verbose(4) << "Lines do not intersect." << endl;
     281    return false;
     282  }
    196283};
    197284
    … …  
    202289double Vector::Projection(const Vector *y) const
    203290{
    204         return (ScalarProduct(y));
     291  return (ScalarProduct(y));
    205292};
    206293
    … …  
    210297double Vector::Norm() const
    211298{
    212         double res = 0.;
    213         for (int i=NDIM;i--;)
    214                 res += this->x[i]*this->x[i];
    215         return (sqrt(res));
     299  double res = 0.;
     300  for (int i=NDIM;i--;)
     301    res += this->x[i]*this->x[i];
     302  return (sqrt(res));
     303};
     304
     305/** Calculates squared norm of this vector.
     306 * \return \f$|x|^2\f$
     307 */
     308double Vector::NormSquared() const
     309{
     310  return (ScalarProduct(this));
    216311};
    217312
    … …  
    220315void Vector::Normalize()
    221316{
    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);
     317  double res = 0.;
     318  for (int i=NDIM;i--;)
     319    res += this->x[i]*this->x[i];
     320  if (fabs(res) > MYEPSILON)
     321    res = 1./sqrt(res);
     322  Scale(&res);
    228323};
    229324
    … …  
    232327void Vector::Zero()
    233328{
    234         for (int i=NDIM;i--;)
    235                 this->x[i] = 0.;
     329  for (int i=NDIM;i--;)
     330    this->x[i] = 0.;
    236331};
    237332
    … …  
    240335void Vector::One(double one)
    241336{
    242         for (int i=NDIM;i--;)
    243                 this->x[i] = one;
     337  for (int i=NDIM;i--;)
     338    this->x[i] = one;
    244339};
    245340
    … …  
    248343void Vector::Init(double x1, double x2, double x3)
    249344{
    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);
     345  x[0] = x1;
     346  x[1] = x2;
     347  x[2] = x3;
    261348};
    262349
    … …  
    267354double Vector::Angle(const Vector *y) const
    268355{
    269   double angle = this->ScalarProduct(y)/Norm()/y->Norm();
     356  double norm1 = Norm(), norm2 = y->Norm();
     357  double angle = 1;
     358  if ((fabs(norm1) > MYEPSILON) && (fabs(norm2) > MYEPSILON))
     359    angle = this->ScalarProduct(y)/norm1/norm2;
    270360  // -1-MYEPSILON occured due to numerical imprecision, catch ...
    271361  //cout << Verbose(2) << "INFO: acos(-1) = " << acos(-1) << ", acos(-1+MYEPSILON) = " << acos(-1+MYEPSILON) << ", acos(-1-MYEPSILON) = " << acos(-1-MYEPSILON) << "." << endl;
    … …  
    274364  if (angle > 1)
    275365    angle = 1;
    276         return acos(angle);
     366  return acos(angle);
    277367};
    278368
    … …  
    283373void Vector::RotateVector(const Vector *axis, const double alpha)
    284374{
    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);
     375  Vector a,y;
     376  // normalise this vector with respect to axis
     377  a.CopyVector(this);
     378  a.Scale(Projection(axis));
     379  SubtractVector(&a);
     380  // construct normal vector
     381  y.MakeNormalVector(axis,this);
     382  y.Scale(Norm());
     383  // scale normal vector by sine and this vector by cosine
     384  y.Scale(sin(alpha));
     385  Scale(cos(alpha));
     386  // add scaled normal vector onto this vector
     387  AddVector(&y);
     388  // add part in axis direction
     389  AddVector(&a);
    300390};
    301391
    … …  
    307397Vector& operator+=(Vector& a, const Vector& b)
    308398{
    309         a.AddVector(&b);
    310         return a;
     399  a.AddVector(&b);
     400  return a;
    311401};
    312402/** factor each component of \a a times a double \a m.
    … …  
    317407Vector& operator*=(Vector& a, const double m)
    318408{
    319         a.Scale(m);
    320         return a;
    321 };
    322 
    323 /** Sums two vectors \a and \b component-wise.
     409  a.Scale(m);
     410  return a;
     411};
     412
     413/** Sums two vectors \a  and \b component-wise.
    324414 * \param a first vector
    325415 * \param b second vector
    … …  
    328418Vector& operator+(const Vector& a, const Vector& b)
    329419{
    330         Vector *x = new Vector;
    331         x->CopyVector(&a);
    332         x->AddVector(&b);
    333         return *x;
     420  Vector *x = new Vector;
     421  x->CopyVector(&a);
     422  x->AddVector(&b);
     423  return *x;
    334424};
    335425
    … …  
    341431Vector& operator*(const Vector& a, const double m)
    342432{
    343         Vector *x = new Vector;
    344         x->CopyVector(&a);
    345         x->Scale(m);
    346         return *x;
     433  Vector *x = new Vector;
     434  x->CopyVector(&a);
     435  x->Scale(m);
     436  return *x;
    347437};
    348438
    … …  
    353443bool Vector::Output(ofstream *out) const
    354444{
    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;
     445  if (out != NULL) {
     446    *out << "(";
     447    for (int i=0;i<NDIM;i++) {
     448      *out << x[i];
     449      if (i != 2)
     450        *out << ",";
     451    }
     452    *out << ")";
     453    return true;
     454  } else
     455    return false;
     456};
     457
     458ostream& operator<<(ostream& ost,Vector& m)
     459{
     460  ost << "(";
     461  for (int i=0;i<NDIM;i++) {
     462    ost << m.x[i];
     463    if (i != 2)
     464      ost << ",";
     465  }
     466  ost << ")";
     467  return ost;
    378468};
    379469
    … …  
    383473void Vector::Scale(double **factor)
    384474{
    385         for (int i=NDIM;i--;)
    386                 x[i] *= (*factor)[i];
     475  for (int i=NDIM;i--;)
     476    x[i] *= (*factor)[i];
    387477};
    388478
    389479void Vector::Scale(double *factor)
    390480{
    391         for (int i=NDIM;i--;)
    392                 x[i] *= *factor;
     481  for (int i=NDIM;i--;)
     482    x[i] *= *factor;
    393483};
    394484
    395485void Vector::Scale(double factor)
    396486{
    397         for (int i=NDIM;i--;)
    398                 x[i] *= factor;
     487  for (int i=NDIM;i--;)
     488    x[i] *= factor;
    399489};
    400490
    … …  
    404494void Vector::Translate(const Vector *trans)
    405495{
    406         for (int i=NDIM;i--;)
    407                 x[i] += trans->x[i];
     496  for (int i=NDIM;i--;)
     497    x[i] += trans->x[i];
    408498};
    409499
    … …  
    413503void Vector::MatrixMultiplication(double *M)
    414504{
    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.
     505  Vector C;
     506  // do the matrix multiplication
     507  C.x[0] = M[0]*x[0]+M[3]*x[1]+M[6]*x[2];
     508  C.x[1] = M[1]*x[0]+M[4]*x[1]+M[7]*x[2];
     509  C.x[2] = M[2]*x[0]+M[5]*x[1]+M[8]*x[2];
     510  // transfer the result into this
     511  for (int i=NDIM;i--;)
     512    x[i] = C.x[i];
     513};
     514
     515/** Calculate the inverse of a 3x3 matrix.
    426516 * \param *matrix NDIM_NDIM array
    427517 */
     518double * Vector::InverseMatrix(double *A)
     519{
     520  double *B = (double *) Malloc(sizeof(double)*NDIM*NDIM, "Vector::InverseMatrix: *B");
     521  double detA = RDET3(A);
     522  double detAReci;
     523
     524  for (int i=0;i<NDIM*NDIM;++i)
     525    B[i] = 0.;
     526  // calculate the inverse B
     527  if (fabs(detA) > MYEPSILON) {;  // RDET3(A) yields precisely zero if A irregular
     528    detAReci = 1./detA;
     529    B[0] =  detAReci*RDET2(A[4],A[5],A[7],A[8]);    // A_11
     530    B[1] = -detAReci*RDET2(A[1],A[2],A[7],A[8]);    // A_12
     531    B[2] =  detAReci*RDET2(A[1],A[2],A[4],A[5]);    // A_13
     532    B[3] = -detAReci*RDET2(A[3],A[5],A[6],A[8]);    // A_21
     533    B[4] =  detAReci*RDET2(A[0],A[2],A[6],A[8]);    // A_22
     534    B[5] = -detAReci*RDET2(A[0],A[2],A[3],A[5]);    // A_23
     535    B[6] =  detAReci*RDET2(A[3],A[4],A[6],A[7]);    // A_31
     536    B[7] = -detAReci*RDET2(A[0],A[1],A[6],A[7]);    // A_32
     537    B[8] =  detAReci*RDET2(A[0],A[1],A[3],A[4]);    // A_33
     538  }
     539  return B;
     540};
     541
     542/** Do a matrix multiplication with the \a *A' inverse.
     543 * \param *matrix NDIM_NDIM array
     544 */
    428545void Vector::InverseMatrixMultiplication(double *A)
    429546{
    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         }
     547  Vector C;
     548  double B[NDIM*NDIM];
     549  double detA = RDET3(A);
     550  double detAReci;
     551
     552  // calculate the inverse B
     553  if (fabs(detA) > MYEPSILON) {;  // RDET3(A) yields precisely zero if A irregular
     554    detAReci = 1./detA;
     555    B[0] =  detAReci*RDET2(A[4],A[5],A[7],A[8]);    // A_11
     556    B[1] = -detAReci*RDET2(A[1],A[2],A[7],A[8]);    // A_12
     557    B[2] =  detAReci*RDET2(A[1],A[2],A[4],A[5]);    // A_13
     558    B[3] = -detAReci*RDET2(A[3],A[5],A[6],A[8]);    // A_21
     559    B[4] =  detAReci*RDET2(A[0],A[2],A[6],A[8]);    // A_22
     560    B[5] = -detAReci*RDET2(A[0],A[2],A[3],A[5]);    // A_23
     561    B[6] =  detAReci*RDET2(A[3],A[4],A[6],A[7]);    // A_31
     562    B[7] = -detAReci*RDET2(A[0],A[1],A[6],A[7]);    // A_32
     563    B[8] =  detAReci*RDET2(A[0],A[1],A[3],A[4]);    // A_33
     564
     565    // do the matrix multiplication
     566    C.x[0] = B[0]*x[0]+B[3]*x[1]+B[6]*x[2];
     567    C.x[1] = B[1]*x[0]+B[4]*x[1]+B[7]*x[2];
     568    C.x[2] = B[2]*x[0]+B[5]*x[1]+B[8]*x[2];
     569    // transfer the result into this
     570    for (int i=NDIM;i--;)
     571      x[i] = C.x[i];
     572  } else {
     573    cerr << "ERROR: inverse of matrix does not exists: det A = " << detA << "." << endl;
     574  }
    458575};
    459576
    … …  
    468585void Vector::LinearCombinationOfVectors(const Vector *x1, const Vector *x2, const Vector *x3, double *factors)
    469586{
    470         for(int i=NDIM;i--;)
    471                 x[i] = factors[0]*x1->x[i] + factors[1]*x2->x[i] + factors[2]*x3->x[i];
     587  for(int i=NDIM;i--;)
     588    x[i] = factors[0]*x1->x[i] + factors[1]*x2->x[i] + factors[2]*x3->x[i];
    472589};
    473590
    … …  
    477594void Vector::Mirror(const Vector *n)
    478595{
    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;
     596  double projection;
     597  projection = ScalarProduct(n)/n->ScalarProduct(n);    // remove constancy from n (keep as logical one)
     598  // withdraw projected vector twice from original one
     599  cout << Verbose(1) << "Vector: ";
     600  Output((ofstream *)&cout);
     601  cout << "\t";
     602  for (int i=NDIM;i--;)
     603    x[i] -= 2.*projection*n->x[i];
     604  cout << "Projected vector: ";
     605  Output((ofstream *)&cout);
     606  cout << endl;
    490607};
    491608
    … …  
    499616bool Vector::MakeNormalVector(const Vector *y1, const Vector *y2, const Vector *y3)
    500617{
    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;
     618  Vector x1, x2;
     619
     620  x1.CopyVector(y1);
     621  x1.SubtractVector(y2);
     622  x2.CopyVector(y3);
     623  x2.SubtractVector(y2);
     624  if ((fabs(x1.Norm()) < MYEPSILON) || (fabs(x2.Norm()) < MYEPSILON) || (fabs(x1.Angle(&x2)) < MYEPSILON)) {
     625    cout << Verbose(4) << "Given vectors are linear dependent." << endl;
     626    return false;
     627  }
     628//  cout << Verbose(4) << "relative, first plane coordinates:";
     629//  x1.Output((ofstream *)&cout);
     630//  cout << endl;
     631//  cout << Verbose(4) << "second plane coordinates:";
     632//  x2.Output((ofstream *)&cout);
     633//  cout << endl;
     634
     635  this->x[0] = (x1.x[1]*x2.x[2] - x1.x[2]*x2.x[1]);
     636  this->x[1] = (x1.x[2]*x2.x[0] - x1.x[0]*x2.x[2]);
     637  this->x[2] = (x1.x[0]*x2.x[1] - x1.x[1]*x2.x[0]);
     638  Normalize();
     639
     640  return true;
    524641};
    525642
    … …  
    535652bool Vector::MakeNormalVector(const Vector *y1, const Vector *y2)
    536653{
    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;
     654  Vector x1,x2;
     655  x1.CopyVector(y1);
     656  x2.CopyVector(y2);
     657  Zero();
     658  if ((fabs(x1.Norm()) < MYEPSILON) || (fabs(x2.Norm()) < MYEPSILON) || (fabs(x1.Angle(&x2)) < MYEPSILON)) {
     659    cout << Verbose(4) << "Given vectors are linear dependent." << endl;
     660    return false;
     661  }
     662//  cout << Verbose(4) << "relative, first plane coordinates:";
     663//  x1.Output((ofstream *)&cout);
     664//  cout << endl;
     665//  cout << Verbose(4) << "second plane coordinates:";
     666//  x2.Output((ofstream *)&cout);
     667//  cout << endl;
     668
     669  this->x[0] = (x1.x[1]*x2.x[2] - x1.x[2]*x2.x[1]);
     670  this->x[1] = (x1.x[2]*x2.x[0] - x1.x[0]*x2.x[2]);
     671  this->x[2] = (x1.x[0]*x2.x[1] - x1.x[1]*x2.x[0]);
     672  Normalize();
     673
     674  return true;
    558675};
    559676
    … …  
    565682bool Vector::MakeNormalVector(const Vector *y1)
    566683{
    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;
     684  bool result = false;
     685  Vector x1;
     686  x1.CopyVector(y1);
     687  x1.Scale(x1.Projection(this));
     688  SubtractVector(&x1);
     689  for (int i=NDIM;i--;)
     690    result = result || (fabs(x[i]) > MYEPSILON);
     691
     692  return result;
    576693};
    577694
    … …  
    584701bool Vector::GetOneNormalVector(const Vector *GivenVector)
    585702{
    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         }
     703  int Components[NDIM]; // contains indices of non-zero components
     704  int Last = 0;  // count the number of non-zero entries in vector
     705  int j;  // loop variables
     706  double norm;
     707
     708  cout << Verbose(4);
     709  GivenVector->Output((ofstream *)&cout);
     710  cout << endl;
     711  for (j=NDIM;j--;)
     712    Components[j] = -1;
     713  // find two components != 0
     714  for (j=0;j<NDIM;j++)
     715    if (fabs(GivenVector->x[j]) > MYEPSILON)
     716      Components[Last++] = j;
     717  cout << Verbose(4) << Last << " Components != 0: (" << Components[0] << "," << Components[1] << "," << Components[2] << ")" << endl;
     718
     719  switch(Last) {
     720    case 3:  // threecomponent system
     721    case 2:  // two component system
     722      norm = sqrt(1./(GivenVector->x[Components[1]]*GivenVector->x[Components[1]]) + 1./(GivenVector->x[Components[0]]*GivenVector->x[Components[0]]));
     723      x[Components[2]] = 0.;
     724      // in skp both remaining parts shall become zero but with opposite sign and third is zero
     725      x[Components[1]] = -1./GivenVector->x[Components[1]] / norm;
     726      x[Components[0]] = 1./GivenVector->x[Components[0]] / norm;
     727      return true;
     728      break;
     729    case 1: // one component system
     730      // set sole non-zero component to 0, and one of the other zero component pendants to 1
     731      x[(Components[0]+2)%NDIM] = 0.;
     732      x[(Components[0]+1)%NDIM] = 1.;
     733      x[Components[0]] = 0.;
     734      return true;
     735      break;
     736    default:
     737      return false;
     738  }
    622739};
    623740
    … …  
    630747double Vector::CutsPlaneAt(Vector *A, Vector *B, Vector *C)
    631748{
    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);
     749//  cout << Verbose(3) << "For comparison: ";
     750//  cout << "A " << A->Projection(this) << "\t";
     751//  cout << "B " << B->Projection(this) << "\t";
     752//  cout << "C " << C->Projection(this) << "\t";
     753//  cout << endl;
     754  return A->Projection(this);
    638755};
    639756
    … …  
    645762bool Vector::LSQdistance(Vector **vectors, int num)
    646763{
    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;
     764  int j;
     765
     766  for (j=0;j<num;j++) {
     767    cout << Verbose(1) << j << "th atom's vector: ";
     768    (vectors[j])->Output((ofstream *)&cout);
     769    cout << endl;
     770  }
     771
     772  int np = 3;
     773  struct LSQ_params par;
     774
     775  const gsl_multimin_fminimizer_type *T =
     776    gsl_multimin_fminimizer_nmsimplex;
     777  gsl_multimin_fminimizer *s = NULL;
     778  gsl_vector *ss, *y;
     779  gsl_multimin_function minex_func;
     780
     781  size_t iter = 0, i;
     782  int status;
     783  double size;
     784
     785  /* Initial vertex size vector */
     786  ss = gsl_vector_alloc (np);
     787  y = gsl_vector_alloc (np);
     788
     789  /* Set all step sizes to 1 */
     790  gsl_vector_set_all (ss, 1.0);
     791
     792  /* Starting point */
     793  par.vectors = vectors;
     794  par.num = num;
     795
     796  for (i=NDIM;i--;)
     797    gsl_vector_set(y, i, (vectors[0]->x[i] - vectors[1]->x[i])/2.);
     798
     799  /* Initialize method and iterate */
     800  minex_func.f = &LSQ;
     801  minex_func.n = np;
     802  minex_func.params = (void *)&par;
     803
     804  s = gsl_multimin_fminimizer_alloc (T, np);
     805  gsl_multimin_fminimizer_set (s, &minex_func, y, ss);
     806
     807  do
     808    {
     809      iter++;
     810      status = gsl_multimin_fminimizer_iterate(s);
     811
     812      if (status)
     813        break;
     814
     815      size = gsl_multimin_fminimizer_size (s);
     816      status = gsl_multimin_test_size (size, 1e-2);
     817
     818      if (status == GSL_SUCCESS)
     819        {
     820          printf ("converged to minimum at\n");
     821        }
     822
     823      printf ("%5d ", (int)iter);
     824      for (i = 0; i < (size_t)np; i++)
     825        {
     826          printf ("%10.3e ", gsl_vector_get (s->x, i));
     827        }
     828      printf ("f() = %7.3f size = %.3f\n", s->fval, size);
     829    }
     830  while (status == GSL_CONTINUE && iter < 100);
     831
     832  for (i=(size_t)np;i--;)
     833    this->x[i] = gsl_vector_get(s->x, i);
     834  gsl_vector_free(y);
     835  gsl_vector_free(ss);
     836  gsl_multimin_fminimizer_free (s);
     837
     838  return true;
    722839};
    723840
    … …  
    727844void Vector::AddVector(const Vector *y)
    728845{
    729         for (int i=NDIM;i--;)
    730                 this->x[i] += y->x[i];
     846  for (int i=NDIM;i--;)
     847    this->x[i] += y->x[i];
    731848}
    732849
    … …  
    736853void Vector::SubtractVector(const Vector *y)
    737854{
    738         for (int i=NDIM;i--;)
    739                 this->x[i] -= y->x[i];
     855  for (int i=NDIM;i--;)
     856    this->x[i] -= y->x[i];
    740857}
    741858
    … …  
    745862void Vector::CopyVector(const Vector *y)
    746863{
    747         for (int i=NDIM;i--;)
    748                 this->x[i] = y->x[i];
     864  for (int i=NDIM;i--;)
     865    this->x[i] = y->x[i];
    749866}
    750867
    … …  
    756873void Vector::AskPosition(double *cell_size, bool check)
    757874{
    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         }
     875  char coords[3] = {'x','y','z'};
     876  int j = -1;
     877  for (int i=0;i<3;i++) {
     878    j += i+1;
     879    do {
     880      cout << Verbose(0) << coords[i] << "[0.." << cell_size[j] << "]: ";
     881      cin >> x[i];
     882    } while (((x[i] < 0) || (x[i] >= cell_size[j])) && (check));
     883  }
    767884};
    768885
    … …  
    784901bool Vector::SolveSystem(Vector *x1, Vector *x2, Vector *y, double alpha, double beta, double c)
    785902{
    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 };
     903  double D1,D2,D3,E1,E2,F1,F2,F3,p,q=0., A, B1, B2, C;
     904  double ang; // angle on testing
     905  double sign[3];
     906  int i,j,k;
     907  A = cos(alpha) * x1->Norm() * c;
     908  B1 = cos(beta + M_PI/2.) * y->Norm() * c;
     909  B2 = cos(beta) * x2->Norm() * c;
     910  C = c * c;
     911  cout << Verbose(2) << "A " << A << "\tB " << B1 << "\tC " << C << endl;
     912  int flag = 0;
     913  if (fabs(x1->x[0]) < MYEPSILON) { // check for zero components for the later flipping and back-flipping
     914    if (fabs(x1->x[1]) > MYEPSILON) {
     915      flag = 1;
     916    } else if (fabs(x1->x[2]) > MYEPSILON) {
     917      flag = 2;
     918    } else {
     919      return false;
     920    }
     921  }
     922  switch (flag) {
     923    default:
     924    case 0:
     925      break;
     926    case 2:
     927      flip(&x1->x[0],&x1->x[1]);
     928      flip(&x2->x[0],&x2->x[1]);
     929      flip(&y->x[0],&y->x[1]);
     930      //flip(&x[0],&x[1]);
     931      flip(&x1->x[1],&x1->x[2]);
     932      flip(&x2->x[1],&x2->x[2]);
     933      flip(&y->x[1],&y->x[2]);
     934      //flip(&x[1],&x[2]);
     935    case 1:
     936      flip(&x1->x[0],&x1->x[1]);
     937      flip(&x2->x[0],&x2->x[1]);
     938      flip(&y->x[0],&y->x[1]);
     939      //flip(&x[0],&x[1]);
     940      flip(&x1->x[1],&x1->x[2]);
     941      flip(&x2->x[1],&x2->x[2]);
     942      flip(&y->x[1],&y->x[2]);
     943      //flip(&x[1],&x[2]);
     944      break;
     945  }
     946  // now comes the case system
     947  D1 = -y->x[0]/x1->x[0]*x1->x[1]+y->x[1];
     948  D2 = -y->x[0]/x1->x[0]*x1->x[2]+y->x[2];
     949  D3 = y->x[0]/x1->x[0]*A-B1;
     950  cout << Verbose(2) << "D1 " << D1 << "\tD2 " << D2 << "\tD3 " << D3 << "\n";
     951  if (fabs(D1) < MYEPSILON) {
     952    cout << Verbose(2) << "D1 == 0!\n";
     953    if (fabs(D2) > MYEPSILON) {
     954      cout << Verbose(3) << "D2 != 0!\n";
     955      x[2] = -D3/D2;
     956      E1 = A/x1->x[0] + x1->x[2]/x1->x[0]*D3/D2;
     957      E2 = -x1->x[1]/x1->x[0];
     958      cout << Verbose(3) << "E1 " << E1 << "\tE2 " << E2 << "\n";
     959      F1 = E1*E1 + 1.;
     960      F2 = -E1*E2;
     961      F3 = E1*E1 + D3*D3/(D2*D2) - C;
     962      cout << Verbose(3) << "F1 " << F1 << "\tF2 " << F2 << "\tF3 " << F3 << "\n";
     963      if (fabs(F1) < MYEPSILON) {
     964        cout << Verbose(4) << "F1 == 0!\n";
     965        cout << Verbose(4) << "Gleichungssystem linear\n";
     966        x[1] = F3/(2.*F2);
     967      } else {
     968        p = F2/F1;
     969        q = p*p - F3/F1;
     970        cout << Verbose(4) << "p " << p << "\tq " << q << endl;
     971        if (q < 0) {
     972          cout << Verbose(4) << "q < 0" << endl;
     973          return false;
     974        }
     975        x[1] = p + sqrt(q);
     976      }
     977      x[0] =  A/x1->x[0] - x1->x[1]/x1->x[0]*x[1] + x1->x[2]/x1->x[0]*x[2];
     978    } else {
     979      cout << Verbose(2) << "Gleichungssystem unterbestimmt\n";
     980      return false;
     981    }
     982  } else {
     983    E1 = A/x1->x[0]+x1->x[1]/x1->x[0]*D3/D1;
     984    E2 = x1->x[1]/x1->x[0]*D2/D1 - x1->x[2];
     985    cout << Verbose(2) << "E1 " << E1 << "\tE2 " << E2 << "\n";
     986    F1 = E2*E2 + D2*D2/(D1*D1) + 1.;
     987    F2 = -(E1*E2 + D2*D3/(D1*D1));
     988    F3 = E1*E1 + D3*D3/(D1*D1) - C;
     989    cout << Verbose(2) << "F1 " << F1 << "\tF2 " << F2 << "\tF3 " << F3 << "\n";
     990    if (fabs(F1) < MYEPSILON) {
     991      cout << Verbose(3) << "F1 == 0!\n";
     992      cout << Verbose(3) << "Gleichungssystem linear\n";
     993      x[2] = F3/(2.*F2);
     994    } else {
     995      p = F2/F1;
     996      q = p*p - F3/F1;
     997      cout << Verbose(3) << "p " << p << "\tq " << q << endl;
     998      if (q < 0) {
     999        cout << Verbose(3) << "q < 0" << endl;
     1000        return false;
     1001      }
     1002      x[2] = p + sqrt(q);
     1003    }
     1004    x[1] = (-D2 * x[2] - D3)/D1;
     1005    x[0] = A/x1->x[0] - x1->x[1]/x1->x[0]*x[1] + x1->x[2]/x1->x[0]*x[2];
     1006  }
     1007  switch (flag) { // back-flipping
     1008    default:
     1009    case 0:
     1010      break;
     1011    case 2:
     1012      flip(&x1->x[0],&x1->x[1]);
     1013      flip(&x2->x[0],&x2->x[1]);
     1014      flip(&y->x[0],&y->x[1]);
     1015      flip(&x[0],&x[1]);
     1016      flip(&x1->x[1],&x1->x[2]);
     1017      flip(&x2->x[1],&x2->x[2]);
     1018      flip(&y->x[1],&y->x[2]);
     1019      flip(&x[1],&x[2]);
     1020    case 1:
     1021      flip(&x1->x[0],&x1->x[1]);
     1022      flip(&x2->x[0],&x2->x[1]);
     1023      flip(&y->x[0],&y->x[1]);
     1024      //flip(&x[0],&x[1]);
     1025      flip(&x1->x[1],&x1->x[2]);
     1026      flip(&x2->x[1],&x2->x[2]);
     1027      flip(&y->x[1],&y->x[2]);
     1028      flip(&x[1],&x[2]);
     1029      break;
     1030  }
     1031  // one z component is only determined by its radius (without sign)
     1032  // thus check eight possible sign flips and determine by checking angle with second vector
     1033  for (i=0;i<8;i++) {
     1034    // set sign vector accordingly
     1035    for (j=2;j>=0;j--) {
     1036      k = (i & pot(2,j)) << j;
     1037      cout << Verbose(2) << "k " << k << "\tpot(2,j) " << pot(2,j) << endl;
     1038      sign[j] = (k == 0) ? 1. : -1.;
     1039    }
     1040    cout << Verbose(2) << i << ": sign matrix is " << sign[0] << "\t" << sign[1] << "\t" << sign[2] << "\n";
     1041    // apply sign matrix
     1042    for (j=NDIM;j--;)
     1043      x[j] *= sign[j];
     1044    // calculate angle and check
     1045    ang = x2->Angle (this);
     1046    cout << Verbose(1) << i << "th angle " << ang << "\tbeta " << cos(beta) << " :\t";
     1047    if (fabs(ang - cos(beta)) < MYEPSILON) {
     1048      break;
     1049    }
     1050    // unapply sign matrix (is its own inverse)
     1051    for (j=NDIM;j--;)
     1052      x[j] *= sign[j];
     1053  }
     1054  return true;
     1055};
Note: See TracChangeset for help on using the changeset viewer.