From caef3fede8efd21ec283270d9bfa5d9f67743737 Mon Sep 17 00:00:00 2001 From: Irlan <-> Date: Wed, 30 May 2018 11:34:41 -0300 Subject: [PATCH] maintain the upper triangle of A, external particle/force creation/destruction, particle force abstraction, testbed update --- examples/testbed/tests/cloth_test.h | 261 ++++------ examples/testbed/tests/particle_types.h | 23 +- examples/testbed/tests/pinned_cloth.h | 52 +- examples/testbed/tests/shirt.h | 8 +- examples/testbed/tests/table_cloth.h | 49 +- examples/testbed/tests/tension_mapping.h | 85 ++- include/bounce/bounce.h | 13 +- include/bounce/dynamics/cloth/cloth.h | 302 +++-------- include/bounce/dynamics/cloth/cloth_mesh.h | 6 +- include/bounce/dynamics/cloth/cloth_solver.h | 78 ++- include/bounce/dynamics/cloth/force.h | 87 ++++ include/bounce/dynamics/cloth/particle.h | 226 ++++++++ include/bounce/dynamics/cloth/sparse_mat33.h | 154 ------ .../bounce/dynamics/cloth/sparse_sym_mat33.h | 236 +++++++++ include/bounce/dynamics/cloth/spring_force.h | 140 +++++ src/bounce/dynamics/cloth/cloth.cpp | 486 ++++++++++-------- src/bounce/dynamics/cloth/cloth_mesh.cpp | 4 + src/bounce/dynamics/cloth/cloth_solver.cpp | 283 ++++------ src/bounce/dynamics/cloth/force.cpp | 62 +++ src/bounce/dynamics/cloth/particle.cpp | 69 +++ src/bounce/dynamics/cloth/spring_force.cpp | 121 +++++ 21 files changed, 1657 insertions(+), 1088 deletions(-) create mode 100644 include/bounce/dynamics/cloth/force.h create mode 100644 include/bounce/dynamics/cloth/particle.h delete mode 100644 include/bounce/dynamics/cloth/sparse_mat33.h create mode 100644 include/bounce/dynamics/cloth/sparse_sym_mat33.h create mode 100644 include/bounce/dynamics/cloth/spring_force.h create mode 100644 src/bounce/dynamics/cloth/force.cpp create mode 100644 src/bounce/dynamics/cloth/particle.cpp create mode 100644 src/bounce/dynamics/cloth/spring_force.cpp diff --git a/examples/testbed/tests/cloth_test.h b/examples/testbed/tests/cloth_test.h index b2aab56..885ed47 100644 --- a/examples/testbed/tests/cloth_test.h +++ b/examples/testbed/tests/cloth_test.h @@ -25,6 +25,7 @@ public: ClothDragger(Ray3* ray, b3Cloth*& cloth) : m_ray(ray), m_cloth(cloth) { m_isSelected = false; + m_spring = false; } ~ClothDragger() @@ -61,31 +62,32 @@ public: { B3_ASSERT(m_isSelected == false); - if (Select(m_selection, m_x) == false) + b3RayCastInput rayIn; + rayIn.p1 = m_ray->A(); + rayIn.p2 = m_ray->B(); + rayIn.maxFraction = B3_MAX_FLOAT; + + b3ClothRayCastOutput rayOut; + if (m_cloth->RayCast(&rayOut, &rayIn) == false) { return false; } m_isSelected = true; + + m_selection = rayOut.triangle; + m_x = rayOut.fraction; 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* p1 = m->particles[t->v1]; + b3Particle* p2 = m->particles[t->v2]; + b3Particle* p3 = m->particles[t->v3]; - 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 v1 = p1->GetPosition(); + b3Vec3 v2 = p2->GetPosition(); + b3Vec3 v3 = p3->GetPosition(); b3Vec3 B = GetPointB(); @@ -102,6 +104,53 @@ public: m_u = m_v = 0.0f; } + if (m_spring) + { + b3ParticleDef pd; + pd.type = e_staticParticle; + pd.position = B; + + m_particle = m_cloth->CreateParticle(pd); + + { + b3SpringForceDef sfd; + sfd.p1 = m_particle; + sfd.p2 = p1; + sfd.restLength = 0.0f; + sfd.structural = 10000.0f; + m_s1 = (b3SpringForce*)m_cloth->CreateForce(sfd); + } + + { + b3SpringForceDef sfd; + sfd.p1 = m_particle; + sfd.p2 = p2; + sfd.restLength = 0.0f; + sfd.structural = 10000.0f; + m_s2 = (b3SpringForce*)m_cloth->CreateForce(sfd); + } + + { + b3SpringForceDef sfd; + sfd.p1 = m_particle; + sfd.p2 = p3; + sfd.restLength = 0.0f; + sfd.structural = 10000.0f; + m_s3 = (b3SpringForce*)m_cloth->CreateForce(sfd); + } + } + else + { + m_t1 = p1->GetType(); + p1->SetType(e_staticParticle); + + m_t2 = p2->GetType(); + p2->SetType(e_staticParticle); + + m_t3 = p3->GetType(); + p3->SetType(e_staticParticle); + } + return true; } @@ -117,14 +166,21 @@ public: b3Vec3 dx = B - A; - b3Particle* p1 = m_cloth->GetParticle(t->v1); - m_cloth->ApplyTranslation(p1, dx); - - b3Particle* p2 = m_cloth->GetParticle(t->v2); - m_cloth->ApplyTranslation(p2, dx); - - b3Particle* p3 = m_cloth->GetParticle(t->v3); - m_cloth->ApplyTranslation(p3, dx); + if (m_spring) + { + m_particle->ApplyTranslation(dx); + } + else + { + b3Particle* p1 = m->particles[t->v1]; + p1->ApplyTranslation(dx); + + b3Particle* p2 = m->particles[t->v2]; + p2->ApplyTranslation(dx); + + b3Particle* p3 = m->particles[t->v3]; + p3->ApplyTranslation(dx); + } } void StopDragging() @@ -133,141 +189,30 @@ public: m_isSelected = false; - b3ClothMesh* m = m_cloth->GetMesh(); - b3ClothMeshTriangle* t = m->triangles + m_selection; + if (m_spring) + { + m_cloth->DestroyForce(m_s1); + m_cloth->DestroyForce(m_s2); + m_cloth->DestroyForce(m_s3); + m_cloth->DestroyParticle(m_particle); + } + else + { + 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); + b3Particle* p1 = m->particles[t->v1]; + p1->SetType(m_t1); + + b3Particle* p2 = m->particles[t->v2]; + p2->SetType(m_t2); + + b3Particle* p3 = m->particles[t->v3]; + p3->SetType(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 = -B3_EPSILON; - - // 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; @@ -276,6 +221,14 @@ private: b3Cloth*& m_cloth; u32 m_selection; float32 m_u, m_v; + + bool m_spring; + + b3Particle* m_particle; + b3SpringForce* m_s1; + b3SpringForce* m_s2; + b3SpringForce* m_s3; + b3ParticleType m_t1, m_t2, m_t3; }; diff --git a/examples/testbed/tests/particle_types.h b/examples/testbed/tests/particle_types.h index 3e94560..5aa4c71 100644 --- a/examples/testbed/tests/particle_types.h +++ b/examples/testbed/tests/particle_types.h @@ -38,10 +38,9 @@ public: void SetClothType(b3ParticleType type) { - for (u32 i = 0; i < m_cloth->GetParticleCount(); ++i) + for (b3Particle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext()) { - b3Particle* p = m_cloth->GetParticle(i); - m_cloth->SetType(p, type); + p->SetType(type); } } @@ -62,10 +61,8 @@ public: SetClothType(e_dynamicParticle); } - for (u32 i = 0; i < m_cloth->GetParticleCount(); ++i) + for (b3Particle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext()) { - b3Particle* p = m_cloth->GetParticle(i); - b3Vec3 d; d.SetZero(); @@ -94,25 +91,25 @@ public: button == GLFW_KEY_UP || button == GLFW_KEY_DOWN) { - if (p->type == e_staticParticle) + if (p->GetType() == e_staticParticle) { - m_cloth->ApplyTranslation(p, d); + p->ApplyTranslation(d); } - if (p->type == e_kinematicParticle) + if (p->GetType() == e_kinematicParticle) { - b3Vec3 v = p->velocity; + b3Vec3 v = p->GetVelocity(); v += 5.0f * d; - m_cloth->SetVelocity(p, d); + p->SetVelocity(v); } - if (p->type == e_dynamicParticle) + if (p->GetType() == e_dynamicParticle) { b3Vec3 f = 100.0f * d; - m_cloth->ApplyForce(p, f); + p->ApplyForce(f); } } } diff --git a/examples/testbed/tests/pinned_cloth.h b/examples/testbed/tests/pinned_cloth.h index a9bf9cf..785c106 100644 --- a/examples/testbed/tests/pinned_cloth.h +++ b/examples/testbed/tests/pinned_cloth.h @@ -22,32 +22,27 @@ class PinnedCloth : public ClothTest { public: - PinnedCloth() + PinnedCloth() : m_rectangleGarment(5.0f, 5.0f) { - m_gridClothMesh.vertexCount = m_gridMesh.vertexCount; - m_gridClothMesh.vertices = m_gridMesh.vertices; + // Generate 2D mesh + m_rectangleGarmentMesh.Set(&m_rectangleGarment, 1.0f); - m_gridClothMesh.triangleCount = m_gridMesh.triangleCount; - m_gridClothMesh.triangles = (b3ClothMeshTriangle*)m_gridMesh.triangles; - - m_gridClothMeshMesh.vertexCount = m_gridClothMesh.vertexCount; - m_gridClothMeshMesh.startVertex = 0; - - m_gridClothMeshMesh.triangleCount = m_gridClothMesh.triangleCount; - m_gridClothMeshMesh.startTriangle = 0; - - m_gridClothMesh.meshCount = 1; - m_gridClothMesh.meshes = &m_gridClothMeshMesh; - - m_gridClothMesh.sewingLineCount = 0; - m_gridClothMesh.sewingLines = nullptr; + // Create 3D mesh + m_rectangleClothMesh.Set(&m_rectangleGarmentMesh); + + // + b3Mat33 dq = b3Mat33RotationX(0.5f * B3_PI); + for (u32 i = 0; i < m_rectangleClothMesh.vertexCount; ++i) + { + m_rectangleClothMesh.vertices[i] = dq * m_rectangleClothMesh.vertices[i]; + } b3ClothDef def; - def.mesh = &m_gridClothMesh; + def.mesh = &m_rectangleClothMesh; + def.radius = 0.05f; def.density = 0.2f; - def.ks = 10000.0f; - def.kd = 0.0f; - def.r = 0.05f; + def.structural = 10000.0f; + def.damping = 0.0f; m_cloth = m_world.CreateCloth(def); @@ -55,12 +50,11 @@ 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->GetParticleCount(); ++i) + for (b3Particle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext()) { - b3Particle* p = m_cloth->GetParticle(i); - if (aabb.Contains(p->position)) + if (aabb.Contains(p->GetPosition())) { - m_cloth->SetType(p, e_staticParticle); + p->SetType(e_staticParticle); } } } @@ -69,10 +63,10 @@ public: { return new PinnedCloth(); } - - b3GridMesh<10, 10> m_gridMesh; - b3ClothMeshMesh m_gridClothMeshMesh; - b3ClothMesh m_gridClothMesh; + + b3RectangleGarment m_rectangleGarment; + b3GarmentMesh m_rectangleGarmentMesh; + b3GarmentClothMesh m_rectangleClothMesh; }; #endif \ No newline at end of file diff --git a/examples/testbed/tests/shirt.h b/examples/testbed/tests/shirt.h index ed0a6c2..0cd2e13 100644 --- a/examples/testbed/tests/shirt.h +++ b/examples/testbed/tests/shirt.h @@ -19,10 +19,6 @@ #ifndef SHIRT_H #define SHIRT_H -#include -#include -#include - class Shirt : public ClothTest { public: @@ -57,10 +53,10 @@ public: // Create cloth b3ClothDef def; + def.radius = 0.2f; def.mesh = &m_shirtClothMesh; def.density = 0.2f; - def.r = 0.2f; - def.ks = 10000.0f; + def.structural = 10000.0f; m_cloth = m_world.CreateCloth(def); } diff --git a/examples/testbed/tests/table_cloth.h b/examples/testbed/tests/table_cloth.h index 2934f33..3f4bd17 100644 --- a/examples/testbed/tests/table_cloth.h +++ b/examples/testbed/tests/table_cloth.h @@ -22,38 +22,28 @@ class TableCloth : public ClothTest { public: - TableCloth() + TableCloth() : m_rectangleGarment(5.0f, 5.0f) { - // Shift vertices up - for (u32 i = 0; i < m_gridMesh.vertexCount; ++i) + // Generate 2D mesh + m_rectangleGarmentMesh.Set(&m_rectangleGarment, 1.0f); + + // Create 3D mesh + m_rectangleClothMesh.Set(&m_rectangleGarmentMesh); + + // + b3Mat33 dq = b3Mat33RotationX(0.5f * B3_PI); + for (u32 i = 0; i < m_rectangleClothMesh.vertexCount; ++i) { - m_gridMesh.vertices[i].y = 5.0f; + m_rectangleClothMesh.vertices[i] = dq * m_rectangleClothMesh.vertices[i]; + m_rectangleClothMesh.vertices[i].y += 5.0f; } - m_gridClothMesh.vertexCount = m_gridMesh.vertexCount; - m_gridClothMesh.vertices = m_gridMesh.vertices; - - m_gridClothMesh.triangleCount = m_gridMesh.triangleCount; - m_gridClothMesh.triangles = (b3ClothMeshTriangle*)m_gridMesh.triangles; - - m_gridClothMeshMesh.vertexCount = m_gridClothMesh.vertexCount; - m_gridClothMeshMesh.startVertex = 0; - - m_gridClothMeshMesh.triangleCount = m_gridClothMesh.triangleCount; - m_gridClothMeshMesh.startTriangle = 0; - - m_gridClothMesh.meshCount = 1; - m_gridClothMesh.meshes = &m_gridClothMeshMesh; - - m_gridClothMesh.sewingLineCount = 0; - m_gridClothMesh.sewingLines = nullptr; - b3ClothDef def; - def.mesh = &m_gridClothMesh; + def.mesh = &m_rectangleClothMesh; + def.radius = 0.05f; def.density = 0.2f; - def.ks = 10000.0f; - def.kd = 0.0f; - def.r = 0.05f; + def.structural = 10000.0f; + def.damping = 0.0f; m_cloth = m_world.CreateCloth(def); @@ -87,10 +77,9 @@ public: return new TableCloth(); } - //b3GridMesh<2, 2> m_gridMesh; - b3GridMesh<10, 10> m_gridMesh; - b3ClothMeshMesh m_gridClothMeshMesh; - b3ClothMesh m_gridClothMesh; + b3RectangleGarment m_rectangleGarment; + b3GarmentMesh m_rectangleGarmentMesh; + b3GarmentClothMesh m_rectangleClothMesh; b3QHull m_tableHull; }; diff --git a/examples/testbed/tests/tension_mapping.h b/examples/testbed/tests/tension_mapping.h index e8bf72a..d49abbb 100644 --- a/examples/testbed/tests/tension_mapping.h +++ b/examples/testbed/tests/tension_mapping.h @@ -58,32 +58,26 @@ static inline b3Color Color(float32 x, float32 a, float32 b) class TensionMapping : public ClothTest { public: - TensionMapping() + TensionMapping() : m_rectangleGarment(5.0f, 5.0f) { - m_gridClothMesh.vertexCount = m_gridMesh.vertexCount; - m_gridClothMesh.vertices = m_gridMesh.vertices; + // Generate 2D mesh + m_rectangleGarmentMesh.Set(&m_rectangleGarment, 1.0f); - m_gridClothMesh.triangleCount = m_gridMesh.triangleCount; - m_gridClothMesh.triangles = (b3ClothMeshTriangle*)m_gridMesh.triangles; - - m_gridClothMeshMesh.vertexCount = m_gridClothMesh.vertexCount; - m_gridClothMeshMesh.startVertex = 0; + // Create 3D mesh + m_rectangleClothMesh.Set(&m_rectangleGarmentMesh); - m_gridClothMeshMesh.triangleCount = m_gridClothMesh.triangleCount; - m_gridClothMeshMesh.startTriangle = 0; + // + b3Mat33 dq = b3Mat33RotationX(0.5f * B3_PI); + for (u32 i = 0; i < m_rectangleClothMesh.vertexCount; ++i) + { + m_rectangleClothMesh.vertices[i] = dq * m_rectangleClothMesh.vertices[i]; + } - m_gridClothMesh.meshCount = 1; - m_gridClothMesh.meshes = &m_gridClothMeshMesh; - - m_gridClothMesh.sewingLineCount = 0; - m_gridClothMesh.sewingLines = nullptr; - b3ClothDef def; - def.mesh = &m_gridClothMesh; + def.mesh = &m_rectangleClothMesh; + def.radius = 0.2f; def.density = 0.2f; - def.ks = 10000.0f; - def.kd = 0.0f; - def.r = 0.2f; + def.structural = 10000.0f; m_cloth = m_world.CreateCloth(def); @@ -91,12 +85,11 @@ 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->GetParticleCount(); ++i) + for (b3Particle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext()) { - b3Particle* p = m_cloth->GetParticle(i); - if (aabb.Contains(p->position)) + if (aabb.Contains(p->GetPosition())) { - m_cloth->SetType(p, e_staticParticle); + p->SetType(e_staticParticle); } } } @@ -107,38 +100,36 @@ public: m_cloth->Apply(); + b3ClothMesh* mesh = m_cloth->GetMesh(); + b3StackArray tension; - tension.Resize(m_cloth->GetParticleCount()); - for (u32 i = 0; i < tension.Count(); ++i) + tension.Resize(mesh->vertexCount); + for (u32 i = 0; i < mesh->vertexCount; ++i) { tension[i].SetZero(); } - for (u32 i = 0; i < m_cloth->GetSpringCount(); ++i) + for (b3Force* f = m_cloth->GetForceList().m_head; f; f = f->GetNext()) { - b3Spring* s = m_cloth->GetSpring(i); + if (f->GetType() == e_springForce) + { + b3SpringForce* s = (b3SpringForce*)f; - b3Particle* p1 = s->p1; - b3Particle* p2 = s->p2; + u32 v1 = s->GetParticle1()->GetVertex(); + u32 v2 = s->GetParticle2()->GetVertex(); - u32 i1 = m_cloth->GetParticleIndex(p1); - u32 i2 = m_cloth->GetParticleIndex(p2); - - tension[i1] += s->f; - tension[i2] -= s->f; + tension[v1] += s->GetActionForce(); + tension[v2] -= s->GetActionForce(); + } } - for (u32 i = 0; i < m_gridClothMesh.triangleCount; ++i) + for (u32 i = 0; i < m_rectangleClothMesh.triangleCount; ++i) { - b3ClothMeshTriangle* t = m_gridClothMesh.triangles + i; + b3ClothMeshTriangle* t = m_rectangleClothMesh.triangles + i; - 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; + b3Vec3 v1 = mesh->vertices[t->v1]; + b3Vec3 v2 = mesh->vertices[t->v2]; + b3Vec3 v3 = mesh->vertices[t->v3]; b3Draw_draw->DrawSegment(v1, v2, b3Color_black); b3Draw_draw->DrawSegment(v2, v3, b3Color_black); @@ -183,9 +174,9 @@ public: return new TensionMapping(); } - b3GridMesh<10, 10> m_gridMesh; - b3ClothMeshMesh m_gridClothMeshMesh; - b3ClothMesh m_gridClothMesh; + b3RectangleGarment m_rectangleGarment; + b3GarmentMesh m_rectangleGarmentMesh; + b3GarmentClothMesh m_rectangleClothMesh; }; #endif \ No newline at end of file diff --git a/include/bounce/bounce.h b/include/bounce/bounce.h index e35c15a..7b60d3c 100644 --- a/include/bounce/bounce.h +++ b/include/bounce/bounce.h @@ -58,18 +58,17 @@ #include +#include +#include +#include + #include #include +#include +#include #include -//#include -//#include -//#include -//#include -//#include -//#include - #include #include diff --git a/include/bounce/dynamics/cloth/cloth.h b/include/bounce/dynamics/cloth/cloth.h index 9b210ce..3fef21b 100644 --- a/include/bounce/dynamics/cloth/cloth.h +++ b/include/bounce/dynamics/cloth/cloth.h @@ -21,153 +21,66 @@ #include #include +#include -class b3StackAllocator; class b3World; class b3Shape; +class b3Particle; +class b3Force; + +struct b3ParticleDef; +struct b3ForceDef; + struct b3ClothMesh; -// Cloth mesh definition +struct b3RayCastInput; + +struct b3ClothRayCastOutput +{ + u32 triangle; // intersected triangle + b3Vec3 point; // intersection point on surface + b3Vec3 normal; // surface normal of intersection + float32 fraction; // time of intersection on segment +}; + +// Cloth definition +// This requires defining a cloth mesh which is typically bound to a render mesh struct b3ClothDef { b3ClothDef() { mesh = nullptr; density = 0.0f; - r = 0.05f; - ks = 0.0f; - kb = 0.0f; - kd = 0.0f; + radius = 0.05f; + structural = 0.0f; + bending = 0.0f; + damping = 0.0f; } - // Cloth proxy mesh + // Cloth mesh b3ClothMesh* mesh; // 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; + float32 radius; // Cloth density in kg/m^3 float32 density; - // Streching stiffness - float32 ks; + // Structural stiffness + float32 structural; // Bending stiffness - float32 kb; + float32 bending; // Damping stiffness - float32 kd; + float32 damping; }; -// Static particle: Has zero mass, can be moved manually. -// Kinematic particle: Has zero mass, non-zero velocity, can be moved by the solver. -// Dynamic particle: Has non-zero mass, non-zero velocity determined by force, can be moved by the solver. -enum b3ParticleType -{ - e_staticParticle, - e_kinematicParticle, - e_dynamicParticle -}; - -// Read-only particle -struct b3Particle -{ - // Type - b3ParticleType type; - - // Position - b3Vec3 position; - - // Velocity - b3Vec3 velocity; - - // Applied external force - b3Vec3 force; - - // Mass - float32 mass; - - // Inverse mass - float32 invMass; - - // Radius - float32 radius; - - // User data. - void* userData; - - // Applied external translation - b3Vec3 translation; - - // Solver temp - - // Identifier - u32 solverId; - - // Solution - b3Vec3 x; -}; - -// Spring types -enum b3SpringType -{ - e_strechSpring, - e_bendSpring, -}; - -struct b3ClothSolverData; - -// Read-only spring -struct b3Spring -{ - // Solver shared - - // Spring type - b3SpringType type; - - // Particle 1 - b3Particle* p1; - - // Particle 2 - b3Particle* p2; - - // Rest length - float32 L0; - - // Structural stiffness - float32 ks; - - // Damping stiffness - float32 kd; - - // Solver temp - - // Force (f_i entry) - b3Vec3 f; - - // Jacobian (J_ii entry) - b3Mat33 Jx, Jv; - - // Initialize forces and its derivatives. - void InitializeForces(const b3ClothSolverData* data); -}; - -// Read-only body contact between a particle and a solid -struct b3BodyContact -{ - b3Particle* p1; - b3Shape* s2; - float32 s; - 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. +// A cloth represents a deformable surface as a collection of particles. +// Particles may be connected with each other. class b3Cloth { public: @@ -175,37 +88,33 @@ public: const b3World* GetWorld() const; b3World* GetWorld(); - // Return the cloth mesh used to initialize this cloth. + // Create a particle. + b3Particle* CreateParticle(const b3ParticleDef& def); + + // Destroy a particle + void DestroyParticle(b3Particle* particle); + + // Create a force. + b3Force* CreateForce(const b3ForceDef& def); + + // Destroy a force. + void DestroyForce(b3Force* force); + + // Perform a ray cast with the cloth. + bool RayCast(b3ClothRayCastOutput* output, const b3RayCastInput* input) const; + + // Perform a ray cast with a given cloth mesh triangle. + bool RayCast(b3ClothRayCastOutput* output, const b3RayCastInput* input, u32 triangleIndex) const; + + // Return the cloth mesh proxy. b3ClothMesh* GetMesh() const; - // Return the number of particles in this cloth. - u32 GetParticleCount() const; + // Return the list of particles in this cloth. + const b3List2& GetParticleList() const; - // Return the particle at a given index in this cloth. - b3Particle* GetParticle(u32 i) const; - - // Convenience function. - // Return the index of a given particle. - u32 GetParticleIndex(const b3Particle* p) const; + // Return the list of forces in this cloth. + const b3List2& GetForceList() const; - // Set the type of a given particle. - void SetType(b3Particle* p, b3ParticleType type); - - // 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); - - // Apply a translation to a given particle. - void ApplyTranslation(b3Particle* p, const b3Vec3& translation); - - // Return the number of springs in this cloth. - u32 GetSpringCount() const; - - // Return the spring at a given index in this cloth. - b3Spring* GetSpring(u32 i) const; - // Return the kinetic (or dynamic) energy in this system. float32 GetEnergy() const; @@ -228,11 +137,12 @@ private: b3Cloth(const b3ClothDef& def, b3World* world); ~b3Cloth(); - // Perform a time step. Called only inside b3World. + // Perform a time step. + // Called only inside b3World. void Step(float32 dt, const b3Vec3& gravity); // Compute mass of each particle. - void ResetMass(); + void ComputeMass(); // Update contacts. // This is where some contacts might be initiated or terminated. @@ -241,19 +151,18 @@ private: // Solve void Solve(float32 dt, const b3Vec3& gravity); - b3StackAllocator* m_allocator; - + // Proxy mesh b3ClothMesh* m_mesh; float32 m_density; - u32 m_particleCount; - b3Particle* m_particles; + // Particle pool + b3BlockPool m_particleBlocks; - u32 m_springCount; - b3Spring* m_springs; - - b3BodyContact* m_contacts; - //u32 m_contactCount; + // List of particles + b3List2 m_particleList; + + // List of forces + b3List2 m_forceList; // The parent world of this cloth. b3World* m_world; @@ -278,87 +187,14 @@ inline b3ClothMesh* b3Cloth::GetMesh() const return m_mesh; } -inline u32 b3Cloth::GetParticleCount() const +inline const b3List2& b3Cloth::GetParticleList() const { - return m_particleCount; + return m_particleList; } -inline b3Particle* b3Cloth::GetParticle(u32 i) const +inline const b3List2& b3Cloth::GetForceList() const { - B3_ASSERT(i < m_particleCount); - return m_particles + i; -} - -inline u32 b3Cloth::GetParticleIndex(const b3Particle* p) const -{ - return u32(p - m_particles); -} - -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::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 void b3Cloth::ApplyTranslation(b3Particle* p, const b3Vec3& translation) -{ - p->translation += translation; -} - -inline u32 b3Cloth::GetSpringCount() const -{ - return m_springCount; -} - -inline b3Spring* b3Cloth::GetSpring(u32 i) const -{ - B3_ASSERT(i < m_springCount); - return m_springs + i; -} - -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; + return m_forceList; } inline const b3Cloth* b3Cloth::GetNext() const diff --git a/include/bounce/dynamics/cloth/cloth_mesh.h b/include/bounce/dynamics/cloth/cloth_mesh.h index e198dd3..f25273f 100644 --- a/include/bounce/dynamics/cloth/cloth_mesh.h +++ b/include/bounce/dynamics/cloth/cloth_mesh.h @@ -41,10 +41,13 @@ struct b3ClothMeshSewingLine u32 v1, v2; }; +class b3Particle; + struct b3ClothMesh { u32 vertexCount; b3Vec3* vertices; + b3Particle** particles; u32 triangleCount; b3ClothMeshTriangle* triangles; u32 meshCount; @@ -61,8 +64,7 @@ struct b3GarmentClothMesh : public b3ClothMesh b3GarmentClothMesh(); ~b3GarmentClothMesh(); - // Maps a given garment mesh to this mesh. - // This garment mesh must be empty. + // Set this mesh from a 2D garment mesh. void Set(const b3GarmentMesh* garment); }; diff --git a/include/bounce/dynamics/cloth/cloth_solver.h b/include/bounce/dynamics/cloth/cloth_solver.h index eb3a0c4..deb91d0 100644 --- a/include/bounce/dynamics/cloth/cloth_solver.h +++ b/include/bounce/dynamics/cloth/cloth_solver.h @@ -22,23 +22,22 @@ #include #include +class b3StackAllocator; + struct b3DenseVec3; struct b3DiagMat33; -struct b3SparseMat33; -struct b3SolverSparseMat33; +struct b3SparseSymMat33; +struct b3SymMat33; -struct b3Particle; -struct b3Spring; +class b3Particle; +class b3Force; struct b3BodyContact; -class b3Shape; -class b3StackAllocator; - struct b3ClothSolverDef { b3StackAllocator* stack; u32 particleCapacity; - u32 springCapacity; + u32 forceCapacity; u32 contactCapacity; }; @@ -47,17 +46,12 @@ struct b3ClothSolverData b3Vec3* x; b3Vec3* v; b3Vec3* f; + b3SymMat33* dfdx; + b3SymMat33* dfdv; float32 dt; float32 invdt; }; -struct b3SpringForce -{ - u32 i1, i2; - b3Vec3 f; - b3Mat33 Jx, Jv; -}; - struct b3AccelerationConstraint { u32 i1; @@ -65,6 +59,45 @@ struct b3AccelerationConstraint b3Vec3 p, q, z; }; +struct b3SymMat33 +{ + b3SymMat33(b3StackAllocator* a, u32 m, u32 n) + { + allocator = a; + M = m; + N = n; + values = (b3Mat33*)b3Alloc(M * N * sizeof(b3Mat33)); + } + + ~b3SymMat33() + { + b3Free(values); + } + + b3Mat33& operator()(u32 i, u32 j) + { + return values[i * N + j]; + } + + const b3Mat33& operator()(u32 i, u32 j) const + { + return values[i * N + j]; + } + + void SetZero() + { + for (u32 v = 0; v < M * N; ++v) + { + values[v].SetZero(); + } + } + + u32 M; + u32 N; + b3Mat33* values; + b3StackAllocator* allocator; +}; + class b3ClothSolver { public: @@ -72,7 +105,7 @@ public: ~b3ClothSolver(); void Add(b3Particle* p); - void Add(b3Spring* s); + void Add(b3Force* f); void Add(b3BodyContact* c); void Solve(float32 dt, const b3Vec3& gravity); @@ -80,17 +113,20 @@ private: // Initialize forces. void InitializeForces(); + // Apply forces. + void ApplyForces(); + // Initialize constraints. void InitializeConstraints(); // Compute A and b in Ax = b - void Compute_A_b(b3SolverSparseMat33& A, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const; + void Compute_A_b(b3SparseSymMat33& A, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const; // Compute S and z. void Compute_S_z(b3DiagMat33& S, b3DenseVec3& z); // Solve Ax = b. - void Solve(b3DenseVec3& x, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const; + void Solve(b3DenseVec3& x, u32& iterations, const b3SparseSymMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const; b3StackAllocator* m_allocator; @@ -98,9 +134,9 @@ private: u32 m_particleCount; b3Particle** m_particles; - u32 m_springCapacity; - u32 m_springCount; - b3Spring** m_springs; + u32 m_forceCapacity; + u32 m_forceCount; + b3Force** m_forces; u32 m_contactCapacity; u32 m_contactCount; diff --git a/include/bounce/dynamics/cloth/force.h b/include/bounce/dynamics/cloth/force.h new file mode 100644 index 0000000..c683369 --- /dev/null +++ b/include/bounce/dynamics/cloth/force.h @@ -0,0 +1,87 @@ +/* +* 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_FORCE_H +#define B3_FORCE_H + +#include +#include + +struct b3ClothSolverData; + +class b3Particle; + +// Force types +enum b3ForceType +{ + e_springForce, +}; + +struct b3ForceDef +{ + b3ForceType type; +}; + +// +class b3Force +{ +public: + // + b3ForceType GetType() const; + + // + b3Force* GetNext(); +protected: + friend class b3List2; + friend class b3Cloth; + friend class b3ClothSolver; + friend class b3Particle; + friend class b3Force; + + static b3Force* Create(const b3ForceDef* def); + static void Destroy(b3Force* f); + + b3Force() { } + virtual ~b3Force() { } + + virtual void Initialize(const b3ClothSolverData* data) = 0; + virtual void Apply(const b3ClothSolverData* data) = 0; + + // Solver shared + + // Force type + b3ForceType m_type; + + // + b3Force* m_prev; + + // + b3Force* m_next; +}; + +inline b3ForceType b3Force::GetType() const +{ + return m_type; +} + +inline b3Force* b3Force::GetNext() +{ + return m_next; +} + +#endif \ No newline at end of file diff --git a/include/bounce/dynamics/cloth/particle.h b/include/bounce/dynamics/cloth/particle.h new file mode 100644 index 0000000..fce588e --- /dev/null +++ b/include/bounce/dynamics/cloth/particle.h @@ -0,0 +1,226 @@ +/* +* 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_PARTICLE_H +#define B3_PARTICLE_H + +#include +#include + +class b3Shape; +class b3Cloth; +class b3Particle; + +// Static particle: Can be moved manually. +// Kinematic particle: Non-zero velocity, can be moved by the solver. +// Dynamic particle: Non-zero velocity determined by force, can be moved by the solver. +enum b3ParticleType +{ + e_staticParticle, + e_kinematicParticle, + e_dynamicParticle +}; + +// +struct b3ParticleDef +{ + b3ParticleDef() + { + type = e_staticParticle; + position.SetZero(); + velocity.SetZero(); + force.SetZero(); + radius = 0.0f; + userData = nullptr; + } + + b3ParticleType type; + b3Vec3 position; + b3Vec3 velocity; + b3Vec3 force; + float32 radius; + void* userData; +}; + +// A contact between a particle and a solid +struct b3BodyContact +{ + b3Particle* p1; + b3Shape* s2; + float32 s; + b3Vec3 n, t1, t2; + float32 Fn, Ft1, Ft2; + bool n_active, t1_active, t2_active; +}; + +// A cloth particle. +class b3Particle +{ +public: + // Set the particle type. + void SetType(b3ParticleType type); + + // Get the particle type. + b3ParticleType GetType() const; + + // Get the vertex index. + u32 GetVertex() const; + + // Get the particle position. + const b3Vec3& GetPosition() const; + + // Set the particle velocity. + void SetVelocity(const b3Vec3& velocity); + + // Get the particle velocity. + const b3Vec3& GetVelocity() const; + + // Get the particle mass. + float32 GetMass() const; + + // Get the particle radius; + float32 GetRadius() const; + + // Apply a force. + void ApplyForce(const b3Vec3& force); + + // Apply a translation. + void ApplyTranslation(const b3Vec3& translation); + + // Get the next particle. + b3Particle* GetNext(); +private: + friend class b3List2; + friend class b3Cloth; + friend class b3ClothSolver; + friend class b3Force; + friend class b3SpringForce; + + b3Particle(const b3ParticleDef& def, b3Cloth* cloth); + ~b3Particle(); + + // Type + b3ParticleType m_type; + + // Position + b3Vec3 m_position; + + // Velocity + b3Vec3 m_velocity; + + // Applied external force + b3Vec3 m_force; + + // Mass + float32 m_mass; + + // Inverse mass + float32 m_invMass; + + // Radius + float32 m_radius; + + // User data. + void* m_userData; + + // Cloth mesh vertex index. + u32 m_vertex; + + // Applied external translation + b3Vec3 m_translation; + + // Contact + b3BodyContact m_contact; + + // Solver temp + + // Identifier + u32 m_solverId; + + // Solution + b3Vec3 m_x; + + // + b3Cloth* m_cloth; + + // + b3Particle* m_prev; + + // + b3Particle* m_next; +}; + +inline b3ParticleType b3Particle::GetType() const +{ + return m_type; +} + +inline u32 b3Particle::GetVertex() const +{ + return m_vertex; +} + +inline const b3Vec3& b3Particle::GetPosition() const +{ + return m_position; +} + +inline void b3Particle::SetVelocity(const b3Vec3& velocity) +{ + if (m_type == e_staticParticle) + { + return; + } + m_velocity = velocity; +} + +inline const b3Vec3& b3Particle::GetVelocity() const +{ + return m_velocity; +} + +inline float32 b3Particle::GetMass() const +{ + return m_mass; +} + +inline float32 b3Particle::GetRadius() const +{ + return m_radius; +} + +inline void b3Particle::ApplyForce(const b3Vec3& force) +{ + if (m_type != e_dynamicParticle) + { + return; + } + m_force += force; +} + +inline void b3Particle::ApplyTranslation(const b3Vec3& translation) +{ + m_translation += translation; +} + +inline b3Particle* b3Particle::GetNext() +{ + return m_next; +} + +#endif \ No newline at end of file diff --git a/include/bounce/dynamics/cloth/sparse_mat33.h b/include/bounce/dynamics/cloth/sparse_mat33.h deleted file mode 100644 index e192b3d..0000000 --- a/include/bounce/dynamics/cloth/sparse_mat33.h +++ /dev/null @@ -1,154 +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_SPARSE_MAT_33_H -#define B3_SPARSE_MAT_33_H - -#include -#include -#include - -// A static sparse matrix stored in Compressed Sparse Row (CSR) format. -// It's efficient when using in iterative solvers such as CG method, where -// the coefficient matrix must be multiplied with a vector at each iteration. -// See https://en.wikipedia.org/wiki/Sparse_matrix -struct b3SparseMat33 -{ - b3SparseMat33() { } - - b3SparseMat33(u32 _M, u32 _N, - u32 _valueCount, b3Mat33* _values, - u32* _row_ptrs, u32* _cols) - { - M = _M; - N = _N; - - values = _values; - valueCount = _valueCount; - - row_ptrs = _row_ptrs; - cols = _cols; - } - - ~b3SparseMat33() - { - - } - - // Output any given row of the original matrix. - // The given buffer must have size of greater or equal than M. - void AssembleRow(b3Mat33* out, u32 row) const; - - // Decompresses this matrix into its original form. - // The output matrix is stored in row-major order. - // The given buffer must have size of greater or equal than M * N. - void AssembleMatrix(b3Mat33* out) const; - - // Output the block diagonal part of the original matrix. - // This matrix must be a square matrix. - // The given buffer must have size of greater or equal than M. - void AssembleDiagonal(b3DiagMat33& out) const; - - // Dimensions of the original 2D matrix - u32 M; - u32 N; - - // Non-zero values - b3Mat33* values; - u32 valueCount; - - // Sparsity structure - u32* row_ptrs; // pointers to the first non-zero value of each row (size is M + 1) - u32* cols; // column indices for each non-zero value (size is valueCount) -}; - -inline void b3SparseMat33::AssembleRow(b3Mat33* out, u32 row) const -{ - B3_ASSERT(row < M + 1); - - // Start with zero row - for (u32 i = 0; i < N; ++i) - { - out[i].SetZero(); - } - - for (u32 i = row_ptrs[row]; i < row_ptrs[row + 1]; ++i) - { - u32 col = cols[i]; - - out[col] = values[i]; - } -} - -inline void b3SparseMat33::AssembleMatrix(b3Mat33* out) const -{ - for (u32 i = 0; i < M; ++i) - { - AssembleRow(out + i * N, i); - } -} - -inline void b3SparseMat33::AssembleDiagonal(b3DiagMat33& out) const -{ - B3_ASSERT(M == N); - - for (u32 row = 0; row < M; ++row) - { - out[row].SetZero(); - - for (u32 row_ptr = row_ptrs[row]; row_ptr < row_ptrs[row + 1]; ++row_ptr) - { - if (cols[row_ptr] > row) - { - break; - } - - if (cols[row_ptr] == row) - { - out[row] = values[row_ptr]; - break; - } - } - } -} - -inline void b3Mul(b3DenseVec3& out, const b3SparseMat33& A, const b3DenseVec3& v) -{ - B3_ASSERT(A.N == out.n); - - for (u32 row = 0; row < A.N; ++row) - { - out[row].SetZero(); - - for (u32 j = A.row_ptrs[row]; j < A.row_ptrs[row + 1]; ++j) - { - u32 col = A.cols[j]; - - out[row] += A.values[j] * v[col]; - } - } -} - -inline b3DenseVec3 operator*(const b3SparseMat33& A, const b3DenseVec3& v) -{ - b3DenseVec3 result(v.n); - b3Mul(result, A, v); - return result; -} - -#endif \ No newline at end of file diff --git a/include/bounce/dynamics/cloth/sparse_sym_mat33.h b/include/bounce/dynamics/cloth/sparse_sym_mat33.h new file mode 100644 index 0000000..ce21e77 --- /dev/null +++ b/include/bounce/dynamics/cloth/sparse_sym_mat33.h @@ -0,0 +1,236 @@ +/* +* 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_SPARSE_SYM_MAT_33_H +#define B3_SPARSE_SYM_MAT_33_H + +#include +#include +#include +#include + +struct b3SparseSymMat33 +{ + // + b3SparseSymMat33(b3StackAllocator* a, u32 m, u32 n); + + // + ~b3SparseSymMat33(); + + // + b3Mat33& operator()(u32 i, u32 j); + + // + const b3Mat33& operator()(u32 i, u32 j) const; + + // + void Diagonal(b3DiagMat33& out) const; + + // + + b3StackAllocator* allocator; + u32 M; + u32 N; + u32* row_ptrs; + u32 value_capacity; + u32 value_count; + b3Mat33* values; + u32* value_columns; +}; + +inline b3SparseSymMat33::b3SparseSymMat33(b3StackAllocator* a, u32 m, u32 n) +{ + B3_ASSERT(m == n); + allocator = a; + M = m; + N = n; + row_ptrs = (u32*)allocator->Allocate((M + 1) * sizeof(u32)); + memset(row_ptrs, 0, (M + 1) * sizeof(u32)); + value_count = 0; + value_capacity = n * (n + 1) / 2; + values = (b3Mat33*)allocator->Allocate(value_capacity * sizeof(b3Mat33)); + value_columns = (u32*)allocator->Allocate(value_capacity * sizeof(u32)); +} + +inline b3SparseSymMat33::~b3SparseSymMat33() +{ + allocator->Free(value_columns); + allocator->Free(values); + allocator->Free(row_ptrs); +} + +inline const b3Mat33& b3SparseSymMat33::operator()(u32 i, u32 j) const +{ + B3_ASSERT(i < M); + B3_ASSERT(j < N); + + // Ensure i, and j is on the upper triangle + if (i > j) + { + b3Swap(i, j); + } + + u32 row_value_begin = row_ptrs[i]; + u32 row_value_count = row_ptrs[i + 1] - row_value_begin; + + for (u32 row_value = 0; row_value < row_value_count; ++row_value) + { + u32 row_value_index = row_value_begin + row_value; + u32 row_value_column = value_columns[row_value_index]; + + if (row_value_column == j) + { + return values[row_value_index]; + } + } + + return b3Mat33_zero; +} + +inline b3Mat33& b3SparseSymMat33::operator()(u32 i, u32 j) +{ + B3_ASSERT(i < M); + B3_ASSERT(j < N); + + // Ensure i, and j is on the upper triangle + if (i > j) + { + b3Swap(i, j); + } + + u32 row_value_begin = row_ptrs[i]; + u32 row_value_count = row_ptrs[i + 1] - row_value_begin; + + for (u32 row_value = 0; row_value < row_value_count; ++row_value) + { + u32 row_value_index = row_value_begin + row_value; + u32 row_value_column = value_columns[row_value_index]; + + if (row_value_column == j) + { + return values[row_value_index]; + } + } + + // Insert sorted by column + u32 max_column_row_value = 0; + for (u32 row_value = 0; row_value < row_value_count; ++row_value) + { + u32 row_value_index = row_value_begin + row_value; + u32 row_value_column = value_columns[row_value_index]; + + if (row_value_column >= j) + { + max_column_row_value = row_value; + break; + } + } + + u32 max_column_row_value_index = row_value_begin + max_column_row_value; + + // Copy the values to be shifted + u32 shift_count = value_count - max_column_row_value_index; + + b3Mat33* shift_values = (b3Mat33*)allocator->Allocate(shift_count * sizeof(b3Mat33)); + memcpy(shift_values, values + max_column_row_value_index, shift_count * sizeof(b3Mat33)); + + u32* shift_value_columns = (u32*)allocator->Allocate(shift_count * sizeof(u32)); + memcpy(shift_value_columns, value_columns + max_column_row_value_index, shift_count * sizeof(u32)); + + // Insert the new value + B3_ASSERT(value_count < value_capacity); + value_columns[max_column_row_value_index] = j; + ++value_count; + + // Shift the old values + memcpy(values + max_column_row_value_index + 1, shift_values, shift_count * sizeof(b3Mat33)); + memcpy(value_columns + max_column_row_value_index + 1, shift_value_columns, shift_count * sizeof(u32)); + + allocator->Free(shift_value_columns); + allocator->Free(shift_values); + + // Shift the old row pointers as well + for (u32 row_ptr_index = i + 1; row_ptr_index < M + 1; ++row_ptr_index) + { + ++row_ptrs[row_ptr_index]; + } + + return values[max_column_row_value_index]; +} + +inline void b3SparseSymMat33::Diagonal(b3DiagMat33& out) const +{ + B3_ASSERT(N == out.n); + + for (u32 row = 0; row < M; ++row) + { + out[row].SetZero(); + + u32 row_value_begin = row_ptrs[row]; + u32 row_value_count = row_ptrs[row + 1] - row_value_begin; + + for (u32 row_value = 0; row_value < row_value_count; ++row_value) + { + u32 row_value_index = row_value_begin + row_value; + u32 row_value_column = value_columns[row_value_index]; + + if (row == row_value_column) + { + out[row] = values[row_value_index]; + break; + } + } + } +} + +inline void b3Mul(b3DenseVec3& out, const b3SparseSymMat33& A, const b3DenseVec3& v) +{ + B3_ASSERT(A.N == out.n); + + out.SetZero(); + + for (u32 row = 0; row < A.N; ++row) + { + u32 row_value_begin = A.row_ptrs[row]; + u32 row_value_count = A.row_ptrs[row + 1] - row_value_begin; + + for (u32 row_value = 0; row_value < row_value_count; ++row_value) + { + u32 row_value_index = row_value_begin + row_value; + + u32 row_value_column = A.value_columns[row_value_index]; + + out[row] += A.values[row_value_index] * v[row_value_column]; + + if (row != row_value_column) + { + // A(i, j) == A(j, i) + out[row_value_column] += A.values[row_value_index] * v[row]; + } + } + } +} + +inline b3DenseVec3 operator*(const b3SparseSymMat33& A, const b3DenseVec3& v) +{ + b3DenseVec3 result(v.n); + b3Mul(result, A, v); + return result; +} + +#endif \ No newline at end of file diff --git a/include/bounce/dynamics/cloth/spring_force.h b/include/bounce/dynamics/cloth/spring_force.h new file mode 100644 index 0000000..9e636bd --- /dev/null +++ b/include/bounce/dynamics/cloth/spring_force.h @@ -0,0 +1,140 @@ +/* +* 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_FORCE_H +#define B3_SPRING_FORCE_H + +#include + +struct b3SpringForceDef : public b3ForceDef +{ + b3SpringForceDef() + { + type = e_springForce; + p1 = nullptr; + p2 = nullptr; + restLength = 0.0f; + structural = 0.0f; + damping = 0.0f; + } + + // + void Initialize(b3Particle* particle1, b3Particle* particle2, float32 structuralStiffness, float32 dampingStiffness); + + // Particle 1 + b3Particle* p1; + + // Particle 2 + b3Particle* p2; + + // Rest length + float32 restLength; + + // Structural stiffness + float32 structural; + + // Damping stiffness + float32 damping; +}; + +// +class b3SpringForce : public b3Force +{ +public: + b3Particle* GetParticle1(); + + b3Particle* GetParticle2(); + + float32 GetRestLenght() const; + + float32 GetStructuralStiffness() const; + + float32 GetDampingStiffness() const; + + b3Vec3 GetActionForce() const; +private: + friend class b3Force; + friend class b3Cloth; + + b3SpringForce(const b3SpringForceDef* def); + ~b3SpringForce(); + + void Initialize(const b3ClothSolverData* data); + + void Apply(const b3ClothSolverData* data); + + // Solver shared + + // Particle 1 + b3Particle* m_p1; + + // Particle 2 + b3Particle* m_p2; + + // Rest length + float32 m_L0; + + // Structural stiffness + float32 m_ks; + + // Damping stiffness + float32 m_kd; + + // Solver temp + + // Force (f_1 entry) + b3Vec3 m_f; + + // Jacobian (J_11 entry) + b3Mat33 m_Jx; + + // Jacobian (J_11 entry) + b3Mat33 m_Jv; +}; + +inline b3Particle * b3SpringForce::GetParticle1() +{ + return m_p1; +} + +inline b3Particle* b3SpringForce::GetParticle2() +{ + return m_p2; +} + +inline float32 b3SpringForce::GetRestLenght() const +{ + return m_L0; +} + +inline float32 b3SpringForce::GetStructuralStiffness() const +{ + return m_ks; +} + +inline float32 b3SpringForce::GetDampingStiffness() const +{ + return m_kd; +} + +inline b3Vec3 b3SpringForce::GetActionForce() const +{ + return m_f; +} + +#endif \ No newline at end of file diff --git a/src/bounce/dynamics/cloth/cloth.cpp b/src/bounce/dynamics/cloth/cloth.cpp index a71ac22..77b4b0f 100644 --- a/src/bounce/dynamics/cloth/cloth.cpp +++ b/src/bounce/dynamics/cloth/cloth.cpp @@ -17,62 +17,24 @@ */ #include -#include -#include -#include #include -#include -#include +#include +#include +#include +#include #include +#include +#include +#include #include #include #define B3_FORCE_THRESHOLD 0.005f -#define B3_CLOTH_BENDING 0 +#define B3_CLOTH_BENDING 1 #define B3_CLOTH_FRICTION 1 -// b3Spring -void b3Spring::InitializeForces(const b3ClothSolverData* data) -{ - u32 i1 = p1->solverId; - u32 i2 = p2->solverId; - - b3Vec3 x1 = data->x[i1]; - b3Vec3 v1 = data->v[i1]; - - b3Vec3 x2 = data->x[i2]; - b3Vec3 v2 = data->v[i2]; - - b3Mat33 I; I.SetIdentity(); - - b3Vec3 dx = x1 - x2; - - if (b3Dot(dx, dx) >= L0 * L0) - { - float32 L = b3Length(dx); - b3Vec3 n = dx / L; - - // Tension - f = -ks * (L - L0) * n; - - // Jacobian - Jx = -ks * (b3Outer(dx, dx) + (1.0f - L0 / L) * (I - b3Outer(dx, dx))); - } - else - { - f.SetZero(); - Jx.SetZero(); - } - - // Damping - b3Vec3 dv = v1 - v2; - - f += -kd * dv; - Jv = -kd * I; -} - static B3_FORCE_INLINE u32 b3NextIndex(u32 i) { return i + 1 < 3 ? i + 1 : 0; @@ -191,92 +153,56 @@ static u32 b3FindSharedEdges(b3SharedEdge* sharedEdges, const b3ClothMesh* m) return sharedCount; } -b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) +b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) : m_particleBlocks(sizeof(b3Particle)) { B3_ASSERT(def.mesh); B3_ASSERT(def.density > 0.0f); m_world = world; - m_allocator = &m_world->m_stackAllocator; m_mesh = def.mesh; m_density = def.density; b3ClothMesh* m = m_mesh; // Create particles - m_particleCount = m->vertexCount; - m_particles = (b3Particle*)b3Alloc(m_particleCount * sizeof(b3Particle)); - m_contacts = (b3BodyContact*)b3Alloc(m_particleCount * sizeof(b3BodyContact)); - - for (u32 i = 0; i < m_particleCount; ++i) + for (u32 i = 0; i < m->vertexCount; ++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; + b3ParticleDef pd; + pd.type = e_dynamicParticle; + pd.position = m->vertices[i]; - p->translation.SetZero(); - p->x.SetZero(); + b3Particle* p = CreateParticle(pd); - b3BodyContact* c = m_contacts + i; - c->n_active = false; - c->t1_active = false; - c->t2_active = false; - c->Fn = 0.0f; - c->Ft1 = 0.0f; - c->Ft2 = 0.0f; + p->m_vertex = i; + m->particles[i] = p; } // Compute mass - ResetMass(); + ComputeMass(); - // Create springs - m_springCount = 0; + // Create forces + b3StackAllocator* allocator = &m_world->m_stackAllocator; + // Worst-case edge memory u32 edgeCount = 3 * m->triangleCount; - b3UniqueEdge* uniqueEdges = (b3UniqueEdge*)m_allocator->Allocate(edgeCount * sizeof(b3UniqueEdge)); + b3UniqueEdge* uniqueEdges = (b3UniqueEdge*)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)); + b3SharedEdge* sharedEdges = (b3SharedEdge*)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]; - b3Particle* p1 = m_particles + e->v1; - b3Particle* p2 = m_particles + e->v2; + b3SpringForceDef fd; + fd.Initialize(p1, p2, def.structural, def.damping); - 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; - s->f.SetZero(); + CreateForce(fd); } #if B3_CLOTH_BENDING @@ -286,112 +212,265 @@ b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) { 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]; - 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; - s->tension.SetZero(); + b3SpringForceDef fd; + fd.Initialize(p1, p2, def.bending, def.damping); + + CreateForce(fd); } - m_allocator->Free(sharedEdges); #endif - m_allocator->Free(uniqueEdges); + allocator->Free(sharedEdges); + 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; + 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; - s->f.SetZero(); + b3SpringForceDef fd; + fd.Initialize(p1, p2, def.structural, def.damping); + + CreateForce(fd); } - - B3_ASSERT(m_springCount <= springCapacity); } b3Cloth::~b3Cloth() { - b3Free(m_particles); - b3Free(m_springs); - b3Free(m_contacts); + b3Particle* p = m_particleList.m_head; + while (p) + { + b3Particle* p0 = p; + p = p->m_next; + p0->~b3Particle(); + } + + b3Force* f = m_forceList.m_head; + while (f) + { + b3Force* f0 = f; + f = f->m_next; + f0->~b3Force(); + b3Free(f0); + } } -void b3Cloth::ResetMass() +b3Particle* b3Cloth::CreateParticle(const b3ParticleDef& def) { - for (u32 i = 0; i < m_particleCount; ++i) + void* mem = m_particleBlocks.Allocate(); + b3Particle* p = new(mem) b3Particle(def, this); + m_particleList.PushFront(p); + return p; +} + +void b3Cloth::DestroyParticle(b3Particle* particle) +{ + m_particleList.Remove(particle); + particle->~b3Particle(); + m_particleBlocks.Free(particle); +} + +b3Force* b3Cloth::CreateForce(const b3ForceDef& def) +{ + b3Force* f = b3Force::Create(&def); + m_forceList.PushFront(f); + return f; +} + +void b3Cloth::DestroyForce(b3Force* force) +{ + m_forceList.Remove(force); + b3Force::Destroy(force); +} + +float32 b3Cloth::GetEnergy() const +{ + float32 E = 0.0f; + for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) { - m_particles[i].mass = 0.0f; - m_particles[i].invMass = 0.0f; + E += p->m_mass * b3Dot(p->m_velocity, p->m_velocity); + } + return 0.5f * E; +} + +void b3Cloth::ComputeMass() +{ + for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) + { + p->m_mass = 0.0f; + p->m_invMass = 0.0f; } const float32 inv3 = 1.0f / 3.0f; const float32 rho = m_density; - // Accumulate the mass about the mesh 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]; + b3Vec3 v1 = m_mesh->vertices[triangle->v1]; + b3Vec3 v2 = m_mesh->vertices[triangle->v2]; + b3Vec3 v3 = m_mesh->vertices[triangle->v3]; - float32 area = b3Area(p1, p2, p3); + float32 area = b3Area(v1, v2, v3); 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; + b3Particle* p1 = m_mesh->particles[triangle->v1]; + b3Particle* p2 = m_mesh->particles[triangle->v2]; + b3Particle* p3 = m_mesh->particles[triangle->v3]; + + p1->m_mass += inv3 * mass; + p2->m_mass += inv3 * mass; + p3->m_mass += inv3 * mass; } // Invert - for (u32 i = 0; i < m_particleCount; ++i) + for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) { - B3_ASSERT(m_particles[i].mass > 0.0f); - m_particles[i].invMass = 1.0f / m_particles[i].mass; + B3_ASSERT(p->m_mass > 0.0f); + p->m_invMass = 1.0f / p->m_mass; } } +bool b3Cloth::RayCast(b3ClothRayCastOutput* output, const b3RayCastInput* input) const +{ + output->triangle = ~0; + output->fraction = input->maxFraction; + + for (u32 i = 0; i < m_mesh->triangleCount; ++i) + { + b3ClothRayCastOutput subOutput; + if (RayCast(&subOutput, input, i)) + { + if (subOutput.fraction < output->fraction) + { + *output = subOutput; + } + } + } + + if (output->triangle != ~0) + { + return true; + } + + return false; +} + +bool b3Cloth::RayCast(b3ClothRayCastOutput* output, const b3RayCastInput* input, u32 triangleIndex) const +{ + B3_ASSERT(triangleIndex < m_mesh->triangleCount); + b3ClothMeshTriangle* triangle = m_mesh->triangles + triangleIndex; + + b3Vec3 v1 = m_mesh->vertices[triangle->v1]; + b3Vec3 v2 = m_mesh->vertices[triangle->v2]; + b3Vec3 v3 = m_mesh->vertices[triangle->v3]; + + b3Vec3 p1 = input->p1; + b3Vec3 p2 = input->p2; + float32 maxFraction = input->maxFraction; + b3Vec3 d = p2 - p1; + B3_ASSERT(b3LengthSquared(d) > B3_EPSILON * B3_EPSILON); + + b3Vec3 n = b3Cross(v2 - v1, v3 - v1); + float32 len = b3Length(n); + if (len <= B3_EPSILON) + { + return false; + } + + B3_ASSERT(len > B3_EPSILON); + n /= len; + + float32 numerator = b3Dot(n, v1 - p1); + float32 denominator = b3Dot(n, d); + + if (denominator == 0.0f) + { + return false; + } + + float32 fraction = numerator / denominator; + + // Is the intersection not on the segment? + if (fraction < 0.0f || maxFraction < fraction) + { + return false; + } + + b3Vec3 Q = p1 + fraction * d; + + 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); + + // Barycentric coordinates for Q + 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 = -B3_EPSILON; + + // Is the intersection on the triangle? + if (u > kTol && v > kTol && w > kTol) + { + output->triangle = triangleIndex; + output->fraction = fraction; + output->point = Q; + + // 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; +} + void b3Cloth::UpdateContacts() { B3_PROFILE("Update Contacts"); // Create contacts - for (u32 i = 0; i < m_particleCount; ++i) + for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) { - b3Particle* p = m_particles + i; - // Static and kinematic particles can't participate in unilateral collisions. - if (p->type != e_dynamicParticle) + if (p->m_type != e_dynamicParticle) { continue; } - b3BodyContact* c = m_contacts + i; + b3BodyContact* c = &p->m_contact; // Save the old contact b3BodyContact c0 = *c; @@ -402,8 +481,8 @@ void b3Cloth::UpdateContacts() c->t2_active = false; b3Sphere s1; - s1.vertex = p->position; - s1.radius = p->radius; + s1.vertex = p->m_position; + s1.radius = p->m_radius; // Find the deepest penetration float32 bestSeparation = 0.0f; @@ -491,7 +570,7 @@ void b3Cloth::UpdateContacts() float32 normalForce = c0.Fn; // Relative velocity - b3Vec3 dv = p->velocity; + b3Vec3 dv = p->m_velocity; b3Vec3 t1 = dv - b3Dot(dv, n) * n; if (b3Dot(t1, t1) > B3_EPSILON * B3_EPSILON) @@ -569,28 +648,28 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity) // Solve b3ClothSolverDef solverDef; - solverDef.stack = m_allocator; - solverDef.particleCapacity = m_particleCount; - solverDef.springCapacity = m_springCount; - solverDef.contactCapacity = m_particleCount; + solverDef.stack = &m_world->m_stackAllocator; + solverDef.particleCapacity = m_particleList.m_count; + solverDef.forceCapacity = m_forceList.m_count; + solverDef.contactCapacity = m_particleList.m_count; b3ClothSolver solver(solverDef); - for (u32 i = 0; i < m_particleCount; ++i) + for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) { - solver.Add(m_particles + i); + solver.Add(p); } - for (u32 i = 0; i < m_springCount; ++i) + for (b3Force* f = m_forceList.m_head; f; f = f->m_next) { - solver.Add(m_springs + i); + solver.Add(f); } - for (u32 i = 0; i < m_particleCount; ++i) + for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) { - if (m_contacts[i].n_active) + if (p->m_contact.n_active) { - solver.Add(m_contacts + i); + solver.Add(&p->m_contact); } } @@ -598,15 +677,11 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity) solver.Solve(dt, 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) + for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) { - m_particles[i].translation.SetZero(); + p->m_force.SetZero(); + p->m_translation.SetZero(); } } @@ -626,50 +701,49 @@ void b3Cloth::Step(float32 dt, const b3Vec3& gravity) void b3Cloth::Apply() const { - for (u32 i = 0; i < m_particleCount; ++i) + for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) { - m_mesh->vertices[i] = m_particles[i].position; + if (p->m_vertex != ~0) + { + m_mesh->vertices[p->m_vertex] = p->m_position; + } } } void b3Cloth::Draw() const { - for (u32 i = 0; i < m_particleCount; ++i) + for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) { - b3Particle* p = m_particles + i; - - if (p->type == e_staticParticle) + if (p->m_type == e_staticParticle) { - b3Draw_draw->DrawPoint(p->position, 4.0f, b3Color_white); + b3Draw_draw->DrawPoint(p->m_position, 4.0f, b3Color_white); } - if (p->type == e_kinematicParticle) + if (p->m_type == e_kinematicParticle) { - b3Draw_draw->DrawPoint(p->position, 4.0f, b3Color_blue); + b3Draw_draw->DrawPoint(p->m_position, 4.0f, b3Color_blue); } - if (p->type == e_dynamicParticle) + if (p->m_type == e_dynamicParticle) { - b3Draw_draw->DrawPoint(p->position, 4.0f, b3Color_green); + b3Draw_draw->DrawPoint(p->m_position, 4.0f, b3Color_green); } - b3BodyContact* c = m_contacts + i; - - if (c->n_active) + if (p->m_contact.n_active) { - b3Draw_draw->DrawSegment(p->position, p->position + c->n, b3Color_yellow); + b3Draw_draw->DrawSegment(p->m_position, p->m_position + p->m_contact.n, b3Color_yellow); } } - for (u32 i = 0; i < m_springCount; ++i) + for (b3Force* f = m_forceList.m_head; f; f = f->m_next) { - b3Spring* s = m_springs + i; - b3Particle* p1 = s->p1; - b3Particle* p2 = s->p2; - - if (s->type == e_strechSpring) + if (f->m_type == e_springForce) { - b3Draw_draw->DrawSegment(p1->position, p2->position, b3Color_black); + b3SpringForce* s = (b3SpringForce*)f; + b3Particle* p1 = s->m_p1; + b3Particle* p2 = s->m_p2; + + b3Draw_draw->DrawSegment(p1->m_position, p2->m_position, b3Color_black); } } @@ -678,23 +752,23 @@ void b3Cloth::Draw() const 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; + b3Particle* p1 = m->particles[s->v1]; + b3Particle* p2 = m->particles[s->v2]; - b3Draw_draw->DrawSegment(p1->position, p2->position, b3Color_white); + b3Draw_draw->DrawSegment(p1->m_position, p2->m_position, b3Color_white); } for (u32 i = 0; i < m->triangleCount; ++i) { b3ClothMeshTriangle* t = m->triangles + i; - b3Particle* p1 = m_particles + t->v1; - b3Particle* p2 = m_particles + t->v2; - b3Particle* p3 = m_particles + t->v3; + b3Particle* p1 = m->particles[t->v1]; + b3Particle* p2 = m->particles[t->v2]; + b3Particle* p3 = m->particles[t->v3]; - b3Vec3 v1 = p1->position; - b3Vec3 v2 = p2->position; - b3Vec3 v3 = p3->position; + b3Vec3 v1 = p1->m_position; + b3Vec3 v2 = p2->m_position; + b3Vec3 v3 = p3->m_position; b3Vec3 n1 = b3Cross(v2 - v1, v3 - v1); n1.Normalize(); diff --git a/src/bounce/dynamics/cloth/cloth_mesh.cpp b/src/bounce/dynamics/cloth/cloth_mesh.cpp index 755c9aa..f15b3e8 100644 --- a/src/bounce/dynamics/cloth/cloth_mesh.cpp +++ b/src/bounce/dynamics/cloth/cloth_mesh.cpp @@ -25,6 +25,7 @@ b3GarmentClothMesh::b3GarmentClothMesh() { vertexCount = 0; vertices = nullptr; + particles = nullptr; triangleCount = 0; triangles = nullptr; meshCount = 0; @@ -36,6 +37,7 @@ b3GarmentClothMesh::b3GarmentClothMesh() b3GarmentClothMesh::~b3GarmentClothMesh() { b3Free(vertices); + b3Free(particles); b3Free(triangles); b3Free(meshes); b3Free(sewingLines); @@ -49,6 +51,8 @@ void b3GarmentClothMesh::Set(const b3GarmentMesh* garment) vertexCount += garment->meshes[i].vertexCount; } vertices = (b3Vec3*)b3Alloc(vertexCount * sizeof(b3Vec3)); + particles = (b3Particle**)b3Alloc(vertexCount * sizeof(b3Particle*)); + memset(particles, 0, vertexCount * sizeof(b3Particle*)); B3_ASSERT(triangleCount == 0); for (u32 i = 0; i < garment->meshCount; ++i) diff --git a/src/bounce/dynamics/cloth/cloth_solver.cpp b/src/bounce/dynamics/cloth/cloth_solver.cpp index 8bf6c11..224b132 100644 --- a/src/bounce/dynamics/cloth/cloth_solver.cpp +++ b/src/bounce/dynamics/cloth/cloth_solver.cpp @@ -18,11 +18,35 @@ #include #include -#include -#include +#include +#include #include #include -#include +#include +#include + +static B3_FORCE_INLINE void b3Mul(b3DenseVec3& out, const b3SymMat33& A, const b3DenseVec3& v) +{ + B3_ASSERT(A.N == v.n); + B3_ASSERT(out.n == A.M); + + for (u32 row = 0; row < A.M; ++row) + { + out[row].SetZero(); + + for (u32 column = 0; column < A.N; ++column) + { + out[row] += A(row, column) * v[column]; + } + } +} + +static B3_FORCE_INLINE b3DenseVec3 operator*(const b3SymMat33& m, const b3DenseVec3& v) +{ + b3DenseVec3 result(v.n); + b3Mul(result, m, v); + return result; +} // Here, we solve Ax = b using the Modified Preconditioned Conjugate Gradient (MPCG) algorithm. // described in the paper: @@ -41,9 +65,9 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def) 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_forceCapacity = def.forceCapacity; + m_forceCount = 0; + m_forces = (b3Force**)m_allocator->Allocate(m_forceCapacity * sizeof(b3Force*));; m_contactCapacity = def.contactCapacity; m_contactCount = 0; @@ -58,13 +82,13 @@ b3ClothSolver::~b3ClothSolver() { m_allocator->Free(m_constraints); m_allocator->Free(m_contacts); - m_allocator->Free(m_springs); + m_allocator->Free(m_forces); m_allocator->Free(m_particles); } void b3ClothSolver::Add(b3Particle* p) { - p->solverId = m_particleCount; + p->m_solverId = m_particleCount; m_particles[m_particleCount++] = p; } @@ -73,16 +97,24 @@ void b3ClothSolver::Add(b3BodyContact* c) m_contacts[m_contactCount++] = c; } -void b3ClothSolver::Add(b3Spring* s) +void b3ClothSolver::Add(b3Force* f) { - m_springs[m_springCount++] = s; + m_forces[m_forceCount++] = f; } void b3ClothSolver::InitializeForces() { - for (u32 i = 0; i < m_springCount; ++i) + for (u32 i = 0; i < m_forceCount; ++i) { - m_springs[i]->InitializeForces(&m_solverData); + m_forces[i]->Initialize(&m_solverData); + } +} + +void b3ClothSolver::ApplyForces() +{ + for (u32 i = 0; i < m_forceCount; ++i) + { + m_forces[i]->Apply(&m_solverData); } } @@ -91,7 +123,7 @@ void b3ClothSolver::InitializeConstraints() for (u32 i = 0; i < m_particleCount; ++i) { b3Particle* p = m_particles[i]; - if (p->type != e_dynamicParticle) + if (p->m_type != e_dynamicParticle) { b3AccelerationConstraint* ac = m_constraints + m_constraintCount; ++m_constraintCount; @@ -108,7 +140,7 @@ void b3ClothSolver::InitializeConstraints() b3AccelerationConstraint* ac = m_constraints + m_constraintCount; ++m_constraintCount; - ac->i1 = p->solverId; + ac->i1 = p->m_solverId; ac->ndof = 2; ac->p = pc->n; ac->z.SetZero(); @@ -134,29 +166,6 @@ void b3ClothSolver::InitializeConstraints() } } -struct b3SolverSparseMat33 : public b3SparseMat33 -{ - b3SolverSparseMat33(b3StackAllocator* a, u32 m, u32 n) - { - allocator = a; - M = m; - N = n; - valueCount = 0; - values = (b3Mat33*)allocator->Allocate(M * N * sizeof(b3Mat33)); - cols = (u32*)allocator->Allocate(M * N * sizeof(u32)); - row_ptrs = (u32*)allocator->Allocate((M + 1) * sizeof(u32)); - } - - ~b3SolverSparseMat33() - { - allocator->Free(row_ptrs); - allocator->Free(cols); - allocator->Free(values); - } - - b3StackAllocator* allocator; -}; - void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) { B3_PROFILE("Integrate"); @@ -167,9 +176,17 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) b3DenseVec3 sy(m_particleCount); b3DenseVec3 sx0(m_particleCount); + b3SymMat33 dfdx(m_allocator, m_particleCount, m_particleCount); + dfdx.SetZero(); + + b3SymMat33 dfdv(m_allocator, m_particleCount, m_particleCount); + dfdv.SetZero(); + m_solverData.x = sx.v; m_solverData.v = sv.v; m_solverData.f = sf.v; + m_solverData.dfdx = &dfdx; + m_solverData.dfdv = &dfdv; m_solverData.dt = dt; m_solverData.invdt = 1.0f / dt; @@ -177,18 +194,18 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) { b3Particle* p = m_particles[i]; - sx[i] = p->position; - sv[i] = p->velocity; - sf[i] = p->force; + sx[i] = p->m_position; + sv[i] = p->m_velocity; + sf[i] = p->m_force; // Apply weight - if (p->type == e_dynamicParticle) + if (p->m_type == e_dynamicParticle) { - sf[i] += p->mass * gravity; + sf[i] += p->m_mass * gravity; } - sy[i] = p->translation; - sx0[i] = p->x; + sy[i] = p->m_translation; + sx0[i] = p->m_x; } // Apply contact position correction @@ -196,19 +213,14 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) { b3BodyContact* c = m_contacts[i]; b3Particle* p = c->p1; - sy[p->solverId] -= c->s * c->n; + sy[p->m_solverId] -= c->s * c->n; } - // Initialize forces + // Initialize internal forces InitializeForces(); - + // Apply internal forces - for (u32 i = 0; i < m_springCount; ++i) - { - b3Spring* s = m_springs[i]; - sf[s->p1->solverId] += s->f; - sf[s->p2->solverId] -= s->f; - } + ApplyForces(); // Initialize constraints InitializeConstraints(); @@ -223,7 +235,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) // b = h * (f0 + h * dfdx * v0 + dfdx * y) // A - b3SolverSparseMat33 A(m_allocator, m_particleCount, m_particleCount); + b3SparseSymMat33 A(m_allocator, m_particleCount, m_particleCount); // b b3DenseVec3 b(m_particleCount); @@ -247,14 +259,14 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) // 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]; + m_particles[i]->m_position = sx[i]; + m_particles[i]->m_velocity = sv[i]; } // Cache x to improve convergence for (u32 i = 0; i < m_particleCount; ++i) { - m_particles[i]->x = x[i]; + m_particles[i]->m_x = x[i]; } // Store the extra contact constraint forces that should have been @@ -269,7 +281,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) b3BodyContact* c = m_contacts[i]; b3Particle* p = c->p1; - b3Vec3 force = f[p->solverId]; + b3Vec3 force = f[p->m_solverId]; // Signed normal force magnitude c->Fn = b3Dot(force, c->n); @@ -280,152 +292,45 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) } } -#define B3_INDEX(i, j, size) (i + j * size) - -// -static void b3SetZero(b3Mat33* out, u32 size) -{ - for (u32 i = 0; i < size; ++i) - { - out[i].SetZero(); - } -} - -// dfdx * v -static void b3Mul_Jx(b3DenseVec3& out, b3Spring** springs, u32 springCount, const b3DenseVec3& v) -{ - out.SetZero(); - - for (u32 i = 0; i < springCount; ++i) - { - b3Spring* s = springs[i]; - u32 i1 = s->p1->solverId; - u32 i2 = s->p2->solverId; - - b3Mat33 J_11 = s->Jx; - 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]; - } -} - -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(b3SolverSparseMat33& SA, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const +void b3ClothSolver::Compute_A_b(b3SparseSymMat33& A, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const { float32 h = m_solverData.dt; - - // 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) - { - b3Spring* s = m_springs[i]; - u32 i1 = s->p1->solverId; - u32 i2 = s->p2->solverId; - - b3Mat33 Jx11 = s->Jx; - 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 = s->Jv; - 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; - } + b3SymMat33& dfdx = *m_solverData.dfdx; + b3SymMat33& dfdv = *m_solverData.dfdv; // 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); - + // Set the upper triangle zero + for (u32 i = 0; i < m_particleCount; ++i) + { + for (u32 j = i; j < m_particleCount; ++j) + { + A(i, j).SetZero(); + } + } + // A += M for (u32 i = 0; i < m_particleCount; ++i) { - A[B3_INDEX(i, i, m_particleCount)] += b3Diagonal(m_particles[i]->mass); + A(i, i) += b3Diagonal(m_particles[i]->m_mass); } // A += - h * dfdv - h * h * dfdx + // Set the upper triangle for (u32 i = 0; i < m_particleCount; ++i) { - for (u32 j = 0; j < m_particleCount; ++j) + for (u32 j = i; j < m_particleCount; ++j) { - A[B3_INDEX(i, j, m_particleCount)] += (-h * dfdv[B3_INDEX(i, j, m_particleCount)]) + (-h * h * dfdx[B3_INDEX(i, j, m_particleCount)]); + A(i, j) += (-h * dfdv(i, j)) + (-h * h * dfdx(i, j)); } } - // Assembly sparsity. - u32 valueCapacity = m_particleCapacity * m_particleCapacity; - - SA.row_ptrs[0] = 0; - - for (u32 i = 0; i < m_particleCount; ++i) - { - u32 rowValueCount = 0; - - for (u32 j = 0; j < m_particleCount; ++j) - { - b3Mat33 a = A[B3_INDEX(i, j, m_particleCount)]; - - if (b3IsZero(a) == false) - { - SA.values[SA.valueCount] = a; - SA.cols[SA.valueCount] = j; - ++SA.valueCount; - ++rowValueCount; - } - } - - SA.row_ptrs[i + 1] = SA.row_ptrs[(i + 1) - 1] + rowValueCount; - } - - B3_ASSERT(SA.valueCount <= valueCapacity); - - m_allocator->Free(A); - - m_allocator->Free(dfdv); - m_allocator->Free(dfdx); - // Compute b - // Jx_v = dfdx * v - b3DenseVec3 Jx_v(m_particleCount); - b3Mul_Jx(Jx_v, m_springs, m_springCount, v); - - // Jx_y = dfdx * y - b3DenseVec3 Jx_y(m_particleCount); - b3Mul_Jx(Jx_y, m_springs, m_springCount, y); - - // b = h * (f0 + h * Jx_v + Jx_y) - b = h * (f + h * Jx_v + Jx_y); + // b = h * (f0 + h * dfdx * v + dfdx * y) + b = h * (f + h * (dfdx * v) + dfdx * y); } void b3ClothSolver::Compute_S_z(b3DiagMat33& S, b3DenseVec3& z) @@ -466,13 +371,13 @@ void b3ClothSolver::Compute_S_z(b3DiagMat33& S, b3DenseVec3& z) } void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations, - const b3SparseMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const + const b3SparseSymMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const { B3_PROFILE("Solve Ax = b"); // P = diag(A) b3DiagMat33 inv_P(m_particleCount); - A.AssembleDiagonal(inv_P); + A.Diagonal(inv_P); // Invert for (u32 i = 0; i < m_particleCount; ++i) @@ -526,8 +431,14 @@ void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations, u32 iteration = 0; // Main iteration loop. - while (iteration < max_iterations) + for (;;) { + // Divergence check. + if (iteration >= max_iterations) + { + break; + } + B3_ASSERT(b3IsValid(delta_new)); // Convergence check. diff --git a/src/bounce/dynamics/cloth/force.cpp b/src/bounce/dynamics/cloth/force.cpp new file mode 100644 index 0000000..f94afed --- /dev/null +++ b/src/bounce/dynamics/cloth/force.cpp @@ -0,0 +1,62 @@ +/* +* 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 + +b3Force* b3Force::Create(const b3ForceDef* def) +{ + b3SpringForce* force = NULL; + switch (def->type) + { + case e_springForce: + { + void* block = b3Alloc(sizeof(b3SpringForce)); + force = new (block) b3SpringForce((b3SpringForceDef*)def); + break; + } + default: + { + B3_ASSERT(false); + break; + } + } + return force; +} + +void b3Force::Destroy(b3Force* force) +{ + B3_ASSERT(force); + + b3ForceType type = force->GetType(); + switch (type) + { + case e_springForce: + { + b3SpringForce* o = (b3SpringForce*)force; + o->~b3SpringForce(); + b3Free(force); + break; + } + default: + { + B3_ASSERT(false); + break; + } + }; +} \ No newline at end of file diff --git a/src/bounce/dynamics/cloth/particle.cpp b/src/bounce/dynamics/cloth/particle.cpp new file mode 100644 index 0000000..7db988d --- /dev/null +++ b/src/bounce/dynamics/cloth/particle.cpp @@ -0,0 +1,69 @@ +/* +* 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 + +b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth) +{ + m_cloth = cloth; + m_type = def.type; + m_position = def.position; + m_velocity = def.velocity; + m_force = def.force; + m_translation.SetZero(); + m_mass = 0.0f; + m_invMass = 0.0f; + m_radius = def.radius; + m_userData = nullptr; + m_x.SetZero(); + m_vertex = ~0; + + m_contact.n_active = false; + m_contact.t1_active = false; + m_contact.t2_active = false; + m_contact.Fn = 0.0f; + m_contact.Ft1 = 0.0f; + m_contact.Ft2 = 0.0f; +} + +b3Particle::~b3Particle() +{ + +} + +void b3Particle::SetType(b3ParticleType type) +{ + if (m_type == type) + { + return; + } + + m_type = type; + m_force.SetZero(); + + if (type == e_staticParticle) + { + m_velocity.SetZero(); + m_translation.SetZero(); + + m_contact.n_active = false; + m_contact.t1_active = false; + m_contact.t2_active = false; + } +} \ No newline at end of file diff --git a/src/bounce/dynamics/cloth/spring_force.cpp b/src/bounce/dynamics/cloth/spring_force.cpp new file mode 100644 index 0000000..b0c793f --- /dev/null +++ b/src/bounce/dynamics/cloth/spring_force.cpp @@ -0,0 +1,121 @@ +/* +* 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 + +void b3SpringForceDef::Initialize(b3Particle* particle1, b3Particle* particle2, float32 structuralStiffness, float32 dampingStiffness) +{ + type = e_springForce; + p1 = particle1; + p2 = particle2; + b3Vec3 x1 = p1->GetPosition(); + b3Vec3 x2 = p2->GetPosition(); + restLength = b3Distance(x1, x2); + structural = structuralStiffness; + damping = dampingStiffness; +} + +b3SpringForce::b3SpringForce(const b3SpringForceDef* def) +{ + m_type = e_springForce; + m_p1 = def->p1; + m_p2 = def->p2; + m_L0 = def->restLength; + m_ks = def->structural; + m_kd = def->damping; +} + +b3SpringForce::~b3SpringForce() +{ + +} + +void b3SpringForce::Initialize(const b3ClothSolverData* data) +{ + u32 i1 = m_p1->m_solverId; + u32 i2 = m_p2->m_solverId; + + b3Vec3 x1 = data->x[i1]; + b3Vec3 v1 = data->v[i1]; + + b3Vec3 x2 = data->x[i2]; + b3Vec3 v2 = data->v[i2]; + + b3Mat33 I; I.SetIdentity(); + + b3Vec3 dx = x1 - x2; + + float32 L = b3Length(dx); + + if (L >= m_L0) + { + b3Vec3 n = dx / L; + + // Tension + m_f = -m_ks * (L - m_L0) * n; + + // Jacobian + m_Jx = -m_ks * (b3Outer(dx, dx) + (1.0f - m_L0 / L) * (I - b3Outer(dx, dx))); + } + else + { + m_f.SetZero(); + m_Jx.SetZero(); + } + + // Damping + b3Vec3 dv = v1 - v2; + + m_f += -m_kd * dv; + m_Jv = -m_kd * I; +} + +void b3SpringForce::Apply(const b3ClothSolverData* data) +{ + b3Vec3* f = data->f; + b3SymMat33& dfdx = *data->dfdx; + b3SymMat33& dfdv = *data->dfdv; + + f[m_p1->m_solverId] += m_f; + f[m_p2->m_solverId] -= m_f; + + u32 i1 = m_p1->m_solverId; + u32 i2 = m_p2->m_solverId; + + b3Mat33 Jx11 = m_Jx; + b3Mat33 Jx12 = -Jx11; + b3Mat33 Jx21 = Jx12; + b3Mat33 Jx22 = Jx11; + + dfdx(i1, i1) += Jx11; + dfdx(i1, i2) += Jx12; + dfdx(i2, i1) += Jx21; + dfdx(i2, i2) += Jx22; + + b3Mat33 Jv11 = m_Jv; + b3Mat33 Jv12 = -Jv11; + b3Mat33 Jv21 = Jv12; + b3Mat33 Jv22 = Jv11; + + dfdv(i1, i1) += Jv11; + dfdv(i1, i2) += Jv12; + dfdv(i2, i1) += Jv21; + dfdv(i2, i2) += Jv22; +} \ No newline at end of file