diff --git a/examples/testbed/tests/pinned_softbody.h b/examples/testbed/tests/pinned_softbody.h index 1b4eefe..e058e24 100644 --- a/examples/testbed/tests/pinned_softbody.h +++ b/examples/testbed/tests/pinned_softbody.h @@ -43,7 +43,7 @@ public: for (u32 i = 0; i < m_mesh.vertexCount; ++i) { b3SoftBodyNode* n = m_body->GetVertexNode(i); - n->SetDamping(0.2f); + n->SetMassDamping(0.2f); } u32 pinIndex = ~0; diff --git a/include/bounce/softbody/softbody.h b/include/bounce/softbody/softbody.h index 9316e95..3e17de5 100644 --- a/include/bounce/softbody/softbody.h +++ b/include/bounce/softbody/softbody.h @@ -22,6 +22,8 @@ #include #include +#include + class b3World; struct b3SoftBodyMesh; @@ -56,6 +58,7 @@ struct b3SoftBodyTriangle { u32 v1, v2, v3; u32 tetrahedron; + u32 treeId; }; // Soft body definition @@ -138,6 +141,7 @@ public: // Debug draw the body using the associated mesh. void Draw() const; private: + friend class b3SoftBodyNode; friend class b3SoftBodySolver; // Compute mass of each node. @@ -185,6 +189,9 @@ private: // Soft body triangles b3SoftBodyTriangle* m_triangles; + // Node tree + b3DynamicTree m_nodeTree; + // Attached world b3World* m_world; }; diff --git a/include/bounce/softbody/softbody_node.h b/include/bounce/softbody/softbody_node.h index 3e9021d..7b7dd6f 100644 --- a/include/bounce/softbody/softbody_node.h +++ b/include/bounce/softbody/softbody_node.h @@ -48,21 +48,8 @@ struct b3NodeBodyContact struct b3NodeBodyContactWorldPoint { - void Initialize(const b3NodeBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB) - { - b3Vec3 nA = c->normal1; - - b3Vec3 cA = xfA * c->localPoint1; - b3Vec3 cB = xfB * c->localPoint2; - - b3Vec3 pA = cA + rA * nA; - b3Vec3 pB = cB - rB * nA; - - point = 0.5f * (pA + pB); - normal = nA; - separation = b3Dot(cB - cA, nA) - rA - rB; - } - + void Initialize(const b3NodeBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB); + b3Vec3 point; b3Vec3 normal; float32 separation; @@ -108,11 +95,11 @@ public: // Get the node mass. float32 GetMass() const; - // Set the node damping. - void SetDamping(float32 damping); + // Set the node mass damping. + void SetMassDamping(float32 damping); - // Get the node damping. - float32 GetDamping() const; + // Get the node mass damping. + float32 GetMassDamping() const; // Set the node radius. void SetRadius(float32 radius); @@ -137,6 +124,9 @@ private: ~b3SoftBodyNode() { } + // Synchronize node + void Synchronize(); + // Type b3SoftBodyNodeType m_type; @@ -155,8 +145,8 @@ private: // Inverse mass float32 m_invMass; - // Damping - float32 m_damping; + // Mass damping + float32 m_massDamping; // Radius float32 m_radius; @@ -173,6 +163,9 @@ private: // Node and body contact b3NodeBodyContact m_bodyContact; + // Tree identifier + u32 m_treeId; + // Soft body b3SoftBody* m_body; }; @@ -190,6 +183,7 @@ inline void b3SoftBodyNode::SetType(b3SoftBodyNodeType type) if (type == e_staticSoftBodyNode) { m_velocity.SetZero(); + Synchronize(); } m_bodyContact.active = false; @@ -208,6 +202,8 @@ inline u32 b3SoftBodyNode::GetVertex() const inline void b3SoftBodyNode::SetPosition(const b3Vec3& position) { m_position = position; + + Synchronize(); } inline const b3Vec3& b3SoftBodyNode::GetPosition() const @@ -234,19 +230,21 @@ inline float32 b3SoftBodyNode::GetMass() const return m_mass; } -inline void b3SoftBodyNode::SetDamping(float32 damping) +inline void b3SoftBodyNode::SetMassDamping(float32 massDamping) { - m_damping = damping; + m_massDamping = massDamping; } -inline float32 b3SoftBodyNode::GetDamping() const +inline float32 b3SoftBodyNode::GetMassDamping() const { - return m_damping; + return m_massDamping; } inline void b3SoftBodyNode::SetRadius(float32 radius) { m_radius = radius; + + Synchronize(); } inline float32 b3SoftBodyNode::GetRadius() const diff --git a/src/bounce/softbody/softbody.cpp b/src/bounce/softbody/softbody.cpp index d326420..c8c2936 100644 --- a/src/bounce/softbody/softbody.cpp +++ b/src/bounce/softbody/softbody.cpp @@ -25,6 +25,7 @@ #include #include +#include #include #include @@ -451,9 +452,8 @@ b3SoftBody::b3SoftBody(const b3SoftBodyDef& def) const b3SoftBodyMesh* m = m_mesh; - m_nodes = (b3SoftBodyNode*)b3Alloc(m->vertexCount * sizeof(b3SoftBodyNode)); - // Initialize nodes + m_nodes = (b3SoftBodyNode*)b3Alloc(m->vertexCount * sizeof(b3SoftBodyNode)); for (u32 i = 0; i < m->vertexCount; ++i) { b3SoftBodyNode* n = m_nodes + i; @@ -465,12 +465,17 @@ b3SoftBody::b3SoftBody(const b3SoftBodyDef& def) n->m_force.SetZero(); n->m_mass = 0.0f; n->m_invMass = 0.0f; - n->m_damping = 0.0f; + n->m_massDamping = 0.0f; n->m_radius = 0.0f; n->m_friction = 0.0f; n->m_userData = nullptr; n->m_vertex = i; n->m_bodyContact.active = false; + + b3AABB3 aabb; + aabb.Set(n->m_position, 0.0f); + + n->m_treeId = m_nodeTree.InsertNode(aabb, n); } // Compute mass @@ -703,6 +708,43 @@ void b3SoftBody::ComputeMass() } } +class b3SoftBodyUpdateContactsQueryListener : public b3QueryListener +{ +public: + bool ReportShape(b3Shape* shape) + { + b3Body* body = shape->GetBody(); + + if (body->GetType() != e_staticBody) + { + //continue; + } + + 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; + } + } + + return true; + } + + b3SoftBodyNode* node; + b3Sphere sphere; + b3Shape* bestShape; + float32 bestSeparation; + b3Vec3 bestPoint; + b3Vec3 bestNormal; +}; + void b3SoftBody::UpdateContacts() { B3_PROFILE("Soft Body Update Contacts"); @@ -718,56 +760,33 @@ void b3SoftBody::UpdateContacts() { b3SoftBodyNode* n = m_nodes + i; - b3Sphere s1; - s1.vertex = n->m_position; - s1.radius = n->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 (n->m_type != e_dynamicSoftBodyNode) { - if (n->m_type != e_dynamicSoftBodyNode) - { - 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_nodeTree.GetAABB(n->m_treeId); + + b3SoftBodyUpdateContactsQueryListener listener; + listener.node = n; + listener.sphere.vertex = n->m_position; + listener.sphere.radius = n->m_radius; + listener.bestShape = nullptr; + listener.bestSeparation = 0.0f; + + m_world->QueryAABB(&listener, aabb); + + if (listener.bestShape == nullptr) { n->m_bodyContact.active = false; continue; } - 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; b3NodeBodyContact* c = &n->m_bodyContact; @@ -822,6 +841,19 @@ void b3SoftBody::Step(float32 dt, u32 velocityIterations, u32 positionIterations { m_nodes[i].m_force.SetZero(); } + + // Synchronize nodes + for (u32 i = 0; i < m_mesh->vertexCount; ++i) + { + b3SoftBodyNode* n = m_nodes + i; + + if (n->m_type == e_staticSoftBodyNode) + { + continue; + } + + n->Synchronize(); + } } void b3SoftBody::Draw() const diff --git a/src/bounce/softbody/softbody_node.cpp b/src/bounce/softbody/softbody_node.cpp new file mode 100644 index 0000000..e18ffed --- /dev/null +++ b/src/bounce/softbody/softbody_node.cpp @@ -0,0 +1,43 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include + +void b3NodeBodyContactWorldPoint::Initialize(const b3NodeBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB) +{ + b3Vec3 nA = c->normal1; + + b3Vec3 cA = xfA * c->localPoint1; + b3Vec3 cB = xfB * c->localPoint2; + + b3Vec3 pA = cA + rA * nA; + b3Vec3 pB = cB - rB * nA; + + point = 0.5f * (pA + pB); + normal = nA; + separation = b3Dot(cB - cA, nA) - rA - rB; +} + +void b3SoftBodyNode::Synchronize() +{ + b3AABB3 aabb; + aabb.Set(m_position, m_radius); + + m_body->m_nodeTree.UpdateNode(m_treeId, aabb); +} \ No newline at end of file diff --git a/src/bounce/softbody/softbody_solver.cpp b/src/bounce/softbody/softbody_solver.cpp index 9907e7a..1b831e3 100644 --- a/src/bounce/softbody/softbody_solver.cpp +++ b/src/bounce/softbody/softbody_solver.cpp @@ -216,7 +216,7 @@ void b3SoftBodySolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIter // Rayleigh damping // C = alpha * M + beta * K // Here the stiffness coefficient beta is zero - C(i, i) = b3Diagonal(n->m_damping * n->m_mass); + C(i, i) = b3Diagonal(n->m_massDamping * n->m_mass); x[i] = m_mesh->vertices[i]; p[i] = n->m_position;