diff --git a/include/bounce/bounce.h b/include/bounce/bounce.h index c6bfbcf..c85d381 100644 --- a/include/bounce/bounce.h +++ b/include/bounce/bounce.h @@ -57,4 +57,6 @@ #include #include +#include + #endif \ No newline at end of file diff --git a/include/bounce/cloth/cloth.h b/include/bounce/cloth/cloth.h new file mode 100644 index 0000000..ab83994 --- /dev/null +++ b/include/bounce/cloth/cloth.h @@ -0,0 +1,135 @@ +/* +* 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_CLOTH_H +#define B3_CLOTH_H + +#include +#include + +struct b3Mesh; +class b3Draw; + +struct b3ClothDef +{ + b3ClothDef() + { + mesh = NULL; + density = 0.0f; + gravity.SetZero(); + k1 = 0.9f; + k2 = 0.2f; + kd = 0.1f; + r = 0.0f; + } + + // Cloth mesh + // Each edge must be shared by at most two triangles (manifold) + const b3Mesh* mesh; + + // Cloth density in kilograms per meter squared + float32 density; + + // Gravity force + b3Vec3 gravity; + + // Streching stiffness + float32 k1; + + // Bending stiffness + float32 k2; + + // Damping + float32 kd; + + // Cloth thickness + float32 r; +}; + +struct b3Particle +{ + float32 im; + b3Vec3 p0; + b3Vec3 p; + b3Vec3 v; +}; + +struct b3C1 +{ + float32 L; + u32 i1; + u32 i2; +}; + +struct b3C2 +{ + float32 angle; + u32 i1; + u32 i2; + u32 i3; + u32 i4; +}; + +class b3Cloth +{ +public: + b3Cloth(); + ~b3Cloth(); + + void Initialize(const b3ClothDef& def); + + void Step(float32 dt, u32 iterations); + + u32 GetVertexCount() const + { + return m_pCount; + } + + const b3Particle* GetVertices() const + { + return m_ps; + } + + b3Particle* GetVertices() + { + return m_ps; + } + + void Draw(b3Draw* draw) const; +private: + void SolveC1(); + + b3Particle* m_ps; + u32 m_pCount; + + b3C1* m_c1s; + u32 m_c1Count; + + b3C2* m_c2s; + u32 m_c2Count; + + float32 m_k1; + float32 m_k2; + float32 m_kd; + float32 m_r; + b3Vec3 m_gravity; + + const b3Mesh* m_mesh; +}; + +#endif \ No newline at end of file diff --git a/include/testbed/tests/cloth_test.h b/include/testbed/tests/cloth_test.h new file mode 100644 index 0000000..5077a5e --- /dev/null +++ b/include/testbed/tests/cloth_test.h @@ -0,0 +1,79 @@ +/* +* 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 CLOTH_H +#define CLOTH_H + +extern DebugDraw* g_debugDraw; +extern Camera g_camera; +extern Settings g_settings; + +class Cloth : public Test +{ +public: + Cloth() + { + g_camera.m_zoom = 25.0f; + + b3ClothDef def; + def.mesh = m_meshes + e_clothMesh; + def.density = 0.4f; + def.gravity.Set(-10.0f, 1.0f, 0.0f); + def.k1 = 0.9f; + def.k2 = 0.2f; + def.kd = 0.1f; + def.r = 1.0f; + + m_cloth.Initialize(def); + + b3Particle* vs = m_cloth.GetVertices(); + for (u32 i = 0; i < 5; ++i) + { + vs[i].im = 0.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, g_settings.positionIterations); + m_cloth.Draw(g_debugDraw); + } + + static Test* Create() + { + return new Cloth(); + } + + b3Cloth m_cloth; +}; + +#endif \ No newline at end of file diff --git a/include/testbed/tests/test.h b/include/testbed/tests/test.h index c6baa5a..da2bca8 100644 --- a/include/testbed/tests/test.h +++ b/include/testbed/tests/test.h @@ -116,6 +116,7 @@ public: { e_gridMesh, e_terrainMesh, + e_clothMesh, e_maxMeshes, }; diff --git a/src/bounce/cloth/cloth.cpp b/src/bounce/cloth/cloth.cpp new file mode 100644 index 0000000..22a4fef --- /dev/null +++ b/src/bounce/cloth/cloth.cpp @@ -0,0 +1,209 @@ +/* +* 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 + +b3Cloth::b3Cloth() +{ + m_pCount = 0; + m_ps = NULL; + m_c1Count = 0; + m_c1s = NULL; + m_c2Count = 0; + m_c2s = NULL; + + 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); +} + +void b3Cloth::Initialize(const b3ClothDef& def) +{ + B3_ASSERT(def.mesh); + m_mesh = def.mesh; + + const b3Mesh* m = m_mesh; + + 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(); + } + + m_c1s = (b3C1*)b3Alloc(3 * m->triangleCount * sizeof(b3C1)); + m_c1Count = 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 i1 = is[j]; + u32 i2 = is[k]; + + b3Vec3 p1 = m->vertices[i1]; + b3Vec3 p2 = m->vertices[i2]; + + b3C1* C = m_c1s + m_c1Count; + C->i1 = i1; + C->i2 = i2; + 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; + } + + m_k1 = def.k1; + m_k2 = def.k2; + m_kd = def.kd; + m_r = def.r; + m_gravity = def.gravity; +} + +void b3Cloth::Step(float32 h, u32 iterations) +{ + if (h == 0.0f) + { + 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) + { + 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); + } +} + +void b3Cloth::SolveC1() +{ + float32 k = m_k1; + + for (u32 i = 0; i < m_c1Count; ++i) + { + b3C1* c = m_c1s + i; + + b3Particle* p1 = m_ps + c->i1; + b3Particle* p2 = m_ps + c->i2; + + b3Vec3 d = p2->p - p1->p; + float32 L = b3Length(d); + if (L > B3_EPSILON) + { + d /= L; + } + + float32 C = L - c->L; + + float32 im1 = p1->im; + float32 im2 = p2->im; + + if (im1 + im2 == 0.0f) + { + continue; + } + + float32 s1 = im1 / (im1 + im2); + float32 s2 = im2 / (im1 + im2); + + p1->p -= k * s1 * -C * d; + p2->p += k * s2 * -C * d; + } +} + +void b3Cloth::Draw(b3Draw* draw) const +{ + const b3Color color1(1.0f, 0.0f, 0.0f); + const b3Color color2(0.0f, 1.0f, 0.0f); + const b3Color color3(0.0f, 0.0f, 1.0f); + const b3Color color4(0.0f, 0.0f, 0.0f); + + for (u32 i = 0; i < m_mesh->triangleCount; ++i) + { + b3Triangle* t = m_mesh->triangles + i; + + b3Particle* p1 = m_ps + t->v1; + b3Particle* p2 = m_ps + t->v2; + b3Particle* p3 = m_ps + t->v3; + + b3Vec3 ps[3]; + ps[0] = p1->p; + ps[1] = p2->p; + ps[2] = p3->p; + + draw->DrawPolygon(ps, 3, color4); + draw->DrawSolidPolygon(ps, 3, color3); + } +} \ No newline at end of file diff --git a/src/bounce/dynamics/island.cpp b/src/bounce/dynamics/island.cpp index 51e8668..b4ff8be 100644 --- a/src/bounce/dynamics/island.cpp +++ b/src/bounce/dynamics/island.cpp @@ -87,6 +87,15 @@ void b3Island::Solve(b3Profile* profile, const b3Vec3& gravity, float32 dt, u32 { float32 h = dt; + // Measure coefficient of damping. + // Box2D. + // ODE: dv/dt + c * v = 0 + // Solution: v(t) = v0 * exp(-c * t) + // Step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) + // v2 = exp(-c * dt) * v1 + const float32 k_d = 0.1f; + float32 d = exp(-h * k_d); + // 1. Integrate velocities for (u32 i = 0; i < m_bodyCount; ++i) { @@ -116,16 +125,9 @@ void b3Island::Solve(b3Profile* profile, const b3Vec3& gravity, float32 dt, u32 // Clear torques b->m_torque.SetZero(); - // Apply some damping - // Box2D. - // ODE: dv/dt + c * v = 0 - // Solution: v(t) = v0 * exp(-c * t) - // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) - // v2 = exp(-c * dt) * v1 - // Pade approximation: - // v2 = v1 * 1 / (1 + c * dt) - v *= 1.0f / (1.0f + h * 0.1f); - w *= 1.0f / (1.0f + h * 0.1f); + // Apply damping + v *= d; + w *= d; } m_velocities[i].v = v; diff --git a/src/testbed/framework/test.cpp b/src/testbed/framework/test.cpp index 8628638..97dfb4f 100644 --- a/src/testbed/framework/test.cpp +++ b/src/testbed/framework/test.cpp @@ -110,7 +110,7 @@ Test::Test() t.y = 0.0f; t.z = -0.5f * float32(h); - b3Mesh* mesh = m_meshes + 0; + b3Mesh* mesh = m_meshes + e_gridMesh; mesh->vertexCount = w * h; mesh->vertices = (b3Vec3*)b3Alloc(mesh->vertexCount * sizeof(b3Vec3)); @@ -168,7 +168,90 @@ Test::Test() mesh->BuildTree(); } - + + { + const u32 w = 5; + const u32 h = 5; + + b3Vec3 t; + t.x = -0.5f * float32(w); + t.y = 0.0f; + t.z = -0.5f * float32(h); + + b3Mesh* mesh = m_meshes + e_clothMesh; + + mesh->vertexCount = w * h; + mesh->vertices = (b3Vec3*)b3Alloc(mesh->vertexCount * sizeof(b3Vec3)); + + for (u32 i = 0; i < w; ++i) + { + for (u32 j = 0; j < h; ++j) + { + u32 v1 = i * w + j; + + b3Vec3 v; + v.x = float32(i); + v.y = 0.0f; + v.z = float32(j); + + v += t; + + mesh->vertices[v1] = v; + } + } + + mesh->triangleCount = 2 * 2 * (w - 1) * (h - 1); + mesh->triangles = (b3Triangle*)b3Alloc(mesh->triangleCount * sizeof(b3Triangle)); + + u32 triangleCount = 0; + for (u32 i = 0; i < w - 1; ++i) + { + for (u32 j = 0; j < h - 1; ++j) + { + u32 v1 = i * w + j; + u32 v2 = (i + 1) * w + j; + u32 v3 = (i + 1) * w + (j + 1); + u32 v4 = i * w + (j + 1); + + B3_ASSERT(triangleCount < mesh->triangleCount); + b3Triangle* t1 = mesh->triangles + triangleCount; + ++triangleCount; + + t1->v1 = v3; + t1->v2 = v2; + t1->v3 = v1; + + B3_ASSERT(triangleCount < mesh->triangleCount); + b3Triangle* t2 = mesh->triangles + triangleCount; + ++triangleCount; + + t2->v1 = v1; + t2->v2 = v4; + t2->v3 = v3; + + B3_ASSERT(triangleCount < mesh->triangleCount); + b3Triangle* t3 = mesh->triangles + triangleCount; + ++triangleCount; + + t3->v1 = v1; + t3->v2 = v2; + t3->v3 = v3; + + B3_ASSERT(triangleCount < mesh->triangleCount); + b3Triangle* t4 = mesh->triangles + triangleCount; + ++triangleCount; + + t4->v1 = v3; + t4->v2 = v4; + t4->v3 = v1; + } + } + + B3_ASSERT(triangleCount == mesh->triangleCount); + + mesh->BuildTree(); + } + { const u32 w = 100; const u32 h = 100; @@ -178,7 +261,7 @@ Test::Test() t.y = 0.0f; t.z = -0.5f * float32(h); - b3Mesh* mesh = m_meshes + 1; + b3Mesh* mesh = m_meshes + e_terrainMesh; mesh->vertexCount = w * h; mesh->vertices = (b3Vec3*)b3Alloc(mesh->vertexCount * sizeof(b3Vec3)); diff --git a/src/testbed/framework/test_entries.cpp b/src/testbed/framework/test_entries.cpp index 9ec0805..e217f2b 100644 --- a/src/testbed/framework/test_entries.cpp +++ b/src/testbed/framework/test_entries.cpp @@ -46,6 +46,7 @@ #include #include #include +#include TestEntry g_tests[] = { @@ -77,5 +78,6 @@ TestEntry g_tests[] = { "Body Types", &BodyTypes::Create }, { "Varying Friction", &VaryingFriction::Create }, { "Varying Restitution", &VaryingRestitution::Create }, + { "Cloth", &Cloth::Create }, { NULL, NULL } }; \ No newline at end of file