diff --git a/examples/testbed/tests/table_cloth.h b/examples/testbed/tests/table_cloth.h index 08adff6..9460ffe 100644 --- a/examples/testbed/tests/table_cloth.h +++ b/examples/testbed/tests/table_cloth.h @@ -35,6 +35,7 @@ public: def.mesh = &m_clothMesh; def.density = 0.2f; def.streching = 10000.0f; + //def.shearing = 10000.0f; def.damping = 100.0f; def.thickness = 0.2f; def.friction = 0.1f; @@ -134,7 +135,6 @@ public: } b3GridClothMesh<10, 10> m_clothMesh; - b3Cloth* m_cloth; b3ClothDragger* m_clothDragger; diff --git a/include/bounce/bounce.h b/include/bounce/bounce.h index 33ca02b..1fdc46f 100644 --- a/include/bounce/bounce.h +++ b/include/bounce/bounce.h @@ -69,6 +69,7 @@ #include #include #include +#include #include #include diff --git a/include/bounce/cloth/cloth.h b/include/bounce/cloth/cloth.h index 4483df2..b4fda69 100644 --- a/include/bounce/cloth/cloth.h +++ b/include/bounce/cloth/cloth.h @@ -58,8 +58,9 @@ struct b3ClothDef mesh = nullptr; density = 0.0f; streching = 0.0f; - sewing = 0.0f; + shearing = 0.0f; bending = 0.0f; + sewing = 0.0f; damping = 0.0f; thickness = 0.0f; friction = 0.2f; @@ -74,6 +75,9 @@ struct b3ClothDef // Streching stiffness float32 streching; + // Shearing stiffness + float32 shearing; + // Bending stiffness float32 bending; @@ -156,6 +160,7 @@ public: private: friend class b3Particle; friend class b3ClothTriangle; + friend class b3ShearForce; friend class b3StrechForce; friend class b3SpringForce; friend class b3ClothContactManager; diff --git a/include/bounce/cloth/cloth_force_solver.h b/include/bounce/cloth/cloth_force_solver.h index 763e3cc..d91786c 100644 --- a/include/bounce/cloth/cloth_force_solver.h +++ b/include/bounce/cloth/cloth_force_solver.h @@ -29,8 +29,8 @@ class b3Force; struct b3DenseVec3; struct b3DiagMat33; -struct b3SparseSymMat33; -struct b3SparseSymMat33View; +struct b3SparseMat33; +struct b3SparseMat33View; struct b3ClothForceSolverDef { @@ -47,8 +47,8 @@ struct b3ClothForceSolverData b3DenseVec3* v; b3DenseVec3* f; b3DenseVec3* y; - b3SparseSymMat33* dfdx; - b3SparseSymMat33* dfdv; + b3SparseMat33* dfdx; + b3SparseMat33* dfdv; b3DiagMat33* S; b3DenseVec3* z; }; diff --git a/include/bounce/cloth/cloth_triangle.h b/include/bounce/cloth/cloth_triangle.h index 5a11fe7..03750d5 100644 --- a/include/bounce/cloth/cloth_triangle.h +++ b/include/bounce/cloth/cloth_triangle.h @@ -42,6 +42,7 @@ public: private: friend class b3Cloth; friend class b3Particle; + friend class b3ShearForce; friend class b3StrechForce; friend class b3ClothContactManager; friend class b3ParticleTriangleContact; diff --git a/include/bounce/cloth/force.h b/include/bounce/cloth/force.h index 5c0b283..3fb51cd 100644 --- a/include/bounce/cloth/force.h +++ b/include/bounce/cloth/force.h @@ -30,6 +30,7 @@ class b3Particle; enum b3ForceType { e_strechForce, + e_shearForce, e_springForce, }; diff --git a/include/bounce/cloth/particle.h b/include/bounce/cloth/particle.h index 71fe08f..4de14cb 100644 --- a/include/bounce/cloth/particle.h +++ b/include/bounce/cloth/particle.h @@ -112,15 +112,16 @@ public: private: friend class b3List2; friend class b3Cloth; - friend class b3ClothTriangle; + friend class b3ClothContactManager; friend class b3ClothSolver; friend class b3ClothForceSolver; - friend class b3ClothContactManager; + friend class b3ClothTriangle; friend class b3ParticleBodyContact; friend class b3ParticleTriangleContact; friend class b3ClothContactSolver; friend class b3Force; friend class b3StrechForce; + friend class b3ShearForce; friend class b3SpringForce; b3Particle(const b3ParticleDef& def, b3Cloth* cloth); diff --git a/include/bounce/cloth/shear_force.h b/include/bounce/cloth/shear_force.h new file mode 100644 index 0000000..8485795 --- /dev/null +++ b/include/bounce/cloth/shear_force.h @@ -0,0 +1,114 @@ +/* +* 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. +*/ + +#ifndef B3_SHEAR_FORCE_H +#define B3_SHEAR_FORCE_H + +#include + +class b3ClothTriangle; + +struct b3ShearForceDef : public b3ForceDef +{ + b3ShearForceDef() + { + type = e_shearForce; + } + + // Triangle + b3ClothTriangle* triangle; + + // Shearing stiffness + float32 shearing; + + // Damping stiffness + float32 damping; +}; + +// Shear force acting on a cloth triangle. +class b3ShearForce : public b3Force +{ +public: + bool HasParticle(const b3Particle* particle) const; + + b3ClothTriangle* GetTriangle() const; + + float32 GetShearingStiffness() const; + + float32 GetDampingStiffness() const; + + b3Vec3 GetActionForce1() const; + + b3Vec3 GetActionForce2() const; + + b3Vec3 GetActionForce3() const; +private: + friend class b3Force; + friend class b3Cloth; + + b3ShearForce(const b3ShearForceDef* def); + ~b3ShearForce(); + + void Apply(const b3ClothForceSolverData* data); + + // Solver shared + + // Triangle + b3ClothTriangle* m_triangle; + + // Shearing stiffness + float32 m_ks; + + // Damping stiffness + float32 m_kd; + + // Action forces + b3Vec3 m_f1, m_f2, m_f3; +}; + +inline b3ClothTriangle* b3ShearForce::GetTriangle() const +{ + return m_triangle; +} + +inline float32 b3ShearForce::GetShearingStiffness() const +{ + return m_ks; +} + +inline float32 b3ShearForce::GetDampingStiffness() const +{ + return m_kd; +} + +inline b3Vec3 b3ShearForce::GetActionForce1() const +{ + return m_f1; +} + +inline b3Vec3 b3ShearForce::GetActionForce2() const +{ + return m_f2; +} + +inline b3Vec3 b3ShearForce::GetActionForce3() const +{ + return m_f3; +} + +#endif \ No newline at end of file diff --git a/include/bounce/sparse/sparse_sym_mat33.h b/include/bounce/sparse/sparse_sym_mat33.h deleted file mode 100644 index e4d4b3e..0000000 --- a/include/bounce/sparse/sparse_sym_mat33.h +++ /dev/null @@ -1,337 +0,0 @@ -/* -* 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. -*/ - -#ifndef B3_SPARSE_SYM_MAT_33_H -#define B3_SPARSE_SYM_MAT_33_H - -#include - -// A sparse symmetric matrix. -// Each row is a list of non-zero elements in the row. -// The total matrix capacity is bounded by -// M * (M + 1) / 2 -// You must write only the upper triangle elements of the original -// matrix to this matrix because a_ji = a_ij. -struct b3SparseSymMat33 -{ - // - b3SparseSymMat33(); - - // - b3SparseSymMat33(u32 m); - - // - b3SparseSymMat33(const b3SparseSymMat33& _m); - - // - ~b3SparseSymMat33(); - - // - b3SparseSymMat33& operator=(const b3SparseSymMat33& _m); - - // - void Copy(const b3SparseSymMat33& _m); - - // - void Destroy(); - - // - b3Mat33& operator()(u32 i, u32 j); - - // - const b3Mat33& operator()(u32 i, u32 j) const; - - // - void operator+=(const b3SparseSymMat33& m); - - // - void operator-=(const b3SparseSymMat33& m); - - u32 rowCount; - b3RowValueList* rows; -}; - -inline b3SparseSymMat33::b3SparseSymMat33() -{ - rowCount = 0; - rows = nullptr; -} - -inline b3SparseSymMat33::b3SparseSymMat33(u32 m) -{ - rowCount = m; - rows = (b3RowValueList*)b3Alloc(rowCount * sizeof(b3RowValueList)); - for (u32 i = 0; i < rowCount; ++i) - { - new (rows + i)b3RowValueList(); - } -} - -inline b3SparseSymMat33::b3SparseSymMat33(const b3SparseSymMat33& m) -{ - rowCount = m.rowCount; - rows = (b3RowValueList*)b3Alloc(rowCount * sizeof(b3RowValueList)); - for (u32 i = 0; i < rowCount; ++i) - { - new (rows + i)b3RowValueList(); - } - - Copy(m); -} - -inline b3SparseSymMat33::~b3SparseSymMat33() -{ - Destroy(); -} - -inline void b3SparseSymMat33::Destroy() -{ - for (u32 i = 0; i < rowCount; ++i) - { - b3RowValueList* vs = rows + i; - - b3RowValue* v = vs->head; - while (v) - { - b3RowValue* v0 = v->next; - b3Free(v); - v = v0; - } - - vs->~b3RowValueList(); - } - - b3Free(rows); -} - -inline b3SparseSymMat33& b3SparseSymMat33::operator=(const b3SparseSymMat33& _m) -{ - if (_m.rows == rows) - { - return *this; - } - - Destroy(); - - rowCount = _m.rowCount; - rows = (b3RowValueList*)b3Alloc(rowCount * sizeof(b3RowValueList)); - for (u32 i = 0; i < rowCount; ++i) - { - new (rows + i)b3RowValueList(); - } - - Copy(_m); - - return *this; -} - -inline void b3SparseSymMat33::Copy(const b3SparseSymMat33& _m) -{ - B3_ASSERT(rowCount == _m.rowCount); - - for (u32 i = 0; i < rowCount; ++i) - { - b3RowValueList* vs1 = _m.rows + i; - b3RowValueList* vs2 = rows + i; - - B3_ASSERT(vs2->count == 0); - - for (b3RowValue* v1 = vs1->head; v1; v1 = v1->next) - { - b3RowValue* v2 = (b3RowValue*)b3Alloc(sizeof(b3RowValue)); - - v2->column = v1->column; - v2->value = v1->value; - - vs2->PushFront(v2); - } - } -} - -inline const b3Mat33& b3SparseSymMat33::operator()(u32 i, u32 j) const -{ - B3_ASSERT(i < rowCount); - B3_ASSERT(j < rowCount); - - if (i > j) - { - b3Swap(i, j); - } - - b3RowValueList* vs = rows + i; - for (b3RowValue* v = vs->head; v; v = v->next) - { - if (v->column == j) - { - return v->value; - } - } - - return b3Mat33_zero; -} - -inline b3Mat33& b3SparseSymMat33::operator()(u32 i, u32 j) -{ - B3_ASSERT(i < rowCount); - B3_ASSERT(j < rowCount); - - if (i > j) - { - b3Swap(i, j); - } - - b3RowValueList* vs = rows + i; - for (b3RowValue* v = vs->head; v; v = v->next) - { - if (v->column == j) - { - return v->value; - } - } - - b3RowValue* v = (b3RowValue*)b3Alloc(sizeof(b3RowValue)); - v->column = j; - v->value.SetZero(); - - vs->PushFront(v); - - return v->value; -} - -inline void b3SparseSymMat33::operator+=(const b3SparseSymMat33& m) -{ - B3_ASSERT(rowCount == m.rowCount); - - for (u32 i = 0; i < m.rowCount; ++i) - { - b3RowValueList* mvs = m.rows + i; - - for (b3RowValue* v = mvs->head; v; v = v->next) - { - u32 j = v->column; - - (*this)(i, j) += v->value; - } - } -} - -inline void b3SparseSymMat33::operator-=(const b3SparseSymMat33& m) -{ - B3_ASSERT(rowCount == m.rowCount); - - for (u32 i = 0; i < m.rowCount; ++i) - { - b3RowValueList* mvs = m.rows + i; - - for (b3RowValue* v = mvs->head; v; v = v->next) - { - u32 j = v->column; - - (*this)(i, j) -= v->value; - } - } -} - -inline void b3Add(b3SparseSymMat33& out, const b3SparseSymMat33& a, const b3SparseSymMat33& b) -{ - out = a; - out += b; -} - -inline void b3Sub(b3SparseSymMat33& out, const b3SparseSymMat33& a, const b3SparseSymMat33& b) -{ - out = a; - out -= b; -} - -inline void b3Mul(b3DenseVec3& out, const b3SparseSymMat33& A, const b3DenseVec3& v) -{ - B3_ASSERT(A.rowCount == out.n); - - out.SetZero(); - - for (u32 i = 0; i < A.rowCount; ++i) - { - b3RowValueList* vs = A.rows + i; - - for (b3RowValue* vA = vs->head; vA; vA = vA->next) - { - u32 j = vA->column; - b3Mat33 a = vA->value; - - out[i] += a * v[j]; - - if (i != j) - { - // a_ij == a_ji - out[j] += a * v[i]; - } - } - } -} - -inline void b3Mul(b3SparseSymMat33& out, float32 s, const b3SparseSymMat33& B) -{ - B3_ASSERT(out.rowCount == B.rowCount); - - if (s == 0.0f) - { - return; - } - - out = B; - - for (u32 i = 0; i < out.rowCount; ++i) - { - b3RowValueList* vs = out.rows + i; - for (b3RowValue* v = vs->head; v; v = v->next) - { - v->value = s * v->value; - } - } -} - -inline b3SparseSymMat33 operator+(const b3SparseSymMat33& A, const b3SparseSymMat33& B) -{ - b3SparseSymMat33 result(A.rowCount); - b3Add(result, A, B); - return result; -} - -inline b3SparseSymMat33 operator-(const b3SparseSymMat33& A, const b3SparseSymMat33& B) -{ - b3SparseSymMat33 result(A.rowCount); - b3Sub(result, A, B); - return result; -} - -inline b3SparseSymMat33 operator*(float32 A, const b3SparseSymMat33& B) -{ - b3SparseSymMat33 result(B.rowCount); - b3Mul(result, A, B); - return result; -} - -inline b3DenseVec3 operator*(const b3SparseSymMat33& A, const b3DenseVec3& v) -{ - b3DenseVec3 result(v.n); - b3Mul(result, A, v); - return result; -} - -#endif diff --git a/include/bounce/sparse/sparse_sym_mat33_view.h b/include/bounce/sparse/sparse_sym_mat33_view.h deleted file mode 100644 index 7d944d6..0000000 --- a/include/bounce/sparse/sparse_sym_mat33_view.h +++ /dev/null @@ -1,132 +0,0 @@ -/* -* 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. -*/ - -#ifndef B3_SPARSE_SYM_MAT_33_VIEW_H -#define B3_SPARSE_SYM_MAT_33_VIEW_H - -#include -#include - -// A read-only sparse symmetric matrix. -struct b3SparseSymMat33View -{ - // - b3SparseSymMat33View(const b3SparseSymMat33& _m); - - // - ~b3SparseSymMat33View(); - - // - const b3Mat33& operator()(u32 i, u32 j) const; - - u32 rowCount; - b3RowValueArray* rows; -}; - -inline b3SparseSymMat33View::b3SparseSymMat33View(const b3SparseSymMat33& _m) -{ - rowCount = _m.rowCount; - rows = (b3RowValueArray*)b3Alloc(rowCount * sizeof(b3RowValueArray)); - for (u32 i = 0; i < _m.rowCount; ++i) - { - b3RowValueList* rowList = _m.rows + i; - b3RowValueArray* rowArray = rows + i; - - rowArray->count = rowList->count; - rowArray->values = (b3ArrayRowValue*)b3Alloc(rowArray->count * sizeof(b3ArrayRowValue)); - - u32 valueIndex = 0; - for (b3RowValue* v = rowList->head; v; v = v->next) - { - rowArray->values[valueIndex].column = v->column; - rowArray->values[valueIndex].value = v->value; - ++valueIndex; - } - } -} - -inline b3SparseSymMat33View::~b3SparseSymMat33View() -{ - for (u32 i = 0; i < rowCount; ++i) - { - b3RowValueArray* rowArray = rows + i; - b3Free(rowArray->values); - } - b3Free(rows); -} - -inline const b3Mat33& b3SparseSymMat33View::operator()(u32 i, u32 j) const -{ - B3_ASSERT(i < rowCount); - B3_ASSERT(j < rowCount); - - if (i > j) - { - b3Swap(i, j); - } - - b3RowValueArray* vs = rows + i; - - for (u32 c = 0; c < vs->count; ++c) - { - b3ArrayRowValue* rv = vs->values + c; - - if (rv->column == j) - { - return rv->value; - } - } - - return b3Mat33_zero; -} - -inline void b3Mul(b3DenseVec3& out, const b3SparseSymMat33View& A, const b3DenseVec3& v) -{ - B3_ASSERT(A.rowCount == out.n); - - out.SetZero(); - - for (u32 i = 0; i < A.rowCount; ++i) - { - b3RowValueArray* rowArray = A.rows + i; - - for (u32 c = 0; c < rowArray->count; ++c) - { - b3ArrayRowValue* rv = rowArray->values + c; - - u32 j = rv->column; - b3Mat33 a = rv->value; - - out[i] += a * v[j]; - - if (i != j) - { - out[j] += a * v[i]; - } - } - } -} - -inline b3DenseVec3 operator*(const b3SparseSymMat33View& A, const b3DenseVec3& v) -{ - b3DenseVec3 result(v.n); - b3Mul(result, A, v); - return result; -} - -#endif \ No newline at end of file diff --git a/src/bounce/cloth/cloth.cpp b/src/bounce/cloth/cloth.cpp index d2a3af6..2e442ab 100644 --- a/src/bounce/cloth/cloth.cpp +++ b/src/bounce/cloth/cloth.cpp @@ -21,8 +21,9 @@ #include #include #include -#include #include +#include +#include #include #include @@ -199,7 +200,20 @@ b3Cloth::b3Cloth(const b3ClothDef& def) : sfdef.bu = 1.0f; sfdef.bv = 1.0f; - CreateForce(sfdef); + if (def.streching > 0.0f) + { + CreateForce(sfdef); + } + + b3ShearForceDef shdef; + shdef.triangle = triangle; + shdef.shearing = def.shearing; + shdef.damping = def.damping; + + if (def.shearing > 0.0f) + { + CreateForce(shdef); + } } // Initialize forces diff --git a/src/bounce/cloth/cloth_force_solver.cpp b/src/bounce/cloth/cloth_force_solver.cpp index e530d54..8da4703 100644 --- a/src/bounce/cloth/cloth_force_solver.cpp +++ b/src/bounce/cloth/cloth_force_solver.cpp @@ -21,8 +21,8 @@ #include #include #include -#include -#include +#include +#include #include // Here, we solve Ax = b using the Modified Preconditioned Conjugate Gradient (MPCG) algorithm. @@ -128,7 +128,7 @@ void b3ClothForceSolver::ApplyConstraints() // Solve Ax = b static void b3SolveMPCG(b3DenseVec3& x, - const b3SparseSymMat33View& A, const b3DenseVec3& b, + const b3SparseMat33View& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y, const b3DiagMat33& I, u32 maxIterations = 20) { @@ -214,9 +214,9 @@ void b3ClothForceSolver::Solve(float32 dt, const b3Vec3& gravity) b3DenseVec3 sy(m_particleCount); b3DenseVec3 sz(m_particleCount); b3DenseVec3 sx0(m_particleCount); - b3SparseSymMat33 M(m_particleCount); - b3SparseSymMat33 dfdx(m_particleCount); - b3SparseSymMat33 dfdv(m_particleCount); + b3SparseMat33 M(m_particleCount); + b3SparseMat33 dfdx(m_particleCount); + b3SparseMat33 dfdv(m_particleCount); b3DiagMat33 S(m_particleCount); b3DiagMat33 I(m_particleCount); I.SetIdentity(); @@ -261,10 +261,10 @@ void b3ClothForceSolver::Solve(float32 dt, const b3Vec3& gravity) // b = h * (f0 + h * dfdx * v0 + dfdx * y) // A - b3SparseSymMat33 A = M - h * dfdv - h * h * dfdx; + b3SparseMat33 A = M - h * dfdv - h * h * dfdx; // View for A - b3SparseSymMat33View viewA(A); + b3SparseMat33View viewA(A); // b b3DenseVec3 b = h * (sf + h * (dfdx * sv) + dfdx * sy); diff --git a/src/bounce/cloth/force.cpp b/src/bounce/cloth/force.cpp index acc3227..4de3bf5 100644 --- a/src/bounce/cloth/force.cpp +++ b/src/bounce/cloth/force.cpp @@ -18,6 +18,7 @@ #include #include +#include #include b3Force* b3Force::Create(const b3ForceDef* def) @@ -31,6 +32,12 @@ b3Force* b3Force::Create(const b3ForceDef* def) force = new (block) b3StrechForce((b3StrechForceDef*)def); break; } + case e_shearForce: + { + void* block = b3Alloc(sizeof(b3ShearForce)); + force = new (block) b3ShearForce((b3ShearForceDef*)def); + break; + } case e_springForce: { void* block = b3Alloc(sizeof(b3SpringForce)); @@ -60,6 +67,13 @@ void b3Force::Destroy(b3Force* force) b3Free(force); break; } + case e_shearForce: + { + b3ShearForce* o = (b3ShearForce*)force; + o->~b3ShearForce(); + b3Free(force); + break; + } case e_springForce: { b3SpringForce* o = (b3SpringForce*)force; diff --git a/src/bounce/cloth/shear_force.cpp b/src/bounce/cloth/shear_force.cpp new file mode 100644 index 0000000..b79b549 --- /dev/null +++ b/src/bounce/cloth/shear_force.cpp @@ -0,0 +1,219 @@ +/* +* 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 +#include +#include +#include +#include +#include +#include + +b3ShearForce::b3ShearForce(const b3ShearForceDef* def) +{ + m_type = e_shearForce; + m_triangle = def->triangle; + m_ks = def->shearing; + m_kd = def->damping; + m_f1.SetZero(); + m_f2.SetZero(); + m_f3.SetZero(); +} + +b3ShearForce::~b3ShearForce() +{ + +} + +bool b3ShearForce::HasParticle(const b3Particle* particle) const +{ + b3Cloth* cloth = m_triangle->m_cloth; + u32 triangleIndex = m_triangle->m_triangle; + b3ClothMeshTriangle* triangle = cloth->m_mesh->triangles + triangleIndex; + + b3Particle* p1 = cloth->m_particles[triangle->v1]; + b3Particle* p2 = cloth->m_particles[triangle->v2]; + b3Particle* p3 = cloth->m_particles[triangle->v3]; + + return p1 == particle || p2 == particle || p3 == particle; +} + +void b3ShearForce::Apply(const b3ClothForceSolverData* data) +{ + b3Cloth* cloth = m_triangle->m_cloth; + u32 triangleIndex = m_triangle->m_triangle; + b3ClothMeshTriangle* triangle = cloth->m_mesh->triangles + triangleIndex; + + float32 alpha = m_triangle->m_alpha; + float32 du1 = m_triangle->m_du1; + float32 dv1 = m_triangle->m_dv1; + float32 du2 = m_triangle->m_du2; + float32 dv2 = m_triangle->m_dv2; + float32 inv_det = m_triangle->m_inv_det; + + b3Particle* p1 = cloth->m_particles[triangle->v1]; + b3Particle* p2 = cloth->m_particles[triangle->v2]; + b3Particle* p3 = cloth->m_particles[triangle->v3]; + + u32 i1 = p1->m_solverId; + u32 i2 = p2->m_solverId; + u32 i3 = p3->m_solverId; + + b3DenseVec3& x = *data->x; + b3DenseVec3& v = *data->v; + b3DenseVec3& f = *data->f; + b3SparseMat33& dfdx = *data->dfdx; + b3SparseMat33& dfdv = *data->dfdv; + + b3Vec3 x1 = x[i1]; + b3Vec3 x2 = x[i2]; + b3Vec3 x3 = x[i3]; + + b3Vec3 v1 = v[i1]; + b3Vec3 v2 = v[i2]; + b3Vec3 v3 = v[i3]; + + b3Mat33 I; I.SetIdentity(); + + b3Vec3 dx1 = x2 - x1; + b3Vec3 dx2 = x3 - x1; + + b3Vec3 wu = inv_det * (dv2 * dx1 - dv1 * dx2); + b3Vec3 wv = inv_det * (-du2 * dx1 + du1 * dx2); + + b3Vec3 dwudx; + dwudx[0] = inv_det * (dv1 - dv2); + dwudx[1] = inv_det * dv2; + dwudx[2] = -inv_det * dv1; + + b3Vec3 dwvdx; + dwvdx[0] = inv_det * (du2 - du1); + dwvdx[1] = -inv_det * du2; + dwvdx[2] = inv_det * du1; + + m_f1.SetZero(); + m_f2.SetZero(); + m_f3.SetZero(); + + // Jacobian + b3Vec3 dCdx[3]; + for (u32 i = 0; i < 3; ++i) + { + dCdx[i] = alpha * (dwudx[i] * wv + dwvdx[i] * wu); + } + + if (m_ks > 0.0f) + { + float32 C = alpha * b3Dot(wu, wv); + + // Force + b3Vec3 fs[3]; + for (u32 i = 0; i < 3; ++i) + { + fs[i] = -m_ks * C * dCdx[i]; + } + + m_f1 += fs[0]; + m_f2 += fs[1]; + m_f3 += fs[2]; + + // Jacobian + b3Mat33 d2Cxij; + for (u32 i = 0; i < 3; ++i) + { + for (u32 j = 0; j < 3; ++j) + { + d2Cxij(i, j) = alpha * (dwudx[i] * dwvdx[j] + dwudx[j] * dwvdx[i]); + } + } + + b3Mat33 J[3][3]; + for (u32 i = 0; i < 3; ++i) + { + for (u32 j = 0; j < 3; ++j) + { + b3Mat33 Jij = -m_ks * (b3Outer(dCdx[i], dCdx[j]) + (C * d2Cxij(i, j) * I)); + + J[i][j] = Jij; + } + } + + dfdx(i1, i1) += J[0][0]; + dfdx(i1, i2) += J[0][1]; + dfdx(i1, i3) += J[0][2]; + + dfdx(i2, i1) += J[1][0]; + dfdx(i2, i2) += J[1][1]; + dfdx(i2, i3) += J[1][2]; + + dfdx(i3, i1) += J[2][0]; + dfdx(i3, i2) += J[2][1]; + dfdx(i3, i3) += J[2][2]; + } + + if (m_kd > 0.0f) + { + b3Vec3 vs[3] = { v1, v2, v3 }; + + float32 dCdt = 0.0f; + for (u32 i = 0; i < 3; ++i) + { + dCdt += b3Dot(dCdx[i], vs[i]); + } + + // Force + b3Vec3 fs[3]; + for (u32 i = 0; i < 3; ++i) + { + fs[i] = -m_kd * dCdt * dCdx[i]; + } + + m_f1 += fs[0]; + m_f2 += fs[1]; + m_f3 += fs[2]; + + // Jacobian + b3Mat33 J[3][3]; + for (u32 i = 0; i < 3; ++i) + { + for (u32 j = 0; j < 3; ++j) + { + b3Mat33 Jij = -m_kd * b3Outer(dCdx[i], dCdx[j]); + + J[i][j] = Jij; + } + } + + dfdv(i1, i1) += J[0][0]; + dfdv(i1, i2) += J[0][1]; + dfdv(i1, i3) += J[0][2]; + + dfdv(i2, i1) += J[1][0]; + dfdv(i2, i2) += J[1][1]; + dfdv(i2, i3) += J[1][2]; + + dfdv(i3, i1) += J[2][0]; + dfdv(i3, i2) += J[2][1]; + dfdv(i3, i3) += J[2][2]; + } + + f[i1] += m_f1; + f[i2] += m_f2; + f[i3] += m_f3; +} \ No newline at end of file diff --git a/src/bounce/cloth/spring_force.cpp b/src/bounce/cloth/spring_force.cpp index 1f687ae..8248c6d 100644 --- a/src/bounce/cloth/spring_force.cpp +++ b/src/bounce/cloth/spring_force.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include void b3SpringForceDef::Initialize(b3Particle* particle1, b3Particle* particle2, float32 structuralStiffness, float32 dampingStiffness) { @@ -55,8 +55,8 @@ void b3SpringForce::Apply(const b3ClothForceSolverData* data) b3DenseVec3& x = *data->x; b3DenseVec3& v = *data->v; b3DenseVec3& f = *data->f; - b3SparseSymMat33& dfdx = *data->dfdx; - b3SparseSymMat33& dfdv = *data->dfdv; + b3SparseMat33& dfdx = *data->dfdx; + b3SparseMat33& dfdv = *data->dfdv; u32 i1 = m_p1->m_solverId; u32 i2 = m_p2->m_solverId; @@ -87,12 +87,12 @@ void b3SpringForce::Apply(const b3ClothForceSolverData* data) // Jacobian b3Mat33 Jx11 = -m_ks * (b3Outer(dx, dx) + (1.0f - m_L0 / L) * (I - b3Outer(dx, dx))); b3Mat33 Jx12 = -Jx11; - //b3Mat33 Jx21 = Jx12; + b3Mat33 Jx21 = Jx12; b3Mat33 Jx22 = Jx11; dfdx(i1, i1) += Jx11; dfdx(i1, i2) += Jx12; - //dfdx(i2, i1) += Jx21; + dfdx(i2, i1) += Jx21; dfdx(i2, i2) += Jx22; } } @@ -106,12 +106,12 @@ void b3SpringForce::Apply(const b3ClothForceSolverData* data) b3Mat33 Jv11 = -m_kd * I; b3Mat33 Jv12 = -Jv11; - //b3Mat33 Jv21 = Jv12; + b3Mat33 Jv21 = Jv12; b3Mat33 Jv22 = Jv11; dfdv(i1, i1) += Jv11; dfdv(i1, i2) += Jv12; - //dfdv(i2, i1) += Jv21; + dfdv(i2, i1) += Jv21; dfdv(i2, i2) += Jv22; } diff --git a/src/bounce/cloth/strech_force.cpp b/src/bounce/cloth/strech_force.cpp index d208441..e918809 100644 --- a/src/bounce/cloth/strech_force.cpp +++ b/src/bounce/cloth/strech_force.cpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include b3StrechForce::b3StrechForce(const b3StrechForceDef* def) { @@ -80,8 +80,8 @@ void b3StrechForce::Apply(const b3ClothForceSolverData* data) b3DenseVec3& x = *data->x; b3DenseVec3& v = *data->v; b3DenseVec3& f = *data->f; - b3SparseSymMat33& dfdx = *data->dfdx; - b3SparseSymMat33& dfdv = *data->dfdv; + b3SparseMat33& dfdx = *data->dfdx; + b3SparseMat33& dfdv = *data->dfdv; b3Vec3 x1 = x[i1]; b3Vec3 x2 = x[i2]; @@ -163,12 +163,12 @@ void b3StrechForce::Apply(const b3ClothForceSolverData* data) dfdx(i1, i2) += J[0][1]; dfdx(i1, i3) += J[0][2]; - //dfdx(i2, i1) += J[1][0]; + dfdx(i2, i1) += J[1][0]; dfdx(i2, i2) += J[1][1]; dfdx(i2, i3) += J[1][2]; - //dfdx(i3, i1) += J[2][0]; - //dfdx(i3, i2) += J[2][1]; + dfdx(i3, i1) += J[2][0]; + dfdx(i3, i2) += J[2][1]; dfdx(i3, i3) += J[2][2]; } } @@ -209,12 +209,12 @@ void b3StrechForce::Apply(const b3ClothForceSolverData* data) dfdv(i1, i2) += J[0][1]; dfdv(i1, i3) += J[0][2]; - //dfdv(i2, i1) += J[1][0]; + dfdv(i2, i1) += J[1][0]; dfdv(i2, i2) += J[1][1]; dfdv(i2, i3) += J[1][2]; - //dfdv(i3, i1) += J[2][0]; - //dfdv(i3, i2) += J[2][1]; + dfdv(i3, i1) += J[2][0]; + dfdv(i3, i2) += J[2][1]; dfdv(i3, i3) += J[2][2]; } } @@ -266,12 +266,12 @@ void b3StrechForce::Apply(const b3ClothForceSolverData* data) dfdx(i1, i2) += J[0][1]; dfdx(i1, i3) += J[0][2]; - //dfdx(i2, i1) += J[1][0]; + dfdx(i2, i1) += J[1][0]; dfdx(i2, i2) += J[1][1]; dfdx(i2, i3) += J[1][2]; - //dfdx(i3, i1) += J[2][0]; - //dfdx(i3, i2) += J[2][1]; + dfdx(i3, i1) += J[2][0]; + dfdx(i3, i2) += J[2][1]; dfdx(i3, i3) += J[2][2]; } } @@ -313,12 +313,12 @@ void b3StrechForce::Apply(const b3ClothForceSolverData* data) dfdv(i1, i2) += J[0][1]; dfdv(i1, i3) += J[0][2]; - //dfdv(i2, i1) += J[1][0]; + dfdv(i2, i1) += J[1][0]; dfdv(i2, i2) += J[1][1]; dfdv(i2, i3) += J[1][2]; - //dfdv(i3, i1) += J[2][0]; - //dfdv(i3, i2) += J[2][1]; + dfdv(i3, i1) += J[2][0]; + dfdv(i3, i2) += J[2][1]; dfdv(i3, i3) += J[2][2]; } }