diff --git a/include/bounce/dynamics/cloth/particle.h b/include/bounce/dynamics/cloth/particle.h index 253d350..894537c 100644 --- a/include/bounce/dynamics/cloth/particle.h +++ b/include/bounce/dynamics/cloth/particle.h @@ -89,8 +89,8 @@ public: b3Shape* s2; float32 s; - bool f1_active, f2_active; - b3FrictionForce f1, f2; + bool f_active; + b3FrictionForce f; bool n_active, t1_active, t2_active; b3Vec3 n, t1, t2; diff --git a/include/bounce/dynamics/cloth/sparse_sym_mat33.h b/include/bounce/dynamics/cloth/sparse_sym_mat33.h index e4e06aa..45c5a57 100644 --- a/include/bounce/dynamics/cloth/sparse_sym_mat33.h +++ b/include/bounce/dynamics/cloth/sparse_sym_mat33.h @@ -85,6 +85,11 @@ inline const b3Mat33& b3SparseSymMat33::operator()(u32 i, u32 j) const u32 row_value_index = row_value_begin + row_value; u32 row_value_column = value_columns[row_value_index]; + if (row_value_column < j) + { + break; + } + if (row_value_column == j) { return values[row_value_index]; @@ -112,7 +117,12 @@ inline b3Mat33& b3SparseSymMat33::operator()(u32 i, u32 j) { u32 row_value_index = row_value_begin + row_value; u32 row_value_column = value_columns[row_value_index]; - + + if (row_value_column < j) + { + break; + } + if (row_value_column == j) { return values[row_value_index]; @@ -120,30 +130,30 @@ inline b3Mat33& b3SparseSymMat33::operator()(u32 i, u32 j) } // Find insert position - u32 row_value_k = 0; + u32 row_value_position = 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) + if (row_value_column > j) { - row_value_k = row_value; + row_value_position = row_value; break; } } // Shift the values B3_ASSERT(value_count < value_capacity); - for (u32 row_value = value_count; row_value > row_value_begin + row_value_k; --row_value) + for (u32 row_value = value_count; row_value > row_value_begin + row_value_position; --row_value) { values[row_value] = values[row_value - 1]; value_columns[row_value] = value_columns[row_value - 1]; } // Insert the value - value_columns[row_value_begin + row_value_k] = j; - values[row_value_begin + row_value_k].SetZero(); + value_columns[row_value_begin + row_value_position] = j; + values[row_value_begin + row_value_position].SetZero(); ++value_count; // Shift the row pointers @@ -153,7 +163,7 @@ inline b3Mat33& b3SparseSymMat33::operator()(u32 i, u32 j) } // Return the inserted value - return values[row_value_begin + row_value_k]; + return values[row_value_begin + row_value_position]; } inline void b3SparseSymMat33::Diagonal(b3DiagMat33& out) const diff --git a/src/bounce/dynamics/cloth/cloth.cpp b/src/bounce/dynamics/cloth/cloth.cpp index e2e2580..e4ff249 100644 --- a/src/bounce/dynamics/cloth/cloth.cpp +++ b/src/bounce/dynamics/cloth/cloth.cpp @@ -483,8 +483,7 @@ void b3Cloth::UpdateContacts() b3BodyContact c0 = *c; // Create a new contact - c->f1_active = false; - c->f2_active = false; + c->f_active = false; c->n_active = false; c->t1_active = false; c->t2_active = false; @@ -591,6 +590,11 @@ void b3Cloth::UpdateContacts() c->t1 = t1; c->t2 = t2; + + //c->f_active = true; + //c->f.m_type = e_frictionForce; + //c->f.m_p = p; + //c->f.m_kd = 10.0f; } b3Vec3 ts[2]; @@ -609,18 +613,6 @@ void b3Cloth::UpdateContacts() Ft0[0] = c0.Ft1; Ft0[1] = c0.Ft2; - bool f_active[2]; - f_active[0] = c->f1_active; - f_active[1] = c->f2_active; - - bool f_active0[2]; - f_active0[0] = c0.f1_active; - f_active0[1] = c0.f2_active; - - b3FrictionForce* sf[2]; - sf[0] = &c->f1; - sf[1] = &c->f2; - for (u32 k = 0; k < 2; ++k) { b3Vec3 t = ts[k]; @@ -633,30 +625,8 @@ void b3Cloth::UpdateContacts() // Lock particle on surface t_active[k] = true; } - - if (t_active0[k] == true && t_active[k] == true) - { - float32 frictionForce = Ft0[k]; - - // Dynamic friction - if (frictionForce * frictionForce > maxFrictionForce * maxFrictionForce) - { - // Unlock particle off surface - //t_active[k] = false; - - // Apply dynamic friction - //f_active[k] = true; - - //sf[k]->m_type = e_frictionForce; - //sf[k]->m_p = p; - //sf[k]->m_kd = 100.0f; - } - } } - c->f1_active = f_active[0]; - c->f2_active = f_active[1]; - c->t1_active = t_active[0]; c->t2_active = t_active[1]; } @@ -687,14 +657,9 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity) for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) { - if (p->m_contact.f1_active) + if (p->m_contact.f_active) { - solver.Add(&p->m_contact.f1); - } - - if (p->m_contact.f2_active) - { - solver.Add(&p->m_contact.f2); + solver.Add(&p->m_contact.f); } if (p->m_contact.n_active) @@ -706,8 +671,7 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity) // Solve solver.Solve(dt, gravity); - // Clear external applied forces - // Clear translations + // Clear external applied forces and translations for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) { p->m_force.SetZero(); diff --git a/src/bounce/dynamics/cloth/cloth_solver.cpp b/src/bounce/dynamics/cloth/cloth_solver.cpp index 8872e0b..b57cde3 100644 --- a/src/bounce/dynamics/cloth/cloth_solver.cpp +++ b/src/bounce/dynamics/cloth/cloth_solver.cpp @@ -223,7 +223,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) sx0[i] = p->m_x; } - // Apply contact position correction + // Apply internal translations for (u32 i = 0; i < m_contactCount; ++i) { b3BodyContact* c = m_contacts[i]; diff --git a/src/bounce/dynamics/cloth/particle.cpp b/src/bounce/dynamics/cloth/particle.cpp index 063d991..650f81d 100644 --- a/src/bounce/dynamics/cloth/particle.cpp +++ b/src/bounce/dynamics/cloth/particle.cpp @@ -57,8 +57,7 @@ b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth) m_x.SetZero(); m_vertex = ~0; - m_contact.f1_active = false; - m_contact.f2_active = false; + m_contact.f_active = false; m_contact.n_active = false; m_contact.t1_active = false; m_contact.t2_active = false; @@ -87,8 +86,7 @@ void b3Particle::SetType(b3ParticleType type) m_velocity.SetZero(); m_translation.SetZero(); - m_contact.f1_active = false; - m_contact.f2_active = false; + m_contact.f_active = false; m_contact.n_active = false; m_contact.t1_active = false; m_contact.t2_active = false;