math_lib.h 15.6 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(); `````` 57 `````` void Print(std::ofstream *Datei); `````` Phillip Lino Rall committed Mar 11, 2020 58 `````` void operator=(const D3vector& v) { x=v.x; y=v.y; z=v.z; } `````` Phillip Lino Rall committed May 25, 2020 59 60 61 62 `````` 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 63 64 `````` //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 65 `````` bool operator==(const double v) {if( (v == x) && (v == y) && (v == z) ){return true;} else {return false;} } `````` Phillip Lino Rall committed Sep 25, 2020 66 `````` bool operator!=(const double v) {if( (v != x) || (v != y) || (v != z) ){return true;} else {return false;} } `````` 67 68 `````` 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;} } `````` 69 70 71 72 ``````// 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 73 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 `````` 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)); } `````` 102 103 104 105 ``````inline double SUM(D3vector V) { return V.x+V.y+V.z; } `````` Phillip Lino Rall committed Mar 11, 2020 106 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 157 158 159 160 ``````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 191 `````` // std::cout << "det "<< det << std::endl; `````` Phillip Lino Rall committed Mar 11, 2020 192 193 194 195 196 197 198 199 200 201 202 203 204 `````` /* 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 205 `````` inline void Print() { `````` Phillip Lino Rall committed Sep 25, 2020 206 207 208 `````` std::cout << "Matrix: " << ";\n"; std::cout << x1 << ", " << y1 << ", " << z1 << ";\n" ; std::cout << x2 << ", " << y2 << ", " << z2 << ";\n" ; `````` Phillip Lino Rall committed May 25, 2020 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 `````` std::cout << x3 << ", " << y3 << ", " << z3 << ";" < 0; i--) { //Swapping each and every element of the two rows if (a[i - 1][0] < a[i][0]) 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; } } // 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) { temp = a[j][i] / a[i][i]; for (int k = 0; k < 2 * order; k++) { a[j][k] -= a[i][k] * temp; } } } } // Multiply each row by a nonzero integer. // Divide row element by the diagonal element for (int i = 0; i < order; i++) { temp = a[i][i]; for (int j = 0; j < 2 * order; j++) { a[i][j] = a[i][j] / temp; } } // 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, DTyp vC, DTyp vD) { return vA + (vB-vA) * lambda.x + (vC-vA) * lambda.y + (vD-vA) * lambda.z; } `````` 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 ``````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 400 401 402 403 404 405 406 407 408 409 410 411 412 `````` ////////////////////////////////////////////////////////////////////// // 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); } `````` 413 414 415 ``````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 416 417 418 419 420 421 422 ``````// 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 423 424 `````` v.z*w.x-v.x*w.z, v.x*w.y-v.y*w.x); `````` Phillip Lino Rall committed Mar 11, 2020 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 ``````} 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) { return arccos(product(va,vb) / (D3VectorNorm(va)*D3VectorNorm(vb))) / M_PI * 180 ; } `````` 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 ``````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 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 `````` 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); ////////////////////////////////////////////// // Implementierung einiger Memberfunktionen ////////////////////////////////////////////// // D3vector // ---------- inline void D3vector::Print() { `````` Phillip Lino Rall committed Sep 25, 2020 498 `````` std::cout << "Coordinate: " << x << ", " << y << ", " << z << ";\n"; `````` Phillip Lino Rall committed Mar 11, 2020 499 500 ``````} `````` 501 ``````inline void D3vector::Print(std::ofstream *Datei) { `````` Phillip Lino Rall committed Mar 11, 2020 502 503 504 505 506 507 508 `````` *Datei << x << " " << y << " " << z; } #endif // MATHLIB_H_ ``````