Use a node tree. Rename damping to mass damping.

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;

include/bounce/softbody/softbody.h

@@ -22,6 +22,8 @@
 #include <bounce/common/math/transform.h>
 #include <bounce/common/memory/stack_allocator.h>
 
+#include <bounce/collision/trees/dynamic_tree.h>
+
 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;
 };

include/bounce/softbody/softbody_node.h

@@ -48,20 +48,7 @@ struct b3NodeBodyContact
 
 struct b3NodeBodyContactWorldPoint
 {
-	void Initialize(const b3NodeBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB)
+	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;
-	}
 	
 	b3Vec3 point;
 	b3Vec3 normal;
@@ -108,11 +95,11 @@ public:
 	// Get the node mass.
 	float32 GetMass() const;
 
-	// Set the node damping.
+	// Set the node mass damping.
-	void SetDamping(float32 damping);
+	void SetMassDamping(float32 damping);
 
-	// Get the node damping.
+	// Get the node mass damping.
-	float32 GetDamping() const;
+	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
+	// Mass damping
-	float32 m_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

src/bounce/softbody/softbody.cpp

@@ -25,6 +25,7 @@
 #include <bounce/collision/collision.h>
 
 #include <bounce/dynamics/world.h>
+#include <bounce/dynamics/world_listeners.h>
 #include <bounce/dynamics/body.h>
 #include <bounce/dynamics/shapes/shape.h>
 
@@ -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)
 		{
 			continue;
 		}
 
-			if (body->GetType() != e_staticBody)
+		b3AABB3 aabb = m_nodeTree.GetAABB(n->m_treeId);
-			{
-				//continue;
-			}
 
-			b3Transform xf = body->GetTransform();
+		b3SoftBodyUpdateContactsQueryListener listener;
-			for (b3Shape* shape = body->GetShapeList().m_head; shape; shape = shape->GetNext())
+		listener.node = n;
-			{
+		listener.sphere.vertex = n->m_position;
-				b3TestSphereOutput output;
+		listener.sphere.radius = n->m_radius;
-				if (shape->TestSphere(&output, s1, xf))
+		listener.bestShape = nullptr;
-				{
+		listener.bestSeparation = 0.0f;
-					if (output.separation < bestSeparation)
-					{
-						bestShape = shape;
-						bestSeparation = output.separation;
-						bestPoint = output.point;
-						bestNormal = output.normal;
-					}
-				}
-			}
-		}
 
-		if (bestShape == nullptr)
+		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;
+		float32 separation = listener.bestSeparation;
-		b3Vec3 point = bestPoint;
+		b3Vec3 point = listener.bestPoint;
-		b3Vec3 normal = -bestNormal;
+		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

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 <bounce/softbody/softbody_node.h>
+#include <bounce/softbody/softbody.h>
+
+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);
+}

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;