math_lib.h 17.8 KB
 Phillip Lino Rall committed Mar 11, 2020 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 ``````/********************************************************************************** * Copyright 2010 Christoph Pflaum * Department Informatik Lehrstuhl 10 - Systemsimulation * Friedrich-Alexander Universität Erlangen-Nürnberg * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. **********************************************************************************/ // ------------------------------------------------------------ // math_lib.h // // ------------------------------------------------------------ #ifndef MATHLIB_H_ #define MATHLIB_H_ `````` 27 ``````#include `````` Phillip Lino Rall committed Mar 11, 2020 28 ``````#include "../../../common_source/mathlib/math_lib_common.h" `````` 29 30 ``````#include #include `````` Phillip Lino Rall committed Mar 11, 2020 31 32 33 34 35 36 37 38 `````` #define eps_for_tet_test 0.000001 ////////////////////////////////////////////////////////////////////// // 1. 3D vector class // 2. a simple 3D matrix class and p in hexahedron test // 3. geometric operators for 3D vectors ////////////////////////////////////////////////////////////////////// `````` 39 40 41 ``````inline bool isNaN(const double pV) { return (pV != pV) || (fabs(pV) == std::numeric_limits::infinity()); } `````` Phillip Lino Rall committed Mar 11, 2020 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 `````` ////////////////////////////////////////////////////////////////////// // 1. a simple 3D vector class ////////////////////////////////////////////////////////////////////// class D3vector { public: double x,y,z; D3vector(double cx, double cy, double cz) : x(cx), y(cy), z(cz) {}; explicit D3vector(double c) : x(c), y(c), z(c) {}; D3vector() : x(0), y(0), z(0) {}; ~D3vector(){}; void Print(); `````` Phillip Lino Rall committed Dec 07, 2020 57 `````` void PrintCoordinatesOnly(); `````` 58 `````` void Print(std::ofstream *Datei); `````` Phillip Lino Rall committed Mar 11, 2020 59 `````` void operator=(const D3vector& v) { x=v.x; y=v.y; z=v.z; } `````` Phillip Lino Rall committed May 25, 2020 60 61 62 63 `````` void operator+=(const D3vector& v) { x+=v.x; y+=v.y; z+=v.z; } void operator-=(const D3vector& v) { x-=v.x; y-=v.y; z-=v.z; } void operator*=(const double v) { x*=v; y*=v; z*=v; } void operator/=(const double v) { x/=v; y/=v; z/=v; } `````` Phillip Lino Rall committed Jun 23, 2020 64 65 `````` //bool operator==(const D3vector& v) {if(fabs(v.x - x) <1e-10 && fabs(v.y - y) <1e-10 && fabs(v.z - z) <1e-10 ){return true;} else {return false;} } bool operator==(const D3vector& v) {if( (v.x == x) && (v.y == y) && (v.z == z) ){return true;} else {return false;} } `````` Phillip Lino Rall committed Aug 12, 2020 66 `````` bool operator==(const double v) {if( (v == x) && (v == y) && (v == z) ){return true;} else {return false;} } `````` Phillip Lino Rall committed Sep 25, 2020 67 `````` bool operator!=(const double v) {if( (v != x) || (v != y) || (v != z) ){return true;} else {return false;} } `````` 68 69 `````` bool operator<(const D3vector& v) {if(v.x > x && v.y > y && v.z > z){return true;} else {return false;} } bool operator>(const D3vector& v) {if(v.x < x && v.y < y && v.z < z){return true;} else {return false;} } `````` 70 71 72 73 ``````// bool operator<=(const D3vector& v) {if(v.x >= x && v.y >= y && v.z >= z){return true;} else {return false;} } // bool operator>=(const D3vector& v) {if(v.x <= x && v.y <= y && v.z <= z){return true;} else {return false;} } bool operator<=(const D3vector& v) {if(v.x-x >= -1e-10 && v.y-y >= -1e-10 && v.z-z >= -1e-10){return true;} else {return false;} } bool operator>=(const D3vector& v) {if(v.x-x <= +1e-10 && v.y-y <= +1e-10 && v.z-z <= +1e-10){return true;} else {return false;} } `````` Phillip Lino Rall committed Mar 11, 2020 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 `````` double operator[](int i) { if(i==0) return x; if(i==1) return y; return z; } bool testZwischen(D3vector A, D3vector B); // testet obPunkt auf Gerade zwischen A und B ist. }; inline double MIN(D3vector V) { if(V.xV.z && V.x>V.y) return V.x; if(V.y>V.z && V.y>V.x) return V.y; return V.z; } inline D3vector MAX(D3vector V1, D3vector V2) { return D3vector(MAX(V1.x,V2.x),MAX(V1.y,V2.y),MAX(V1.z,V2.z)); } inline D3vector MIN(D3vector V1, D3vector V2) { return D3vector(MIN(V1.x,V2.x),MIN(V1.y,V2.y),MIN(V1.z,V2.z)); } `````` 103 104 105 106 ``````inline double SUM(D3vector V) { return V.x+V.y+V.z; } `````` Phillip Lino Rall committed Mar 11, 2020 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 ``````inline D3vector operator+(const D3vector& v,const D3vector& w) { return D3vector(v.x+w.x,v.y+w.y,v.z+w.z); } inline D3vector operator*(const D3vector& v,const D3vector& w) { return D3vector(v.x*w.x,v.y*w.y,v.z*w.z); } inline D3vector operator/(const D3vector& v,const D3vector& w) { return D3vector(v.x/w.x,v.y/w.y,v.z/w.z); } inline D3vector operator-(const D3vector& v,const D3vector& w) { return D3vector(v.x-w.x,v.y-w.y,v.z-w.z); } inline D3vector operator/(const D3vector& v,const double f) { return D3vector(v.x/f,v.y/f,v.z/f); } inline D3vector operator*(const D3vector& v,const double f) { return D3vector(v.x*f,v.y*f,v.z*f); } inline D3vector operator*(const double f, const D3vector& v) { return D3vector(v.x*f,v.y*f,v.z*f); } inline bool operator<(const D3vector& v,const D3vector& w) { return (v.x 1.0+eps_for_tet_test) return false; t = (- (y1*z3 - y3*z1) * v.x + (x1*z3 - x3*z1) * v.y - (x1*y3 - x3*y1) * v.z) / det; if(t < -eps_for_tet_test || t > 1.0+eps_for_tet_test) return false; t = ((y1*z2 - y2*z1) * v.x - (x1*z2 - x2*z1) * v.y + (x1*y2 - x2*y1) * v.z ) / det; if(t < -eps_for_tet_test || t > 1.0+eps_for_tet_test) return false; return true; } */ D3vector invert_apply(const D3vector& v) { double x,y,z; double det = Determinante(); //x1 * (y2*z3 - y3*z2) - y1 * (x2*z3 - x3*z2) + z1 * (x2*y3 - x3*y2); `````` Phillip Lino Rall committed May 25, 2020 197 `````` // std::cout << "det "<< det << std::endl; `````` Phillip Lino Rall committed Mar 11, 2020 198 199 200 201 202 203 204 205 206 207 208 209 210 `````` /* x = ( (y2*z3 - y3*z2) * v.x - (x2*z3 - x3*z2) * v.y + (x2*y3 - x3*y2) * v.z ) / det; y = (- (y1*z3 - y3*z1) * v.x + (x1*z3 - x3*z1) * v.y - (x1*y3 - x3*y1) * v.z ) / det; z = ( (y1*z2 - y2*z1) * v.x - (x1*z2 - x2*z1) * v.y + (x1*y2 - x2*y1) * v.z ) / det; */ x = ( (y2*z3 - y3*z2) * v.x - (y1*z3 - y3*z1) * v.y + (y1*z2 - y2*z1) * v.z ) / det; y = (- (x2*z3 - x3*z2) * v.x + (x1*z3 - x3*z1) * v.y - (x1*z2 - x2*z1) * v.z ) / det; z = ( (x2*y3 - x3*y2)* v.x - (x1*y3 - x3*y1) * v.y + (x1*y2 - x2*y1) * v.z ) / det; return D3vector(x,y,z); } `````` Phillip Lino Rall committed May 25, 2020 211 `````` inline void Print() { `````` Phillip Lino Rall committed Sep 25, 2020 212 213 214 `````` std::cout << "Matrix: " << ";\n"; std::cout << x1 << ", " << y1 << ", " << z1 << ";\n" ; std::cout << x2 << ", " << y2 << ", " << z2 << ";\n" ; `````` Phillip Lino Rall committed May 25, 2020 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 `````` std::cout << x3 << ", " << y3 << ", " << z3 << ";" < 0; i--) { //Swapping each and every element of the two rows `````` Phillip Lino Rall committed Nov 10, 2020 312 `````` if (a[i - 1][0] < a[i][0])// && a[i][0]!=0) `````` Phillip Lino Rall committed May 25, 2020 313 314 315 316 317 318 319 320 321 `````` for (int j = 0; j < 2 * order; j++) { // Swapping of the row, if above // condition satisfied. temp = a[i][j]; a[i][j] = a[i - 1][j]; a[i - 1][j] = temp; } } `````` 322 323 324 325 326 327 328 ``````// std::cout << "matrix after swapping: \n" ; // for (int i = 0; i < n; i++) { // for (int j = 0; j < 6; j++) { // std::cout << a[i][j] << " "; // } // std::cout<<"\n"; // } `````` Phillip Lino Rall committed May 25, 2020 329 330 331 332 333 334 335 336 337 338 `````` // Replace a row by sum of itself and a // constant multiple of another row of the matrix for (int i = 0; i < order; i++) { for (int j = 0; j < order; j++) { if (j != i) { `````` 339 340 341 342 `````` if (a[i][i] == 0) { std::cout << "debug here" << std::endl; } `````` Phillip Lino Rall committed May 25, 2020 343 344 345 346 `````` temp = a[j][i] / a[i][i]; for (int k = 0; k < 2 * order; k++) { a[j][k] -= a[i][k] * temp; `````` Phillip Lino Rall committed Nov 10, 2020 347 348 349 350 ``````// if (fabs(a[j][k]) < 1e-20 ) // { // std::cout << "is small???"; // } `````` Phillip Lino Rall committed May 25, 2020 351 352 353 354 355 356 357 358 `````` } } } } // Multiply each row by a nonzero integer. // Divide row element by the diagonal element for (int i = 0; i < order; i++) { `````` Phillip Lino Rall committed Nov 10, 2020 359 360 361 362 ``````// if (a[i][i] == 0) // { // std::cout << "debug here"; // } `````` Phillip Lino Rall committed May 25, 2020 363 364 365 366 `````` temp = a[i][i]; for (int j = 0; j < 2 * order; j++) { a[i][j] = a[i][j] / temp; `````` Phillip Lino Rall committed Nov 10, 2020 367 368 369 370 ``````// if (a[i][j] == 0) // { // std::cout << "is zero here"; // } `````` Phillip Lino Rall committed May 25, 2020 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 `````` } } // std::cout << "inverted matrix : " << std::endl; // for (int i = 0; i < n; i++) { // for (int j = order; j < 6; j++) { // std::cout << a[i][j] << " "; // } // std::cout< DTyp interpolate_in_tet(D3vector lambda, DTyp vA, DTyp vB, `````` Phillip Lino Rall committed Nov 10, 2020 412 413 414 415 416 417 418 `````` DTyp vC, DTyp vD) { return vA + (vB-vA) * lambda.x + (vC-vA) * lambda.y + (vD-vA) * lambda.z; } inline D3vector interpolate_in_tet_D3vector(D3vector lambda, D3vector vA, D3vector vB, D3vector vC, D3vector vD) { `````` Phillip Lino Rall committed Mar 11, 2020 419 420 421 `````` return vA + (vB-vA) * lambda.x + (vC-vA) * lambda.y + (vD-vA) * lambda.z; } `````` Phillip Lino Rall committed Nov 10, 2020 422 `````` `````` 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 ``````template DTyp interpolate_in_tet_trilinear(D3vector lambda, DTyp x0, DTyp x1, DTyp x2, DTyp x3, DTyp x4, DTyp x5, DTyp x6, DTyp x7) { DTyp A = x0; DTyp B = x1-x0; DTyp C = x3-x0; DTyp D = x7-x0; DTyp E = x2-x1-x3+x0; DTyp F = x4-x3-x7+x0; DTyp G = x6-x1-x7+x0; DTyp H = x1+x3+x5+x7-x0-x2-x4-x6; DTyp R = A + B * lambda.x + C * lambda.y + D * lambda.z + E * lambda.x*lambda.y + F * lambda.y*lambda.z + G * lambda.x * lambda.z + H * lambda.x*lambda.y*lambda.z; return R; // return vA + (vB-vA) * lambda.x + (vC-vA) * lambda.y + (vD-vA) * lambda.z; } `````` Phillip Lino Rall committed Mar 11, 2020 443 `````` `````` Phillip Lino Rall committed Nov 10, 2020 444 445 446 447 448 449 450 451 452 453 454 455 456 457 ``````template DTyp interpolate_in_tet_bilinear(D3vector lambda, DTyp x1, DTyp x2, DTyp x3, DTyp x4) { DTyp A = x1; DTyp B = x2-x1; DTyp C = x3-x1; DTyp D = x4-x2+x1-x3; DTyp R = A + lambda.x * B + lambda.y * C + lambda.x * lambda.y * D; return R; } `````` Phillip Lino Rall committed Mar 11, 2020 458 459 460 461 462 463 464 465 466 467 468 469 ``````////////////////////////////////////////////////////////////////////// // 3. geometric operators for 3D vectors /* inline double L_infty(const D3vector& v) { return MAX(ABS(v.x),ABS(v.y),ABS(v.z)); } */ inline double D3VectorNorm(const D3vector& v) { return my_sqrt(v.x*v.x+v.y*v.y+v.z*v.z); } `````` 470 471 472 ``````inline double D3VectorNormSquared(const D3vector& v){ return (v.x*v.x+v.y*v.y+v.z*v.z); } `````` Phillip Lino Rall committed Mar 11, 2020 473 474 475 476 477 478 479 ``````// scalar product inline double product(const D3vector& v, const D3vector& w) { return v.x*w.x+v.y*w.y+v.z*w.z; } inline D3vector cross_product(const D3vector& v, const D3vector& w) { return D3vector(v.y*w.z-v.z*w.y, `````` Phillip Lino Rall committed Sep 25, 2020 480 481 `````` v.z*w.x-v.x*w.z, v.x*w.y-v.y*w.x); `````` Phillip Lino Rall committed Mar 11, 2020 482 483 484 485 486 487 488 489 490 491 492 493 494 ``````} inline D3vector normal_vector_of_triangle(const D3vector& va, const D3vector& vb, const D3vector& vc) { D3vector z; z = cross_product(va-vb,vc-vb); return z / D3VectorNorm(z); } inline double angle_between_vectors(const D3vector& va, const D3vector& vb) { `````` 495 496 497 498 499 500 501 502 503 504 `````` double var = product(va,vb) / (D3VectorNorm(va)*D3VectorNorm(vb)) ; if (var > 1.0) { var = 1.0; } else if (var < -1.0) { var = -1.0; } return acos(var) / M_PI * 180 ; `````` Phillip Lino Rall committed Mar 11, 2020 505 506 ``````} `````` 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 ``````inline double angle_between_vectors_rad(const D3vector& va, const D3vector& vb) { /* * calculates angle in rad, also checks if var in range [-1,1]. can result in error if not! */ double var = product(va,vb) / (D3VectorNorm(va)*D3VectorNorm(vb)) ; if (var > 1.0) { var = 1.0; } else if (var < -1.0) { var = -1.0; } return acos(var); } `````` Phillip Lino Rall committed Mar 11, 2020 523 `````` `````` Phillip Lino Rall committed Nov 10, 2020 524 `````` `````` Phillip Lino Rall committed Mar 11, 2020 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 ``````inline double max_interior_angel_of_triangle(const D3vector& va, const D3vector& vb, const D3vector& vc) { return MAX(angle_between_vectors(vc-va,vb-va), angle_between_vectors(va-vb,vc-vb), angle_between_vectors(vb-vc,va-vc)); } // va, vb, vc is one face // vb, vd, vc is one face inline double angle_between_faces(const D3vector& va, const D3vector& vb, const D3vector& vc, const D3vector& vd) { return 180 - angle_between_vectors(normal_vector_of_triangle(va,vb,vc), normal_vector_of_triangle(vb,vd,vc)); } double calc_maximal_edge_angle(const D3vector& va, const D3vector& vb, const D3vector& vc, const D3vector& vd); double calc_maximal_face_angle(const D3vector& va, const D3vector& vb, const D3vector& vc, const D3vector& vd); `````` Phillip Lino Rall committed Nov 10, 2020 555 ``````D3matrix rotationMatrix(D3vector vIn, D3vector vOut); `````` Phillip Lino Rall committed Mar 11, 2020 556 557 558 559 560 561 562 563 564 565 `````` ////////////////////////////////////////////// // Implementierung einiger Memberfunktionen ////////////////////////////////////////////// // D3vector // ---------- inline void D3vector::Print() { `````` Phillip Lino Rall committed Sep 25, 2020 566 `````` std::cout << "Coordinate: " << x << ", " << y << ", " << z << ";\n"; `````` Phillip Lino Rall committed Mar 11, 2020 567 568 ``````} `````` Phillip Lino Rall committed Dec 07, 2020 569 570 571 ``````inline void D3vector::PrintCoordinatesOnly() { std::cout << x << ", " << y << ", " << z; } `````` 572 ``````inline void D3vector::Print(std::ofstream *Datei) { `````` Phillip Lino Rall committed Mar 11, 2020 573 574 575 576 577 578 579 `````` *Datei << x << " " << y << " " << z; } #endif // MATHLIB_H_ ``````