diff --git a/include/bounce/cloth/cloth.h b/include/bounce/cloth/cloth.h index c78e921..17c25c0 100644 --- a/include/bounce/cloth/cloth.h +++ b/include/bounce/cloth/cloth.h @@ -156,9 +156,6 @@ private: // Update particle contacts. void UpdateParticleContacts(); - - // Update triangle contacts. - void UpdateTriangleContacts(); // Update contacts void UpdateContacts(); @@ -192,10 +189,7 @@ private: // Pool of particle contacts b3BlockPool m_particleContactBlocks; - - // Pool of triangle contacts - b3BlockPool m_triangleContactBlocks; - + // List of particles b3List2 m_particleList; diff --git a/src/bounce/cloth/cloth.cpp b/src/bounce/cloth/cloth.cpp index 3ca1e95..7e36935 100644 --- a/src/bounce/cloth/cloth.cpp +++ b/src/bounce/cloth/cloth.cpp @@ -149,8 +149,7 @@ static u32 b3FindSharedEdges(b3SharedEdge* sharedEdges, const b3ClothMesh* m) b3Cloth::b3Cloth(const b3ClothDef& def) : m_particleBlocks(sizeof(b3Particle)), m_bodyContactBlocks(sizeof(b3BodyContact)), - m_particleContactBlocks(sizeof(b3ParticleContact)), - m_triangleContactBlocks(sizeof(b3TriangleContact)) + m_particleContactBlocks(sizeof(b3ParticleContact)) { B3_ASSERT(def.mesh); B3_ASSERT(def.density > 0.0f); @@ -624,241 +623,6 @@ void b3Cloth::UpdateParticleContacts() } } -static B3_FORCE_INLINE void b3Barycentric(float32 out[3], - const b3Vec3& A, const b3Vec3& B, - const b3Vec3& Q) -{ - b3Vec3 AB = B - A; - b3Vec3 QA = A - Q; - b3Vec3 QB = B - Q; - - //float32 divisor = b3Dot(AB, AB); - - out[0] = b3Dot(QB, AB); - out[1] = -b3Dot(QA, AB); - out[2] = out[0] + out[1]; -} - -// Convert a point Q from Cartesian coordinates to Barycentric coordinates (u, v, w) -// with respect to a triangle ABC. -// The last output value is the divisor. -static B3_FORCE_INLINE void b3Barycentric(float32 out[4], - const b3Vec3& A, const b3Vec3& B, const b3Vec3& C, - const b3Vec3& Q) -{ - 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 divisor = b3Dot(AB_x_AC, AB_x_AC); - - out[0] = b3Dot(QB_x_QC, AB_x_AC); - out[1] = b3Dot(QC_x_QA, AB_x_AC); - out[2] = b3Dot(QA_x_QB, AB_x_AC); - out[3] = out[0] + out[1] + out[2]; -} - -static B3_FORCE_INLINE void b3Solve3(float32 out[3], - const b3Vec3& A, const b3Vec3& B, const b3Vec3& C, - const b3Vec3& Q) -{ - // Test vertex regions - float32 wAB[3], wBC[3], wCA[3]; - b3Barycentric(wAB, A, B, Q); - b3Barycentric(wBC, B, C, Q); - b3Barycentric(wCA, C, A, Q); - - // R A - if (wAB[1] <= 0.0f && wCA[0] <= 0.0f) - { - out[0] = 1.0f; - out[1] = 0.0f; - out[2] = 0.0f; - return; - } - - // R B - if (wAB[0] <= 0.0f && wBC[1] <= 0.0f) - { - out[0] = 0.0f; - out[1] = 1.0f; - out[2] = 0.0f; - return; - } - - // R C - if (wBC[0] <= 0.0f && wCA[1] <= 0.0f) - { - out[0] = 0.0f; - out[1] = 0.0f; - out[2] = 1.0f; - return; - } - - // Test edge regions - float32 wABC[4]; - b3Barycentric(wABC, A, B, C, Q); - - // This is used to help testing if the face degenerates - // into an edge. - float32 area = wABC[3]; - - // R AB - if (wAB[0] > 0.0f && wAB[1] > 0.0f && area * wABC[2] <= 0.0f) - { - float32 divisor = wAB[2]; - B3_ASSERT(divisor > 0.0f); - float32 s = 1.0f / divisor; - out[0] = s * wAB[0]; - out[1] = s * wAB[1]; - out[2] = 0.0f; - return; - } - - // R BC - if (wBC[0] > 0.0f && wBC[1] > 0.0f && area * wABC[0] <= 0.0f) - { - float32 divisor = wBC[2]; - B3_ASSERT(divisor > 0.0f); - float32 s = 1.0f / divisor; - out[0] = 0.0f; - out[1] = s * wBC[0]; - out[2] = s * wBC[1]; - return; - } - - // R CA - if (wCA[0] > 0.0f && wCA[1] > 0.0f && area * wABC[1] <= 0.0f) - { - float32 divisor = wCA[2]; - B3_ASSERT(divisor > 0.0f); - float32 s = 1.0f / divisor; - out[0] = 0.0f; - out[2] = s * wCA[0]; - out[1] = s * wCA[1]; - return; - } - - // R ABC/ACB - float32 divisor = wABC[3]; - if (divisor <= 0.0f) - { - float32 s = 1.0f / 3.0f; - out[0] = s; - out[2] = s; - out[1] = s; - return; - } - - B3_ASSERT(divisor > 0.0f); - float32 s = 1.0f / divisor; - out[0] = s * wABC[0]; - out[1] = s * wABC[1]; - out[2] = s * wABC[2]; -} - -void b3Cloth::UpdateTriangleContacts() -{ - B3_PROFILE("Cloth Update Triangle Contacts"); - - // Clear buffer - b3TriangleContact* c = m_triangleContactList.m_head; - while (c) - { - b3TriangleContact* c0 = c; - c = c->m_next; - m_triangleContactList.Remove(c0); - c0->~b3TriangleContact(); - m_triangleContactBlocks.Free(c0); - } - - // Create triangle contacts - for (b3Particle* p1 = m_particleList.m_head; p1; p1 = p1->m_next) - { - for (u32 i = 0; i < m_mesh->triangleCount; ++i) - { - b3ClothMeshTriangle* triangle = m_mesh->triangles + i; - - b3Particle* p2 = m_vertexParticles[triangle->v1]; - b3Particle* p3 = m_vertexParticles[triangle->v2]; - b3Particle* p4 = m_vertexParticles[triangle->v3]; - - if (p1 == p2 || p1 == p3 || p1 == p4) - { - continue; - } - - float32 r1 = p1->m_radius; - float32 r2 = 0.0f; - - float32 totalRadius = r1 + r2; - - b3Vec3 A = p2->m_position; - b3Vec3 B = p3->m_position; - b3Vec3 C = p4->m_position; - - b3Vec3 P = p1->m_position; - - b3Vec3 n = b3Cross(B - A, C - A); - float32 n_len = n.Normalize(); - - float32 distance = b3Dot(n, P - A); - - if (distance < -totalRadius) - { - continue; - } - - if (distance > totalRadius) - { - continue; - } - - // Project particle on triangle plane - b3Vec3 Q = P - distance * n; - - // Barycentric coordinates for Q - float32 wABC[3]; - b3Solve3(wABC, A, B, C, Q); - - b3Vec3 c1 = p1->m_position; - b3Vec3 c2 = wABC[0] * A + wABC[1] * B + wABC[2] * C; - - if (b3DistanceSquared(c1, c2) > totalRadius * totalRadius) - { - continue; - } - - bool front = false; - - // Is the the other point in front of the triangle plane? - if (distance >= 0.0f) - { - front = true; - } - - b3TriangleContact* c = (b3TriangleContact*)m_triangleContactBlocks.Allocate(); - c->p1 = p1; - c->p2 = p2; - c->p3 = p3; - c->p4 = p4; - c->front = front; - c->normalImpulse = 0.0f; - - m_triangleContactList.PushFront(c); - } - } -} - void b3Cloth::Solve(float32 dt, const b3Vec3& gravity) { B3_PROFILE("Cloth Solve"); @@ -918,9 +682,6 @@ void b3Cloth::UpdateContacts() #if 0 // Update particle contacts UpdateParticleContacts(); - - // Update triangle contacts - UpdateTriangleContacts(); #endif } diff --git a/src/bounce/cloth/cloth_solver.cpp b/src/bounce/cloth/cloth_solver.cpp index 9e3b325..95c2c2f 100644 --- a/src/bounce/cloth/cloth_solver.cpp +++ b/src/bounce/cloth/cloth_solver.cpp @@ -155,6 +155,7 @@ void b3ClothSolver::ApplyConstraints() { b3DiagMat33& S = *m_solverData.S; b3DenseVec3& z = *m_solverData.z; + b3DenseVec3& x = *m_solverData.x; S.SetIdentity(); z.SetZero(); @@ -172,6 +173,35 @@ void b3ClothSolver::ApplyConstraints() } } +#if 0 + for (u32 i = 0; i < m_bodyContactCount; ++i) + { + b3BodyContact* bc = m_bodyContacts[i]; + + b3Particle* p1 = bc->p1; + + B3_ASSERT(p1->m_type == e_dynamicParticle); + + b3Transform xf1; + xf1.position = x[p1->m_solverId]; + xf1.rotation.SetIdentity(); + + b3Shape* s2 = bc->s2; + b3Body* b2 = s2->GetBody(); + b3Transform xf2 = b2->GetTransform(); + + b3BodyContactWorldPoint bcwp; + bcwp.Initialize(bc, p1->m_radius, xf1, s2->m_radius, xf2); + + b3AccelerationConstraint* ac = m_constraints + m_constraintCount; + ++m_constraintCount; + ac->i1 = p1->m_solverId; + ac->ndof = 2; + ac->p = bcwp.normal; + ac->z.SetZero(); + } +#endif + for (u32 i = 0; i < m_constraintCount; ++i) { m_constraints[i].Apply(&m_solverData); @@ -223,6 +253,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) m_solverData.S = &S; m_solverData.z = &z; + // Apply position correction for (u32 i = 0; i < m_particleCount; ++i) { b3Particle* p = m_particles[i]; @@ -230,7 +261,27 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) sy[i] = p->m_translation; sx0[i] = p->m_x; } + +#if 0 + for (u32 i = 0; i < m_bodyContactCount; ++i) + { + b3BodyContact* bc = m_bodyContacts[i]; + b3Particle* p1 = bc->p1; + b3Transform xf1; + xf1.position = sx[p1->m_solverId]; + xf1.rotation.SetIdentity(); + + b3Shape* s2 = bc->s2; + b3Body* b2 = s2->GetBody(); + b3Transform xf2 = b2->GetTransform(); + + b3BodyContactWorldPoint bcwp; + bcwp.Initialize(bc, p1->m_radius, xf1, s2->m_radius, xf2); + + sy[p1->m_solverId] += bcwp.separation * bcwp.normal; + } +#endif // Apply internal forces ApplyForces();