diff --git a/include/bounce/cloth/cloth_solver.h b/include/bounce/cloth/cloth_solver.h index 2df48a2..7eb4229 100644 --- a/include/bounce/cloth/cloth_solver.h +++ b/include/bounce/cloth/cloth_solver.h @@ -87,7 +87,9 @@ private: void ApplyConstraints(); // Solve Ax = b. - void Solve(b3DenseVec3& x, u32& iterations, const b3SparseSymMat33View& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const; + void SolveMPCG(b3DenseVec3& x, + const b3SparseSymMat33View& A, const b3DenseVec3& b, + const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y, u32 maxIterations = 30) const; b3StackAllocator* m_allocator; diff --git a/include/bounce/softbody/sparse_mat33.h b/include/bounce/cloth/sparse_mat33.h similarity index 94% rename from include/bounce/softbody/sparse_mat33.h rename to include/bounce/cloth/sparse_mat33.h index a746849..273b7ad 100644 --- a/include/bounce/softbody/sparse_mat33.h +++ b/include/bounce/cloth/sparse_mat33.h @@ -22,7 +22,36 @@ #include #include #include -#include + +// An element in a sparse matrix. +struct b3RowValue +{ + u32 column; + b3Mat33 value; + b3RowValue* next; +}; + +// Singly linked list of row elements. +struct b3RowValueList +{ + b3RowValueList() + { + head = nullptr; + count = 0; + } + + ~b3RowValueList() { } + + void PushFront(b3RowValue* link) + { + link->next = head; + head = link; + ++count; + } + + b3RowValue* head; + u32 count; +}; // A sparse matrix. // Each row is a list of non-zero elements in the row. diff --git a/include/bounce/cloth/sparse_mat33_view.h b/include/bounce/cloth/sparse_mat33_view.h new file mode 100644 index 0000000..9abb275 --- /dev/null +++ b/include/bounce/cloth/sparse_mat33_view.h @@ -0,0 +1,137 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B3_SPARSE_MAT_33_VIEW_H +#define B3_SPARSE_MAT_33_VIEW_H + +#include + +struct b3ArrayRowValue +{ + u32 column; + b3Mat33 value; +}; + +struct b3RowValueArray +{ + u32 count; + b3ArrayRowValue* values; +}; + +// A read-only sparse matrix. +struct b3SparseMat33View +{ + // + b3SparseMat33View(const b3SparseMat33& _m); + + // + ~b3SparseMat33View(); + + // + const b3Mat33& operator()(u32 i, u32 j) const; + + u32 rowCount; + b3RowValueArray* rows; +}; + +inline b3SparseMat33View::b3SparseMat33View(const b3SparseMat33& _m) +{ + rowCount = _m.rowCount; + rows = (b3RowValueArray*)b3Alloc(rowCount * sizeof(b3RowValueArray)); + for (u32 i = 0; i < _m.rowCount; ++i) + { + b3RowValueList* rowList = _m.rows + i; + b3RowValueArray* rowArray = rows + i; + + rowArray->count = rowList->count; + rowArray->values = (b3ArrayRowValue*)b3Alloc(rowArray->count * sizeof(b3ArrayRowValue)); + + u32 valueIndex = 0; + for (b3RowValue* v = rowList->head; v; v = v->next) + { + rowArray->values[valueIndex].column = v->column; + rowArray->values[valueIndex].value = v->value; + ++valueIndex; + } + } +} + +inline b3SparseMat33View::~b3SparseMat33View() +{ + for (u32 i = 0; i < rowCount; ++i) + { + b3RowValueArray* rowArray = rows + i; + b3Free(rowArray->values); + } + b3Free(rows); +} + +inline const b3Mat33& b3SparseMat33View::operator()(u32 i, u32 j) const +{ + B3_ASSERT(i < rowCount); + B3_ASSERT(j < rowCount); + + if (i > j) + { + b3Swap(i, j); + } + + b3RowValueArray* vs = rows + i; + + for (u32 c = 0; c < vs->count; ++c) + { + b3ArrayRowValue* rv = vs->values + c; + if (rv->column == j) + { + return rv->value; + } + } + + return b3Mat33_zero; +} + +inline void b3Mul(b3DenseVec3& out, const b3SparseMat33View& A, const b3DenseVec3& v) +{ + B3_ASSERT(A.rowCount == out.n); + + out.SetZero(); + + for (u32 i = 0; i < A.rowCount; ++i) + { + b3RowValueArray* rowArray = A.rows + i; + + for (u32 c = 0; c < rowArray->count; ++c) + { + b3ArrayRowValue* rv = rowArray->values + c; + + u32 j = rv->column; + b3Mat33 a = rv->value; + + out[i] += a * v[j]; + } + } +} + +inline b3DenseVec3 operator*(const b3SparseMat33View& A, const b3DenseVec3& v) +{ + b3DenseVec3 result(v.n); + b3Mul(result, A, v); + return result; +} + +#endif diff --git a/include/bounce/cloth/sparse_sym_mat33.h b/include/bounce/cloth/sparse_sym_mat33.h index 9d36f35..c8af4a2 100644 --- a/include/bounce/cloth/sparse_sym_mat33.h +++ b/include/bounce/cloth/sparse_sym_mat33.h @@ -19,44 +19,14 @@ #ifndef B3_SPARSE_SYM_MAT_33_H #define B3_SPARSE_SYM_MAT_33_H -#include -#include -#include - -// An element in a sparse symmetric matrix. -struct b3RowValue -{ - u32 column; - b3Mat33 value; - b3RowValue* next; -}; - -// Doubly linked list of row elements. -struct b3RowValueList -{ - b3RowValueList() - { - head = nullptr; - count = 0; - } - - ~b3RowValueList() { } - - void PushFront(b3RowValue* link) - { - link->next = head; - head = link; - ++count; - } - - b3RowValue* head; - u32 count; -}; +#include // A sparse symmetric matrix. // Each row is a list of non-zero elements in the row. // The total matrix capacity is bounded by // M * (M + 1) / 2 +// You must write only the upper triangle elements of the original +// matrix to this matrix because a_ji = a_ij. struct b3SparseSymMat33 { // diff --git a/include/bounce/cloth/sparse_sym_mat33_view.h b/include/bounce/cloth/sparse_sym_mat33_view.h index 48a3858..269d4dc 100644 --- a/include/bounce/cloth/sparse_sym_mat33_view.h +++ b/include/bounce/cloth/sparse_sym_mat33_view.h @@ -19,20 +19,9 @@ #ifndef B3_SPARSE_SYM_MAT_33_VIEW_H #define B3_SPARSE_SYM_MAT_33_VIEW_H +#include #include -struct b3ArrayRowValue -{ - u32 column; - b3Mat33 value; -}; - -struct b3RowValueArray -{ - u32 count; - b3ArrayRowValue* values; -}; - // A read-only sparse symmetric matrix. struct b3SparseSymMat33View { @@ -45,9 +34,6 @@ struct b3SparseSymMat33View // const b3Mat33& operator()(u32 i, u32 j) const; - // - void Diagonal(b3DiagMat33& out) const; - u32 rowCount; b3RowValueArray* rows; }; @@ -99,6 +85,7 @@ inline const b3Mat33& b3SparseSymMat33View::operator()(u32 i, u32 j) const for (u32 c = 0; c < vs->count; ++c) { b3ArrayRowValue* rv = vs->values + c; + if (rv->column == j) { return rv->value; @@ -108,16 +95,6 @@ inline const b3Mat33& b3SparseSymMat33View::operator()(u32 i, u32 j) const return b3Mat33_zero; } -inline void b3SparseSymMat33View::Diagonal(b3DiagMat33& out) const -{ - B3_ASSERT(rowCount == out.n); - - for (u32 i = 0; i < rowCount; ++i) - { - out[i] = (*this)(i, i); - } -} - inline void b3Mul(b3DenseVec3& out, const b3SparseSymMat33View& A, const b3DenseVec3& v) { B3_ASSERT(A.rowCount == out.n); @@ -152,4 +129,4 @@ inline b3DenseVec3 operator*(const b3SparseSymMat33View& A, const b3DenseVec3& v return result; } -#endif +#endif \ No newline at end of file diff --git a/src/bounce/cloth/cloth_solver.cpp b/src/bounce/cloth/cloth_solver.cpp index 29269f3..632f04e 100644 --- a/src/bounce/cloth/cloth_solver.cpp +++ b/src/bounce/cloth/cloth_solver.cpp @@ -287,9 +287,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) // x b3DenseVec3 x(m_particleCount); - u32 iterations = 0; - Solve(x, iterations, viewA, b, S, z, sx0); - b3_clothSolverIterations = iterations; + SolveMPCG(x, viewA, b, S, z, sx0); #if 0 // Copy constraint forces to the contacts b3DenseVec3 f = A * x - b; @@ -398,17 +396,21 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) } } -void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations, - const b3SparseSymMat33View& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const +void b3ClothSolver::SolveMPCG(b3DenseVec3& x, + const b3SparseSymMat33View& A, const b3DenseVec3& b, + const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y, u32 maxIterations) const { B3_PROFILE("Cloth Solve Ax = b"); // P = diag(A) - b3DiagMat33 inv_P(m_particleCount); - A.Diagonal(inv_P); + b3DiagMat33 inv_P(A.rowCount); + for (u32 i = 0; i < A.rowCount; ++i) + { + inv_P[i] = A(i, i); + } // Invert - for (u32 i = 0; i < m_particleCount; ++i) + for (u32 i = 0; i < inv_P.n; ++i) { b3Mat33& D = inv_P[i]; @@ -427,81 +429,53 @@ void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations, D = b3Diagonal(xx, yy, zz); } - // I b3DiagMat33 I(m_particleCount); I.SetIdentity(); - // x = S * y + (I - S) * z x = (S * y) + (I - S) * z; - // b^ = S * (b - A * (I - S) * z) b3DenseVec3 b_hat = S * (b - A * ((I - S) * z)); - // b_delta = dot(b^, P^-1 * b_^) float32 b_delta = b3Dot(b_hat, inv_P * b_hat); - // r = S * (b - A * x) b3DenseVec3 r = S * (b - A * x); - // p = S * (P^-1 * r) b3DenseVec3 p = S * (inv_P * r); - // delta_new = dot(r, p) float32 delta_new = b3Dot(r, p); - // Set the tolerance. - const float32 tolerance = 2.0f * B3_EPSILON; - - // Maximum number of iterations. - // Stop at this iteration if diverged. - const u32 max_iterations = 50; - u32 iteration = 0; - - // Main iteration loop. for (;;) { - // Divergence check. - if (iteration >= max_iterations) + if (iteration >= maxIterations) + { + break; + } + + if (delta_new <= B3_EPSILON * B3_EPSILON * b_delta) { break; } - // Convergence check. - if (delta_new <= tolerance * tolerance * b_delta) - { - break; - } - - // s = S * (A * p) b3DenseVec3 s = S * (A * p); - // alpha = delta_new / dot(p, s) float32 alpha = delta_new / b3Dot(p, s); - // x = x + alpha * p x = x + alpha * p; - - // r = r - alpha * s r = r - alpha * s; - // h = inv_P * r b3DenseVec3 h = inv_P * r; - // delta_old = delta_new float32 delta_old = delta_new; - // delta_new = dot(r, h) delta_new = b3Dot(r, h); - // beta = delta_new / delta_old float32 beta = delta_new / delta_old; - // p = S * (h + beta * p) p = S * (h + beta * p); ++iteration; } - iterations = iteration; + b3_clothSolverIterations = iteration; } \ No newline at end of file diff --git a/src/bounce/softbody/softbody_solver.cpp b/src/bounce/softbody/softbody_solver.cpp index 973586e..9907e7a 100644 --- a/src/bounce/softbody/softbody_solver.cpp +++ b/src/bounce/softbody/softbody_solver.cpp @@ -20,12 +20,14 @@ #include #include #include -#include #include #include #include +#include +#include + // This work is based on the paper "Interactive Virtual Materials" written by // Matthias Mueller Fischer // The paper is available here: @@ -91,9 +93,11 @@ static void b3ExtractRotation(b3Mat33& out, b3Quat& q, const b3Mat33& A, u32 max } static void b3SolveMPCG(b3DenseVec3& x, - const b3SparseMat33& A, const b3DenseVec3& b, + const b3SparseMat33View& A, const b3DenseVec3& b, const b3DenseVec3& z, const b3DiagMat33& S, u32 maxIterations = 20) { + B3_PROFILE("Soft Body Solve Ax = b"); + b3DiagMat33 P(A.rowCount); b3DiagMat33 invP(A.rowCount); for (u32 i = 0; i < A.rowCount; ++i) @@ -377,12 +381,14 @@ void b3SoftBodySolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIter f0 = -f0; b3SparseMat33 A = M + h * C + h * h * K; + + b3SparseMat33View viewA(A); b3DenseVec3 b = M * v - h * (K * p + f0 - (f_plastic + fe)); // Solve Ax = b b3DenseVec3 sx(m_mesh->vertexCount); - b3SolveMPCG(sx, A, b, z, S); + b3SolveMPCG(sx, viewA, b, z, S); // Solve constraints b3SoftBodyContactSolverDef contactSolverDef;