diff --git a/examples/testbed/framework/test_entries.cpp b/examples/testbed/framework/test_entries.cpp index 9598c7f..b54d95e 100644 --- a/examples/testbed/framework/test_entries.cpp +++ b/examples/testbed/framework/test_entries.cpp @@ -60,11 +60,10 @@ #include #include #include -#include #include #include #include -#include +#include #include #include #include @@ -113,11 +112,10 @@ TestEntry g_tests[] = { "Tumbler", &Tumbler::Create }, { "Initial Overlap", &InitialOverlap::Create }, { "Multiple Pendulum", &MultiplePendulum::Create }, - { "Cloth", &Cloth::Create }, { "Table Cloth", &TableCloth::Create }, { "Pinned Cloth", &PinnedCloth::Create }, { "Shirt", &Shirt::Create }, - { "Mass Types", &MassTypes::Create }, + { "Particle Types", &ParticleTypes::Create }, { "Tension Mapping", &TensionMapping::Create }, { "Mass-Spring System", &MassSpring::Create }, { "Single Pendulum", &SinglePendulum::Create }, diff --git a/examples/testbed/tests/cloth_test.h b/examples/testbed/tests/cloth_test.h index f96afc3..a6e5c7f 100644 --- a/examples/testbed/tests/cloth_test.h +++ b/examples/testbed/tests/cloth_test.h @@ -16,52 +16,344 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CLOTH_H -#define CLOTH_H +#ifndef CLOTH_TESH_H +#define CLOTH_TESH_H -class Cloth : public Test +class ClothDragger { public: - Cloth() + ClothDragger(Ray3* ray, b3Cloth* cloth) { - b3ClothDef def; - def.mesh = &m_clothMesh; - def.density = 0.2f; - def.gravity.Set(0.0f, -10.0f, 0.0f); - def.k1 = 0.2f; - def.k2 = 0.1f; - def.kd = 0.005f; - def.r = 1.0f; + m_ray = ray; + m_cloth = cloth; + m_isSelected = false; + } - m_cloth.Initialize(def); + ~ClothDragger() + { - b3AABB3 aabb; - aabb.m_lower.Set(-5.0f, -1.0f, -6.0f); - aabb.m_upper.Set(5.0f, 1.0f, -4.0f); + } - b3Particle* vs = m_cloth.GetVertices(); - for (u32 i = 0; i < m_cloth.GetVertexCount(); ++i) + bool IsSelected() const + { + return m_isSelected; + } + + b3Vec3 GetPointA() const + { + B3_ASSERT(m_isSelected); + + b3ClothMesh* m = m_cloth->GetMesh(); + b3ClothMeshTriangle* t = m->triangles + m_selection; + + b3Vec3 A = m->vertices[t->v1]; + b3Vec3 B = m->vertices[t->v2]; + b3Vec3 C = m->vertices[t->v3]; + + return m_u * A + m_v * B + (1.0f - m_u - m_v) * C; + } + + b3Vec3 GetPointB() const + { + B3_ASSERT(m_isSelected); + return (1.0f - m_x) * m_ray->A() + m_x * m_ray->B(); + } + + bool StartDragging() + { + B3_ASSERT(m_isSelected == false); + + if (Select(m_selection, m_x) == false) { - if (aabb.Contains(vs[i].p)) + return false; + } + + m_isSelected = true; + + b3ClothMesh* m = m_cloth->GetMesh(); + b3ClothMeshTriangle* t = m->triangles + m_selection; + + b3Particle* p1 = m_cloth->GetParticle(t->v1); + m_t1 = p1->type; + m_cloth->SetType(p1, e_staticParticle); + + b3Particle* p2 = m_cloth->GetParticle(t->v2); + m_t2 = p2->type; + m_cloth->SetType(p2, e_staticParticle); + + b3Particle* p3 = m_cloth->GetParticle(t->v3); + m_t3 = p3->type; + m_cloth->SetType(p3, e_staticParticle); + + b3Vec3 v1 = p1->position; + b3Vec3 v2 = p2->position; + b3Vec3 v3 = p3->position; + + b3Vec3 B = GetPointB(); + + float32 wABC[4]; + b3BarycentricCoordinates(wABC, v1, v2, v3, B); + + if (wABC[3] > B3_EPSILON) + { + m_u = wABC[0] / wABC[3]; + m_v = wABC[1] / wABC[3]; + } + else + { + m_u = m_v = 0.0f; + } + + return true; + } + + void Drag() + { + B3_ASSERT(m_isSelected); + + b3ClothMesh* m = m_cloth->GetMesh(); + b3ClothMeshTriangle* t = m->triangles + m_selection; + + b3Vec3 A = GetPointA(); + b3Vec3 B = GetPointB(); + + b3Vec3 dx = B - A; + + b3Particle* p1 = m_cloth->GetParticle(t->v1); + m_cloth->Translate(p1, dx); + + b3Particle* p2 = m_cloth->GetParticle(t->v2); + m_cloth->Translate(p2, dx); + + b3Particle* p3 = m_cloth->GetParticle(t->v3); + m_cloth->Translate(p3, dx); + } + + void StopDragging() + { + B3_ASSERT(m_isSelected); + + m_isSelected = false; + + b3ClothMesh* m = m_cloth->GetMesh(); + b3ClothMeshTriangle* t = m->triangles + m_selection; + + b3Particle* p1 = m_cloth->GetParticle(t->v1); + m_cloth->SetType(p1, m_t1); + + b3Particle* p2 = m_cloth->GetParticle(t->v2); + m_cloth->SetType(p2, m_t2); + + b3Particle* p3 = m_cloth->GetParticle(t->v3); + m_cloth->SetType(p3, m_t3); + } + +private: + bool RayCast(b3RayCastOutput* output, u32 triangleIndex) const + { + b3ClothMesh* mesh = m_cloth->GetMesh(); + B3_ASSERT(triangleIndex < mesh->triangleCount); + b3ClothMeshTriangle* triangle = mesh->triangles + triangleIndex; + + b3Vec3 v1 = mesh->vertices[triangle->v1]; + b3Vec3 v2 = mesh->vertices[triangle->v2]; + b3Vec3 v3 = mesh->vertices[triangle->v3]; + + b3Vec3 p1 = m_ray->A(); + b3Vec3 p2 = m_ray->B(); + b3Vec3 d = p2 - p1; + float32 maxFraction = b3Length(d); + + b3Vec3 n = b3Cross(v2 - v1, v3 - v1); + n.Normalize(); + + float32 numerator = b3Dot(n, v1 - p1); + float32 denominator = b3Dot(n, d); + + if (denominator == 0.0f) + { + return false; + } + + float32 t = numerator / denominator; + + // Is the intersection not on the segment? + if (t < 0.0f || maxFraction < t) + { + return false; + } + + b3Vec3 q = p1 + t * d; + + // Barycentric coordinates for q + b3Vec3 Q = q; + b3Vec3 A = v1; + b3Vec3 B = v2; + b3Vec3 C = v3; + + b3Vec3 AB = B - A; + b3Vec3 AC = C - A; + + b3Vec3 QA = A - Q; + b3Vec3 QB = B - Q; + b3Vec3 QC = C - Q; + + b3Vec3 QB_x_QC = b3Cross(QB, QC); + b3Vec3 QC_x_QA = b3Cross(QC, QA); + b3Vec3 QA_x_QB = b3Cross(QA, QB); + + b3Vec3 AB_x_AC = b3Cross(AB, AC); + + float32 u = b3Dot(QB_x_QC, AB_x_AC); + float32 v = b3Dot(QC_x_QA, AB_x_AC); + float32 w = b3Dot(QA_x_QB, AB_x_AC); + + // This tolerance helps intersections lying on + // shared edges to not be missed. + const float32 kTol = -0.005f; + + // Is the intersection on the triangle? + if (u > kTol && v > kTol && w > kTol) + { + output->fraction = t; + + // Does the ray start from below or above the triangle? + if (numerator > 0.0f) { - vs[i].im = 0.0f; + output->normal = -n; + } + else + { + output->normal = n; + } + + return true; + } + + return false; + } + + bool Select(u32& selection, float32& fraction) const + { + b3ClothMesh* m = m_cloth->GetMesh(); + + b3RayCastInput input; + input.p1 = m_ray->A(); + input.p2 = m_ray->B(); + input.maxFraction = m_ray->fraction; + + float32 minFraction = B3_MAX_FLOAT; + b3Vec3 minNormal(0.0f, 0.0f, 0.0f); + u32 minIndex = ~0; + + for (u32 i = 0; i < m->triangleCount; ++i) + { + b3RayCastOutput subOutput; + if (RayCast(&subOutput, i) == true) + { + if (subOutput.fraction < minFraction) + { + minFraction = subOutput.fraction; + minNormal = subOutput.normal; + minIndex = i; + } } } + + if (minIndex != ~0) + { + selection = minIndex; + fraction = minFraction; + return true; + } + + return false; + } + + bool m_isSelected; + + Ray3* m_ray; + float32 m_x; + + b3Cloth * m_cloth; + u32 m_selection; + float32 m_u, m_v; + b3ParticleType m_t1, m_t2, m_t3; +}; + +class ClothTest : public Test +{ +public: + ClothTest() : m_clothDragger(&m_clothRay, &m_cloth) + { + m_cloth.SetGravity(b3Vec3(0.0f, -10.0f, 0.0f)); + + m_clothRay.origin.SetZero(); + m_clothRay.direction.Set(0.0f, 0.0f, -1.0f); + m_clothRay.fraction = g_camera->m_zFar; } void Step() { - m_cloth.Step(g_testSettings->inv_hertz, g_testSettings->positionIterations); + float32 dt = g_testSettings->inv_hertz; + + m_cloth.Step(dt); + m_cloth.Apply(); + + b3Shape** shapes = m_cloth.GetShapeList(); + for (u32 i = 0; i < m_cloth.GetShapeCount(); ++i) + { + b3Shape* s = shapes[i]; + + b3Transform xf; + xf.SetIdentity(); + + g_draw->DrawSolidShape(s, b3Color_white, xf); + } + m_cloth.Draw(); + + extern u32 b3_clothSolverIterations; + g_draw->DrawString(b3Color_white, "Iterations = %u", b3_clothSolverIterations); + + float32 E = m_cloth.GetEnergy(); + g_draw->DrawString(b3Color_white, "E = %f", E); + + if (m_clothDragger.IsSelected() == true) + { + g_draw->DrawSegment(m_clothDragger.GetPointA(), m_clothDragger.GetPointB(), b3Color_white); + } } - static Test* Create() + void MouseMove(const Ray3& pw) { - return new Cloth(); + m_clothRay = pw; + + if (m_clothDragger.IsSelected() == true) + { + m_clothDragger.Drag(); + } } - b3GridMesh<10, 10> m_clothMesh; + void MouseLeftDown(const Ray3& pw) + { + if (m_clothDragger.IsSelected() == false) + { + m_clothDragger.StartDragging(); + } + } + + void MouseLeftUp(const Ray3& pw) + { + if (m_clothDragger.IsSelected() == true) + { + m_clothDragger.StopDragging(); + } + } + + Ray3 m_clothRay; b3Cloth m_cloth; + ClothDragger m_clothDragger; }; #endif \ No newline at end of file diff --git a/examples/testbed/tests/mass_types.h b/examples/testbed/tests/mass_types.h deleted file mode 100644 index 5bf5b42..0000000 --- a/examples/testbed/tests/mass_types.h +++ /dev/null @@ -1,175 +0,0 @@ -/* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net -* -* 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 MASS_TYPES_H -#define MASS_TYPES_H - -class MassTypes : public PinnedCloth -{ -public: - MassTypes() - { - } - - void Step() - { - PinnedCloth::Step(); - - g_draw->DrawString(b3Color_white, "S - Static"); - g_draw->DrawString(b3Color_white, "D - Dynamic"); - g_draw->DrawString(b3Color_white, "K - Kinematic"); - g_draw->DrawString(b3Color_white, "Arrows - Apply Force/Velocity/Position"); - } - - void SetClothType(b3MassType type) - { - for (u32 i = 0; i < m_cloth.GetMassCount(); ++i) - { - m_cloth.SetType(i, type); - } - } - - void KeyDown(int button) - { - if (button == GLFW_KEY_S) - { - SetClothType(b3MassType::e_staticMass); - } - - if (button == GLFW_KEY_K) - { - SetClothType(b3MassType::e_kinematicMass); - } - - if (button == GLFW_KEY_D) - { - SetClothType(b3MassType::e_dynamicMass); - } - - for (u32 i = 0; i < m_cloth.GetMassCount(); ++i) - { - if (m_cloth.GetType(i) == b3MassType::e_staticMass) - { - if (button == GLFW_KEY_LEFT) - { - b3Vec3 p = m_cloth.GetPosition(i); - - p.x -= 1.0f; - - m_cloth.SetPosition(i, p); - } - - if (button == GLFW_KEY_RIGHT) - { - b3Vec3 p = m_cloth.GetPosition(i); - - p.x += 1.0f; - - m_cloth.SetPosition(i, p); - } - - if (button == GLFW_KEY_UP) - { - b3Vec3 p = m_cloth.GetPosition(i); - - p.z += 1.0f; - - m_cloth.SetPosition(i, p); - } - - if (button == GLFW_KEY_DOWN) - { - b3Vec3 p = m_cloth.GetPosition(i); - - p.z -= 1.0f; - - m_cloth.SetPosition(i, p); - } - } - - if (m_cloth.GetType(i) == b3MassType::e_kinematicMass) - { - if (button == GLFW_KEY_LEFT) - { - b3Vec3 v = m_cloth.GetVelocity(i); - - v.x -= 5.0f; - - m_cloth.SetVelocity(i, v); - } - - if (button == GLFW_KEY_RIGHT) - { - b3Vec3 v = m_cloth.GetVelocity(i); - - v.x += 5.0f; - - m_cloth.SetVelocity(i, v); - } - - if (button == GLFW_KEY_UP) - { - b3Vec3 v = m_cloth.GetVelocity(i); - - v.z -= 5.0f; - - m_cloth.SetVelocity(i, v); - } - - if (button == GLFW_KEY_DOWN) - { - b3Vec3 v = m_cloth.GetVelocity(i); - - v.z += 5.0f; - - m_cloth.SetVelocity(i, v); - } - } - - if (m_cloth.GetType(i) == b3MassType::e_dynamicMass) - { - if (button == GLFW_KEY_LEFT) - { - m_cloth.ApplyForce(i, b3Vec3(-100.0f, 0.0f, 0.0f)); - } - - if (button == GLFW_KEY_RIGHT) - { - m_cloth.ApplyForce(i, b3Vec3(100.0f, 0.0f, 0.0f)); - } - - if (button == GLFW_KEY_UP) - { - m_cloth.ApplyForce(i, b3Vec3(0.0f, 0.0f, -100.0f)); - } - - if (button == GLFW_KEY_DOWN) - { - m_cloth.ApplyForce(i, b3Vec3(0.0f, 0.0f, 100.0f)); - } - } - } - } - - static Test* Create() - { - return new MassTypes(); - } -}; - -#endif \ No newline at end of file diff --git a/examples/testbed/tests/particle_types.h b/examples/testbed/tests/particle_types.h new file mode 100644 index 0000000..48afdb9 --- /dev/null +++ b/examples/testbed/tests/particle_types.h @@ -0,0 +1,127 @@ +/* +* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* +* 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 PARTICLE_TYPES_H +#define PARTICLE_TYPES_H + +class ParticleTypes : public PinnedCloth +{ +public: + ParticleTypes() + { + } + + void Step() + { + PinnedCloth::Step(); + + g_draw->DrawString(b3Color_white, "S - Static"); + g_draw->DrawString(b3Color_white, "D - Dynamic"); + g_draw->DrawString(b3Color_white, "K - Kinematic"); + g_draw->DrawString(b3Color_white, "Arrows - Apply Force/Velocity/Position"); + } + + void SetClothType(b3ParticleType type) + { + for (u32 i = 0; i < m_cloth.GetParticleCount(); ++i) + { + b3Particle* p = m_cloth.GetParticle(i); + m_cloth.SetType(p, type); + } + } + + void KeyDown(int button) + { + if (button == GLFW_KEY_S) + { + SetClothType(e_staticParticle); + } + + if (button == GLFW_KEY_K) + { + SetClothType(e_kinematicParticle); + } + + if (button == GLFW_KEY_D) + { + SetClothType(e_dynamicParticle); + } + + for (u32 i = 0; i < m_cloth.GetParticleCount(); ++i) + { + b3Particle* p = m_cloth.GetParticle(i); + + b3Vec3 d; + d.SetZero(); + + if (button == GLFW_KEY_LEFT) + { + d.x = -1.0f; + } + + if (button == GLFW_KEY_RIGHT) + { + d.x = 1.0f; + } + + if (button == GLFW_KEY_UP) + { + d.y = 1.0f; + } + + if (button == GLFW_KEY_DOWN) + { + d.y = -1.0f; + } + + if (button == GLFW_KEY_LEFT || + button == GLFW_KEY_RIGHT || + button == GLFW_KEY_UP || + button == GLFW_KEY_DOWN) + { + if (p->type == e_staticParticle) + { + m_cloth.Translate(p, d); + } + + if (p->type == e_kinematicParticle) + { + b3Vec3 v = p->velocity; + + v += 5.0f * d; + + m_cloth.SetVelocity(p, d); + } + + if (p->type == e_dynamicParticle) + { + b3Vec3 f = 100.0f * d; + + m_cloth.ApplyForce(p, f); + } + } + } + } + + static Test* Create() + { + return new ParticleTypes(); + } +}; + +#endif \ No newline at end of file diff --git a/examples/testbed/tests/pinned_cloth.h b/examples/testbed/tests/pinned_cloth.h index 6dd3ebd..f1d3ffa 100644 --- a/examples/testbed/tests/pinned_cloth.h +++ b/examples/testbed/tests/pinned_cloth.h @@ -19,7 +19,7 @@ #ifndef PINNED_CLOTH_H #define PINNED_CLOTH_H -class PinnedCloth : public SpringClothTest +class PinnedCloth : public ClothTest { public: PinnedCloth() @@ -42,14 +42,12 @@ public: m_gridClothMesh.sewingLineCount = 0; m_gridClothMesh.sewingLines = nullptr; - b3SpringClothDef def; - def.allocator = &m_clothAllocator; + b3ClothDef def; def.mesh = &m_gridClothMesh; def.density = 0.2f; def.ks = 10000.0f; def.kd = 0.0f; def.r = 0.05f; - def.gravity.Set(0.0f, -10.0f, 0.0f); m_cloth.Initialize(def); @@ -57,11 +55,12 @@ public: aabb.m_lower.Set(-5.0f, -1.0f, -6.0f); aabb.m_upper.Set(5.0f, 1.0f, -4.0f); - for (u32 i = 0; i < m_cloth.GetMassCount(); ++i) + for (u32 i = 0; i < m_cloth.GetParticleCount(); ++i) { - if (aabb.Contains(m_cloth.GetPosition(i))) + b3Particle* p = m_cloth.GetParticle(i); + if (aabb.Contains(p->position)) { - m_cloth.SetType(i, b3MassType::e_staticMass); + m_cloth.SetType(p, e_staticParticle); } } } diff --git a/examples/testbed/tests/shirt.h b/examples/testbed/tests/shirt.h index 2f371c5..784b42e 100644 --- a/examples/testbed/tests/shirt.h +++ b/examples/testbed/tests/shirt.h @@ -23,7 +23,7 @@ #include #include -class Shirt : public SpringClothTest +class Shirt : public ClothTest { public: Shirt() @@ -56,13 +56,11 @@ public: } // Create cloth - b3SpringClothDef def; - def.allocator = &m_clothAllocator; + b3ClothDef def; def.mesh = &m_shirtClothMesh; def.density = 0.2f; - def.ks = 10000.0f; def.r = 0.2f; - def.gravity.Set(0.0f, -10.0f, 0.0f); + def.ks = 10000.0f; m_cloth.Initialize(def); } diff --git a/examples/testbed/tests/spring_cloth_test.h b/examples/testbed/tests/spring_cloth_test.h deleted file mode 100644 index e988186..0000000 --- a/examples/testbed/tests/spring_cloth_test.h +++ /dev/null @@ -1,356 +0,0 @@ -/* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net -* -* 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 SPRING_CLOTH_TESH_H -#define SPRING_CLOTH_TESH_H - -class ClothDragger -{ -public: - ClothDragger(Ray3* ray, b3SpringCloth* cloth) - { - m_ray = ray; - m_cloth = cloth; - m_isSelected = false; - } - - ~ClothDragger() - { - - } - - bool IsSelected() const - { - return m_isSelected; - } - - b3Vec3 GetPointA() const - { - B3_ASSERT(m_isSelected); - - b3ClothMesh* m = m_cloth->GetMesh(); - b3ClothMeshTriangle* t = m->triangles + m_selection; - - b3Vec3 A = m->vertices[t->v1]; - b3Vec3 B = m->vertices[t->v2]; - b3Vec3 C = m->vertices[t->v3]; - - return m_u * A + m_v * B + (1.0f - m_u - m_v) * C; - } - - b3Vec3 GetPointB() const - { - B3_ASSERT(m_isSelected); - return (1.0f - m_x) * m_ray->A() + m_x * m_ray->B(); - } - - bool StartDragging() - { - B3_ASSERT(m_isSelected == false); - - if (Select(m_selection, m_x) == false) - { - return false; - } - - m_isSelected = true; - - b3ClothMesh* m = m_cloth->GetMesh(); - b3ClothMeshTriangle* t = m->triangles + m_selection; - - m_t1 = m_cloth->GetType(t->v1); - m_cloth->SetType(t->v1, b3MassType::e_staticMass); - - m_t2 = m_cloth->GetType(t->v2); - m_cloth->SetType(t->v2, b3MassType::e_staticMass); - - m_t3 = m_cloth->GetType(t->v3); - m_cloth->SetType(t->v3, b3MassType::e_staticMass); - - b3Vec3 v1 = m_cloth->GetPosition(t->v1); - b3Vec3 v2 = m_cloth->GetPosition(t->v2); - b3Vec3 v3 = m_cloth->GetPosition(t->v3); - - b3Vec3 B = GetPointB(); - - float32 wABC[4]; - b3BarycentricCoordinates(wABC, v1, v2, v3, B); - - if (wABC[3] > B3_EPSILON) - { - m_u = wABC[0] / wABC[3]; - m_v = wABC[1] / wABC[3]; - } - else - { - m_u = m_v = 0.0f; - } - - return true; - } - - void Drag() - { - B3_ASSERT(m_isSelected); - - b3ClothMesh* m = m_cloth->GetMesh(); - b3ClothMeshTriangle* t = m->triangles + m_selection; - - b3Vec3 A = GetPointA(); - b3Vec3 B = GetPointB(); - - b3Vec3 dx = B - A; - - b3Vec3 v1 = m_cloth->GetPosition(t->v1); - v1 += dx; - m_cloth->SetPosition(t->v1, v1); - - b3Vec3 v2 = m_cloth->GetPosition(t->v2); - v2 += dx; - m_cloth->SetPosition(t->v2, v2); - - b3Vec3 v3 = m_cloth->GetPosition(t->v3); - v3 += dx; - m_cloth->SetPosition(t->v3, v3); - } - - void StopDragging() - { - B3_ASSERT(m_isSelected); - - m_isSelected = false; - - b3ClothMesh* m = m_cloth->GetMesh(); - b3ClothMeshTriangle* t = m->triangles + m_selection; - - m_cloth->SetType(t->v1, m_t1); - m_cloth->SetType(t->v2, m_t2); - m_cloth->SetType(t->v3, m_t3); - } - -private: - bool RayCast(b3RayCastOutput* output, u32 triangleIndex) const - { - b3ClothMesh* mesh = m_cloth->GetMesh(); - B3_ASSERT(triangleIndex < mesh->triangleCount); - b3ClothMeshTriangle* triangle = mesh->triangles + triangleIndex; - - b3Vec3 v1 = mesh->vertices[triangle->v1]; - b3Vec3 v2 = mesh->vertices[triangle->v2]; - b3Vec3 v3 = mesh->vertices[triangle->v3]; - - b3Vec3 p1 = m_ray->A(); - b3Vec3 p2 = m_ray->B(); - b3Vec3 d = p2 - p1; - float32 maxFraction = b3Length(d); - - b3Vec3 n = b3Cross(v2 - v1, v3 - v1); - n.Normalize(); - - float32 numerator = b3Dot(n, v1 - p1); - float32 denominator = b3Dot(n, d); - - if (denominator == 0.0f) - { - return false; - } - - float32 t = numerator / denominator; - - // Is the intersection not on the segment? - if (t < 0.0f || maxFraction < t) - { - return false; - } - - b3Vec3 q = p1 + t * d; - - // Barycentric coordinates for q - b3Vec3 Q = q; - b3Vec3 A = v1; - b3Vec3 B = v2; - b3Vec3 C = v3; - - b3Vec3 AB = B - A; - b3Vec3 AC = C - A; - - b3Vec3 QA = A - Q; - b3Vec3 QB = B - Q; - b3Vec3 QC = C - Q; - - b3Vec3 QB_x_QC = b3Cross(QB, QC); - b3Vec3 QC_x_QA = b3Cross(QC, QA); - b3Vec3 QA_x_QB = b3Cross(QA, QB); - - b3Vec3 AB_x_AC = b3Cross(AB, AC); - - float32 u = b3Dot(QB_x_QC, AB_x_AC); - float32 v = b3Dot(QC_x_QA, AB_x_AC); - float32 w = b3Dot(QA_x_QB, AB_x_AC); - - // This tolerance helps intersections lying on - // shared edges to not be missed. - const float32 kTol = -0.005f; - - // Is the intersection on the triangle? - if (u > kTol && v > kTol && w > kTol) - { - output->fraction = t; - - // Does the ray start from below or above the triangle? - if (numerator > 0.0f) - { - output->normal = -n; - } - else - { - output->normal = n; - } - - return true; - } - - return false; - } - - bool Select(u32& selection, float32& fraction) const - { - b3ClothMesh* m = m_cloth->GetMesh(); - - b3RayCastInput input; - input.p1 = m_ray->A(); - input.p2 = m_ray->B(); - input.maxFraction = m_ray->fraction; - - float32 minFraction = B3_MAX_FLOAT; - b3Vec3 minNormal(0.0f, 0.0f, 0.0f); - u32 minIndex = ~0; - - for (u32 i = 0; i < m->triangleCount; ++i) - { - b3RayCastOutput subOutput; - if (RayCast(&subOutput, i) == true) - { - if (subOutput.fraction < minFraction) - { - minFraction = subOutput.fraction; - minNormal = subOutput.normal; - minIndex = i; - } - } - } - - if (minIndex != ~0) - { - selection = minIndex; - fraction = minFraction; - return true; - } - - return false; - } - - bool m_isSelected; - - Ray3* m_ray; - float32 m_x; - - b3SpringCloth * m_cloth; - u32 m_selection; - float32 m_u, m_v; - b3MassType m_t1, m_t2, m_t3; -}; - -class SpringClothTest : public Test -{ -public: - SpringClothTest() : m_clothDragger(&m_clothRay, &m_cloth) - { - m_clothRay.origin.SetZero(); - m_clothRay.direction.Set(0.0f, 0.0f, -1.0f); - m_clothRay.fraction = g_camera->m_zFar; - } - - void Step() - { - float32 dt = g_testSettings->inv_hertz; - - m_cloth.Step(dt); - m_cloth.Apply(); - - b3Shape** shapes = m_cloth.GetShapes(); - for (u32 i = 0; i < m_cloth.GetShapeCount(); ++i) - { - b3Shape* s = shapes[i]; - - b3Transform xf; - xf.SetIdentity(); - - g_draw->DrawSolidShape(s, b3Color_white, xf); - } - - m_cloth.Draw(); - - b3SpringClothStep step = m_cloth.GetStep(); - - g_draw->DrawString(b3Color_white, "Iterations = %u", step.iterations); - - float32 E = m_cloth.GetEnergy(); - g_draw->DrawString(b3Color_white, "E = %f", E); - - if (m_clothDragger.IsSelected() == true) - { - g_draw->DrawSegment(m_clothDragger.GetPointA(), m_clothDragger.GetPointB(), b3Color_white); - } - } - - void MouseMove(const Ray3& pw) - { - m_clothRay = pw; - - if (m_clothDragger.IsSelected() == true) - { - m_clothDragger.Drag(); - } - } - - void MouseLeftDown(const Ray3& pw) - { - if (m_clothDragger.IsSelected() == false) - { - m_clothDragger.StartDragging(); - } - } - - void MouseLeftUp(const Ray3& pw) - { - if (m_clothDragger.IsSelected() == true) - { - m_clothDragger.StopDragging(); - } - } - - Ray3 m_clothRay; - - b3StackAllocator m_clothAllocator; - b3SpringCloth m_cloth; - - ClothDragger m_clothDragger; -}; - -#endif \ No newline at end of file diff --git a/examples/testbed/tests/table_cloth.h b/examples/testbed/tests/table_cloth.h index 6717a2d..e204c04 100644 --- a/examples/testbed/tests/table_cloth.h +++ b/examples/testbed/tests/table_cloth.h @@ -19,7 +19,7 @@ #ifndef TABLE_CLOTH_H #define TABLE_CLOTH_H -class TableCloth : public SpringClothTest +class TableCloth : public ClothTest { public: TableCloth() @@ -48,14 +48,12 @@ public: m_gridClothMesh.sewingLineCount = 0; m_gridClothMesh.sewingLines = nullptr; - b3SpringClothDef def; - def.allocator = &m_clothAllocator; + b3ClothDef def; def.mesh = &m_gridClothMesh; def.density = 0.2f; def.ks = 10000.0f; def.kd = 0.0f; def.r = 0.05f; - def.gravity.Set(0.0f, -10.0f, 0.0f); m_cloth.Initialize(def); diff --git a/examples/testbed/tests/tension_mapping.h b/examples/testbed/tests/tension_mapping.h index f072de3..362206f 100644 --- a/examples/testbed/tests/tension_mapping.h +++ b/examples/testbed/tests/tension_mapping.h @@ -55,7 +55,7 @@ static inline b3Color Color(float32 x, float32 a, float32 b) return c; } -class TensionMapping : public SpringClothTest +class TensionMapping : public ClothTest { public: TensionMapping() @@ -78,14 +78,12 @@ public: m_gridClothMesh.sewingLineCount = 0; m_gridClothMesh.sewingLines = nullptr; - b3SpringClothDef def; - def.allocator = &m_clothAllocator; + b3ClothDef def; def.mesh = &m_gridClothMesh; def.density = 0.2f; def.ks = 10000.0f; def.kd = 0.0f; def.r = 0.2f; - def.gravity.Set(0.0f, -10.0f, 0.0f); m_cloth.Initialize(def); @@ -93,11 +91,12 @@ public: aabb.m_lower.Set(-5.0f, -1.0f, -6.0f); aabb.m_upper.Set(5.0f, 1.0f, -4.0f); - for (u32 i = 0; i < m_cloth.GetMassCount(); ++i) + for (u32 i = 0; i < m_cloth.GetParticleCount(); ++i) { - if (aabb.Contains(m_cloth.GetPosition(i))) + b3Particle* p = m_cloth.GetParticle(i); + if (aabb.Contains(p->position)) { - m_cloth.SetType(i, b3MassType::e_staticMass); + m_cloth.SetType(p, e_staticParticle); } } } @@ -108,29 +107,30 @@ public: m_cloth.Step(dt); m_cloth.Apply(); - - b3StackArray T; - m_cloth.GetTension(T); for (u32 i = 0; i < m_gridClothMesh.triangleCount; ++i) { b3ClothMeshTriangle* t = m_gridClothMesh.triangles + i; - b3Vec3 v1 = m_gridClothMesh.vertices[t->v1]; - b3Vec3 v2 = m_gridClothMesh.vertices[t->v2]; - b3Vec3 v3 = m_gridClothMesh.vertices[t->v3]; + b3Particle* p1 = m_cloth.GetParticle(t->v1); + b3Particle* p2 = m_cloth.GetParticle(t->v2); + b3Particle* p3 = m_cloth.GetParticle(t->v3); + + b3Vec3 v1 = p1->position; + b3Vec3 v2 = p2->position; + b3Vec3 v3 = p3->position; b3Draw_draw->DrawSegment(v1, v2, b3Color_black); b3Draw_draw->DrawSegment(v2, v3, b3Color_black); b3Draw_draw->DrawSegment(v3, v1, b3Color_black); - b3Vec3 f1 = T[t->v1]; + b3Vec3 f1 = p1->tension; float32 L1 = b3Length(f1); - b3Vec3 f2 = T[t->v2]; + b3Vec3 f2 = p2->tension; float32 L2 = b3Length(f2); - b3Vec3 f3 = T[t->v3]; + b3Vec3 f3 = p3->tension; float32 L3 = b3Length(f3); float32 L = (L1 + L2 + L3) / 3.0f; @@ -140,16 +140,14 @@ public: b3Vec3 n1 = b3Cross(v2 - v1, v3 - v1); n1.Normalize(); + g_draw->DrawSolidTriangle(n1, v1, v2, v3, color); b3Vec3 n2 = -n1; - - g_draw->DrawSolidTriangle(n1, v1, v2, v3, color); g_draw->DrawSolidTriangle(n2, v1, v3, v2, color); } - b3SpringClothStep step = m_cloth.GetStep(); - - g_draw->DrawString(b3Color_white, "Iterations = %u", step.iterations); + extern u32 b3_clothSolverIterations; + g_draw->DrawString(b3Color_white, "Iterations = %u", b3_clothSolverIterations); if (m_clothDragger.IsSelected() == true) { diff --git a/include/bounce/bounce.h b/include/bounce/bounce.h index 23ad763..e35c15a 100644 --- a/include/bounce/bounce.h +++ b/include/bounce/bounce.h @@ -60,7 +60,6 @@ #include #include -#include #include diff --git a/include/bounce/dynamics/cloth/cloth.h b/include/bounce/dynamics/cloth/cloth.h index 84aca19..9906864 100644 --- a/include/bounce/dynamics/cloth/cloth.h +++ b/include/bounce/dynamics/cloth/cloth.h @@ -19,117 +19,324 @@ #ifndef B3_CLOTH_H #define B3_CLOTH_H -#include -#include +#include +#include +#include -struct b3Mesh; +// Maximum number of shapes per cloth. +#define B3_CLOTH_SHAPE_CAPACITY 32 +class b3Shape; + +struct b3ClothMesh; + +// Cloth mesh definition struct b3ClothDef { b3ClothDef() { - mesh = NULL; + mesh = nullptr; density = 0.0f; - gravity.SetZero(); - k1 = 0.9f; - k2 = 0.2f; - kd = 0.1f; - r = 0.0f; + r = 0.05f; + ks = 0.0f; + kb = 0.0f; + kd = 0.0f; } - // Cloth mesh - // Each edge must be shared by at most two triangles (manifold) - const b3Mesh* mesh; + // Cloth proxy mesh + b3ClothMesh* mesh; - // Cloth density in kilograms per meter squared + // Radius + // This should be a small value. It can be used for correcting visual artifacts when + // the masses are colliding against a solid. + float32 r; + + // Cloth density in kg/m^3 float32 density; - // Gravity force - b3Vec3 gravity; - // Streching stiffness - float32 k1; + float32 ks; // Bending stiffness - float32 k2; - - // Damping - float32 kd; + float32 kb; - // Cloth thickness - float32 r; + // Damping stiffness + float32 kd; }; +// Static particles have zero mass and velocity, and therefore they can't move. +// Kinematic particles are't moved by external and internal forces but can be moved by contact forces. +// Dynamic particles have non-zero mass and can move due to internal and external forces. +enum b3ParticleType +{ + e_staticParticle, + e_kinematicParticle, + e_dynamicParticle +}; + +// Read-only particle struct b3Particle { - float32 im; - b3Vec3 p0; - b3Vec3 p; - b3Vec3 v; + // Particle type + b3ParticleType type; + + // Mass position + b3Vec3 position; + + // Mass velocity + b3Vec3 velocity; + + // Mass force + b3Vec3 force; + + // Mass tension force for visualization + b3Vec3 tension; + + // Mass + float32 mass; + + // Inverse mass + float32 invMass; + + // Radius + float32 radius; + + // User data + void* userData; + + // Translation used for direct position manipulation + b3Vec3 translation; + + // Solver temp + + // Identifier + u32 solverId; + + // Solution + b3Vec3 x; }; -struct b3C1 +// Spring types +enum b3SpringType { - float32 L; - u32 i1; - u32 i2; + e_strechSpring, + e_bendSpring, }; -struct b3C2 +// Read-only spring +struct b3Spring { - float32 angle; - u32 i1; - u32 i2; - u32 i3; - u32 i4; + // Spring type + b3SpringType type; + + // Particle 1 + b3Particle* p1; + + // Particle 2 + b3Particle* p2; + + // Rest length + float32 L0; + + // Structural stiffness + float32 ks; + + // Damping stiffness + float32 kd; }; +// Read-only contact +struct b3ParticleContact +{ + b3Particle* p1; + b3Shape* s2; + b3Vec3 n, t1, t2; + float32 Fn, Ft1, Ft2; + bool n_active, t1_active, t2_active; +}; + +// A cloth represents a deformable surface/mesh. +// b3Cloth simulates this surface motion using particles and springs. class b3Cloth { public: b3Cloth(); ~b3Cloth(); + // Initialize this cloth from a definition. void Initialize(const b3ClothDef& def); - void Step(float32 dt, u32 iterations); + // Return the cloth mesh used to initialize this cloth. + b3ClothMesh* GetMesh() const; - u32 GetVertexCount() const - { - return m_pCount; - } + // Set the gravitational acceleration applied to this cloth. + // Units are m/s^2. + void SetGravity(const b3Vec3& gravity); - const b3Particle* GetVertices() const - { - return m_ps; - } + // Return the gravitational acceleration applied to this cloth. + const b3Vec3& GetGravity() const; - b3Particle* GetVertices() - { - return m_ps; - } + // Return the number of particles in this cloth. + u32 GetParticleCount() const; + // Return the particle at a given index in this cloth. + b3Particle* GetParticle(u32 i) const; + + // Set the type of a given particle. + void SetType(b3Particle* p, b3ParticleType type); + + // Translate a particle in the next time step. + void Translate(b3Particle* p, const b3Vec3& translation); + + // Set the velocity of a given particle. + void SetVelocity(b3Particle* p, const b3Vec3& velocity); + + // Apply a force to a given particle. + void ApplyForce(b3Particle* p, const b3Vec3& force); + + // Return the kinetic (or dynamic) energy in this system. + float32 GetEnergy() const; + + // Add a collision shape to the list of shapes in this cloth. + // The cloth will be able to respond to collisions with each shape in the list of shapes. + // Current the shape will be treated as a static shape. + void AddShape(b3Shape* shape); + + // Return the number of collision shapes in this cloth. + u32 GetShapeCount() const; + + // Return the list of collision shapes added to this cloth. + b3Shape** GetShapeList(); + + // Perform a time step. + void Step(float32 dt); + + // Set the positions of the mesh vertices to the positions of their associated particles. + void Apply() const; + + // Debug draw the cloth using the associated cloth mesh. void Draw() const; -private: - void SolveC1(); - void SolveC2(); +protected: + // Compute mass of each particle. + void ResetMass(); - b3Particle* m_ps; - u32 m_pCount; - - b3C1* m_c1s; - u32 m_c1Count; - - b3C2* m_c2s; - u32 m_c2Count; + // Update contacts. + // This is where some contacts might be initiated or terminated. + void UpdateContacts(); + + // Solve + void Solve(float32 dt); + + b3StackAllocator m_allocator; - float32 m_k1; - float32 m_k2; - float32 m_kd; - float32 m_r; b3Vec3 m_gravity; - const b3Mesh* m_mesh; + u32 m_particleCount; + b3Particle* m_particles; + + b3Spring* m_springs; + u32 m_springCount; + + b3ParticleContact* m_contacts; + //u32 m_contactCount; + + b3Shape* m_shapes[B3_CLOTH_SHAPE_CAPACITY]; + u32 m_shapeCount; + + b3ClothMesh* m_mesh; + float32 m_density; }; +inline b3ClothMesh* b3Cloth::GetMesh() const +{ + return m_mesh; +} + +inline const b3Vec3& b3Cloth::GetGravity() const +{ + return m_gravity; +} + +inline void b3Cloth::SetGravity(const b3Vec3& gravity) +{ + m_gravity = gravity; +} + +inline u32 b3Cloth::GetParticleCount() const +{ + return m_particleCount; +} + +inline b3Particle* b3Cloth::GetParticle(u32 i) const +{ + B3_ASSERT(i < m_particleCount); + return m_particles + i; +} + +inline void b3Cloth::SetType(b3Particle* p, b3ParticleType type) +{ + if (p->type == type) + { + return; + } + + p->type = type; + p->force.SetZero(); + + if (type == e_staticParticle) + { + p->velocity.SetZero(); + p->translation.SetZero(); + + u32 ip = u32(p - m_particles); + + m_contacts[ip].n_active = false; + m_contacts[ip].t1_active = false; + m_contacts[ip].t2_active = false; + } +} + +inline void b3Cloth::Translate(b3Particle* p, const b3Vec3& translation) +{ + p->translation += translation; +} + +inline void b3Cloth::SetVelocity(b3Particle* p, const b3Vec3& velocity) +{ + if (p->type == e_staticParticle) + { + return; + } + p->velocity = velocity; +} + +inline void b3Cloth::ApplyForce(b3Particle* p, const b3Vec3& force) +{ + if (p->type != e_dynamicParticle) + { + return; + } + p->force += force; +} + +inline float32 b3Cloth::GetEnergy() const +{ + float32 E = 0.0f; + for (u32 i = 0; i < m_particleCount; ++i) + { + E += m_particles[i].mass * b3Dot(m_particles[i].velocity, m_particles[i].velocity); + } + return 0.5f * E; +} + +inline u32 b3Cloth::GetShapeCount() const +{ + return m_shapeCount; +} + +inline b3Shape** b3Cloth::GetShapeList() +{ + return m_shapes; +} + #endif \ No newline at end of file diff --git a/include/bounce/dynamics/cloth/spring_solver.h b/include/bounce/dynamics/cloth/cloth_solver.h similarity index 57% rename from include/bounce/dynamics/cloth/spring_solver.h rename to include/bounce/dynamics/cloth/cloth_solver.h index d771bc2..df3f776 100644 --- a/include/bounce/dynamics/cloth/spring_solver.h +++ b/include/bounce/dynamics/cloth/cloth_solver.h @@ -16,48 +16,48 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#ifndef B3_SPRING_SOLVER_H -#define B3_SPRING_SOLVER_H +#ifndef B3_CLOTH_SOLVER_H +#define B3_CLOTH_SOLVER_H #include #include -class b3Shape; - -class b3SpringCloth; -class b3StackAllocator; - struct b3DenseVec3; struct b3DiagMat33; struct b3SparseMat33; -struct b3MassContact; +struct b3Particle; struct b3Spring; +struct b3ParticleContact; -enum class b3MassType : u32; +class b3Shape; +class b3StackAllocator; -struct b3SpringSolverDef +struct b3ClothSolverDef { - b3SpringCloth* cloth; - float32 dt; + b3StackAllocator* stack; + u32 particleCapacity; + u32 springCapacity; + u32 contactCapacity; }; -class b3SpringSolver +class b3ClothSolver { public: - b3SpringSolver(const b3SpringSolverDef& def); + b3ClothSolver(const b3ClothSolverDef& def); + ~b3ClothSolver(); + + void Add(b3Particle* p); + void Add(b3Spring* s); + void Add(b3ParticleContact* c); - ~b3SpringSolver(); - - void Solve(b3DenseVec3& extraForces); - - u32 GetIterations() const; + void Solve(float32 dt, const b3Vec3& gravity); private: - // Apply internal forces and store their unique derivatives. - void ApplySpringForces(); - + // Compute forces. + void Compute_f(b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3Vec3& gravity); + // Compute A and b in Ax = b - void Compute_A_b(b3SparseMat33& A, b3DenseVec3& b) const; + void Compute_A_b(b3SparseMat33& A, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const; // Compute S. void Compute_S(b3DiagMat33& S); @@ -67,38 +67,25 @@ private: // Solve Ax = b. // Output x and the residual error f = Ax - b ~ 0. - void Solve(b3DenseVec3& x, b3DenseVec3& f, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const; - - b3SpringCloth * m_cloth; - float32 m_h; - b3Mat33* m_Jx; - b3Mat33* m_Jv; - u32 m_iterations; + void Solve(b3DenseVec3& x, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const; b3StackAllocator* m_allocator; - b3Vec3* m_x; - b3Vec3* m_v; - b3Vec3* m_f; - float32* m_m; - float32* m_inv_m; - b3Vec3* m_y; - b3Vec3* m_z; - b3Vec3* m_x0; - b3MassType* m_types; - u32 m_massCount; + float32 m_h; + + u32 m_particleCapacity; + u32 m_particleCount; + b3Particle** m_particles; - b3MassContact* m_contacts; - - b3Spring* m_springs; + u32 m_springCapacity; u32 m_springCount; - - b3Shape** m_shapes; + b3Spring** m_springs; + b3Mat33* m_Jx; + b3Mat33* m_Jv; + + u32 m_contactCapacity; + u32 m_contactCount; + b3ParticleContact** m_contacts; }; -inline u32 b3SpringSolver::GetIterations() const -{ - return m_iterations; -} - #endif \ No newline at end of file diff --git a/include/bounce/dynamics/cloth/spring_cloth.h b/include/bounce/dynamics/cloth/spring_cloth.h deleted file mode 100644 index 7afc816..0000000 --- a/include/bounce/dynamics/cloth/spring_cloth.h +++ /dev/null @@ -1,356 +0,0 @@ -/* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net -* -* 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_SPRING_CLOTH_H -#define B3_SPRING_CLOTH_H - -#include -#include - -// Maximum number of shapes per cloth. -#define B3_CLOTH_SHAPE_CAPACITY 32 - -class b3StackAllocator; - -class b3Shape; - -struct b3ClothMesh; - -struct b3SpringClothDef -{ - b3SpringClothDef() - { - allocator = nullptr; - mesh = nullptr; - density = 0.0f; - ks = 0.0f; - kb = 0.0f; - kd = 0.0f; - r = 0.05f; - gravity.SetZero(); - } - - // Stack allocator - b3StackAllocator* allocator; - - // Cloth mesh - b3ClothMesh* mesh; - - // Cloth density in kg/m^3 - float32 density; - - // Streching stiffness - float32 ks; - - // Bending stiffness - float32 kb; - - // Damping stiffness - float32 kd; - - // Mass radius - // This should be a small value. It can be used for correcting visual artifacts when - // the masses are colliding against a solid. - float32 r; - - // Acceleration due to gravity (m/s^2) - b3Vec3 gravity; -}; - -enum b3SpringType -{ - e_strechSpring, - e_bendSpring, - // e_sewingSpring -}; - -struct b3Spring -{ - // Spring type - b3SpringType type; - - // Mass 1 - u32 i1; - - // Mass 2 - u32 i2; - - // Rest length - float32 L0; - - // Structural stiffness - float32 ks; - - // Damping stiffness - float32 kd; -}; - -// Static masses have zero mass and velocity, and therefore they can't move. -// Kinematic masses are't moved by external and internal forces but can be moved by contact forces. -// Dynamic masses have non-zero mass and can move due to internal and external forces. -enum class b3MassType : u32 -{ - e_staticMass, - e_kinematicMass, - e_dynamicMass -}; - -// -struct b3MassContact -{ - u32 j; - b3Vec3 n, t1, t2; - float32 Fn, Ft1, Ft2; - bool lockN, lockT1, lockT2; -}; - -// Time step statistics -struct b3SpringClothStep -{ - u32 iterations; -}; - -// A cloth it treats cloth as a collection of masses connected by springs. -// Large time steps can be taken. -// If accuracy and stability are required, not performance, -// you can use this class instead of using b3Cloth. -class b3SpringCloth -{ -public: - b3SpringCloth(); - ~b3SpringCloth(); - - // Initialize this cloth from a definition. - void Initialize(const b3SpringClothDef& def); - - // Return the cloth mesh used to initialize this cloth. - b3ClothMesh* GetMesh() const; - - // Set the gravitational acceleration applied to this cloth. - // Units are m/s^2. - void SetGravity(const b3Vec3& gravity); - - // Return the number of masses in this cloth. - u32 GetMassCount() const; - - // Return the gravitational acceleration applied to this cloth. - const b3Vec3& GetGravity() const; - - // Set the type of a given point mass. - void SetType(u32 i, b3MassType type); - - // Return the type of a given point mass. - b3MassType GetType(u32 i) const; - - // Set the position of a given point mass. - // This function will have effect on the position of the point mass - // after performing a time step. - void SetPosition(u32 i, const b3Vec3& position); - - // Return the position of a given point mass. - const b3Vec3& GetPosition(u32 i) const; - - // Set the velocity of a given point mass. - void SetVelocity(u32 i, const b3Vec3& velocity); - - // Return the velocity of a given point mass. - const b3Vec3& GetVelocity(u32 i) const; - - // Apply a force to a given point mass. - void ApplyForce(u32 i, const b3Vec3& force); - - // Return the kinetic (or dynamic) energy in this system. - float32 GetEnergy() const; - - // Return the tension forces (due to springs) acting at each point mass. - // Units are kg * m / s^2 - void GetTension(b3Array& tensions) const; - - // Add a shape to the list of shapes in this cloth. - // The cloth will be able to respond to collisions with each shape in the list of shapes. - void AddShape(b3Shape* shape); - - // Return the number of shapes added to this cloth. - u32 GetShapeCount() const; - - // Return the list of shapes added to this cloth. - b3Shape** GetShapes(); - - // Return the statistics of the last time step. - const b3SpringClothStep& GetStep() const; - - // Perform a time step (marches time forward). - void Step(float32 dt); - - // Set the positions of the mesh vertices to the positions of their associated point masses. - void Apply() const; - - // Debug draw the cloth mesh. - void Draw() const; -protected: - friend class b3SpringSolver; - - // Update contacts. - // This is where some contacts might be initiated or terminated. - void UpdateContacts(); - - b3StackAllocator* m_allocator; - - b3ClothMesh* m_mesh; - float32 m_r; - - b3Vec3 m_gravity; - - b3Vec3* m_x; - b3Vec3* m_v; - b3Vec3* m_f; - float32* m_m; - float32* m_inv_m; - b3Vec3* m_y; - b3Vec3* m_z; - b3Vec3* m_x0; - b3MassType* m_types; - u32 m_massCount; - - b3MassContact* m_contacts; - - b3Spring* m_springs; - u32 m_springCount; - - b3Shape* m_shapes[B3_CLOTH_SHAPE_CAPACITY]; - u32 m_shapeCount; - - b3SpringClothStep m_step; -}; - -inline b3ClothMesh* b3SpringCloth::GetMesh() const -{ - return m_mesh; -} - -inline const b3Vec3& b3SpringCloth::GetGravity() const -{ - return m_gravity; -} - -inline void b3SpringCloth::SetGravity(const b3Vec3& gravity) -{ - m_gravity = gravity; -} - -inline u32 b3SpringCloth::GetMassCount() const -{ - return m_massCount; -} - -inline b3MassType b3SpringCloth::GetType(u32 i) const -{ - B3_ASSERT(i < m_massCount); - return m_types[i]; -} - -inline void b3SpringCloth::SetType(u32 i, b3MassType type) -{ - B3_ASSERT(i < m_massCount); - - if (m_types[i] == type) - { - return; - } - - m_types[i] = type; - - m_f[i].SetZero(); - - if (type == b3MassType::e_staticMass) - { - m_v[i].SetZero(); - m_y[i].SetZero(); - - m_contacts[i].lockN = false; - } -} - -inline void b3SpringCloth::SetPosition(u32 i, const b3Vec3& position) -{ - B3_ASSERT(i < m_massCount); - m_y[i] += position - m_x[i]; -} - -inline const b3Vec3& b3SpringCloth::GetPosition(u32 i) const -{ - B3_ASSERT(i < m_massCount); - return m_x[i]; -} - -inline void b3SpringCloth::SetVelocity(u32 i, const b3Vec3& velocity) -{ - B3_ASSERT(i < m_massCount); - - if (m_types[i] == b3MassType::e_staticMass) - { - return; - } - - m_v[i] = velocity; -} - -inline const b3Vec3& b3SpringCloth::GetVelocity(u32 i) const -{ - B3_ASSERT(i < m_massCount); - return m_v[i]; -} - -inline void b3SpringCloth::ApplyForce(u32 i, const b3Vec3& force) -{ - B3_ASSERT(i < m_massCount); - - if (m_types[i] != b3MassType::e_dynamicMass) - { - return; - } - - m_f[i] += force; -} - -inline float32 b3SpringCloth::GetEnergy() const -{ - float32 E = 0.0f; - for (u32 i = 0; i < m_massCount; ++i) - { - E += m_m[i] * b3Dot(m_v[i], m_v[i]); - } - return 0.5f * E; -} - -inline u32 b3SpringCloth::GetShapeCount() const -{ - return m_shapeCount; -} - -inline b3Shape** b3SpringCloth::GetShapes() -{ - return m_shapes; -} - -inline const b3SpringClothStep& b3SpringCloth::GetStep() const -{ - return m_step; -} - -#endif \ No newline at end of file diff --git a/src/bounce/dynamics/cloth/cloth.cpp b/src/bounce/dynamics/cloth/cloth.cpp index 55bb6a1..d6a2e59 100644 --- a/src/bounce/dynamics/cloth/cloth.cpp +++ b/src/bounce/dynamics/cloth/cloth.cpp @@ -17,137 +17,116 @@ */ #include -#include -#include +#include +#include +#include +#include +#include +#include #include +#define B3_FORCE_THRESHOLD 0.005f + +#define B3_CLOTH_BENDING 0 + +#define B3_CLOTH_FRICTION 0 + b3Cloth::b3Cloth() { - m_pCount = 0; - m_ps = NULL; - m_c1Count = 0; - m_c1s = NULL; - m_c2Count = 0; - m_c2s = NULL; + m_gravity.SetZero(); + + m_particleCount = 0; + m_particles = nullptr; + + m_springs = nullptr; + m_springCount = 0; + + m_contacts = nullptr; + //m_contactCount = 0; + + m_shapeCount = 0; + + m_mesh = nullptr; - m_k1 = 0.0f; - m_k2 = 0.0f; - m_kd = 0.0f; - m_r = 0.0f; m_gravity.SetZero(); } b3Cloth::~b3Cloth() { - b3Free(m_ps); - b3Free(m_c1s); - b3Free(m_c2s); + b3Free(m_particles); + b3Free(m_springs); + b3Free(m_contacts); } -void b3Cloth::Initialize(const b3ClothDef& def) +static B3_FORCE_INLINE u32 b3NextIndex(u32 i) { - B3_ASSERT(def.mesh); - m_mesh = def.mesh; - - const b3Mesh* m = m_mesh; + return i + 1 < 3 ? i + 1 : 0; +} - m_pCount = m->vertexCount; - m_ps = (b3Particle*)b3Alloc(m_pCount * sizeof(b3Particle)); - - for (u32 i = 0; i < m->vertexCount; ++i) - { - b3Particle* p = m_ps + i; - p->im = 0.0f; - p->p = m->vertices[i]; - p->p0 = p->p; - p->v.SetZero(); - } +struct b3UniqueEdge +{ + u32 v1, v2; +}; - m_c1s = (b3C1*)b3Alloc(3 * m->triangleCount * sizeof(b3C1)); - m_c1Count = 0; +static u32 b3FindUniqueEdges(b3UniqueEdge* uniqueEdges, const b3ClothMesh* m) +{ + u32 uniqueCount = 0; for (u32 i = 0; i < m->triangleCount; ++i) { - b3Triangle* t = m->triangles + i; - - u32 is[3] = { t->v1, t->v2, t->v3 }; - for (u32 j = 0; j < 3; ++j) - { - u32 k = j + 1 < 3 ? j + 1 : 0; - - u32 v1 = is[j]; - u32 v2 = is[k]; - - b3Vec3 p1 = m->vertices[v1]; - b3Vec3 p2 = m->vertices[v2]; - - b3C1* C = m_c1s + m_c1Count; - C->i1 = v1; - C->i2 = v2; - C->L = b3Distance(p1, p2); - ++m_c1Count; - } - - b3Vec3 p1 = m->vertices[t->v1]; - b3Vec3 p2 = m->vertices[t->v2]; - b3Vec3 p3 = m->vertices[t->v3]; - - float32 area = b3Area(p1, p2, p3); - float32 mass = def.density * area; - - const float32 inv3 = 1.0f / 3.0f; - - m_ps[t->v1].im += inv3 * mass; - m_ps[t->v2].im += inv3 * mass; - m_ps[t->v3].im += inv3 * mass; - } - - for (u32 i = 0; i < m_pCount; ++i) - { - m_ps[i].im = m_ps[i].im > 0.0f ? 1.0f / m_ps[i].im : 0.0f; - } - - u32 c2Capacity = 0; - - for (u32 i = 0; i < m->triangleCount; ++i) - { - b3Triangle* t1 = m->triangles + i; + b3ClothMeshTriangle* t1 = m->triangles + i; u32 i1s[3] = { t1->v1, t1->v2, t1->v3 }; for (u32 j1 = 0; j1 < 3; ++j1) { - u32 k1 = j1 + 1 < 3 ? j1 + 1 : 0; - u32 t1v1 = i1s[j1]; - u32 t1v2 = i1s[k1]; + u32 t1v2 = i1s[b3NextIndex(j1)]; - for (u32 j = i + 1; j < m->triangleCount; ++j) + bool unique = true; + + for (u32 j = 0; j < uniqueCount; ++j) { - b3Triangle* t2 = m->triangles + j; - u32 i2s[3] = { t2->v1, t2->v2, t2->v3 }; + b3UniqueEdge* ue = uniqueEdges + j; - for (u32 j2 = 0; j2 < 3; ++j2) + if (ue->v1 == t1v1 && ue->v2 == t1v2) { - u32 k2 = j2 + 1 < 3 ? j2 + 1 : 0; - - u32 t2v1 = i2s[j2]; - u32 t2v2 = i2s[k2]; - - if (t1v1 == t2v2 && t1v2 == t2v1) - { - ++c2Capacity; - } + 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; } } } - m_c2Count = 0; - m_c2s = (b3C2*)b3Alloc(c2Capacity * sizeof(b3C2)); + return uniqueCount; +} + +struct b3SharedEdge +{ + u32 v1, v2; + u32 nsv1, nsv2; +}; + +static u32 b3FindSharedEdges(b3SharedEdge* sharedEdges, const b3ClothMesh* m) +{ + u32 sharedCount = 0; for (u32 i = 0; i < m->triangleCount; ++i) { - b3Triangle* t1 = m->triangles + i; + b3ClothMeshTriangle* t1 = m->triangles + i; u32 i1s[3] = { t1->v1, t1->v2, t1->v3 }; for (u32 j1 = 0; j1 < 3; ++j1) @@ -159,7 +138,7 @@ void b3Cloth::Initialize(const b3ClothDef& def) for (u32 j = i + 1; j < m->triangleCount; ++j) { - b3Triangle* t2 = m->triangles + j; + b3ClothMeshTriangle* t2 = m->triangles + j; u32 i2s[3] = { t2->v1, t2->v2, t2->v3 }; for (u32 j2 = 0; j2 < 3; ++j2) @@ -171,36 +150,21 @@ void b3Cloth::Initialize(const b3ClothDef& def) if (t1v1 == t2v2 && t1v2 == t2v1) { + // The triangles are adjacent. u32 k3 = k1 + 1 < 3 ? k1 + 1 : 0; u32 t1v3 = i1s[k3]; u32 k4 = k2 + 1 < 3 ? k2 + 1 : 0; u32 t2v3 = i2s[k4]; - b3Vec3 p1 = m->vertices[t1v1]; - b3Vec3 p2 = m->vertices[t1v2]; - b3Vec3 p3 = m->vertices[t1v3]; - b3Vec3 p4 = m->vertices[t2v3]; + // Add shared edge and non-shared vertices. + b3SharedEdge se; + se.v1 = t1v1; + se.v2 = t1v2; + se.nsv1 = t1v3; + se.nsv2 = t2v3; - b3Vec3 n1 = b3Cross(p2 - p1, p3 - p1); - n1.Normalize(); - - b3Vec3 n2 = b3Cross(p2 - p1, p4 - p1); - n2.Normalize(); - - float32 x = b3Dot(n1, n2); - - b3Vec3 n3 = b3Cross(n1, n2); - float32 y = b3Length(n3); - - b3C2* c = m_c2s + m_c2Count; - c->i1 = t1v1; - c->i2 = t1v2; - c->i3 = t1v3; - c->i4 = t2v3; - c->angle = atan2(y, x); - - ++m_c2Count; + sharedEdges[sharedCount++] = se; break; } @@ -209,188 +173,525 @@ void b3Cloth::Initialize(const b3ClothDef& def) } } - m_k1 = def.k1; - m_k2 = def.k2; - m_kd = def.kd; - m_r = def.r; - m_gravity = def.gravity; + return sharedCount; } -void b3Cloth::Step(float32 h, u32 iterations) +void b3Cloth::Initialize(const b3ClothDef& def) { - if (h == 0.0f) + B3_ASSERT(def.mesh); + B3_ASSERT(def.density > 0.0f); + + m_mesh = def.mesh; + m_density = def.density; + + const b3ClothMesh* m = m_mesh; + + m_particleCount = m->vertexCount; + m_particles = (b3Particle*)b3Alloc(m_particleCount * sizeof(b3Particle)); + m_contacts = (b3ParticleContact*)b3Alloc(m_particleCount * sizeof(b3ParticleContact)); + + for (u32 i = 0; i < m_particleCount; ++i) + { + b3Particle* p = m_particles + i; + p->type = e_dynamicParticle; + p->position = m->vertices[i]; + p->velocity.SetZero(); + p->force.SetZero(); + p->mass = 0.0f; + p->invMass = 0.0f; + p->radius = def.r; + p->userData = nullptr; + + p->translation.SetZero(); + p->x.SetZero(); + p->tension.SetZero(); + + b3ParticleContact* c = m_contacts + i; + c->n_active = false; + c->t1_active = false; + c->t2_active = false; + } + + // Compute mass + ResetMass(); + + // Initialize springs + u32 edgeCount = 3 * m->triangleCount; + + b3UniqueEdge* uniqueEdges = (b3UniqueEdge*)m_allocator.Allocate(edgeCount * sizeof(b3UniqueEdge)); + u32 uniqueCount = b3FindUniqueEdges(uniqueEdges, m); + + u32 springCapacity = uniqueCount; + +#if B3_CLOTH_BENDING + + b3SharedEdge* sharedEdges = (b3SharedEdge*)m_allocator.Allocate(edgeCount * sizeof(b3SharedEdge)); + u32 sharedCount = b3FindSharedEdges(sharedEdges, m); + + springCapacity += sharedCount; + +#endif + + springCapacity += m->sewingLineCount; + + m_springs = (b3Spring*)b3Alloc(springCapacity * sizeof(b3Spring)); + + // Tension + for (u32 i = 0; i < uniqueCount; ++i) + { + b3UniqueEdge* e = uniqueEdges + i; + + b3Vec3 v1 = m->vertices[e->v1]; + b3Vec3 v2 = m->vertices[e->v2]; + + b3Particle* p1 = m_particles + e->v1; + b3Particle* p2 = m_particles + e->v2; + + b3Spring* s = m_springs + m_springCount++; + s->type = e_strechSpring; + s->p1 = p1; + s->p2 = p2; + s->L0 = b3Distance(p1->position, p2->position); + s->ks = def.ks; + s->kd = def.kd; + } + +#if B3_CLOTH_BENDING + + // Bending + for (u32 i = 0; i < sharedCount; ++i) + { + b3SharedEdge* e = sharedEdges + i; + + b3Vec3 v1 = m->vertices[e->nsv1]; + b3Vec3 v2 = m->vertices[e->nsv2]; + + b3Particle* p1 = m_particles + e->nsv1; + b3Particle* p2 = m_particles + e->nsv2; + + b3Spring* s = m_springs + m_springCount++; + s->type = e_bendSpring; + s->p1 = p1; + s->p2 = p2; + s->L0 = b3Distance(p1->position, p2->position); + s->ks = def.kb; + s->kd = def.kd; + } + + m_allocator.Free(sharedEdges); +#endif + + m_allocator.Free(uniqueEdges); + + // Sewing + for (u32 i = 0; i < m->sewingLineCount; ++i) + { + b3ClothMeshSewingLine* line = m->sewingLines + i; + + b3Particle* p1 = m_particles + line->v1; + b3Particle* p2 = m_particles + line->v2; + + b3Spring* s = m_springs + m_springCount++; + s->type = e_strechSpring; + s->p1 = p1; + s->p2 = p2; + s->L0 = 0.0f; + s->ks = def.ks; + s->kd = def.kd; + } + + B3_ASSERT(m_springCount <= springCapacity); +} + +void b3Cloth::ResetMass() +{ + for (u32 i = 0; i < m_particleCount; ++i) + { + m_particles[i].mass = 0.0f; + m_particles[i].invMass = 0.0f; + } + + const float32 inv3 = 1.0f / 3.0f; + const float32 rho = m_density; + + // Accumulate the mass about the body origin of all triangles. + for (u32 i = 0; i < m_mesh->triangleCount; ++i) + { + b3ClothMeshTriangle* triangle = m_mesh->triangles + i; + u32 v1 = triangle->v1; + u32 v2 = triangle->v2; + u32 v3 = triangle->v3; + + b3Vec3 p1 = m_mesh->vertices[v1]; + b3Vec3 p2 = m_mesh->vertices[v2]; + b3Vec3 p3 = m_mesh->vertices[v3]; + + float32 area = b3Area(p1, p2, p3); + B3_ASSERT(area > B3_EPSILON); + + float32 mass = rho * area; + + m_particles[v1].mass += inv3 * mass; + m_particles[v2].mass += inv3 * mass; + m_particles[v3].mass += inv3 * mass; + } + + // Invert + for (u32 i = 0; i < m_particleCount; ++i) + { + B3_ASSERT(m_particles[i].mass > 0.0f); + m_particles[i].invMass = 1.0f / m_particles[i].mass; + } +} + +void b3Cloth::AddShape(b3Shape* shape) +{ + B3_ASSERT(m_shapeCount < B3_CLOTH_SHAPE_CAPACITY); + + if (m_shapeCount == B3_CLOTH_SHAPE_CAPACITY) { return; } - float32 d = exp(-h * m_kd); - - for (u32 i = 0; i < m_pCount; ++i) - { - b3Particle* p = m_ps + i; - - p->v += h * p->im * m_gravity; - p->v *= d; - - p->p0 = p->p; - p->p += h * p->v; - } - - for (u32 i = 0; i < iterations; ++i) - { - SolveC2(); - SolveC1(); - } - - float32 inv_h = 1.0f / h; - for (u32 i = 0; i < m_pCount; ++i) - { - b3Particle* p = m_ps + i; - p->v = inv_h * (p->p - p->p0); - } + m_shapes[m_shapeCount++] = shape; } -void b3Cloth::SolveC1() +void b3Cloth::UpdateContacts() { - for (u32 i = 0; i < m_c1Count; ++i) + B3_PROFILE("Update Contacts"); + + // Clear active flags + for (u32 i = 0; i < m_particleCount; ++i) { - b3C1* c = m_c1s + i; - - b3Particle* p1 = m_ps + c->i1; - b3Particle* p2 = m_ps + c->i2; + m_contacts[i].n_active = false; + m_contacts[i].t1_active = false; + m_contacts[i].t2_active = false; + } - float32 m1 = p1->im; - float32 m2 = p2->im; + // Create contacts + for (u32 i = 0; i < m_particleCount; ++i) + { + b3Particle* p = m_particles + i; - float32 mass = m1 + m2; - if (mass == 0.0f) + // Static particles can't participate in collisions. + if (p->type == e_staticParticle) { continue; } - mass = 1.0f / mass; + b3ParticleContact* c = m_contacts + i; - b3Vec3 J2 = p2->p - p1->p; - float32 L = b3Length(J2); - if (L > B3_EPSILON) + // Save the old contact + b3ParticleContact c0 = *c; + + b3Sphere s1; + s1.vertex = p->position; + s1.radius = p->radius; + + // Find the deepest penetration + float32 bestSeparation = 0.0f; + b3Vec3 bestNormal(0.0f, 0.0f, 0.0f); + u32 bestIndex = ~0; + + for (u32 j = 0; j < m_shapeCount; ++j) { - J2 /= L; + b3Shape* s2 = m_shapes[j]; + + b3Transform xf2; + xf2.SetIdentity(); + + b3TestSphereOutput output; + if (s2->TestSphere(&output, s1, xf2) == false) + { + continue; + } + + if (output.separation < bestSeparation) + { + bestSeparation = output.separation; + bestNormal = output.normal; + bestIndex = j; + } } - b3Vec3 J1 = -J2; - - float32 C = L - c->L; - float32 impulse = -m_k1 * mass * C; - - p1->p += (m1 * impulse) * J1; - p2->p += (m2 * impulse) * J2; - } -} - -void b3Cloth::SolveC2() -{ - for (u32 i = 0; i < m_c2Count; ++i) - { - b3C2* c = m_c2s + i; - - b3Particle* p1 = m_ps + c->i1; - b3Particle* p2 = m_ps + c->i2; - b3Particle* p3 = m_ps + c->i3; - b3Particle* p4 = m_ps + c->i4; - - float32 m1 = p1->im; - float32 m2 = p2->im; - float32 m3 = p3->im; - float32 m4 = p4->im; - - b3Vec3 v2 = p2->p - p1->p; - b3Vec3 v3 = p3->p - p1->p; - b3Vec3 v4 = p4->p - p1->p; - - b3Vec3 n1 = b3Cross(v2, v3); - n1.Normalize(); - - b3Vec3 n2 = b3Cross(v2, v4); - n2.Normalize(); - - float32 x = b3Dot(n1, n2); - - b3Vec3 J3 = b3Cross(v2, n2) + x * b3Cross(n1, v2); - float32 L3 = b3Length(b3Cross(v2, v3)); - if (L3 > B3_EPSILON) + if (bestIndex != ~0) { - J3 /= L3; - } - - b3Vec3 J4 = b3Cross(v2, n1) + x * b3Cross(n2, v2); - float32 L4 = b3Length(b3Cross(v2, v4)); - if (L4 > B3_EPSILON) - { - J4 /= L4; + B3_ASSERT(bestSeparation <= 0.0f); + + b3Shape* shape = m_shapes[bestIndex]; + float32 s = bestSeparation; + b3Vec3 n = bestNormal; + + // Update contact manifold + // Remember the normal orientation is from shape 2 to shape 1 (mass) + c->n_active = true; + c->p1 = p; + c->s2 = shape; + c->n = n; + c->t1 = b3Perp(n); + c->t2 = b3Cross(c->t1, n); + + // Apply position correction + p->translation -= s * n; } - b3Vec3 J2_1 = b3Cross(v3, n2) + x * b3Cross(n1, v3); - if (L3 > B3_EPSILON) + // Update contact state + if (c0.n_active == true && c->n_active == true) { - J2_1 /= L3; + // The contact persists + + // Has the contact constraint been satisfied? + if (c0.Fn <= -B3_FORCE_THRESHOLD) + { + // Contact force is attractive. + + // Terminate the contact. + c->n_active = false; + } } - b3Vec3 J2_2 = b3Cross(v4, n1) + x * b3Cross(n2, v4); - if (L4 > B3_EPSILON) +#if 0 + // Notify the new contact state + if (c0.n_active == false && c->n_active == true) { - J2_2 /= L4; + // The contact has begun } - - b3Vec3 J2 = -J2_1 - J2_2; - - b3Vec3 J1 = -J2 - J3 - J4; - float32 mass = m1 * b3Dot(J1, J1) + m2 * b3Dot(J2, J2) + m3 * b3Dot(J3, J3) + m4 * b3Dot(J4, J4); - if (mass == 0.0f) + if (c0.n_active == true && c->active_n == false) + { + // The contact has ended + } +#endif + if (c->n_active == false) { continue; } - mass = 1.0f / mass; - - b3Vec3 n3 = b3Cross(n1, n2); - float32 y = b3Length(n3); +#if B3_CLOTH_FRICTION == 1 - float32 angle = atan2(y, x); - float32 C = angle - c->angle; + // A friction force requires an associated normal force. + if (c0.n_active == false) + { + continue; + } - float32 impulse = -m_k2 * mass * y * C; + b3Shape* s = c->s; + b3Vec3 n = c->n; + float32 u = s->GetFriction(); + float32 normalForce = c0.Fn; - p1->p += (m1 * impulse) * J1; - p2->p += (m2 * impulse) * J2; - p3->p += (m3 * impulse) * J3; - p4->p += (m4 * impulse) * J4; + // Relative velocity + b3Vec3 dv = p->velocity; + + b3Vec3 t1 = dv - b3Dot(dv, n) * n; + if (b3Dot(t1, t1) > B3_EPSILON * B3_EPSILON) + { + // Create a dynamic basis + t1.Normalize(); + + b3Vec3 t2 = b3Cross(t1, n); + t2.Normalize(); + + c->t1 = t1; + c->t2 = t2; + } + else + { + c->t1_active = true; + c->t2_active = true; + continue; + } + + b3Vec3 ts[2]; + ts[0] = c->t1; + ts[1] = c->t2; + + bool t_active[2]; + t_active[0] = c->t1_active; + t_active[1] = c->t2_active; + + bool t_active0[2]; + t_active0[0] = c0.t1_active; + t_active0[1] = c0.t2_active; + + float32 Ft0[2]; + Ft0[0] = c0.Ft1; + Ft0[1] = c0.Ft2; + + for (u32 k = 0; k < 2; ++k) + { + b3Vec3 t = ts[k]; + + // Relative tangential velocity + float32 dvt = b3Dot(dv, t); + + if (dvt * dvt <= B3_EPSILON * B3_EPSILON) + { + // Lock particle on surface + t_active[k] = true; + } + + if (t_active0[k] == true && t_active[k] == true) + { + // The contact persists + float32 maxForce = u * normalForce; + + if (Ft0[k] * Ft0[k] > maxForce * maxForce) + { + // Unlock particle off surface + t_active[k] = false; + } + } + } + + c->t1_active = t_active[0]; + c->t2_active = t_active[1]; + +#endif + + } + +} + +void b3Cloth::Solve(float32 dt) +{ + B3_PROFILE("Solve"); + + // Clear tension + for (u32 i = 0; i < m_particleCount; ++i) + { + m_particles[i].tension.SetZero(); + } + + // Solve + b3ClothSolverDef solverDef; + solverDef.stack = &m_allocator; + solverDef.particleCapacity = m_particleCount; + solverDef.springCapacity = m_springCount; + solverDef.contactCapacity = m_particleCount; + + b3ClothSolver solver(solverDef); + + for (u32 i = 0; i < m_particleCount; ++i) + { + solver.Add(m_particles + i); + } + + for (u32 i = 0; i < m_springCount; ++i) + { + solver.Add(m_springs + i); + } + + for (u32 i = 0; i < m_particleCount; ++i) + { + if (m_contacts[i].n_active) + { + solver.Add(m_contacts + i); + } + } + + // Solve + solver.Solve(dt, m_gravity); + + // Clear external applied forces + for (u32 i = 0; i < m_particleCount; ++i) + { + m_particles[i].force.SetZero(); + } + + // Clear translations + for (u32 i = 0; i < m_particleCount; ++i) + { + m_particles[i].translation.SetZero(); + } +} + +void b3Cloth::Step(float32 dt) +{ + B3_PROFILE("Step"); + + // Update contacts + UpdateContacts(); + + // Solve constraints, integrate state, clear forces and translations. + if (dt > 0.0f) + { + Solve(dt); + } +} + +void b3Cloth::Apply() const +{ + for (u32 i = 0; i < m_particleCount; ++i) + { + m_mesh->vertices[i] = m_particles[i].position; } } void b3Cloth::Draw() const { - const b3Mesh* m = m_mesh; - + const b3ClothMesh* m = m_mesh; + for (u32 i = 0; i < m->vertexCount; ++i) { - b3Draw_draw->DrawPoint(m_ps[i].p, 6.0f, b3Color_green); + b3Particle* p = m_particles + i; + + if (p->type == e_staticParticle) + { + b3Draw_draw->DrawPoint(p->position, 4.0f, b3Color_white); + } + + if (p->type == e_kinematicParticle) + { + b3Draw_draw->DrawPoint(p->position, 4.0f, b3Color_blue); + } + + if (p->type == e_dynamicParticle) + { + b3Draw_draw->DrawPoint(p->position, 4.0f, b3Color_green); + } + + b3ParticleContact* c = m_contacts + i; + + if (c->n_active) + { + b3Draw_draw->DrawSegment(p->position, p->position + c->n, b3Color_yellow); + } + } + + for (u32 i = 0; i < m_springCount; ++i) + { + b3Spring* s = m_springs + i; + b3Particle* p1 = s->p1; + b3Particle* p2 = s->p2; + + if (s->type == e_strechSpring) + { + b3Draw_draw->DrawSegment(p1->position, p2->position, b3Color_black); + } + } + + for (u32 i = 0; i < m->sewingLineCount; ++i) + { + b3ClothMeshSewingLine* s = m->sewingLines + i; + b3Particle* p1 = m_particles + s->v1; + b3Particle* p2 = m_particles + s->v2; + + b3Draw_draw->DrawSegment(p1->position, p2->position, b3Color_white); } for (u32 i = 0; i < m->triangleCount; ++i) { - b3Triangle* t = m->triangles + i; + b3ClothMeshTriangle* t = m->triangles + i; - b3Particle* p1 = m_ps + t->v1; - b3Particle* p2 = m_ps + t->v2; - b3Particle* p3 = m_ps + t->v3; + b3Particle* p1 = m_particles + t->v1; + b3Particle* p2 = m_particles + t->v2; + b3Particle* p3 = m_particles + t->v3; - b3Vec3 v1 = p1->p; - b3Vec3 v2 = p2->p; - b3Vec3 v3 = p3->p; + b3Vec3 v1 = p1->position; + b3Vec3 v2 = p2->position; + b3Vec3 v3 = p3->position; - b3Draw_draw->DrawTriangle(v1, v2, v3, b3Color_black); - b3Vec3 n1 = b3Cross(v2 - v1, v3 - v1); n1.Normalize(); b3Draw_draw->DrawSolidTriangle(n1, v1, v2, v3, b3Color_blue); diff --git a/src/bounce/dynamics/cloth/cloth_solver.cpp b/src/bounce/dynamics/cloth/cloth_solver.cpp new file mode 100644 index 0000000..ef76da6 --- /dev/null +++ b/src/bounce/dynamics/cloth/cloth_solver.cpp @@ -0,0 +1,656 @@ +/* +* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* +* 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 + +// Here, we solve Ax = b using the Modified Preconditioned Conjugate Gradient (MPCG) algorithm. +// described in the paper: +// "Large Steps in Cloth Simulation - David Baraff, Andrew Witkin". + +// Some improvements for the original MPCG algorithm are described in the paper: +// "On the modified conjugate gradient method in cloth simulation - Uri M. Ascher, Eddy Boxerman". + +u32 b3_clothSolverIterations = 0; + +b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def) +{ + m_allocator = def.stack; + + m_particleCapacity = def.particleCapacity; + m_particleCount = 0; + m_particles = (b3Particle**)m_allocator->Allocate(m_particleCapacity * sizeof(b3Particle*)); + + m_springCapacity = def.springCapacity; + m_springCount = 0; + m_springs = (b3Spring**)m_allocator->Allocate(m_springCapacity * sizeof(b3Spring*));; + m_Jx = (b3Mat33*)m_allocator->Allocate(m_springCapacity * sizeof(b3Mat33)); + m_Jv = (b3Mat33*)m_allocator->Allocate(m_springCapacity * sizeof(b3Mat33)); + + m_contactCapacity = def.contactCapacity; + m_contactCount = 0; + m_contacts = (b3ParticleContact**)m_allocator->Allocate(m_contactCapacity * sizeof(b3ParticleContact*));; +} + +b3ClothSolver::~b3ClothSolver() +{ + m_allocator->Free(m_contacts); + m_allocator->Free(m_Jv); + m_allocator->Free(m_Jx); + m_allocator->Free(m_springs); + m_allocator->Free(m_particles); +} + +void b3ClothSolver::Add(b3Particle* p) +{ + p->solverId = m_particleCount; + m_particles[m_particleCount++] = p; +} + +void b3ClothSolver::Add(b3ParticleContact* c) +{ + m_contacts[m_contactCount++] = c; +} + +void b3ClothSolver::Add(b3Spring* s) +{ + m_springs[m_springCount++] = s; +} + +static B3_FORCE_INLINE b3SparseMat33 b3AllocSparse(b3StackAllocator* allocator, u32 M, u32 N) +{ + u32 size = M * N; + b3Mat33* elements = (b3Mat33*)allocator->Allocate(size * sizeof(b3Mat33)); + u32* cols = (u32*)allocator->Allocate(size * sizeof(u32)); + u32* row_ptrs = (u32*)allocator->Allocate((M + 1) * sizeof(u32)); + + return b3SparseMat33(M, N, size, elements, row_ptrs, cols); +} + +static B3_FORCE_INLINE void b3FreeSparse(b3SparseMat33& matrix, b3StackAllocator* allocator) +{ + allocator->Free(matrix.row_ptrs); + allocator->Free(matrix.cols); + allocator->Free(matrix.values); +} + +static B3_FORCE_INLINE void b3CopyPosition(b3DenseVec3& v, b3Particle** const particles, u32 count) +{ + for (u32 i = 0; i < count; ++i) + { + v[i] = particles[i]->position; + } +} + +static B3_FORCE_INLINE void b3CopyVelocity(b3DenseVec3& v, b3Particle** const particles, u32 count) +{ + for (u32 i = 0; i < count; ++i) + { + v[i] = particles[i]->velocity; + } +} + +static B3_FORCE_INLINE void b3CopyForce(b3DenseVec3& v, b3Particle** const particles, u32 count) +{ + for (u32 i = 0; i < count; ++i) + { + v[i] = particles[i]->force; + } +} + +static B3_FORCE_INLINE void b3CopyTranslation(b3DenseVec3& v, b3Particle** const particles, u32 count) +{ + for (u32 i = 0; i < count; ++i) + { + v[i] = particles[i]->translation; + } +} + +static B3_FORCE_INLINE void b3CopyGuess(b3DenseVec3& v, b3Particle** const particles, u32 count) +{ + for (u32 i = 0; i < count; ++i) + { + v[i] = particles[i]->x; + } +} + +void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) +{ + B3_PROFILE("Integrate"); + + m_h = dt; + + b3DenseVec3 sx(m_particleCount); + b3CopyPosition(sx, m_particles, m_particleCount); + + b3DenseVec3 sv(m_particleCount); + b3CopyVelocity(sv, m_particles, m_particleCount); + + b3DenseVec3 sf(m_particleCount); + b3CopyForce(sf, m_particles, m_particleCount); + + b3DenseVec3 sy(m_particleCount); + b3CopyTranslation(sy, m_particles, m_particleCount); + + Compute_f(sf, sx, sv, gravity); + + // Solve Ax = b, where + // A = M - h * dfdv - h * h * dfdx + // b = h * (f0 + h * dfdx * v0 + dfdx * y) + + // A + b3SparseMat33 A = b3AllocSparse(m_allocator, m_particleCount, m_particleCount); + + // b + b3DenseVec3 b(m_particleCount); + + Compute_A_b(A, b, sf, sx, sv, sy); + + // S + b3DiagMat33 S(m_particleCount); + Compute_S(S); + + // z + b3DenseVec3 z(m_particleCount); + Compute_z(z); + + // x0 + b3DenseVec3 x0(m_particleCount); + b3CopyGuess(x0, m_particles, m_particleCount); + + // x + b3DenseVec3 x(m_particleCount); + + // Solve Ax = b + u32 iterations = 0; + Solve(x, iterations, A, b, S, z, x0); + + b3_clothSolverIterations = iterations; + + // f = A * x - b + b3DenseVec3 f = A * x - b; + + // Update state + float32 h = m_h; + + for (u32 i = 0; i < m_particleCount; ++i) + { + b3Particle* p = m_particles[i]; + b3ParticleType type = p->type; + + b3Vec3 ix0 = sx[i]; + b3Vec3 iv0 = sv[i]; + b3Vec3 iy = sy[i]; + + b3Vec3 dv = x[i]; + + // v1 = v0 + dv + b3Vec3 v1 = iv0; + if (type == e_dynamicParticle) + { + v1 += dv; + } + + // dx = h * (v0 + dv) + y = h * v1 + y + b3Vec3 dx = h * v1 + iy; + + // x1 = x0 + dx + b3Vec3 x1 = ix0 + dx; + + sv[i] = v1; + sx[i] = x1; + } + + // Write x to the solution cache for improving convergence + for (u32 i = 0; i < m_particleCount; ++i) + { + m_particles[i]->x = x[i]; + } + + // Store the extra contact constraint forces that should have been + // supplied in order to enforce the contact constraints exactly + // These forces can be used in contact constraint logic + for (u32 i = 0; i < m_contactCount; ++i) + { + b3ParticleContact* c = m_contacts[i]; + b3Particle* p = c->p1; + + b3Vec3 force = f[p->solverId]; + + // Signed normal force magnitude + c->Fn = b3Dot(force, c->n); + + // Signed tangent force magnitude + c->Ft1 = b3Dot(force, c->t1); + c->Ft2 = b3Dot(force, c->t2); + } + + // Copy state buffers back to the particles + for (u32 i = 0; i < m_particleCount; ++i) + { + m_particles[i]->position = sx[i]; + m_particles[i]->velocity = sv[i]; + } + + b3FreeSparse(A, m_allocator); +} + +#define B3_INDEX(i, j, size) (i + j * size) + +// +static void b3SetZero(b3Vec3* out, u32 size) +{ + for (u32 i = 0; i < size; ++i) + { + out[i].SetZero(); + } +} + +// +static void b3SetZero(b3Mat33* out, u32 size) +{ + for (u32 i = 0; i < size; ++i) + { + out[i].SetZero(); + } +} + +// +static void b3Mul_Jacobian(b3Vec3* out, const b3Vec3* v, u32 massCount, + const b3Mat33* J_ii, b3Spring** const springs, u32 springCount) +{ + b3SetZero(out, massCount); + + for (u32 i = 0; i < springCount; ++i) + { + const b3Spring* S = springs[i]; + u32 i1 = S->p1->solverId; + u32 i2 = S->p2->solverId; + + b3Mat33 J_11 = J_ii[i]; + b3Mat33 J_12 = -J_11; + b3Mat33 J_21 = J_12; + b3Mat33 J_22 = J_11; + + out[i1] += J_11 * v[i1] + J_12 * v[i2]; + out[i2] += J_21 * v[i1] + J_22 * v[i2]; + } +} + +void b3ClothSolver::Compute_f(b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3Vec3& gravity) +{ + // Zero force + f.SetZero(); + + // Apply weight + for (u32 i = 0; i < m_particleCount; ++i) + { + b3Particle* p = m_particles[i]; + + if (p->type == e_dynamicParticle) + { + f[i] += p->mass * gravity; + } + } + + // Apply tension, damping + // Store the Jacobians + + // Zero Jacobians + b3SetZero(m_Jx, m_springCount); + b3SetZero(m_Jv, m_springCount); + + for (u32 i = 0; i < m_springCount; ++i) + { + b3Spring* S = m_springs[i]; + b3SpringType type = S->type; + b3Particle* p1 = S->p1; + b3Particle* p2 = S->p2; + float32 L0 = S->L0; + float32 ks = S->ks; + float32 kd = S->kd; + + u32 i1 = p1->solverId; + u32 i2 = p2->solverId; + + b3Vec3 x1 = x[i1]; + b3Vec3 v1 = v[i1]; + + b3Vec3 x2 = x[i2]; + b3Vec3 v2 = v[i2]; + + const b3Mat33 I = b3Mat33_identity; + + b3Vec3 dx = x1 - x2; + + if (b3Dot(dx, dx) >= L0 * L0) + { + // Tension + float32 L = b3Length(dx); + b3Vec3 n = dx / L; + + b3Vec3 sf1 = -ks * (L - L0) * n; + b3Vec3 sf2 = -sf1; + + f[i1] += sf1; + f[i2] += sf2; + + p1->tension += sf1; + p2->tension += sf2; + + b3Mat33 Jx11 = -ks * (b3Outer(dx, dx) + (1.0f - L0 / L) * (I - b3Outer(dx, dx))); + + m_Jx[i] = Jx11; + } + + // Damping + b3Vec3 dv = v1 - v2; + + b3Vec3 df1 = -kd * dv; + b3Vec3 df2 = -df1; + + f[i1] += df1; + f[i2] += df2; + + b3Mat33 Jv11 = -kd * I; + + m_Jv[i] = Jv11; + } +} + +static B3_FORCE_INLINE bool b3IsZero(const b3Mat33& A) +{ + bool isZeroX = b3Dot(A.x, A.x) == 0.0f; + bool isZeroY = b3Dot(A.y, A.y) == 0.0f; + bool isZeroZ = b3Dot(A.z, A.z) == 0.0f; + + return isZeroX * isZeroY * isZeroZ; +} + +void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const +{ + // Compute dfdx, dfdv + b3Mat33* dfdx = (b3Mat33*)m_allocator->Allocate(m_particleCount * m_particleCount * sizeof(b3Mat33)); + b3SetZero(dfdx, m_particleCount * m_particleCount); + + b3Mat33* dfdv = (b3Mat33*)m_allocator->Allocate(m_particleCount * m_particleCount * sizeof(b3Mat33)); + b3SetZero(dfdv, m_particleCount * m_particleCount); + + for (u32 i = 0; i < m_springCount; ++i) + { + const b3Spring* S = m_springs[i]; + u32 i1 = S->p1->solverId; + u32 i2 = S->p2->solverId; + + b3Mat33 Jx11 = m_Jx[i]; + b3Mat33 Jx12 = -Jx11; + b3Mat33 Jx21 = Jx12; + b3Mat33 Jx22 = Jx11; + + dfdx[B3_INDEX(i1, i1, m_particleCount)] += Jx11; + dfdx[B3_INDEX(i1, i2, m_particleCount)] += Jx12; + dfdx[B3_INDEX(i2, i1, m_particleCount)] += Jx21; + dfdx[B3_INDEX(i2, i2, m_particleCount)] += Jx22; + + b3Mat33 Jv11 = m_Jv[i]; + b3Mat33 Jv12 = -Jv11; + b3Mat33 Jv21 = Jv12; + b3Mat33 Jv22 = Jv11; + + dfdv[B3_INDEX(i1, i1, m_particleCount)] += Jv11; + dfdv[B3_INDEX(i1, i2, m_particleCount)] += Jv12; + dfdv[B3_INDEX(i2, i1, m_particleCount)] += Jv21; + dfdv[B3_INDEX(i2, i2, m_particleCount)] += Jv22; + } + + // Compute A + // A = M - h * dfdv - h * h * dfdx + + // A = 0 + b3Mat33* A = (b3Mat33*)m_allocator->Allocate(m_particleCount * m_particleCount * sizeof(b3Mat33)); + b3SetZero(A, m_particleCount * m_particleCount); + + // A += M + for (u32 i = 0; i < m_particleCount; ++i) + { + A[B3_INDEX(i, i, m_particleCount)] += b3Diagonal(m_particles[i]->mass); + } + + // A += - h * dfdv - h * h * dfdx + for (u32 i = 0; i < m_particleCount; ++i) + { + for (u32 j = 0; j < m_particleCount; ++j) + { + A[B3_INDEX(i, j, m_particleCount)] += (-m_h * dfdv[B3_INDEX(i, j, m_particleCount)]) + (-m_h * m_h * dfdx[B3_INDEX(i, j, m_particleCount)]); + } + } + + // Assembly sparsity + u32 nzCount = 0; + + SA.row_ptrs[0] = 0; + + for (u32 i = 0; i < m_particleCount; ++i) + { + u32 rowNzCount = 0; + + for (u32 j = 0; j < m_particleCount; ++j) + { + b3Mat33 a = A[B3_INDEX(i, j, m_particleCount)]; + + if (b3IsZero(a) == false) + { + B3_ASSERT(nzCount <= SA.valueCount); + + SA.values[nzCount] = a; + SA.cols[nzCount] = j; + + ++nzCount; + ++rowNzCount; + } + } + + SA.row_ptrs[i + 1] = SA.row_ptrs[(i + 1) - 1] + rowNzCount; + } + + B3_ASSERT(nzCount <= SA.valueCount); + SA.valueCount = nzCount; + + m_allocator->Free(A); + + m_allocator->Free(dfdv); + m_allocator->Free(dfdx); + + // Compute b + // b = h * (f0 + h * Jx_v + Jx_y) + + // Jx_v = dfdx * v + b3Vec3* Jx_v = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3)); + b3Mul_Jacobian(Jx_v, v.v, m_particleCount, m_Jx, m_springs, m_springCount); + + // Jx_y = dfdx * y + b3Vec3* Jx_y = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3)); + b3Mul_Jacobian(Jx_y, y.v, m_particleCount, m_Jx, m_springs, m_springCount); + + for (u32 i = 0; i < m_particleCount; ++i) + { + b[i] = m_h * (f[i] + m_h * Jx_v[i] + Jx_y[i]); + } + + m_allocator->Free(Jx_y); + m_allocator->Free(Jx_v); +} + +void b3ClothSolver::Compute_z(b3DenseVec3& z) +{ + // TODO + z.SetZero(); +} + +void b3ClothSolver::Compute_S(b3DiagMat33& out) +{ + for (u32 i = 0; i < m_particleCount; ++i) + { + b3Particle* p = m_particles[i]; + + if (p->type == e_staticParticle) + { + out[i].SetZero(); + continue; + } + + out[i].SetIdentity(); + } + + for (u32 i = 0; i < m_contactCount; ++i) + { + b3ParticleContact* c = m_contacts[i]; + + B3_ASSERT(c->n_active); + + b3Vec3 n = c->n; + + b3Mat33 S = b3Mat33_identity - b3Outer(n, n); + + if (c->t1_active == true && c->t2_active == true) + { + S.SetZero(); + } + else + { + if (c->t1_active == true) + { + b3Vec3 t1 = c->t1; + + S -= b3Outer(t1, t1); + } + + if (c->t2_active == true) + { + b3Vec3 t2 = c->t2; + + S -= b3Outer(t2, t2); + } + } + + b3Particle* p = c->p1; + out[p->solverId] = S; + } +} + +void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations, + const b3SparseMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const +{ + B3_PROFILE("Solve A * x = b"); + + // P = diag(A) + b3DiagMat33 inv_P(m_particleCount); + A.AssembleDiagonal(inv_P); + + // Invert + for (u32 i = 0; i < m_particleCount; ++i) + { + b3Mat33& D = inv_P[i]; + + // Sylvester Criterion to ensure PD-ness + B3_ASSERT(b3Det(D.x, D.y, D.z) > B3_EPSILON); + + B3_ASSERT(D.x.x != 0.0f); + float32 xx = 1.0f / D.x.x; + + B3_ASSERT(D.y.y != 0.0f); + float32 yy = 1.0f / D.y.y; + + B3_ASSERT(D.z.z != 0.0f); + float32 zz = 1.0f / D.z.z; + + D = b3Diagonal(xx, yy, zz); + } + + // I + b3DiagMat33 I(m_particleCount); + I.SetIdentity(); + + // x = S * y + (I - S) * z + x = (S * y) + (I - S) * z; + + // b^ = S * (b - A * (I - S) * z) + b3DenseVec3 b_hat = S * (b - A * ((I - S) * z)); + + // b_delta = dot(b^, P^-1 * b_^) + float32 b_delta = b3Dot(b_hat, inv_P * b_hat); + + // r = S * (b - A * x) + b3DenseVec3 r = S * (b - A * x); + + // p = S * (P^-1 * r) + b3DenseVec3 p = S * (inv_P * r); + + // deltaNew = dot(r, p) + float32 deltaNew = b3Dot(r, p); + + // Tolerance. + // This is the main stopping criteria. + // [0, 1] + const float32 tolerance = 10.0f * B3_EPSILON; + + // Maximum number of iterations. + const u32 maxIters = 1000; + + // Main iteration loop. + u32 iter = 0; + while (deltaNew > tolerance * tolerance * b_delta && iter < maxIters) + { + // s = S * (A * p) + b3DenseVec3 s = S * (A * p); + + // alpha = deltaNew / dot(c, q) + float32 alpha = deltaNew / b3Dot(p, s); + + // x = x + alpha * p + x = x + alpha * p; + + // r = r - alpha * s + r = r - alpha * s; + + // h = inv_P * r + b3DenseVec3 h = inv_P * r; + + // deltaOld = deltaNew + float32 deltaOld = deltaNew; + + // deltaNew = dot(r, h) + deltaNew = b3Dot(r, h); + //B3_ASSERT(b3IsValid(deltaNew)); + + // beta = deltaNew / deltaOld + float32 beta = deltaNew / deltaOld; + + // p = S * (h + beta * p) + p = S * (h + beta * p); + + ++iter; + } + + iterations = iter; +} \ No newline at end of file diff --git a/src/bounce/dynamics/cloth/spring_cloth.cpp b/src/bounce/dynamics/cloth/spring_cloth.cpp deleted file mode 100644 index dbd5f91..0000000 --- a/src/bounce/dynamics/cloth/spring_cloth.cpp +++ /dev/null @@ -1,765 +0,0 @@ -/* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net -* -* 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 - -#define B3_FORCE_THRESHOLD 0.005f - -#define B3_CLOTH_BENDING 0 - -#define B3_CLOTH_FRICTION 1 - -b3SpringCloth::b3SpringCloth() -{ - m_allocator = nullptr; - - m_mesh = nullptr; - - m_gravity.SetZero(); - - m_x = nullptr; - m_v = nullptr; - m_f = nullptr; - m_y = nullptr; - m_z = nullptr; - m_x0 = nullptr; - m_m = nullptr; - m_inv_m = nullptr; - m_types = nullptr; - m_contacts = nullptr; - m_massCount = 0; - - m_springs = nullptr; - m_springCount = 0; - - m_r = 0.0f; - - m_shapeCount = 0; - - m_step.iterations = 0; -} - -b3SpringCloth::~b3SpringCloth() -{ - b3Free(m_x); - b3Free(m_v); - b3Free(m_f); - b3Free(m_inv_m); - b3Free(m_m); - b3Free(m_y); - b3Free(m_z); - b3Free(m_x0); - b3Free(m_types); - b3Free(m_contacts); - b3Free(m_springs); -} - -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; - u32 nsv1, nsv2; -}; - -static u32 b3FindSharedEdges(b3SharedEdge* sharedEdges, const b3ClothMesh* m) -{ - u32 sharedCount = 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 k1 = j1 + 1 < 3 ? j1 + 1 : 0; - - u32 t1v1 = i1s[j1]; - u32 t1v2 = i1s[k1]; - - for (u32 j = i + 1; j < m->triangleCount; ++j) - { - b3ClothMeshTriangle* t2 = m->triangles + j; - u32 i2s[3] = { t2->v1, t2->v2, t2->v3 }; - - for (u32 j2 = 0; j2 < 3; ++j2) - { - u32 k2 = j2 + 1 < 3 ? j2 + 1 : 0; - - u32 t2v1 = i2s[j2]; - u32 t2v2 = i2s[k2]; - - if (t1v1 == t2v2 && t1v2 == t2v1) - { - // The triangles are adjacent. - u32 k3 = k1 + 1 < 3 ? k1 + 1 : 0; - u32 t1v3 = i1s[k3]; - - u32 k4 = k2 + 1 < 3 ? k2 + 1 : 0; - u32 t2v3 = i2s[k4]; - - // Add shared edge and non-shared vertices. - b3SharedEdge se; - se.v1 = t1v1; - se.v2 = t1v2; - se.nsv1 = t1v3; - se.nsv2 = t2v3; - - sharedEdges[sharedCount++] = se; - - break; - } - } - } - } - } - - return sharedCount; -} - -void b3SpringCloth::Initialize(const b3SpringClothDef& def) -{ - B3_ASSERT(def.allocator); - B3_ASSERT(def.mesh); - B3_ASSERT(def.density > 0.0f); - - m_allocator = def.allocator; - - m_mesh = def.mesh; - m_r = def.r; - - m_gravity = def.gravity; - - const b3ClothMesh* m = m_mesh; - - m_massCount = m->vertexCount; - m_x = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3)); - m_v = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3)); - m_f = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3)); - m_m = (float32*)b3Alloc(m_massCount * sizeof(float32)); - m_inv_m = (float32*)b3Alloc(m_massCount * sizeof(float32)); - m_y = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3)); - m_z = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3)); - m_x0 = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3)); - m_types = (b3MassType*)b3Alloc(m_massCount * sizeof(b3MassType)); - m_contacts = (b3MassContact*)b3Alloc(m_massCount * sizeof(b3MassContact)); - - for (u32 i = 0; i < m->vertexCount; ++i) - { - m_contacts[i].Fn = 0.0f; - m_contacts[i].Ft1 = 0.0f; - m_contacts[i].Ft2 = 0.0f; - m_contacts[i].lockN = false; - m_contacts[i].lockT1 = false; - m_contacts[i].lockT2 = false; - - m_x[i] = m->vertices[i]; - m_v[i].SetZero(); - m_f[i].SetZero(); - m_m[i] = 0.0f; - m_inv_m[i] = 0.0f; - m_y[i].SetZero(); - m_z[i].SetZero(); - m_x0[i].SetZero(); - m_types[i] = b3MassType::e_staticMass; - } - - // Initialize mass - for (u32 i = 0; i < m->triangleCount; ++i) - { - b3ClothMeshTriangle* t = m->triangles + i; - - b3Vec3 p1 = m->vertices[t->v1]; - b3Vec3 p2 = m->vertices[t->v2]; - b3Vec3 p3 = m->vertices[t->v3]; - - float32 area = b3Area(p1, p2, p3); - - B3_ASSERT(area > B3_EPSILON); - - float32 mass = def.density * area; - - const float32 inv3 = 1.0f / 3.0f; - - m_m[t->v1] += inv3 * mass; - m_m[t->v2] += inv3 * mass; - m_m[t->v3] += inv3 * mass; - } - - // Invert - for (u32 i = 0; i < m_massCount; ++i) - { - B3_ASSERT(m_m[i] > 0.0f); - m_inv_m[i] = 1.0f / m_m[i]; - m_types[i] = b3MassType::e_dynamicMass; - } - - // Initialize springs - u32 edgeCount = 3 * m->triangleCount; - - b3UniqueEdge* uniqueEdges = (b3UniqueEdge*)m_allocator->Allocate(edgeCount * sizeof(b3UniqueEdge)); - u32 uniqueCount = b3FindUniqueEdges(uniqueEdges, m); - - u32 springCapacity = uniqueCount; - -#if B3_CLOTH_BENDING - - b3SharedEdge* sharedEdges = (b3SharedEdge*)m_allocator->Allocate(edgeCount * sizeof(b3SharedEdge)); - u32 sharedCount = b3FindSharedEdges(sharedEdges, m); - - springCapacity += sharedCount; - -#endif - - springCapacity += m->sewingLineCount; - - m_springs = (b3Spring*)b3Alloc(springCapacity * sizeof(b3Spring)); - - // Tension - for (u32 i = 0; i < uniqueCount; ++i) - { - b3UniqueEdge* e = uniqueEdges + i; - - b3Vec3 p1 = m->vertices[e->v1]; - b3Vec3 p2 = m->vertices[e->v2]; - - b3Spring* S = m_springs + m_springCount; - S->type = e_strechSpring; - S->i1 = e->v1; - S->i2 = e->v2; - S->L0 = b3Distance(p1, p2); - - B3_ASSERT(S->L0 > B3_EPSILON); - - S->ks = def.ks; - S->kd = def.kd; - ++m_springCount; - } - -#if B3_CLOTH_BENDING - - // Bending - for (u32 i = 0; i < sharedCount; ++i) - { - b3SharedEdge* e = sharedEdges + i; - - b3Vec3 p1 = m->vertices[e->nsv1]; - b3Vec3 p2 = m->vertices[e->nsv2]; - - b3Spring* S = m_springs + m_springCount; - S->type = e_bendSpring; - S->i1 = e->nsv1; - S->i2 = e->nsv2; - S->L0 = b3Distance(p1, p2); - S->ks = def.kb; - S->kd = def.kd; - ++m_springCount; - } - - m_allocator->Free(sharedEdges); -#endif - - m_allocator->Free(uniqueEdges); - - // Sewing - for (u32 i = 0; i < m->sewingLineCount; ++i) - { - b3ClothMeshSewingLine* line = m->sewingLines + i; - - b3Spring* S = m_springs + m_springCount; - S->type = e_strechSpring; - S->i1 = line->v1; - S->i2 = line->v2; - S->L0 = 0.0f; - S->ks = def.ks; - S->kd = def.kd; - ++m_springCount; - } - - B3_ASSERT(m_springCount <= springCapacity); -} - -void b3SpringCloth::AddShape(b3Shape* shape) -{ - B3_ASSERT(m_shapeCount < B3_CLOTH_SHAPE_CAPACITY); - - if (m_shapeCount == B3_CLOTH_SHAPE_CAPACITY) - { - return; - } - - m_shapes[m_shapeCount++] = shape; -} - -void b3SpringCloth::GetTension(b3Array& T) const -{ - B3_ASSERT(T.Count() == 0); - - T.Resize(m_massCount); - - for (u32 i = 0; i < T.Count(); ++i) - { - T[i].SetZero(); - } - - // T = F - mg - for (u32 i = 0; i < m_springCount; ++i) - { - b3Spring* S = m_springs + i; - - b3SpringType type = S->type; - u32 i1 = S->i1; - u32 i2 = S->i2; - float32 L0 = S->L0; - float32 ks = S->ks; - float32 kd = S->kd; - - b3Vec3 x1 = m_x[i1]; - b3Vec3 v1 = m_v[i1]; - - b3Vec3 x2 = m_x[i2]; - b3Vec3 v2 = m_v[i2]; - - // Streching - b3Vec3 dx = x1 - x2; - float32 L = b3Length(dx); - - if (L >= L0) - { - // Force is tension. - b3Vec3 n = dx / L; - - b3Vec3 sf1 = -ks * (L - L0) * n; - b3Vec3 sf2 = -sf1; - - T[i1] += sf1; - T[i2] += sf2; - } - - // Damping - b3Vec3 dv = v1 - v2; - - b3Vec3 df1 = -kd * dv; - b3Vec3 df2 = -df1; - - T[i1] += df1; - T[i2] += df2; - } - - for (u32 i = 0; i < T.Count(); ++i) - { - if (m_types[i] != b3MassType::e_dynamicMass) - { - T[i].SetZero(); - } - } -} - -void b3SpringCloth::UpdateContacts() -{ - for (u32 i = 0; i < m_massCount; ++i) - { - // Static masses can't participate in collisions. - if (m_types[i] == b3MassType::e_staticMass) - { - continue; - } - - b3MassContact* c = m_contacts + i; - - // Save the old contact - b3MassContact c0 = *c; - - // Create a new contact - c->lockN = false; - c->lockT1 = false; - c->lockT2 = false; - - b3Sphere s1; - s1.vertex = m_x[i]; - s1.radius = m_r; - - // Find the deepest penetration - float32 bestSeparation = 0.0f; - b3Vec3 bestNormal(0.0f, 0.0f, 0.0f); - u32 bestIndex = ~0; - - for (u32 j = 0; j < m_shapeCount; ++j) - { - b3Shape* s2 = m_shapes[j]; - - b3Transform xf2; - xf2.SetIdentity(); - - b3TestSphereOutput output; - if (s2->TestSphere(&output, s1, xf2) == false) - { - continue; - } - - if (output.separation < bestSeparation) - { - bestSeparation = output.separation; - bestNormal = output.normal; - bestIndex = j; - } - } - - if (bestIndex != ~0) - { - B3_ASSERT(bestSeparation <= 0.0f); - - b3Shape* shape = m_shapes[bestIndex]; - float32 s = bestSeparation; - b3Vec3 n = bestNormal; - - // Update contact manifold - // Remember the normal orientation is from shape 2 to shape 1 (mass) - c->j = bestIndex; - c->n = n; - c->t1 = b3Perp(n); - c->t2 = b3Cross(c->t1, n); - c->lockN = true; - - // Apply position correction - m_y[i] -= s * n; - } - - // Update contact state - if (c0.lockN == true && c->lockN == true) - { - // The contact persists - - // Has the contact constraint been satisfied? - if (c0.Fn <= -B3_FORCE_THRESHOLD) - { - // Contact force is attractive. - - // Terminate the contact. - c->lockN = false; - } - } - -#if 0 - // Notify the new contact state - if (wasLockedN == false && c->lockN == true) - { - // The contact has begun - } - - if (wasLockedN == true && c->lockN == false) - { - // The contact has ended - } -#endif - if (c->lockN == false) - { - continue; - } - -#if B3_CLOTH_FRICTION == 1 - - // A friction force requires an associated normal force. - if (c0.lockN == false) - { - continue; - } - - b3Shape* s = m_shapes[c->j]; - b3Vec3 n = c->n; - float32 u = s->GetFriction(); - float32 normalForce = c0.Fn; - - // Relative velocity - b3Vec3 dv = m_v[i]; - - b3Vec3 t1 = dv - b3Dot(dv, n) * n; - if (b3Dot(t1, t1) > B3_EPSILON * B3_EPSILON) - { - // Create a dynamic basis - t1.Normalize(); - - b3Vec3 t2 = b3Cross(t1, n); - t2.Normalize(); - - c->t1 = t1; - c->t2 = t2; - } - else - { - c->lockT1 = true; - c->lockT2 = true; - continue; - } - - b3Vec3 ts[2]; - ts[0] = c->t1; - ts[1] = c->t2; - - bool lockT[2]; - lockT[0] = c->lockT1; - lockT[1] = c->lockT2; - - bool lockT0[2]; - lockT0[0] = c0.lockT1; - lockT0[1] = c0.lockT2; - - float32 Ft0[2]; - Ft0[0] = c0.Ft1; - Ft0[1] = c0.Ft2; - - for (u32 k = 0; k < 2; ++k) - { - b3Vec3 t = ts[k]; - - // Relative tangential velocity - float32 dvt = b3Dot(dv, t); - - if (dvt * dvt <= B3_EPSILON * B3_EPSILON) - { - // Lock mass on surface - lockT[k] = true; - } - - if (lockT0[k] == true && lockT[k] == true) - { - // The contact persists - float32 maxForce = u * normalForce; - - if (Ft0[k] * Ft0[k] > maxForce * maxForce) - { - // Unlock mass off surface - lockT[k] = false; - } - } - } - - c->lockT1 = lockT[0]; - c->lockT2 = lockT[1]; - -#endif - - } - -} - -void b3SpringCloth::Step(float32 dt) -{ - if (dt == 0.0f) - { - return; - } - - // Update contacts - UpdateContacts(); - - // Apply weights - for (u32 i = 0; i < m_massCount; ++i) - { - if (m_types[i] == b3MassType::e_dynamicMass) - { - m_f[i] += m_m[i] * m_gravity; - } - } - - // Integrate - b3SpringSolverDef solverDef; - solverDef.cloth = this; - solverDef.dt = dt; - - b3SpringSolver solver(solverDef); - - // Extra constraint forces that should have been applied to satisfy the constraints - // todo Find the applied constraint forces. - b3DenseVec3 forces(m_massCount); - - solver.Solve(forces); - - m_step.iterations = solver.GetIterations(); - - // Store constraint forces for physics logic - for (u32 i = 0; i < m_massCount; ++i) - { - b3MassContact* c = m_contacts + i; - - if (c->lockN == false) - { - continue; - } - - b3Vec3 force = forces[i]; - - // Signed normal force magnitude - c->Fn = b3Dot(force, c->n); - - // Signed tangent force magnitude - c->Ft1 = b3Dot(force, c->t1); - c->Ft2 = b3Dot(force, c->t2); - } - - // Clear position alteration - for (u32 i = 0; i < m_massCount; ++i) - { - m_y[i].SetZero(); - } - - // Clear forces - for (u32 i = 0; i < m_massCount; ++i) - { - m_f[i].SetZero(); - } -} - -void b3SpringCloth::Apply() const -{ - for (u32 i = 0; i < m_massCount; ++i) - { - m_mesh->vertices[i] = m_x[i]; - } -} - -void b3SpringCloth::Draw() const -{ - const b3ClothMesh* m = m_mesh; - - for (u32 i = 0; i < m->vertexCount; ++i) - { - if (m_types[i] == b3MassType::e_staticMass) - { - b3Draw_draw->DrawPoint(m_x[i], 4.0f, b3Color_white); - } - - if (m_types[i] == b3MassType::e_kinematicMass) - { - b3Draw_draw->DrawPoint(m_x[i], 4.0f, b3Color_blue); - } - - if (m_types[i] == b3MassType::e_dynamicMass) - { - b3Draw_draw->DrawPoint(m_x[i], 4.0f, b3Color_green); - } - - b3MassContact* c = m_contacts + i; - - if (c->lockN) - { - b3Draw_draw->DrawSegment(m_x[i], m_x[i] + c->n, b3Color_yellow); - } - } - - for (u32 i = 0; i < m_springCount; ++i) - { - b3Spring* s = m_springs + i; - - b3Vec3 x1 = m_x[s->i1]; - b3Vec3 x2 = m_x[s->i2]; - - if (s->type == e_strechSpring) - { - b3Draw_draw->DrawSegment(x1, x2, b3Color_black); - } - } - - for (u32 i = 0; i < m->sewingLineCount; ++i) - { - b3ClothMeshSewingLine* s = m->sewingLines + i; - - b3Vec3 x1 = m_x[s->v1]; - b3Vec3 x2 = m_x[s->v2]; - - b3Draw_draw->DrawSegment(x1, x2, b3Color_white); - } - - for (u32 i = 0; i < m->triangleCount; ++i) - { - b3ClothMeshTriangle* t = m->triangles + i; - - b3Vec3 v1 = m_x[t->v1]; - b3Vec3 v2 = m_x[t->v2]; - b3Vec3 v3 = m_x[t->v3]; - - b3Vec3 n1 = b3Cross(v2 - v1, v3 - v1); - n1.Normalize(); - b3Draw_draw->DrawSolidTriangle(n1, v1, v2, v3, b3Color_blue); - - b3Vec3 n2 = -n1; - b3Draw_draw->DrawSolidTriangle(n2, v1, v3, v2, b3Color_blue); - } -} \ No newline at end of file diff --git a/src/bounce/dynamics/cloth/spring_solver.cpp b/src/bounce/dynamics/cloth/spring_solver.cpp deleted file mode 100644 index 8316f6a..0000000 --- a/src/bounce/dynamics/cloth/spring_solver.cpp +++ /dev/null @@ -1,550 +0,0 @@ -/* -* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net -* -* 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 - -// Here, we solve Ax = b using the Modified Preconditioned Conjugate Gradient (MPCG) algorithm. -// described in the paper: -// "Large Steps in Cloth Simulation - David Baraff, Andrew Witkin". - -// Some improvements for the original MPCG algorithm are described in the paper: -// "On the modified conjugate gradient method in cloth simulation - Uri M. Ascher, Eddy Boxerman". - -b3SpringSolver::b3SpringSolver(const b3SpringSolverDef& def) -{ - m_cloth = def.cloth; - m_h = def.dt; - m_iterations = 0; - m_Jx = nullptr; - m_Jv = nullptr; - - m_allocator = m_cloth->m_allocator; - - m_x = m_cloth->m_x; - m_v = m_cloth->m_v; - m_f = m_cloth->m_f; - m_m = m_cloth->m_m; - m_inv_m = m_cloth->m_inv_m; - m_y = m_cloth->m_y; - m_z = m_cloth->m_y; - m_x0 = m_cloth->m_x0; - m_types = m_cloth->m_types; - m_massCount = m_cloth->m_massCount; - - m_contacts = m_cloth->m_contacts; - - m_springs = m_cloth->m_springs; - m_springCount = m_cloth->m_springCount; - - m_shapes = m_cloth->m_shapes; -} - -b3SpringSolver::~b3SpringSolver() -{ - -} - -static B3_FORCE_INLINE b3SparseMat33 b3AllocSparse(b3StackAllocator* allocator, u32 M, u32 N) -{ - u32 size = M * N; - b3Mat33* elements = (b3Mat33*)allocator->Allocate(size * sizeof(b3Mat33)); - u32* cols = (u32*)allocator->Allocate(size * sizeof(u32)); - u32* row_ptrs = (u32*)allocator->Allocate((M + 1) * sizeof(u32)); - - return b3SparseMat33(M, N, size, elements, row_ptrs, cols); -} - -static B3_FORCE_INLINE void b3FreeSparse(b3SparseMat33& matrix, b3StackAllocator* allocator) -{ - allocator->Free(matrix.row_ptrs); - allocator->Free(matrix.cols); - allocator->Free(matrix.values); -} - -void b3SpringSolver::Solve(b3DenseVec3& f) -{ - B3_PROFILE("Solve"); - - // - m_Jx = (b3Mat33*)m_allocator->Allocate(m_springCount * sizeof(b3Mat33)); - m_Jv = (b3Mat33*)m_allocator->Allocate(m_springCount * sizeof(b3Mat33)); - - // Apply internal forces. Also, store their unique derivatives. - ApplySpringForces(); - - // Integrate - - // Solve Ax = b, where - // A = M - h * dfdv - h * h * dfdx - // b = h * (f0 + h * dfdx * v0 + dfdx * y) - - { - // A - b3SparseMat33 A = b3AllocSparse(m_allocator, m_massCount, m_massCount); - - // b - b3DenseVec3 b(m_massCount); - - Compute_A_b(A, b); - - // S - b3DiagMat33 S(m_massCount); - Compute_S(S); - - // z - b3DenseVec3 z(m_massCount); - Compute_z(z); - - // x0 - b3DenseVec3 x0(m_massCount); - for (u32 i = 0; i < m_massCount; ++i) - { - x0[i] = m_x0[i]; - } - - // x - b3DenseVec3 x(m_massCount); - - // Solve Ax = b - Solve(x, f, m_iterations, A, b, S, z, x0); - - // Store x0 - for (u32 i = 0; i < m_massCount; ++i) - { - m_x0[i] = x[i]; - } - - // Update state - for (u32 i = 0; i < m_massCount; ++i) - { - if (m_types[i] == b3MassType::e_dynamicMass) - { - m_v[i] += x[i]; - } - - // dx = h * (v0 + dv) + y = h * v1 + y - m_x[i] += m_h * m_v[i] + m_y[i]; - } - - b3FreeSparse(A, m_allocator); - } - - m_allocator->Free(m_Jv); - m_allocator->Free(m_Jx); - m_Jv = nullptr; - m_Jx = nullptr; -} - -#define B3_INDEX(i, j, size) (i + j * size) - -// -static void b3SetZero(b3Vec3* out, u32 size) -{ - for (u32 i = 0; i < size; ++i) - { - out[i].SetZero(); - } -} - -// -static void b3SetZero(b3Mat33* out, u32 size) -{ - for (u32 i = 0; i < size; ++i) - { - out[i].SetZero(); - } -} - -// -static void b3Mul_Jacobian(b3Vec3* out, const b3Vec3* v, u32 massCount, - const b3Mat33* J_ii, const b3Spring* springs, u32 springCount) -{ - b3SetZero(out, massCount); - - for (u32 i = 0; i < springCount; ++i) - { - const b3Spring* S = springs + i; - u32 i1 = S->i1; - u32 i2 = S->i2; - - b3Mat33 J_11 = J_ii[i]; - b3Mat33 J_12 = -J_11; - b3Mat33 J_21 = J_12; - b3Mat33 J_22 = J_11; - - out[i1] += J_11 * v[i1] + J_12 * v[i2]; - out[i2] += J_21 * v[i1] + J_22 * v[i2]; - } -} - -void b3SpringSolver::ApplySpringForces() -{ - // Zero Jacobians - b3SetZero(m_Jx, m_springCount); - b3SetZero(m_Jv, m_springCount); - - // Compute spring forces and Jacobians - for (u32 i = 0; i < m_springCount; ++i) - { - b3Spring* S = m_springs + i; - - b3SpringType type = S->type; - u32 i1 = S->i1; - u32 i2 = S->i2; - float32 L0 = S->L0; - B3_ASSERT(L0 > 0.0f); - float32 ks = S->ks; - float32 kd = S->kd; - - b3Vec3 x1 = m_x[i1]; - b3Vec3 v1 = m_v[i1]; - - b3Vec3 x2 = m_x[i2]; - b3Vec3 v2 = m_v[i2]; - - const b3Mat33 I = b3Mat33_identity; - - // Strech - b3Vec3 dx = x1 - x2; - float32 L = b3Length(dx); - - if (L >= L0) - { - // Force is tension. - b3Vec3 n = dx / L; - - b3Vec3 sf1 = -ks * (L - L0) * n; - b3Vec3 sf2 = -sf1; - - m_f[i1] += sf1; - m_f[i2] += sf2; - - b3Mat33 Jx11 = -ks * (b3Outer(dx, dx) + (1.0f - L0 / L) * (I - b3Outer(dx, dx))); - - m_Jx[i] = Jx11; - } - - // Damping - b3Vec3 dv = v1 - v2; - - b3Vec3 df1 = -kd * dv; - b3Vec3 df2 = -df1; - - m_f[i1] += df1; - m_f[i2] += df2; - - b3Mat33 Jv11 = -kd * I; - - m_Jv[i] = Jv11; - } -} - -static B3_FORCE_INLINE bool b3IsZero(const b3Mat33& A) -{ - bool isZeroX = b3Dot(A.x, A.x) == 0.0f; - bool isZeroY = b3Dot(A.y, A.y) == 0.0f; - bool isZeroZ = b3Dot(A.z, A.z) == 0.0f; - - return isZeroX * isZeroY * isZeroZ; -} - -void b3SpringSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b) const -{ - // Compute dfdx, dfdv - b3Mat33* dfdx = (b3Mat33*)m_allocator->Allocate(m_massCount * m_massCount * sizeof(b3Mat33)); - b3SetZero(dfdx, m_massCount * m_massCount); - - b3Mat33* dfdv = (b3Mat33*)m_allocator->Allocate(m_massCount * m_massCount * sizeof(b3Mat33)); - b3SetZero(dfdv, m_massCount * m_massCount); - - for (u32 i = 0; i < m_springCount; ++i) - { - const b3Spring* S = m_springs + i; - u32 i1 = S->i1; - u32 i2 = S->i2; - - b3Mat33 Jx11 = m_Jx[i]; - b3Mat33 Jx12 = -Jx11; - b3Mat33 Jx21 = Jx12; - b3Mat33 Jx22 = Jx11; - - dfdx[B3_INDEX(i1, i1, m_massCount)] += Jx11; - dfdx[B3_INDEX(i1, i2, m_massCount)] += Jx12; - dfdx[B3_INDEX(i2, i1, m_massCount)] += Jx21; - dfdx[B3_INDEX(i2, i2, m_massCount)] += Jx22; - - b3Mat33 Jv11 = m_Jv[i]; - b3Mat33 Jv12 = -Jv11; - b3Mat33 Jv21 = Jv12; - b3Mat33 Jv22 = Jv11; - - dfdv[B3_INDEX(i1, i1, m_massCount)] += Jv11; - dfdv[B3_INDEX(i1, i2, m_massCount)] += Jv12; - dfdv[B3_INDEX(i2, i1, m_massCount)] += Jv21; - dfdv[B3_INDEX(i2, i2, m_massCount)] += Jv22; - } - - // Compute A - // A = M - h * dfdv - h * h * dfdx - - // A = 0 - b3Mat33* A = (b3Mat33*)m_allocator->Allocate(m_massCount * m_massCount * sizeof(b3Mat33)); - b3SetZero(A, m_massCount * m_massCount); - - // A += M - for (u32 i = 0; i < m_massCount; ++i) - { - A[B3_INDEX(i, i, m_massCount)] += b3Diagonal(m_m[i]); - } - - // A += - h * dfdv - h * h * dfdx - for (u32 i = 0; i < m_massCount; ++i) - { - for (u32 j = 0; j < m_massCount; ++j) - { - A[B3_INDEX(i, j, m_massCount)] += (-m_h * dfdv[B3_INDEX(i, j, m_massCount)]) + (-m_h * m_h * dfdx[B3_INDEX(i, j, m_massCount)]); - } - } - - // Assembly sparsity - u32 nzCount = 0; - - SA.row_ptrs[0] = 0; - - for (u32 i = 0; i < m_massCount; ++i) - { - u32 rowNzCount = 0; - - for (u32 j = 0; j < m_massCount; ++j) - { - b3Mat33 a = A[B3_INDEX(i, j, m_massCount)]; - - if (b3IsZero(a) == false) - { - B3_ASSERT(nzCount <= SA.valueCount); - - SA.values[nzCount] = a; - SA.cols[nzCount] = j; - - ++nzCount; - ++rowNzCount; - } - } - - SA.row_ptrs[i + 1] = SA.row_ptrs[(i + 1) - 1] + rowNzCount; - } - - B3_ASSERT(nzCount <= SA.valueCount); - SA.valueCount = nzCount; - - m_allocator->Free(A); - - // Compute b - // b = h * (f0 + h * Jx_v + Jx_y) - - // Jx_v = dfdx * v - b3Vec3* Jx_v = (b3Vec3*)m_allocator->Allocate(m_massCount * sizeof(b3Vec3)); - b3Mul_Jacobian(Jx_v, m_v, m_massCount, m_Jx, m_springs, m_springCount); - - // Jx_y = dfdx * y - b3Vec3* Jx_y = (b3Vec3*)m_allocator->Allocate(m_massCount * sizeof(b3Vec3)); - b3Mul_Jacobian(Jx_y, m_y, m_massCount, m_Jx, m_springs, m_springCount); - - for (u32 i = 0; i < m_massCount; ++i) - { - b[i] = m_h * (m_f[i] + m_h * Jx_v[i] + Jx_y[i]); - } - - m_allocator->Free(Jx_y); - m_allocator->Free(Jx_v); - - m_allocator->Free(dfdv); - m_allocator->Free(dfdx); -} - -void b3SpringSolver::Compute_z(b3DenseVec3& z) -{ - for (u32 i = 0; i < m_massCount; ++i) - { - z[i] = m_z[i]; - } -} - -void b3SpringSolver::Compute_S(b3DiagMat33& out) -{ - for (u32 i = 0; i < m_massCount; ++i) - { - out[i].SetIdentity(); - - if (m_types[i] == b3MassType::e_staticMass) - { - out[i].SetZero(); - continue; - } - - b3MassContact* c = m_contacts + i; - - if (c->lockN == true) - { - b3Vec3 n = c->n; - - b3Mat33 S = b3Mat33_identity - b3Outer(n, n); - - if (c->lockT1 == true && c->lockT2 == true) - { - S.SetZero(); - } - else - { - if (c->lockT1 == true) - { - b3Vec3 t1 = c->t1; - - S -= b3Outer(t1, t1); - } - - if (c->lockT2 == true) - { - b3Vec3 t2 = c->t2; - - S -= b3Outer(t2, t2); - } - } - - out[i] = S; - } - } -} - -void b3SpringSolver::Solve(b3DenseVec3& x, b3DenseVec3& f, u32& iterations, - const b3SparseMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const -{ - // P = diag(A) - b3DiagMat33 inv_P(m_massCount); - A.AssembleDiagonal(inv_P); - - // Invert - for (u32 i = 0; i < m_massCount; ++i) - { - b3Mat33& D = inv_P[i]; - - // Sylvester Criterion to ensure PD-ness - B3_ASSERT(b3Det(D.x, D.y, D.z) > B3_EPSILON); - - B3_ASSERT(D.x.x != 0.0f); - float32 xx = 1.0f / D.x.x; - - B3_ASSERT(D.y.y != 0.0f); - float32 yy = 1.0f / D.y.y; - - B3_ASSERT(D.z.z != 0.0f); - float32 zz = 1.0f / D.z.z; - - D = b3Diagonal(xx, yy, zz); - } - - // I - b3DiagMat33 I(m_massCount); - I.SetIdentity(); - - // x = S * y + (I - S) * z - x = (S * y) + (I - S) * z; - - // b^ = S * (b - A * (I - S) * z) - b3DenseVec3 b_hat = S * (b - A * ((I - S) * z)); - - // b_delta = dot(b^, P^-1 * b_^) - float32 b_delta = b3Dot(b_hat, inv_P * b_hat); - - // r = S * (b - A * x) - b3DenseVec3 r = S * (b - A * x); - - // p = S * (P^-1 * r) - b3DenseVec3 p = S * (inv_P * r); - - // deltaNew = dot(r, p) - float32 deltaNew = b3Dot(r, p); - - // Tolerance. - // This is the main stopping criteria. - // [0, 1] - const float32 tolerance = 10.0f * B3_EPSILON; - - // Maximum number of iterations. - const u32 maxIters = 1000; - - // Main iteration loop. - u32 iter = 0; - while (deltaNew > tolerance * tolerance * b_delta && iter < maxIters) - { - // s = S * (A * p) - b3DenseVec3 s = S * (A * p); - - // alpha = deltaNew / dot(c, q) - float32 alpha = deltaNew / b3Dot(p, s); - - // x = x + alpha * p - x = x + alpha * p; - - // r = r - alpha * s - r = r - alpha * s; - - // h = inv_P * r - b3DenseVec3 h = inv_P * r; - - // deltaOld = deltaNew - float32 deltaOld = deltaNew; - - // deltaNew = dot(r, h) - deltaNew = b3Dot(r, h); - //B3_ASSERT(b3IsValid(deltaNew)); - - // beta = deltaNew / deltaOld - float32 beta = deltaNew / deltaOld; - - // p = S * (h + beta * p) - p = S * (h + beta * p); - - ++iter; - } - - iterations = iter; - - float32 x2 = b3Dot(x, x); - if (b3IsValid(x2) == false) - { - // Probably the condition number of A is large. - // Solve unconstrained (S = I)? - - // Reject the solution. - x.SetZero(); - f.SetZero(); - } - else - { - // Accept the solution. - // Residual - f = A * x - b; - } -} \ No newline at end of file