diff --git a/include/bounce/cloth/cloth.h b/include/bounce/cloth/cloth.h index a200861..e24e17f 100644 --- a/include/bounce/cloth/cloth.h +++ b/include/bounce/cloth/cloth.h @@ -23,6 +23,7 @@ #include #include #include +#include class b3World; class b3Shape; @@ -138,12 +139,11 @@ public: // Debug draw the cloth using the associated cloth mesh. void Draw() const; private: + friend class b3Particle; + // Compute mass of each particle. void ComputeMass(); - // Update body contacts. - void UpdateBodyContacts(); - // Update contacts void UpdateContacts(); @@ -173,7 +173,10 @@ private: // List of particles b3List2 m_particleList; - + + // Particle tree + b3DynamicTree m_particleTree; + // List of forces b3List2 m_forceList; }; diff --git a/include/bounce/cloth/particle.h b/include/bounce/cloth/particle.h index b496662..6d96c4a 100644 --- a/include/bounce/cloth/particle.h +++ b/include/bounce/cloth/particle.h @@ -152,6 +152,9 @@ private: b3Particle(const b3ParticleDef& def, b3Cloth* cloth); ~b3Particle(); + // Synchronize AABB + void Synchronize(); + // Type b3ParticleType m_type; @@ -193,12 +196,15 @@ private: // Solution b3Vec3 m_x; - // + // Parent cloth b3Cloth* m_cloth; // Contact b3ParticleBodyContact m_bodyContact; + // Particle tree identifier + u32 m_treeId; + // b3Particle* m_prev; @@ -220,6 +226,8 @@ inline void b3Particle::SetPosition(const b3Vec3& position) { m_position = position; m_translation.SetZero(); + + Synchronize(); } inline const b3Vec3& b3Particle::GetPosition() const @@ -249,6 +257,8 @@ inline float32 b3Particle::GetMass() const inline void b3Particle::SetRadius(float32 radius) { m_radius = radius; + + Synchronize(); } inline float32 b3Particle::GetRadius() const diff --git a/src/bounce/cloth/cloth.cpp b/src/bounce/cloth/cloth.cpp index b86c517..a5be051 100644 --- a/src/bounce/cloth/cloth.cpp +++ b/src/bounce/cloth/cloth.cpp @@ -22,10 +22,14 @@ #include #include #include + #include +#include #include #include + #include + #include static B3_FORCE_INLINE u32 b3NextIndex(u32 i) @@ -157,9 +161,8 @@ b3Cloth::b3Cloth(const b3ClothDef& def) : const b3ClothMesh* m = m_mesh; - m_vertexParticles = (b3Particle**)b3Alloc(m->vertexCount * sizeof(b3Particle*)); - // Create particles + m_vertexParticles = (b3Particle**)b3Alloc(m->vertexCount * sizeof(b3Particle*)); for (u32 i = 0; i < m->vertexCount; ++i) { b3ParticleDef pd; @@ -170,6 +173,11 @@ b3Cloth::b3Cloth(const b3ClothDef& def) : p->m_vertex = i; + b3AABB3 aabb; + aabb.Set(p->m_position, p->m_radius); + + p->m_treeId = m_particleTree.InsertNode(aabb, p); + m_vertexParticles[i] = p; } @@ -245,6 +253,9 @@ b3Cloth::~b3Cloth() { b3Particle* p0 = p; p = p->m_next; + + m_particleTree.RemoveNode(p0->m_treeId); + p0->~b3Particle(); } @@ -264,7 +275,14 @@ b3Particle* b3Cloth::CreateParticle(const b3ParticleDef& def) { void* mem = m_particleBlocks.Allocate(); b3Particle* p = new(mem) b3Particle(def, this); + + b3AABB3 aabb; + aabb.Set(p->m_position, p->m_radius); + + p->m_treeId = m_particleTree.InsertNode(aabb, p); + m_particleList.PushFront(p); + return p; } @@ -275,6 +293,8 @@ void b3Cloth::DestroyParticle(b3Particle* particle) m_vertexParticles[particle->m_vertex] = NULL; } + m_particleTree.RemoveNode(particle->m_treeId); + m_particleList.Remove(particle); particle->~b3Particle(); m_particleBlocks.Free(particle); @@ -399,9 +419,43 @@ bool b3Cloth::RayCast(b3RayCastOutput* output, const b3RayCastInput* input, u32 return b3RayCast(output, input, v1, v2, v3); } -void b3Cloth::UpdateBodyContacts() +class b3ClothUpdateContactsQueryListener : public b3QueryListener { - B3_PROFILE("Cloth Update Body Contacts"); +public: + bool ReportShape(b3Shape* shape) + { + b3Body* body = shape->GetBody(); + + if (body->GetType() != e_staticBody) + { + return true; + } + + b3Transform xf = body->GetTransform(); + + b3TestSphereOutput output; + if (shape->TestSphere(&output, sphere, xf)) + { + if (output.separation < bestSeparation) + { + bestShape = shape; + bestSeparation = output.separation; + bestPoint = output.point; + bestNormal = output.normal; + } + } + } + + b3Sphere sphere; + b3Shape* bestShape; + float32 bestSeparation; + b3Vec3 bestPoint; + b3Vec3 bestNormal; +}; + +void b3Cloth::UpdateContacts() +{ + B3_PROFILE("Cloth Update Contacts"); // Is there a world attached to this cloth? if (m_world == nullptr) @@ -412,57 +466,32 @@ void b3Cloth::UpdateBodyContacts() // Create contacts for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) { - b3Sphere s1; - s1.vertex = p->m_position; - s1.radius = p->m_radius; - - // Find the deepest penetration - b3Shape* bestShape = nullptr; - float32 bestSeparation = 0.0f; - b3Vec3 bestPoint(0.0f, 0.0f, 0.0f); - b3Vec3 bestNormal(0.0f, 0.0f, 0.0f); - - for (b3Body* body = m_world->GetBodyList().m_head; body; body = body->GetNext()) + if (p->m_type != e_dynamicParticle) { - if (p->m_type != e_dynamicParticle) - { - continue; - } - - if (body->GetType() != e_staticBody) - { - continue; - } - - b3Transform xf = body->GetTransform(); - for (b3Shape* shape = body->GetShapeList().m_head; shape; shape = shape->GetNext()) - { - b3TestSphereOutput output; - if (shape->TestSphere(&output, s1, xf)) - { - if (output.separation < bestSeparation) - { - bestShape = shape; - bestSeparation = output.separation; - bestPoint = output.point; - bestNormal = output.normal; - } - } - } + continue; } - if (bestShape == nullptr) + b3AABB3 aabb = m_particleTree.GetAABB(p->m_treeId); + + b3ClothUpdateContactsQueryListener listener; + listener.sphere.vertex = p->m_position; + listener.sphere.radius = p->m_radius; + listener.bestShape = nullptr; + listener.bestSeparation = 0.0f; + + m_world->QueryAABB(&listener, aabb); + + if (listener.bestShape == nullptr) { p->m_bodyContact.active = false; continue; } - // Ensure the the normal points from the particle 1 to shape 2 - b3Shape* shape = bestShape; + b3Shape* shape = listener.bestShape; b3Body* body = shape->GetBody(); - float32 separation = bestSeparation; - b3Vec3 point = bestPoint; - b3Vec3 normal = -bestNormal; + float32 separation = listener.bestSeparation; + b3Vec3 point = listener.bestPoint; + b3Vec3 normal = -listener.bestNormal; b3ParticleBodyContact* c = &p->m_bodyContact; @@ -522,12 +551,6 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u solver.Solve(dt, gravity, velocityIterations, positionIterations); } -void b3Cloth::UpdateContacts() -{ - // Update body contacts - UpdateBodyContacts(); -} - void b3Cloth::Step(float32 dt, u32 velocityIterations, u32 positionIterations) { B3_PROFILE("Cloth Step"); @@ -535,7 +558,7 @@ void b3Cloth::Step(float32 dt, u32 velocityIterations, u32 positionIterations) // Update contacts UpdateContacts(); - // Solve constraints, integrate state, clear forces and translations. + // Integrate state, solve constraints. if (dt > 0.0f) { Solve(dt, m_gravity, velocityIterations, positionIterations); @@ -547,6 +570,17 @@ void b3Cloth::Step(float32 dt, u32 velocityIterations, u32 positionIterations) p->m_force.SetZero(); p->m_translation.SetZero(); } + + // Synchronize particles + for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) + { + if (p->m_type == e_staticParticle) + { + continue; + } + + p->Synchronize(); + } } void b3Cloth::Draw() const diff --git a/src/bounce/cloth/cloth_solver.cpp b/src/bounce/cloth/cloth_solver.cpp index 665b27a..0daf1a8 100644 --- a/src/bounce/cloth/cloth_solver.cpp +++ b/src/bounce/cloth/cloth_solver.cpp @@ -379,7 +379,7 @@ void b3ClothSolver::SolveMPCG(b3DenseVec3& x, u32 iteration = 0; for (;;) { - if (iteration >= maxIterations) + if (iteration == maxIterations) { break; } diff --git a/src/bounce/cloth/particle.cpp b/src/bounce/cloth/particle.cpp index a0e9fad..3316a77 100644 --- a/src/bounce/cloth/particle.cpp +++ b/src/bounce/cloth/particle.cpp @@ -18,9 +18,6 @@ #include #include -#include -#include -#include void b3ParticleBodyContactWorldPoint::Initialize(const b3ParticleBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB) { @@ -60,6 +57,14 @@ b3Particle::~b3Particle() } +void b3Particle::Synchronize() +{ + b3AABB3 aabb; + aabb.Set(m_position, m_radius); + + m_cloth->m_particleTree.UpdateNode(m_treeId, aabb); +} + void b3Particle::SetType(b3ParticleType type) { if (m_type == type) @@ -74,6 +79,8 @@ void b3Particle::SetType(b3ParticleType type) { m_velocity.SetZero(); m_translation.SetZero(); + + Synchronize(); } m_bodyContact.active = false; diff --git a/src/bounce/softbody/softbody.cpp b/src/bounce/softbody/softbody.cpp index 388ea3b..fe97ac4 100644 --- a/src/bounce/softbody/softbody.cpp +++ b/src/bounce/softbody/softbody.cpp @@ -717,7 +717,7 @@ public: if (body->GetType() != e_staticBody) { - //continue; + // return true; } b3Transform xf = body->GetTransform();