From 3d3d9f09106ae7facb032fc0cc1c528e3c46eee5 Mon Sep 17 00:00:00 2001 From: Irlan <-> Date: Thu, 28 Jun 2018 19:52:11 -0300 Subject: [PATCH] optimization, friction force --- include/bounce/dynamics/cloth/cloth_solver.h | 2 +- include/bounce/dynamics/cloth/force.h | 1 + include/bounce/dynamics/cloth/particle.h | 36 ++++++++- src/bounce/dynamics/cloth/cloth.cpp | 65 ++++++++++------ src/bounce/dynamics/cloth/cloth_solver.cpp | 7 +- src/bounce/dynamics/cloth/particle.cpp | 21 ++++++ src/bounce/dynamics/cloth/spring_force.cpp | 78 ++++++++++---------- 7 files changed, 139 insertions(+), 71 deletions(-) diff --git a/include/bounce/dynamics/cloth/cloth_solver.h b/include/bounce/dynamics/cloth/cloth_solver.h index bbf146c..087a6e5 100644 --- a/include/bounce/dynamics/cloth/cloth_solver.h +++ b/include/bounce/dynamics/cloth/cloth_solver.h @@ -26,7 +26,7 @@ class b3StackAllocator; class b3Particle; class b3Force; -struct b3BodyContact; +class b3BodyContact; struct b3DenseVec3; struct b3DiagMat33; diff --git a/include/bounce/dynamics/cloth/force.h b/include/bounce/dynamics/cloth/force.h index 78a18ba..ca5a323 100644 --- a/include/bounce/dynamics/cloth/force.h +++ b/include/bounce/dynamics/cloth/force.h @@ -29,6 +29,7 @@ class b3Particle; // Force types enum b3ForceType { + e_frictionForce, e_springForce, }; diff --git a/include/bounce/dynamics/cloth/particle.h b/include/bounce/dynamics/cloth/particle.h index f8a3f40..253d350 100644 --- a/include/bounce/dynamics/cloth/particle.h +++ b/include/bounce/dynamics/cloth/particle.h @@ -21,6 +21,7 @@ #include #include +#include class b3Shape; class b3Cloth; @@ -57,15 +58,43 @@ struct b3ParticleDef void* userData; }; -// A contact between a particle and a solid -struct b3BodyContact +class b3FrictionForce : public b3Force { +public: + b3FrictionForce() { } + + ~b3FrictionForce() { } + + void Apply(const b3ClothSolverData* data); + + b3Particle* m_p; + float32 m_kd; +}; + +// A contact between a particle and a solid +class b3BodyContact +{ +public: + b3BodyContact() + { + + } + + ~b3BodyContact() + { + + } + b3Particle* p1; b3Shape* s2; float32 s; + + bool f1_active, f2_active; + b3FrictionForce f1, f2; + + bool n_active, t1_active, t2_active; b3Vec3 n, t1, t2; float32 Fn, Ft1, Ft2; - bool n_active, t1_active, t2_active; }; // A cloth particle. @@ -115,6 +144,7 @@ private: friend class b3ClothSolver; friend class b3Force; friend class b3SpringForce; + friend class b3FrictionForce; b3Particle(const b3ParticleDef& def, b3Cloth* cloth); ~b3Particle(); diff --git a/src/bounce/dynamics/cloth/cloth.cpp b/src/bounce/dynamics/cloth/cloth.cpp index 10bd692..e2e2580 100644 --- a/src/bounce/dynamics/cloth/cloth.cpp +++ b/src/bounce/dynamics/cloth/cloth.cpp @@ -33,8 +33,6 @@ #define B3_CLOTH_BENDING 0 -#define B3_CLOTH_FRICTION 1 - static B3_FORCE_INLINE u32 b3NextIndex(u32 i) { return i + 1 < 3 ? i + 1 : 0; @@ -217,7 +215,7 @@ b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) : m_particleBlocks(sizeo b3SpringForceDef fd; fd.Initialize(p1, p2, def.bending, def.damping); - + CreateForce(fd); } @@ -236,7 +234,7 @@ b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) : m_particleBlocks(sizeo b3SpringForceDef fd; fd.Initialize(p1, p2, def.structural, def.damping); - + CreateForce(fd); } } @@ -485,6 +483,8 @@ void b3Cloth::UpdateContacts() b3BodyContact c0 = *c; // Create a new contact + c->f1_active = false; + c->f2_active = false; c->n_active = false; c->t1_active = false; c->t2_active = false; @@ -565,8 +565,6 @@ void b3Cloth::UpdateContacts() continue; } -#if B3_CLOTH_FRICTION == 1 - // A friction force requires an associated normal force. if (c0.n_active == false) { @@ -577,6 +575,7 @@ void b3Cloth::UpdateContacts() b3Vec3 n = c->n; float32 u = s->GetFriction(); float32 normalForce = c0.Fn; + float32 maxFrictionForce = u * normalForce; // Relative velocity b3Vec3 dv = p->m_velocity; @@ -593,12 +592,6 @@ void b3Cloth::UpdateContacts() c->t1 = t1; c->t2 = t2; } - else - { - c->t1_active = true; - c->t2_active = true; - continue; - } b3Vec3 ts[2]; ts[0] = c->t1; @@ -616,6 +609,18 @@ void b3Cloth::UpdateContacts() Ft0[0] = c0.Ft1; Ft0[1] = c0.Ft2; + bool f_active[2]; + f_active[0] = c->f1_active; + f_active[1] = c->f2_active; + + bool f_active0[2]; + f_active0[0] = c0.f1_active; + f_active0[1] = c0.f2_active; + + b3FrictionForce* sf[2]; + sf[0] = &c->f1; + sf[1] = &c->f2; + for (u32 k = 0; k < 2; ++k) { b3Vec3 t = ts[k]; @@ -628,27 +633,33 @@ void b3Cloth::UpdateContacts() // Lock particle on surface t_active[k] = true; } - + if (t_active0[k] == true && t_active[k] == true) { - // The contact persists - float32 maxForce = u * normalForce; + float32 frictionForce = Ft0[k]; - if (Ft0[k] * Ft0[k] > maxForce * maxForce) + // Dynamic friction + if (frictionForce * frictionForce > maxFrictionForce * maxFrictionForce) { // Unlock particle off surface - t_active[k] = false; + //t_active[k] = false; + + // Apply dynamic friction + //f_active[k] = true; + + //sf[k]->m_type = e_frictionForce; + //sf[k]->m_p = p; + //sf[k]->m_kd = 100.0f; } } } + c->f1_active = f_active[0]; + c->f2_active = f_active[1]; + c->t1_active = t_active[0]; c->t2_active = t_active[1]; - -#endif - } - } void b3Cloth::Solve(float32 dt, const b3Vec3& gravity) @@ -659,7 +670,7 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity) b3ClothSolverDef solverDef; solverDef.stack = &m_world->m_stackAllocator; solverDef.particleCapacity = m_particleList.m_count; - solverDef.forceCapacity = m_forceList.m_count; + solverDef.forceCapacity = m_forceList.m_count + (2 * m_particleList.m_count); solverDef.contactCapacity = m_particleList.m_count; b3ClothSolver solver(solverDef); @@ -676,6 +687,16 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity) for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) { + if (p->m_contact.f1_active) + { + solver.Add(&p->m_contact.f1); + } + + if (p->m_contact.f2_active) + { + solver.Add(&p->m_contact.f2); + } + if (p->m_contact.n_active) { solver.Add(&p->m_contact); diff --git a/src/bounce/dynamics/cloth/cloth_solver.cpp b/src/bounce/dynamics/cloth/cloth_solver.cpp index 2dfbf33..8872e0b 100644 --- a/src/bounce/dynamics/cloth/cloth_solver.cpp +++ b/src/bounce/dynamics/cloth/cloth_solver.cpp @@ -268,11 +268,8 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) { m_particles[i]->m_position = sx[i]; m_particles[i]->m_velocity = sv[i]; - } - - // Cache x to improve convergence - for (u32 i = 0; i < m_particleCount; ++i) - { + + // Cache x to improve convergence m_particles[i]->m_x = x[i]; } diff --git a/src/bounce/dynamics/cloth/particle.cpp b/src/bounce/dynamics/cloth/particle.cpp index 7db988d..16074fa 100644 --- a/src/bounce/dynamics/cloth/particle.cpp +++ b/src/bounce/dynamics/cloth/particle.cpp @@ -18,6 +18,25 @@ #include #include +#include +#include +#include + +void b3FrictionForce::Apply(const b3ClothSolverData* data) +{ + b3DenseVec3& v = *data->v; + b3DenseVec3& f = *data->f; + b3SparseSymMat33& dfdv = *data->dfdv; + + u32 i = m_p->m_solverId; + + f[i] += -m_kd * v[i]; + + b3Mat33 I; I.SetIdentity(); + b3Mat33 Jv = -m_kd * I; + + dfdv(i, i) += Jv; +} b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth) { @@ -34,6 +53,8 @@ b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth) m_x.SetZero(); m_vertex = ~0; + m_contact.f1_active = false; + m_contact.f2_active = false; m_contact.n_active = false; m_contact.t1_active = false; m_contact.t2_active = false; diff --git a/src/bounce/dynamics/cloth/spring_force.cpp b/src/bounce/dynamics/cloth/spring_force.cpp index 20c4ec2..1e3ac57 100644 --- a/src/bounce/dynamics/cloth/spring_force.cpp +++ b/src/bounce/dynamics/cloth/spring_force.cpp @@ -69,54 +69,52 @@ void b3SpringForce::Apply(const b3ClothSolverData* data) b3Mat33 I; I.SetIdentity(); - b3Vec3 dx = x1 - x2; + m_f.SetZero(); - float32 L = b3Length(dx); - - b3Mat33 Jx; - - if (L >= m_L0) + if (m_ks > 0.0f) { - b3Vec3 n = dx / L; + b3Vec3 dx = x1 - x2; - // Tension - m_f = -m_ks * (L - m_L0) * n; + float32 L = b3Length(dx); - // Jacobian - Jx = -m_ks * (b3Outer(dx, dx) + (1.0f - m_L0 / L) * (I - b3Outer(dx, dx))); - } - else - { - m_f.SetZero(); - Jx.SetZero(); + if (L >= m_L0) + { + b3Vec3 n = dx / L; + + // Apply tension + m_f += -m_ks * (L - m_L0) * n; + + // Jacobian + b3Mat33 Jx11 = -m_ks * (b3Outer(dx, dx) + (1.0f - m_L0 / L) * (I - b3Outer(dx, dx))); + b3Mat33 Jx12 = -Jx11; + //b3Mat33 Jx21 = Jx12; + b3Mat33 Jx22 = Jx11; + + dfdx(i1, i1) += Jx11; + dfdx(i1, i2) += Jx12; + //dfdx(i2, i1) += Jx21; + dfdx(i2, i2) += Jx22; + } } - // Damping - b3Vec3 dv = v1 - v2; + if (m_kd > 0.0f) + { + // Apply damping + b3Vec3 dv = v1 - v2; - m_f += -m_kd * dv; - b3Mat33 Jv = -m_kd * I; + m_f += -m_kd * dv; + + b3Mat33 Jv11 = -m_kd * I; + b3Mat33 Jv12 = -Jv11; + //b3Mat33 Jv21 = Jv12; + b3Mat33 Jv22 = Jv11; + + dfdv(i1, i1) += Jv11; + dfdv(i1, i2) += Jv12; + //dfdv(i2, i1) += Jv21; + dfdv(i2, i2) += Jv22; + } f[i1] += m_f; f[i2] -= m_f; - - b3Mat33 Jx11 = Jx; - b3Mat33 Jx12 = -Jx11; - //b3Mat33 Jx21 = Jx12; - b3Mat33 Jx22 = Jx11; - - dfdx(i1, i1) += Jx11; - dfdx(i1, i2) += Jx12; - //dfdx(i2, i1) += Jx21; - dfdx(i2, i2) += Jx22; - - b3Mat33 Jv11 = Jv; - b3Mat33 Jv12 = -Jv11; - //b3Mat33 Jv21 = Jv12; - b3Mat33 Jv22 = Jv11; - - dfdv(i1, i1) += Jv11; - dfdv(i1, i2) += Jv12; - //dfdv(i2, i1) += Jv21; - dfdv(i2, i2) += Jv22; } \ No newline at end of file