Use full matrices for computing the stiffness matrices. Enable/disable stiffness warping.
This commit is contained in:
Irlan
parent
9a14c1903c
commit
e0d2f9f512
@ -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();
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user