From dba5ffbe06ad6847356a2cc4b041deba1c462346 Mon Sep 17 00:00:00 2001 From: Irlan <-> Date: Sun, 27 May 2018 02:50:40 -0300 Subject: [PATCH] through an acceleration constraint, the solver can remove acceleration from kinematic particles; consistency; in effect test update --- examples/testbed/tests/cloth_test.h | 6 +- examples/testbed/tests/particle_types.h | 2 +- examples/testbed/tests/table_cloth.h | 6 + examples/testbed/tests/tension_mapping.h | 4 +- include/bounce/dynamics/cloth/cloth.h | 24 +-- include/bounce/dynamics/cloth/cloth_solver.h | 15 +- src/bounce/dynamics/cloth/cloth.cpp | 51 ++---- src/bounce/dynamics/cloth/cloth_solver.cpp | 183 +++++++++---------- 8 files changed, 146 insertions(+), 145 deletions(-) diff --git a/examples/testbed/tests/cloth_test.h b/examples/testbed/tests/cloth_test.h index ba6dfbf..b2aab56 100644 --- a/examples/testbed/tests/cloth_test.h +++ b/examples/testbed/tests/cloth_test.h @@ -118,13 +118,13 @@ public: b3Vec3 dx = B - A; b3Particle* p1 = m_cloth->GetParticle(t->v1); - m_cloth->Translate(p1, dx); + m_cloth->ApplyTranslation(p1, dx); b3Particle* p2 = m_cloth->GetParticle(t->v2); - m_cloth->Translate(p2, dx); + m_cloth->ApplyTranslation(p2, dx); b3Particle* p3 = m_cloth->GetParticle(t->v3); - m_cloth->Translate(p3, dx); + m_cloth->ApplyTranslation(p3, dx); } void StopDragging() diff --git a/examples/testbed/tests/particle_types.h b/examples/testbed/tests/particle_types.h index dc45e6c..3e94560 100644 --- a/examples/testbed/tests/particle_types.h +++ b/examples/testbed/tests/particle_types.h @@ -96,7 +96,7 @@ public: { if (p->type == e_staticParticle) { - m_cloth->Translate(p, d); + m_cloth->ApplyTranslation(p, d); } if (p->type == e_kinematicParticle) diff --git a/examples/testbed/tests/table_cloth.h b/examples/testbed/tests/table_cloth.h index 94a8506..2934f33 100644 --- a/examples/testbed/tests/table_cloth.h +++ b/examples/testbed/tests/table_cloth.h @@ -69,6 +69,11 @@ public: tableShape.m_hull = &m_tableHull; tableShape.m_radius = 0.2f; + //b3CapsuleShape tableShape; + //tableShape.m_centers[0].Set(0.0f, 0.0f, -1.0f); + //tableShape.m_centers[1].Set(0.0f, 0.0f, 1.0f); + //tableShape.m_radius = 2.0f; + b3ShapeDef sd; sd.shape = &tableShape; sd.friction = 1.0f; @@ -82,6 +87,7 @@ public: return new TableCloth(); } + //b3GridMesh<2, 2> m_gridMesh; b3GridMesh<10, 10> m_gridMesh; b3ClothMeshMesh m_gridClothMeshMesh; b3ClothMesh m_gridClothMesh; diff --git a/examples/testbed/tests/tension_mapping.h b/examples/testbed/tests/tension_mapping.h index 3660b65..e8bf72a 100644 --- a/examples/testbed/tests/tension_mapping.h +++ b/examples/testbed/tests/tension_mapping.h @@ -124,8 +124,8 @@ public: u32 i1 = m_cloth->GetParticleIndex(p1); u32 i2 = m_cloth->GetParticleIndex(p2); - tension[i1] += s->tension; - tension[i2] -= s->tension; + tension[i1] += s->f; + tension[i2] -= s->f; } for (u32 i = 0; i < m_gridClothMesh.triangleCount; ++i) diff --git a/include/bounce/dynamics/cloth/cloth.h b/include/bounce/dynamics/cloth/cloth.h index 6865d5f..9b210ce 100644 --- a/include/bounce/dynamics/cloth/cloth.h +++ b/include/bounce/dynamics/cloth/cloth.h @@ -145,14 +145,14 @@ struct b3Spring // Solver temp - // Action tensile force (f_i entry) - b3Vec3 tension; + // Force (f_i entry) + b3Vec3 f; // Jacobian (J_ii entry) b3Mat33 Jx, Jv; - // Apply spring forces. - void ApplyForces(const b3ClothSolverData* data); + // Initialize forces and its derivatives. + void InitializeForces(const b3ClothSolverData* data); }; // Read-only body contact between a particle and a solid @@ -191,15 +191,15 @@ public: // Set the type of a given particle. void SetType(b3Particle* p, b3ParticleType type); - // Translate a given particle in the next time step. - void Translate(b3Particle* p, const b3Vec3& translation); - // Set the velocity of a given particle. void SetVelocity(b3Particle* p, const b3Vec3& velocity); // Apply a force to a given particle. void ApplyForce(b3Particle* p, const b3Vec3& force); + // Apply a translation to a given particle. + void ApplyTranslation(b3Particle* p, const b3Vec3& translation); + // Return the number of springs in this cloth. u32 GetSpringCount() const; @@ -317,11 +317,6 @@ inline void b3Cloth::SetType(b3Particle* p, b3ParticleType type) } } -inline void b3Cloth::Translate(b3Particle* p, const b3Vec3& translation) -{ - p->translation += translation; -} - inline void b3Cloth::SetVelocity(b3Particle* p, const b3Vec3& velocity) { if (p->type == e_staticParticle) @@ -340,6 +335,11 @@ inline void b3Cloth::ApplyForce(b3Particle* p, const b3Vec3& force) p->force += force; } +inline void b3Cloth::ApplyTranslation(b3Particle* p, const b3Vec3& translation) +{ + p->translation += translation; +} + inline u32 b3Cloth::GetSpringCount() const { return m_springCount; diff --git a/include/bounce/dynamics/cloth/cloth_solver.h b/include/bounce/dynamics/cloth/cloth_solver.h index 0d889ad..eb3a0c4 100644 --- a/include/bounce/dynamics/cloth/cloth_solver.h +++ b/include/bounce/dynamics/cloth/cloth_solver.h @@ -25,6 +25,7 @@ struct b3DenseVec3; struct b3DiagMat33; struct b3SparseMat33; +struct b3SolverSparseMat33; struct b3Particle; struct b3Spring; @@ -50,6 +51,13 @@ struct b3ClothSolverData float32 invdt; }; +struct b3SpringForce +{ + u32 i1, i2; + b3Vec3 f; + b3Mat33 Jx, Jv; +}; + struct b3AccelerationConstraint { u32 i1; @@ -69,11 +77,14 @@ public: void Solve(float32 dt, const b3Vec3& gravity); private: + // Initialize forces. + void InitializeForces(); + // Initialize constraints. void InitializeConstraints(); - + // Compute A and b in Ax = b - void Compute_A_b(b3SparseMat33& A, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const; + void Compute_A_b(b3SolverSparseMat33& A, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const; // Compute S and z. void Compute_S_z(b3DiagMat33& S, b3DenseVec3& z); diff --git a/src/bounce/dynamics/cloth/cloth.cpp b/src/bounce/dynamics/cloth/cloth.cpp index 91de182..a71ac22 100644 --- a/src/bounce/dynamics/cloth/cloth.cpp +++ b/src/bounce/dynamics/cloth/cloth.cpp @@ -31,10 +31,10 @@ #define B3_CLOTH_BENDING 0 -#define B3_CLOTH_FRICTION 0 +#define B3_CLOTH_FRICTION 1 // b3Spring -void b3Spring::ApplyForces(const b3ClothSolverData* data) +void b3Spring::InitializeForces(const b3ClothSolverData* data) { u32 i1 = p1->solverId; u32 i2 = p2->solverId; @@ -51,36 +51,25 @@ void b3Spring::ApplyForces(const b3ClothSolverData* data) if (b3Dot(dx, dx) >= L0 * L0) { - // Tension float32 L = b3Length(dx); b3Vec3 n = dx / L; - b3Vec3 sf1 = -ks * (L - L0) * n; - b3Vec3 sf2 = -sf1; - - tension = sf1; - - data->f[i1] += sf1; - data->f[i2] += sf2; + // Tension + f = -ks * (L - L0) * n; // Jacobian Jx = -ks * (b3Outer(dx, dx) + (1.0f - L0 / L) * (I - b3Outer(dx, dx))); } else { - tension.SetZero(); + f.SetZero(); Jx.SetZero(); } // Damping b3Vec3 dv = v1 - v2; - b3Vec3 df1 = -kd * dv; - b3Vec3 df2 = -df1; - - data->f[i1] += df1; - data->f[i2] += df2; - + f += -kd * dv; Jv = -kd * I; } @@ -238,6 +227,9 @@ b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) c->n_active = false; c->t1_active = false; c->t2_active = false; + c->Fn = 0.0f; + c->Ft1 = 0.0f; + c->Ft2 = 0.0f; } // Compute mass @@ -284,7 +276,7 @@ b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) s->L0 = b3Distance(p1->position, p2->position); s->ks = def.ks; s->kd = def.kd; - s->tension.SetZero(); + s->f.SetZero(); } #if B3_CLOTH_BENDING @@ -330,7 +322,7 @@ b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) s->L0 = 0.0f; s->ks = def.ks; s->kd = def.kd; - s->tension.SetZero(); + s->f.SetZero(); } B3_ASSERT(m_springCount <= springCapacity); @@ -388,21 +380,13 @@ void b3Cloth::UpdateContacts() { B3_PROFILE("Update Contacts"); - // Clear active flags - for (u32 i = 0; i < m_particleCount; ++i) - { - m_contacts[i].n_active = false; - m_contacts[i].t1_active = false; - m_contacts[i].t2_active = false; - } - // Create contacts for (u32 i = 0; i < m_particleCount; ++i) { b3Particle* p = m_particles + i; - // Static particles can't participate in unilateral collisions. - if (p->type == e_staticParticle) + // Static and kinematic particles can't participate in unilateral collisions. + if (p->type != e_dynamicParticle) { continue; } @@ -412,6 +396,11 @@ void b3Cloth::UpdateContacts() // Save the old contact b3BodyContact c0 = *c; + // Create a new contact + c->n_active = false; + c->t1_active = false; + c->t2_active = false; + b3Sphere s1; s1.vertex = p->position; s1.radius = p->radius; @@ -456,7 +445,7 @@ void b3Cloth::UpdateContacts() c->t2 = b3Cross(c->t1, n); } - // Update contact state + // Update the contact state if (c0.n_active == true && c->n_active == true) { // The contact persists @@ -496,7 +485,7 @@ void b3Cloth::UpdateContacts() continue; } - b3Shape* s = c->s; + b3Shape* s = c->s2; b3Vec3 n = c->n; float32 u = s->GetFriction(); float32 normalForce = c0.Fn; diff --git a/src/bounce/dynamics/cloth/cloth_solver.cpp b/src/bounce/dynamics/cloth/cloth_solver.cpp index 7c0ff8b..8bf6c11 100644 --- a/src/bounce/dynamics/cloth/cloth_solver.cpp +++ b/src/bounce/dynamics/cloth/cloth_solver.cpp @@ -48,7 +48,7 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def) m_contactCapacity = def.contactCapacity; m_contactCount = 0; m_contacts = (b3BodyContact**)m_allocator->Allocate(m_contactCapacity * sizeof(b3BodyContact*)); - + m_constraintCapacity = def.particleCapacity; m_constraintCount = 0; m_constraints = (b3AccelerationConstraint*)m_allocator->Allocate(m_constraintCapacity * sizeof(b3AccelerationConstraint)); @@ -78,12 +78,20 @@ void b3ClothSolver::Add(b3Spring* s) m_springs[m_springCount++] = s; } +void b3ClothSolver::InitializeForces() +{ + for (u32 i = 0; i < m_springCount; ++i) + { + m_springs[i]->InitializeForces(&m_solverData); + } +} + void b3ClothSolver::InitializeConstraints() { for (u32 i = 0; i < m_particleCount; ++i) { b3Particle* p = m_particles[i]; - if (p->type == e_staticParticle) + if (p->type != e_dynamicParticle) { b3AccelerationConstraint* ac = m_constraints + m_constraintCount; ++m_constraintCount; @@ -98,8 +106,6 @@ void b3ClothSolver::InitializeConstraints() b3BodyContact* pc = m_contacts[i]; b3Particle* p = pc->p1; - B3_ASSERT(p->type != e_staticParticle); - b3AccelerationConstraint* ac = m_constraints + m_constraintCount; ++m_constraintCount; ac->i1 = p->solverId; @@ -118,7 +124,7 @@ void b3ClothSolver::InitializeConstraints() ac->ndof = 1; ac->q = pc->t1; } - + if (pc->t2_active) { ac->ndof = 1; @@ -128,22 +134,28 @@ void b3ClothSolver::InitializeConstraints() } } -static B3_FORCE_INLINE b3SparseMat33 b3AllocSparse(b3StackAllocator* allocator, u32 M, u32 N) +struct b3SolverSparseMat33 : public b3SparseMat33 { - u32 size = M * N; - b3Mat33* elements = (b3Mat33*)allocator->Allocate(size * sizeof(b3Mat33)); - u32* cols = (u32*)allocator->Allocate(size * sizeof(u32)); - u32* row_ptrs = (u32*)allocator->Allocate((M + 1) * sizeof(u32)); + b3SolverSparseMat33(b3StackAllocator* a, u32 m, u32 n) + { + allocator = a; + M = m; + N = n; + valueCount = 0; + values = (b3Mat33*)allocator->Allocate(M * N * sizeof(b3Mat33)); + cols = (u32*)allocator->Allocate(M * N * sizeof(u32)); + row_ptrs = (u32*)allocator->Allocate((M + 1) * sizeof(u32)); + } - return b3SparseMat33(M, N, size, elements, row_ptrs, cols); -} + ~b3SolverSparseMat33() + { + allocator->Free(row_ptrs); + allocator->Free(cols); + allocator->Free(values); + } -static B3_FORCE_INLINE void b3FreeSparse(b3SparseMat33& matrix, b3StackAllocator* allocator) -{ - allocator->Free(matrix.row_ptrs); - allocator->Free(matrix.cols); - allocator->Free(matrix.values); -} + b3StackAllocator* allocator; +}; void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) { @@ -183,19 +195,25 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) for (u32 i = 0; i < m_contactCount; ++i) { b3BodyContact* c = m_contacts[i]; - b3Particle* p = c->p1; + b3Particle* p = c->p1; sy[p->solverId] -= c->s * c->n; } - // Apply spring forces and derivatives + // Initialize forces + InitializeForces(); + + // Apply internal forces for (u32 i = 0; i < m_springCount; ++i) { - m_springs[i]->ApplyForces(&m_solverData); + b3Spring* s = m_springs[i]; + sf[s->p1->solverId] += s->f; + sf[s->p2->solverId] -= s->f; } // Initialize constraints InitializeConstraints(); + // Compute S, z b3DiagMat33 S(m_particleCount); b3DenseVec3 z(m_particleCount); Compute_S_z(S, z); @@ -205,7 +223,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) // b = h * (f0 + h * dfdx * v0 + dfdx * y) // A - b3SparseMat33 A = b3AllocSparse(m_allocator, m_particleCount, m_particleCount); + b3SolverSparseMat33 A(m_allocator, m_particleCount, m_particleCount); // b b3DenseVec3 b(m_particleCount); @@ -220,38 +238,17 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) Solve(x, iterations, A, b, S, z, sx0); b3_clothSolverIterations = iterations; - // f = A * x - b - b3DenseVec3 f = A * x - b; - - // Update state + // Compute the new state + // Clamp large translations? float32 h = dt; + sv = sv + x; + sx = sx + h * sv + sy; + // Copy state buffers back to the particles for (u32 i = 0; i < m_particleCount; ++i) { - b3Particle* p = m_particles[i]; - b3ParticleType type = p->type; - - b3Vec3 ix0 = sx[i]; - b3Vec3 iv0 = sv[i]; - b3Vec3 iy = sy[i]; - - b3Vec3 dv = x[i]; - - // v1 = v0 + dv - b3Vec3 v1 = iv0; - if (type == e_dynamicParticle) - { - v1 += dv; - } - - // dx = h * (v0 + dv) + y = h * v1 + y - b3Vec3 dx = h * v1 + iy; - - // x1 = x0 + dx - b3Vec3 x1 = ix0 + dx; - - sv[i] = v1; - sx[i] = x1; + m_particles[i]->position = sx[i]; + m_particles[i]->velocity = sv[i]; } // Cache x to improve convergence @@ -263,6 +260,10 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) // Store the extra contact constraint forces that should have been // supplied to enforce the contact constraints exactly. // These forces can be used in contact constraint logic. + + // f = A * x - b + b3DenseVec3 f = A * x - b; + for (u32 i = 0; i < m_contactCount; ++i) { b3BodyContact* c = m_contacts[i]; @@ -277,15 +278,6 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) c->Ft1 = b3Dot(force, c->t1); c->Ft2 = b3Dot(force, c->t2); } - - // Copy state buffers back to the particles - for (u32 i = 0; i < m_particleCount; ++i) - { - m_particles[i]->position = sx[i]; - m_particles[i]->velocity = sv[i]; - } - - b3FreeSparse(A, m_allocator); } #define B3_INDEX(i, j, size) (i + j * size) @@ -329,7 +321,7 @@ static B3_FORCE_INLINE bool b3IsZero(const b3Mat33& A) return isZeroX * isZeroY * isZeroZ; } -void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const +void b3ClothSolver::Compute_A_b(b3SolverSparseMat33& SA, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const { float32 h = m_solverData.dt; @@ -368,7 +360,7 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense } // Compute A - + // A = M - h * dfdv - h * h * dfdx // A = 0 @@ -390,14 +382,14 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense } } - // Assembly sparsity - u32 nzCount = 0; + // Assembly sparsity. + u32 valueCapacity = m_particleCapacity * m_particleCapacity; SA.row_ptrs[0] = 0; for (u32 i = 0; i < m_particleCount; ++i) { - u32 rowNzCount = 0; + u32 rowValueCount = 0; for (u32 j = 0; j < m_particleCount; ++j) { @@ -405,21 +397,17 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense if (b3IsZero(a) == false) { - B3_ASSERT(nzCount <= SA.valueCount); - - SA.values[nzCount] = a; - SA.cols[nzCount] = j; - - ++nzCount; - ++rowNzCount; + SA.values[SA.valueCount] = a; + SA.cols[SA.valueCount] = j; + ++SA.valueCount; + ++rowValueCount; } } - SA.row_ptrs[i + 1] = SA.row_ptrs[(i + 1) - 1] + rowNzCount; + SA.row_ptrs[i + 1] = SA.row_ptrs[(i + 1) - 1] + rowValueCount; } - B3_ASSERT(nzCount <= SA.valueCount); - SA.valueCount = nzCount; + B3_ASSERT(SA.valueCount <= valueCapacity); m_allocator->Free(A); @@ -525,26 +513,34 @@ void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations, // p = S * (P^-1 * r) b3DenseVec3 p = S * (inv_P * r); - // deltaNew = dot(r, p) - float32 deltaNew = b3Dot(r, p); + // delta_new = dot(r, p) + float32 delta_new = b3Dot(r, p); - // Tolerance. - // This is the main stopping criteria. - // [0, 1] - const float32 tolerance = 10.0f * B3_EPSILON; + // Set the tolerance. + const float32 tolerance = 1.e-4f; // Maximum number of iterations. - const u32 maxIters = 1000; + // Stop at this iteration if diverged. + const u32 max_iterations = 100; + + u32 iteration = 0; // Main iteration loop. - u32 iter = 0; - while (deltaNew > tolerance * tolerance * b_delta && iter < maxIters) + while (iteration < max_iterations) { + B3_ASSERT(b3IsValid(delta_new)); + + // Convergence check. + if (delta_new <= tolerance * tolerance * b_delta) + { + break; + } + // s = S * (A * p) b3DenseVec3 s = S * (A * p); - // alpha = deltaNew / dot(c, q) - float32 alpha = deltaNew / b3Dot(p, s); + // alpha = delta_new / dot(p, s) + float32 alpha = delta_new / b3Dot(p, s); // x = x + alpha * p x = x + alpha * p; @@ -555,21 +551,20 @@ void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations, // h = inv_P * r b3DenseVec3 h = inv_P * r; - // deltaOld = deltaNew - float32 deltaOld = deltaNew; + // delta_old = delta_new + float32 delta_old = delta_new; - // deltaNew = dot(r, h) - deltaNew = b3Dot(r, h); - //B3_ASSERT(b3IsValid(deltaNew)); + // delta_new = dot(r, h) + delta_new = b3Dot(r, h); - // beta = deltaNew / deltaOld - float32 beta = deltaNew / deltaOld; + // beta = delta_new / delta_old + float32 beta = delta_new / delta_old; // p = S * (h + beta * p) p = S * (h + beta * p); - ++iter; + ++iteration; } - iterations = iter; + iterations = iteration; } \ No newline at end of file