PeriDyno 1.0.0
Loading...
Searching...
No Matches
MatrixFunc.inl
Go to the documentation of this file.
1#include "Vector.h"
2#include "Matrix.h"
3
4#ifdef CUDA_BACKEND
5 #include "SparseMatrix/svd3_cuda.h"
6#endif // CUDA_BACKEND
7
8namespace dyno
9{
10 template<typename Real>
11 DYN_FUNC void jacobiRotate(SquareMatrix<Real, 3> &A, SquareMatrix<Real, 3> &R, int p, int q)
12 {
13 // rotates A through phi in pq-plane to set A(p,q) = 0
14 // rotation stored in R whose columns are eigenvectors of A
15 if (A(p, q) == 0.0f)
16 return;
17
18 Real d = (A(p, p) - A(q, q)) / (2.0f*A(p, q));
19 Real t = 1.0f / (fabs(d) + sqrt(d*d + 1.0f));
20 if (d < 0.0f) t = -t;
21 Real c = 1.0f / sqrt(t*t + 1);
22 Real s = t*c;
23 A(p, p) += t*A(p, q);
24 A(q, q) -= t*A(p, q);
25 A(p, q) = A(q, p) = 0.0f;
26 // transform A
27 int k;
28 for (k = 0; k < 3; k++) {
29 if (k != p && k != q) {
30 Real Akp = c*A(k, p) + s*A(k, q);
31 Real Akq = -s*A(k, p) + c*A(k, q);
32 A(k, p) = A(p, k) = Akp;
33 A(k, q) = A(q, k) = Akq;
34 }
35 }
36 // store rotation in R
37 for (k = 0; k < 3; k++) {
38 Real Rkp = c*R(k, p) + s*R(k, q);
39 Real Rkq = -s*R(k, p) + c*R(k, q);
40 R(k, p) = Rkp;
41 R(k, q) = Rkq;
42 }
43 }
44
45 template<typename Real>
46 DYN_FUNC void EigenDecomposition(const SquareMatrix<Real, 3> &A, SquareMatrix<Real, 3> &eigenVecs, Vector<Real, 3> &eigenVals)
47 {
48 const int numJacobiIterations = 10;
49 const Real epsilon = 1e-15f;
50
51 SquareMatrix<Real, 3> D = A;
52
53 // only for symmetric Matrix!
54 eigenVecs = SquareMatrix<Real, 3>::identityMatrix(); // unit matrix
55 int iter = 0;
56 while (iter < numJacobiIterations) { // 3 off diagonal elements
57 // find off diagonal element with maximum modulus
58 int p, q;
59 Real a, max;
60 max = fabs(D(0, 1));
61 p = 0; q = 1;
62 a = fabs(D(0, 2));
63 if (a > max) { p = 0; q = 2; max = a; }
64 a = fabs(D(1, 2));
65 if (a > max) { p = 1; q = 2; max = a; }
66 // all small enough -> done
67 if (max < epsilon) break;
68 // rotate matrix with respect to that element
69 jacobiRotate<Real>(D, eigenVecs, p, q);
70 iter++;
71 }
72 eigenVals[0] = D(0, 0);
73 eigenVals[1] = D(1, 1);
74 eigenVals[2] = D(2, 2);
75 }
76
77 template<typename Real>
78 DYN_FUNC void QRDecomposition(SquareMatrix<Real, 3> &A, SquareMatrix<Real, 3> &R, int p, int q)
79 {
80
81 }
82
83#ifdef CUDA_BACKEND
84 template<typename Real>
85 DYN_FUNC void polarDecomposition(const SquareMatrix<Real, 3> &A, SquareMatrix<Real, 3> &R, SquareMatrix<Real, 3> &U, SquareMatrix<Real, 3> &D, SquareMatrix<Real, 3> &V)
86 {
87 // A = SR, where S is symmetric and R is orthonormal
88 // -> S = (A A^T)^(1/2)
89
90 D = SquareMatrix<Real, 3>(0);
91 svd(A(0, 0), A(0, 1), A(0, 2), A(1, 0), A(1, 1), A(1, 2), A(2, 0), A(2, 1), A(2, 2),
92 U(0, 0), U(0, 1), U(0, 2), U(1, 0), U(1, 1), U(1, 2), U(2, 0), U(2, 1), U(2, 2),
93 D(0, 0), D(1, 1), D(2, 2),
94 V(0, 0), V(0, 1), V(0, 2), V(1, 0), V(1, 1), V(1, 2), V(2, 0), V(2, 1), V(2, 2));
95
96 SquareMatrix<Real, 3> H(0);
97 H(0, 0) = 1;
98 H(1, 1) = 1;
99 H(2, 2) = (V*U.transpose()).determinant();
100
101 R = U*H*V.transpose();
102
103 // A = U D U^T R
104/* Vector<Real, 3> eigenVals;
105 SquareMatrix<Real, 3> ATA = A.transpose()*A;
106 EigenDecomposition<Real>(ATA, V, eigenVals);
107
108 Real d0 = sqrt(eigenVals[0]);
109 Real d1 = sqrt(eigenVals[1]);
110 Real d2 = sqrt(eigenVals[2]);
111 D = SquareMatrix<Real, 3>(0);
112 D(0, 0) = d0;
113 D(1, 1) = d1;
114 D(2, 2) = d2;
115
116 const Real eps = 1e-6;
117
118 Real l0 = eigenVals[0]; if (l0 <= eps) l0 = 0.0f; else l0 = 1.0f / d0;
119 Real l1 = eigenVals[1]; if (l1 <= eps) l1 = 0.0f; else l1 = 1.0f / d1;
120 Real l2 = eigenVals[2]; if (l2 <= eps) l2 = 0.0f; else l2 = 1.0f / d2;
121
122 SquareMatrix<Real, 3> invD(0);
123 invD(0, 0) = l0;
124 invD(1, 1) = l1;
125 invD(2, 2) = l2;
126 SquareMatrix<Real, 3> S1 = V*invD*V.transpose();
127 R = A*S1;*/
128
129
130/* SquareMatrix<Real, 3> AAT = A*A.transpose();
131
132 R = SquareMatrix<Real, 3>::identityMatrix();
133 Vector<Real, 3> eigenVals;
134 EigenDecomposition<Real>(AAT, U, eigenVals);
135
136 Real d0 = sqrt(eigenVals[0]);
137 Real d1 = sqrt(eigenVals[1]);
138 Real d2 = sqrt(eigenVals[2]);
139 D = SquareMatrix<Real, 3>(0);
140 D(0, 0) = d0;
141 D(1, 1) = d1;
142 D(2, 2) = d2;
143
144 const Real eps = 1e-6;
145
146 Real l0 = eigenVals[0]; if (l0 <= eps) l0 = 0.0f; else l0 = 1.0f / d0;
147 Real l1 = eigenVals[1]; if (l1 <= eps) l1 = 0.0f; else l1 = 1.0f / d1;
148 Real l2 = eigenVals[2]; if (l2 <= eps) l2 = 0.0f; else l2 = 1.0f / d2;
149
150 SquareMatrix<Real, 3> invD(0);
151 invD(0, 0) = l0;
152 invD(1, 1) = l1;
153 invD(2, 2) = l2;
154
155 SquareMatrix<Real, 3> S1 = U*invD*U.transpose();
156 R = S1*A;
157
158 Vector<Real, 3> c0, c1, c2;
159 c0 = R.col(0);
160 c1 = R.col(1);
161 c2 = R.col(2);
162
163 int maxCol = 0;
164 Real maxMag = c0.normSquared();
165 if (c1.normSquared() > maxMag)
166 {
167 maxCol = 1;
168 maxMag = c1.normSquared();
169 }
170 if (c2.normSquared() > maxMag)
171 {
172 maxCol = 2;
173 maxMag = c2.normSquared();
174 }
175
176 if (R.col(maxCol).normSquared() < eps)
177 {
178 R = SquareMatrix<Real, 3>::identityMatrix();
179 }
180 else
181 {
182 Vector<Real, 3> col_0 = R.col(maxCol);
183 col_0 = col_0.normalize();
184 if (R.col((maxCol + 1) % 3).normSquared() > R.col((maxCol + 2) % 3).normSquared())
185 {
186 R.setCol(maxCol, col_0);
187 Vector<Real, 3> col_1 = R.col((maxCol + 1) % 3);
188 col_1 = col_1.normalize();
189 R.setCol((maxCol + 1) % 3, col_1);
190 Vector<Real, 3> col_2 = col_0.cross(col_1);
191 R.setCol((maxCol + 2) % 3, col_2);
192 }
193 else
194 {
195 if (R.col((maxCol + 2) % 3).normSquared() > eps)
196 {
197 Vector<Real, 3> col_2 = R.col((maxCol + 2) % 3);
198 col_2 = col_2.normalize();
199 R.setCol((maxCol + 2) % 3, col_2);
200 Vector<Real, 3> col_1 = col_2.cross(col_0);
201 R.setCol((maxCol + 1) % 3, col_1);
202 }
203 else
204 {
205 SquareMatrix<Real, 3> unity = SquareMatrix<Real, 3>::identityMatrix();
206 Vector<Real, 3> col_1;
207 for (int i = 0; i < 3; i++)
208 {
209 col_1 = unity.col(i);
210 if (col_1.cross(col_0).norm() > eps)
211 break;
212 }
213 col_1 = col_0.cross(col_1).normalize();
214 col_1 = col_0.cross(col_1).normalize();
215 R.setCol((maxCol + 1) % 3, col_1);
216 R.setCol((maxCol + 2) % 3, col_0.cross(col_1).normalize());
217 }
218 }
219 }
220 */
221
222 }
223#endif
224
225 template<typename Real>
226 DYN_FUNC void polarDecomposition(const SquareMatrix<Real, 3> &A, SquareMatrix<Real, 3> &R, SquareMatrix<Real, 3> &U, SquareMatrix<Real, 3> &D)
227 {
228 // A = SR, where S is symmetric and R is orthonormal
229 // -> S = (A A^T)^(1/2)
230
231 // A = U D U^T R
232
233// float a11, a12, a13, a21, a22, a23, a31, a32, a33;
234// a11 = ; a12 = ; a13 = ;
235// a11 = A(0, 0); a12 = A(0, 1); a13 = A(0, 2);
236// a11 = A(0, 0); a12 = A(0, 1); a13 = A(0, 2);
237//
238// float u11, u12, u13, u21, u22, u23, u31, u32, u33;
239// float s11, s12, s13, s21, s22, s23, s31, s32, s33;
240// float v11, v12, v13, v21, v22, v23, v31, v32, v33;
241
242// SquareMatrix<Real, 3> V;
243// D = SquareMatrix<Real, 3>(0);
244// svd(A(0, 0), A(0, 1), A(0, 2), A(1, 0), A(1, 1), A(1, 2), A(2, 0), A(2, 1), A(2, 2),
245// U(0, 0), U(0, 1), U(0, 2), U(1, 0), U(1, 1), U(1, 2), U(2, 0), U(2, 1), U(2, 2),
246// D(0, 0), D(1, 1), D(2, 2),
247// V(0, 0), V(0, 1), V(0, 2), V(1, 0), V(1, 1), V(1, 2), V(2, 0), V(2, 1), V(2, 2));
248//
249// SquareMatrix<Real, 3> H(0);
250// H(0, 0) = 1;
251// H(1, 1) = 1;
252// H(2, 2) = (V*U.transpose()).determinant();
253
254// R = U*H*V.transpose();
255
256 SquareMatrix<Real, 3> AAT;
257 AAT(0, 0) = A(0, 0)*A(0, 0) + A(0, 1)*A(0, 1) + A(0, 2)*A(0, 2);
258 AAT(1, 1) = A(1, 0)*A(1, 0) + A(1, 1)*A(1, 1) + A(1, 2)*A(1, 2);
259 AAT(2, 2) = A(2, 0)*A(2, 0) + A(2, 1)*A(2, 1) + A(2, 2)*A(2, 2);
260
261 AAT(0, 1) = A(0, 0)*A(1, 0) + A(0, 1)*A(1, 1) + A(0, 2)*A(1, 2);
262 AAT(0, 2) = A(0, 0)*A(2, 0) + A(0, 1)*A(2, 1) + A(0, 2)*A(2, 2);
263 AAT(1, 2) = A(1, 0)*A(2, 0) + A(1, 1)*A(2, 1) + A(1, 2)*A(2, 2);
264
265 AAT(1, 0) = AAT(0, 1);
266 AAT(2, 0) = AAT(0, 2);
267 AAT(2, 1) = AAT(1, 2);
268
269 R = SquareMatrix<Real, 3>::identityMatrix();
270 Vector<Real, 3> eigenVals;
271 EigenDecomposition<Real>(AAT, U, eigenVals);
272
273 Real d0 = sqrt(eigenVals[0]);
274 Real d1 = sqrt(eigenVals[1]);
275 Real d2 = sqrt(eigenVals[2]);
276 D = SquareMatrix<Real, 3>(0);
277 D(0, 0) = d0;
278 D(1, 1) = d1;
279 D(2, 2) = d2;
280
281 const Real eps = 1e-15f;
282
283 Real l0 = eigenVals[0]; if (l0 <= eps) l0 = 0.0f; else l0 = 1.0f / d0;
284 Real l1 = eigenVals[1]; if (l1 <= eps) l1 = 0.0f; else l1 = 1.0f / d1;
285 Real l2 = eigenVals[2]; if (l2 <= eps) l2 = 0.0f; else l2 = 1.0f / d2;
286
287 SquareMatrix<Real, 3> S1;
288 S1(0, 0) = l0*U(0, 0)*U(0, 0) + l1*U(0, 1)*U(0, 1) + l2*U(0, 2)*U(0, 2);
289 S1(1, 1) = l0*U(1, 0)*U(1, 0) + l1*U(1, 1)*U(1, 1) + l2*U(1, 2)*U(1, 2);
290 S1(2, 2) = l0*U(2, 0)*U(2, 0) + l1*U(2, 1)*U(2, 1) + l2*U(2, 2)*U(2, 2);
291
292 S1(0, 1) = l0*U(0, 0)*U(1, 0) + l1*U(0, 1)*U(1, 1) + l2*U(0, 2)*U(1, 2);
293 S1(0, 2) = l0*U(0, 0)*U(2, 0) + l1*U(0, 1)*U(2, 1) + l2*U(0, 2)*U(2, 2);
294 S1(1, 2) = l0*U(1, 0)*U(2, 0) + l1*U(1, 1)*U(2, 1) + l2*U(1, 2)*U(2, 2);
295
296 S1(1, 0) = S1(0, 1);
297 S1(2, 0) = S1(0, 2);
298 S1(2, 1) = S1(1, 2);
299
300 R = S1*A;
301
302 // stabilize
303 Vector<Real, 3> c0, c1, c2;
304 // c0 = R.col(0);
305 // c1 = R.col(1);
306 // c2 = R.col(2);
307 c0[0] = R(0, 0); c1[0] = R(0, 1); c2[0] = R(0, 2);
308 c0[1] = R(1, 0); c1[1] = R(1, 1); c2[1] = R(1, 2);
309 c0[2] = R(2, 0); c1[2] = R(2, 1); c2[2] = R(2, 2);
310
311 if (c0.normSquared() < eps)
312 c0 = c1.cross(c2);
313 else if (c1.normSquared() < eps)
314 c1 = c2.cross(c0);
315 else
316 c2 = c0.cross(c1);
317 // R.col(0) = c0;
318 // R.col(1) = c1;
319 // R.col(2) = c2;
320 R(0, 0) = c0[0]; R(0, 1) = c1[0]; R(0, 2) = c2[0];
321 R(1, 0) = c0[1]; R(1, 1) = c1[1]; R(1, 2) = c2[1];
322 R(2, 0) = c0[2]; R(2, 1) = c1[2]; R(2, 2) = c2[2];
323 }
324
325 template<typename Real>
326 DYN_FUNC void polarDecomposition(const SquareMatrix<Real, 3> &M, SquareMatrix<Real, 3> &R, Real tolerance)
327 {
328 SquareMatrix<Real, 3> Mt = M.transpose();
329 Real Mone = M.oneNorm();
330 Real Minf = M.infNorm();
331 Real Eone;
332 SquareMatrix<Real, 3> MadjTt, Et;
333 do
334 {
335 MadjTt.setRow(0, Mt.row(1).cross(Mt.row(2))); //glm::cross(Mt[1], Mt[2]);
336 MadjTt.setRow(1, Mt.row(2).cross(Mt.row(0))); //glm::cross(Mt[2], Mt[0]);
337 MadjTt.setRow(2, Mt.row(0).cross(Mt.row(1))); //glm::cross(Mt[0], Mt[1]);
338
339 Real det = Mt(0, 0) * MadjTt(0, 0) + Mt(1, 0) * MadjTt(1, 0) + Mt(2, 0) * MadjTt(2, 0);
340
341 if (fabs(det) < 1.0e-12)
342 {
343 Vector<Real, 3> len;
344 unsigned int index = 0xffffffff;
345 for (unsigned int i = 0; i < 3; i++)
346 {
347 len[i] = MadjTt.col(i).norm();
348 if (len[i] > 1.0e-12)
349 {
350 // index of valid cross product
351 // => is also the index of the vector in Mt that must be exchanged
352 index = i;
353 break;
354 }
355 }
356 if (index == 0xffffffff)
357 {
358 R = SquareMatrix<Real, 3>::identityMatrix();
359 return;
360 }
361 else
362 {
363 Mt.setRow(index, Mt.row((index + 1) % 3).cross(Mt.row((index + 2) % 3))); //Mt[index] = glm::cross(Mt[(index + 1) % 3], Mt[(index + 2) % 3]);
364 MadjTt.setRow((index + 1) % 3, Mt.row((index + 2) % 3).cross(Mt.row((index) % 3))); //MadjTt[(index + 1) % 3] = glm::cross(Mt[(index + 2) % 3], Mt[(index) % 3]);;
365 MadjTt.setRow((index + 2) % 3, Mt.row((index) % 3).cross(Mt.row((index + 1) % 3))); //MadjTt[(index + 2) % 3] = glm::cross(Mt[(index) % 3], Mt[(index + 1) % 3]);
366 SquareMatrix<Real, 3> M2 = Mt.transpose();
367 Mone = M2.oneNorm();
368 Minf = M2.infNorm();
369 det = Mt(0, 0) * MadjTt(0, 0) + Mt(1, 0) * MadjTt(1, 0) + Mt(2, 0) * MadjTt(2, 0);
370 }
371 }
372
373 const Real MadjTone = MadjTt.oneNorm();
374 const Real MadjTinf = MadjTt.infNorm();
375
376 const Real gamma = sqrt(sqrt((MadjTone*MadjTinf) / (Mone*Minf)) / fabs(det));
377
378 const Real g1 = gamma*0.5f;
379 const Real g2 = 0.5f / (gamma*det);
380
381 for (unsigned char i = 0; i < 3; i++)
382 {
383 for (unsigned char j = 0; j < 3; j++)
384 {
385 Et(i, j) = Mt(i, j);
386 Mt(i, j) = g1*Mt(i, j) + g2*MadjTt(i, j);
387 Et(i, j) -= Mt(i, j);
388 }
389 }
390
391 Eone = Et.oneNorm();
392
393 Mone = Mt.oneNorm();
394 Minf = Mt.infNorm();
395 } while (Eone > Mone * tolerance);
396
397 // Q = Mt^T
398 R = Mt.transpose();
399 }
400}
Real
double Real
Definition Typedef.inl:23
V
#define V(a, b, c)
dyno
This is an implementation of AdditiveCCD based on peridyno.
Definition Array.h:25
dyno::EigenDecomposition
DYN_FUNC void EigenDecomposition(const SquareMatrix< Real, 3 > &A, SquareMatrix< Real, 3 > &eigenVecs, Vector< Real, 3 > &eigenVals)
Definition MatrixFunc.inl:46
dyno::QRDecomposition
DYN_FUNC void QRDecomposition(SquareMatrix< Real, 3 > &A, SquareMatrix< Real, 3 > &R, int p, int q)
Definition MatrixFunc.inl:78
dyno::jacobiRotate
DYN_FUNC void jacobiRotate(SquareMatrix< Real, 3 > &A, SquareMatrix< Real, 3 > &R, int p, int q)
Definition MatrixFunc.inl:11
dyno::polarDecomposition
DYN_FUNC void polarDecomposition(const SquareMatrix< Real, Dim > &A, SquareMatrix< Real, Dim > &R, SquareMatrix< Real, Dim > &U, SquareMatrix< Real, Dim > &D)
dyno::sqrt
DYN_FUNC Complex< Real > sqrt(const Complex< Real > &)
Definition Complex.inl:321
svd3_cuda.h
svd
__host__ __forceinline__ void svd(float a11, float a12, float a13, float a21, float a22, float a23, float a31, float a32, float a33, float &u11, float &u12, float &u13, float &u21, float &u22, float &u23, float &u31, float &u32, float &u33, float &s11, float &s22, float &s33, float &v11, float &v12, float &v13, float &v21, float &v22, float &v23, float &v31, float &v32, float &v33)
Definition svd3_cuda.h:54
max
#define max(x, y)
Definition svd3_cuda.h:41
M
#define M(X, Y)
Definition vgMath.h:291
Generated by doxygen 1.13.2