diff --git a/include/bounce/softbody/softbody.h b/include/bounce/softbody/softbody.h index a9b6d0d..b9f7c2c 100644 --- a/include/bounce/softbody/softbody.h +++ b/include/bounce/softbody/softbody.h @@ -42,7 +42,7 @@ struct b3SoftBodyRayCastSingleOutput // Soft body tetrahedron element struct b3SoftBodyElement { - b3Mat33 K[16]; + b3Mat33 K[16]; b3Mat33 invE; b3Quat q; }; @@ -120,6 +120,8 @@ public: // Debug draw the body using the associated mesh. void Draw() const; private: + friend class b3SoftBodySolver; + // Compute mass of each node. void ComputeMass(); diff --git a/include/bounce/softbody/softbody_solver.h b/include/bounce/softbody/softbody_solver.h index 30661da..3018b38 100644 --- a/include/bounce/softbody/softbody_solver.h +++ b/include/bounce/softbody/softbody_solver.h @@ -24,6 +24,7 @@ class b3StackAllocator; +class b3SoftBody; class b3SoftBodyMesh; class b3SoftBodyNode; @@ -33,10 +34,7 @@ class b3NodeBodyContact; struct b3SoftBodySolverDef { - b3StackAllocator* stack; - const b3SoftBodyMesh* mesh; - b3SoftBodyNode* nodes; - b3SoftBodyElement* elements; + b3SoftBody* body; }; class b3SoftBodySolver @@ -47,6 +45,7 @@ public: void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations); private: + b3SoftBody* m_body; b3StackAllocator* m_allocator; const b3SoftBodyMesh* m_mesh; b3SoftBodyNode* m_nodes; diff --git a/src/bounce/softbody/softbody.cpp b/src/bounce/softbody/softbody.cpp index d3395d1..186fbe3 100644 --- a/src/bounce/softbody/softbody.cpp +++ b/src/bounce/softbody/softbody.cpp @@ -82,16 +82,16 @@ static B3_FORCE_INLINE void b3ComputeD(float32 out[36], float32 E, float32 nu) } } -// Compute derivatives of shape functions N -static B3_FORCE_INLINE void b3ComputeBVec(b3Vec3 out[4], - const b3Vec3& e10, - const b3Vec3& e20, +static B3_FORCE_INLINE void b3ComputeB(float32 out[72], + const b3Vec3& e10, + const b3Vec3& e20, const b3Vec3& e30, float32 V) { + // Compute derivatives of shape functions N float32 inv6V = 1.0f / (6.0f * V); - b3Vec3* B = out; + b3Vec3 B[4]; B[1][0] = (e20[2] * e30[1] - e20[1] * e30[2]) * inv6V; B[2][0] = (e10[1] * e30[2] - e10[2] * e30[1]) * inv6V; @@ -101,35 +101,332 @@ static B3_FORCE_INLINE void b3ComputeBVec(b3Vec3 out[4], B[1][1] = (e20[0] * e30[2] - e20[2] * e30[0]) * inv6V; B[2][1] = (e10[2] * e30[0] - e10[0] * e30[2]) * inv6V; B[3][1] = (e10[0] * e20[2] - e10[2] * e20[0]) * inv6V; - B[0][1] = -B[1][1] - B[2][1] - B[3][1]; + B[0][1] = -B[1][1] - B[2][1] - B[3][1]; B[1][2] = (e20[1] * e30[0] - e20[0] * e30[1]) * inv6V; B[2][2] = (e10[0] * e30[1] - e10[1] * e30[0]) * inv6V; B[3][2] = (e10[1] * e20[0] - e10[0] * e20[1]) * inv6V; - B[0][2] = -B[1][2] - B[2][2] - B[3][2]; -} + B[0][2] = -B[1][2] - B[2][2] - B[3][2]; -// Convert a B vector to its corresponding matrix form. -// This is a 6 x 3 matrix. -static B3_FORCE_INLINE void b3ComputeBMat(float32 out[18], const b3Vec3& v) -{ - float32 bi = v.x; - float32 ci = v.y; - float32 di = v.z; + float32 b1 = B[0].x; + float32 c1 = B[0].y; + float32 d1 = B[0].z; - float32 B[18] = + float32 b2 = B[1].x; + float32 c2 = B[1].y; + float32 d2 = B[1].z; + + float32 b3 = B[2].x; + float32 c3 = B[2].y; + float32 d3 = B[2].z; + + float32 b4 = B[3].x; + float32 c4 = B[3].y; + float32 d4 = B[3].z; + + float32 MB[72] = { - bi, 0, 0, ci, di, 0, - 0, ci, 0, bi, 0, di, - 0, 0, di, 0, bi, ci + b1, 0, 0, c1, d1, 0, + 0, c1, 0, b1, 0, d1, + 0, 0, d1, 0, b1, c1, + + b2, 0, 0, c2, d2, 0, + 0, c2, 0, b2, 0, d2, + 0, 0, d2, 0, b2, c2, + + b3, 0, 0, c3, d3, 0, + 0, c3, 0, b3, 0, d3, + 0, 0, d3, 0, b3, c3, + + b4, 0, 0, c4, d4, 0, + 0, c4, 0, b4, 0, d4, + 0, 0, d4, 0, b4, c4, }; - for (u32 i = 0; i < 18; ++i) + for (u32 i = 0; i < 72; ++i) { - out[i] = B[i]; + out[i] = MB[i]; } } +static B3_FORCE_INLINE void b3SetK(b3Mat33 K[16], float32 Ke[144]) +{ + b3Mat33& k11 = K[0 + 4 * 0]; + b3Mat33& k12 = K[0 + 4 * 1]; + b3Mat33& k13 = K[0 + 4 * 2]; + b3Mat33& k14 = K[0 + 4 * 3]; + + b3Mat33& k21 = K[1 + 4 * 0]; + b3Mat33& k22 = K[1 + 4 * 1]; + b3Mat33& k23 = K[1 + 4 * 2]; + b3Mat33& k24 = K[1 + 4 * 3]; + + b3Mat33& k31 = K[2 + 4 * 0]; + b3Mat33& k32 = K[2 + 4 * 1]; + b3Mat33& k33 = K[2 + 4 * 2]; + b3Mat33& k34 = K[2 + 4 * 3]; + + b3Mat33& k41 = K[3 + 4 * 0]; + b3Mat33& k42 = K[3 + 4 * 1]; + b3Mat33& k43 = K[3 + 4 * 2]; + b3Mat33& k44 = K[3 + 4 * 3]; + + // k11 + // a11 a12 a13 + // a21 a22 a23 + // a31 a32 a33 + k11.x.x = Ke[0 + 12 * 0]; + k11.x.y = Ke[1 + 12 * 0]; + k11.x.z = Ke[2 + 12 * 0]; + + k11.y.x = Ke[0 + 12 * 1]; + k11.y.y = Ke[1 + 12 * 1]; + k11.y.z = Ke[2 + 12 * 1]; + + k11.z.x = Ke[0 + 12 * 2]; + k11.z.y = Ke[1 + 12 * 2]; + k11.z.z = Ke[2 + 12 * 2]; + + // k12 + // a14 a15 a16 + // a24 a25 a26 + // a34 a35 a36 + k12.x.x = Ke[0 + 12 * 3]; + k12.x.y = Ke[1 + 12 * 3]; + k12.x.z = Ke[2 + 12 * 3]; + + k12.y.x = Ke[0 + 12 * 4]; + k12.y.y = Ke[1 + 12 * 4]; + k12.y.z = Ke[2 + 12 * 4]; + + k12.z.x = Ke[0 + 12 * 5]; + k12.z.y = Ke[1 + 12 * 5]; + k12.z.z = Ke[2 + 12 * 5]; + + // k13 + // a17 a18 a19 + // a27 a28 a29 + // a37 a38 a39 + k13.x.x = Ke[0 + 12 * 6]; + k13.x.y = Ke[1 + 12 * 6]; + k13.x.z = Ke[2 + 12 * 6]; + + k13.y.x = Ke[0 + 12 * 7]; + k13.y.y = Ke[1 + 12 * 7]; + k13.y.z = Ke[2 + 12 * 7]; + + k13.z.x = Ke[0 + 12 * 8]; + k13.z.y = Ke[1 + 12 * 8]; + k13.z.z = Ke[2 + 12 * 8]; + + // k14 + // a1_10 a1_11 a1_12 + // a2_10 a2_11 a2_12 + // a3_10 a3_11 a3_12 + k14.x.x = Ke[0 + 12 * 9]; + k14.x.y = Ke[1 + 12 * 9]; + k14.x.z = Ke[2 + 12 * 9]; + + k14.y.x = Ke[0 + 12 * 10]; + k14.y.y = Ke[1 + 12 * 10]; + k14.y.z = Ke[2 + 12 * 10]; + + k14.z.x = Ke[0 + 12 * 11]; + k14.z.y = Ke[1 + 12 * 11]; + k14.z.z = Ke[2 + 12 * 11]; + + // k21 + // a41 a42 a43 + // a51 a52 a53 + // a61 a62 a63 + k21.x.x = Ke[3 + 12 * 0]; + k21.x.y = Ke[4 + 12 * 0]; + k21.x.z = Ke[5 + 12 * 0]; + + k21.y.x = Ke[3 + 12 * 1]; + k21.y.y = Ke[4 + 12 * 1]; + k21.y.z = Ke[5 + 12 * 1]; + + k21.z.x = Ke[3 + 12 * 2]; + k21.z.y = Ke[4 + 12 * 2]; + k21.z.z = Ke[5 + 12 * 2]; + + // k22 + // a44 a45 a46 + // a54 a55 a56 + // a64 a65 a66 + k22.x.x = Ke[3 + 12 * 3]; + k22.x.y = Ke[4 + 12 * 3]; + k22.x.z = Ke[5 + 12 * 3]; + + k22.y.x = Ke[3 + 12 * 4]; + k22.y.y = Ke[4 + 12 * 4]; + k22.y.z = Ke[5 + 12 * 4]; + + k22.z.x = Ke[3 + 12 * 5]; + k22.z.y = Ke[4 + 12 * 5]; + k22.z.z = Ke[5 + 12 * 5]; + + // k23 + // a47 a48 a49 + // a57 a58 a59 + // a67 a68 a69 + k23.x.x = Ke[3 + 12 * 6]; + k23.x.y = Ke[4 + 12 * 6]; + k23.x.z = Ke[5 + 12 * 6]; + + k23.y.x = Ke[3 + 12 * 7]; + k23.y.y = Ke[4 + 12 * 7]; + k23.y.z = Ke[5 + 12 * 7]; + + k23.z.x = Ke[3 + 12 * 8]; + k23.z.y = Ke[4 + 12 * 8]; + k23.z.z = Ke[5 + 12 * 8]; + + // k24 + // a4_10 a4_11 a4_12 + // a5_10 a5_11 a5_12 + // a6_10 a6_11 a6_12 + k24.x.x = Ke[3 + 12 * 9]; + k24.x.y = Ke[4 + 12 * 9]; + k24.x.z = Ke[5 + 12 * 9]; + + k24.y.x = Ke[3 + 12 * 10]; + k24.y.y = Ke[4 + 12 * 10]; + k24.y.z = Ke[5 + 12 * 10]; + + k24.z.x = Ke[3 + 12 * 11]; + k24.z.y = Ke[4 + 12 * 11]; + k24.z.z = Ke[5 + 12 * 11]; + + // k31 + // a71 a72 a73 + // a81 a82 a83 + // a91 a92 a93 + k31.x.x = Ke[6 + 12 * 0]; + k31.x.y = Ke[7 + 12 * 0]; + k31.x.z = Ke[8 + 12 * 0]; + + k31.y.x = Ke[6 + 12 * 1]; + k31.y.y = Ke[7 + 12 * 1]; + k31.y.z = Ke[8 + 12 * 1]; + + k31.z.x = Ke[6 + 12 * 2]; + k31.z.y = Ke[7 + 12 * 2]; + k31.z.z = Ke[8 + 12 * 2]; + + // k32 + // a74 a75 a76 + // a84 a85 a86 + // a94 a95 a96 + k32.x.x = Ke[6 + 12 * 3]; + k32.x.y = Ke[7 + 12 * 3]; + k32.x.z = Ke[8 + 12 * 3]; + + k32.y.x = Ke[6 + 12 * 4]; + k32.y.y = Ke[7 + 12 * 4]; + k32.y.z = Ke[8 + 12 * 4]; + + k32.z.x = Ke[6 + 12 * 5]; + k32.z.y = Ke[7 + 12 * 5]; + k32.z.z = Ke[8 + 12 * 5]; + + // k33 + // a77 a78 a79 + // a87 a88 a89 + // a97 a98 a99 + k33.x.x = Ke[6 + 12 * 6]; + k33.x.y = Ke[7 + 12 * 6]; + k33.x.z = Ke[8 + 12 * 6]; + + k33.y.x = Ke[6 + 12 * 7]; + k33.y.y = Ke[7 + 12 * 7]; + k33.y.z = Ke[8 + 12 * 7]; + + k33.z.x = Ke[6 + 12 * 8]; + k33.z.y = Ke[7 + 12 * 8]; + k33.z.z = Ke[8 + 12 * 8]; + + // k34 + // a7_10 a7_11 a7_12 + // a8_10 a8_11 a8_12 + // a9_10 a9_11 a9_12 + k34.x.x = Ke[6 + 12 * 9]; + k34.x.y = Ke[7 + 12 * 9]; + k34.x.z = Ke[8 + 12 * 9]; + + k34.y.x = Ke[6 + 12 * 10]; + k34.y.y = Ke[7 + 12 * 10]; + k34.y.z = Ke[8 + 12 * 10]; + + k34.z.x = Ke[6 + 12 * 11]; + k34.z.y = Ke[7 + 12 * 11]; + k34.z.z = Ke[8 + 12 * 11]; + + // k41 + // a10_1 a10_2 a10_3 + // a11_1 a11_2 a11_3 + // a12_1 a12_2 a12_3 + k41.x.x = Ke[9 + 12 * 0]; + k41.x.y = Ke[10 + 12 * 0]; + k41.x.z = Ke[11 + 12 * 0]; + + k41.y.x = Ke[9 + 12 * 1]; + k41.y.y = Ke[10 + 12 * 1]; + k41.y.z = Ke[11 + 12 * 1]; + + k41.z.x = Ke[9 + 12 * 2]; + k41.z.y = Ke[10 + 12 * 2]; + k41.z.z = Ke[11 + 12 * 2]; + + // k42 + // a10_4 a10_5 a10_6 + // a11_4 a11_5 a11_6 + // a12_4 a12_5 a12_6 + k42.x.x = Ke[9 + 12 * 3]; + k42.x.y = Ke[10 + 12 * 3]; + k42.x.z = Ke[11 + 12 * 3]; + + k42.y.x = Ke[9 + 12 * 4]; + k42.y.y = Ke[10 + 12 * 4]; + k42.y.z = Ke[11 + 12 * 4]; + + k42.z.x = Ke[9 + 12 * 5]; + k42.z.y = Ke[10 + 12 * 5]; + k42.z.z = Ke[11 + 12 * 5]; + + // k43 + // a10_7 a10_8 a10_9 + // a11_7 a11_8 a11_9 + // a12_7 a12_8 a12_9 + k43.x.x = Ke[9 + 12 * 6]; + k43.x.y = Ke[10 + 12 * 6]; + k43.x.z = Ke[11 + 12 * 6]; + + k43.y.x = Ke[9 + 12 * 7]; + k43.y.y = Ke[10 + 12 * 7]; + k43.y.z = Ke[11 + 12 * 7]; + + k43.z.x = Ke[9 + 12 * 8]; + k43.z.y = Ke[10 + 12 * 8]; + k43.z.z = Ke[11 + 12 * 8]; + + // k44 + // a10_10 a10_11 a10_12 + // a11_10 a11_11 a11_12 + // a12_10 a12_11 a12_12 + k44.x.x = Ke[9 + 12 * 9]; + k44.x.y = Ke[10 + 12 * 9]; + k44.x.z = Ke[11 + 12 * 9]; + + k44.y.x = Ke[9 + 12 * 10]; + k44.y.y = Ke[10 + 12 * 10]; + k44.y.z = Ke[11 + 12 * 10]; + + k44.z.x = Ke[9 + 12 * 11]; + k44.z.y = Ke[10 + 12 * 11]; + k44.z.z = Ke[11 + 12 * 11]; +} b3SoftBody::b3SoftBody(const b3SoftBodyDef& def) { B3_ASSERT(def.mesh); @@ -201,36 +498,27 @@ b3SoftBody::b3SoftBody(const b3SoftBodyDef& def) float32 D[36]; b3ComputeD(D, m_E, m_nu); - b3Vec3 B[4]; - b3ComputeBVec(B, e10, e20, e30, V); + // 6 x 12 + float32 B[72]; + b3ComputeB(B, e10, e20, e30, V); - for (u32 i = 0; i < 4; ++i) + // 12 x 6 + float32 BT[72]; + b3Transpose(BT, B, 6, 12); + + // 12 x 6 + float32 BT_D[72]; + b3Mul(BT_D, BT, 12, 6, D, 6, 6); + + // 12 x 12 + float32 BT_D_B[144]; + b3Mul(BT_D_B, BT_D, 12, 6, B, 6, 12); + for (u32 i = 0; i < 144; ++i) { - // 6 x 3 - float32 B_i[18]; - b3ComputeBMat(B_i, B[i]); - - // 3 x 6 - float32 B_i_T[18]; - b3Transpose(B_i_T, B_i, 6, 3); - - for (u32 j = 0; j < 4; ++j) - { - // 6 x 3 - float32 B_j[18]; - b3ComputeBMat(B_j, B[j]); - - // 6 x 3 - float32 D_B_j[18]; - b3Mul(D_B_j, D, 6, 6, B_j, 6, 3); - - // 3 x 3 - b3Mat33& Ke = e->K[i + 4 * j]; - b3Mul(&Ke.x.x, B_i_T, 3, 6, D_B_j, 6, 3); - - Ke = V * Ke; - } + BT_D_B[i] *= V; } + + b3SetK(e->K, BT_D_B); } // Initialize triangles @@ -485,10 +773,7 @@ void b3SoftBody::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations B3_PROFILE("Soft Body Solve"); b3SoftBodySolverDef def; - def.stack = &m_stackAllocator; - def.mesh = m_mesh; - def.nodes = m_nodes; - def.elements = m_elements; + def.body = this; b3SoftBodySolver solver(def); diff --git a/src/bounce/softbody/softbody_solver.cpp b/src/bounce/softbody/softbody_solver.cpp index 188844f..f46653c 100644 --- a/src/bounce/softbody/softbody_solver.cpp +++ b/src/bounce/softbody/softbody_solver.cpp @@ -38,14 +38,15 @@ u32 b3_softBodySolverIterations = 0; // Enables the stiffness warping solver. -// bool b3_enableStiffnessWarping = true; +bool b3_enableStiffnessWarping = true; b3SoftBodySolver::b3SoftBodySolver(const b3SoftBodySolverDef& def) { - m_allocator = def.stack; - m_mesh = def.mesh; - m_nodes = def.nodes; - m_elements = def.elements; + m_body = def.body; + m_allocator = &m_body->m_stackAllocator; + m_mesh = m_body->m_mesh; + m_nodes = m_body->m_nodes; + m_elements = m_body->m_elements; } b3SoftBodySolver::~b3SoftBodySolver() @@ -178,9 +179,20 @@ static B3_FORCE_INLINE void b3Mul(float32* C, float32* A, u32 AM, u32 AN, float3 } } +static B3_FORCE_INLINE float32 b3Length(float32* a, u32 an) +{ + float32 result = 0.0f; + for (u32 i = 0; i < an; ++i) + { + result += a[i] * a[i]; + } + return b3Sqrt(result); +} + void b3SoftBodySolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations) { float32 h = dt; + float32 inv_h = 1.0f / h; b3SparseMat33 M(m_mesh->vertexCount); b3DenseVec3 x(m_mesh->vertexCount); @@ -219,6 +231,9 @@ void b3SoftBodySolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIter b3DenseVec3 f0(m_mesh->vertexCount); f0.SetZero(); + b3DenseVec3 f_plastic(m_mesh->vertexCount); + f_plastic.SetZero(); + for (u32 ei = 0; ei < m_mesh->tetrahedronCount; ++ei) { b3SoftBodyMeshTetrahedron* mt = m_mesh->tetrahedrons + ei; @@ -236,16 +251,23 @@ void b3SoftBodySolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIter b3Vec3 p3 = p[v3]; b3Vec3 p4 = p[v4]; - b3Vec3 e1 = p2 - p1; - b3Vec3 e2 = p3 - p1; - b3Vec3 e3 = p4 - p1; - - b3Mat33 E(e1, e2, e3); - - b3Mat33 A = E * e->invE; - b3Mat33 R; - b3ExtractRotation(R, e->q, A); + if (b3_enableStiffnessWarping) + { + b3Vec3 e1 = p2 - p1; + b3Vec3 e2 = p3 - p1; + b3Vec3 e3 = p4 - p1; + + b3Mat33 E(e1, e2, e3); + + b3Mat33 A = E * e->invE; + + b3ExtractRotation(R, e->q, A); + } + else + { + R.SetIdentity(); + } b3Mat33 RT = b3Transpose(R); @@ -292,7 +314,7 @@ void b3SoftBodySolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIter b3SparseMat33 A = M + h * h * K; - b3DenseVec3 b = M * v - h * (K * p + f0 - fe); + b3DenseVec3 b = M * v - h * (K * p + f0 + f_plastic - fe); // Solve Ax = b b3DenseVec3 sx(m_mesh->vertexCount);