From be76409ffcf888aa2e0692b7d3b929edcba84b56 Mon Sep 17 00:00:00 2001 From: Irlan <-> Date: Tue, 17 Jul 2018 22:00:10 -0300 Subject: [PATCH] bilateral contact response for dynamic particles and rigid bodies, test update --- examples/testbed/tests/pinned_cloth.h | 37 +++++++++++-- include/bounce/dynamics/cloth/particle.h | 19 +++++-- src/bounce/dynamics/cloth/cloth.cpp | 22 ++++---- src/bounce/dynamics/cloth/cloth_solver.cpp | 60 +++++++++++++++------- src/bounce/dynamics/cloth/particle.cpp | 5 ++ 5 files changed, 106 insertions(+), 37 deletions(-) diff --git a/examples/testbed/tests/pinned_cloth.h b/examples/testbed/tests/pinned_cloth.h index da00c56..dfafeab 100644 --- a/examples/testbed/tests/pinned_cloth.h +++ b/examples/testbed/tests/pinned_cloth.h @@ -40,21 +40,48 @@ public: b3ClothDef def; def.mesh = &m_rectangleClothMesh; def.density = 0.2f; - def.structural = 10000.0f; + def.structural = 100000.0f; def.damping = 0.0f; m_cloth = m_world.CreateCloth(def); - b3AABB3 aabb; - aabb.m_lower.Set(-5.0f, -1.0f, -6.0f); - aabb.m_upper.Set(5.0f, 1.0f, -4.0f); + b3AABB3 aabb1; + aabb1.m_lower.Set(-5.0f, -1.0f, -6.0f); + aabb1.m_upper.Set(5.0f, 1.0f, -4.0f); + + b3AABB3 aabb2; + aabb2.m_lower.Set(-5.0f, -1.0f, 4.0f); + aabb2.m_upper.Set(5.0f, 1.0f, 6.0f); for (b3Particle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext()) { - if (aabb.Contains(p->GetPosition())) + if (aabb1.Contains(p->GetPosition())) { p->SetType(e_staticParticle); } + + if (aabb2.Contains(p->GetPosition())) + { + p->SetType(e_staticParticle); + } + } + + { + b3BodyDef bd; + bd.type = e_dynamicBody; + bd.position.Set(0.0f, 5.0f, 0.0f); + + b3Body* b = m_world.CreateBody(bd); + + b3SphereShape sphere; + sphere.m_center.SetZero(); + sphere.m_radius = 2.0f; + + b3ShapeDef sd; + sd.shape = &sphere; + sd.density = 1.0f; + + b3Shape* s = b->CreateShape(sd); } } diff --git a/include/bounce/dynamics/cloth/particle.h b/include/bounce/dynamics/cloth/particle.h index f2025d9..86e60ca 100644 --- a/include/bounce/dynamics/cloth/particle.h +++ b/include/bounce/dynamics/cloth/particle.h @@ -65,6 +65,8 @@ public: b3FrictionForce() { } ~b3FrictionForce() { } + void SolvePositionConstraints(const b3ClothSolverData* data); + void Apply(const b3ClothSolverData* data); b3Particle* m_p; @@ -79,14 +81,21 @@ public: b3Particle* p1; b3Shape* s2; - float32 s; + // Contact constraint + bool n_active; + b3Vec3 p; + b3Vec3 n; + float32 Fn; + + // Friction constraint + bool t1_active, t2_active; + b3Vec3 t1, t2; + float32 Ft1, Ft2; + + // Friction force bool f_active; b3FrictionForce f; - - bool n_active, t1_active, t2_active; - b3Vec3 n, t1, t2; - float32 Fn, Ft1, Ft2; }; // A cloth particle. diff --git a/src/bounce/dynamics/cloth/cloth.cpp b/src/bounce/dynamics/cloth/cloth.cpp index e85f251..d13f65f 100644 --- a/src/bounce/dynamics/cloth/cloth.cpp +++ b/src/bounce/dynamics/cloth/cloth.cpp @@ -509,9 +509,9 @@ void b3Cloth::UpdateContacts() // Create contacts for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) { - // Static and kinematic particles can't participate in unilateral collisions. - if (p->m_type != e_dynamicParticle) + if (p->m_type == e_staticParticle) { + // TODO continue; } @@ -537,8 +537,9 @@ void b3Cloth::UpdateContacts() for (b3Body* body = m_world->GetBodyList().m_head; body; body = body->GetNext()) { - if (body->GetType() != e_staticBody) + if (p->m_type != e_dynamicParticle && body->GetType() != e_dynamicBody) { + // At least one body should be kinematic or dynamic. continue; } @@ -563,13 +564,14 @@ void b3Cloth::UpdateContacts() b3Shape* shape = bestShape; float32 s = bestSeparation; b3Vec3 n = bestNormal; + b3Vec3 cp = p->m_position - s * n; // Store the contact manifold - // Here the normal points from shape 2 to shape 1 (mass) - c->n_active = true; + // Here the normal points from shape 2 to the particle c->p1 = p; c->s2 = shape; - c->s = s; + c->n_active = true; + c->p = cp; c->n = n; c->t1 = b3Perp(n); c->t2 = b3Cross(c->t1, n); @@ -711,7 +713,7 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity) } } - // Solve + // Solve solver.Solve(dt, gravity); // Clear external applied forces and translations @@ -766,9 +768,11 @@ void b3Cloth::Draw() const b3Draw_draw->DrawPoint(p->m_position, 4.0f, b3Color_green); } - if (p->m_contact.n_active) + b3BodyContact* c = &p->m_contact; + + if (c->n_active) { - b3Draw_draw->DrawSegment(p->m_position, p->m_position + p->m_contact.n, b3Color_yellow); + b3Draw_draw->DrawSegment(c->p, c->p + c->n, b3Color_yellow); } } diff --git a/src/bounce/dynamics/cloth/cloth_solver.cpp b/src/bounce/dynamics/cloth/cloth_solver.cpp index 8de1774..60ef6e2 100644 --- a/src/bounce/dynamics/cloth/cloth_solver.cpp +++ b/src/bounce/dynamics/cloth/cloth_solver.cpp @@ -23,6 +23,8 @@ #include #include #include +#include +#include #include // Here, we solve Ax = b using the Modified Preconditioned Conjugate Gradient (MPCG) algorithm. @@ -149,6 +151,11 @@ void b3ClothSolver::ApplyConstraints() b3BodyContact* pc = m_contacts[i]; b3Particle* p = pc->p1; + if (p->m_type != e_dynamicParticle) + { + continue; + } + b3AccelerationConstraint* ac = m_constraints + m_constraintCount; ++m_constraintCount; ac->i1 = p->m_solverId; @@ -230,7 +237,10 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) { b3BodyContact* c = m_contacts[i]; b3Particle* p = c->p1; - sy[p->m_solverId] -= c->s * c->n; + + b3Vec3 dx = c->p - p->m_position; + + sy[p->m_solverId] += dx; } // Apply internal forces @@ -242,7 +252,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) // Solve Ax = b, where // A = M - h * dfdv - h * h * dfdx // b = h * (f0 + h * dfdx * v0 + dfdx * y) - + // A b3SparseSymMat33 A(m_particleCount); for (u32 i = 0; i < m_particleCount; ++i) @@ -257,7 +267,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) // x b3DenseVec3 x(m_particleCount); u32 iterations = 0; - + Solve(x, iterations, A, b, S, z, sx0); b3_clothSolverIterations = iterations; @@ -268,16 +278,14 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) // Copy state buffers back to the particles for (u32 i = 0; i < m_particleCount; ++i) { - m_particles[i]->m_position = sx[i]; - m_particles[i]->m_velocity = sv[i]; - - // Cache x to improve convergence - m_particles[i]->m_x = x[i]; - } + b3Particle* p = m_particles[i]; - // 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. + p->m_position = sx[i]; + p->m_velocity = sv[i]; + + // Cache x to improve convergence + p->m_x = x[i]; + } // f = A * x - b b3DenseVec3 f = A * x - b; @@ -285,16 +293,32 @@ 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; - b3Vec3 force = f[p->m_solverId]; + b3Particle* p1 = c->p1; + b3Body* b2 = c->s2->GetBody(); + + b3Vec3 f1 = f[p1->m_solverId]; + b3Vec3 f2 = -f1; + + // Apply constraint reaction force at the contact point on the body + b2->ApplyForce(f2, c->p, true); + + // Store constraint force acted on the particle // Signed normal force magnitude - c->Fn = b3Dot(force, c->n); + c->Fn = b3Dot(f1, c->n); - // Signed tangent force magnitude - c->Ft1 = b3Dot(force, c->t1); - c->Ft2 = b3Dot(force, c->t2); + if (c->t1_active) + { + // Signed tangent force magnitude + c->Ft1 = b3Dot(f1, c->t1); + } + + if (c->t2_active) + { + // Signed tangent force magnitude + c->Ft2 = b3Dot(f1, c->t2); + } } } diff --git a/src/bounce/dynamics/cloth/particle.cpp b/src/bounce/dynamics/cloth/particle.cpp index 234293e..7cdea5f 100644 --- a/src/bounce/dynamics/cloth/particle.cpp +++ b/src/bounce/dynamics/cloth/particle.cpp @@ -22,6 +22,11 @@ #include #include +void b3FrictionForce::SolvePositionConstraints(const b3ClothSolverData* data) +{ + B3_NOT_USED(data); +} + void b3FrictionForce::Apply(const b3ClothSolverData* data) { // TODO