From 8d2affb0b280fea2c9e1a0c2808131854e524571 Mon Sep 17 00:00:00 2001 From: Irlan <-> Date: Fri, 25 May 2018 22:15:00 -0300 Subject: [PATCH] typo, optimization, consistency --- include/bounce/dynamics/cloth/cloth.h | 19 ++- include/bounce/dynamics/cloth/cloth_solver.h | 22 ++- src/bounce/dynamics/cloth/cloth.cpp | 43 +++--- src/bounce/dynamics/cloth/cloth_solver.cpp | 151 ++++++++++++------- 4 files changed, 144 insertions(+), 91 deletions(-) diff --git a/include/bounce/dynamics/cloth/cloth.h b/include/bounce/dynamics/cloth/cloth.h index 03f579c..20dca0e 100644 --- a/include/bounce/dynamics/cloth/cloth.h +++ b/include/bounce/dynamics/cloth/cloth.h @@ -64,9 +64,9 @@ struct b3ClothDef float32 kd; }; -// Static particles have zero mass and velocity, and therefore they can't move. -// Kinematic particles are't moved by external and internal forces but can be moved by contact forces. -// Dynamic particles have non-zero mass and can move due to internal and external forces. +// 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, @@ -98,10 +98,10 @@ struct b3Particle // Radius float32 radius; - // User data + // User data. void* userData; - // Translation used for direct position manipulation + // Applied external translation b3Vec3 translation; // Solver temp @@ -125,6 +125,8 @@ struct b3ClothSolverData; // Read-only spring struct b3Spring { + // Solver shared + // Spring type b3SpringType type; @@ -151,8 +153,8 @@ struct b3Spring // Jacobian (J_ii entry) b3Mat33 Jx, Jv; - // Initialize solver data. - void Initialize(const b3ClothSolverData* data); + // Apply spring forces. + void ApplyForces(const b3ClothSolverData* data); }; // Read-only contact @@ -160,6 +162,7 @@ struct b3ParticleContact { b3Particle* p1; b3Shape* s2; + float32 s; b3Vec3 n, t1, t2; float32 Fn, Ft1, Ft2; bool n_active, t1_active, t2_active; @@ -254,8 +257,8 @@ protected: u32 m_particleCount; b3Particle* m_particles; - b3Spring* m_springs; u32 m_springCount; + b3Spring* m_springs; b3ParticleContact* m_contacts; //u32 m_contactCount; diff --git a/include/bounce/dynamics/cloth/cloth_solver.h b/include/bounce/dynamics/cloth/cloth_solver.h index 2adcd9e..52b7181 100644 --- a/include/bounce/dynamics/cloth/cloth_solver.h +++ b/include/bounce/dynamics/cloth/cloth_solver.h @@ -50,6 +50,13 @@ struct b3ClothSolverData float32 invdt; }; +struct b3AccelerationConstraint +{ + u32 i1; + u32 ndof; + b3Vec3 p, q, z; +}; + class b3ClothSolver { public: @@ -62,17 +69,16 @@ public: void Solve(float32 dt, const b3Vec3& gravity); private: + // Initialize constraints. + void InitializeConstraints(); + // Compute A and b in Ax = b void Compute_A_b(b3SparseMat33& A, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const; - // Compute S. - void Compute_S(b3DiagMat33& S); - - // Compute z. - void Compute_z(b3DenseVec3& z); + // Compute S and z. + void Compute_S_z(b3DiagMat33& S, b3DenseVec3& z); // Solve Ax = b. - // Output x and the residual error f = Ax - b ~ 0. void Solve(b3DenseVec3& x, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const; b3StackAllocator* m_allocator; @@ -89,6 +95,10 @@ private: u32 m_contactCount; b3ParticleContact** m_contacts; + u32 m_constraintCapacity; + u32 m_constraintCount; + b3AccelerationConstraint* m_constraints; + b3ClothSolverData m_solverData; }; diff --git a/src/bounce/dynamics/cloth/cloth.cpp b/src/bounce/dynamics/cloth/cloth.cpp index 214d2b4..160c723 100644 --- a/src/bounce/dynamics/cloth/cloth.cpp +++ b/src/bounce/dynamics/cloth/cloth.cpp @@ -32,7 +32,7 @@ #define B3_CLOTH_FRICTION 0 // b3Spring -void b3Spring::Initialize(const b3ClothSolverData* data) +void b3Spring::ApplyForces(const b3ClothSolverData* data) { u32 i1 = p1->solverId; u32 i2 = p2->solverId; @@ -43,7 +43,7 @@ void b3Spring::Initialize(const b3ClothSolverData* data) b3Vec3 x2 = data->x[i2]; b3Vec3 v2 = data->v[i2]; - const b3Mat33 I = b3Mat33_identity; + b3Mat33 I; I.SetIdentity(); b3Vec3 dx = x1 - x2; @@ -145,7 +145,7 @@ static u32 b3FindUniqueEdges(b3UniqueEdge* uniqueEdges, const b3ClothMesh* m) unique = false; break; } - + if (ue->v2 == t1v1 && ue->v1 == t1v2) { unique = false; @@ -238,13 +238,14 @@ void b3Cloth::Initialize(const b3ClothDef& def) const b3ClothMesh* m = m_mesh; + // Create particles m_particleCount = m->vertexCount; m_particles = (b3Particle*)b3Alloc(m_particleCount * sizeof(b3Particle)); m_contacts = (b3ParticleContact*)b3Alloc(m_particleCount * sizeof(b3ParticleContact)); for (u32 i = 0; i < m_particleCount; ++i) { - b3Particle* p = m_particles + i; + b3Particle* p = m_particles + i; p->type = e_dynamicParticle; p->position = m->vertices[i]; p->velocity.SetZero(); @@ -266,23 +267,23 @@ void b3Cloth::Initialize(const b3ClothDef& def) // Compute mass ResetMass(); - // Initialize springs + // Create springs u32 edgeCount = 3 * m->triangleCount; b3UniqueEdge* uniqueEdges = (b3UniqueEdge*)m_allocator.Allocate(edgeCount * sizeof(b3UniqueEdge)); u32 uniqueCount = b3FindUniqueEdges(uniqueEdges, m); u32 springCapacity = uniqueCount; - + #if B3_CLOTH_BENDING - + b3SharedEdge* sharedEdges = (b3SharedEdge*)m_allocator.Allocate(edgeCount * sizeof(b3SharedEdge)); u32 sharedCount = b3FindSharedEdges(sharedEdges, m); springCapacity += sharedCount; #endif - + springCapacity += m->sewingLineCount; m_springs = (b3Spring*)b3Alloc(springCapacity * sizeof(b3Spring)); @@ -340,7 +341,7 @@ void b3Cloth::Initialize(const b3ClothDef& def) 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; @@ -368,7 +369,7 @@ void b3Cloth::ResetMass() const float32 inv3 = 1.0f / 3.0f; const float32 rho = m_density; - // Accumulate the mass about the body origin of all triangles. + // 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; @@ -413,7 +414,7 @@ void b3Cloth::AddShape(b3Shape* shape) void b3Cloth::UpdateContacts() { B3_PROFILE("Update Contacts"); - + // Clear active flags for (u32 i = 0; i < m_particleCount; ++i) { @@ -477,23 +478,21 @@ void b3Cloth::UpdateContacts() b3Vec3 n = bestNormal; // Update contact manifold - // Remember the normal orientation is from shape 2 to shape 1 (mass) + // Remember the normal points from shape 2 to shape 1 (mass) c->n_active = true; c->p1 = p; c->s2 = shape; + c->s = s; c->n = n; c->t1 = b3Perp(n); c->t2 = b3Cross(c->t1, n); - - // Apply position correction - p->translation -= s * n; } // Update contact state if (c0.n_active == true && c->n_active == true) { // The contact persists - + // Has the contact constraint been satisfied? if (c0.Fn <= -B3_FORCE_THRESHOLD) { @@ -542,7 +541,7 @@ void b3Cloth::UpdateContacts() { // Create a dynamic basis t1.Normalize(); - + b3Vec3 t2 = b3Cross(t1, n); t2.Normalize(); @@ -589,7 +588,7 @@ void b3Cloth::UpdateContacts() { // The contact persists float32 maxForce = u * normalForce; - + if (Ft0[k] * Ft0[k] > maxForce * maxForce) { // Unlock particle off surface @@ -686,19 +685,19 @@ void b3Cloth::Draw() const { b3Draw_draw->DrawPoint(p->position, 4.0f, b3Color_white); } - + if (p->type == e_kinematicParticle) { b3Draw_draw->DrawPoint(p->position, 4.0f, b3Color_blue); } - + if (p->type == e_dynamicParticle) { b3Draw_draw->DrawPoint(p->position, 4.0f, b3Color_green); } b3ParticleContact* c = m_contacts + i; - + if (c->n_active) { b3Draw_draw->DrawSegment(p->position, p->position + c->n, b3Color_yellow); @@ -716,7 +715,7 @@ void b3Cloth::Draw() const b3Draw_draw->DrawSegment(p1->position, p2->position, b3Color_black); } } - + const b3ClothMesh* m = m_mesh; for (u32 i = 0; i < m->sewingLineCount; ++i) diff --git a/src/bounce/dynamics/cloth/cloth_solver.cpp b/src/bounce/dynamics/cloth/cloth_solver.cpp index a3a347c..597bc31 100644 --- a/src/bounce/dynamics/cloth/cloth_solver.cpp +++ b/src/bounce/dynamics/cloth/cloth_solver.cpp @@ -47,11 +47,16 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def) m_contactCapacity = def.contactCapacity; m_contactCount = 0; - m_contacts = (b3ParticleContact**)m_allocator->Allocate(m_contactCapacity * sizeof(b3ParticleContact*));; + m_contacts = (b3ParticleContact**)m_allocator->Allocate(m_contactCapacity * sizeof(b3ParticleContact*)); + + m_constraintCapacity = def.particleCapacity; + m_constraintCount = 0; + m_constraints = (b3AccelerationConstraint*)m_allocator->Allocate(m_constraintCapacity * sizeof(b3AccelerationConstraint)); } b3ClothSolver::~b3ClothSolver() { + m_allocator->Free(m_constraints); m_allocator->Free(m_contacts); m_allocator->Free(m_springs); m_allocator->Free(m_particles); @@ -73,6 +78,56 @@ void b3ClothSolver::Add(b3Spring* s) m_springs[m_springCount++] = s; } +void b3ClothSolver::InitializeConstraints() +{ + for (u32 i = 0; i < m_particleCount; ++i) + { + b3Particle* p = m_particles[i]; + if (p->type == e_staticParticle) + { + b3AccelerationConstraint* ac = m_constraints + m_constraintCount; + ++m_constraintCount; + ac->i1 = i; + ac->ndof = 0; + ac->z.SetZero(); + } + } + + for (u32 i = 0; i < m_contactCount; ++i) + { + b3ParticleContact* pc = m_contacts[i]; + b3Particle* p = pc->p1; + + B3_ASSERT(p->type != e_staticParticle); + + b3AccelerationConstraint* ac = m_constraints + m_constraintCount; + ++m_constraintCount; + ac->i1 = p->solverId; + ac->ndof = 2; + ac->p = pc->n; + ac->z.SetZero(); + + if (pc->t1_active && pc->t2_active) + { + ac->ndof = 0; + } + else + { + if (pc->t1_active) + { + ac->ndof = 1; + ac->q = pc->t1; + } + + if (pc->t2_active) + { + ac->ndof = 1; + ac->q = pc->t2; + } + } + } +} + static B3_FORCE_INLINE b3SparseMat33 b3AllocSparse(b3StackAllocator* allocator, u32 M, u32 N) { u32 size = M * N; @@ -124,13 +179,27 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) sx0[i] = p->x; } - // Initialize spring forces + // Apply contact position corrections + for (u32 i = 0; i < m_contactCount; ++i) + { + b3ParticleContact* c = m_contacts[i]; + b3Particle* p = c->p1; + sy[p->solverId] -= c->s * c->n; + } + + // Apply spring forces and derivatives for (u32 i = 0; i < m_springCount; ++i) { - b3Spring* s = m_springs[i]; - s->Initialize(&m_solverData); + m_springs[i]->ApplyForces(&m_solverData); } + // Initialize constraints + InitializeConstraints(); + + b3DiagMat33 S(m_particleCount); + b3DenseVec3 z(m_particleCount); + Compute_S_z(S, z); + // Solve Ax = b, where // A = M - h * dfdv - h * h * dfdx // b = h * (f0 + h * dfdx * v0 + dfdx * y) @@ -143,14 +212,6 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) Compute_A_b(A, b, sf, sx, sv, sy); - // S - b3DiagMat33 S(m_particleCount); - Compute_S(S); - - // z - b3DenseVec3 z(m_particleCount); - Compute_z(z); - // x b3DenseVec3 x(m_particleCount); @@ -379,67 +440,47 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense b = h * (f + h * Jx_v + Jx_y); } -void b3ClothSolver::Compute_z(b3DenseVec3& z) +void b3ClothSolver::Compute_S_z(b3DiagMat33& S, b3DenseVec3& z) { - // TODO + S.SetIdentity(); z.SetZero(); -} -void b3ClothSolver::Compute_S(b3DiagMat33& out) -{ - for (u32 i = 0; i < m_particleCount; ++i) + for (u32 i = 0; i < m_constraintCount; ++i) { - b3Particle* p = m_particles[i]; + b3AccelerationConstraint* ac = m_constraints + i; + u32 ip = ac->i1; + u32 ndof = ac->ndof; + b3Vec3 p = ac->p; + b3Vec3 q = ac->q; + b3Vec3 cz = ac->z; - if (p->type == e_staticParticle) + z[ip] = cz; + + if (ndof == 2) { - out[i].SetZero(); - continue; + b3Mat33 I; I.SetIdentity(); + + S[ip] = I - b3Outer(p, p); } - out[i].SetIdentity(); - } - - for (u32 i = 0; i < m_contactCount; ++i) - { - b3ParticleContact* c = m_contacts[i]; - - B3_ASSERT(c->n_active); - - b3Vec3 n = c->n; - - b3Mat33 S = b3Mat33_identity - b3Outer(n, n); - - if (c->t1_active == true && c->t2_active == true) + if (ndof == 1) { - S.SetZero(); - } - else - { - if (c->t1_active == true) - { - b3Vec3 t1 = c->t1; + b3Mat33 I; I.SetIdentity(); - S -= b3Outer(t1, t1); - } - - if (c->t2_active == true) - { - b3Vec3 t2 = c->t2; - - S -= b3Outer(t2, t2); - } + S[ip] = I - b3Outer(p, p) - b3Outer(q, q); } - b3Particle* p = c->p1; - out[p->solverId] = S; + if (ndof == 0) + { + S[ip].SetZero(); + } } } void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const { - B3_PROFILE("Solve A * x = b"); + B3_PROFILE("Solve Ax = b"); // P = diag(A) b3DiagMat33 inv_P(m_particleCount);