Changeset e7ea64 for molecuilder/src/vector.cpp
- Timestamp:
- Apr 15, 2010, 10:54:26 AM (16 years ago)
- Children:
- 32842d8
- Parents:
- 1f591b
- File:
-
- 1 edited
-
molecuilder/src/vector.cpp (modified) (35 diffs)
Legend:
- Unmodified
- Added
- Removed
-
molecuilder/src/vector.cpp
r1f591b re7ea64 6 6 7 7 8 #include "defs.hpp" 9 #include "gslmatrix.hpp" 10 #include "leastsquaremin.hpp" 11 #include "memoryallocator.hpp" 12 #include "vector.hpp" 13 #include "Helpers/fast_functions.hpp" 8 #include "SingleVector.hpp" 14 9 #include "Helpers/Assert.hpp" 15 #include "Plane.hpp" 16 #include "Exceptions/LinearDependenceException.hpp" 17 18 #include <gsl/gsl_linalg.h> 19 #include <gsl/gsl_matrix.h> 20 #include <gsl/gsl_permutation.h> 21 #include <gsl/gsl_vector.h> 10 11 #include <iostream> 12 13 using namespace std; 14 22 15 23 16 /************************************ Functions for class vector ************************************/ … … 25 18 /** Constructor of class vector. 26 19 */ 27 Vector::Vector() 28 { 29 x[0] = x[1] = x[2] = 0.; 30 }; 20 Vector::Vector() : 21 rep(new SingleVector()) 22 {}; 23 24 Vector::Vector(Baseconstructor) // used by derived objects to construct their bases 25 {} 26 27 Vector::Vector(Baseconstructor,const Vector* v) : 28 rep(v->clone()) 29 {} 30 31 Vector Vector::VecFromRep(const Vector* v){ 32 return Vector(Baseconstructor(),v); 33 } 31 34 32 35 /** Constructor of class vector. 33 36 */ 34 Vector::Vector(const double x1, const double x2, const double x3) 35 { 36 x[0] = x1; 37 x[1] = x2; 38 x[2] = x3; 39 }; 37 Vector::Vector(const double x1, const double x2, const double x3) : 38 rep(new SingleVector(x1,x2,x3)) 39 {}; 40 40 41 41 /** 42 42 * Copy constructor 43 43 */ 44 Vector::Vector(const Vector& src) 45 { 46 x[0] = src[0]; 47 x[1] = src[1]; 48 x[2] = src[2]; 49 } 44 Vector::Vector(const Vector& src) : 45 rep(src.rep->clone()) 46 {} 50 47 51 48 /** … … 53 50 */ 54 51 Vector& Vector::operator=(const Vector& src){ 52 ASSERT(isBaseClass(),"Operator used on Derived Vector object"); 55 53 // check for self assignment 56 54 if(&src!=this){ 57 x[0] = src[0]; 58 x[1] = src[1]; 59 x[2] = src[2]; 55 rep.reset(src.rep->clone()); 60 56 } 61 57 return *this; … … 72 68 double Vector::DistanceSquared(const Vector &y) const 73 69 { 74 double res = 0.; 75 for (int i=NDIM;i--;) 76 res += (x[i]-y[i])*(x[i]-y[i]); 77 return (res); 70 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 71 return rep->DistanceSquared(y); 78 72 }; 79 73 … … 94 88 double Vector::PeriodicDistance(const Vector &y, const double * const cell_size) const 95 89 { 96 double res = Distance(y), tmp, matrix[NDIM*NDIM]; 97 Vector Shiftedy, TranslationVector; 98 int N[NDIM]; 99 matrix[0] = cell_size[0]; 100 matrix[1] = cell_size[1]; 101 matrix[2] = cell_size[3]; 102 matrix[3] = cell_size[1]; 103 matrix[4] = cell_size[2]; 104 matrix[5] = cell_size[4]; 105 matrix[6] = cell_size[3]; 106 matrix[7] = cell_size[4]; 107 matrix[8] = cell_size[5]; 108 // in order to check the periodic distance, translate one of the vectors into each of the 27 neighbouring cells 109 for (N[0]=-1;N[0]<=1;N[0]++) 110 for (N[1]=-1;N[1]<=1;N[1]++) 111 for (N[2]=-1;N[2]<=1;N[2]++) { 112 // create the translation vector 113 TranslationVector.Zero(); 114 for (int i=NDIM;i--;) 115 TranslationVector.x[i] = (double)N[i]; 116 TranslationVector.MatrixMultiplication(matrix); 117 // add onto the original vector to compare with 118 Shiftedy = y + TranslationVector; 119 // get distance and compare with minimum so far 120 tmp = Distance(Shiftedy); 121 if (tmp < res) res = tmp; 122 } 123 return (res); 90 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 91 return rep->PeriodicDistance(y,cell_size); 124 92 }; 125 93 … … 131 99 double Vector::PeriodicDistanceSquared(const Vector &y, const double * const cell_size) const 132 100 { 133 double res = DistanceSquared(y), tmp, matrix[NDIM*NDIM]; 134 Vector Shiftedy, TranslationVector; 135 int N[NDIM]; 136 matrix[0] = cell_size[0]; 137 matrix[1] = cell_size[1]; 138 matrix[2] = cell_size[3]; 139 matrix[3] = cell_size[1]; 140 matrix[4] = cell_size[2]; 141 matrix[5] = cell_size[4]; 142 matrix[6] = cell_size[3]; 143 matrix[7] = cell_size[4]; 144 matrix[8] = cell_size[5]; 145 // in order to check the periodic distance, translate one of the vectors into each of the 27 neighbouring cells 146 for (N[0]=-1;N[0]<=1;N[0]++) 147 for (N[1]=-1;N[1]<=1;N[1]++) 148 for (N[2]=-1;N[2]<=1;N[2]++) { 149 // create the translation vector 150 TranslationVector.Zero(); 151 for (int i=NDIM;i--;) 152 TranslationVector.x[i] = (double)N[i]; 153 TranslationVector.MatrixMultiplication(matrix); 154 // add onto the original vector to compare with 155 Shiftedy = y + TranslationVector; 156 // get distance and compare with minimum so far 157 tmp = DistanceSquared(Shiftedy); 158 if (tmp < res) res = tmp; 159 } 160 return (res); 101 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 102 return rep->PeriodicDistanceSquared(y,cell_size); 161 103 }; 162 104 … … 167 109 void Vector::KeepPeriodic(const double * const matrix) 168 110 { 169 // int N[NDIM]; 170 // bool flag = false; 171 //vector Shifted, TranslationVector; 172 Vector TestVector; 173 // Log() << Verbose(1) << "Begin of KeepPeriodic." << endl; 174 // Log() << Verbose(2) << "Vector is: "; 175 // Output(out); 176 // Log() << Verbose(0) << endl; 177 TestVector = (*this); 178 TestVector.InverseMatrixMultiplication(matrix); 179 for(int i=NDIM;i--;) { // correct periodically 180 if (TestVector.x[i] < 0) { // get every coefficient into the interval [0,1) 181 TestVector.x[i] += ceil(TestVector.x[i]); 182 } else { 183 TestVector.x[i] -= floor(TestVector.x[i]); 184 } 185 } 186 TestVector.MatrixMultiplication(matrix); 187 (*this) = TestVector; 188 // Log() << Verbose(2) << "New corrected vector is: "; 189 // Output(out); 190 // Log() << Verbose(0) << endl; 191 // Log() << Verbose(1) << "End of KeepPeriodic." << endl; 111 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 112 rep->KeepPeriodic(matrix); 192 113 }; 193 114 … … 198 119 double Vector::ScalarProduct(const Vector &y) const 199 120 { 200 double res = 0.; 201 for (int i=NDIM;i--;) 202 res += x[i]*y[i]; 203 return (res); 121 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 122 return rep->ScalarProduct(y); 204 123 }; 205 124 … … 213 132 void Vector::VectorProduct(const Vector &y) 214 133 { 215 Vector tmp; 216 tmp[0] = x[1]* (y[2]) - x[2]* (y[1]); 217 tmp[1] = x[2]* (y[0]) - x[0]* (y[2]); 218 tmp[2] = x[0]* (y[1]) - x[1]* (y[0]); 219 (*this) = tmp; 134 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 135 rep->VectorProduct(y); 220 136 }; 221 137 … … 227 143 void Vector::ProjectOntoPlane(const Vector &y) 228 144 { 229 Vector tmp = y; 230 tmp.Normalize(); 231 tmp *= ScalarProduct(tmp); 232 (*this) -= tmp; 145 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 146 rep->ProjectOntoPlane(y); 233 147 }; 234 148 … … 241 155 double Vector::DistanceToPlane(const Vector &PlaneNormal, const Vector &PlaneOffset) const 242 156 { 243 // first create part that is orthonormal to PlaneNormal with withdraw 244 Vector temp = (*this) - PlaneOffset; 245 temp.MakeNormalTo(PlaneNormal); 246 temp *= -1.; 247 // then add connecting vector from plane to point 248 temp += (*this); 249 temp -= PlaneOffset; 250 double sign = temp.ScalarProduct(PlaneNormal); 251 if (fabs(sign) > MYEPSILON) 252 sign /= fabs(sign); 253 else 254 sign = 0.; 255 256 return (temp.Norm()*sign); 157 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 158 return rep->DistanceToPlane(PlaneNormal,PlaneOffset); 257 159 }; 258 160 … … 262 164 void Vector::ProjectIt(const Vector &y) 263 165 { 264 Vector helper = y; 265 helper.Scale(-(ScalarProduct(y))); 266 AddVector(helper); 166 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 167 rep->ProjectIt(y); 267 168 }; 268 169 … … 273 174 Vector Vector::Projection(const Vector &y) const 274 175 { 275 Vector helper = y; 276 helper.Scale((ScalarProduct(y)/y.NormSquared())); 277 278 return helper; 176 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 177 return rep->Projection(y); 279 178 }; 280 179 … … 299 198 void Vector::Normalize() 300 199 { 301 double res = 0.; 302 for (int i=NDIM;i--;) 303 res += this->x[i]*this->x[i]; 304 if (fabs(res) > MYEPSILON) 305 res = 1./sqrt(res); 306 Scale(&res); 200 double factor = Norm(); 201 (*this) *= 1/factor; 307 202 }; 308 203 … … 311 206 void Vector::Zero() 312 207 { 313 for (int i=NDIM;i--;) 314 this->x[i] = 0.; 208 rep.reset(new SingleVector()); 315 209 }; 316 210 … … 319 213 void Vector::One(const double one) 320 214 { 321 for (int i=NDIM;i--;) 322 this->x[i] = one; 323 }; 324 325 /** Initialises all components of this vector. 326 */ 327 void Vector::Init(const double x1, const double x2, const double x3) 328 { 329 x[0] = x1; 330 x[1] = x2; 331 x[2] = x3; 215 rep.reset(new SingleVector(one,one,one)); 332 216 }; 333 217 … … 337 221 bool Vector::IsZero() const 338 222 { 339 return (fabs(x[0])+fabs(x[1])+fabs(x[2]) < MYEPSILON); 223 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 224 return rep->IsZero(); 340 225 }; 341 226 … … 345 230 bool Vector::IsOne() const 346 231 { 347 return (fabs(Norm() - 1.) < MYEPSILON); 232 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 233 return rep->IsOne(); 348 234 }; 349 235 … … 353 239 bool Vector::IsNormalTo(const Vector &normal) const 354 240 { 355 if (ScalarProduct(normal) < MYEPSILON) 356 return true; 357 else 358 return false; 241 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 242 return rep->IsNormalTo(normal); 359 243 }; 360 244 … … 364 248 bool Vector::IsEqualTo(const Vector &a) const 365 249 { 366 bool status = true; 367 for (int i=0;i<NDIM;i++) { 368 if (fabs(x[i] - a[i]) > MYEPSILON) 369 status = false; 370 } 371 return status; 250 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 251 return rep->IsEqualTo(a); 372 252 }; 373 253 … … 378 258 double Vector::Angle(const Vector &y) const 379 259 { 380 double norm1 = Norm(), norm2 = y.Norm(); 381 double angle = -1; 382 if ((fabs(norm1) > MYEPSILON) && (fabs(norm2) > MYEPSILON)) 383 angle = this->ScalarProduct(y)/norm1/norm2; 384 // -1-MYEPSILON occured due to numerical imprecision, catch ... 385 //Log() << Verbose(2) << "INFO: acos(-1) = " << acos(-1) << ", acos(-1+MYEPSILON) = " << acos(-1+MYEPSILON) << ", acos(-1-MYEPSILON) = " << acos(-1-MYEPSILON) << "." << endl; 386 if (angle < -1) 387 angle = -1; 388 if (angle > 1) 389 angle = 1; 390 return acos(angle); 260 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 261 return rep->Angle(y); 391 262 }; 392 263 393 264 394 265 double& Vector::operator[](size_t i){ 395 ASSERT( i<=NDIM && i>=0,"Vector Index out of Range");396 return x[i];266 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 267 return (*rep)[i]; 397 268 } 398 269 399 270 const double& Vector::operator[](size_t i) const{ 400 ASSERT( i<=NDIM && i>=0,"Vector Index out of Range");401 return x[i];271 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 272 return (*rep)[i]; 402 273 } 403 274 … … 411 282 412 283 double* Vector::get(){ 413 return x; 284 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 285 return rep->get(); 414 286 } 415 287 … … 421 293 bool Vector::operator==(const Vector& b) const 422 294 { 423 bool status = true; 424 for (int i=0;i<NDIM;i++) 425 status = status && (fabs((*this)[i] - b[i]) < MYEPSILON); 426 return status; 295 ASSERT(isBaseClass(),"Operator used on Derived Vector object"); 296 return IsEqualTo(b); 427 297 }; 428 298 … … 467 337 Vector const Vector::operator+(const Vector& b) const 468 338 { 339 ASSERT(isBaseClass(),"Operator used on Derived Vector object"); 469 340 Vector x = *this; 470 341 x.AddVector(b); … … 479 350 Vector const Vector::operator-(const Vector& b) const 480 351 { 352 ASSERT(isBaseClass(),"Operator used on Derived Vector object"); 481 353 Vector x = *this; 482 354 x.SubtractVector(b); … … 520 392 }; 521 393 522 /** Scales each atom coordinate by an individual \a factor. 523 * \param *factor pointer to scaling factor 524 */ 525 void Vector::Scale(const double ** const factor) 526 { 527 for (int i=NDIM;i--;) 528 x[i] *= (*factor)[i]; 529 }; 530 531 void Vector::Scale(const double * const factor) 532 { 533 for (int i=NDIM;i--;) 534 x[i] *= *factor; 535 }; 394 395 void Vector::ScaleAll(const double *factor) 396 { 397 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 398 rep->ScaleAll(factor); 399 }; 400 401 536 402 537 403 void Vector::Scale(const double factor) 538 404 { 539 for (int i=NDIM;i--;) 540 x[i] *= factor; 541 }; 542 543 /** Translate atom by given vector. 544 * \param trans[] translation vector. 545 */ 546 void Vector::Translate(const Vector &trans) 547 { 548 for (int i=NDIM;i--;) 549 x[i] += trans[i]; 405 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 406 rep->Scale(factor); 550 407 }; 551 408 … … 556 413 void Vector::WrapPeriodically(const double * const M, const double * const Minv) 557 414 { 558 MatrixMultiplication(Minv); 559 // truncate to [0,1] for each axis 560 for (int i=0;i<NDIM;i++) { 561 x[i] += 0.5; // set to center of box 562 while (x[i] >= 1.) 563 x[i] -= 1.; 564 while (x[i] < 0.) 565 x[i] += 1.; 566 } 567 MatrixMultiplication(M); 415 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 416 rep->WrapPeriodically(M,Minv); 568 417 }; 569 418 … … 573 422 void Vector::MatrixMultiplication(const double * const M) 574 423 { 575 Vector C; 576 // do the matrix multiplication 577 C.x[0] = M[0]*x[0]+M[3]*x[1]+M[6]*x[2]; 578 C.x[1] = M[1]*x[0]+M[4]*x[1]+M[7]*x[2]; 579 C.x[2] = M[2]*x[0]+M[5]*x[1]+M[8]*x[2]; 580 // transfer the result into this 581 for (int i=NDIM;i--;) 582 x[i] = C.x[i]; 424 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 425 rep->MatrixMultiplication(M); 583 426 }; 584 427 … … 588 431 bool Vector::InverseMatrixMultiplication(const double * const A) 589 432 { 590 Vector C; 591 double B[NDIM*NDIM]; 592 double detA = RDET3(A); 593 double detAReci; 594 595 // calculate the inverse B 596 if (fabs(detA) > MYEPSILON) {; // RDET3(A) yields precisely zero if A irregular 597 detAReci = 1./detA; 598 B[0] = detAReci*RDET2(A[4],A[5],A[7],A[8]); // A_11 599 B[1] = -detAReci*RDET2(A[1],A[2],A[7],A[8]); // A_12 600 B[2] = detAReci*RDET2(A[1],A[2],A[4],A[5]); // A_13 601 B[3] = -detAReci*RDET2(A[3],A[5],A[6],A[8]); // A_21 602 B[4] = detAReci*RDET2(A[0],A[2],A[6],A[8]); // A_22 603 B[5] = -detAReci*RDET2(A[0],A[2],A[3],A[5]); // A_23 604 B[6] = detAReci*RDET2(A[3],A[4],A[6],A[7]); // A_31 605 B[7] = -detAReci*RDET2(A[0],A[1],A[6],A[7]); // A_32 606 B[8] = detAReci*RDET2(A[0],A[1],A[3],A[4]); // A_33 607 608 // do the matrix multiplication 609 C.x[0] = B[0]*x[0]+B[3]*x[1]+B[6]*x[2]; 610 C.x[1] = B[1]*x[0]+B[4]*x[1]+B[7]*x[2]; 611 C.x[2] = B[2]*x[0]+B[5]*x[1]+B[8]*x[2]; 612 // transfer the result into this 613 for (int i=NDIM;i--;) 614 x[i] = C.x[i]; 615 return true; 616 } else { 617 return false; 618 } 433 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 434 return rep->InverseMatrixMultiplication(A); 619 435 }; 620 436 … … 639 455 void Vector::Mirror(const Vector &n) 640 456 { 641 double projection; 642 projection = ScalarProduct(n)/n.NormSquared(); // remove constancy from n (keep as logical one) 643 // withdraw projected vector twice from original one 644 for (int i=NDIM;i--;) 645 x[i] -= 2.*projection*n[i]; 457 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 458 rep->Mirror(n); 646 459 }; 647 460 … … 655 468 bool Vector::MakeNormalTo(const Vector &y1) 656 469 { 657 bool result = false; 658 double factor = y1.ScalarProduct(*this)/y1.NormSquared(); 659 Vector x1 = factor * y1 ; 660 SubtractVector(x1); 661 for (int i=NDIM;i--;) 662 result = result || (fabs(x[i]) > MYEPSILON); 663 664 return result; 470 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 471 return rep->MakeNormalTo(y1); 665 472 }; 666 473 … … 673 480 bool Vector::GetOneNormalVector(const Vector &GivenVector) 674 481 { 675 int Components[NDIM]; // contains indices of non-zero components 676 int Last = 0; // count the number of non-zero entries in vector 677 int j; // loop variables 678 double norm; 679 680 for (j=NDIM;j--;) 681 Components[j] = -1; 682 // find two components != 0 683 for (j=0;j<NDIM;j++) 684 if (fabs(GivenVector[j]) > MYEPSILON) 685 Components[Last++] = j; 686 687 switch(Last) { 688 case 3: // threecomponent system 689 case 2: // two component system 690 norm = sqrt(1./(GivenVector[Components[1]]*GivenVector[Components[1]]) + 1./(GivenVector[Components[0]]*GivenVector[Components[0]])); 691 x[Components[2]] = 0.; 692 // in skp both remaining parts shall become zero but with opposite sign and third is zero 693 x[Components[1]] = -1./GivenVector[Components[1]] / norm; 694 x[Components[0]] = 1./GivenVector[Components[0]] / norm; 695 return true; 696 break; 697 case 1: // one component system 698 // set sole non-zero component to 0, and one of the other zero component pendants to 1 699 x[(Components[0]+2)%NDIM] = 0.; 700 x[(Components[0]+1)%NDIM] = 1.; 701 x[Components[0]] = 0.; 702 return true; 703 break; 704 default: 705 return false; 706 } 482 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 483 return rep->GetOneNormalVector(GivenVector); 707 484 }; 708 485 … … 712 489 void Vector::AddVector(const Vector &y) 713 490 { 714 for (int i=NDIM;i--;)715 this->x[i] += y[i];491 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 492 rep->AddVector(y); 716 493 } 717 494 … … 721 498 void Vector::SubtractVector(const Vector &y) 722 499 { 723 for (int i=NDIM;i--;) 724 this->x[i] -= y[i]; 725 } 726 727 /** Copy vector \a y componentwise. 728 * \param y vector 729 */ 730 void Vector::CopyVector(const Vector &y) 731 { 732 // check for self assignment 733 if(&y!=this) { 734 for (int i=NDIM;i--;) 735 this->x[i] = y.x[i]; 736 } 737 } 738 739 // this function is completely unused so it is deactivated until new uses arrive and a new 740 // place can be found 741 #if 0 742 /** Solves a vectorial system consisting of two orthogonal statements and a norm statement. 743 * This is linear system of equations to be solved, however of the three given (skp of this vector\ 744 * with either of the three hast to be zero) only two are linear independent. The third equation 745 * is that the vector should be of magnitude 1 (orthonormal). This all leads to a case-based solution 746 * where very often it has to be checked whether a certain value is zero or not and thus forked into 747 * another case. 748 * \param *x1 first vector 749 * \param *x2 second vector 750 * \param *y third vector 751 * \param alpha first angle 752 * \param beta second angle 753 * \param c norm of final vector 754 * \return a vector with \f$\langle x1,x2 \rangle=A\f$, \f$\langle x1,y \rangle = B\f$ and with norm \a c. 755 * \bug this is not yet working properly 756 */ 757 bool Vector::SolveSystem(Vector * x1, Vector * x2, Vector * y, const double alpha, const double beta, const double c) 758 { 759 double D1,D2,D3,E1,E2,F1,F2,F3,p,q=0., A, B1, B2, C; 760 double ang; // angle on testing 761 double sign[3]; 762 int i,j,k; 763 A = cos(alpha) * x1->Norm() * c; 764 B1 = cos(beta + M_PI/2.) * y->Norm() * c; 765 B2 = cos(beta) * x2->Norm() * c; 766 C = c * c; 767 Log() << Verbose(2) << "A " << A << "\tB " << B1 << "\tC " << C << endl; 768 int flag = 0; 769 if (fabs(x1->x[0]) < MYEPSILON) { // check for zero components for the later flipping and back-flipping 770 if (fabs(x1->x[1]) > MYEPSILON) { 771 flag = 1; 772 } else if (fabs(x1->x[2]) > MYEPSILON) { 773 flag = 2; 774 } else { 775 return false; 776 } 777 } 778 switch (flag) { 779 default: 780 case 0: 781 break; 782 case 2: 783 flip(x1->x[0],x1->x[1]); 784 flip(x2->x[0],x2->x[1]); 785 flip(y->x[0],y->x[1]); 786 //flip(x[0],x[1]); 787 flip(x1->x[1],x1->x[2]); 788 flip(x2->x[1],x2->x[2]); 789 flip(y->x[1],y->x[2]); 790 //flip(x[1],x[2]); 791 case 1: 792 flip(x1->x[0],x1->x[1]); 793 flip(x2->x[0],x2->x[1]); 794 flip(y->x[0],y->x[1]); 795 //flip(x[0],x[1]); 796 flip(x1->x[1],x1->x[2]); 797 flip(x2->x[1],x2->x[2]); 798 flip(y->x[1],y->x[2]); 799 //flip(x[1],x[2]); 800 break; 801 } 802 // now comes the case system 803 D1 = -y->x[0]/x1->x[0]*x1->x[1]+y->x[1]; 804 D2 = -y->x[0]/x1->x[0]*x1->x[2]+y->x[2]; 805 D3 = y->x[0]/x1->x[0]*A-B1; 806 Log() << Verbose(2) << "D1 " << D1 << "\tD2 " << D2 << "\tD3 " << D3 << "\n"; 807 if (fabs(D1) < MYEPSILON) { 808 Log() << Verbose(2) << "D1 == 0!\n"; 809 if (fabs(D2) > MYEPSILON) { 810 Log() << Verbose(3) << "D2 != 0!\n"; 811 x[2] = -D3/D2; 812 E1 = A/x1->x[0] + x1->x[2]/x1->x[0]*D3/D2; 813 E2 = -x1->x[1]/x1->x[0]; 814 Log() << Verbose(3) << "E1 " << E1 << "\tE2 " << E2 << "\n"; 815 F1 = E1*E1 + 1.; 816 F2 = -E1*E2; 817 F3 = E1*E1 + D3*D3/(D2*D2) - C; 818 Log() << Verbose(3) << "F1 " << F1 << "\tF2 " << F2 << "\tF3 " << F3 << "\n"; 819 if (fabs(F1) < MYEPSILON) { 820 Log() << Verbose(4) << "F1 == 0!\n"; 821 Log() << Verbose(4) << "Gleichungssystem linear\n"; 822 x[1] = F3/(2.*F2); 823 } else { 824 p = F2/F1; 825 q = p*p - F3/F1; 826 Log() << Verbose(4) << "p " << p << "\tq " << q << endl; 827 if (q < 0) { 828 Log() << Verbose(4) << "q < 0" << endl; 829 return false; 830 } 831 x[1] = p + sqrt(q); 832 } 833 x[0] = A/x1->x[0] - x1->x[1]/x1->x[0]*x[1] + x1->x[2]/x1->x[0]*x[2]; 834 } else { 835 Log() << Verbose(2) << "Gleichungssystem unterbestimmt\n"; 836 return false; 837 } 838 } else { 839 E1 = A/x1->x[0]+x1->x[1]/x1->x[0]*D3/D1; 840 E2 = x1->x[1]/x1->x[0]*D2/D1 - x1->x[2]; 841 Log() << Verbose(2) << "E1 " << E1 << "\tE2 " << E2 << "\n"; 842 F1 = E2*E2 + D2*D2/(D1*D1) + 1.; 843 F2 = -(E1*E2 + D2*D3/(D1*D1)); 844 F3 = E1*E1 + D3*D3/(D1*D1) - C; 845 Log() << Verbose(2) << "F1 " << F1 << "\tF2 " << F2 << "\tF3 " << F3 << "\n"; 846 if (fabs(F1) < MYEPSILON) { 847 Log() << Verbose(3) << "F1 == 0!\n"; 848 Log() << Verbose(3) << "Gleichungssystem linear\n"; 849 x[2] = F3/(2.*F2); 850 } else { 851 p = F2/F1; 852 q = p*p - F3/F1; 853 Log() << Verbose(3) << "p " << p << "\tq " << q << endl; 854 if (q < 0) { 855 Log() << Verbose(3) << "q < 0" << endl; 856 return false; 857 } 858 x[2] = p + sqrt(q); 859 } 860 x[1] = (-D2 * x[2] - D3)/D1; 861 x[0] = A/x1->x[0] - x1->x[1]/x1->x[0]*x[1] + x1->x[2]/x1->x[0]*x[2]; 862 } 863 switch (flag) { // back-flipping 864 default: 865 case 0: 866 break; 867 case 2: 868 flip(x1->x[0],x1->x[1]); 869 flip(x2->x[0],x2->x[1]); 870 flip(y->x[0],y->x[1]); 871 flip(x[0],x[1]); 872 flip(x1->x[1],x1->x[2]); 873 flip(x2->x[1],x2->x[2]); 874 flip(y->x[1],y->x[2]); 875 flip(x[1],x[2]); 876 case 1: 877 flip(x1->x[0],x1->x[1]); 878 flip(x2->x[0],x2->x[1]); 879 flip(y->x[0],y->x[1]); 880 //flip(x[0],x[1]); 881 flip(x1->x[1],x1->x[2]); 882 flip(x2->x[1],x2->x[2]); 883 flip(y->x[1],y->x[2]); 884 flip(x[1],x[2]); 885 break; 886 } 887 // one z component is only determined by its radius (without sign) 888 // thus check eight possible sign flips and determine by checking angle with second vector 889 for (i=0;i<8;i++) { 890 // set sign vector accordingly 891 for (j=2;j>=0;j--) { 892 k = (i & pot(2,j)) << j; 893 Log() << Verbose(2) << "k " << k << "\tpot(2,j) " << pot(2,j) << endl; 894 sign[j] = (k == 0) ? 1. : -1.; 895 } 896 Log() << Verbose(2) << i << ": sign matrix is " << sign[0] << "\t" << sign[1] << "\t" << sign[2] << "\n"; 897 // apply sign matrix 898 for (j=NDIM;j--;) 899 x[j] *= sign[j]; 900 // calculate angle and check 901 ang = x2->Angle (this); 902 Log() << Verbose(1) << i << "th angle " << ang << "\tbeta " << cos(beta) << " :\t"; 903 if (fabs(ang - cos(beta)) < MYEPSILON) { 904 break; 905 } 906 // unapply sign matrix (is its own inverse) 907 for (j=NDIM;j--;) 908 x[j] *= sign[j]; 909 } 910 return true; 911 }; 912 913 #endif 500 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 501 rep->SubtractVector(y); 502 } 914 503 915 504 /** … … 922 511 bool Vector::IsInParallelepiped(const Vector &offset, const double * const parallelepiped) const 923 512 { 924 Vector a = (*this) - offset; 925 a.InverseMatrixMultiplication(parallelepiped); 926 bool isInside = true; 927 928 for (int i=NDIM;i--;) 929 isInside = isInside && ((a.x[i] <= 1) && (a.x[i] >= 0)); 930 931 return isInside; 932 } 513 ASSERT((rep.get()) && (!rep->isBaseClass()),"Representation stored in vector Object was not of derived type"); 514 return rep->IsInParallelepiped(offset, parallelepiped); 515 } 516 517 bool Vector::isBaseClass() const{ 518 return true; 519 } 520 521 Vector* Vector::clone() const{ 522 ASSERT(false, "Cannot clone a base Vector object"); 523 return 0; 524 }
Note:
See TracChangeset
for help on using the changeset viewer.
