/********************************************************************************** * 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_ #include #include "../../../common_source/mathlib/math_lib_common.h" #include #include #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 ////////////////////////////////////////////////////////////////////// inline bool isNaN(const double pV) { return (pV != pV) || (fabs(pV) == std::numeric_limits::infinity()); } ////////////////////////////////////////////////////////////////////// // 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(); void PrintCoordinatesOnly(); void Print(std::ofstream *Datei); 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 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; } //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;} } bool operator==(const double v) {if( (v == x) && (v == y) && (v == z) ){return true;} else {return false;} } bool operator!=(const double v) {if( (v != x) || (v != y) || (v != 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 && 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 && 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;} } 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)); } inline double SUM(D3vector V) { return V.x+V.y+V.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 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); // std::cout << "det "<< det << std::endl; /* 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); } inline void Print() { std::cout << "Matrix: " << ";\n"; std::cout << x1 << ", " << y1 << ", " << z1 << ";\n" ; std::cout << x2 << ", " << y2 << ", " << z2 << ";\n" ; std::cout << x3 << ", " << y3 << ", " << z3 << ";" < 0; i--) { //Swapping each and every element of the two rows if (a[i - 1][0] < a[i][0])// && a[i][0]!=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; } } // 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"; // } // 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) { if (a[i][i] == 0) { std::cout << "debug here" << std::endl; } temp = a[j][i] / a[i][i]; for (int k = 0; k < 2 * order; k++) { a[j][k] -= a[i][k] * temp; // if (fabs(a[j][k]) < 1e-20 ) // { // std::cout << "is small???"; // } } } } } // Multiply each row by a nonzero integer. // Divide row element by the diagonal element for (int i = 0; i < order; i++) { // if (a[i][i] == 0) // { // std::cout << "debug here"; // } temp = a[i][i]; for (int j = 0; j < 2 * order; j++) { a[i][j] = a[i][j] / temp; // if (a[i][j] == 0) // { // std::cout << "is zero here"; // } } } // 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; } inline D3vector interpolate_in_tet_D3vector(D3vector lambda, D3vector vA, D3vector vB, D3vector vC, D3vector vD) { return vA + (vB-vA) * lambda.x + (vC-vA) * lambda.y + (vD-vA) * lambda.z; } 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; } 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; } ////////////////////////////////////////////////////////////////////// // 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); } inline double D3VectorNormSquared(const D3vector& v){ return (v.x*v.x+v.y*v.y+v.z*v.z); } // 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, v.z*w.x-v.x*w.z, v.x*w.y-v.y*w.x); } 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) { 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 ; } 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); } 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); D3matrix rotationMatrix(D3vector vIn, D3vector vOut); ////////////////////////////////////////////// // Implementierung einiger Memberfunktionen ////////////////////////////////////////////// // D3vector // ---------- inline void D3vector::Print() { std::cout << "Coordinate: " << x << ", " << y << ", " << z << ";\n"; } inline void D3vector::PrintCoordinatesOnly() { std::cout << x << ", " << y << ", " << z; } inline void D3vector::Print(std::ofstream *Datei) { *Datei << x << " " << y << " " << z; } #endif // MATHLIB_H_