From 8b775361a971846d92b6d3d0b7c415a41a7f7db5 Mon Sep 17 00:00:00 2001 From: Irlan <-> Date: Fri, 25 May 2018 00:00:07 -0300 Subject: [PATCH] store tension action force inside spring, make spring output force and derivative for abstraction, cleanup --- examples/testbed/tests/tension_mapping.h | 27 ++- include/bounce/dynamics/cloth/cloth.h | 50 +++- include/bounce/dynamics/cloth/cloth_solver.h | 18 +- src/bounce/dynamics/cloth/cloth.cpp | 62 ++++- src/bounce/dynamics/cloth/cloth_solver.cpp | 238 +++++-------------- 5 files changed, 191 insertions(+), 204 deletions(-) diff --git a/examples/testbed/tests/tension_mapping.h b/examples/testbed/tests/tension_mapping.h index 362206f..ac8617d 100644 --- a/examples/testbed/tests/tension_mapping.h +++ b/examples/testbed/tests/tension_mapping.h @@ -108,6 +108,27 @@ public: m_cloth.Step(dt); m_cloth.Apply(); + b3StackArray tension; + tension.Resize(m_cloth.GetParticleCount()); + for (u32 i = 0; i < tension.Count(); ++i) + { + tension[i].SetZero(); + } + + for (u32 i = 0; i < m_cloth.GetSpringCount(); ++i) + { + b3Spring* s = m_cloth.GetSpring(i); + + b3Particle* p1 = s->p1; + b3Particle* p2 = s->p2; + + u32 i1 = m_cloth.GetParticleIndex(p1); + u32 i2 = m_cloth.GetParticleIndex(p2); + + tension[i1] += s->tension; + tension[i2] -= s->tension; + } + for (u32 i = 0; i < m_gridClothMesh.triangleCount; ++i) { b3ClothMeshTriangle* t = m_gridClothMesh.triangles + i; @@ -124,13 +145,13 @@ public: b3Draw_draw->DrawSegment(v2, v3, b3Color_black); b3Draw_draw->DrawSegment(v3, v1, b3Color_black); - b3Vec3 f1 = p1->tension; + b3Vec3 f1 = tension[t->v1]; float32 L1 = b3Length(f1); - b3Vec3 f2 = p2->tension; + b3Vec3 f2 = tension[t->v2]; float32 L2 = b3Length(f2); - b3Vec3 f3 = p3->tension; + b3Vec3 f3 = tension[t->v3]; float32 L3 = b3Length(f3); float32 L = (L1 + L2 + L3) / 3.0f; diff --git a/include/bounce/dynamics/cloth/cloth.h b/include/bounce/dynamics/cloth/cloth.h index 550eca9..03f579c 100644 --- a/include/bounce/dynamics/cloth/cloth.h +++ b/include/bounce/dynamics/cloth/cloth.h @@ -77,21 +77,18 @@ enum b3ParticleType // Read-only particle struct b3Particle { - // Particle type + // Type b3ParticleType type; - // Mass position + // Position b3Vec3 position; - // Mass velocity + // Velocity b3Vec3 velocity; - // Mass force + // Applied external force b3Vec3 force; - // Mass tension force for visualization - b3Vec3 tension; - // Mass float32 mass; @@ -123,6 +120,8 @@ enum b3SpringType e_bendSpring, }; +struct b3ClothSolverData; + // Read-only spring struct b3Spring { @@ -143,6 +142,17 @@ struct b3Spring // Damping stiffness float32 kd; + + // Solver temp + + // Action tensile force (f_i entry) + b3Vec3 tension; + + // Jacobian (J_ii entry) + b3Mat33 Jx, Jv; + + // Initialize solver data. + void Initialize(const b3ClothSolverData* data); }; // Read-only contact @@ -182,6 +192,10 @@ public: // Return the particle at a given index in this cloth. b3Particle* GetParticle(u32 i) const; + // Convenience function. + // Return the index of a given particle. + u32 GetParticleIndex(const b3Particle* p) const; + // Set the type of a given particle. void SetType(b3Particle* p, b3ParticleType type); @@ -194,6 +208,12 @@ public: // Apply a force to a given particle. void ApplyForce(b3Particle* p, const b3Vec3& force); + // Return the number of springs in this cloth. + u32 GetSpringCount() const; + + // Return the spring at a given index in this cloth. + b3Spring* GetSpring(u32 i) const; + // Return the kinetic (or dynamic) energy in this system. float32 GetEnergy() const; @@ -273,6 +293,11 @@ inline b3Particle* b3Cloth::GetParticle(u32 i) const return m_particles + i; } +inline u32 b3Cloth::GetParticleIndex(const b3Particle* p) const +{ + return u32(p - m_particles); +} + inline void b3Cloth::SetType(b3Particle* p, b3ParticleType type) { if (p->type == type) @@ -319,6 +344,17 @@ inline void b3Cloth::ApplyForce(b3Particle* p, const b3Vec3& force) p->force += force; } +inline u32 b3Cloth::GetSpringCount() const +{ + return m_springCount; +} + +inline b3Spring* b3Cloth::GetSpring(u32 i) const +{ + B3_ASSERT(i < m_springCount); + return m_springs + i; +} + inline float32 b3Cloth::GetEnergy() const { float32 E = 0.0f; diff --git a/include/bounce/dynamics/cloth/cloth_solver.h b/include/bounce/dynamics/cloth/cloth_solver.h index df3f776..2adcd9e 100644 --- a/include/bounce/dynamics/cloth/cloth_solver.h +++ b/include/bounce/dynamics/cloth/cloth_solver.h @@ -41,6 +41,15 @@ struct b3ClothSolverDef u32 contactCapacity; }; +struct b3ClothSolverData +{ + b3Vec3* x; + b3Vec3* v; + b3Vec3* f; + float32 dt; + float32 invdt; +}; + class b3ClothSolver { public: @@ -53,9 +62,6 @@ public: void Solve(float32 dt, const b3Vec3& gravity); private: - // Compute forces. - void Compute_f(b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3Vec3& gravity); - // 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; @@ -71,8 +77,6 @@ private: b3StackAllocator* m_allocator; - float32 m_h; - u32 m_particleCapacity; u32 m_particleCount; b3Particle** m_particles; @@ -80,12 +84,12 @@ private: u32 m_springCapacity; u32 m_springCount; b3Spring** m_springs; - b3Mat33* m_Jx; - b3Mat33* m_Jv; u32 m_contactCapacity; u32 m_contactCount; b3ParticleContact** m_contacts; + + b3ClothSolverData m_solverData; }; #endif \ No newline at end of file diff --git a/src/bounce/dynamics/cloth/cloth.cpp b/src/bounce/dynamics/cloth/cloth.cpp index 264b74b..214d2b4 100644 --- a/src/bounce/dynamics/cloth/cloth.cpp +++ b/src/bounce/dynamics/cloth/cloth.cpp @@ -31,6 +31,58 @@ #define B3_CLOTH_FRICTION 0 +// b3Spring +void b3Spring::Initialize(const b3ClothSolverData* data) +{ + u32 i1 = p1->solverId; + u32 i2 = p2->solverId; + + b3Vec3 x1 = data->x[i1]; + b3Vec3 v1 = data->v[i1]; + + b3Vec3 x2 = data->x[i2]; + b3Vec3 v2 = data->v[i2]; + + const b3Mat33 I = b3Mat33_identity; + + b3Vec3 dx = x1 - x2; + + 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; + + // Jacobian + Jx = -ks * (b3Outer(dx, dx) + (1.0f - L0 / L) * (I - b3Outer(dx, dx))); + } + else + { + tension.SetZero(); + Jx.SetZero(); + } + + // Damping + b3Vec3 dv = v1 - v2; + + b3Vec3 df1 = -kd * dv; + b3Vec3 df2 = -df1; + + data->f[i1] += df1; + data->f[i2] += df2; + + Jv = -kd * I; +} + +// b3Cloth b3Cloth::b3Cloth() { m_gravity.SetZero(); @@ -204,7 +256,6 @@ void b3Cloth::Initialize(const b3ClothDef& def) p->translation.SetZero(); p->x.SetZero(); - p->tension.SetZero(); b3ParticleContact* c = m_contacts + i; c->n_active = false; @@ -254,6 +305,7 @@ void b3Cloth::Initialize(const b3ClothDef& def) s->L0 = b3Distance(p1->position, p2->position); s->ks = def.ks; s->kd = def.kd; + s->tension.SetZero(); } #if B3_CLOTH_BENDING @@ -276,6 +328,7 @@ void b3Cloth::Initialize(const b3ClothDef& def) s->L0 = b3Distance(p1->position, p2->position); s->ks = def.kb; s->kd = def.kd; + s->tension.SetZero(); } m_allocator.Free(sharedEdges); @@ -298,6 +351,7 @@ void b3Cloth::Initialize(const b3ClothDef& def) s->L0 = 0.0f; s->ks = def.ks; s->kd = def.kd; + s->tension.SetZero(); } B3_ASSERT(m_springCount <= springCapacity); @@ -557,12 +611,6 @@ void b3Cloth::Solve(float32 dt) { B3_PROFILE("Solve"); - // Clear tension - for (u32 i = 0; i < m_particleCount; ++i) - { - m_particles[i].tension.SetZero(); - } - // Solve b3ClothSolverDef solverDef; solverDef.stack = &m_allocator; diff --git a/src/bounce/dynamics/cloth/cloth_solver.cpp b/src/bounce/dynamics/cloth/cloth_solver.cpp index a7e81a0..a3a347c 100644 --- a/src/bounce/dynamics/cloth/cloth_solver.cpp +++ b/src/bounce/dynamics/cloth/cloth_solver.cpp @@ -44,8 +44,6 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def) m_springCapacity = def.springCapacity; m_springCount = 0; m_springs = (b3Spring**)m_allocator->Allocate(m_springCapacity * sizeof(b3Spring*));; - m_Jx = (b3Mat33*)m_allocator->Allocate(m_springCapacity * sizeof(b3Mat33)); - m_Jv = (b3Mat33*)m_allocator->Allocate(m_springCapacity * sizeof(b3Mat33)); m_contactCapacity = def.contactCapacity; m_contactCount = 0; @@ -55,8 +53,6 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def) b3ClothSolver::~b3ClothSolver() { m_allocator->Free(m_contacts); - m_allocator->Free(m_Jv); - m_allocator->Free(m_Jx); m_allocator->Free(m_springs); m_allocator->Free(m_particles); } @@ -94,65 +90,46 @@ static B3_FORCE_INLINE void b3FreeSparse(b3SparseMat33& matrix, b3StackAllocator allocator->Free(matrix.values); } -static B3_FORCE_INLINE void b3CopyPosition(b3DenseVec3& v, b3Particle** const particles, u32 count) -{ - for (u32 i = 0; i < count; ++i) - { - v[i] = particles[i]->position; - } -} - -static B3_FORCE_INLINE void b3CopyVelocity(b3DenseVec3& v, b3Particle** const particles, u32 count) -{ - for (u32 i = 0; i < count; ++i) - { - v[i] = particles[i]->velocity; - } -} - -static B3_FORCE_INLINE void b3CopyForce(b3DenseVec3& v, b3Particle** const particles, u32 count) -{ - for (u32 i = 0; i < count; ++i) - { - v[i] = particles[i]->force; - } -} - -static B3_FORCE_INLINE void b3CopyTranslation(b3DenseVec3& v, b3Particle** const particles, u32 count) -{ - for (u32 i = 0; i < count; ++i) - { - v[i] = particles[i]->translation; - } -} - -static B3_FORCE_INLINE void b3CopyGuess(b3DenseVec3& v, b3Particle** const particles, u32 count) -{ - for (u32 i = 0; i < count; ++i) - { - v[i] = particles[i]->x; - } -} - void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) { B3_PROFILE("Integrate"); - m_h = dt; - b3DenseVec3 sx(m_particleCount); - b3CopyPosition(sx, m_particles, m_particleCount); - b3DenseVec3 sv(m_particleCount); - b3CopyVelocity(sv, m_particles, m_particleCount); - b3DenseVec3 sf(m_particleCount); - b3CopyForce(sf, m_particles, m_particleCount); - b3DenseVec3 sy(m_particleCount); - b3CopyTranslation(sy, m_particles, m_particleCount); + b3DenseVec3 sx0(m_particleCount); - Compute_f(sf, sx, sv, gravity); + m_solverData.x = sx.v; + m_solverData.v = sv.v; + m_solverData.f = sf.v; + m_solverData.dt = dt; + m_solverData.invdt = 1.0f / dt; + + for (u32 i = 0; i < m_particleCount; ++i) + { + b3Particle* p = m_particles[i]; + + sx[i] = p->position; + sv[i] = p->velocity; + sf[i] = p->force; + + // Apply weight + if (p->type == e_dynamicParticle) + { + sf[i] += p->mass * gravity; + } + + sy[i] = p->translation; + sx0[i] = p->x; + } + + // Initialize spring forces + for (u32 i = 0; i < m_springCount; ++i) + { + b3Spring* s = m_springs[i]; + s->Initialize(&m_solverData); + } // Solve Ax = b, where // A = M - h * dfdv - h * h * dfdx @@ -174,24 +151,19 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) b3DenseVec3 z(m_particleCount); Compute_z(z); - // x0 - b3DenseVec3 x0(m_particleCount); - b3CopyGuess(x0, m_particles, m_particleCount); - // x b3DenseVec3 x(m_particleCount); // Solve Ax = b u32 iterations = 0; - Solve(x, iterations, A, b, S, z, x0); - + Solve(x, iterations, A, b, S, z, sx0); b3_clothSolverIterations = iterations; // f = A * x - b b3DenseVec3 f = A * x - b; // Update state - float32 h = m_h; + float32 h = dt; for (u32 i = 0; i < m_particleCount; ++i) { @@ -221,15 +193,15 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) sx[i] = x1; } - // Write x to the solution cache for improving convergence + // Cache x to improve convergence for (u32 i = 0; i < m_particleCount; ++i) { m_particles[i]->x = x[i]; } // Store the extra contact constraint forces that should have been - // supplied in order to enforce the contact constraints exactly - // These forces can be used in contact constraint logic + // supplied to enforce the contact constraints exactly. + // These forces can be used in contact constraint logic. for (u32 i = 0; i < m_contactCount; ++i) { b3ParticleContact* c = m_contacts[i]; @@ -257,15 +229,6 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) #define B3_INDEX(i, j, size) (i + j * size) -// -static void b3SetZero(b3Vec3* out, u32 size) -{ - for (u32 i = 0; i < size; ++i) - { - out[i].SetZero(); - } -} - // static void b3SetZero(b3Mat33* out, u32 size) { @@ -275,19 +238,18 @@ static void b3SetZero(b3Mat33* out, u32 size) } } -// -static void b3Mul_Jacobian(b3Vec3* out, const b3Vec3* v, u32 massCount, - const b3Mat33* J_ii, b3Spring** const springs, u32 springCount) +// dfdx * v +static void b3Mul_Jx(b3DenseVec3& out, b3Spring** springs, u32 springCount, const b3DenseVec3& v) { - b3SetZero(out, massCount); + out.SetZero(); for (u32 i = 0; i < springCount; ++i) { - const b3Spring* S = springs[i]; - u32 i1 = S->p1->solverId; - u32 i2 = S->p2->solverId; + b3Spring* s = springs[i]; + u32 i1 = s->p1->solverId; + u32 i2 = s->p2->solverId; - b3Mat33 J_11 = J_ii[i]; + b3Mat33 J_11 = s->Jx; b3Mat33 J_12 = -J_11; b3Mat33 J_21 = J_12; b3Mat33 J_22 = J_11; @@ -297,87 +259,6 @@ static void b3Mul_Jacobian(b3Vec3* out, const b3Vec3* v, u32 massCount, } } -void b3ClothSolver::Compute_f(b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3Vec3& gravity) -{ - // Zero force - f.SetZero(); - - // Apply weight - for (u32 i = 0; i < m_particleCount; ++i) - { - b3Particle* p = m_particles[i]; - - if (p->type == e_dynamicParticle) - { - f[i] += p->mass * gravity; - } - } - - // Apply tension, damping - // Store the Jacobians - - // Zero Jacobians - b3SetZero(m_Jx, m_springCount); - b3SetZero(m_Jv, m_springCount); - - for (u32 i = 0; i < m_springCount; ++i) - { - b3Spring* S = m_springs[i]; - b3SpringType type = S->type; - b3Particle* p1 = S->p1; - b3Particle* p2 = S->p2; - float32 L0 = S->L0; - float32 ks = S->ks; - float32 kd = S->kd; - - u32 i1 = p1->solverId; - u32 i2 = p2->solverId; - - b3Vec3 x1 = x[i1]; - b3Vec3 v1 = v[i1]; - - b3Vec3 x2 = x[i2]; - b3Vec3 v2 = v[i2]; - - const b3Mat33 I = b3Mat33_identity; - - b3Vec3 dx = x1 - x2; - - if (b3Dot(dx, dx) >= L0 * L0) - { - // Tension - float32 L = b3Length(dx); - b3Vec3 n = dx / L; - - b3Vec3 sf1 = -ks * (L - L0) * n; - b3Vec3 sf2 = -sf1; - - f[i1] += sf1; - f[i2] += sf2; - - p1->tension += sf1; - p2->tension += sf2; - - b3Mat33 Jx11 = -ks * (b3Outer(dx, dx) + (1.0f - L0 / L) * (I - b3Outer(dx, dx))); - - m_Jx[i] = Jx11; - } - - // Damping - b3Vec3 dv = v1 - v2; - - b3Vec3 df1 = -kd * dv; - b3Vec3 df2 = -df1; - - f[i1] += df1; - f[i2] += df2; - - b3Mat33 Jv11 = -kd * I; - - m_Jv[i] = Jv11; - } -} - static B3_FORCE_INLINE bool b3IsZero(const b3Mat33& A) { bool isZeroX = b3Dot(A.x, A.x) == 0.0f; @@ -389,6 +270,8 @@ static B3_FORCE_INLINE bool b3IsZero(const b3Mat33& A) void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const { + float32 h = m_solverData.dt; + // Compute dfdx, dfdv b3Mat33* dfdx = (b3Mat33*)m_allocator->Allocate(m_particleCount * m_particleCount * sizeof(b3Mat33)); b3SetZero(dfdx, m_particleCount * m_particleCount); @@ -398,11 +281,11 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense for (u32 i = 0; i < m_springCount; ++i) { - const b3Spring* S = m_springs[i]; - u32 i1 = S->p1->solverId; - u32 i2 = S->p2->solverId; + b3Spring* s = m_springs[i]; + u32 i1 = s->p1->solverId; + u32 i2 = s->p2->solverId; - b3Mat33 Jx11 = m_Jx[i]; + b3Mat33 Jx11 = s->Jx; b3Mat33 Jx12 = -Jx11; b3Mat33 Jx21 = Jx12; b3Mat33 Jx22 = Jx11; @@ -412,7 +295,7 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense dfdx[B3_INDEX(i2, i1, m_particleCount)] += Jx21; dfdx[B3_INDEX(i2, i2, m_particleCount)] += Jx22; - b3Mat33 Jv11 = m_Jv[i]; + b3Mat33 Jv11 = s->Jv; b3Mat33 Jv12 = -Jv11; b3Mat33 Jv21 = Jv12; b3Mat33 Jv22 = Jv11; @@ -424,6 +307,7 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense } // Compute A + // A = M - h * dfdv - h * h * dfdx // A = 0 @@ -441,7 +325,7 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense { for (u32 j = 0; j < m_particleCount; ++j) { - A[B3_INDEX(i, j, m_particleCount)] += (-m_h * dfdv[B3_INDEX(i, j, m_particleCount)]) + (-m_h * m_h * dfdx[B3_INDEX(i, j, m_particleCount)]); + A[B3_INDEX(i, j, m_particleCount)] += (-h * dfdv[B3_INDEX(i, j, m_particleCount)]) + (-h * h * dfdx[B3_INDEX(i, j, m_particleCount)]); } } @@ -482,23 +366,17 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense m_allocator->Free(dfdx); // Compute b - // b = h * (f0 + h * Jx_v + Jx_y) // Jx_v = dfdx * v - b3Vec3* Jx_v = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3)); - b3Mul_Jacobian(Jx_v, v.v, m_particleCount, m_Jx, m_springs, m_springCount); + b3DenseVec3 Jx_v(m_particleCount); + b3Mul_Jx(Jx_v, m_springs, m_springCount, v); // Jx_y = dfdx * y - b3Vec3* Jx_y = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3)); - b3Mul_Jacobian(Jx_y, y.v, m_particleCount, m_Jx, m_springs, m_springCount); + b3DenseVec3 Jx_y(m_particleCount); + b3Mul_Jx(Jx_y, m_springs, m_springCount, y); - for (u32 i = 0; i < m_particleCount; ++i) - { - b[i] = m_h * (f[i] + m_h * Jx_v[i] + Jx_y[i]); - } - - m_allocator->Free(Jx_y); - m_allocator->Free(Jx_v); + // b = h * (f0 + h * Jx_v + Jx_y) + b = h * (f + h * Jx_v + Jx_y); } void b3ClothSolver::Compute_z(b3DenseVec3& z)