1 /* -*- c++ -*- ----------------------------------------------------------
2 *
3 * *** Smooth Mach Dynamics ***
4 *
5 * This file is part of the MACHDYN package for LAMMPS.
6 * Copyright (2014) Georg C. Ganzenmueller, georg.ganzenmueller@emi.fhg.de
7 * Fraunhofer Ernst-Mach Institute for High-Speed Dynamics, EMI,
8 * Eckerstrasse 4, D-79104 Freiburg i.Br, Germany.
9 *
10 * ----------------------------------------------------------------------- */
11
12 #ifndef SMD_MATH_H
13 #define SMD_MATH_H
14
15 #include <Eigen/Eigen>
16 #include <iostream>
17
18 namespace SMD_Math {
LimitDoubleMagnitude(double & x,const double limit)19 static inline void LimitDoubleMagnitude(double &x, const double limit)
20 {
21 /*
22 * if |x| exceeds limit, set x to limit with the sign of x
23 */
24 if (fabs(x) > limit) { // limit delVdotDelR to a fraction of speed of sound
25 x = limit * copysign(1.0, x);
26 }
27 }
28
29 /*
30 * deviator of a tensor
31 */
Deviator(const Eigen::Matrix3d M)32 static inline Eigen::Matrix3d Deviator(const Eigen::Matrix3d M)
33 {
34 Eigen::Matrix3d eye;
35 eye.setIdentity();
36 eye *= M.trace() / 3.0;
37 return M - eye;
38 }
39
40 /*
41 * Polar Decomposition M = R * T
42 * where R is a rotation and T a pure translation/stretch matrix.
43 *
44 * The decomposition is achieved using SVD, i.e. M = U S V^T,
45 * where U = R V and S is diagonal.
46 *
47 *
48 * For any physically admissible deformation gradient, the determinant of R must equal +1.
49 * However, scenerios can arise, where the particles interpenetrate and cause inversion, leading to a determinant of R equal to -1.
50 * In this case, the inversion direction is heuristically identified with the eigenvector of the smallest entry of S, which should work for most cases.
51 * The sign of this corresponding eigenvalue is flipped, the original matrix M is recomputed using the flipped S, and the rotation and translation matrices are
52 * obtained again from an SVD. The rotation should proper now, i.e., det(R) = +1.
53 */
54
PolDec(Eigen::Matrix3d M,Eigen::Matrix3d & R,Eigen::Matrix3d & T,bool scaleF)55 static inline bool PolDec(Eigen::Matrix3d M, Eigen::Matrix3d &R, Eigen::Matrix3d &T, bool scaleF)
56 {
57
58 Eigen::JacobiSVD<Eigen::Matrix3d> svd(
59 M, Eigen::ComputeFullU | Eigen::ComputeFullV); // SVD(A) = U S V*
60 Eigen::Vector3d S_eigenvalues = svd.singularValues();
61 Eigen::Matrix3d S = svd.singularValues().asDiagonal();
62 Eigen::Matrix3d U = svd.matrixU();
63 Eigen::Matrix3d V = svd.matrixV();
64 Eigen::Matrix3d eye;
65 eye.setIdentity();
66
67 // now do polar decomposition into M = R * T, where R is rotation
68 // and T is translation matrix
69 R = U * V.transpose();
70 T = V * S * V.transpose();
71
72 if (R.determinant() < 0.0) { // this is an improper rotation
73 // identify the smallest entry in S and flip its sign
74 int imin;
75 S_eigenvalues.minCoeff(&imin);
76 S(imin, imin) *= -1.0;
77
78 R = M * V * S.inverse() * V.transpose(); // recompute R using flipped stretch eigenvalues
79 }
80
81 /*
82 * scale S to avoid small principal strains
83 */
84
85 if (scaleF) {
86 double min = 0.3; // 0.3^2 = 0.09, should suffice for most problems
87 double max = 2.0;
88 for (int i = 0; i < 3; i++) {
89 if (S(i, i) < min) {
90 S(i, i) = min;
91 } else if (S(i, i) > max) {
92 S(i, i) = max;
93 }
94 }
95 T = V * S * V.transpose();
96 }
97
98 if (R.determinant() > 0.0) {
99 return true;
100 } else {
101 return false;
102 }
103 }
104
105 /*
106 * Pseudo-inverse via SVD
107 */
108
pseudo_inverse_SVD(Eigen::Matrix3d & M)109 static inline void pseudo_inverse_SVD(Eigen::Matrix3d &M)
110 {
111
112 Eigen::JacobiSVD<Eigen::Matrix3d> svd(
113 M,
114 Eigen::
115 ComputeFullU); // one Eigevector base is sufficient because matrix is square and symmetric
116
117 Eigen::Vector3d singularValuesInv;
118 Eigen::Vector3d singularValues = svd.singularValues();
119
120 double pinvtoler =
121 1.0e-16; // 2d machining example goes unstable if this value is increased (1.0e-16).
122 for (int row = 0; row < 3; row++) {
123 if (singularValues(row) > pinvtoler) {
124 singularValuesInv(row) = 1.0 / singularValues(row);
125 } else {
126 singularValuesInv(row) = 0.0;
127 }
128 }
129
130 M = svd.matrixU() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();
131 }
132
133 /*
134 * test if two matrices are equal
135 */
TestMatricesEqual(Eigen::Matrix3d A,Eigen::Matrix3d B,double eps)136 static inline double TestMatricesEqual(Eigen::Matrix3d A, Eigen::Matrix3d B, double eps)
137 {
138 Eigen::Matrix3d diff;
139 diff = A - B;
140 double norm = diff.norm();
141 if (norm > eps) {
142 std::cout << "Matrices A and B are not equal! The L2-norm difference is: " << norm << "\n"
143 << "Here is matrix A:\n"
144 << A << "\n"
145 << "Here is matrix B:\n"
146 << B << std::endl;
147 }
148 return norm;
149 }
150
151 /* ----------------------------------------------------------------------
152 Limit eigenvalues of a matrix to upper and lower bounds.
153 ------------------------------------------------------------------------- */
154
LimitEigenvalues(Eigen::Matrix3d S,double limitEigenvalue)155 static inline Eigen::Matrix3d LimitEigenvalues(Eigen::Matrix3d S, double limitEigenvalue)
156 {
157
158 /*
159 * compute Eigenvalues of matrix S
160 */
161 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es;
162 es.compute(S);
163
164 double max_eigenvalue = es.eigenvalues().maxCoeff();
165 double min_eigenvalue = es.eigenvalues().minCoeff();
166 double amax_eigenvalue = fabs(max_eigenvalue);
167 double amin_eigenvalue = fabs(min_eigenvalue);
168
169 if ((amax_eigenvalue > limitEigenvalue) || (amin_eigenvalue > limitEigenvalue)) {
170 if (amax_eigenvalue > amin_eigenvalue) { // need to scale with max_eigenvalue
171 double scale = amax_eigenvalue / limitEigenvalue;
172 Eigen::Matrix3d V = es.eigenvectors();
173 Eigen::Matrix3d S_diag = V.inverse() * S * V; // diagonalized input matrix
174 S_diag /= scale;
175 Eigen::Matrix3d S_scaled = V * S_diag * V.inverse(); // undiagonalize matrix
176 return S_scaled;
177 } else { // need to scale using min_eigenvalue
178 double scale = amin_eigenvalue / limitEigenvalue;
179 Eigen::Matrix3d V = es.eigenvectors();
180 Eigen::Matrix3d S_diag = V.inverse() * S * V; // diagonalized input matrix
181 S_diag /= scale;
182 Eigen::Matrix3d S_scaled = V * S_diag * V.inverse(); // undiagonalize matrix
183 return S_scaled;
184 }
185 } else { // limiting does not apply
186 return S;
187 }
188 }
189
LimitMinMaxEigenvalues(Eigen::Matrix3d & S,double min,double max)190 static inline bool LimitMinMaxEigenvalues(Eigen::Matrix3d &S, double min, double max)
191 {
192
193 /*
194 * compute Eigenvalues of matrix S
195 */
196 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es;
197 es.compute(S);
198
199 if ((es.eigenvalues().maxCoeff() > max) || (es.eigenvalues().minCoeff() < min)) {
200 Eigen::Matrix3d S_diag = es.eigenvalues().asDiagonal();
201 Eigen::Matrix3d V = es.eigenvectors();
202 for (int i = 0; i < 3; i++) {
203 if (S_diag(i, i) < min) {
204 //printf("limiting eigenvalue %f --> %f\n", S_diag(i, i), min);
205 //printf("these are the eigenvalues of U: %f %f %f\n", es.eigenvalues()(0), es.eigenvalues()(1), es.eigenvalues()(2));
206 S_diag(i, i) = min;
207 } else if (S_diag(i, i) > max) {
208 //printf("limiting eigenvalue %f --> %f\n", S_diag(i, i), max);
209 S_diag(i, i) = max;
210 }
211 }
212 S = V * S_diag * V.inverse(); // undiagonalize matrix
213 return true;
214 } else {
215 return false;
216 }
217 }
218
reconstruct_rank_deficient_shape_matrix(Eigen::Matrix3d & K)219 static inline void reconstruct_rank_deficient_shape_matrix(Eigen::Matrix3d &K)
220 {
221
222 Eigen::JacobiSVD<Eigen::Matrix3d> svd(K, Eigen::ComputeFullU | Eigen::ComputeFullV);
223 Eigen::Vector3d singularValues = svd.singularValues();
224
225 for (int i = 0; i < 3; i++) {
226 if (singularValues(i) < 1.0e-8) { singularValues(i) = 1.0; }
227 }
228
229 // int imin;
230 // double minev = singularValues.minCoeff(&imin);
231 //
232 // printf("min eigenvalue=%f has index %d\n", minev, imin);
233 // Vector3d singularVec = U.col(0).cross(U.col(1));
234 // cout << "the eigenvalues are " << endl << singularValues << endl;
235 // cout << "the singular vector is " << endl << singularVec << endl;
236 //
237 // // reconstruct original K
238 //
239 // singularValues(2) = 1.0;
240
241 K = svd.matrixU() * singularValues.asDiagonal() * svd.matrixV().transpose();
242 //cout << "the reconstructed K is " << endl << K << endl;
243 //exit(1);
244 }
245
246 /* ----------------------------------------------------------------------
247 helper functions for crack_exclude
248 ------------------------------------------------------------------------- */
IsOnSegment(double xi,double yi,double xj,double yj,double xk,double yk)249 static inline bool IsOnSegment(double xi, double yi, double xj, double yj, double xk, double yk)
250 {
251 return (xi <= xk || xj <= xk) && (xk <= xi || xk <= xj) && (yi <= yk || yj <= yk) &&
252 (yk <= yi || yk <= yj);
253 }
254
ComputeDirection(double xi,double yi,double xj,double yj,double xk,double yk)255 static inline char ComputeDirection(double xi, double yi, double xj, double yj, double xk,
256 double yk)
257 {
258 double a = (xk - xi) * (yj - yi);
259 double b = (xj - xi) * (yk - yi);
260 return a < b ? -1.0 : a > b ? 1.0 : 0;
261 }
262
263 /** Do line segments (x1, y1)--(x2, y2) and (x3, y3)--(x4, y4) intersect? */
DoLineSegmentsIntersect(double x1,double y1,double x2,double y2,double x3,double y3,double x4,double y4)264 static inline bool DoLineSegmentsIntersect(double x1, double y1, double x2, double y2, double x3,
265 double y3, double x4, double y4)
266 {
267 char d1 = ComputeDirection(x3, y3, x4, y4, x1, y1);
268 char d2 = ComputeDirection(x3, y3, x4, y4, x2, y2);
269 char d3 = ComputeDirection(x1, y1, x2, y2, x3, y3);
270 char d4 = ComputeDirection(x1, y1, x2, y2, x4, y4);
271 return (((d1 > 0 && d2 < 0) || (d1 < 0 && d2 > 0)) &&
272 ((d3 > 0 && d4 < 0) || (d3 < 0 && d4 > 0))) ||
273 (d1 == 0 && IsOnSegment(x3, y3, x4, y4, x1, y1)) ||
274 (d2 == 0 && IsOnSegment(x3, y3, x4, y4, x2, y2)) ||
275 (d3 == 0 && IsOnSegment(x1, y1, x2, y2, x3, y3)) ||
276 (d4 == 0 && IsOnSegment(x1, y1, x2, y2, x4, y4));
277 }
278
279 } // namespace SMD_Math
280
281 #endif /* SMD_MATH_H_ */
282