diff --git a/examples/testbed/tests/cloth_self_collision.h b/examples/testbed/tests/cloth_self_collision.h index dfc088e..b363025 100644 --- a/examples/testbed/tests/cloth_self_collision.h +++ b/examples/testbed/tests/cloth_self_collision.h @@ -34,7 +34,7 @@ public: b3ClothDef def; def.mesh = &m_clothMesh; def.density = 1.0f; - def.structural = 100000.0f; + def.streching = 100000.0f; def.thickness = 0.2f; def.friction = 0.3f; diff --git a/examples/testbed/tests/pinned_cloth.h b/examples/testbed/tests/pinned_cloth.h index b6c8543..e856ac5 100644 --- a/examples/testbed/tests/pinned_cloth.h +++ b/examples/testbed/tests/pinned_cloth.h @@ -28,8 +28,7 @@ public: b3ClothDef def; def.mesh = &m_clothMesh; def.density = 0.2f; - def.structural = 100000.0f; - def.damping = 0.0f; + def.streching = 100000.0f; m_cloth = new b3Cloth(def); diff --git a/examples/testbed/tests/table_cloth.h b/examples/testbed/tests/table_cloth.h index f3329bc..b88edf4 100644 --- a/examples/testbed/tests/table_cloth.h +++ b/examples/testbed/tests/table_cloth.h @@ -34,8 +34,7 @@ public: b3ClothDef def; def.mesh = &m_clothMesh; def.density = 0.2f; - def.structural = 10000.0f; - def.damping = 0.0f; + def.streching = 10000.0f; def.thickness = 0.2f; def.friction = 0.1f; diff --git a/examples/testbed/tests/tension_mapping.h b/examples/testbed/tests/tension_mapping.h index bb53a8f..531512c 100644 --- a/examples/testbed/tests/tension_mapping.h +++ b/examples/testbed/tests/tension_mapping.h @@ -64,7 +64,8 @@ public: b3ClothDef def; def.mesh = &m_clothMesh; def.density = 0.2f; - def.structural = 10000.0f; + def.streching = 10000.0f; + def.damping = 100.0f; m_cloth = new b3Cloth(def); @@ -110,27 +111,25 @@ public: for (b3Force* f = m_cloth->GetForceList().m_head; f; f = f->GetNext()) { - if (f->GetType() == e_springForce) + if (f->GetType() == e_strechForce) { - b3SpringForce* s = (b3SpringForce*)f; + b3StrechForce* s = (b3StrechForce*)f; + + b3ClothTriangle* triangle = s->GetTriangle(); + u32 triangleIndex = triangle->GetTriangle(); + b3ClothMeshTriangle* mesh_triangle = m_clothMesh.triangles + triangleIndex; + + u32 v1 = mesh_triangle->v1; + u32 v2 = mesh_triangle->v2; + u32 v3 = mesh_triangle->v3; - u32 v1 = s->GetParticle1()->GetVertex(); - u32 v2 = s->GetParticle2()->GetVertex(); + b3Vec3 f1 = s->GetActionForce1(); + b3Vec3 f2 = s->GetActionForce2(); + b3Vec3 f3 = s->GetActionForce3(); - if (v1 != ~0) - { - tension[v1] += s->GetActionForce(); - } - - if (v2 != ~0) - { - tension[v2] -= s->GetActionForce(); - } - - b3Vec3 p1 = s->GetParticle1()->GetPosition(); - b3Vec3 p2 = s->GetParticle2()->GetPosition(); - - g_draw->DrawSegment(p1, p2, b3Color_black); + tension[v1] += f1; + tension[v2] += f2; + tension[v3] += f3; } } @@ -142,6 +141,8 @@ public: b3Vec3 v2 = m_cloth->GetParticle(t->v2)->GetPosition(); b3Vec3 v3 = m_cloth->GetParticle(t->v3)->GetPosition(); + g_draw->DrawTriangle(v1, v2, v3, b3Color_black); + b3Vec3 c = (v1 + v2 + v3) / 3.0f; float32 s = 0.9f; @@ -161,7 +162,7 @@ public: float32 L = (L1 + L2 + L3) / 3.0f; - const float32 kMaxT = 100000.0f; + const float32 kMaxT = 10000.0f; b3Color color = Color(L, 0.0f, kMaxT); b3Vec3 n1 = b3Cross(v2 - v1, v3 - v1); diff --git a/include/bounce/bounce.h b/include/bounce/bounce.h index d392a9d..33ca02b 100644 --- a/include/bounce/bounce.h +++ b/include/bounce/bounce.h @@ -68,6 +68,7 @@ #include #include #include +#include #include #include diff --git a/include/bounce/cloth/cloth.h b/include/bounce/cloth/cloth.h index 4e7136c..4483df2 100644 --- a/include/bounce/cloth/cloth.h +++ b/include/bounce/cloth/cloth.h @@ -57,7 +57,8 @@ struct b3ClothDef { mesh = nullptr; density = 0.0f; - structural = 0.0f; + streching = 0.0f; + sewing = 0.0f; bending = 0.0f; damping = 0.0f; thickness = 0.0f; @@ -70,12 +71,15 @@ struct b3ClothDef // Cloth density in kg/m^2 float32 density; - // Structural stiffness - float32 structural; + // Streching stiffness + float32 streching; // Bending stiffness float32 bending; + // Sewing stiffness + float32 sewing; + // Damping stiffness float32 damping; @@ -152,6 +156,8 @@ public: private: friend class b3Particle; friend class b3ClothTriangle; + friend class b3StrechForce; + friend class b3SpringForce; friend class b3ClothContactManager; // Compute mass of each particle. diff --git a/include/bounce/cloth/cloth_triangle.h b/include/bounce/cloth/cloth_triangle.h index 1a1e6db..5a11fe7 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 b3StrechForce; friend class b3ClothContactManager; friend class b3ParticleTriangleContact; friend class b3ClothSolver; @@ -70,6 +71,14 @@ private: // Broadphase ID u32 m_broadPhaseId; + + // Alpha + float32 m_alpha; + + // Strech matrix + float32 m_du1, m_dv1; + float32 m_du2, m_dv2; + float32 m_inv_det; }; inline u32 b3ClothTriangle::GetTriangle() const diff --git a/include/bounce/cloth/force.h b/include/bounce/cloth/force.h index 392e0ef..5c0b283 100644 --- a/include/bounce/cloth/force.h +++ b/include/bounce/cloth/force.h @@ -29,6 +29,7 @@ class b3Particle; // Force types enum b3ForceType { + e_strechForce, e_springForce, }; diff --git a/include/bounce/cloth/particle.h b/include/bounce/cloth/particle.h index 225b280..71fe08f 100644 --- a/include/bounce/cloth/particle.h +++ b/include/bounce/cloth/particle.h @@ -120,6 +120,7 @@ private: friend class b3ParticleTriangleContact; friend class b3ClothContactSolver; friend class b3Force; + friend class b3StrechForce; friend class b3SpringForce; b3Particle(const b3ParticleDef& def, b3Cloth* cloth); diff --git a/include/bounce/cloth/strech_force.h b/include/bounce/cloth/strech_force.h new file mode 100644 index 0000000..b183c05 --- /dev/null +++ b/include/bounce/cloth/strech_force.h @@ -0,0 +1,126 @@ +/* +* 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_STRECH_FORCE_H +#define B3_STRECH_FORCE_H + +#include + +class b3ClothTriangle; + +struct b3StrechForceDef : public b3ForceDef +{ + b3StrechForceDef() + { + type = e_strechForce; + } + + // Triangle + b3ClothTriangle* triangle; + + // Streching stiffness + float32 streching; + + // Damping stiffness + float32 damping; + + // Desired strechiness in u direction + float32 bu; + + // Desired strechiness in v direction + float32 bv; +}; + +// Strech force acting on a cloth triangle. +class b3StrechForce : public b3Force +{ +public: + bool HasParticle(const b3Particle* particle) const; + + b3ClothTriangle* GetTriangle() const; + + float32 GetStrechingStiffness() const; + + float32 GetDampingStiffness() const; + + b3Vec3 GetActionForce1() const; + + b3Vec3 GetActionForce2() const; + + b3Vec3 GetActionForce3() const; +private: + friend class b3Force; + friend class b3Cloth; + + b3StrechForce(const b3StrechForceDef* def); + ~b3StrechForce(); + + void Apply(const b3ClothForceSolverData* data); + + // Solver shared + + // Triangle + b3ClothTriangle* m_triangle; + + // Streching stiffness + float32 m_ks; + + // Damping stiffness + float32 m_kd; + + // bu + float32 m_bu; + + // bv + float32 m_bv; + + // Action forces + b3Vec3 m_f1, m_f2, m_f3; +}; + +inline b3ClothTriangle* b3StrechForce::GetTriangle() const +{ + return m_triangle; +} + +inline float32 b3StrechForce::GetStrechingStiffness() const +{ + return m_ks; +} + +inline float32 b3StrechForce::GetDampingStiffness() const +{ + return m_kd; +} + +inline b3Vec3 b3StrechForce::GetActionForce1() const +{ + return m_f1; +} + +inline b3Vec3 b3StrechForce::GetActionForce2() const +{ + return m_f2; +} + +inline b3Vec3 b3StrechForce::GetActionForce3() const +{ + return m_f3; +} + +#endif \ No newline at end of file diff --git a/src/bounce/cloth/cloth.cpp b/src/bounce/cloth/cloth.cpp index 5f2c825..d2a3af6 100644 --- a/src/bounce/cloth/cloth.cpp +++ b/src/bounce/cloth/cloth.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -30,57 +31,6 @@ static B3_FORCE_INLINE u32 b3NextIndex(u32 i) return i + 1 < 3 ? i + 1 : 0; } -struct b3UniqueEdge -{ - u32 v1, v2; -}; - -static u32 b3FindUniqueEdges(b3UniqueEdge* uniqueEdges, const b3ClothMesh* m) -{ - u32 uniqueCount = 0; - - for (u32 i = 0; i < m->triangleCount; ++i) - { - b3ClothMeshTriangle* t1 = m->triangles + i; - u32 i1s[3] = { t1->v1, t1->v2, t1->v3 }; - - for (u32 j1 = 0; j1 < 3; ++j1) - { - u32 t1v1 = i1s[j1]; - u32 t1v2 = i1s[b3NextIndex(j1)]; - - bool unique = true; - - for (u32 j = 0; j < uniqueCount; ++j) - { - b3UniqueEdge* ue = uniqueEdges + j; - - if (ue->v1 == t1v1 && ue->v2 == t1v2) - { - unique = false; - break; - } - - if (ue->v2 == t1v1 && ue->v1 == t1v2) - { - unique = false; - break; - } - } - - if (unique) - { - b3UniqueEdge ue; - ue.v1 = t1v1; - ue.v2 = t1v2; - uniqueEdges[uniqueCount++] = ue; - } - } - } - - return uniqueCount; -} - struct b3SharedEdge { u32 v1, v2; @@ -187,17 +137,69 @@ b3Cloth::b3Cloth(const b3ClothDef& def) : triangle->m_friction = def.friction; triangle->m_triangle = i; - b3Vec3 v1 = m_mesh->vertices[meshTriangle->v1]; - b3Vec3 v2 = m_mesh->vertices[meshTriangle->v2]; - b3Vec3 v3 = m_mesh->vertices[meshTriangle->v3]; + b3Vec3 A = m_mesh->vertices[meshTriangle->v1]; + b3Vec3 B = m_mesh->vertices[meshTriangle->v2]; + b3Vec3 C = m_mesh->vertices[meshTriangle->v3]; b3AABB3 aabb; - aabb.Set(v1, v2, v3); + aabb.Set(A, B, C); aabb.Extend(triangle->m_radius); triangle->m_aabbProxy.type = e_triangleProxy; triangle->m_aabbProxy.owner = triangle; triangle->m_broadPhaseId = m_contactManager.m_broadPhase.CreateProxy(aabb, &triangle->m_aabbProxy); + + // uv coordinates for triangle + + // v1 + b3Vec2 uv1; + uv1.SetZero(); + + // v2 + b3Vec3 AB = B - A; + + b3Vec2 uv2; + uv2.x = b3Length(AB); + uv2.y = 0.0f; + + // v3 + b3Vec3 n_AB = b3Normalize(AB); + b3Vec3 AC = C - A; + float32 len_AC = b3Length(AC); + + b3Vec2 uv3; + uv3.x = b3Dot(n_AB, AC); + uv3.y = b3Sqrt(len_AC * len_AC - uv3.x * uv3.x); + + // Strech matrix + float32 du1 = uv2.x - uv1.x; + float32 dv1 = uv2.y - uv1.y; + float32 du2 = uv3.x - uv1.x; + float32 dv2 = uv3.y - uv1.y; + + triangle->m_du1 = du1; + triangle->m_dv1 = dv1; + triangle->m_du2 = du2; + triangle->m_dv2 = dv2; + + float32 det = du1 * dv2 - du2 * dv1; + B3_ASSERT(det != 0.0f); + triangle->m_inv_det = 1.0f / det; + + // Triangle area + b3Vec3 v1(du1, dv1, 0.0f); + b3Vec3 v2(du2, dv2, 0.0f); + triangle->m_alpha = 0.5f * b3Length(b3Cross(v1, v2)); + + // Create strech force + b3StrechForceDef sfdef; + sfdef.triangle = triangle; + sfdef.streching = def.streching; + sfdef.damping = def.damping; + sfdef.bu = 1.0f; + sfdef.bv = 1.0f; + + CreateForce(sfdef); } // Initialize forces @@ -206,28 +208,9 @@ b3Cloth::b3Cloth(const b3ClothDef& def) : // Worst-case edge memory u32 edgeCount = 3 * m->triangleCount; - b3UniqueEdge* uniqueEdges = (b3UniqueEdge*)allocator->Allocate(edgeCount * sizeof(b3UniqueEdge)); - u32 uniqueCount = b3FindUniqueEdges(uniqueEdges, m); - b3SharedEdge* sharedEdges = (b3SharedEdge*)allocator->Allocate(edgeCount * sizeof(b3SharedEdge)); u32 sharedCount = b3FindSharedEdges(sharedEdges, m); - for (u32 i = 0; i < uniqueCount; ++i) - { - b3UniqueEdge* e = uniqueEdges + i; - - b3Particle* p1 = m_particles[e->v1]; - b3Particle* p2 = m_particles[e->v2]; - - b3SpringForceDef fd; - fd.Initialize(p1, p2, def.structural, def.damping); - - if (def.structural > 0.0f) - { - CreateForce(fd); - } - } - // Bending for (u32 i = 0; i < sharedCount; ++i) { @@ -248,7 +231,6 @@ b3Cloth::b3Cloth(const b3ClothDef& def) : } allocator->Free(sharedEdges); - allocator->Free(uniqueEdges); // Sewing for (u32 i = 0; i < m->sewingLineCount; ++i) @@ -259,9 +241,9 @@ b3Cloth::b3Cloth(const b3ClothDef& def) : b3Particle* p2 = m_particles[line->v2]; b3SpringForceDef fd; - fd.Initialize(p1, p2, def.structural, def.damping); + fd.Initialize(p1, p2, def.sewing, def.damping); - if (def.structural > 0.0f) + if (def.sewing > 0.0f) { CreateForce(fd); } @@ -661,6 +643,8 @@ void b3Cloth::Draw() const b3Vec3 v2 = p2->m_position; b3Vec3 v3 = p3->m_position; + b3Draw_draw->DrawTriangle(v1, v2, v3, b3Color_black); + b3Vec3 c = (v1 + v2 + v3) / 3.0f; float32 s = 0.9f; diff --git a/src/bounce/cloth/cloth_solver.cpp b/src/bounce/cloth/cloth_solver.cpp index 2cae7b4..44203a2 100644 --- a/src/bounce/cloth/cloth_solver.cpp +++ b/src/bounce/cloth/cloth_solver.cpp @@ -90,11 +90,10 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterati forceSolver.Solve(dt, gravity); } - + // Copy particle state to state buffer b3Vec3* positions = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3)); b3Vec3* velocities = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3)); - for (u32 i = 0; i < m_particleCount; ++i) { positions[i] = m_particles[i]->m_position; @@ -147,8 +146,8 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterati positions[i] += h * velocities[i]; } - // Solve position constraints { + // Solve position constraints bool positionSolved = false; for (u32 i = 0; i < positionIterations; ++i) { diff --git a/src/bounce/cloth/force.cpp b/src/bounce/cloth/force.cpp index 4f658df..acc3227 100644 --- a/src/bounce/cloth/force.cpp +++ b/src/bounce/cloth/force.cpp @@ -17,6 +17,7 @@ */ #include +#include #include b3Force* b3Force::Create(const b3ForceDef* def) @@ -24,6 +25,12 @@ b3Force* b3Force::Create(const b3ForceDef* def) b3Force* force = NULL; switch (def->type) { + case e_strechForce: + { + void* block = b3Alloc(sizeof(b3StrechForce)); + force = new (block) b3StrechForce((b3StrechForceDef*)def); + break; + } case e_springForce: { void* block = b3Alloc(sizeof(b3SpringForce)); @@ -46,6 +53,13 @@ void b3Force::Destroy(b3Force* force) b3ForceType type = force->GetType(); switch (type) { + case e_strechForce: + { + b3StrechForce* o = (b3StrechForce*)force; + o->~b3StrechForce(); + b3Free(force); + break; + } case e_springForce: { b3SpringForce* o = (b3SpringForce*)force; diff --git a/src/bounce/cloth/strech_force.cpp b/src/bounce/cloth/strech_force.cpp new file mode 100644 index 0000000..cc4513a --- /dev/null +++ b/src/bounce/cloth/strech_force.cpp @@ -0,0 +1,247 @@ +/* +* 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 + +b3StrechForce::b3StrechForce(const b3StrechForceDef* def) +{ + m_type = e_strechForce; + m_triangle = def->triangle; + m_ks = def->streching; + m_kd = def->damping; + m_bu = def->bu; + m_bv = def->bv; + m_f1.SetZero(); + m_f2.SetZero(); + m_f3.SetZero(); +} + +b3StrechForce::~b3StrechForce() +{ + +} + +bool b3StrechForce::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 b3StrechForce::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; + b3SparseSymMat33& dfdx = *data->dfdx; + b3SparseSymMat33& 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); + float32 len_wu = b3Length(wu); + if (len_wu == 0.0f) + { + return; + } + B3_ASSERT(len_wu > 0.0f); + float32 inv_len_wu = 1.0f / len_wu; + b3Vec3 n_wu = inv_len_wu * wu; + + b3Vec3 wv = inv_det * (-du2 * dx1 + du1 * dx2); + float32 len_wv = b3Length(wv); + if (len_wv == 0.0f) + { + return; + } + B3_ASSERT(len_wv > 0.0f); + float32 inv_len_wv = 1.0f / len_wv; + b3Vec3 n_wv = inv_len_wv * wv; + + 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; + + float32 Cu = alpha * (len_wu - m_bu); + float32 Cv = alpha * (len_wv - m_bv); + + // This is a small trick to ensure PD-ness of the linear system + Cu = b3Max(Cu, 0.0f); + Cv = b3Max(Cv, 0.0f); + + if (Cu == 0.0f && Cv == 0.0f) + { + return; + } + + // Jacobian + b3Vec3 dCudx[3]; + b3Vec3 dCvdx[3]; + for (u32 i = 0; i < 3; ++i) + { + dCudx[i] = alpha * dwudx[i] * n_wu; + dCvdx[i] = alpha * dwvdx[i] * n_wv; + } + + m_f1.SetZero(); + m_f2.SetZero(); + m_f3.SetZero(); + + if (m_ks > 0.0f) + { + // Force + b3Vec3 fs[3]; + for (u32 i = 0; i < 3; ++i) + { + fs[i] = -m_ks * (Cu * dCudx[i] + Cv * dCvdx[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 d2Cuxij = (alpha * inv_len_wu * dwudx[i] * dwudx[j]) * (I - b3Outer(n_wu, n_wu)); + + b3Mat33 d2Cvxij = (alpha * inv_len_wv * dwvdx[i] * dwvdx[j]) * (I - b3Outer(n_wv, n_wv)); + + b3Mat33 Jij = -m_ks * (b3Outer(dCudx[i], dCudx[j]) + b3Outer(dCvdx[i], dCvdx[j]) + Cu * d2Cuxij + Cv * d2Cvxij); + + 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) + { + float32 dCudt = 0.0f; + float32 dCvdt = 0.0f; + + b3Vec3 vs[3] = { v1, v2, v3 }; + for (u32 i = 0; i < 3; ++i) + { + dCudt += b3Dot(dCudx[i], vs[i]); + dCvdt += b3Dot(dCvdx[i], vs[i]); + } + + // Force + b3Vec3 fs[3]; + for (u32 i = 0; i < 3; ++i) + { + fs[i] = -m_kd * (dCudt * dCudx[i] + dCvdt * dCvdx[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(dCudx[i], dCudx[j]) + b3Outer(dCvdx[i], dCvdx[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