Incorporate plasticity model. Update the tests

examples/testbed/tests/pinned_softbody.h

@@ -33,7 +33,10 @@ public:
 		def.mesh = &m_mesh;
 		def.density = 0.2f;
 		def.E = 100.0f;
-		def.nu = 0.33f;
+		def.nu = 0.33f;		
+		def.c_yield = 0.1f;
+		def.c_creep = 0.5f;
+		def.c_max = 1.0f;
 
 		m_body = new b3SoftBody(def);
 

examples/testbed/tests/smash_softbody.h

@@ -39,6 +39,9 @@ public:
 		def.density = 0.2f;
 		def.E = 100.0f;
 		def.nu = 0.33f;
+		def.c_yield = 0.6f;
+		def.c_creep = 1.0f;
+		def.c_max = 1.0f;
 
 		m_body = new b3SoftBody(def);
 

include/bounce/softbody/softbody.h

@@ -42,9 +42,12 @@ struct b3SoftBodyRayCastSingleOutput
 // Soft body tetrahedron element
 struct b3SoftBodyElement
 {
-	b3Mat33 K[16]; 
+	b3Mat33 K[16]; // 12 x 12
 	b3Mat33 invE;
 	b3Quat q;
+	float32 B[72]; // 6 x 12
+	float32 P[72]; // V * BT * E -> 12 x 6
+	float32 epsilon_plastic[6]; // 6 x 1
 };
 
 // Soft body tetrahedron triangle
@@ -64,6 +67,9 @@ struct b3SoftBodyDef
 		density = 0.1f;
 		E = 100.0f;
 		nu = 0.3f;
+		c_yield = B3_MAX_FLOAT;
+		c_creep = 0.0f;
+		c_max = 0.0f;
 	}
 
 	// Soft body mesh
@@ -79,6 +85,17 @@ struct b3SoftBodyDef
 	// Material Poisson ratio in [0, 0.5]
 	// This is a dimensionless value
 	float32 nu;
+
+	// Material yield in [0, inf]
+	// This is a dimensionless value
+	float32 c_yield;
+
+	// Material creep rate in [0, 1 / dt]
+	// Units are inverse seconds
+	float32 c_creep;
+
+	// Material maximum plastic strain in [0, inf]
+	float32 c_max;
 };
 
 // A soft body represents a deformable volume as a collection of nodes and elements.
@@ -149,6 +166,15 @@ private:
 	// Material poisson ratio
 	float32 m_nu;
 
+	// Material yield
+	float32 m_c_yield;
+
+	// Material creep rate
+	float32 m_c_creep;
+
+	// Material maximum plastic strain
+	float32 m_c_max;
+
 	// Soft body nodes
 	b3SoftBodyNode* m_nodes;
 

src/bounce/softbody/softbody.cpp

@@ -436,6 +436,9 @@ b3SoftBody::b3SoftBody(const b3SoftBodyDef& def)
 	m_density = def.density;
 	m_E = def.E;
 	m_nu = def.nu;
+	m_c_yield = def.c_yield;
+	m_c_creep = def.c_creep;
+	m_c_max = def.c_max;
 	m_gravity.SetZero();
 	m_world = nullptr;
 
@@ -499,7 +502,7 @@ b3SoftBody::b3SoftBody(const b3SoftBodyDef& def)
 		b3ComputeD(D, m_E, m_nu);
 
 		// 6 x 12
-		float32 B[72];
+		float32* B = e->B;
 		b3ComputeB(B, e10, e20, e30, V);
 
 		// 12 x 6
@@ -519,6 +522,19 @@ b3SoftBody::b3SoftBody(const b3SoftBodyDef& def)
 		}
 
 		b3SetK(e->K, BT_D_B);
+
+		// 12 x 6
+		float32* P = e->P;
+		b3Mul(P, BT, 12, 6, D, 6, 6);
+		for (u32 i = 0; i < 72; ++i)
+		{
+			P[i] *= V;
+		}
+
+		for (u32 i = 0; i < 6; ++i)
+		{
+			e->epsilon_plastic[i] = 0.0f;
+		}
 	}
 
 	// Initialize triangles

src/bounce/softbody/softbody_solver.cpp

@@ -241,6 +241,10 @@ void b3SoftBodySolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIter
 
 		b3Mat33* Ke = e->K;
 
+		float32* Be = e->B;
+		float32* Pe = e->P;
+		float32* epsilon_plastic = e->epsilon_plastic;
+
 		u32 v1 = mt->v1;
 		u32 v2 = mt->v2;
 		u32 v3 = mt->v3;
@@ -285,6 +289,7 @@ void b3SoftBodySolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIter
 			}
 		}
 
+		// Elasticity
 		b3Vec3 x1 = x[v1];
 		b3Vec3 x2 = x[v2];
 		b3Vec3 x3 = x[v3];
@@ -308,13 +313,66 @@ void b3SoftBodySolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIter
 		f0[v2] += f0s[1];
 		f0[v3] += f0s[2];
 		f0[v4] += f0s[3];
+
+		// Plasticity
+		b3Vec3 ps[4] = { p1, p2, p3, p4 };
+
+		b3Vec3 RT_x_x0[4];
+		for (u32 i = 0; i < 4; ++i)
+		{
+			RT_x_x0[i] = RT * ps[i] - xs[i];
+		}
+
+		// 6 x 1
+		float32 epsilon_total[6];
+		b3Mul(epsilon_total, Be, 6, 12, &RT_x_x0[0].x, 12, 1);
+
+		// 6 x 1
+		float32 epsilon_elastic[6];
+		for (u32 i = 0; i < 6; ++i)
+		{
+			epsilon_elastic[i] = epsilon_total[i] - epsilon_plastic[i];
+		}
+
+		float32 len_epsilon_elastic = b3Length(epsilon_elastic, 6);
+		if (len_epsilon_elastic > m_body->m_c_yield)
+		{
+			float32 amount = h * b3Min(m_body->m_c_creep, inv_h);
+			for (u32 i = 0; i < 6; ++i)
+			{
+				epsilon_plastic[i] += amount * epsilon_elastic[i];
+			}
+		}
+
+		float32 len_epsilon_plastic = b3Length(epsilon_plastic, 6);
+		if (len_epsilon_plastic > m_body->m_c_max)
+		{
+			float32 scale = m_body->m_c_max / len_epsilon_plastic;
+			for (u32 i = 0; i < 6; ++i)
+			{
+				epsilon_plastic[i] *= scale;
+			}
+		}
+
+		b3Vec3 fs_plastic[4];
+		b3Mul(&fs_plastic[0].x, Pe, 12, 6, epsilon_plastic, 6, 1);
+		for (u32 i = 0; i < 4; ++i)
+		{
+			fs_plastic[i] = R * fs_plastic[i];
+		}
+
+		f_plastic[v1] += fs_plastic[0];
+		f_plastic[v2] += fs_plastic[1];
+		f_plastic[v3] += fs_plastic[2];
+		f_plastic[v4] += fs_plastic[3];
 	}
 
 	f0 = -f0;
 
 	b3SparseMat33 A = M + h * h * K;
 
-	b3DenseVec3 b = M * v - h * (K * p + f0 + f_plastic - fe);
+	//b3DenseVec3 b = M * v - h * (K * p + f0 + f_plastic - fe);
+	b3DenseVec3 b = M * v - h * (K * p + f0 - (f_plastic + fe));
 
 	// Solve Ax = b
 	b3DenseVec3 sx(m_mesh->vertexCount);