From 13d8415a155030138436922b4714560a37e21f8d Mon Sep 17 00:00:00 2001 From: Irlan Date: Tue, 18 Jun 2019 11:43:03 -0300 Subject: [PATCH] Support cloth self friction, thickness. Small refactor --- include/bounce/cloth/cloth.h | 24 ++- include/bounce/cloth/cloth_contact.h | 13 +- include/bounce/cloth/cloth_contact_solver.h | 10 +- include/bounce/cloth/cloth_triangle.h | 98 +++++++++++ include/bounce/cloth/particle.h | 10 +- src/bounce/cloth/cloth.cpp | 120 +++++++------- src/bounce/cloth/cloth_contact_manager.cpp | 15 +- src/bounce/cloth/cloth_contact_solver.cpp | 174 ++++++++++++++++---- src/bounce/cloth/cloth_triangle.cpp | 40 +++++ src/bounce/cloth/particle.cpp | 9 +- 10 files changed, 390 insertions(+), 123 deletions(-) create mode 100644 include/bounce/cloth/cloth_triangle.h create mode 100644 src/bounce/cloth/cloth_triangle.cpp diff --git a/include/bounce/cloth/cloth.h b/include/bounce/cloth/cloth.h index 330c842..98109b2 100644 --- a/include/bounce/cloth/cloth.h +++ b/include/bounce/cloth/cloth.h @@ -29,6 +29,8 @@ class b3World; class b3Shape; class b3Particle; +class b3ClothTriangle; + class b3Force; struct b3ParticleBodyContact; @@ -62,6 +64,8 @@ struct b3ClothDef structural = 0.0f; bending = 0.0f; damping = 0.0f; + thickness = 0.0f; + friction = 0.2f; } // Cloth mesh @@ -78,6 +82,12 @@ struct b3ClothDef // Damping stiffness float32 damping; + + // Cloth thickness + float32 thickness; + + // Cloth coefficient of friction + float32 friction; }; // A cloth represents a deformable surface as a collection of particles. @@ -126,6 +136,9 @@ public: // Return the particle associated with the given vertex. b3Particle* GetVertexParticle(u32 i); + // Return the cloth triangle given the triangle index. + b3ClothTriangle* GetTriangle(u32 i); + // Return the list of particles in this cloth. const b3List2& GetParticleList() const; @@ -142,6 +155,7 @@ public: void Draw() const; private: friend class b3Particle; + friend class b3ClothTriangle; friend class b3ClothContactManager; // Compute mass of each particle. @@ -153,12 +167,6 @@ private: // Solve void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations); - // Synchronize triangle AABB. - void SynchronizeTriangle(u32 triangleIndex); - - // Time-step - float32 m_dt; - // Stack allocator b3StackAllocator m_stackAllocator; @@ -174,8 +182,8 @@ private: // Vertex particles b3Particle** m_vertexParticles; - // Triangle proxies - b3ClothAABBProxy* m_triangleProxies; + // Triangles + b3ClothTriangle* m_triangles; // Cloth density float32 m_density; diff --git a/include/bounce/cloth/cloth_contact.h b/include/bounce/cloth/cloth_contact.h index 86646b0..6e7e291 100644 --- a/include/bounce/cloth/cloth_contact.h +++ b/include/bounce/cloth/cloth_contact.h @@ -22,8 +22,7 @@ #include class b3Particle; -struct b3ClothMeshTriangle; -struct b3ClothAABBProxy; +class b3ClothTriangle; // Contact between particle and a triangle class b3ParticleTriangleContact @@ -32,6 +31,7 @@ public: private: friend class b3Cloth; friend class b3Particle; + friend class b3ClothTriangle; friend class b3ClothContactManager; friend class b3List2; friend class b3ClothContactSolver; @@ -45,16 +45,17 @@ private: b3Particle* m_p1; // Triangle - b3ClothMeshTriangle* m_triangle; - b3ClothAABBProxy* m_triangleProxy; + b3ClothTriangle* m_t2; b3Particle* m_p2; b3Particle* m_p3; b3Particle* m_p4; - float32 m_normalImpulse; - float32 m_w2, m_w3, m_w4; + float32 m_normalImpulse; + float32 m_tangentImpulse1; + float32 m_tangentImpulse2; + bool m_active; b3ParticleTriangleContact* m_prev; diff --git a/include/bounce/cloth/cloth_contact_solver.h b/include/bounce/cloth/cloth_contact_solver.h index 27c0a46..5d69900 100644 --- a/include/bounce/cloth/cloth_contact_solver.h +++ b/include/bounce/cloth/cloth_contact_solver.h @@ -96,9 +96,17 @@ struct b3ClothSolverTriangleContactVelocityConstraint float32 wB, wC, wD; b3Vec3 normal; - float32 normalMass; float32 normalImpulse; + + float32 friction; + + b3Vec3 tangent1; + b3Vec3 tangent2; + float32 tangentMass1; + float32 tangentMass2; + float32 tangentImpulse1; + float32 tangentImpulse2; }; struct b3ClothSolverTriangleContactPositionConstraint diff --git a/include/bounce/cloth/cloth_triangle.h b/include/bounce/cloth/cloth_triangle.h new file mode 100644 index 0000000..6d4fcc3 --- /dev/null +++ b/include/bounce/cloth/cloth_triangle.h @@ -0,0 +1,98 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B3_CLOTH_TRIANGLE_H +#define B3_CLOTH_TRIANGLE_H + +#include + +// A cloth triangle +class b3ClothTriangle +{ +public: + // Return the triangle index. + u32 GetTriangle() const; + + // Set the triangle radius. + void SetRadius(float32 radius); + + // Return the triangle radius. + float32 GetRadius() const; + + // Set the triangle coefficient of friction. + void SetFriction(float32 friction); + + // Return the triangle coefficient of friction. + float32 GetFriction() const; +private: + friend class b3Cloth; + friend class b3Particle; + friend class b3ClothContactManager; + friend class b3ParticleTriangleContact; + friend class b3ClothSolver; + friend class b3ClothContactSolver; + + b3ClothTriangle() { } + ~b3ClothTriangle() { } + + // Synchronize AABB + void Synchronize(const b3Vec3& displacement); + + // Cloth + b3Cloth* m_cloth; + + // Triangle index + u32 m_triangle; + + // Radius + float32 m_radius; + + // Coefficient of friction + float32 m_friction; + + // AABB Proxy + b3ClothAABBProxy m_aabbProxy; +}; + +inline u32 b3ClothTriangle::GetTriangle() const +{ + return m_triangle; +} + +inline void b3ClothTriangle::SetRadius(float32 radius) +{ + m_radius = radius; + Synchronize(b3Vec3_zero); +} + +inline float32 b3ClothTriangle::GetRadius() const +{ + return m_radius; +} + +inline void b3ClothTriangle::SetFriction(float32 friction) +{ + m_friction = friction; +} + +inline float32 b3ClothTriangle::GetFriction() const +{ + return m_friction; +} + +#endif \ No newline at end of file diff --git a/include/bounce/cloth/particle.h b/include/bounce/cloth/particle.h index bdef984..9adbf2c 100644 --- a/include/bounce/cloth/particle.h +++ b/include/bounce/cloth/particle.h @@ -103,7 +103,6 @@ struct b3ClothAABBProxy { b3ClothAABBProxyType type; void* data; - u32 index; u32 broadPhaseId; }; @@ -160,6 +159,7 @@ public: private: friend class b3List2; friend class b3Cloth; + friend class b3ClothTriangle; friend class b3ClothSolver; friend class b3ClothContactManager; friend class b3ParticleTriangleContact; @@ -171,7 +171,7 @@ private: ~b3Particle(); // Synchronize particle AABB - void Synchronize(); + void Synchronize(const b3Vec3& displacement); // Synchronize triangles AABB void SynchronizeTriangles(); @@ -246,10 +246,12 @@ inline u32 b3Particle::GetVertex() const inline void b3Particle::SetPosition(const b3Vec3& position) { + b3Vec3 displacement = position - m_position; + m_position = position; m_translation.SetZero(); - Synchronize(); + Synchronize(displacement); SynchronizeTriangles(); } @@ -281,7 +283,7 @@ inline void b3Particle::SetRadius(float32 radius) { m_radius = radius; - Synchronize(); + Synchronize(b3Vec3_zero); } inline float32 b3Particle::GetRadius() const diff --git a/src/bounce/cloth/cloth.cpp b/src/bounce/cloth/cloth.cpp index 3c66427..13aa712 100644 --- a/src/bounce/cloth/cloth.cpp +++ b/src/bounce/cloth/cloth.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -161,11 +162,10 @@ b3Cloth::b3Cloth(const b3ClothDef& def) : m_mesh = def.mesh; m_density = def.density; m_contactManager.m_cloth = this; - m_dt = 0.0f; const b3ClothMesh* m = m_mesh; - // Create particles + // Initialize particles m_vertexParticles = (b3Particle**)b3Alloc(m->vertexCount * sizeof(b3Particle*)); for (u32 i = 0; i < m->vertexCount; ++i) { @@ -176,7 +176,6 @@ b3Cloth::b3Cloth(const b3ClothDef& def) : b3Particle* p = CreateParticle(pd); - p->m_aabbProxy.index = i; p->m_vertex = i; m_vertexParticles[i] = p; } @@ -184,7 +183,32 @@ b3Cloth::b3Cloth(const b3ClothDef& def) : // Compute mass ComputeMass(); - // Create forces + // Initialize triangles + m_triangles = (b3ClothTriangle*)b3Alloc(m_mesh->triangleCount * sizeof(b3ClothTriangle)); + for (u32 i = 0; i < m_mesh->triangleCount; ++i) + { + b3ClothMeshTriangle* meshTriangle = m_mesh->triangles + i; + b3ClothTriangle* triangle = m_triangles + i; + + triangle->m_cloth = this; + triangle->m_radius = def.thickness; + triangle->m_friction = def.friction; + triangle->m_triangle = i; + + b3Vec3 v1 = m_mesh->vertices[meshTriangle->v1]; + b3Vec3 v2 = m_mesh->vertices[meshTriangle->v2]; + b3Vec3 v3 = m_mesh->vertices[meshTriangle->v3]; + + b3AABB3 aabb; + aabb.Set(v1, v2, v3); + aabb.Extend(triangle->m_radius); + + triangle->m_aabbProxy.type = e_triangleProxy; + triangle->m_aabbProxy.data = triangle; + triangle->m_aabbProxy.broadPhaseId = m_contactManager.m_broadPhase.CreateProxy(aabb, &triangle->m_aabbProxy); + } + + // Initialize forces b3StackAllocator* allocator = &m_stackAllocator; // Worst-case edge memory @@ -251,32 +275,15 @@ b3Cloth::b3Cloth(const b3ClothDef& def) : } } - // Initialize triangle proxies - m_triangleProxies = (b3ClothAABBProxy*)b3Alloc(m_mesh->triangleCount * sizeof(b3ClothAABBProxy)); - for (u32 i = 0; i < m_mesh->triangleCount; ++i) - { - b3ClothMeshTriangle* triangle = m_mesh->triangles + i; - b3ClothAABBProxy* triangleProxy = m_triangleProxies + i; - - b3Vec3 v1 = m_mesh->vertices[triangle->v1]; - b3Vec3 v2 = m_mesh->vertices[triangle->v2]; - b3Vec3 v3 = m_mesh->vertices[triangle->v3]; - - b3AABB3 aabb; - aabb.Set(v1, v2, v3); - - triangleProxy->type = e_triangleProxy; - triangleProxy->index = i; - triangleProxy->data = triangle; - triangleProxy->broadPhaseId = m_contactManager.m_broadPhase.CreateProxy(aabb, triangleProxy); - } - m_gravity.SetZero(); m_world = nullptr; } b3Cloth::~b3Cloth() { + b3Free(m_vertexParticles); + b3Free(m_triangles); + b3Particle* p = m_particleList.m_head; while (p) { @@ -285,9 +292,6 @@ b3Cloth::~b3Cloth() p0->~b3Particle(); } - b3Free(m_vertexParticles); - b3Free(m_triangleProxies); - b3Force* f = m_forceList.m_head; while (f) { @@ -307,7 +311,6 @@ b3Particle* b3Cloth::CreateParticle(const b3ParticleDef& def) p->m_aabbProxy.type = e_particleProxy; p->m_aabbProxy.data = p; - p->m_aabbProxy.index = p->m_vertex; p->m_aabbProxy.broadPhaseId = m_contactManager.m_broadPhase.CreateProxy(aabb, &p->m_aabbProxy); m_particleList.PushFront(p); @@ -373,6 +376,12 @@ b3Particle* b3Cloth::GetVertexParticle(u32 i) return m_vertexParticles[i]; } +b3ClothTriangle* b3Cloth::GetTriangle(u32 i) +{ + B3_ASSERT(i < m_mesh->triangleCount); + return m_triangles + i; +} + void b3Cloth::ComputeMass() { for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) @@ -428,7 +437,8 @@ struct b3ClothRayCastSingleCallback return input.maxFraction; } - u32 triangleIndex = proxy->index; + b3ClothTriangle* triangle = (b3ClothTriangle*)proxy->data; + u32 triangleIndex = triangle->GetTriangle(); b3RayCastOutput subOutput; if (cloth->RayCast(&subOutput, &input, triangleIndex)) @@ -436,7 +446,7 @@ struct b3ClothRayCastSingleCallback // Ray hits triangle. if (subOutput.fraction < output0.fraction) { - triangle0 = proxy->index; + triangle0 = triangleIndex; output0.fraction = subOutput.fraction; output0.normal = subOutput.normal; } @@ -638,8 +648,6 @@ void b3Cloth::Step(float32 dt, u32 velocityIterations, u32 positionIterations) { B3_PROFILE("Cloth Step"); - m_dt = dt; - // Update particle-body contacts UpdateParticleBodyContacts(); @@ -662,13 +670,29 @@ void b3Cloth::Step(float32 dt, u32 velocityIterations, u32 positionIterations) // Synchronize particles for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) { - p->Synchronize(); + b3Vec3 displacement = dt * p->m_velocity; + + p->Synchronize(displacement); } // Synchronize triangles for (u32 i = 0; i < m_mesh->triangleCount; ++i) { - SynchronizeTriangle(i); + b3ClothMeshTriangle* triangle = m_mesh->triangles + i; + + b3Particle* p1 = m_vertexParticles[triangle->v1]; + b3Particle* p2 = m_vertexParticles[triangle->v2]; + b3Particle* p3 = m_vertexParticles[triangle->v3]; + + b3Vec3 v1 = p1->m_velocity; + b3Vec3 v2 = p2->m_velocity; + b3Vec3 v3 = p3->m_velocity; + + b3Vec3 velocity = (v1 + v2 + v3) / 3.0f; + + b3Vec3 displacement = dt * velocity; + + m_triangles[i].Synchronize(displacement); } #if B3_ENABLE_SELF_COLLISION @@ -679,34 +703,6 @@ void b3Cloth::Step(float32 dt, u32 velocityIterations, u32 positionIterations) #endif } -void b3Cloth::SynchronizeTriangle(u32 triangleIndex) -{ - b3ClothMeshTriangle* triangle = m_mesh->triangles + triangleIndex; - b3ClothAABBProxy* triangleProxy = m_triangleProxies + triangleIndex; - - b3Particle* p1 = m_vertexParticles[triangle->v1]; - b3Particle* p2 = m_vertexParticles[triangle->v2]; - b3Particle* p3 = m_vertexParticles[triangle->v3]; - - b3Vec3 x1 = p1->m_position; - b3Vec3 x2 = p2->m_position; - b3Vec3 x3 = p3->m_position; - - b3Vec3 v1 = p1->m_velocity; - b3Vec3 v2 = p2->m_velocity; - b3Vec3 v3 = p3->m_velocity; - - b3AABB3 aabb; - aabb.Set(x1, x2, x3); - - const float32 kInv3 = 1.0f / 3.0f; - - b3Vec3 center_velocity = kInv3 * (v1 + v2 + v3); - b3Vec3 center_displacement = m_dt * center_velocity; - - m_contactManager.m_broadPhase.MoveProxy(triangleProxy->broadPhaseId, aabb, center_displacement); -} - void b3Cloth::Draw() const { for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) diff --git a/src/bounce/cloth/cloth_contact_manager.cpp b/src/bounce/cloth/cloth_contact_manager.cpp index 33d70d7..eef9ae1 100644 --- a/src/bounce/cloth/cloth_contact_manager.cpp +++ b/src/bounce/cloth/cloth_contact_manager.cpp @@ -20,6 +20,7 @@ #include #include #include +#include b3ClothContactManager::b3ClothContactManager() : m_particleTriangleContactBlocks(sizeof(b3ParticleTriangleContact)) @@ -62,7 +63,8 @@ void b3ClothContactManager::AddPair(void* data1, void* data2) b3Particle* p1 = (b3Particle*)proxy1->data; - b3ClothMeshTriangle* triangle = (b3ClothMeshTriangle*)proxy2->data; + b3ClothTriangle* t2 = (b3ClothTriangle*)proxy2->data; + b3ClothMeshTriangle* triangle = m_cloth->m_mesh->triangles + t2->m_triangle; b3Particle* p2 = m_cloth->m_vertexParticles[triangle->v1]; b3Particle* p3 = m_cloth->m_vertexParticles[triangle->v2]; b3Particle* p4 = m_cloth->m_vertexParticles[triangle->v3]; @@ -70,7 +72,7 @@ void b3ClothContactManager::AddPair(void* data1, void* data2) // Check if there is a contact between the two entities. for (b3ParticleTriangleContact* c = m_particleTriangleContactList.m_head; c; c = c->m_next) { - if (c->m_p1 == p1 && c->m_triangle == triangle) + if (c->m_p1 == p1 && c->m_t2 == t2) { // A contact already exists. return; @@ -86,7 +88,7 @@ void b3ClothContactManager::AddPair(void* data1, void* data2) return; } - if (triangle->v1 == p1->m_vertex || triangle->v2 == p1->m_vertex || triangle->v3 == p1->m_vertex) + if (p1 == p2 || p1 == p3 || p1 == p4) { // The entities must not collide with each other. return; @@ -96,12 +98,13 @@ void b3ClothContactManager::AddPair(void* data1, void* data2) b3ParticleTriangleContact* c = CreateParticleTriangleContact(); c->m_p1 = p1; - c->m_triangle = triangle; - c->m_triangleProxy = proxy2; + c->m_t2 = t2; c->m_p2 = p2; c->m_p3 = p3; c->m_p4 = p4; c->m_normalImpulse = 0.0f; + c->m_tangentImpulse1 = 0.0f; + c->m_tangentImpulse2 = 0.0f; c->m_active = false; // Add the contact to the cloth contact list. @@ -144,7 +147,7 @@ void b3ClothContactManager::UpdateContacts() } u32 proxy1 = c->m_p1->m_aabbProxy.broadPhaseId; - u32 proxy2 = c->m_triangleProxy->broadPhaseId; + u32 proxy2 = c->m_t2->m_aabbProxy.broadPhaseId; // Destroy the contact if primitive AABBs are not overlapping. bool overlap = m_broadPhase.TestOverlap(proxy1, proxy2); diff --git a/src/bounce/cloth/cloth_contact_solver.cpp b/src/bounce/cloth/cloth_contact_solver.cpp index b713c4f..d736e7c 100644 --- a/src/bounce/cloth/cloth_contact_solver.cpp +++ b/src/bounce/cloth/cloth_contact_solver.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -214,7 +215,7 @@ void b3ClothContactSolver::InitializeTriangleContactConstraints() pc->indexD = c->m_p4->m_solverId; pc->invMassD = c->m_p4->m_type == e_staticParticle ? 0.0f : c->m_p4->m_invMass; - pc->triangleRadius = 0.0f; + pc->triangleRadius = c->m_t2->m_radius; pc->wB = c->m_w2; pc->wC = c->m_w3; @@ -255,6 +256,14 @@ void b3ClothContactSolver::InitializeTriangleContactConstraints() vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f; vc->normalImpulse = c->m_normalImpulse; + + vc->friction = b3MixFriction(c->m_p1->m_friction, c->m_t2->m_friction); + vc->tangent1 = b3Perp(n); + vc->tangent2 = b3Cross(vc->tangent1, n); + vc->tangentMass1 = vc->normalMass; + vc->tangentMass2 = vc->normalMass; + vc->tangentImpulse1 = c->m_tangentImpulse1; + vc->tangentImpulse2 = c->m_tangentImpulse2; } } @@ -328,21 +337,57 @@ void b3ClothContactSolver::WarmStartTriangleContactConstraints() float32 mC = vc->invMassC; float32 mD = vc->invMassD; - b3Vec3 JA = -vc->normal; - b3Vec3 JB = vc->wB * vc->normal; - b3Vec3 JC = vc->wC * vc->normal; - b3Vec3 JD = vc->wD * vc->normal; + { + b3Vec3 JA = -vc->normal; + b3Vec3 JB = vc->wB * vc->normal; + b3Vec3 JC = vc->wC * vc->normal; + b3Vec3 JD = vc->wD * vc->normal; - b3Vec3 PA = vc->normalImpulse * JA; - b3Vec3 PB = vc->normalImpulse * JB; - b3Vec3 PC = vc->normalImpulse * JC; - b3Vec3 PD = vc->normalImpulse * JD; + b3Vec3 PA = vc->normalImpulse * JA; + b3Vec3 PB = vc->normalImpulse * JB; + b3Vec3 PC = vc->normalImpulse * JC; + b3Vec3 PD = vc->normalImpulse * JD; - vA += mA * PA; - vB += mB * PB; - vC += mC * PC; - vD += mD * PD; + vA += mA * PA; + vB += mB * PB; + vC += mC * PC; + vD += mD * PD; + } + { + b3Vec3 JA = -vc->tangent1; + b3Vec3 JB = vc->wB * vc->tangent1; + b3Vec3 JC = vc->wC * vc->tangent1; + b3Vec3 JD = vc->wD * vc->tangent1; + + b3Vec3 PA = vc->tangentImpulse1 * JA; + b3Vec3 PB = vc->tangentImpulse1 * JB; + b3Vec3 PC = vc->tangentImpulse1 * JC; + b3Vec3 PD = vc->tangentImpulse1 * JD; + + vA += mA * PA; + vB += mB * PB; + vC += mC * PC; + vD += mD * PD; + } + + { + b3Vec3 JA = -vc->tangent2; + b3Vec3 JB = vc->wB * vc->tangent2; + b3Vec3 JC = vc->wC * vc->tangent2; + b3Vec3 JD = vc->wD * vc->tangent2; + + b3Vec3 PA = vc->tangentImpulse2 * JA; + b3Vec3 PB = vc->tangentImpulse2 * JB; + b3Vec3 PC = vc->tangentImpulse2 * JC; + b3Vec3 PD = vc->tangentImpulse2 * JD; + + vA += mA * PA; + vB += mB * PB; + vC += mC * PC; + vD += mD * PD; + } + v[indexA] = vA; v[indexB] = vB; v[indexC] = vC; @@ -467,30 +512,95 @@ void b3ClothContactSolver::SolveTriangleContactVelocityConstraints() b3Vec3 vC = v[indexC]; b3Vec3 vD = v[indexD]; - b3Vec3 vCB = wB * vB + wC * vC + wD * vD; + // Solve normal constraint. + { + b3Vec3 vCB = wB * vB + wC * vC + wD * vD; - float32 Cdot = b3Dot(vCB - vA, vc->normal); + float32 Cdot = b3Dot(vCB - vA, vc->normal); - float32 impulse = -vc->normalMass * Cdot; + float32 impulse = -vc->normalMass * Cdot; - float32 oldImpulse = vc->normalImpulse; - vc->normalImpulse = b3Max(vc->normalImpulse + impulse, 0.0f); - impulse = vc->normalImpulse - oldImpulse; + float32 oldImpulse = vc->normalImpulse; + vc->normalImpulse = b3Max(vc->normalImpulse + impulse, 0.0f); + impulse = vc->normalImpulse - oldImpulse; - b3Vec3 JA = -vc->normal; - b3Vec3 JB = wB * vc->normal; - b3Vec3 JC = wC * vc->normal; - b3Vec3 JD = wD * vc->normal; + b3Vec3 JA = -vc->normal; + b3Vec3 JB = wB * vc->normal; + b3Vec3 JC = wC * vc->normal; + b3Vec3 JD = wD * vc->normal; - b3Vec3 PA = impulse * JA; - b3Vec3 PB = impulse * JB; - b3Vec3 PC = impulse * JC; - b3Vec3 PD = impulse * JD; + b3Vec3 PA = impulse * JA; + b3Vec3 PB = impulse * JB; + b3Vec3 PC = impulse * JC; + b3Vec3 PD = impulse * JD; - vA += mA * PA; - vB += mB * PB; - vC += mC * PC; - vD += mD * PD; + vA += mA * PA; + vB += mB * PB; + vC += mC * PC; + vD += mD * PD; + } + + // Solve tangent constraint. + { + float32 hi = vc->friction * vc->normalImpulse; + float32 lo = -hi; + + b3Vec3 vCB = wB * vB + wC * vC + wD * vD; + + float32 Cdot = b3Dot(vCB - vA, vc->tangent1); + + float32 impulse = -vc->tangentMass1 * Cdot; + + float32 oldImpulse = vc->tangentImpulse1; + vc->tangentImpulse1 = b3Clamp(vc->tangentImpulse1 + impulse, lo, hi); + impulse = vc->tangentImpulse1 - oldImpulse; + + b3Vec3 JA = -vc->tangent1; + b3Vec3 JB = wB * vc->tangent1; + b3Vec3 JC = wC * vc->tangent1; + b3Vec3 JD = wD * vc->tangent1; + + b3Vec3 PA = impulse * JA; + b3Vec3 PB = impulse * JB; + b3Vec3 PC = impulse * JC; + b3Vec3 PD = impulse * JD; + + vA += mA * PA; + vB += mB * PB; + vC += mC * PC; + vD += mD * PD; + } + + // Solve bitangent constraint. + { + float32 hi = vc->friction * vc->normalImpulse; + float32 lo = -hi; + + b3Vec3 vCB = wB * vB + wC * vC + wD * vD; + + float32 Cdot = b3Dot(vCB - vA, vc->tangent2); + + float32 impulse = -vc->tangentMass2 * Cdot; + + float32 oldImpulse = vc->tangentImpulse2; + vc->tangentImpulse2 = b3Clamp(vc->tangentImpulse2 + impulse, lo, hi); + impulse = vc->tangentImpulse2 - oldImpulse; + + b3Vec3 JA = -vc->tangent2; + b3Vec3 JB = wB * vc->tangent2; + b3Vec3 JC = wC * vc->tangent2; + b3Vec3 JD = wD * vc->tangent2; + + b3Vec3 PA = impulse * JA; + b3Vec3 PB = impulse * JB; + b3Vec3 PC = impulse * JC; + b3Vec3 PD = impulse * JD; + + vA += mA * PA; + vB += mB * PB; + vC += mC * PC; + vD += mD * PD; + } v[indexA] = vA; v[indexB] = vB; @@ -516,6 +626,8 @@ void b3ClothContactSolver::StoreImpulses() b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i; c->m_normalImpulse = vc->normalImpulse; + c->m_tangentImpulse1 = vc->tangentImpulse1; + c->m_tangentImpulse2 = vc->tangentImpulse2; } } diff --git a/src/bounce/cloth/cloth_triangle.cpp b/src/bounce/cloth/cloth_triangle.cpp new file mode 100644 index 0000000..f4776f5 --- /dev/null +++ b/src/bounce/cloth/cloth_triangle.cpp @@ -0,0 +1,40 @@ +/* +* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include + +void b3ClothTriangle::Synchronize(const b3Vec3& displacement) +{ + b3ClothMeshTriangle* triangle = m_cloth->m_mesh->triangles + m_triangle; + + b3Particle* p1 = m_cloth->m_vertexParticles[triangle->v1]; + b3Particle* p2 = m_cloth->m_vertexParticles[triangle->v2]; + b3Particle* p3 = m_cloth->m_vertexParticles[triangle->v3]; + + b3Vec3 x1 = p1->m_position; + b3Vec3 x2 = p2->m_position; + b3Vec3 x3 = p3->m_position; + + b3AABB3 aabb; + aabb.Set(x1, x2, x3); + aabb.Extend(m_radius); + + m_cloth->m_contactManager.m_broadPhase.MoveProxy(m_aabbProxy.broadPhaseId, aabb, displacement); +} \ No newline at end of file diff --git a/src/bounce/cloth/particle.cpp b/src/bounce/cloth/particle.cpp index 5eeddb0..b6caf0b 100644 --- a/src/bounce/cloth/particle.cpp +++ b/src/bounce/cloth/particle.cpp @@ -19,6 +19,7 @@ #include #include #include +#include void b3ParticleBodyContactWorldPoint::Initialize(const b3ParticleBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB) { @@ -70,13 +71,11 @@ b3Particle::~b3Particle() } -void b3Particle::Synchronize() +void b3Particle::Synchronize(const b3Vec3& displacement) { b3AABB3 aabb; aabb.Set(m_position, m_radius); - b3Vec3 displacement = m_cloth->m_dt * m_velocity; - m_cloth->m_contactManager.m_broadPhase.MoveProxy(m_aabbProxy.broadPhaseId, aabb, displacement); } @@ -93,7 +92,7 @@ void b3Particle::SynchronizeTriangles() if (triangle->v1 == m_vertex || triangle->v2 == m_vertex || triangle->v3 == m_vertex) { - m_cloth->SynchronizeTriangle(i); + m_cloth->GetTriangle(i)->Synchronize(b3Vec3_zero); } } } @@ -134,7 +133,7 @@ void b3Particle::SetType(b3ParticleType type) m_velocity.SetZero(); m_translation.SetZero(); - Synchronize(); + Synchronize(b3Vec3_zero); SynchronizeTriangles(); }