diff --git a/examples/testbed/framework/test_entries.cpp b/examples/testbed/framework/test_entries.cpp index 5b5daa4..130849f 100644 --- a/examples/testbed/framework/test_entries.cpp +++ b/examples/testbed/framework/test_entries.cpp @@ -58,11 +58,12 @@ #include #include #include -#include #include -#include #include +#include +#include //#include +#include TestEntry g_tests[] = { @@ -106,10 +107,11 @@ TestEntry g_tests[] = { "Initial Overlap", &InitialOverlap::Create }, { "Single Pendulum", &SinglePendulum::Create }, { "Multiple Pendulum", &MultiplePendulum::Create }, - { "Rope", &Rope::Create }, - { "Implicit Spring", &ImplicitSpring::Create }, { "Cloth", &Cloth::Create }, { "Spring Cloth", &SpringCloth::Create }, + { "Spring Cloth Collision", &SpringClothCollision::Create }, + { "Rope", &Rope::Create }, //{ "Tree", &Tree::Create }, + { "Implicit Spring", &ImplicitSpring::Create }, { NULL, NULL } }; \ No newline at end of file diff --git a/examples/testbed/tests/spring_cloth_collision_test.h b/examples/testbed/tests/spring_cloth_collision_test.h new file mode 100644 index 0000000..966149a --- /dev/null +++ b/examples/testbed/tests/spring_cloth_collision_test.h @@ -0,0 +1,82 @@ +/* +* 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_COLLISION_TESH_H +#define SPRING_CLOTH_COLLISION_TESH_H + +extern DebugDraw* g_debugDraw; +extern Camera g_camera; +extern Settings g_settings; + +class SpringClothCollision : public Test +{ +public: + SpringClothCollision() + { + g_camera.m_zoom = 25.0f; + + b3SpringClothDef def; + def.allocator = &m_clothAllocator; + def.mesh = m_meshes + e_clothMesh; + def.density = 0.2f; + def.ks = 1000.0f; + def.kd = 10.0f; + def.r = 0.2f; + def.gravity.Set(0.0f, -10.0f, 0.0f); + + m_cloth.Initialize(def); + + b3Sphere* sphere = m_cloth.CreateSphere(b3Vec3(0.0f, -5.0f, 0.0f), 4.0f); + } + + void Step() + { + float32 dt = g_settings.hertz > 0.0f ? 1.0f / g_settings.hertz : 0.0f; + + if (g_settings.pause) + { + if (g_settings.singleStep) + { + g_settings.singleStep = false; + } + else + { + dt = 0.0f; + } + } + + m_cloth.Step(dt); + m_cloth.Draw(g_debugDraw); + + b3SpringClothStep step = m_cloth.GetStep(); + + char text[256]; + sprintf(text, "Iterations = %u", step.iterations); + g_debugDraw->DrawString(text, b3Color_white); + } + + static Test* Create() + { + return new SpringClothCollision(); + } + + b3StackAllocator m_clothAllocator; + b3SpringCloth m_cloth; +}; + +#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 index 455bed6..66a3aaf 100644 --- a/include/bounce/dynamics/cloth/spring_cloth.h +++ b/include/bounce/dynamics/cloth/spring_cloth.h @@ -20,6 +20,9 @@ #define B3_SPRING_CLOTH_H #include +#include + +#define B3_CLOTH_SPHERE_CAPACITY 32 class b3StackAllocator; class b3Draw; @@ -36,6 +39,7 @@ struct b3SpringClothDef ks = 0.0f; kb = 0.0f; kd = 0.0f; + r = 0.05f; gravity.SetZero(); } @@ -57,7 +61,10 @@ struct b3SpringClothDef // Damping stiffness float32 kd; - // Force due to gravity + // Mass radius + float32 r; + + // Force due to gravity b3Vec3 gravity; }; @@ -87,6 +94,15 @@ enum b3MassType e_dynamicMass }; +// This structure represents an acceleration constraint. +struct b3MassCollision +{ + u32 j; + float32 s; + b3Vec3 n; + bool active; +}; + // Time step statistics struct b3SpringClothStep { @@ -119,6 +135,9 @@ public: // b3MassType GetType(u32 i) const; + // + b3Sphere* CreateSphere(const b3Vec3& center, float32 radius); + // const b3SpringClothStep& GetStep() const; @@ -131,6 +150,8 @@ public: // void Draw(b3Draw* draw) const; protected: + void UpdateCollisions() const; + b3StackAllocator* m_allocator; b3Mesh* m_mesh; @@ -141,12 +162,19 @@ protected: b3Vec3* m_v; b3Vec3* m_f; float32* m_inv_m; + b3Vec3* m_y; b3MassType* m_massTypes; + b3MassCollision* m_collisions; u32 m_massCount; b3Spring* m_springs; u32 m_springCount; + float32 m_r; + + b3Sphere m_spheres[B3_CLOTH_SPHERE_CAPACITY]; + u32 m_sphereCount; + b3SpringClothStep m_step; }; diff --git a/src/bounce/dynamics/cloth/spring_cloth.cpp b/src/bounce/dynamics/cloth/spring_cloth.cpp index 19ce060..1ea3b9f 100644 --- a/src/bounce/dynamics/cloth/spring_cloth.cpp +++ b/src/bounce/dynamics/cloth/spring_cloth.cpp @@ -34,12 +34,18 @@ b3SpringCloth::b3SpringCloth() m_x = nullptr; m_v = nullptr; m_f = nullptr; + m_y = nullptr; m_inv_m = nullptr; m_massTypes = nullptr; + m_collisions = nullptr; m_massCount = 0; m_springs = nullptr; - m_springCount = 0; + m_springCount = 0; + + m_r = 0.0f; + + m_sphereCount = 0; m_step.iterations = 0; } @@ -50,7 +56,9 @@ b3SpringCloth::~b3SpringCloth() b3Free(m_v); b3Free(m_f); b3Free(m_inv_m); + b3Free(m_y); b3Free(m_massTypes); + b3Free(m_collisions); b3Free(m_springs); } @@ -63,6 +71,8 @@ void b3SpringCloth::Initialize(const b3SpringClothDef& def) m_mesh = def.mesh; m_gravity = def.gravity; + m_r = def.r; + const b3Mesh* m = m_mesh; m_massCount = m->vertexCount; @@ -70,14 +80,18 @@ void b3SpringCloth::Initialize(const b3SpringClothDef& def) m_v = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3)); m_f = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3)); m_inv_m = (float32*)b3Alloc(m_massCount * sizeof(float32)); + m_y = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3)); m_massTypes = (b3MassType*)b3Alloc(m_massCount * sizeof(b3MassType)); + m_collisions = (b3MassCollision*)b3Alloc(m_massCount * sizeof(b3MassCollision)); for (u32 i = 0; i < m->vertexCount; ++i) { + m_collisions[i].active = false; m_x[i] = m->vertices[i]; m_v[i].SetZero(); m_f[i].SetZero(); m_inv_m[i] = 0.0f; + m_y[i].SetZero(); m_massTypes[i] = e_staticMass; } @@ -154,7 +168,24 @@ void b3SpringCloth::Initialize(const b3SpringClothDef& def) } } -static B3_FORCE_INLINE void b3Filter(b3Vec3* out, const b3Vec3* v, u32 size, const b3MassType* types) +b3Sphere* b3SpringCloth::CreateSphere(const b3Vec3& center, float32 radius) +{ + B3_ASSERT(m_sphereCount < B3_CLOTH_SPHERE_CAPACITY); + + if (m_sphereCount == B3_CLOTH_SPHERE_CAPACITY) + { + return nullptr; + } + + b3Sphere* sphere = m_spheres + m_sphereCount; + sphere->vertex = center; + sphere->radius = radius; + ++m_sphereCount; + return sphere; +} + +static B3_FORCE_INLINE void b3Make_z(b3Vec3* out, u32 size, + const b3MassType* types, const b3MassCollision* collisions) { for (u32 i = 0; i < size; ++i) { @@ -167,6 +198,47 @@ static B3_FORCE_INLINE void b3Filter(b3Vec3* out, const b3Vec3* v, u32 size, con } case e_dynamicMass: { + if (collisions[i].active) + { + out[i].SetZero(); + break; + } + + out[i].SetZero(); + break; + } + default: + { + B3_ASSERT(false); + break; + } + } + } +} + +static B3_FORCE_INLINE void b3Filter(b3Vec3* out, const b3Vec3* v, u32 size, + const b3MassType* types, const b3MassCollision* collisions) +{ + for (u32 i = 0; i < size; ++i) + { + switch (types[i]) + { + case e_staticMass: + { + out[i].SetZero(); + break; + } + case e_dynamicMass: + { + if (collisions[i].active) + { + b3Vec3 n = collisions[i].n; + + b3Mat33 S = b3Mat33_identity - b3Outer(n, n); + + out[i] = S * v[i]; + break; + } out[i] = v[i]; break; @@ -319,8 +391,90 @@ static B3_FORCE_INLINE void b3Mul_A(b3Vec3* out, const b3Vec3* v, u32 mass_size, allocator->Free(v1); } +void b3SpringCloth::UpdateCollisions() const +{ + // Compute cloth-solid collision position alteration + for (u32 i = 0; i < m_massCount; ++i) + { + // Clear flag + m_collisions[i].active = false; + + b3Vec3 c1 = m_x[i]; + float32 r1 = m_r; + + // Only solve the deepest penetrations + float32 bestSeparation = B3_MAX_FLOAT; + u32 bestIndex = ~0; + + for (u32 j = 0; j < m_sphereCount; ++j) + { + const b3Sphere* sphere = m_spheres + j; + b3Vec3 c2 = sphere->vertex; + float32 r2 = sphere->radius; + + b3Vec3 d = c2 - c1; + float32 dd = b3Dot(d, d); + float32 totalRadius = r1 + r2; + if (dd > totalRadius * totalRadius) + { + continue; + } + + float32 distance = b3Length(d); + float32 separation = distance - totalRadius; + + if (separation < bestSeparation) + { + bestSeparation = separation; + bestIndex = j; + } + } + + if (bestIndex != ~0) + { + const b3Sphere* sphere = m_spheres + bestIndex; + b3Vec3 c2 = sphere->vertex; + float32 r2 = sphere->radius; + + float32 totalRadius = r1 + r2; + + b3Vec3 d = c2 - c1; + float32 distance = b3Length(d); + float32 separation = distance - totalRadius; + + b3Vec3 n(0.0f, 1.0f, 0.0f); + if (distance > B3_EPSILON) + { + n = d / distance; + } + + // Avoid large corrections + const float32 kMaxCorrection = 0.75f; + separation = b3Clamp(separation, -kMaxCorrection, 0.0f); + + b3Vec3 dx1 = separation * n; + + // Add position alteration + m_y[i] += dx1; + + m_collisions[i].active = true; + m_collisions[i].j = bestIndex; + m_collisions[i].s = separation; + m_collisions[i].n = n; + } + } +} + void b3SpringCloth::Step(float32 h) { + if (h == 0.0f) + { + return; + } + + // Detect and store collisions + UpdateCollisions(); + u32 size = m_massCount; b3MassType* types = m_massTypes; u32 spring_size = m_springCount; @@ -328,9 +482,12 @@ void b3SpringCloth::Step(float32 h) // Add gravity for (u32 i = 0; i < size; ++i) { - m_f[i] += m_gravity; + if (types[i] == e_dynamicMass) + { + m_f[i] += m_gravity; + } } - + // Compute non-zero Jacobians Jx, Jv b3Mat33* Jx = (b3Mat33*)m_allocator->Allocate(spring_size * sizeof(b3Mat33)); b3SetZero_Jacobian(Jx, spring_size); @@ -403,19 +560,28 @@ void b3SpringCloth::Step(float32 h) // Compute b - // b = h * (f0 + h * dfdx * v0) + // b = h * (f0 + h * dfdx * v0 + dfdx * y) ) b3Vec3* b = (b3Vec3*) m_allocator->Allocate(size * sizeof(b3Vec3)); - // b = dfdx * v0 - // b3Mul(b, dfdx, m_v, size); - b3Mul_Jacobian(b, m_v, size, Jx, m_springs, m_springCount); + // Jx_v = dfdx * v + b3Vec3* Jx_v = (b3Vec3*)m_allocator->Allocate(size * sizeof(b3Vec3)); + // b3Mul(Jx_v, dfdx, v, size); + b3Mul_Jacobian(Jx_v, m_v, size, Jx, m_springs, m_springCount); - // b = h * (f0 + h * b) + // Jx_v0y = dfdx * y + b3Vec3* Jx_y = (b3Vec3*)m_allocator->Allocate(size * sizeof(b3Vec3)); + // b3Mul(Jx_y, dfdx, y, size); + b3Mul_Jacobian(Jx_y, m_y, size, Jx, m_springs, m_springCount); + + // b = h * (f0 + h * Jx_v + Jx_y ) for (u32 i = 0; i < size; ++i) { - b[i] = h * (m_f[i] + h * b[i]); + b[i] = h * (m_f[i] + h * Jx_v[i] + Jx_y[i]); } + m_allocator->Free(Jx_y); + m_allocator->Free(Jx_v); + // Solve Ax = b b3Vec3* z = (b3Vec3*)m_allocator->Allocate(size * sizeof(b3Vec3)); @@ -436,27 +602,7 @@ void b3SpringCloth::Step(float32 h) b3Vec3* inv_P = (b3Vec3*)m_allocator->Allocate(size * sizeof(b3Vec3)); // Compute z - for (u32 i = 0; i < size; ++i) - { - switch (types[i]) - { - case e_staticMass: - { - z[i].SetZero(); - break; - } - case e_dynamicMass: - { - z[i].SetZero(); - break; - } - default: - { - B3_ASSERT(false); - break; - } - } - } + b3Make_z(z, size, types, m_collisions); // dv = z b3Copy(dv, z, size); @@ -543,7 +689,7 @@ void b3SpringCloth::Step(float32 h) // eps0 = dot( filter(b), P * filter(b) ) b3Vec3* filter_b = (b3Vec3*)m_allocator->Allocate(size * sizeof(b3Vec3)); - b3Filter(filter_b, b, size, types); + b3Filter(filter_b, b, size, types, m_collisions); b3Vec3* P_filter_b = (b3Vec3*)m_allocator->Allocate(size * sizeof(b3Vec3)); for (u32 i = 0; i < size; ++i) @@ -560,7 +706,7 @@ void b3SpringCloth::Step(float32 h) // r = filter(b - Adv) b3Sub(r, b, Adv, size); - b3Filter(r, r, size, types); + b3Filter(r, r, size, types, m_collisions); // c = filter(P^-1 * r) for (u32 i = 0; i < m_massCount; ++i) @@ -569,7 +715,7 @@ void b3SpringCloth::Step(float32 h) c[i][1] = inv_P[i][1] * r[i][1]; c[i][2] = inv_P[i][2] * r[i][2]; } - b3Filter(c, c, size, types); + b3Filter(c, c, size, types, m_collisions); // epsNew = dot(r, c) float32 epsNew = b3Dot(r, c, size); @@ -577,10 +723,10 @@ void b3SpringCloth::Step(float32 h) // This is in [0, 1] // Making it smaller can increase accuracy, but it might increase the number // of iterations to be taken by the solver. - const float32 kTol = 0.75f; + const float32 kTol = 0.25f; // Limit number of iterations to prevent cycling. - const u32 kMaxIters = 100; + const u32 kMaxIters = 200; // Main iteration loop. u32 iter = 0; @@ -589,7 +735,7 @@ void b3SpringCloth::Step(float32 h) // q = filter(A * c) // b3Mul(q, A, c, size); b3Mul_A(q, c, size, m_allocator, m_inv_m, h, Jx, Jv, m_springs, m_springCount); - b3Filter(q, q, size, types); + b3Filter(q, q, size, types, m_collisions); // alpha = epsNew / dot(c, q) float32 alpha = epsNew / b3Dot(c, q, size); @@ -628,7 +774,7 @@ void b3SpringCloth::Step(float32 h) { c[i] = s[i] + beta * c[i]; } - b3Filter(c, c, size, types); + b3Filter(c, c, size, types, m_collisions); ++iter; } @@ -639,7 +785,7 @@ void b3SpringCloth::Step(float32 h) for (u32 i = 0; i < m_massCount; ++i) { m_v[i] += dv[i]; - m_x[i] += h * m_v[i]; + m_x[i] += h * m_v[i] + m_y[i]; } // Clear forces @@ -648,6 +794,12 @@ void b3SpringCloth::Step(float32 h) m_f[i].SetZero(); } + // Clear position alteration + for (u32 i = 0; i < m_massCount; ++i) + { + m_y[i].SetZero(); + } + m_allocator->Free(inv_P); m_allocator->Free(P); m_allocator->Free(s); @@ -672,6 +824,11 @@ void b3SpringCloth::Apply() const void b3SpringCloth::Draw(b3Draw* draw) const { + for (u32 i = 0; i < m_sphereCount; ++i) + { + draw->DrawSolidSphere(m_spheres[i].vertex, m_spheres[i].radius, b3Color_white); + } + const b3Mesh* m = m_mesh; for (u32 i = 0; i < m->vertexCount; ++i)