From 625310be71cc9f2d3de76b2849e42d9c63547429 Mon Sep 17 00:00:00 2001 From: Irlan Date: Wed, 19 Jun 2019 12:39:28 -0300 Subject: [PATCH] Split the solvers into force solver and contact solver --- include/bounce/cloth/cloth_contact_solver.h | 10 +- include/bounce/cloth/cloth_force_solver.h | 89 +++++ include/bounce/cloth/cloth_solver.h | 41 +- include/bounce/cloth/force.h | 6 +- include/bounce/cloth/particle.h | 1 + include/bounce/cloth/spring_force.h | 2 +- src/bounce/cloth/cloth_contact_solver.cpp | 88 ++--- src/bounce/cloth/cloth_force_solver.cpp | 293 ++++++++++++++ src/bounce/cloth/cloth_solver.cpp | 401 ++++---------------- src/bounce/cloth/spring_force.cpp | 4 +- 10 files changed, 511 insertions(+), 424 deletions(-) create mode 100644 include/bounce/cloth/cloth_force_solver.h create mode 100644 src/bounce/cloth/cloth_force_solver.cpp diff --git a/include/bounce/cloth/cloth_contact_solver.h b/include/bounce/cloth/cloth_contact_solver.h index 5d69900..76f13bc 100644 --- a/include/bounce/cloth/cloth_contact_solver.h +++ b/include/bounce/cloth/cloth_contact_solver.h @@ -30,8 +30,6 @@ class b3Body; struct b3ParticleBodyContact; class b3ParticleTriangleContact; -struct b3DenseVec3; - struct b3ClothSolverBodyContactVelocityConstraint { u32 indexA; @@ -130,8 +128,8 @@ struct b3ClothContactSolverDef { b3StackAllocator* allocator; - b3DenseVec3* positions; - b3DenseVec3* velocities; + b3Vec3* positions; + b3Vec3* velocities; u32 bodyContactCount; b3ParticleBodyContact** bodyContacts; @@ -167,8 +165,8 @@ public: protected: b3StackAllocator* m_allocator; - b3DenseVec3* m_positions; - b3DenseVec3* m_velocities; + b3Vec3* m_positions; + b3Vec3* m_velocities; u32 m_bodyContactCount; b3ParticleBodyContact** m_bodyContacts; diff --git a/include/bounce/cloth/cloth_force_solver.h b/include/bounce/cloth/cloth_force_solver.h new file mode 100644 index 0000000..763e3cc --- /dev/null +++ b/include/bounce/cloth/cloth_force_solver.h @@ -0,0 +1,89 @@ +/* +* 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_CLOTH_FORCE_SOLVER_H +#define B3_CLOTH_FORCE_SOLVER_H + +#include +#include + +class b3StackAllocator; + +class b3Particle; +class b3Force; + +struct b3DenseVec3; +struct b3DiagMat33; +struct b3SparseSymMat33; +struct b3SparseSymMat33View; + +struct b3ClothForceSolverDef +{ + b3StackAllocator* stack; + u32 particleCount; + b3Particle** particles; + u32 forceCount; + b3Force** forces; +}; + +struct b3ClothForceSolverData +{ + b3DenseVec3* x; + b3DenseVec3* v; + b3DenseVec3* f; + b3DenseVec3* y; + b3SparseSymMat33* dfdx; + b3SparseSymMat33* dfdv; + b3DiagMat33* S; + b3DenseVec3* z; +}; + +struct b3AccelerationConstraint +{ + u32 i1; + u32 ndof; + b3Vec3 p, q, z; + + void Apply(const b3ClothForceSolverData* data); +}; + +class b3ClothForceSolver +{ +public: + b3ClothForceSolver(const b3ClothForceSolverDef& def); + ~b3ClothForceSolver(); + + void Solve(float32 dt, const b3Vec3& gravity); +private: + void ApplyForces(); + void ApplyConstraints(); + + b3StackAllocator* m_allocator; + + u32 m_particleCount; + b3Particle** m_particles; + + u32 m_forceCount; + b3Force** m_forces; + + b3AccelerationConstraint* m_constraints; + + b3ClothForceSolverData m_solverData; +}; + +#endif \ No newline at end of file diff --git a/include/bounce/cloth/cloth_solver.h b/include/bounce/cloth/cloth_solver.h index 85ed648..2edf011 100644 --- a/include/bounce/cloth/cloth_solver.h +++ b/include/bounce/cloth/cloth_solver.h @@ -25,14 +25,7 @@ class b3StackAllocator; class b3Particle; -class b3Body; class b3Force; - -struct b3DenseVec3; -struct b3DiagMat33; -struct b3SparseSymMat33; -struct b3SparseSymMat33View; - struct b3ParticleBodyContact; class b3ParticleTriangleContact; @@ -45,29 +38,6 @@ struct b3ClothSolverDef u32 triangleContactCapacity; }; -struct b3ClothSolverData -{ - b3DenseVec3* x; - b3DenseVec3* v; - b3DenseVec3* f; - b3DenseVec3* y; - b3SparseSymMat33* dfdx; - b3SparseSymMat33* dfdv; - b3DiagMat33* S; - b3DenseVec3* z; - float32 dt; - float32 invdt; -}; - -struct b3AccelerationConstraint -{ - u32 i1; - u32 ndof; - b3Vec3 p, q, z; - - void Apply(const b3ClothSolverData* data); -}; - class b3ClothSolver { public: @@ -81,9 +51,6 @@ public: void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations); private: - void ApplyForces(); - void ApplyConstraints(); - b3StackAllocator* m_allocator; u32 m_particleCapacity; @@ -93,11 +60,7 @@ private: u32 m_forceCapacity; u32 m_forceCount; b3Force** m_forces; - - u32 m_constraintCapacity; - u32 m_constraintCount; - b3AccelerationConstraint* m_constraints; - + u32 m_bodyContactCapacity; u32 m_bodyContactCount; b3ParticleBodyContact** m_bodyContacts; @@ -105,8 +68,6 @@ private: u32 m_triangleContactCapacity; u32 m_triangleContactCount; b3ParticleTriangleContact** m_triangleContacts; - - b3ClothSolverData m_solverData; }; #endif \ No newline at end of file diff --git a/include/bounce/cloth/force.h b/include/bounce/cloth/force.h index 9e9132f..392e0ef 100644 --- a/include/bounce/cloth/force.h +++ b/include/bounce/cloth/force.h @@ -22,7 +22,7 @@ #include #include -struct b3ClothSolverData; +struct b3ClothForceSolverData; class b3Particle; @@ -52,7 +52,7 @@ public: protected: friend class b3List2; friend class b3Cloth; - friend class b3ClothSolver; + friend class b3ClothForceSolver; friend class b3Particle; static b3Force* Create(const b3ForceDef* def); @@ -61,7 +61,7 @@ protected: b3Force() { } virtual ~b3Force() { } - virtual void Apply(const b3ClothSolverData* data) = 0; + virtual void Apply(const b3ClothForceSolverData* data) = 0; // Force type b3ForceType m_type; diff --git a/include/bounce/cloth/particle.h b/include/bounce/cloth/particle.h index ce909e2..6844e28 100644 --- a/include/bounce/cloth/particle.h +++ b/include/bounce/cloth/particle.h @@ -160,6 +160,7 @@ private: friend class b3Cloth; friend class b3ClothTriangle; friend class b3ClothSolver; + friend class b3ClothForceSolver; friend class b3ClothContactManager; friend class b3ParticleTriangleContact; friend class b3ClothContactSolver; diff --git a/include/bounce/cloth/spring_force.h b/include/bounce/cloth/spring_force.h index f7da89f..fa01405 100644 --- a/include/bounce/cloth/spring_force.h +++ b/include/bounce/cloth/spring_force.h @@ -76,7 +76,7 @@ private: b3SpringForce(const b3SpringForceDef* def); ~b3SpringForce(); - void Apply(const b3ClothSolverData* data); + void Apply(const b3ClothForceSolverData* data); // Solver shared diff --git a/src/bounce/cloth/cloth_contact_solver.cpp b/src/bounce/cloth/cloth_contact_solver.cpp index d736e7c..d6ef10c 100644 --- a/src/bounce/cloth/cloth_contact_solver.cpp +++ b/src/bounce/cloth/cloth_contact_solver.cpp @@ -20,7 +20,6 @@ #include #include #include -#include #include #include #include @@ -54,9 +53,6 @@ b3ClothContactSolver::~b3ClothContactSolver() void b3ClothContactSolver::InitializeBodyContactConstraints() { - b3DenseVec3& x = *m_positions; - b3DenseVec3& v = *m_velocities; - for (u32 i = 0; i < m_bodyContactCount; ++i) { b3ParticleBodyContact* c = m_bodyContacts[i]; @@ -109,7 +105,7 @@ void b3ClothContactSolver::InitializeBodyContactConstraints() b3Mat33 iA = vc->invIA; b3Mat33 iB = vc->invIB; - b3Vec3 xA = x[indexA]; + b3Vec3 xA = m_positions[indexA]; b3Vec3 xB = bodyB->m_sweep.worldCenter; b3Quat qA; qA.SetIdentity(); @@ -182,8 +178,6 @@ void b3ClothContactSolver::InitializeBodyContactConstraints() } void b3ClothContactSolver::InitializeTriangleContactConstraints() { - b3DenseVec3& x = *m_positions; - for (u32 i = 0; i < m_triangleContactCount; ++i) { b3ParticleTriangleContact* c = m_triangleContacts[i]; @@ -233,10 +227,10 @@ void b3ClothContactSolver::InitializeTriangleContactConstraints() u32 indexD = pc->indexD; float32 mD = pc->invMassD; - b3Vec3 xA = x[indexA]; - b3Vec3 xB = x[indexB]; - b3Vec3 xC = x[indexC]; - b3Vec3 xD = x[indexD]; + b3Vec3 xA = m_positions[indexA]; + b3Vec3 xB = m_positions[indexB]; + b3Vec3 xC = m_positions[indexC]; + b3Vec3 xD = m_positions[indexD]; float32 wB = pc->wB; float32 wC = pc->wC; @@ -269,8 +263,6 @@ void b3ClothContactSolver::InitializeTriangleContactConstraints() void b3ClothContactSolver::WarmStartBodyContactConstraints() { - b3DenseVec3& v = *m_velocities; - for (u32 i = 0; i < m_bodyContactCount; ++i) { b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; @@ -278,7 +270,7 @@ void b3ClothContactSolver::WarmStartBodyContactConstraints() u32 indexA = vc->indexA; b3Body* bodyB = vc->bodyB; - b3Vec3 vA = v[indexA]; + b3Vec3 vA = m_velocities[indexA]; b3Vec3 vB = bodyB->GetLinearVelocity(); b3Vec3 wA; wA.SetZero(); @@ -307,7 +299,7 @@ void b3ClothContactSolver::WarmStartBodyContactConstraints() vB += mB * (P1 + P2); wB += iB * b3Cross(vc->rB, P1 + P2); - v[indexA] = vA; + m_velocities[indexA] = vA; bodyB->SetLinearVelocity(vB); bodyB->SetAngularVelocity(wB); @@ -316,8 +308,6 @@ void b3ClothContactSolver::WarmStartBodyContactConstraints() void b3ClothContactSolver::WarmStartTriangleContactConstraints() { - b3DenseVec3& v = *m_velocities; - for (u32 i = 0; i < m_triangleContactCount; ++i) { b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i; @@ -327,10 +317,10 @@ void b3ClothContactSolver::WarmStartTriangleContactConstraints() u32 indexC = vc->indexC; u32 indexD = vc->indexD; - b3Vec3 vA = v[indexA]; - b3Vec3 vB = v[indexB]; - b3Vec3 vC = v[indexC]; - b3Vec3 vD = v[indexD]; + b3Vec3 vA = m_velocities[indexA]; + b3Vec3 vB = m_velocities[indexB]; + b3Vec3 vC = m_velocities[indexC]; + b3Vec3 vD = m_velocities[indexD]; float32 mA = vc->invMassA; float32 mB = vc->invMassB; @@ -388,17 +378,15 @@ void b3ClothContactSolver::WarmStartTriangleContactConstraints() vD += mD * PD; } - v[indexA] = vA; - v[indexB] = vB; - v[indexC] = vC; - v[indexD] = vD; + m_velocities[indexA] = vA; + m_velocities[indexB] = vB; + m_velocities[indexC] = vC; + m_velocities[indexD] = vD; } } void b3ClothContactSolver::SolveBodyContactVelocityConstraints() { - b3DenseVec3& v = *m_velocities; - for (u32 i = 0; i < m_bodyContactCount; ++i) { b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; @@ -406,7 +394,7 @@ void b3ClothContactSolver::SolveBodyContactVelocityConstraints() u32 indexA = vc->indexA; b3Body* bodyB = vc->bodyB; - b3Vec3 vA = v[indexA]; + b3Vec3 vA = m_velocities[indexA]; b3Vec3 vB = bodyB->GetLinearVelocity(); b3Vec3 wA; wA.SetZero(); @@ -476,7 +464,7 @@ void b3ClothContactSolver::SolveBodyContactVelocityConstraints() wB += iB * b3Cross(rB, P); } - v[indexA] = vA; + m_velocities[indexA] = vA; bodyB->SetLinearVelocity(vB); bodyB->SetAngularVelocity(wB); @@ -485,8 +473,6 @@ void b3ClothContactSolver::SolveBodyContactVelocityConstraints() void b3ClothContactSolver::SolveTriangleContactVelocityConstraints() { - b3DenseVec3& v = *m_velocities; - for (u32 i = 0; i < m_triangleContactCount; ++i) { b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i; @@ -507,10 +493,10 @@ void b3ClothContactSolver::SolveTriangleContactVelocityConstraints() float32 wC = vc->wC; float32 wD = vc->wD; - b3Vec3 vA = v[indexA]; - b3Vec3 vB = v[indexB]; - b3Vec3 vC = v[indexC]; - b3Vec3 vD = v[indexD]; + b3Vec3 vA = m_velocities[indexA]; + b3Vec3 vB = m_velocities[indexB]; + b3Vec3 vC = m_velocities[indexC]; + b3Vec3 vD = m_velocities[indexD]; // Solve normal constraint. { @@ -602,10 +588,10 @@ void b3ClothContactSolver::SolveTriangleContactVelocityConstraints() vD += mD * PD; } - v[indexA] = vA; - v[indexB] = vB; - v[indexC] = vC; - v[indexD] = vD; + m_velocities[indexA] = vA; + m_velocities[indexB] = vB; + m_velocities[indexC] = vC; + m_velocities[indexD] = vD; } } @@ -658,8 +644,6 @@ struct b3ClothSolverBodyContactSolverPoint bool b3ClothContactSolver::SolveBodyContactPositionConstraints() { - b3DenseVec3& x = *m_positions; - float32 minSeparation = 0.0f; for (u32 i = 0; i < m_bodyContactCount; ++i) @@ -676,7 +660,7 @@ bool b3ClothContactSolver::SolveBodyContactPositionConstraints() b3Mat33 iB = pc->invIB; b3Vec3 localCenterB = pc->localCenterB; - b3Vec3 cA = x[indexA]; + b3Vec3 cA = m_positions[indexA]; b3Quat qA; qA.SetIdentity(); b3Vec3 cB = bodyB->m_sweep.worldCenter; @@ -724,7 +708,7 @@ bool b3ClothContactSolver::SolveBodyContactPositionConstraints() qB += b3Derivative(qB, iB * b3Cross(rB, P)); qB.Normalize(); - x[indexA] = cA; + m_positions[indexA] = cA; bodyB->m_sweep.worldCenter = cB; bodyB->m_sweep.orientation = qB; @@ -735,8 +719,6 @@ bool b3ClothContactSolver::SolveBodyContactPositionConstraints() bool b3ClothContactSolver::SolveTriangleContactPositionConstraints() { - b3DenseVec3& x = *m_positions; - float32 minSeparation = 0.0f; for (u32 i = 0; i < m_triangleContactCount; ++i) @@ -762,10 +744,10 @@ bool b3ClothContactSolver::SolveTriangleContactPositionConstraints() float32 wC = pc->wC; float32 wD = pc->wD; - b3Vec3 xA = x[indexA]; - b3Vec3 xB = x[indexB]; - b3Vec3 xC = x[indexC]; - b3Vec3 xD = x[indexD]; + b3Vec3 xA = m_positions[indexA]; + b3Vec3 xB = m_positions[indexB]; + b3Vec3 xC = m_positions[indexC]; + b3Vec3 xD = m_positions[indexD]; b3Vec3 cB = wB * xB + wC * xC + wD * xD; @@ -802,10 +784,10 @@ bool b3ClothContactSolver::SolveTriangleContactPositionConstraints() xC += mC * PC; xD += mD * PD; - x[indexA] = xA; - x[indexB] = xB; - x[indexC] = xC; - x[indexD] = xD; + m_positions[indexA] = xA; + m_positions[indexB] = xB; + m_positions[indexC] = xC; + m_positions[indexD] = xD; } return minSeparation >= -3.0f * B3_LINEAR_SLOP; diff --git a/src/bounce/cloth/cloth_force_solver.cpp b/src/bounce/cloth/cloth_force_solver.cpp new file mode 100644 index 0000000..0dd373e --- /dev/null +++ b/src/bounce/cloth/cloth_force_solver.cpp @@ -0,0 +1,293 @@ +/* +* 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. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include + +// Here, we solve Ax = b using the Modified Preconditioned Conjugate Gradient (MPCG) algorithm. +// described in the paper: +// "Large Steps in Cloth Simulation - David Baraff, Andrew Witkin". + +// Some improvements for the original MPCG algorithm are described in the paper: +// "On the modified conjugate gradient method in cloth simulation - Uri M. Ascher, Eddy Boxerman". + +u32 b3_clothSolverIterations = 0; + +b3ClothForceSolver::b3ClothForceSolver(const b3ClothForceSolverDef& def) +{ + m_allocator = def.stack; + + m_particleCount = def.particleCount; + m_particles = def.particles; + + m_forceCount = def.forceCount; + m_forces = def.forces; + + m_constraints = (b3AccelerationConstraint*)m_allocator->Allocate(m_particleCount * sizeof(b3AccelerationConstraint)); +} + +b3ClothForceSolver::~b3ClothForceSolver() +{ + m_allocator->Free(m_constraints); +} + +void b3ClothForceSolver::ApplyForces() +{ + for (u32 i = 0; i < m_forceCount; ++i) + { + m_forces[i]->Apply(&m_solverData); + } +} + +void b3AccelerationConstraint::Apply(const b3ClothForceSolverData* data) +{ + b3DiagMat33& sS = *data->S; + b3DenseVec3& sz = *data->z; + + sz[i1] = z; + + b3Mat33 I; I.SetIdentity(); + + switch (ndof) + { + case 3: + { + sS[i1] = I; + break; + } + case 2: + { + sS[i1] = I - b3Outer(p, p); + break; + } + case 1: + { + sS[i1] = I - b3Outer(p, p) - b3Outer(q, q); + break; + } + case 0: + { + sS[i1].SetZero(); + break; + } + default: + { + B3_ASSERT(false); + break; + } + } +} + +void b3ClothForceSolver::ApplyConstraints() +{ + for (u32 i = 0; i < m_particleCount; ++i) + { + b3Particle* p = m_particles[i]; + b3AccelerationConstraint* c = m_constraints + i; + + c->i1 = i; + + if (p->m_type != e_dynamicParticle) + { + c->ndof = 0; + c->z.SetZero(); + } + else + { + c->ndof = 3; + c->z.SetZero(); + } + } + + for (u32 i = 0; i < m_particleCount; ++i) + { + m_constraints[i].Apply(&m_solverData); + } +} + +// Solve Ax = b +static void b3SolveMPCG(b3DenseVec3& x, + const b3SparseSymMat33View& A, const b3DenseVec3& b, + const b3DiagMat33& S, const b3DenseVec3& z, + const b3DenseVec3& y, const b3DiagMat33& I, u32 maxIterations = 20) +{ + B3_PROFILE("Cloth Solve MPCG"); + + // Jacobi preconditioner + // P = diag(A) + b3DiagMat33 inv_P(A.rowCount); + for (u32 i = 0; i < A.rowCount; ++i) + { + b3Mat33 a = A(i, i); + + // Sylvester Criterion to ensure PD-ness + B3_ASSERT(b3Det(a.x, a.y, a.z) > 0.0f); + + B3_ASSERT(a.x.x > 0.0f); + float32 xx = 1.0f / a.x.x; + + B3_ASSERT(a.y.y > 0.0f); + float32 yy = 1.0f / a.y.y; + + B3_ASSERT(a.z.z > 0.0f); + float32 zz = 1.0f / a.z.z; + + inv_P[i] = b3Diagonal(xx, yy, zz); + } + + x = (S * y) + (I - S) * z; + + b3DenseVec3 b_hat = S * (b - A * ((I - S) * z)); + + float32 b_delta = b3Dot(b_hat, inv_P * b_hat); + + b3DenseVec3 r = S * (b - A * x); + + b3DenseVec3 p = S * (inv_P * r); + + float32 delta_new = b3Dot(r, p); + + u32 iteration = 0; + for (;;) + { + if (iteration == maxIterations) + { + break; + } + + if (delta_new <= B3_EPSILON * B3_EPSILON * b_delta) + { + break; + } + + b3DenseVec3 s = S * (A * p); + + float32 alpha = delta_new / b3Dot(p, s); + + x = x + alpha * p; + r = r - alpha * s; + + b3DenseVec3 h = inv_P * r; + + float32 delta_old = delta_new; + + delta_new = b3Dot(r, h); + + float32 beta = delta_new / delta_old; + + p = S * (h + beta * p); + + ++iteration; + } + + b3_clothSolverIterations = iteration; +} + +void b3ClothForceSolver::Solve(float32 dt, const b3Vec3& gravity) +{ + float32 h = dt; + + b3DenseVec3 sx(m_particleCount); + b3DenseVec3 sv(m_particleCount); + b3DenseVec3 sf(m_particleCount); + b3DenseVec3 sy(m_particleCount); + b3DenseVec3 sz(m_particleCount); + b3DenseVec3 sx0(m_particleCount); + b3SparseSymMat33 dfdx(m_particleCount); + b3SparseSymMat33 dfdv(m_particleCount); + b3DiagMat33 S(m_particleCount); + + m_solverData.x = &sx; + m_solverData.v = &sv; + m_solverData.f = &sf; + m_solverData.y = &sy; + m_solverData.z = &sz; + m_solverData.dfdx = &dfdx; + m_solverData.dfdv = &dfdv; + m_solverData.S = &S; + + for (u32 i = 0; i < m_particleCount; ++i) + { + b3Particle* p = m_particles[i]; + + sx[i] = p->m_position; + sv[i] = p->m_velocity; + sf[i] = p->m_force; + + if (p->m_type == e_dynamicParticle) + { + // Apply weight + sf[i] += p->m_mass * gravity; + } + + sy[i] = p->m_translation; + sx0[i] = p->m_x; + } + + // Apply internal forces + ApplyForces(); + + // Apply constraints + ApplyConstraints(); + + // Solve Ax = b, where + // A = M - h * dfdv - h * h * dfdx + // b = h * (f0 + h * dfdx * v0 + dfdx * y) + b3SparseSymMat33 M(m_particleCount); + for (u32 i = 0; i < m_particleCount; ++i) + { + M(i, i) = b3Diagonal(m_particles[i]->m_mass); + } + + // A + b3SparseSymMat33 A = M - h * dfdv - h * h * dfdx; + + // View for A + b3SparseSymMat33View viewA(A); + + // b + b3DenseVec3 b = h * (sf + h * (dfdx * sv) + dfdx * sy); + + // I + b3DiagMat33 I(m_particleCount); + I.SetIdentity(); + + // x + b3DenseVec3 x(m_particleCount); + b3SolveMPCG(x, viewA, b, S, sz, sx0, I); + + // Velocity update + sv = sv + x; + sx = sx + sy; + + // Copy state buffers back to the particle + for (u32 i = 0; i < m_particleCount; ++i) + { + b3Particle* p = m_particles[i]; + + p->m_x = x[i]; + p->m_position = sx[i]; + p->m_velocity = sv[i]; + } +} \ No newline at end of file diff --git a/src/bounce/cloth/cloth_solver.cpp b/src/bounce/cloth/cloth_solver.cpp index da71c12..5584b29 100644 --- a/src/bounce/cloth/cloth_solver.cpp +++ b/src/bounce/cloth/cloth_solver.cpp @@ -17,27 +17,13 @@ */ #include +#include #include #include #include -#include -#include -#include -#include -#include -#include -#include #include #include - -// Here, we solve Ax = b using the Modified Preconditioned Conjugate Gradient (MPCG) algorithm. -// described in the paper: -// "Large Steps in Cloth Simulation - David Baraff, Andrew Witkin". - -// Some improvements for the original MPCG algorithm are described in the paper: -// "On the modified conjugate gradient method in cloth simulation - Uri M. Ascher, Eddy Boxerman". - -u32 b3_clothSolverIterations = 0; +#include b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def) { @@ -51,10 +37,6 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def) m_forceCount = 0; m_forces = (b3Force**)m_allocator->Allocate(m_forceCapacity * sizeof(b3Force*));; - m_constraintCapacity = def.particleCapacity; - m_constraintCount = 0; - m_constraints = (b3AccelerationConstraint*)m_allocator->Allocate(m_constraintCapacity * sizeof(b3AccelerationConstraint)); - m_bodyContactCapacity = def.bodyContactCapacity; m_bodyContactCount = 0; m_bodyContacts = (b3ParticleBodyContact**)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3ParticleBodyContact*));; @@ -68,8 +50,6 @@ b3ClothSolver::~b3ClothSolver() { m_allocator->Free(m_triangleContacts); m_allocator->Free(m_bodyContacts); - - m_allocator->Free(m_constraints); m_allocator->Free(m_forces); m_allocator->Free(m_particles); } @@ -95,326 +75,106 @@ void b3ClothSolver::Add(b3ParticleTriangleContact* c) m_triangleContacts[m_triangleContactCount++] = c; } -void b3ClothSolver::ApplyForces() -{ - for (u32 i = 0; i < m_forceCount; ++i) - { - m_forces[i]->Apply(&m_solverData); - } -} - -void b3AccelerationConstraint::Apply(const b3ClothSolverData* data) -{ - b3DiagMat33& sS = *data->S; - b3DenseVec3& sz = *data->z; - - sz[i1] = z; - - b3Mat33 I; I.SetIdentity(); - - switch (ndof) - { - case 3: - { - sS[i1] = I; - break; - } - case 2: - { - sS[i1] = I - b3Outer(p, p); - break; - } - case 1: - { - sS[i1] = I - b3Outer(p, p) - b3Outer(q, q); - break; - } - case 0: - { - sS[i1].SetZero(); - break; - } - default: - { - B3_ASSERT(false); - break; - } - } -} - -void b3ClothSolver::ApplyConstraints() -{ - b3DiagMat33& S = *m_solverData.S; - b3DenseVec3& z = *m_solverData.z; - b3DenseVec3& x = *m_solverData.x; - - S.SetIdentity(); - z.SetZero(); - - for (u32 i = 0; i < m_particleCount; ++i) - { - b3Particle* p = m_particles[i]; - - if (p->m_type != e_dynamicParticle) - { - b3AccelerationConstraint* ac = m_constraints + m_constraintCount; - ++m_constraintCount; - ac->i1 = i; - ac->ndof = 0; - ac->z.SetZero(); - } - } - - for (u32 i = 0; i < m_constraintCount; ++i) - { - m_constraints[i].Apply(&m_solverData); - } -} - -// -static void b3SolveMPCG(b3DenseVec3& x, - const b3SparseSymMat33View& A, const b3DenseVec3& b, - const b3DiagMat33& S, const b3DenseVec3& z, - const b3DenseVec3& y, const b3DiagMat33& I, u32 maxIterations = 20) -{ - B3_PROFILE("Cloth Solve MPCG"); - - // Jacobi preconditioner - // P = diag(A) - b3DiagMat33 inv_P(A.rowCount); - for (u32 i = 0; i < A.rowCount; ++i) - { - b3Mat33 a = A(i, i); - - // Sylvester Criterion to ensure PD-ness - B3_ASSERT(b3Det(a.x, a.y, a.z) > 0.0f); - - B3_ASSERT(a.x.x > 0.0f); - float32 xx = 1.0f / a.x.x; - - B3_ASSERT(a.y.y > 0.0f); - float32 yy = 1.0f / a.y.y; - - B3_ASSERT(a.z.z > 0.0f); - float32 zz = 1.0f / a.z.z; - - inv_P[i] = b3Diagonal(xx, yy, zz); - } - - x = (S * y) + (I - S) * z; - - b3DenseVec3 b_hat = S * (b - A * ((I - S) * z)); - - float32 b_delta = b3Dot(b_hat, inv_P * b_hat); - - b3DenseVec3 r = S * (b - A * x); - - b3DenseVec3 p = S * (inv_P * r); - - float32 delta_new = b3Dot(r, p); - - u32 iteration = 0; - for (;;) - { - if (iteration == maxIterations) - { - break; - } - - if (delta_new <= B3_EPSILON * B3_EPSILON * b_delta) - { - break; - } - - b3DenseVec3 s = S * (A * p); - - float32 alpha = delta_new / b3Dot(p, s); - - x = x + alpha * p; - r = r - alpha * s; - - b3DenseVec3 h = inv_P * r; - - float32 delta_old = delta_new; - - delta_new = b3Dot(r, h); - - float32 beta = delta_new / delta_old; - - p = S * (h + beta * p); - - ++iteration; - } - - b3_clothSolverIterations = iteration; -} - void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations) { - float32 h = dt; - float32 inv_h = 1.0f / h; + { + // Solve internal dynamics + b3ClothForceSolverDef forceSolverDef; + forceSolverDef.stack = m_allocator; + forceSolverDef.particleCount = m_particleCount; + forceSolverDef.particles = m_particles; + forceSolverDef.forceCount = m_forceCount; + forceSolverDef.forces = m_forces; - b3DenseVec3 sx(m_particleCount); - b3DenseVec3 sv(m_particleCount); - b3DenseVec3 sf(m_particleCount); + b3ClothForceSolver forceSolver(forceSolverDef); - m_solverData.dt = h; - m_solverData.invdt = inv_h; - m_solverData.x = &sx; - m_solverData.v = &sv; - m_solverData.f = &sf; + forceSolver.Solve(dt, gravity); + } + + // Copy particle state to state buffer + b3Vec3* positions = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3)); + b3Vec3* velocities = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3)); for (u32 i = 0; i < m_particleCount; ++i) { - b3Particle* p = m_particles[i]; - - sx[i] = p->m_position; - sv[i] = p->m_velocity; - sf[i] = p->m_force; - - // Apply weight - if (p->m_type == e_dynamicParticle) - { - sf[i] += p->m_mass * gravity; - } + positions[i] = m_particles[i]->m_position; + velocities[i] = m_particles[i]->m_velocity; } - // Integrate velocities { - b3SparseSymMat33 dfdx(m_particleCount); - b3SparseSymMat33 dfdv(m_particleCount); - b3DiagMat33 S(m_particleCount); - b3DenseVec3 z(m_particleCount); - b3DenseVec3 sy(m_particleCount); - b3DenseVec3 sx0(m_particleCount); + // Solve constraints + b3ClothContactSolverDef contactSolverDef; + contactSolverDef.allocator = m_allocator; + contactSolverDef.positions = positions; + contactSolverDef.velocities = velocities; + contactSolverDef.bodyContactCount = m_bodyContactCount; + contactSolverDef.bodyContacts = m_bodyContacts; + contactSolverDef.triangleContactCount = m_triangleContactCount; + contactSolverDef.triangleContacts = m_triangleContacts; - m_solverData.y = &sy; - m_solverData.dfdx = &dfdx; - m_solverData.dfdv = &dfdv; - m_solverData.S = &S; - m_solverData.z = &z; + b3ClothContactSolver contactSolver(contactSolverDef); - // Apply position correction - for (u32 i = 0; i < m_particleCount; ++i) { - b3Particle* p = m_particles[i]; - - sy[i] = p->m_translation; - sx0[i] = p->m_x; + // Initialize constraints + contactSolver.InitializeBodyContactConstraints(); + contactSolver.InitializeTriangleContactConstraints(); } - // Apply internal forces - ApplyForces(); - - // Apply constraints - ApplyConstraints(); - - // Solve Ax = b, where - // A = M - h * dfdv - h * h * dfdx - // b = h * (f0 + h * dfdx * v0 + dfdx * y) - - b3SparseSymMat33 M(m_particleCount); - for (u32 i = 0; i < m_particleCount; ++i) { - M(i, i) = b3Diagonal(m_particles[i]->m_mass); + // Warm start velocity constraints + contactSolver.WarmStartBodyContactConstraints(); + contactSolver.WarmStartTriangleContactConstraints(); } - // A - b3SparseSymMat33 A = M - h * dfdv - h * h * dfdx; - - // View for A - b3SparseSymMat33View viewA(A); - - // b - b3DenseVec3 b = h * (sf + h * (dfdx * sv) + dfdx * sy); - - // I - b3DiagMat33 I(m_particleCount); - I.SetIdentity(); - - // x - b3DenseVec3 x(m_particleCount); - b3SolveMPCG(x, viewA, b, S, z, sx0, I); - - // Velocity update - sv = sv + x; - sx = sx + sy; - - // Copy state buffers back to the particle - for (u32 i = 0; i < m_particleCount; ++i) { - b3Particle* p = m_particles[i]; - - p->m_x = x[i]; - p->m_position = sx[i]; - p->m_velocity = sv[i]; - } - } - - // Solve constraints - b3ClothContactSolverDef contactSolverDef; - contactSolverDef.allocator = m_allocator; - contactSolverDef.positions = m_solverData.x; - contactSolverDef.velocities = m_solverData.v; - contactSolverDef.bodyContactCount = m_bodyContactCount; - contactSolverDef.bodyContacts = m_bodyContacts; - contactSolverDef.triangleContactCount = m_triangleContactCount; - contactSolverDef.triangleContacts = m_triangleContacts; - - b3ClothContactSolver contactSolver(contactSolverDef); - - { - contactSolver.InitializeBodyContactConstraints(); - contactSolver.InitializeTriangleContactConstraints(); - } - - { - contactSolver.WarmStartBodyContactConstraints(); - contactSolver.WarmStartTriangleContactConstraints(); - } - - { - for (u32 i = 0; i < velocityIterations; ++i) - { - contactSolver.SolveBodyContactVelocityConstraints(); - contactSolver.SolveTriangleContactVelocityConstraints(); - } - } - - { - contactSolver.StoreImpulses(); - } - - // Integrate positions - sx = sx + h * sv; - - // Solve position constraints - { - bool positionSolved = false; - for (u32 i = 0; i < positionIterations; ++i) - { - bool bodyContactsSolved = contactSolver.SolveBodyContactPositionConstraints(); - bool triangleContactsSolved = contactSolver.SolveTriangleContactPositionConstraints(); - - if (bodyContactsSolved && triangleContactsSolved) + // Solve velocity constraints + for (u32 i = 0; i < velocityIterations; ++i) { - // Early out if the position errors are small. - positionSolved = true; - break; + contactSolver.SolveBodyContactVelocityConstraints(); + contactSolver.SolveTriangleContactVelocityConstraints(); } } - } - // Synchronize bodies - for (u32 i = 0; i < m_bodyContactCount; ++i) - { - b3Body* body = m_bodyContacts[i]->s2->GetBody(); + { + // Cache impulses for warm-starting + contactSolver.StoreImpulses(); + } - body->SynchronizeTransform(); + // Integrate positions + float32 h = dt; + for (u32 i = 0; i < m_particleCount; ++i) + { + positions[i] += h * velocities[i]; + } - body->m_worldInvI = b3RotateToFrame(body->m_invI, body->m_xf.rotation); + // Solve position constraints + { + bool positionSolved = false; + for (u32 i = 0; i < positionIterations; ++i) + { + bool bodyContactsSolved = contactSolver.SolveBodyContactPositionConstraints(); + bool triangleContactsSolved = contactSolver.SolveTriangleContactPositionConstraints(); - body->SynchronizeShapes(); + if (bodyContactsSolved && triangleContactsSolved) + { + // Early out if the position errors are small. + positionSolved = true; + break; + } + } + } + + // Synchronize bodies + for (u32 i = 0; i < m_bodyContactCount; ++i) + { + b3Body* body = m_bodyContacts[i]->s2->GetBody(); + + body->SynchronizeTransform(); + + body->m_worldInvI = b3RotateToFrame(body->m_invI, body->m_xf.rotation); + + body->SynchronizeShapes(); + } } // Copy state buffers back to the particles @@ -422,7 +182,10 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterati { b3Particle* p = m_particles[i]; - p->m_position = sx[i]; - p->m_velocity = sv[i]; + p->m_position = positions[i]; + p->m_velocity = velocities[i]; } -} + + m_allocator->Free(velocities); + m_allocator->Free(positions); +} \ No newline at end of file diff --git a/src/bounce/cloth/spring_force.cpp b/src/bounce/cloth/spring_force.cpp index 723b1ac..e2d0265 100644 --- a/src/bounce/cloth/spring_force.cpp +++ b/src/bounce/cloth/spring_force.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -50,7 +50,7 @@ b3SpringForce::~b3SpringForce() } -void b3SpringForce::Apply(const b3ClothSolverData* data) +void b3SpringForce::Apply(const b3ClothForceSolverData* data) { b3DenseVec3& x = *data->x; b3DenseVec3& v = *data->v;